【从零开始学习SLAM】分解ros小海龟跟随代码

参考教程

「ROS Kinetic」官方tf教程小海龟跟随程序原理解读
这是官方教程，但是太过于封装，所以我这里分布实现，便于理解tf的工作原理。

转换坐标点和跟随程序

turtle_point_follow.cpp

//1.包含头文件
#include "ros/ros.h"
#include "geometry_msgs/PointStamped.h"
#include <tf/transform_listener.h>

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"dynamic_tf_sub");
    ros::NodeHandle nh;


    tf::StampedTransform transform_tf;
    tf::TransformListener listener_tf(ros::Duration(10));


    ros::Publisher turtle_vel = nh.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);

    ros::Rate rate(1);
    while (ros::ok())
    {
    // 4.生成一个坐标点(相对于子级坐标系)
        geometry_msgs::PointStamped turtle1_eye_point;
        turtle1_eye_point.header.frame_id = "turtle1_eye";
        turtle1_eye_point.header.stamp = ros::Time();
        turtle1_eye_point.point.x = 0.1;
        turtle1_eye_point.point.y = 0;
        turtle1_eye_point.point.z = 0.1;
    // 5.转换坐标点(相对于父级坐标系)
        //新建一个坐标点，用于接收转换结果  
        //--------------使用 try 语句或休眠，否则可能由于缓存接收延迟而导致坐标转换失败------------------------
        try
        {
            try{
              listener_tf.waitForTransform("/turtle1", "/turtle1_eye", ros::Time(), ros::Duration(10.0) );
              listener_tf.lookupTransform("/turtle1", "/turtle1_eye", ros::Time(), transform_tf);
              ROS_INFO("子坐标系%s相对于父坐标系%s的坐标关系(%.2f,%.2f,%.2f)",transform_tf.child_frame_id_.c_str(),transform_tf.frame_id_.c_str(),transform_tf.getOrigin().x(), transform_tf.getOrigin().y(), transform_tf.getOrigin().z());
            }
            catch (tf::TransformException &ex) {
              ROS_ERROR("%s",ex.what());
            }

            try{
              listener_tf.waitForTransform("/turtle2", "/turtle1", ros::Time(), ros::Duration(10.0) );
              listener_tf.lookupTransform("/turtle2", "/turtle1", ros::Time(), transform_tf);
              ROS_INFO("子坐标系%s相对于父坐标系%s的坐标关系(%.2f,%.2f,%.2f)",transform_tf.child_frame_id_.c_str(),transform_tf.frame_id_.c_str(),transform_tf.getOrigin().x(), transform_tf.getOrigin().y(), transform_tf.getOrigin().z());
              // turtle2跟随turtle1在动
              geometry_msgs::Twist vel_msg;
              vel_msg.angular.z =  atan2(transform_tf.getOrigin().y(), transform_tf.getOrigin().x());
              vel_msg.linear.x =  sqrt(pow(transform_tf.getOrigin().x(), 2) + pow(transform_tf.getOrigin().y(), 2));
              turtle_vel.publish(vel_msg);
            }
            catch (tf::TransformException &ex) {
              ROS_ERROR("%s",ex.what());
            }

            try{
              listener_tf.waitForTransform("/turtle2", "/turtle1_eye", ros::Time(), ros::Duration(10.0) );
              listener_tf.lookupTransform("/turtle2", "/turtle1_eye", ros::Time(), transform_tf);
              ROS_INFO("子坐标系%s相对于父坐标系%s的坐标关系(%.2f,%.2f,%.2f)",transform_tf.child_frame_id_.c_str(),transform_tf.frame_id_.c_str(),transform_tf.getOrigin().x(), transform_tf.getOrigin().y(), transform_tf.getOrigin().z());
            }
            catch (tf::TransformException &ex) {
              ROS_ERROR("%s",ex.what());
            }

            /**
             * 子坐标系turtle1_eye相对于父坐标系turtle1的坐标关系(0.05,0.00,0.10) +
             * 子坐标系turtle1相对于父坐标系turtle2的坐标关系(7.67,5.54,0.00) =
             * 子坐标系turtle1_eye相对于父坐标系turtle2的坐标关系(7.72,5.54,0.10)
             * 
             * turtle1_eye坐标系下的坐标点相对于turtle1_eye坐标系的坐标为:(0.10,0.00,0.10) + 
             * 子坐标系turtle1_eye相对于父坐标系turtle2的坐标关系(7.72,5.54,0.10) = 
             * turtle1_eye坐标系下的坐标点相对于turtle2坐标系的坐标为:(7.82,5.54,0.20)
             */
            

            geometry_msgs::PointStamped turtle2_point;
            // tf的写法
            listener_tf.transformPoint("turtle2", turtle1_eye_point, turtle2_point);
            ROS_INFO("turtle1_eye坐标系下的坐标点相对于turtle1_eye坐标系的坐标为:(%.2f,%.2f,%.2f)",turtle1_eye_point.point.x,turtle1_eye_point.point.y,turtle1_eye_point.point.z);
            ROS_INFO("turtle1_eye坐标系下的坐标点相对于turtle2坐标系的坐标为:(%.2f,%.2f,%.2f)",turtle2_point.point.x,turtle2_point.point.y,turtle2_point.point.z);


            try{
              listener_tf.waitForTransform("/turtle2_eye", "/turtle2", ros::Time(), ros::Duration(10.0) );
              listener_tf.lookupTransform("/turtle2_eye", "/turtle2", ros::Time(), transform_tf);
              ROS_INFO("子坐标系%s相对于父坐标系%s的坐标关系(%.2f,%.2f,%.2f)",transform_tf.child_frame_id_.c_str(),transform_tf.frame_id_.c_str(),transform_tf.getOrigin().x(), transform_tf.getOrigin().y(), transform_tf.getOrigin().z());
            }
            catch (tf::TransformException &ex) {
              ROS_ERROR("%s",ex.what());
            }

             /**
             * turtle1_eye坐标系下的坐标点相对于turtle1_eye坐标系的坐标为:(0.10,0.00,0.10) +
             * 子坐标系turtle1_eye相对于父坐标系turtle1的坐标关系(0.05,0.00,0.10) + 
             * 子坐标系turtle1相对于父坐标系turtle2的坐标关系(5.59,5.54,0.00) -
             * 子坐标系turtle2相对于父坐标系turtle2_eye的坐标关系(-0.05,0.00,-0.10) =
             * turtle1_eye坐标系下的坐标点相对于turtle2_eye坐标系的坐标为:(5.69,5.54,0.10)
             */

            geometry_msgs::PointStamped turtle2_eye_point;
            // tf的写法
            listener_tf.transformPoint("turtle2_eye", turtle1_eye_point, turtle2_eye_point);
            ROS_INFO("turtle1_eye坐标系下的坐标点相对于turtle1_eye坐标系的坐标为:(%.2f,%.2f,%.2f)",turtle1_eye_point.point.x,turtle1_eye_point.point.y,turtle1_eye_point.point.z);
            ROS_INFO("turtle1_eye坐标系下的坐标点相对于turtle2_eye坐标系的坐标为:(%.2f,%.2f,%.2f)",turtle2_eye_point.point.x,turtle2_eye_point.point.y,turtle2_eye_point.point.z);




        }
        catch(const std::exception& e)
        {
            // std::cerr << e.what() << '\n';
            ROS_INFO("程序异常:%s",e.what());
        }

        rate.sleep();  
        ros::spinOnce();
    }


    return 0;
}

