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

void RosAriaNode::spin() { ros::spin(); }

void RosAriaNode::publish() { // Note, this is called via SensorInterpTask callback (myPublishCB, named \be locked or unlocked. pos = robot->getPose(); tf::poseTFToMsg(tf::Transform(tf::createQuaternionFromYaw(pos.getTh()*M_PI/180), tf::Vector3(pos.getX()/1000, pos.getY()/1000, 0)), position.pose.pose); //Aria returns pose in mm. position.twist.twist.linear.x = robot->getVel()/1000; //Aria returns velocity in mm/s. position.twist.twist.linear.y = robot->getLatVel()/1000.0; position.twist.twist.angular.z = robot->getRotVel()*M_PI/180; position.header.frame_id = frame_id_odom; position.child_frame_id = frame_id_base_link; position.header.stamp = ros::Time::now(); pose_pub.publish(position); ROS_DEBUG(\, position.header.stamp.toSec(), (double)position.pose.pose.position.x, (double)position.pose.pose.position.y, (double)position.pose.pose.orientation.w, (double) position.twist.twist.linear.x, (double) position.twist.twist.linear.y, (double) position.twist.twist.angular.z ); // publishing transform odom->base_link odom_trans.header.stamp = ros::Time::now(); odom_trans.header.frame_id = frame_id_odom; odom_trans.child_frame_id = frame_id_base_link; odom_trans.transform.translation.x = pos.getX()/1000; odom_trans.transform.translation.y = pos.getY()/1000; odom_trans.transform.translation.z = 0.0; odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(pos.getTh()*M_PI/180); odom_broadcaster.sendTransform(odom_trans);//也可以根据该tf消息, 来获得位姿啊! // getStallValue returns 2 bytes with stall bit and bumper bits, packed as (00 00 FrontBumpers RearBumpers) int stall = robot->getStallValue(); unsigned char front_bumpers = (unsigned char)(stall >> 8); unsigned char rear_bumpers = (unsigned char)(stall); bumpers.header.frame_id = frame_id_bumper; bumpers.header.stamp = ros::Time::now(); std::stringstream bumper_info(std::stringstream::out); // Bit 0 is for stall, next bits are for bumpers (leftmost is LSB) for (unsigned int i=0; igetNumFrontBumpers(); i++) { bumpers.front_bumpers[i] = (front_bumpers & (1 << (i+1))) == 0 ? 0 : 1; bumper_info << \ << (front_bumpers & (1 << (i+1))); } ROS_DEBUG(\, bumper_info.str().c_str()); bumper_info.str(\); // Rear bumpers have reverse order (rightmost is LSB) unsigned int numRearBumpers = robot->getNumRearBumpers(); for (unsigned int i=0; i

16

bumpers.rear_bumpers[i] = (rear_bumpers & (1 << (numRearBumpers-i))) == 0 ? 0 : 1; bumper_info << \ << (rear_bumpers & (1 << (numRearBumpers-i))); }

ROS_DEBUG(\, bumper_info.str().c_str()); bumpers_pub.publish(bumpers);

//Publish battery information

// TODO: Decide if BatteryVoltageNow (normalized to (0,12)V) is a better option std_msgs::Float64 batteryVoltage;

batteryVoltage.data = robot->getRealBatteryVoltageNow(); voltage_pub.publish(batteryVoltage);

if(robot->haveStateOfCharge()) { std_msgs::Float32 soc; soc.data = robot->getStateOfCharge()/100.0; state_of_charge_pub.publish(soc); }

// publish recharge state if changed char s = robot->getChargeState(); if(s != recharge_state.data) { ROS_INFO(\, s); recharge_state.data = s; recharge_state_pub.publish(recharge_state); }

// publish motors state if changed bool e = robot->areMotorsEnabled();

if(e != motors_state.data || !published_motors_state) { ROS_INFO(\, e); motors_state.data = e; motors_state_pub.publish(motors_state); published_motors_state = true; }

// Publish sonar information, if enabled. if (use_sonar) { sensor_msgs::PointCloud cloud; //sonar readings. cloud.header.stamp = position.header.stamp; //copy time. // sonar sensors relative to base_link cloud.header.frame_id = frame_id_sonar;

// Log debugging info

std::stringstream sonar_debug_info;

sonar_debug_info << \;

