完整功能包上传至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);
};