坐标系发布程序

msg:geometry_msgs/TransformStamped表示坐标系的变换关系
geometry_msgs/PointStamped表示坐标系内坐标点

turtle_world_eye_tf.cpp

接收位置，发布坐标系之间的关系

#include <ros/ros.h>
#include <geometry_msgs/PointStamped.h>
#include <tf/transform_listener.h>
#include "turtlesim/Pose.h"
#include "tf/transform_broadcaster.h"
// #include "geometry_msgs/TransformStamped.h"
// #include "tf/LinearMath/Quaternion.h"


// 获取turtle1的位姿打印在屏幕上然后发布turtle1坐标系相对于world坐标系的tf
void turtle1_pose(const turtlesim::Pose::ConstPtr & pose){
    ROS_INFO("turtle1的坐标：(%.2f,　%.2f),　朝向rad：%.2f,　线速度m/s：%.2f,　角速度rad/s：%.2f", pose->x, pose->y, pose->theta, pose->linear_velocity, pose->angular_velocity);  
    static tf::TransformBroadcaster broadcaster;
    geometry_msgs::TransformStamped tf;
    tf.header.frame_id = "world";
    tf.header.stamp = ros::Time::now();

    tf.child_frame_id = "turtle1";
    tf.transform.translation.x = pose->x;
    tf.transform.translation.y = pose->y;

    tf::Quaternion quaternion;
    quaternion.setRPY(0, 0, pose->theta);
    tf.transform.rotation.x = quaternion.getX(); 
    tf.transform.rotation.y = quaternion.getY(); 
    tf.transform.rotation.z = quaternion.getZ(); 
    tf.transform.rotation.w = quaternion.getW(); 

    broadcaster.sendTransform(tf);
    ROS_INFO("发布turtle1相对于world的坐标系关系");

    // turtle1的眼睛，也就是相机之间是静态坐标系关系，二者相对位置不会改变
    broadcaster.sendTransform(tf::StampedTransform(tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.05, 0.0, 0.1)), ros::Time::now(),"turtle1", "turtle1_eye"));
    ROS_INFO("发布相机相对于turtle1的坐标系关系");
}

