// 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, "odom");
frame_id_base_link = tf::resolve(tf_prefix, "base_link");
frame_id_bumper = tf::resolve(tf_prefix, "bumpers_frame");
frame_id_sonar = tf::resolve(tf_prefix, "sonar_frame");
// advertise services for data topics
// second argument to advertise() is queue size.
// other argmuments (optional) are callbacks, or a boolean "latch" flag (whether to send current data to new
// subscribers when they subscribe).
// See ros::NodeHandle API docs.
pose_pub = n.advertise<nav_msgs::Odometry>("pose",1000);//用于输出机器人姿态 //话题名称为”pose”
bumpers_pub = n.advertise<rosaria::BumperState>("bumper_state",1000);
sonar_pub = n.advertise<sensor_msgs::PointCloud>("sonar", 50,
boost::bind(&RosAriaNode::sonarConnectCb, this),
boost::bind(&RosAriaNode::sonarConnectCb, this));
voltage_pub = n.advertise<std_msgs::Float64>("battery_voltage", 1000);
recharge_state_pub = n.advertise<std_msgs::Int8>("battery_recharge_state", 5, true /*latch*/ );
recharge_state.data = -2;
state_of_charge_pub = n.advertise<std_msgs::Float32>("battery_state_of_charge", 100);
motors_state_pub = n.advertise<std_msgs::Bool>("motors_state", 5, true /*latch*/ );
motors_state.data = false;
published_motors_state = false;
// subscribe to services//话题名称为”cmd_vel”
//将Topic"cmd_vel"与Subscribe:cmdvel_sub以及函数cmdvel_cb对应上
//是不是凡是接受者, 都用ConstPtr??
cmdvel_sub = n.subscribe( "cmd_vel", 1, (boost::function <void(const geometry_msgs::TwistConstPtr&)>)
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("enable_motors", &RosAriaNode::enable_motors_cb, this);
disable_srv = n.advertiseService("disable_motors", &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