机器人操作系统ROS_典型功能代码详解(3)

if (n_.hasParam(\)) { n_.getParam( \, RevCount); ROS_INFO(\, RevCount); robot->comInt(88, RevCount); } else { RevCount = robot->getOrigRobotConfig()->getRevCount(); n_.setParam( \, RevCount); ROS_INFO(\, RevCount); } robot->unlock(); }

void RosAriaNode::dynamic_reconfigureCB(rosaria::RosAriaConfig &config, uint32_t level) { // // Odometry Settings // robot->lock(); if(TicksMM != config.TicksMM and config.TicksMM > 0) { ROS_INFO(\, TicksMM, config.TicksMM); TicksMM = config.TicksMM; robot->comInt(93, TicksMM); } if(DriftFactor != config.DriftFactor) { ROS_INFO(\, DriftFactor, config.DriftFactor); DriftFactor = config.DriftFactor; robot->comInt(89, DriftFactor); } if(RevCount != config.RevCount and config.RevCount > 0) { ROS_INFO(\, RevCount, config.RevCount); RevCount = config.RevCount; robot->comInt(88, RevCount); } // // Acceleration Parameters // int value; value = config.trans_accel * 1000; if(value != robot->getTransAccel() and value > 0) { ROS_INFO(\, value); robot->setTransAccel(value); } value = config.trans_decel * 1000; if(value != robot->getTransDecel() and value > 0) { ROS_INFO(\, value); robot->setTransDecel(value); } value = config.lat_accel * 1000; if(value != robot->getLatAccel() and value > 0) { ROS_INFO(\, value); if (robot->getAbsoluteMaxLatAccel() > 0 ) robot->setLatAccel(value);

11

} value = config.lat_decel * 1000; if(value != robot->getLatDecel() and value > 0) { ROS_INFO(\, value); if (robot->getAbsoluteMaxLatDecel() > 0 ) robot->setLatDecel(value); } value = config.rot_accel * 180/M_PI; if(value != robot->getRotAccel() and value > 0) { ROS_INFO(\, value); robot->setRotAccel(value); } value = config.rot_decel * 180/M_PI; if(value != robot->getRotDecel() and value > 0) { ROS_INFO(\, value); robot->setRotDecel(value); } robot->unlock(); }

void RosAriaNode::sonarConnectCb() { robot->lock(); if (sonar_pub.getNumSubscribers() == 0) { robot->disableSonar(); use_sonar = false; } else { robot->enableSonar(); use_sonar = true; } robot->unlock(); }

RosAriaNode::RosAriaNode(ros::NodeHandle nh) :

myPublishCB(this, &RosAriaNode::publish), serial_port(\), serial_baud(0), use_sonar(false) //整合了publish()函数?? //不使用声纳 { // read in config options n = nh; // !!! port !!! n.param( \, serial_port, std::string(\) ); //给NodeHandle加入了一个名为”port”的标签?? ROS_INFO( \, serial_port.c_str() ); n.param(\, serial_baud, 0); if(serial_baud != 0) ROS_INFO(\, serial_baud); // handle debugging more elegantly n.param( \, debug_aria, false ); // default not to debug n.param( \, aria_log_filename, std::string(\) ); // Figure out what frame_id's to use. if a tf_prefix param is specified, // it will be added to the beginning of the frame_ids. // // e.g. rosrun ... _tf_prefix:=MyRobot (or equivalently using s in // roslaunch files)

12

