机器人直行和旋转

系统版本：Ubuntu14.04，ROS jade
机器人：小强
一、功能介绍
    编写两个简单程序，一个是机器人直行，另一个是原地旋转
二、实验步骤
    1、远程登录

ssh -X xiaoqiang@192.168.31.70

    2、创建程序包

mkdir -p ~/Documents/mytest/src

cd ~/Documents/mytest/srccatkin_init_workspace

cd ~/Documents/mytest/

catkin_make

source devel/setup.bash

cd ~/Documents/mytest/src/catkin_create_pkg robot_move roscpp geometry_msgs tf

    3、 在robot_move/src/里创建goforward.cpp

cd robot_move/src/vi goforward.cpp

    并粘贴如下代码：

#include <ros/ros.h>#include <signal.h>#include <geometry_msgs/Twist.h>
ros::Publisher cmdVelPub;
void shutdown(int sig){  cmdVelPub.publish(geometry_msgs::Twist());//使机器人停止运动  ROS_INFO("goforward cpp ended!");  ros::shutdown();}
int main(int argc, char** argv){  ros::init(argc, argv, "GoForward");//初始化ROS,它允许ROS通过命令行进行名称重映射  std::string topic = "/cmd_vel";  ros::NodeHandle node;//为这个进程的节点创建一个句柄  cmdVelPub = node.advertise<geometry_msgs::Twist>(topic, 1);//告诉master将要在/cmd_vel topic上发布一个geometry_msgs/Twist的消息  ros::Rate loopRate(10);//The desired rate to run at in Hz,ros::Rate对象可以允许你指定自循环的频率  // Override the default ros sigint handler. This must be set after the first NodeHandle is creat  signal(SIGINT, shutdown);  ROS_INFO("goforward cpp start...");  geometry_msgs::Twist speed; // 控制信号载体 Twist message  while (ros::ok())  {    speed.linear.x = 0.5; // 设置线速度为0.1m/s，正为前进，负为后退    speed.angular.z = 0; // 设置角速度为0rad/s，正为左转，负为右转    cmdVelPub.publish(speed); // 将刚才设置的指令发送给机器人    loopRate.sleep();//休眠直到一个频率周期的时间  }
  return 0;}

     4、 在robot_move/src/里创建goincircles.cpp

vi goincircles.cpp

将goforward.cpp的源代码复制到goincircles.cpp，将speed.linear.x设置为0，speed.angular.z设置为0.5，其它不变。

     5、在robot_move/src/里创建odom_out_and_back.cpp

#include <ros/ros.h>#include <signal.h>#include <geometry_msgs/Twist.h>#include <tf/transform_listener.h>#include <nav_msgs/Odometry.h>#include <string.h>ros::Publisher cmdVelPub;void shutdown(int sig){  cmdVelPub.publish(geometry_msgs::Twist());  ROS_INFO("odom_out_and_back.cpp ended!");  ros::shutdown();}int main(int argc, char** argv){  //How fast will we update the robot's movement?  double rate = 20;  int count = 0;//Loop through the two legs of the trip  ros::init(argc, argv, "go_and_back");  std::string topic = "/cmd_vel";  ros::NodeHandle node;  cmdVelPub = node.advertise<geometry_msgs::Twist>(topic, 1);  //Set the equivalent ROS rate variable  ros::Rate loopRate(rate);  geometry_msgs::Twist speed; // 控制信号载体 Twist message  signal(SIGINT, shutdown);  ROS_INFO("odom_out_and_back.cpp start...");  //Set the forward linear speed to 0.2 meters per second  float linear_speed = 0.2;  //Set the travel distance to 1.0 meters  float goal_distance = 1.0;  //Set the rotation speed to 0.5 radians per second  float angular_speed = 0.5;  //Set the rotation angle to Pi radians (180 degrees)  double goal_angle = M_PI;  //Set the angular tolerance in degrees converted to radians  double angular_tolerance = 2.5*M_PI/180; //角度转换成弧度:deg*PI/180  tf::TransformListener listener;  tf::StampedTransform transform;  //Find out if the robot uses /base_link or /base_footprint  std::string odom_frame = "/odom";  std::string base_frame;  try  {    listener.waitForTransform(odom_frame, "/base_footprint", ros::Time(), ros::Duration(2.0) );    base_frame = "/base_footprint";    ROS_INFO("base_frame = /base_footprint");  }  catch (tf::TransformException & ex)  {      try      {        listener.waitForTransform(odom_frame, "/base_link", ros::Time(), ros::Duration(2.0) );        base_frame = "/base_link";        ROS_INFO("base_frame = /base_link");      }      catch (tf::TransformException ex)      {          ROS_INFO("Cannot find transform between /odom and /base_link or /base_footprint");          cmdVelPub.publish(geometry_msgs::Twist());          ros::shutdown();      }  }  //Loop once for each leg of the trip  for(int i = 0;i < 2;i++)  {    ROS_INFO("go straight...!");    speed.linear.x = linear_speed; // 设置线速度，正为前进，负为后退    //Get the starting position values    listener.lookupTransform(odom_frame, base_frame, ros::Time(0), transform);    float x_start = transform.getOrigin().x();    float y_start = transform.getOrigin().y();    // Keep track of the distance traveled    float distance = 0;    while( (distance < goal_distance) && (ros::ok()) )    {         //Publish the Twist message and sleep 1 cycle         cmdVelPub.publish(speed);         loopRate.sleep();         listener.lookupTransform(odom_frame, base_frame, ros::Time(0), transform);         //Get the current position         float x = transform.getOrigin().x();         float y = transform.getOrigin().y();         //Compute the Euclidean distance from the start         distance = sqrt(pow((x - x_start), 2) +  pow((y - y_start), 2));    }    //Stop the robot before the rotation    cmdVelPub.publish(geometry_msgs::Twist());    ros::Duration(1).sleep(); // sleep for  a second    ROS_INFO("rotation...!");    //Now rotate left roughly 180 degrees    speed.linear.x = 0;    //Set the angular speed    speed.angular.z = angular_speed; // 设置角速度，正为左转，负为右转    //yaw是围绕Y轴旋转，也叫偏航角    //Track the last angle measured    double last_angle = fabs(tf::getYaw(transform.getRotation()));    //Track how far we have turned    double turn_angle = 0;    while( (fabs(turn_angle + angular_tolerance) < M_PI) && (ros::ok()) )    {        //Publish the Twist message and sleep 1 cycle        cmdVelPub.publish(speed);        loopRate.sleep();        // Get the current rotation        listener.lookupTransform(odom_frame, base_frame, ros::Time(0), transform);        //C++: abs()求得是正数的绝对值,fabs()求得是浮点数的绝对值;python:abs(x),参数可以是：负数、正数、浮点数或者长整形        double rotation = fabs(tf::getYaw(transform.getRotation()));        //Compute the amount of rotation since the last loop        double delta_angle = fabs(rotation - last_angle);        //Add to the running total        turn_angle += delta_angle;        last_angle = rotation;    }    //Stop the robot before the rotation    //Set the angular speed    speed.angular.z = 0;    cmdVelPub.publish(geometry_msgs::Twist());    ros::Duration(1).sleep(); // sleep for  a second  }  cmdVelPub.publish(geometry_msgs::Twist());  ros::Duration(1).sleep(); // sleep for  a second  ROS_INFO("odom_out_and_back.cpp ended!");  ros::shutdown();  return 0;}

     6、 修改robot_move目录下的CMakeLists.txt

cd ..vi CMakeLists.txt

将catkin_package()中的CATKIN_DEPENDS geometry_msgs roscpp去注释
在CMakeLists.txt文件末尾加入几条语句:

add_executable(goforward src/goforward.cpp)target_link_libraries(goforward ${catkin_LIBRARIES})add_executable(goincircles src/goincircles.cpp)target_link_libraries(goincircles ${catkin_LIBRARIES})

add_executable(odom_out_and_back src/odom_out_and_back.cpp)target_link_libraries(odom_out_and_back ${catkin_LIBRARIES})

    7、 编译程序

    在mytest目录下

catkin_make

    8、 测试程序

    8.1 启动goforward程序

 source devel/setup.bash

 rosrun robot_move goforward

    8.2 Ctrl+C结束goforward程序，再测试goincircles程序

 rosrun robot_move goincircles

  8.3 Ctrl+C结束goincircles程序，再测试odom_out_and_back程序

  rosrun robot_move odom_out_and_back