cartographer建图，重定位及发布消息结构为nav

完整功能包（包含carto建图，重定位以及odom话题发布）上传至https://download.csdn.net/download/zhaohaowu/33647981
众所周知，cartographer的重定位模式没有发布消息结构为nav_msgs::Odometry的odom话题
但cartographer_ros功能包中包含tracked_pose话题，里面包含了map坐标系下雷达的位姿，
默认不打开，我们在此基础上完成odom话题的发布

首先找到node.cc里面的node_options_.publish_tracked_pose，可以看到如果if为true，将发布tracked_pose话题，选中publish_tracked_pose按f12，跳转到定义处，在node_options.h文件下，可以看到默认为false，改为true，这样cartographer将发布tracked_pose话题

然后，两种方法发布odom

方法一，在node.cc中发布

在node.h中
ctrl+f搜索tracked_pose_publisher_
在其下面添加::ros::Publisher pub;
在node.cc中，
第一个if (node_options_.publish_tracked_pose) 中加入

pub = node_handle_.advertise<::nav_msgs::Odometry>("carto_odom", 100);

第二个if (node_options_.publish_tracked_pose) 中加入

     current_time = ros::Time::now();current_x = pose_msg.pose.position.x;current_yaw = tf::getYaw(pose_msg.pose.orientation);dt = (current_time - last_time).toSec();dx = current_x - last_x;d_yaw = current_yaw - last_yaw;lin_speed = dx/dt;ang_speed = d_yaw/dt;::nav_msgs::Odometry msg;msg.header.stamp = ros::Time::now();msg.header.frame_id = "odom";msg.child_frame_id = "rslidar";msg.pose.pose.position = pose_msg.pose.position;msg.pose.pose.orientation = pose_msg.pose.orientation;msg.twist.twist.linear.x = lin_speed;msg.twist.twist.linear.y = 0.0;msg.twist.twist.angular.z = ang_speed;pub.publish(msg);last_time = ros::Time::now();last_x = pose_msg.pose.position.x;last_yaw = tf::getYaw(pose_msg.pose.orientation);

方法二，使用rosparam传递参数，另外写个节点

在node.cc中
第二个if (node_options_.publish_tracked_pose) 中加入

node_handle_.setParam("position_x", pose_msg.pose.position.x);
node_handle_.setParam("position_y", pose_msg.pose.position.y);
node_handle_.setParam("orientation_x", pose_msg.pose.orientation.x);
node_handle_.setParam("orientation_y", pose_msg.pose.orientation.y);
node_handle_.setParam("orientation_z", pose_msg.pose.orientation.z);
node_handle_.setParam("orientation_w", pose_msg.pose.orientation.w);

