多机编队—(3)Fast_planner无人机模型替换为Turtlebot3模型实现无地图的轨迹规划

文章目录

前言
一、模型替换
二、Riz可视化
三、坐标变换
四、轨迹规划
最后

前言

前段时间已经成功将Fast_planner配置到ubuntu机器人中，这段时间将Fast_planner中的无人机模型替换为了Turtlebot3_waffle模型，机器人识别到环境中的三维障碍物信息，并完成无地图的轨迹规划。

一、模型替换

先把turtlebot3功能包拷贝到配置成功的Fast_planner工作空间下，turtlebot3中含有turtlebot3_description功能包，里面包含了turtlebot3所有的模型文件。

普通在gazebo中加载turtlebot3的模型文件如图所示：

在这里插入图片描述
但Fast_planner需要环境中的三维信息，以进行路径的规划，故这里需要将2D激光雷达替换为3D激光雷达，如下所示在turtlebot3_waffle.urdf.xacro文件中加入VLP-16激光雷达，以获取环境中的三维信息。

<?xml version="1.0" ?>
<robot name="turtlebot3_waffle" xmlns:xacro="http://ros.org/wiki/xacro"><xacro:include filename="$(find turtlebot3_description)/urdf/common_properties.xacro"/><xacro:include filename="$(find turtlebot3_description)/urdf/turtlebot3_waffle.gazebo.xacro"/><xacro:property name="support_length" value="0.3" />      <!--0.3--><xacro:arg name="gpu" default="false"/><xacro:property name="gpu" value="$(arg gpu)" /><xacro:arg name="organize_cloud" default="true"/><xacro:property name="organize_cloud" value="$(arg organize_cloud)" /><xacro:property name="base_z_size" value="0.122" />        <!--0.122--><xacro:include filename="$(find turtlebot3_description)/urdf/sensors/inertial.xacro" /><xacro:include filename="$(find turtlebot3_description)/urdf/sensors/laser_support.xacro" /><xacro:include filename="$(find velodyne_description)/urdf/VLP-16.urdf.xacro"/><xacro:VLP-16 parent="support" name="velodyne" topic="/velodyne_points" organize_cloud="${organize_cloud}" hz="10" samples="400" gpu="${gpu}"><origin xyz="0 0 ${support_length/4}" rpy="0 0 0" /> </xacro:VLP-16><xacro:include filename="$(find turtlebot3_description)/urdf/sensors/imu.xacro"/><xacro:imu sensor_name="imu" parent_link="base_link"><origin rpy="0 0 0" xyz="-0.064 0 ${base_z_size/2}"/> </xacro:imu>

其中的关键点为（上面代码也已展现出）：需要将相关的模型加载文件放在设置的路径下。

然后在gazebo中加载新模型：

在这里插入图片描述

二、Riz可视化

<launch><node name="rvizvisualisation" pkg="rviz" type="rviz" output="log" args="-d $(find plan_manage)/config/traj.rviz" />
</launch>

在这里插入图片描述

三、坐标变换

在模型加装VLP-16激光雷达时设置了雷达的话题：topic=“/velodyne_points”，为使雷达能得到地图中障碍物信息。

写下面的一段代码实现订阅激光雷达（如Velodyne）产生的点云数据，并将这些点云从base_link的坐标系转换到odom坐标系。转换后的点云数据再发布出去，以供其他节点使用。

#include <ros/ros.h>
#include <sensor_msgs/PointCloud.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <tf2/transform_datatypes.h>ros::Publisher points_pub;
tf2_ros::Buffer tfBuffer;void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr &msg) {try {geometry_msgs::TransformStamped transformStamped = tfBuffer.lookupTransform("odom", "base_link", ros::Time(0));sensor_msgs::PointCloud2 transformed_cloud;tf2::doTransform(*msg, transformed_cloud, transformStamped);transformed_cloud.header.frame_id = "odom";points_pub.publish(transformed_cloud);} catch(tf2::TransformException &e) {ROS_WARN("Failed to transform point cloud: %s", e.what());}}int main(int argc, char **argv) 
{ros::init(argc, argv, "point_cloud_transform_node");ros::NodeHandle nh;ros::Subscriber points_sub = nh.subscribe("/velodyne_points", 10, pointCloudCallback);points_pub = nh.advertise<sensor_msgs::PointCloud2>("point_cloud_map", 10);tf2_ros::TransformListener tfListener(tfBuffer);ros::spin();return 0;
}

