ROS：编写节点，让ROS小乌龟画圆和矩形

操作系统：ubuntu1404，ROS indigo

软件：Roboware

实验开始

在my_turtle_package(自己创建的package)->src 中创建cpp文件：(如下图)

让小乌龟画圆的代码：

#include "ros/ros.h"
#include //运动速度结构体类型  geometry_msgs::Twist的定义文件

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "vel_ctrl");  //对该节点进行初始化操作
    ros::NodeHandle n;         //申明一个NodeHandle对象n，并用n生成一个广播对象vel_pub
    ros::Publisher vel_pub = n.advertise("/cmd_vel", 10);
    //vel_pub会在主题"/cmd_vel"(机器人速度控制主题)里广播geometry_msgs::Twist类型的数据
    ROS_INFO("draw_circle start...");//输出显示信息
    while(ros::ok())
    {
        geometry_msgs::Twist vel_cmd; //声明一个geometry_msgs::Twist 类型的对象vel_cmd，并将速度的值赋值到这个对象里面

        vel_cmd.linear.x = 2.0;//前后（+-） m/s
        vel_cmd.linear.y = 0.0;  //左右（+-） m/s
        vel_cmd.linear.z = 0.0;

        vel_cmd.angular.x = 0;
        vel_cmd.angular.y = 0;
        vel_cmd.angular.z = 1.8; //机器人的自转速度，+左转，-右转，单位是rad/s
        vel_pub.publish(vel_cmd); //赋值完毕后，发送到主题"/cmd_vel"。机器人的核心节点会从这个主题接受发送过去的速度值，并转发到硬件体上去执行

        ros::spinOnce();//调用此函数给其他回调函数得以执行(比例程未使用回调函数)
    }
    return 0;
}

让小乌龟画矩形的代码：

#include 
#include 
#define PI 3.14159265358979323846

int main(int argc, char **argv){
  ros::init(argc, argv, "draw_rectangle");   //"draw_rectangle"必须是nodename
  std::string topic = "/turtle1/cmd_vel"; //topic name
  ros::NodeHandle n;
  ros::Publisher cmdVelPub = n.advertise(topic, 1);
  //第一个参数也可以写成"/turtle1/cmd_vel"这样的topic name
  //第二个参数是发布的缓冲区大小,是消息类型
  ros::Rate loopRate(2);
  //与Rate::sleep();配合指定自循环频率
  ROS_INFO("draw_retangle start...");//输出显示信息
  geometry_msgs::Twist speed; // 控制信号载体 Twist message
//声明一个geometry_msgs::Twist 类型的对象speed，并将速度的值赋值到这个对象里面
  int count = 0;
  while (ros::ok()){
    speed.linear.x = 1; // 设置线速度为1m/s，正为前进，负为后退
    speed.linear.y = 0;
    speed.linear.z = 0;
    speed.angular.x = 0;
    speed.angular.y = 0;
    speed.angular.z = 0; 
    count++;
    while(count == 5)
    {
        count=0;
        speed.angular.z = PI; //转90°
    }
    cmdVelPub.publish(speed); // 将刚才设置的指令发送给机器人
    ros::spinOnce();//调用此函数给其他回调函数得以执行
    loopRate.sleep();//按loopRate(2)设置的2HZ将程序挂起
  }

  return 0;
}

实验测试

测试代码：

$:roscore 
$:rosrun turtlesim turtlesim_node
$:rosrun my_turtle_package draw_circle      #让小乌龟作圆周运动
$:rosrun my_turtle_package draw_rectangle   #让小乌龟作矩形运动

效果图：

画圆：

画矩形：

文章地址：https://mp.csdn.net/postedit/84950696    作者：IMBA_09       转载请注明出处~