反光道钉cartographer+navigation定位及⾥程计调试实验记录（⼀） 实验时间： 2021.08.17-2021.08.21
实验地点： 科技园B座⼀楼楼道
调试内容：
1、0.2m/s速度重新建图的定位效果；
2、cartographer重定位的时间；
实验重要部分记录：
（1）建图的时候，⽤以下⽅法较好，⽤offline_XXX需要另外配置参数： roslaunch cartographer_ros demo_backpack_2d.launch bag_filename:=floor_12.bag
rosservice call /finish_trajectory 0
rosservice call /write_state "{filename: '${HOME}/Downloads/floor_12.pbstream'}"
rosrun cartographer_ros cartographer_pbstream_to_ros_map -map_filestem=/XXX/floor_12 -pbstream_filename=/XXX/floor_12.pbstream -resolution=0.05并且要配置lua⽂件参数，lua⽂件参数会影响建图效果。
demo_backpack_2d.launch⽂件内容如下：
<launch>
<param name="/use_sim_time" value="true" />
<include file="$(find cartographer_ros)/launch/backpack_2d.launch" />
<node name="rviz" pkg="rviz" type="rviz" required="true"
args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
<node name="playbag" pkg="rosbag" type="play"
args="--clock $(arg bag_filename)" />
</launch>
backpack_2d.launch⽂件内容：
<launch>
<param name="robot_description"
textfile="$(find cartographer_ros)/urdf/backpack_2d.urdf" />
<node name="robot_state_publisher" pkg="robot_state_publisher"
type="robot_state_publisher" />
<node name="cartographer_node" pkg="cartographer_ros"
type="cartographer_node" args="
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename backpack_2d.lua"
output="screen">
<remap from="scan" to="scan" />
</node>
<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
</launch>
backpack_2d.lua⽂件内容：
include "map_builder.lua"
include "trajectory_builder.lua"
options ={
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame ="map",
tracking_frame ="base_link",
published_frame ="base_link",
odom_frame ="odom",
provide_odom_frame =true,
publish_frame_projected_to_2d =false,
use_pose_extrapolator =true,
use_odometry =false,
use_nav_sat =false,
use_landmarks =false,
num_laser_scans =1,
num_multi_echo_laser_scans =0,
num_subdivisions_per_laser_scan =1,
num_point_clouds =0,
lookup_transform_timeout_sec =0.2,
submap_publish_period_sec =0.3,
pose_publish_period_sec =5e-3,
trajectory_publish_period_sec =30e-3,
rangefinder_sampling_ratio =1.,
odometry_sampling_ratio =1.,
fixed_frame_pose_sampling_ratio =1.,
imu_sampling_ratio =1.,
landmarks_sampling_ratio =1.,
}
MAP_BUILDER.use_trajectory_builder_2d =true
TRAJECTORY_BUILDER_2D.num_accumulated_range_data =4
return options
这⾥⾯需要格外注意 published_frame = "base_link", provide_odom_frame = true, use_odometry = false，这三个参数。
⼀般地，建图最好只⽤激光的数据（/scan）或者是/scan+/odom+/imu。尽量不要是/scan+/odom。
（2）修图的时候⼀定不能将原始图尺⼨做修改，否则可能初始位置不准。修图只⽤清除⼀些杂点，添加禁⾏线（禁⾏区）。设置禁⾏线的时候，尽量贴着地图边界，确保把扫描边界包围进去。
（3）将pbstream、pgm和yaml拷贝到导航包的对应⽂件夹内，并记下路径。修图的禁⾏线csv⽂件也
要放在read_markfile_pub⽂件夹下。最重要的是，新建地图后，还要更新cartographer⽂件中的定位launch⽂件，写⼊对应的pbstream⽂件路径
否则，机器⼈定位的时候有可能⾥程计严重漂移。⼀开始，定位就失败了，⾥程计坐标系乱跳。
（4）测试cartographer重定位的时候，在距离初始点45⽶初，有时候可以成功定位，有时候等待五六分钟也定位不成功。在重定位的时候，或者导航过程中，cartographer可以⾃动优化地图，更新定位。
（5）设置初始点校正机器⼈位置，在机器⼈附近，根据机器⼈位姿设置初始化点，⼤概率可以重定位成功。
initial_pose_set.cpp
#include"ros/ros.h"
#include"cartographer_ros_msgs/FinishTrajectory.h"
#include"cartographer_ros_msgs/StartTrajectory.h"
#include"geometry_msgs/PoseWithCovarianceStamped.h"
#include"tf/tf.h"
ros::Subscriber _pose_init_sub;
int traj_id =1;
void init_pose_callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg)
{
double x = msg->pose.pose.position.x;
double y = msg->pose.pose.position.y;
double theta = tf::getYaw(msg->ientation);
ros::NodeHandle nh;
ros::ServiceClient client_traj_finish = nh.serviceClient<cartographer_ros_msgs::FinishTrajectory>("finish_trajectory");
cartographer_ros_msgs::FinishTrajectory srv_traj_finish;
srv_ajectory_id = traj_id;
if(client_traj_finish.call(srv_traj_finish))
{
ROS_INFO("Call finish_trajectory %d success!", traj_id);
}
else
{
ROS_INFO("Failed to call finish_trajectory service!");
}
ros::ServiceClient client_traj_start = nh.serviceClient<cartographer_ros_msgs::StartTrajectory>("start_trajectory");
cartographer_ros_msgs::StartTrajectory srv_traj_start;
srv_figuration_directory ="/home/lp_hy/carto_ws/install_isolated/share/cartographer_ros/configuration_files";//.lua⽂件所在路径 srv_figuration_basename ="backpack_2d_localization_test.lua";//lua⽂件
srv_quest.use_initial_pose =1;
srv_quest.initial_pose = msg->pose.pose;
srv_lative_to_trajectory_id =0;
printf("&&&&&: %f__%f\n",srv_quest.initial_pose.position.x,srv_quest.initial_pose.position.y);
if(client_traj_start.call(srv_traj_start))
{
// ROS_INFO("Status ", srv_sponse.status)
ROS_INFO("Call start_trajectory %d success!", traj_id);
traj_id++;
}
else
{
ROS_INFO("Failed to call start_trajectory service!");真空泵叶片
}
}
int main(int argc,char**argv)
{
ros::init(argc,argv,"initial_pose_set");
ros::NodeHandle n;
_pose_init_sub = n.subscribe("/initialpose",1000,&init_pose_callback);
ros::spin();
return0;
}
<
cmake_minimum_required(VERSION 3.0.2)
project(initial_pose_set)
## Compile as C++11, supported in ROS Kinetic and newer
对接扣件
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
roscpp
cartographer_ros_msgs
tf
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See /doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
>>>>>>>>>##### Declare ROS messages, services and actions ##
>>>>>>>>>##### To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the l:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a exec_depend tag for "message_runtime"
## * In this file ():
## * add "message_generation" and every package in MSG_DEP_SET to
自动关窗器## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
高苏宁# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# geometry_msgs
# )
>>>>>>>>>##### Declare ROS dynamic reconfigure parameters ##
>>>>>>>>>##### To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the l:
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file ():
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
微波电视天线# cfg/DynReconf2.cfg
# )
>>>>>>>
## catkin specific configuration ##
>>>>>>>## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES initial_pose_set
CATKIN_DEPENDS geometry_msgs roscpp cartographer_ros_msgs tf
# DEPENDS system_lib
)
>>### Build ##
>>### Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
${catkin_INCLUDE_DIRS}
)
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/initial_pose_set.cpp
# )
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ## Declare a C++ executable
豫公网安备41160202000603
站长QQ:729038198
关于我们
投诉建议