在这里插入图片描述

四、轨迹规划

该部分借助Fast_planner来实现，前端通过混合A*生成初始路径，后端在对其进行B样条优化和重规划。
为使Turtlebot3使用下面是配置的接口文件：

<launch><!-- size of map, change the size in x, y, z according to your application --><arg name="map_size_x" value="40.0"/><arg name="map_size_y" value="40.0"/><arg name="map_size_z" value="0.5 "/><!-- topic of your odometry such as VIO or LIO --><arg name="odom_topic" value="/odom" /><!-- main algorithm params --><include file="$(find plan_manage)/launch/lidar.xml"><arg name="map_size_x_" value="$(arg map_size_x)"/><arg name="map_size_y_" value="$(arg map_size_y)"/><arg name="map_size_z_" value="$(arg map_size_z)"/><arg name="odometry_topic" value="$(arg odom_topic)"/><!-- camera pose: transform of camera frame in the world frame --><!-- depth topic: depth image, 640x480 by default --><!-- don't set cloud_topic if you already set these ones! --><arg name="camera_pose_topic" value="/pcl_render_node/camera_pose"/><!--geometry_msgs::PoseStamped--><arg name="depth_topic" value="/pcl_render_node/depth"/><!-- topic of point cloud measurement, such as from LIDAR  --><!-- don't set camera pose and depth, if you already set this one! --><arg name="cloud_topic" value="/point_cloud_map"/><!-- intrinsic params of the depth camera --><arg name="cx" value="321.04638671875"/><arg name="cy" value="243.44969177246094"/><arg name="fx" value="387.229248046875"/><arg name="fy" value="387.229248046875"/><!-- maximum velocity and acceleration the drone will reach --><arg name="max_vel" value="3" /><arg name="max_acc" value="2" /><!-- 1: use 2D Nav Goal to select goal  --><!-- 2: use global waypoints below  --><arg name="flight_type" value="1" /><!-- global waypoints --><!-- If flight_type is set to 2, the drone will travel these waypoints one by one --><arg name="point_num" value="3" /><arg name="point0_x" value="2" /><arg name="point0_y" value="0" /><arg name="point0_z" value="0" /><!-- set more waypoints if you need --><arg name="point1_x" value="2" /><arg name="point1_y" value="-5" /><arg name="point1_z" value="0" /><arg name="point2_x" value="5.0" /><arg name="point2_y" value="-10.0" /><arg name="point2_z" value="0.0" /></include><!-- trajectory server --><node pkg="plan_manage" name="traj_server" type="traj_server" output="screen"><remap from="/position_cmd" to="/planning/pos_cmd"/><remap from="/odom_world" to="$(arg odom_topic)"/><param name="traj_server/time_forward" value="1.5" type="double"/></node><node pkg="waypoint_generator" name="waypoint_generator" type="waypoint_generator" output="screen"><remap from="~odom" to="$(arg odom_topic)"/>        <remap from="~goal" to="/move_base_simple/goal"/><remap from="~traj_start_trigger" to="/traj_start_trigger" /><param name="waypoint_type" value="manual-lonely-waypoint"/>    </node></launch>

lidar.xml