// 获取turtle2的位姿打印在屏幕上然后发布turtle2坐标系相对于world坐标系的tf
void turtle2_pose(const turtlesim::Pose::ConstPtr & pose){
    ROS_INFO("turtle2的坐标：(%.2f,　%.2f),　朝向rad：%.2f,　线速度m/s：%.2f,　角速度rad/s：%.2f", pose->x, pose->y, pose->theta, pose->linear_velocity, pose->angular_velocity);
    static tf::TransformBroadcaster broadcaster;
    geometry_msgs::TransformStamped tf;

    tf.header.frame_id = "world";
    tf.header.stamp = ros::Time::now();

    tf.child_frame_id = "turtle2";
    tf.transform.translation.x = pose->x;
    tf.transform.translation.y = pose->y;

    tf::Quaternion quaternion;
    quaternion.setRPY(0, 0, pose->theta);
    tf.transform.rotation.x = quaternion.getX(); 
    tf.transform.rotation.y = quaternion.getY(); 
    tf.transform.rotation.z = quaternion.getZ(); 
    tf.transform.rotation.w = quaternion.getW(); 


    broadcaster.sendTransform(tf);
    ROS_INFO("发布turtle2相对于world的坐标系关系");

    // turtle2的眼睛，也就是相机之间是静态坐标系关系，二者相对位置不会改变
    broadcaster.sendTransform(tf::StampedTransform(tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.05, 0.0, 0.1)), ros::Time::now(),"turtle2", "turtle2_eye"));
    ROS_INFO("发布相机相对于turtle2的坐标系关系");
}

int main(int argc, char** argv){
  setlocale(LC_ALL,"");
  ros::init(argc, argv, "turtle_tf");
  ros::NodeHandle nh;

  // 订阅turtle1在world下的位置姿态，然后发布turtle1相对world的坐标关系
  ros::Subscriber turtle1_sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose", 100, turtle1_pose);

  // 订阅turtle1在world下的位置姿态，然后发布turtle２相对world的坐标关系
  ros::Subscriber turtle2_sub = nh.subscribe<turtlesim::Pose>("/turtle2/pose", 100, turtle2_pose);

  ros::spin();
}

速度和位置姿态发布程序

官方海龟运动流程

1. 启动ros
2. 启动海龟仿真图形化界面默认生成一只海龟
3. 启动键盘控制，通过键盘方向键控制海龟运动
   

rqt_graph查看节点和消息

查看发布的话题和运动控制消息的信息

查看运动控制消息的内容

弧度：单位弧度定义为圆弧长度等于半径时的圆心角

官方小海龟教程发布的位置姿态消息

turtle_pose_twist.cpp

#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "turtlesim/Pose.h"

void pub_pose(const turtlesim::Pose::ConstPtr &pose){
    ROS_INFO("小海龟的坐标：(%.2f,　%.2f),　朝向rad：%.2f,　线速度m/s：%.2f,　角速度rad/s：%.2f", pose->x, pose->y, pose->theta, pose->linear_velocity, pose->angular_velocity);
}

