ROS开发小乌龟自动爬行与实时位姿获取

本文从下面博客改编而来

ROS编程入门教程（全过程示例代码）_lhb0709的专栏-CSDN博客

首先创建工作空间（working space）

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
catkin_init_workspace

创建功能包pkg ,

catkin_create_pkg beginner_tutorials std_msgs rospy roscpp

创建源代码目录和源码cpp

mkdir -p ~/catkin_ws/src/beginner_tutorials/src

 talker.cpp 

#include "ros/ros.h"
#include "std_msgs/String.h"

#include 
#include    
#include "geometry_msgs/Twist.h" 


int main(int argc, char **argv)
{
  
  ros::init(argc, argv, "talker");

  
  ros::NodeHandle n;

  
  ros::Publisher chatter_pub = n.advertise("chatter", 1000);
  ros::Publisher turtle1_pub=n.advertise("/turtle1/cmd_vel",10); //topic


  ros::Rate loop_rate(10);

  
  int count = 1;
  while (ros::ok())
  {
    
    std_msgs::String msg;

    std::stringstream ss;
    ss << "hello world from talker" << count;
    
    msg.data = ss.str();

    ROS_INFO("%s", msg.data.c_str());

    
    chatter_pub.publish(msg);
    geometry_msgs::Twist turtle_msg;

    turtle_msg.linear.x = 1.0;  //前后（+ -） m/s
    turtle_msg.linear.y = 0.0;  //左右（+-） m/s
    turtle_msg.linear.z = 0.0;
 
    turtle_msg.angular.x = 0;
    turtle_msg.angular.y = 0;
    turtle_msg.angular.z = 0.99; //机器人的自转速度，+左转，-右转，单位是rad/s
    turtle1_pub.publish(turtle_msg);
    ROS_INFO("publish turtle run command[%0.2f m/s,%0.2f rad/s]", turtle_msg.linear.x, turtle_msg.angular.z);


    ros::spinonce();

    loop_rate.sleep();
    ++count;
  }


  return 0;
}

 listener.cpp 

#include "ros/ros.h"
#include "std_msgs/String.h"

#include "turtlesim/Pose.h"

void chatterCallback(const std_msgs::String::ConstPtr& msg)
{
  ROS_INFO("I heard: [%s]", msg->data.c_str());
}

void turtleCallback(const turtlesim::Pose::ConstPtr&  msg)
{
  ROS_INFO("turtle pose :x=%.2f,y=%.2f,linear=%.2f,angular=%.2f,theta=%.2f",
        msg->x,msg->y,msg->linear_velocity,msg->angular_velocity,msg->theta);
}

int main(int argc, char **argv)
{
  
  ros::init(argc, argv, "listener");

  
  ros::NodeHandle n;

  
  ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
  ros::Subscriber turtle_sub = n.subscribe("/turtle1/pose", 100, turtleCallback);


  

  ros::MultiThreadedSpinner spinner(4); // Use 4 threads
  ros::spin();

  return 0;
}

程序包（~/catkin_ws/src/beginner_tutorials）的 CMakeLists.txt文件，修改如下（最后两行，要不要都行）

 返回工作空间编译

catkin_make 

启动小乌龟

roscore
rosrun turtlesim turtlesim_node

同样在工作空间分别开启两个控制台分别启动talker 和 listener

source ./devel/setup.bash

rosrun beginner_tutorials talker 

source ./devel/setup.bash

rosrun beginner_tutorials listener

测试效果如下图，

实现原理:

启动手动控制小乌龟游戏 ，方法见

ROS 安装教程Ubuntu16.04（2022年最新）_lhb0709的专栏-CSDN博客

teleop_turtle    是虚拟机键盘节点

turtlesim    是小乌龟节点

/turtle1/cmd_vel 是topic

依次使用命令查看turtle1/cmd_vel的类型和数据结构（代码中要用）

rostopic list
rostopic info /turtle1/cmd_vel
rosmsg info geometry_msgs/Twist

 依次使用命令查看turtle1/pose的类型和数据结构（代码中要用）

rostopic list
rostopic info /turtle1/cmd_vel
rosmsg info geometry_msgs/Twist

 Python 实现的位姿显示代码参考，会python和QT一看就懂

ROS实操入门系列(二）操控小乌龟时，监控行走的路线（第二章补充Topic干货！） - 古月居 (guyuehome.com)