机器人导航

一、半稠密点云图转换为二维栅格图

参考链接

https://blog.csdn.net/m0_60355964/article/details/129560899

根据我自己建立的半稠密点云图坐标定义问题，输出的栅格图是投影在xy上的，导致地图不能使用，要将其投影到xz轴上
所以将/home/ybl/pcd_to_pgm_ws/src/pcd2pgm_package/pcd2pgm/src里面的pcd2pgm.cpp文件中的所有关于y,z坐标定义互换，将.launch文件中的z轴改为y轴。
然后进行编译

cd pcd_to_pgm_ws
catkin_make

生成二维栅格图map.pgm和相关文件map.yaml：

cd pcd_to_pgm_ws
source devel/setup.bash
roslaunch pcd2pgm run.launch

rosrun map_server map_saver

二、使用rviz显示move_base所要用到的输入信息

         首先建立tf树，其实就是建立各个坐标系之间的联系，我自己的机器人所涉及到的坐标系有：map

        odom（足式里程计还没有具体写，暂时用gazebo插件libgazebo_ros_p3d.so代替）

        camera_link（其实模型中是在laser_link上安装的camera，当时建机器人模型的问题）

        platform_link

        path

        查看tf树：

        rosrun rqt_tf_tree rqt_tf_tree

        tf关系树为：

具体实现是用以下代码（后续考虑将其写为.launch文件）

rosrun tf static_transform_publisher 0 0 0 0 0 0 1 map odom 5
rosrun tf static_transform_publisher 0 0 0 0 0 0 1 odom platform_link 5
rosrun tf static_transform_publisher 0 0 0.215 0 3.14 -1.57 platform_link camera_link 5
rosrun tf static_transform_publisher 0 0 0 0 0 0 1 platform_link path 5




rosrun tf static_transform_publisher 0 0 -0.215 3.14 0 1.57 camera_link platform_link 5
rosrun tf static_transform_publisher 0 0 0 0 0 0 1 camera_link path 5

由于camera_link相对于map是动态变化的，所以需要用到动态tf变换。/map话题在转换栅格图时发出，/ORB_SLAM/pose话题是由ORB-SLAM2发出的，编写以下程序用来发布和订阅地图与相机位姿之间的转换关系：

参考：[ROS 系列学习教程] ROS TF坐标变换 - 动态坐标变换_tf::transformpose 坐标系变换文件在哪-CSDN博客

dynamic_frame_broadcast.cpp

/**
 * @file: dynamic_frame_broadcast.cpp
 * @brief: 动态的坐标系相对姿态发布
 * @author: 万俟淋曦(1055311345@qq.com)
 * @date: 2023-12-30 22:47:33
 * @modifier:
 * @date: 2023-12-30 22:47:33
 */

#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "geometry_msgs/PoseStamped.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"

void turtle1PoseCallback(const geometry_msgs::PoseStamped::ConstPtr &pose)
{
    // 创建 TF 广播器
    static tf2_ros::TransformBroadcaster broadcaster;

    // 创建 广播的数据
    geometry_msgs::TransformStamped tfs;
    // --头设置
    tfs.header.frame_id = "map";
    tfs.header.stamp = ros::Time::now();
    // --坐标系id
    tfs.child_frame_id = "camera_link";
    // --坐标系相对信息设置
    tfs.transform.translation.x = pose->pose.position.x;
    tfs.transform.translation.y = pose->pose.position.y;
    tfs.transform.translation.z = 0.0; // 二维, z为0
    //  --欧拉角转四元数
    //tf2::Quaternion qtn;
    //qtn.setRPY(0, 0, pose->theta); // 二维, 只有偏航角
    //tfs.transform.rotation.x = qtn.getX();
    //tfs.transform.rotation.y = qtn.getY();
    //tfs.transform.rotation.z = qtn.getZ();
    //tfs.transform.rotation.w = qtn.getW();
    tfs.transform.rotation.x = pose->pose.orientation.x;
    tfs.transform.rotation.y = pose->pose.orientation.y;
    tfs.transform.rotation.z = pose->pose.orientation.z;
    tfs.transform.rotation.w = pose->pose.orientation.w;

    // 广播器发布数据
    broadcaster.sendTransform(tfs);
}

int main(int argc, char **argv)
{
    // 初始化 ROS 节点
    ros::init(argc, argv, "dynamic_frame_broadcast");

    // 创建 ROS 句柄
    ros::NodeHandle nh;

    // 创建订阅对象，订阅乌龟的世界位姿
    ros::Subscriber sub = nh.subscribe<geometry_msgs::PoseStamped>("/ORB_SLAM/pose", 1000, turtle1PoseCallback);

    ros::spin();

    return 0;
}

dynamic_frame_listen.cpp

/**
 * @file: dynamic_frame_listen.cpp
 * @brief: 订阅动态坐标系并转换相应坐标
 * @author: 万俟淋曦(1055311345@qq.com)
 * @date: 2023-12-31 11:55:40
 * @modifier:
 * @date: 2023-12-31 11:55:40
 */
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "geometry_msgs/PoseStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h" // 包含TF坐标转换方法

