ROS編程示例---隨機移動

運行roscore,turtlesim

設置速度rosparam set /publish_velocity/max_vel 1

//發佈者程序,改進最大速度版，教程的老是撞牆，重新調整了角度
//先手動設置rosparam set /publish_velocity/max_vel 1 速度爲一避免有那種撞牆的反應
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>//這裏使用了turtlesim的消息類型
#include <cstdlib>//使用隨機函數
int main(int argc,char **argv)
{
  //創建發佈者對象，聲明發布類型和發佈的topic，並且，這裏的topic恰好是烏龜默認的turtle1/cmd_vel
  //當我們不使用默認名稱的時候就會失效，這是程序的一個缺點吧

  ros::init(argc,argv,"publish_velocity");//創建節點
  ros::NodeHandle n;//創建節點句柄

  ros::Publisher pub = n.advertise<geometry_msgs::Twist>("turtle1/cmd_vel",1000);//發佈消息，隊列消息容量1000

  //設置隨機種子  
  srand(time(0));

  const std::string PARAM_NAME = "~max_vel" ;
  //設置最大速度，且這個數值是我們自己設置的


  double maxVel;

  //判斷是否能獲取參數
  bool ok = ros::param::get (PARAM_NAME, maxVel) ;

  if(!ok)
  {

    //fatal級通知
    ROS_FATAL_STREAM("Could not get parameter "<<PARAM_NAME);
    //退出
    exit(1);
  }

  ros::Rate rate(2);
  //固定頻率2Hz

  while(ros::ok())
  {
    geometry_msgs::Twist msg;

    //我們只需要參數中的兩個值，平面運動
    //填補消息:
    //速度：-max_vel- max_vel
    //角度：-3-3
    msg.linear.x=maxVel*double(rand())/double(RAND_MAX);

    msg.angular.z=6*double(rand())/double(RAND_MAX)-3;

    //publish
    pub.publish(msg);

    //控制檯實時顯示
    ROS_INFO_STREAM("Randomly control"
    <<"   v:"<<msg.linear.x
    <<"   theta:"<<msg.angular.z);

    //暫停
    rate.sleep();
  }
}