int main(int argc, char *argv[]){
    // 中文
    setlocale(LC_ALL,"");
    ros::init(argc, argv, "twist_pose");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("/turtle1/pose", 10, pub_pose);
    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);
    ros::Rate rate(10);
    geometry_msgs::Twist twist;
    // 直线运动
    twist.linear.x = 0.01;
    // 圆周运动
    // twist.linear.x = 0.1;
    // twist.angular.z = 0.1;
    while (ros::ok()){
        pub.publish(twist);
        ROS_INFO("小海龟的速度：(%.2f,　%.2f)",twist.linear.x, twist.angular.z);
        rate.sleep();
        ros::spinOnce();
    }
}

小海龟生成程序

大佬图文教程

turtle_spawn.cpp

// 1.包含头文件
#include "ros/ros.h"
#include "turtlesim/Spawn.h"

int main(int argc, char *argv[]){
    setlocale(LC_ALL,"");
    // 2.初始化 ros 节点
    ros::init(argc,argv,"turtle_spawn");
    // 3.创建 ros 句柄
    ros::NodeHandle nh;
    // 4.创建 service 客户端
    ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn");
    // 5.等待服务启动
    // client.waitForExistence();
    ros::service::waitForService("/spawn");
    // 6.发送请求
    turtlesim::Spawn spawn;
    // 位置
    spawn.request.x = 0.0;
    spawn.request.y = 0.0;
    // 朝向
    spawn.request.theta = 0;
    // 名字
    spawn.request.name = "turtle2";
    bool flag = client.call(spawn);
    // 7.处理响应结果
    if (flag){
        ROS_INFO("成功创建一只海龟名字：%s",spawn.response.name.c_str());
    } 
    else {
        ROS_INFO("海龟生成失败");
    }
    return 0;
}

功能包依赖文件package.xml

<?xml version="1.0"?>
<package format="2">
  <name>learning_tf</name>
  <version>0.0.0</version>
  <description>The learning_tf package</description>

  <maintainer email="q@todo.todo">q</maintainer>

  <license>TODO</license>

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>tf</build_depend>
  <build_depend>turtlesim</build_depend>
  <build_depend>geometry_msgs</build_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>rospy</build_export_depend>
  <build_export_depend>tf</build_export_depend>
  <build_export_depend>turtlesim</build_export_depend>
  <build_export_depend>geometry_msgs</build_export_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>rospy</exec_depend>
  <exec_depend>tf</exec_depend>
  <exec_depend>turtlesim</exec_depend>
  <exec_depend>geometry_msgs</exec_depend>


  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- Other tools can request additional information be placed here -->

  </export>
</package>

编译文件CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(learning_tf)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  tf
  turtlesim
  geometry_msgs
)

catkin_package(
)


include_directories(
  ${catkin_INCLUDE_DIRS}
)

# roscore
# rosrun turtlesim turtlesim_node

# 发布速度消息
add_executable(turtle_pose_twist src/turtle_pose_twist.cpp)
target_link_libraries(turtle_pose_twist ${catkin_LIBRARIES})

# 创造第二只海龟
add_executable(turtle_spawn src/turtle_spawn.cpp)
target_link_libraries(turtle_spawn ${catkin_LIBRARIES})


# １、订阅小海龟的实时位置姿态
# ２、解析小海龟的实时位置姿态
# ３、发布小海龟相对于世界坐标系之间的tf
# ４、发布相机相对于小海龟坐标系之间的tf
# ５、将相机坐标系下面的点通过tf变换到小海龟１坐标系下、变换到小海龟２坐标系下、变换到世界坐标系下
add_executable(turtle_world_eye_tf src/turtle_world_eye_tf.cpp)
target_link_libraries(turtle_world_eye_tf ${catkin_LIBRARIES})

add_executable(turtle_point_follow src/turtle_point_follow.cpp)
target_link_libraries(turtle_point_follow ${catkin_LIBRARIES})

执行流程

catkin_ws_tf$roscore

source devel/setup.bash
catkin_ws_tf$rosrun turtlesim turtlesim_node

source devel/setup.bash
catkin_ws_tf$rosrun learning_tf turtle_spawn

source devel/setup.bash
catkin_ws_tf$rosrun learning_tf turtle_pose_twist

source devel/setup.bash
catkin_ws_tf$rosrun learning_tf turtle_world_eye_tf

source devel/setup.bash
catkin_ws_tf$rosrun learning_tf turtle_point_follow