// will result in the frame_ids being set to /MyRobot/odom etc, // rather than /odom. This is useful for Multi Robot Systems. // See ROS Wiki for further details. tf_prefix = tf::getPrefixParam(n); frame_id_odom = tf::resolve(tf_prefix, \); frame_id_base_link = tf::resolve(tf_prefix, \); frame_id_bumper = tf::resolve(tf_prefix, \); frame_id_sonar = tf::resolve(tf_prefix, \); // advertise services for data topics // second argument to advertise() is queue size. // other argmuments (optional) are callbacks, or a boolean \ // subscribers when they subscribe). // See ros::NodeHandle API docs. pose_pub = n.advertise(\,1000);//用于输出机器人姿态 //话题名称为”pose” bumpers_pub = n.advertise(\,1000); sonar_pub = n.advertise(\, 50, boost::bind(&RosAriaNode::sonarConnectCb, this), boost::bind(&RosAriaNode::sonarConnectCb, this)); voltage_pub = n.advertise(\, 1000); recharge_state_pub = n.advertise(\, 5, true /*latch*/ ); recharge_state.data = -2; state_of_charge_pub = n.advertise(\, 100); motors_state_pub = n.advertise(\, 5, true /*latch*/ ); motors_state.data = false; published_motors_state = false; // subscribe to services//话题名称为”cmd_vel” //将Topic\与Subscribe:cmdvel_sub以及函数cmdvel_cb对应上 //是不是凡是接受者, 都用ConstPtr?? cmdvel_sub = n.subscribe( \, 1, (boost::function ) boost::bind(&RosAriaNode::cmdvel_cb, this, _1 )); //绑定函数?? //实现的功能: 其他节点发布到话题”cmd_vel”上的数据类型geometry_msgs::Twist, 就会传到到函数RosAriaNode::cmdvel_cb(const geometry_msgs::TwistConstPtr &msg)中, 并执行该函数(即控制机器人的速度) // advertise enable/disable services enable_srv = n.advertiseService(\, &RosAriaNode::enable_motors_cb, this); disable_srv = n.advertiseService(\, &RosAriaNode::disable_motors_cb, this); veltime = ros::Time::now(); }

