话题和消息结构的转换odom2current_pose和current_velocity2cmd_vel

完整功能包上传至csdn
https://download.csdn.net/download/zhaohaowu/21442016
CMakeLists.txt
感觉比较简洁易懂

cmake_minimum_required(VERSION 3.0.2)
project(odom_velocity2pose_cmd)

find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  nav_msgs
  roscpp
  rospy
)

catkin_package(
CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy
)

include_directories(
  ${catkin_INCLUDE_DIRS}
)

add_executable(odom2pose src/odom2pose.cpp)
target_link_libraries(odom2pose ${catkin_LIBRARIES})

add_executable(velocity2cmd src/velocity2cmd.cpp)
target_link_libraries(velocity2cmd ${catkin_LIBRARIES})

odom2current_pose.cpp

#include "odom2pose.h"

odom2pose::odom2pose(){
	sub = n.subscribe<nav_msgs::Odometry>("/odom", 100, &odom2pose::callback, this);
	pub = n.advertise<geometry_msgs::PoseStamped>("/current_pose", 100);
	
}

void odom2pose::callback(const nav_msgs::Odometry::ConstPtr& odom_data){
	geometry_msgs::PoseStamped current_pose_data;
	current_pose_data.header.frame_id = "map";
	current_pose_data.header.seq = odom_data->header.seq;
	current_pose_data.header.stamp.sec = odom_data->header.stamp.sec;
	current_pose_data.header.stamp.nsec = odom_data->header.stamp.nsec;
	current_pose_data.pose.position.x = odom_data->pose.pose.position.x;
	current_pose_data.pose.position.y = odom_data->pose.pose.position.y;
	current_pose_data.pose.position.z = odom_data->pose.pose.position.z;
	current_pose_data.pose.orientation.x = odom_data->pose.pose.orientation.x;
	current_pose_data.pose.orientation.y = odom_data->pose.pose.orientation.y;
	current_pose_data.pose.orientation.z = odom_data->pose.pose.orientation.z;
	current_pose_data.pose.orientation.w = odom_data->pose.pose.orientation.w;
	pub.publish(current_pose_data);
}

int main(int argc, char** argv){
	ros::init(argc, argv, "odom2pose");
    odom2pose odom2pose;
    ros::spin();
    return 0;
}

odom2current_pose.h

#include <iostream>
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>		//current_pose
#include <geometry_msgs/TwistStamped.h>		//current_velocity
#include <nav_msgs/Odometry.h>				//odom
#include <geometry_msgs/Twist.h>			//cmd_vel

class odom2pose{
	private:
		ros::NodeHandle n;
		ros::Publisher pub;
		ros::Subscriber sub;
	public:
		odom2pose();
		void callback(const nav_msgs::Odometry::ConstPtr& odom_data);
};

velocity2cmd.cpp

#include "velocity2cmd.h"

velocity2cmd::velocity2cmd(){
	sub = n.subscribe<geometry_msgs::TwistStamped>("/current_velocity", 100, &velocity2cmd::callback, this);
	pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 100);
	
}

void velocity2cmd::callback(const geometry_msgs::TwistStamped::ConstPtr& current_velocity_data){
	geometry_msgs::Twist cmd_vel_data;
	cmd_vel_data.linear.x = current_velocity_data->twist.linear.x;
	cmd_vel_data.linear.y = current_velocity_data->twist.linear.y;
	cmd_vel_data.linear.z = current_velocity_data->twist.linear.z;
	cmd_vel_data.angular.x = current_velocity_data->twist.angular.x;
	cmd_vel_data.angular.y = current_velocity_data->twist.angular.y;
	cmd_vel_data.angular.z = current_velocity_data->twist.angular.z;
	pub.publish(cmd_vel_data);
}

int main(int argc, char** argv){
	ros::init(argc, argv, "velocity2cmd");
    velocity2cmd velocity2cmd;
    ros::spin();
    return 0;
}

velocity2cmd.h

#include <iostream>
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>		//current_pose
#include <geometry_msgs/TwistStamped.h>		//current_velocity
#include <nav_msgs/Odometry.h>				//odom
#include <geometry_msgs/Twist.h>			//cmd_vel

class velocity2cmd{
	private:
		ros::NodeHandle n;
		ros::Publisher pub;
		ros::Subscriber sub;
	public:
		velocity2cmd();
		void callback(const geometry_msgs::TwistStamped::ConstPtr& current_velocity_data);
};