for (int i = 0; i < robot->getNumSonar(); i++) { ArSensorReading* reading = NULL; reading = robot->getSonarReading(i); if(!reading) { ROS_WARN(\); continue; }

// getRange() will return an integer between 0 and 5000 (5m) sonar_debug_info << reading->getRange() << \;

// local (x,y). Appears to be from the centre of the robot, since values may // exceed 5000. This is good, since it means we only need 1 transform. // x & y seem to be swapped though, i.e. if the robot is driving north

17

// x is north/south and y is east/west. // //ArPose sensor = reading->getSensorPosition(); //position of sensor. // sonar_debug_info << \ // << \ // << \ // << sensor.getY() << \ //add sonar readings (robot-local coordinate frame) to cloud geometry_msgs::Point32 p; p.x = reading->getLocalX() / 1000.0; p.y = reading->getLocalY() / 1000.0; p.z = 0.0; cloud.points.push_back(p); } ROS_DEBUG_STREAM(sonar_debug_info.str()); sonar_pub.publish(cloud); } }

bool RosAriaNode::enable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) { ROS_INFO(\); robot->lock(); if(robot->isEStopPressed()) ROS_WARN(\); robot->enableMotors(); robot->unlock(); // todo could wait and see if motors do become enabled, and send a response with an error flag if not return true; }

bool RosAriaNode::disable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) { ROS_INFO(\); robot->lock(); robot->disableMotors(); robot->unlock(); // todo could wait and see if motors do become disabled, and send a response with an error flag if not return true; }

void RosAriaNode::cmdvel_cb( const geometry_msgs::TwistConstPtr &msg) //传入参数为接收的数据类型 { veltime = ros::Time::now(); ROS_INFO( \, msg->linear.x*1e3, msg->angular.z, veltime.toSec() ); robot->lock(); robot->setVel(msg->linear.x*1e3); //设置线速度 if(robot->hasLatVel()) robot->setLatVel(msg->linear.y*1e3); robot->setRotVel(msg->angular.z*180/M_PI); //设置角速度 robot->unlock(); ROS_DEBUG(\, veltime.toSec(), (double) msg->linear.x * 1e3, (double) msg->linear.y * 1.3, (double) msg->angular.z * 180/M_PI); }

int main( int argc, char** argv ) { ros::init(argc,argv, \); ros::NodeHandle n(std::string(\));//带有”~”的NodeHandle的定义时, 在发布话题时, 自动在前面加上该节点的名字”RosAria” Aria::init(); RosAriaNode *node = new RosAriaNode(n);

18

}

if( node->Setup() != 0 )//难道成功了就返回0? { ROS_FATAL( \ ); return -1; }

node->spin();

delete node;//会启动析构函数,从而断开与机器人的连接 ROS_INFO( \ ); return 0;

#CmakeList.txt

2//rosaria_teleop_key.cpp

#include \

#include \

#include \ #include #include

#include //从这里学习了在CmakeList.txt中如何添加动态链接库

#define MAX_VELOCITY_LINEAR 5 #define MAX_VELOCITY_ANGULAR 1

int main (int argc, char **argv) {

ros::init(argc, argv, \); ros::NodeHandle n;

ros::Publisher key_publisher = n.advertise(\, 1000);

//对应上RosAria节点的话题名称\名称本为\知不是直接写它就可以了?? ros::Rate loop_rate(30);

int fwd_velocity = 0; int rw_velocity = 0; int ang_velocity = 0;

initscr();//initscr() 是一般 curses 程式必须先呼叫的函数, 一但这个函数被呼叫之后, 系统将根据终端机的形态并启动 curses 模式

cbreak();//把终端的CBREAK模式打开，如果CBREAK打开则程序就可以立刻使用读取的输入信息 noecho();//关闭回送 keypad(stdscr, TRUE); //当开启 keypad 后, 可以使用键盘上的一些特殊字元, 如上下左右>等方向键,

//curses 会将这些特殊字元转换成 curses.h 内定义的一些特殊键. 这些定义的特殊键通常以 KEY_ 开头 nodelay(stdscr, TRUE);

int count = 0; while (ros::ok()) {

geometry_msgs::Twist twist_msg;

int keyStroke = getch(); if (keyStroke == ERR) {

if (rw_velocity > 0) rw_velocity--; if (rw_velocity < 0) rw_velocity++; if (fwd_velocity > 0)

19

fwd_velocity--; if (fwd_velocity < 0) fwd_velocity++; if (ang_velocity > 0) ang_velocity--; if (ang_velocity < 0) ang_velocity++; } else {

switch (keyStroke) {

case KEY_UP:

if (rw_velocity > 0) rw_velocity-=1;

else if (fwd_velocity < MAX_VELOCITY_LINEAR) fwd_velocity+=1; break;

case KEY_DOWN:

if (fwd_velocity > 0) fwd_velocity-=1;

else if (rw_velocity < MAX_VELOCITY_LINEAR) rw_velocity+=1; break;

case KEY_LEFT:

if (ang_velocity < MAX_VELOCITY_ANGULAR) ang_velocity++; break;

case KEY_RIGHT:

if (ang_velocity > -MAX_VELOCITY_ANGULAR) ang_velocity--; break; default:

if (rw_velocity > 0) rw_velocity-=1; if (rw_velocity < 0) rw_velocity+=1; if (fwd_velocity > 0) fwd_velocity-=1; if (fwd_velocity < 0) fwd_velocity+=1; if (ang_velocity > 0) ang_velocity--; if (ang_velocity < 0) ang_velocity++; } }

twist_msg.linear.x = fwd_velocity - rw_velocity; twist_msg.angular.z = ang_velocity;

ROS_INFO(\, fwd_velocity-rw_velocity, ang_velocity);

key_publisher.publish(twist_msg);

ros::spinOnce();

loop_rate.sleep(); ++count; }

endwin(); return 0; }

20