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 <sstream>
#include <string>   
#include "geometry_msgs/Twist.h" 

/**
 * This tutorial demonstrates simple sending of messages over the ROS system.
 */
int main(int argc, char **argv)
{
  /**
   * The ros::init() function needs to see argc and argv so that it can perform
   * any ROS arguments and name remapping that were provided at the command line. For programmatic
   * remappings you can use a different version of init() which takes remappings
   * directly, but for most command-line programs, passing argc and argv is the easiest
   * way to do it.  The third argument to init() is the name of the node.
   *
   * You must call one of the versions of ros::init() before using any other
   * part of the ROS system.
   */
  ros::init(argc, argv, "talker");

  /**
   * NodeHandle is the main access point to communications with the ROS system.
   * The first NodeHandle constructed will fully initialize this node, and the last
   * NodeHandle destructed will close down the node.
   */
  ros::NodeHandle n;

  /**
   * The advertise() function is how you tell ROS that you want to
   * publish on a given topic name. This invokes a call to the ROS
   * master node, which keeps a registry of who is publishing and who
   * is subscribing. After this advertise() call is made, the master
   * node will notify anyone who is trying to subscribe to this topic name,
   * and they will in turn negotiate a peer-to-peer connection with this
   * node.  advertise() returns a Publisher object which allows you to
   * publish messages on that topic through a call to publish().  Once
   * all copies of the returned Publisher object are destroyed, the topic
   * will be automatically unadvertised.
   *
   * The second parameter to advertise() is the size of the message queue
   * used for publishing messages.  If messages are published more quickly
   * than we can send them, the number here specifies how many messages to
   * buffer up before throwing some away.
   */
  ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
  ros::Publisher turtle1_pub=n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",10); //topic


  ros::Rate loop_rate(10);

  /**
   * A count of how many messages we have sent. This is used to create
   * a unique string for each message.
   */
  int count = 1;
  while (ros::ok())
  {
    /**
     * This is a message object. You stuff it with data, and then publish it.
     */
    std_msgs::String msg;

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

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

    /**
     * The publish() function is how you send messages. The parameter
     * is the message object. The type of this object must agree with the type
     * given as a template parameter to the advertise<>() call, as was done
     * in the constructor above.
     */
    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"
/**
 * This tutorial demonstrates simple receipt of messages over the ROS system.
 */
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)
{
  /**
   * The ros::init() function needs to see argc and argv so that it can perform
   * any ROS arguments and name remapping that were provided at the command line. For programmatic
   * remappings you can use a different version of init() which takes remappings
   * directly, but for most command-line programs, passing argc and argv is the easiest
   * way to do it.  The third argument to init() is the name of the node.
   *
   * You must call one of the versions of ros::init() before using any other
   * part of the ROS system.
   */
  ros::init(argc, argv, "listener");

  /**
   * NodeHandle is the main access point to communications with the ROS system.
   * The first NodeHandle constructed will fully initialize this node, and the last
   * NodeHandle destructed will close down the node.
   */
  ros::NodeHandle n;

  /**
   * The subscribe() call is how you tell ROS that you want to receive messages
   * on a given topic.  This invokes a call to the ROS
   * master node, which keeps a registry of who is publishing and who
   * is subscribing.  Messages are passed to a callback function, here
   * called chatterCallback.  subscribe() returns a Subscriber object that you
   * must hold on to until you want to unsubscribe.  When all copies of the Subscriber
   * object go out of scope, this callback will automatically be unsubscribed from
   * this topic.
   *
   * The second parameter to the subscribe() function is the size of the message
   * queue.  If messages are arriving faster than they are being processed, this
   * is the number of messages that will be buffered up before beginning to throw
   * away the oldest ones.
   */
  ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
  ros::Subscriber turtle_sub = n.subscribe<turtlesim::Pose>("/turtle1/pose", 100, turtleCallback);


  /**
   * ros::spin() will enter a loop, pumping callbacks.  With this version, all
   * callbacks will be called from within this thread (the main one).  ros::spin()
   * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
   */

  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/pose
rosmsg info turtlesim/Pose

 

 Python 实现的位姿显示代码参考，同时会python和QT，几分钟就看懂了

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