int main(int argc, char **argv)
{
    // 初始化 ROS 节点
    ros::init(argc, argv, "dynamic_frame_listen");
    ros::NodeHandle nh;

    // 创建 TF 订阅节点
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);

    ros::Rate r(1);
    while (ros::ok())
    {
        // 生成一个坐标点, 模拟末端执行器坐标系下的点坐标(小乌龟坐标系下的坐标)
        geometry_msgs::PointStamped point_turtle1;
        point_turtle1.header.frame_id = "camera_link";
        point_turtle1.header.stamp = ros::Time();
        point_turtle1.point.x = 1;
        point_turtle1.point.y = 1;
        point_turtle1.point.z = 0;

        // 转换坐标点, 计算小乌龟坐标系下的坐标点在 world 下的坐标
        try
        {
            geometry_msgs::PointStamped point_base;
            point_base = buffer.transform(point_turtle1, "map");
            ROS_INFO("point_base: (%.2f, %.2f, %.2f), frame: %s", 
                point_base.point.x, 
                point_base.point.y, 
                point_base.point.z,
                point_base.header.frame_id.c_str());
        }
        catch (const std::exception &e)
        {
            ROS_ERROR("%s", e.what());
        }

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

    return 0;
}

然后，在tf目录下编译并运行：

catkin_make

source devel/setup.bash

rosrun tf2_learning tf2_learning_dynamic_broadcast

转换关系得以发布，可在rviz里查看。

修正TF树

~/tf$ rosrun tf2_learning tf2_learning_dynamic_broadcast
rosrun tf static_transform_publisher 0 0 0 0 0 0 1 odom camera_link 5
rosrun tf static_transform_publisher 0 0 -0.215 3.14 0 1.57 camera_link platform_link 5
rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x:, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0}}'//模拟小车运动信息

成功实现，具体打开顺序为：

~/Robot_ws$ roslaunch test_model stewart_new_shinei.launch
~/demo06_ws$ rosrun plumbing_pub_sub main
~/ORB-SLAM2_RGBD_DENSE_MAP-master$ rosrun ORB_SLAM21 RGBD Vocabulary/ORBvoc.bin Examples/ROS/ORB_SLAM21/model.yaml
~/pcd2pgm_ws$ rosrun pcd2pgm pcd2topic
~/pcd2pgm_ws$ rosrun map_server map_saver
~/tf$ rosrun tf2_learning tf2_learning_dynamic_broadcast
~/tf$ rosrun tf static_transform_publisher 0 0 0 0 0 0 1 odom camera_link 5
~/tf$ rosrun tf static_transform_publisher 0 0 -0.215 3.14 0 1.57 camera_link platform_link 5
~/tf$ rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x:, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0}}'
~/tf$ rostopic pub -r 10 /odom nav_msgs/Odometry '{pose: {pose: {position: {x: 0, y: 0, z: 0}, orientation: {x: 0, y: 0, z: 0, w: 0}}}, twist: {twist: {linear: {x: 0, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0}}}}'
~/catkin_ws$ roslaunch turtlebot3_navigation turtlebot3_navigation.launch 
~$ rosrun rqt_tf_tree rqt_tf_tree

turtlebot3_navigation.launch文件具体内容：

<launch>
  <!-- Arguments -->
  <arg name="model" default="burger" doc="model type [burger, waffle, waffle_pi]"/>
  <arg name="map_file" default="$(find turtlebot3_navigation)/maps/map.yaml"/>
  <arg name="open_rviz" default="true"/>
  <arg name="move_forward_only" default="false"/>

<!-- 深度图转激光雷达 -->
  <node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan">
      <remap from="image" to="/camera/depth/image_raw" />
      <param name="output_frame_id" value="camera_link"  />
  </node>

  <!-- Map server -->
  <node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)"/>

  <!-- AMCL -->
  <include file="$(find turtlebot3_navigation)/launch/amcl.launch"/>

  <!-- move_base -->
  <include file="$(find turtlebot3_navigation)/launch/move_base.launch">
    <arg name="model" value="$(arg model)" />
    <arg name="move_forward_only" value="$(arg move_forward_only)"/>
  </include>

  <!-- 设置一个/odom与/map之间的静态坐标变换 -->
    <node pkg="tf" type="static_transform_publisher" name="map_odom_broadcaster" args="0 0 0 0 0 0 /map /odom 100" />

  <!-- rviz -->
  <group if="$(arg open_rviz)">
    <node pkg="rviz" type="rviz" name="rviz" required="true"
          args="-d $(find turtlebot3_navigation)/rviz/turtlebot3_navigation.rviz"/>
  </group>
</launch>

将默认的给定里程计odom换为视觉里程计，重新跑turtlebot3_navigation.launch

~/Robot_ws$ roslaunch test_model stewart_new_shinei.launch
~/demo06_ws$ rosrun plumbing_pub_sub main

~/ORB-SLAM2_RGBD_DENSE_MAP-master$ rosrun ORB_SLAM21 RGBD Vocabulary/ORBvoc.bin Examples/ROS/ORB_SLAM21/model.yaml

~/pcd2pgm_ws$ rosrun pcd2pgm pcd2topic
~/pcd2pgm_ws$ rosrun map_server map_saver


~/tf$ rosrun tf2_learning tf2_learning_pose_odom 
~/tf$ rosrun tf2_learning tf2_learning_odom_dynamic_broadcast


~/tf$ rosrun tf static_transform_publisher 0 0 0 0 0 0 1 odom camera_link 5
~/tf$ rosrun tf static_transform_publisher 0 0 -0.215 3.14 0 1.57 camera_link platform_link 5


~/tf$ rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x:, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0}}'


~/catkin_ws$ roslaunch turtlebot3_navigation turtlebot3_navigation.launch