RosAriaNode::~RosAriaNode() { // disable motors and sonar. robot->disableMotors(); robot->disableSonar(); robot->stopRunning(); robot->waitForRunExit(); Aria::shutdown(); }

int RosAriaNode::Setup() { // Note, various objects are allocated here which are never deleted (freed), since Setup() is only supposed to be // called once per instance, and these objects need to persist until the process terminates. robot = new ArRobot(); ArArgumentBuilder *args = new ArArgumentBuilder(); // never freed ArArgumentParser *argparser = new ArArgumentParser(args); // Warning never freed argparser->loadDefaultArguments(); // adds any arguments given in /etc/Aria.args. Useful on robots with unusual serial port or baud rate (e.g. pioneer lx)

13

// Now add any parameters given via ros params (see RosAriaNode constructor): // if serial port parameter contains a ':' character, then interpret it as hostname:tcpport // for wireless serial connection. Otherwise, interpret it as a serial port name. size_t colon_pos = serial_port.find(\); if (colon_pos != std::string::npos) { args->add(\); // pass robot's hostname/IP address to Aria args->add(serial_port.substr(0, colon_pos).c_str()); args->add(\); // pass robot's TCP port to Aria args->add(serial_port.substr(colon_pos+1).c_str()); } else { args->add(\); // pass robot's serial port to Aria args->add(serial_port.c_str()); } // if a baud rate was specified in baud parameter if(serial_baud != 0) { args->add(\); char tmp[100]; snprintf(tmp, 100, \, serial_baud); args->add(tmp); } if( debug_aria ) { // turn on all ARIA debugging args->add(\); // log received packets args->add(\); // log sent packets args->add(\); // log received velocities args->add(\); args->add(\); ArLog::init(ArLog::File, ArLog::Verbose, aria_log_filename.c_str(), true); } // Connect to the robot//嗯,新版本的Aria就得这样连接 conn = new ArRobotConnector(argparser, robot); // warning never freed if (!conn->connectRobot()) { ROS_ERROR(\device.)\); return 1; } // causes ARIA to load various robot-specific hardware parameters from the robot parameter file in /usr/local/Aria/params if(!Aria::parseArgs()) { ROS_ERROR(\); return 1; } readParameters(); // Start dynamic_reconfigure server dynamic_reconfigure_server = new dynamic_reconfigure::Server; // Setup Parameter Minimums rosaria::RosAriaConfig dynConf_min; dynConf_min.trans_accel = robot->getAbsoluteMaxTransAccel() / 1000; dynConf_min.trans_decel = robot->getAbsoluteMaxTransDecel() / 1000; // TODO: Fix rqt dynamic_reconfigure gui to handle empty intervals // Until then, set unit length interval.

14

}

dynConf_min.lat_accel = ((robot->getAbsoluteMaxLatAccel() > 0.0) ? robot->getAbsoluteMaxLatAccel() : 0.1) / 1000; dynConf_min.lat_decel = ((robot->getAbsoluteMaxLatDecel() > 0.0) ? robot->getAbsoluteMaxLatDecel() : 0.1) / 1000; dynConf_min.rot_accel = robot->getAbsoluteMaxRotAccel() * M_PI/180; dynConf_min.rot_decel = robot->getAbsoluteMaxRotDecel() * M_PI/180; // I'm setting these upper bounds relitivly arbitrarily, feel free to increase them. dynConf_min.TicksMM = 10; dynConf_min.DriftFactor = -200;

dynConf_min.RevCount = -32760;

dynamic_reconfigure_server->setConfigMin(dynConf_min);

rosaria::RosAriaConfig dynConf_max;

dynConf_max.trans_accel = robot->getAbsoluteMaxTransAccel() / 1000; dynConf_max.trans_decel = robot->getAbsoluteMaxTransDecel() / 1000; // TODO: Fix rqt dynamic_reconfigure gui to handle empty intervals // Until then, set unit length interval.

dynConf_max.lat_accel = ((robot->getAbsoluteMaxLatAccel() > 0.0) ? robot->getAbsoluteMaxLatAccel() : 0.1) / 1000; dynConf_max.lat_decel = ((robot->getAbsoluteMaxLatDecel() > 0.0) ? robot->getAbsoluteMaxLatDecel() : 0.1) / 1000; dynConf_max.rot_accel = robot->getAbsoluteMaxRotAccel() * M_PI/180; dynConf_max.rot_decel = robot->getAbsoluteMaxRotDecel() * M_PI/180; // I'm setting these upper bounds relitivly arbitrarily, feel free to increase them. dynConf_max.TicksMM = 200; dynConf_max.DriftFactor = 200;

dynConf_max.RevCount = 32760;

dynamic_reconfigure_server->setConfigMax(dynConf_max); rosaria::RosAriaConfig dynConf_default;

dynConf_default.trans_accel = robot->getTransAccel() / 1000; dynConf_default.trans_decel = robot->getTransDecel() / 1000; dynConf_default.lat_accel = robot->getLatAccel() / 1000; dynConf_default.lat_decel = robot->getLatDecel() / 1000;

dynConf_default.rot_accel = robot->getRotAccel() * M_PI/180; dynConf_default.rot_decel = robot->getRotDecel() * M_PI/180; dynConf_default.TicksMM = TicksMM; dynConf_default.DriftFactor = DriftFactor; dynConf_default.RevCount = RevCount;

dynamic_reconfigure_server->setConfigDefault(dynConf_max);

dynamic_reconfigure_server->setCallback(boost::bind(&RosAriaNode::dynamic_reconfigureCB, this, _1, _2)); // Enable the motors robot->enableMotors(); // disable sonars on startup robot->disableSonar();

// callback will be called by ArRobot background processing thread for every SIP data packet received from robot robot->addSensorInterpTask(\, 100, &myPublishCB); // Initialize bumpers with robot number of bumpers

bumpers.front_bumpers.resize(robot->getNumFrontBumpers()); bumpers.rear_bumpers.resize(robot->getNumRearBumpers()); // Run ArRobot background processing thread robot->runAsync(true); return 0;

15