<launch><arg name="map_size_x_"/><arg name="map_size_y_"/><arg name="map_size_z_"/><arg name="odometry_topic"/><arg name="camera_pose_topic"/><arg name="depth_topic"/><arg name="cloud_topic"/><arg name="cx"/><arg name="cy"/><arg name="fx"/><arg name="fy"/><arg name="max_vel"/><arg name="max_acc"/><arg name="point_num"/><arg name="point0_x"/><arg name="point0_y"/><arg name="point0_z"/><arg name="point1_x"/><arg name="point1_y"/><arg name="point1_z"/><arg name="point2_x"/><arg name="point2_y"/><arg name="point2_z"/><arg name="flight_type"/><!-- main node --><node pkg="plan_manage" name="fast_planner_node" type="fast_planner_node" output="screen"><remap from="/odom_world" to="$(arg odometry_topic)"/><remap from="/sdf_map/odom" to="$(arg odometry_topic)"/><remap from="/sdf_map/cloud" to="$(arg cloud_topic)"/><remap from = "/sdf_map/pose"   to = "$(arg camera_pose_topic)"/> <remap from = "/sdf_map/depth" to = "$(arg depth_topic)"/><!-- replanning method --><param name="planner_node/planner" value="1" type="int"/><!-- planning fsm --><param name="fsm/flight_type" value="$(arg flight_type)" type="int"/><param name="fsm/thresh_replan" value="1.5" type="double"/><param name="fsm/thresh_no_replan" value="1.0" type="double"/><param name="fsm/waypoint_num" value="$(arg point_num)" type="int"/><param name="fsm/waypoint0_x" value="$(arg point0_x)" type="double"/><param name="fsm/waypoint0_y" value="$(arg point0_y)" type="double"/><param name="fsm/waypoint0_z" value="$(arg point0_z)" type="double"/><param name="fsm/waypoint1_x" value="$(arg point1_x)" type="double"/><param name="fsm/waypoint1_y" value="$(arg point1_y)" type="double"/><param name="fsm/waypoint1_z" value="$(arg point1_z)" type="double"/><param name="fsm/waypoint2_x" value="$(arg point2_x)" type="double"/><param name="fsm/waypoint2_y" value="$(arg point2_y)" type="double"/><param name="fsm/waypoint2_z" value="$(arg point2_z)" type="double"/><param name="sdf_map/resolution"      value="0.15" /> <param name="sdf_map/map_size_x"   value="$(arg map_size_x_)" /> <param name="sdf_map/map_size_y"   value="$(arg map_size_y_)" /> <param name="sdf_map/map_size_z"   value="$(arg map_size_z_)" /> <param name="sdf_map/local_update_range_x"  value="5.5" /> <param name="sdf_map/local_update_range_y"  value="5.5" /> <param name="sdf_map/local_update_range_z"  value="4.5" /> <param name="sdf_map/obstacles_inflation"     value="0.3" /> <!--<膨胀的半径  单位米>--><param name="sdf_map/local_bound_inflate"    value="0.0"/><param name="sdf_map/local_map_margin" value="10"/><param name="sdf_map/ground_height"        value="0"/><!-- camera parameter --><param name="sdf_map/cx" value="$(arg cx)"/><param name="sdf_map/cy" value="$(arg cy)"/><param name="sdf_map/fx" value="$(arg fx)"/><param name="sdf_map/fy" value="$(arg fy)"/><!-- depth filter --><param name="sdf_map/use_depth_filter" value="false"/><param name="sdf_map/depth_filter_tolerance" value="0.15"/><param name="sdf_map/depth_filter_maxdist"   value="5.0"/><!--5.0--><param name="sdf_map/depth_filter_mindist"   value="0.2"/><param name="sdf_map/depth_filter_margin"    value="2"/><param name="sdf_map/k_depth_scaling_factor" value="1000.0"/><param name="sdf_map/skip_pixel" value="2"/><!-- local fusion --><param name="sdf_map/p_hit"  value="0.65"/><param name="sdf_map/p_miss" value="0.35"/><param name="sdf_map/p_min"  value="0.12"/><param name="sdf_map/p_max"  value="0.90"/><param name="sdf_map/p_occ"  value="0.80"/><param name="sdf_map/min_ray_length" value="0.3"/><param name="sdf_map/max_ray_length" value="5.0"/><param name="sdf_map/esdf_slice_height" value="0.3"/><param name="sdf_map/visualization_truncate_height"   value="2.49"/><param name="sdf_map/virtual_ceil_height"   value="2.5"/><param name="sdf_map/show_occ_time"  value="false"/><param name="sdf_map/show_esdf_time" value="false"/><param name="sdf_map/pose_type"     value="1"/>  <param name="sdf_map/frame_id"      value="odom"/><!-- planner manager --><param name="manager/max_vel" value="$(arg max_vel)" type="double"/><param name="manager/max_acc" value="$(arg max_acc)" type="double"/><param name="manager/max_jerk" value="4" type="double"/><param name="manager/dynamic_environment" value="0" type="int"/><param name="manager/local_segment_length" value="12.0" type="double"/><param name="manager/clearance_threshold" value="0.2" type="double"/><param name="manager/control_points_distance" value="0.5" type="double"/><param name="manager/use_geometric_path" value="false" type="bool"/><param name="manager/use_kinodynamic_path" value="true" type="bool"/><param name="manager/use_topo_path" value="false" type="bool"/><param name="manager/use_optimization" value="true" type="bool"/><!-- kinodynamic path searching --><param name="search/max_tau" value="1.0" type="double"/><!--如果考虑对时间维度进行划分才设置--><param name="search/init_max_tau" value="0.8" type="double"/><param name="search/max_vel" value="$(arg max_vel)" type="double"/><param name="search/max_acc" value="$(arg max_acc)" type="double"/><param name="search/w_time" value="10.0" type="double"/><param name="search/horizon" value="5.0" type="double"/><!--限制全局规划的距离，保证实时性--><param name="search/lambda_heu" value="4.0" type="double"/><!--启发函数权重--><param name="search/resolution_astar" value="0.1" type="double"/><!--空间分辨率--><param name="search/time_resolution" value="0.8" type="double"/><!--时间维度分辨率--><param name="search/margin" value="0.2" type="double"/><!--检测碰撞--><param name="search/allocate_num" value="100000" type="int"/><!--最大节点数目--><param name="search/check_num" value="5" type="int"/><!--对中间状态安全检查--><!-- trajectory optimization --><param name="optimization/lambda1" value="10.0" type="double"/><param name="optimization/lambda2" value="5.0" type="double"/><param name="optimization/lambda3" value="0.00001" type="double"/><param name="optimization/lambda4" value="0.01" type="double"/><param name="optimization/lambda7" value="100.0" type="double"/><!-- reduces 'optimization/lambda7' to make the yaw changes slower--><param name="optimization/dist0" value="0.4" type="double"/><param name="optimization/max_vel" value="$(arg max_vel)" type="double"/><param name="optimization/max_acc" value="$(arg max_acc)" type="double"/><param name="optimization/algorithm1" value="15" type="int"/><param name="optimization/algorithm2" value="11" type="int"/><param name="optimization/max_iteration_num1" value="2" type="int"/><param name="optimization/max_iteration_num2" value="300" type="int"/><param name="optimization/max_iteration_num3" value="200" type="int"/><param name="optimization/max_iteration_num4" value="200" type="int"/><param name="optimization/max_iteration_time1" value="0.0001" type="double"/><param name="optimization/max_iteration_time2" value="0.005" type="double"/><param name="optimization/max_iteration_time3" value="0.003" type="double"/><param name="optimization/max_iteration_time4" value="0.003" type="double"/><param name="optimization/order" value="3" type="int"/><param name="bspline/limit_vel" value="$(arg max_vel)" type="double"/><param name="bspline/limit_acc" value="$(arg max_acc)" type="double"/><param name="bspline/limit_ratio" value="1.1" type="double"/><param name="bspline/limit_vel" value="$(arg max_vel)" type="double"/><param name="bspline/limit_acc" value="$(arg max_acc)" type="double"/><param name="bspline/limit_ratio" value="1.1" type="double"/></node>
</launch>

可能会出现规划出的路径实际位置与目标期望位置存在偏差的情况：

Fast_planner规划期望位置与实际位置偏差-解决前

这是由于OneShot中检查边界出现问题，令其返回了false，即认为最后寻找的目标点是不可达的，上一节点可达，故此时规划目标位置接近目标点但不是目标点。
sdf_map.cpp

  node_.param("sdf_map/map_size_x", x_size, -1.0);node_.param("sdf_map/map_size_y", y_size, -1.0);node_.param("sdf_map/map_size_z", z_size, -1.0);

kinodynamic_astar.cpp

if (coord(0) < origin_(0) || coord(0) >= map_size_3d_(0) || coord(1) < origin_(1) || coord(1) >= map_size_3d_(1) || coord(2) < origin_(2)  || coord(2) >= map_size_3d_(2)){return false;}