完整功能包(包含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 COMPONENTS
roscpp
nav_msgs
tf
# 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)