在新节点中，
carto_odom.cpp

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <tf/tf.h>
#include <unistd.h>int main(int argc, char** argv)
{ros::init(argc, argv, "carto_odom");ros::NodeHandle(n);ros::Publisher pub = n.advertise<nav_msgs::Odometry>("carto_odom_test", 1);double position_x, position_y, orientation_x, orientation_y, orientation_z, orientation_w;nav_msgs::Odometry tmp;ros::Time current_time, last_time;double current_x, last_x, current_yaw, last_yaw, lin_speed, ang_speed;while (ros::ok()){n.getParam("position_x",position_x);n.getParam("position_y",position_y);n.getParam("orientation_x",orientation_x);n.getParam("orientation_y",orientation_y);n.getParam("orientation_z",orientation_z);n.getParam("orientation_w",orientation_w);current_time = ros::Time::now();current_x = position_x;tmp.pose.pose.orientation.x = orientation_x;tmp.pose.pose.orientation.y = orientation_y;tmp.pose.pose.orientation.z = orientation_z;tmp.pose.pose.orientation.w = orientation_w;current_yaw = tf::getYaw(tmp.pose.pose.orientation);nav_msgs::Odometry msg;msg.header.stamp = ros::Time::now();msg.header.frame_id = "odom";msg.child_frame_id = "rslidar";msg.pose.pose.position.x = position_x;msg.pose.pose.position.y = position_y;msg.pose.pose.orientation.x = orientation_x;msg.pose.pose.orientation.y = orientation_y;msg.pose.pose.orientation.z = orientation_z;msg.pose.pose.orientation.w = orientation_w;lin_speed = (current_x - last_x)/((current_time - last_time).toSec());ang_speed = (current_yaw - last_yaw)/((current_time - last_time).toSec());msg.twist.twist.linear.x = lin_speed;msg.twist.twist.angular.z = ang_speed;pub.publish(msg);n.getParam("position_x",position_x);n.getParam("position_y",position_y);n.getParam("orientation_x",orientation_x);n.getParam("orientation_y",orientation_y);n.getParam("orientation_z",orientation_z);n.getParam("orientation_w",orientation_w);last_time = ros::Time::now();last_x = position_x;tmp.pose.pose.orientation.x = orientation_x;tmp.pose.pose.orientation.y = orientation_y;tmp.pose.pose.orientation.z = orientation_z;tmp.pose.pose.orientation.w = orientation_w;last_yaw = tf::getYaw(tmp.pose.pose.orientation);usleep(80 * 1000); //100ms}ros::spin();std::cout << "\n\n结束odom发布\n\n";return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(carto_test)# add_compile_options(-std=c++11)
SET(CMAKE_BUILD_TYPE Debug)find_package(catkin REQUIRED COMPONENTSroscppnav_msgstf# message_generation
)
# add_message_files(
#   FILES
#   position.msg
# )
# generate_messages(
#   DEPENDENCIES
#   std_msgs
# )catkin_package(CATKIN_DEPENDS roscpp nav_msgs tf# message_runtime
)# find_package(OpenCV  REQUIRED)
# find_package(aruco  REQUIRED )include_directories(${catkin_INCLUDE_DIRS}
#   ${OpenCV_INCLUDE_DIRS}
#   ${aruco_INCLUDE_DIRS}
)add_executable(carto_odom src/carto_odom.cpp)
target_link_libraries(carto_odom${catkin_LIBRARIES}
#   ${OpenCV_LIBS}
#   ${aruco_LIBS}
)
# add_dependencies(aruco_test ${PROJECT_NAME}_gencpp)

cartographer建图，重定位及发布消息结构为nav_msgs::Odometry的odom话题相关推荐

Cartographer建图和纯定位
(一)cartographer在ros下安装参考: 要求: 64-bit, modern CPU (e.g. 3rd generation i7) 16 GB RAM Ubuntu 18.04 (Bi ...
【cartographer】园区场景自动驾驶-利用GPS实现ENU坐标系下的建图与定位
园区场景自动驾驶-利用GPS实现当地地理坐标系下的建图与定位概述主要实现如下的功能: 1.利用gps信息,修正题图为当地地理ENU坐标系: 2.在纯定位模式下,利用gps与imu(rtk)信息,实 ...
在室内停车场使用道路标记语义进行厘米级建图和定位
标题:Mapping and Localization using Semantic Road Marking withCentimeter-level Accuracy in Indoor Park ...
ROS 教程2 机器人雷达建图蒙特卡洛定位导航路径规划仿真
ros 机器人雷达建图蒙特卡洛定位导航路径规划仿真 move_base gmapping acml 博文github 一.安装 turtlebot 移动机器人底座进行导航 1.安装系统依赖 ...
差速小车的Cartographer建图
目录在之前的一些文章中,我们创建了DiffCart的仿真模型并能通过键盘控制它. 还为之提供了里程计和IMU用于估计机器人的位姿. 现在我们来给DiffCart装上激光雷达,并使用Cartograp ...
odomimu融合用于Cartographer建图
任务动机:使用odom&imu融合用于Cartographer建图,提高Cartographer建图时的匹配精度,同时降低运算消耗. 任务描述:使用odom&imu融合用于Cartog ...
基于视觉语义信息的建图与定位综述
点云PCL免费知识星球,点云论文速读. 文章:Semantic Visual Simultaneous Localization and Mapping: A Survey 作者:Kaiqi Chen ...
利用稀疏的语义视觉特征进行道路建图和定位(ICRA2021)
Road Mapping and Localization using Sparse Semantic Visual Features Cheng W, Yang S, Zhou M, et al. ...
ndtmapping建图_自动驾驶系列：激光雷达建图和定位(NDT)
该系列主要为对前期工作进行梳理,以后所进行的工作也会部分在此记录. 使用NDT(正态分布变换)进行点云建图和定位前言定位模块是自动驾驶最核心的模块之一,定位又包括全局定位和局部定位,对于自动驾驶, ...

cartographer建图，重定位及发布消息结构为nav_msgs::Odometry的odom话题

cartographer建图，重定位及发布消息结构为nav_msgs::Odometry的odom话题相关推荐

最新文章

热门文章