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

本文从下面博客改编而来

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

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

mkdir -p ~/catkin_ws/srccd ~/catkin_ws/srccatkin_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

启动小乌龟

roscorerosrun turtlesim turtlesim_node

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

source ./devel/setup.bashrosrun beginner_tutorials talker

 

source ./devel/setup.bashrosrun beginner_tutorials listener

 

测试效果如下图，

 

 

实现原理:

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

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

teleop_turtle    是虚拟机键盘节点

turtlesim    是小乌龟节点

/turtle1/cmd_vel 是topic

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

rostopic listrostopic info /turtle1/cmd_velrosmsg info geometry_msgs/Twist

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

rostopic listrostopic info /turtle1/cmd_velrosmsg info geometry_msgs/Twist

 

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

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