odom

cartographer建图，重定位及发布消息结构为nav_msgs：：Odometry的。。。

⾸先找到⾥⾯的node_options_.publish_tracked_pose，可以看到如果if为true，将发布tracked_pose话题，选中

publish_tracked_pose按f12，跳转到定义处，在node_options.h⽂件下，可以看到默认为false，改为true，这样cartographer将发布

tracked_pose话题

然后，两种⽅法发布odom

⽅法⼀，在中发布

在node.h中

ctrl+f搜索tracked_pose_publisher_

在其下⾯添加::ros::Publisherpub;

在中，

第⼀个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_on.x;

current_yaw=tf::getYaw(pose_ation);

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::Odometrymsg;

=ros::Time::now();

_id="odom";

_frame_id="rslidar";

on=pose_on;

ation=pose_ation;

.x=lin_speed;

.y=0.0;

r.z=ang_speed;

h(msg);

last_time=ros::Time::now();

last_x=pose_on.x;

last_yaw=tf::getYaw(pose_ation);

⽅法⼆，使⽤rosparam传递参数，另外写个节点

在中

第⼆个if(node_options_.publish_tracked_pose)中加⼊

node_handle_.setParam("position_x",pose_on.x);

node_handle_.setParam("position_y",pose_on.y);

node_handle_.setParam("orientation_x",pose_ation.x);

node_handle_.setParam("orientation_y",pose_ation.y);

node_handle_.setParam("orientation_z",pose_ation.z);

node_handle_.setParam("orientation_w",pose_ation.w);

在新节点中，

carto_

#include

#include

#include

#include

intmain(intargc,char**argv)

{

ros::init(argc,argv,"carto_odom");

ros::NodeHandle(n);

ros::Publisherpub=ise("carto_odom_test",1);

doubleposition_x,position_y,orientation_x,orientation_y,orientation_z,orientation_w;

nav_msgs::Odometrytmp;

ros::Timecurrent_time,last_time;

doublecurrent_x,last_x,current_yaw,last_yaw,lin_speed,ang_speed;

while(ros::ok())

{

am("position_x",position_x);

am("position_y",position_y);

am("orientation_x",orientation_x);

am("orientation_y",orientation_y);

am("orientation_z",orientation_z);

am("orientation_w",orientation_w);

current_time=ros::Time::now();

current_x=position_x;

ation.x=orientation_x;

ation.y=orientation_y;

ation.z=orientation_z;

ation.w=orientation_w;

current_yaw=tf::getYaw(ation);

nav_msgs::Odometrymsg;

=ros::Time::now();

_id="odom";

_frame_id="rslidar";

on.x=position_x;

on.y=position_y;

ation.x=orientation_x;

ation.y=orientation_y;

ation.z=orientation_z;

ation.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());

.x=lin_speed;

r.z=ang_speed;

h(msg);

am("position_x",position_x);

am("position_y",position_y);

am("orientation_x",orientation_x);

am("orientation_y",orientation_y);

am("orientation_z",orientation_z);

am("orientation_w",orientation_w);

last_time=ros::Time::now();

last_x=position_x;

ation.x=orientation_x;

ation.y=orientation_y;

ation.z=orientation_z;

ation.w=orientation_w;

last_yaw=tf::getYaw(ation);

usleep(80*1000);//100ms

}

ros::spin();

std::cout<<"nn结束odom发布nn";

return0;

}

cmake_minimum_required(VERSION3.0.2)

project(carto_test)

#add_compile_options(-std=c++11)

SET(CMAKE_BUILD_TYPEDebug)

find_package(catkinREQUIREDCOMPONENTS

roscpp

nav_msgs

tf

#message_generation

)

#add_message_files(

#FILES

#

#)

#generate_messages(

#DEPENDENCIES

#std_msgs

#)

catkin_package(

CATKIN_DEPENDSroscppnav_msgstf

#message_runtime

)

#find_package(OpenCVREQUIRED)

#find_package(arucoREQUIRED)

include_directories(

${catkin_INCLUDE_DIRS}

#${OpenCV_INCLUDE_DIRS}

#${aruco_INCLUDE_DIRS}

)

add_executable(carto_odomsrc/carto_)

target_link_libraries(carto_odom

${catkin_LIBRARIES}

#${OpenCV_LIBS}

#${aruco_LIBS}

)

#add_dependencies(aruco_test${PROJECT_NAME}_gencpp)