pose2d_to_odom_converter.cpp

Go to the documentation of this file.

 
#include <sick_scan/sick_ros_wrapper.h>
 
#if defined __ROS_VERSION && __ROS_VERSION == 1
ros::Publisher nav_odom_publisher;
void process_pose_msg(const ros_geometry_msgs::Pose2D::ConstPtr& pose2d_msg)
{
  ROS_INFO_STREAM("pose2d_to_odom_converter: Pose2D message received, x = " << pose2d_msg->x << "m, y = " << pose2d_msg->y << " m, theta=" << (180.0 * pose2d_msg->theta /M_PI) << " deg");
  ros_nav_msgs::Odometry odom_msg;
  // The pose in odom_msg should be specified in the coordinate frame given by header.frame_id.
  // The twist in odom_msgshould be specified in the coordinate frame given by the child_frame_id.
  tf2::Quaternion theta_quaternion;
  theta_quaternion.setRPY(0, 0, pose2d_msg->theta);
  odom_msg.header.stamp = rosTimeNow();
  odom_msg.header.frame_id = "map";
  odom_msg.child_frame_id = "laser";
  odom_msg.pose.pose.position.x = pose2d_msg->x;
  odom_msg.pose.pose.position.y = pose2d_msg->y;
  odom_msg.pose.pose.position.z = 0;
  odom_msg.pose.pose.orientation.x = theta_quaternion.getX();
  odom_msg.pose.pose.orientation.y = theta_quaternion.getY();
  odom_msg.pose.pose.orientation.z = theta_quaternion.getZ();
  odom_msg.pose.pose.orientation.w = theta_quaternion.getW();
  odom_msg.pose.covariance = {0};
  odom_msg.twist.twist.linear.x = 0;
  odom_msg.twist.twist.linear.y = 0;
  odom_msg.twist.twist.linear.z = 0;
  odom_msg.twist.twist.angular.x = 0;
  odom_msg.twist.twist.angular.y = 0;
  odom_msg.twist.twist.angular.z = 0;
  odom_msg.twist.covariance = {0};
  nav_odom_publisher.publish(odom_msg);
  ROS_INFO_STREAM("pose2d_to_odom_converter: Odometry message published, x = " << odom_msg.pose.pose.position.x << "m, y = " << odom_msg.pose.pose.position.y << " m, yaw = " << (180.0 * pose2d_msg->theta /M_PI) << " deg, frame_id = " << odom_msg.header.frame_id << ", child_frame_id = " << odom_msg.child_frame_id);
}
#endif // __ROS_VERSION == 1
 
 
int main(int argc, char** argv)
{
#if defined __ROS_VERSION && __ROS_VERSION == 1
  ros::init(argc, argv, "pose2d_to_odom_converter");
  ros::NodeHandle nh("~");
  rosNodePtr node = &nh;
 
  ROS_INFO_STREAM("pose2d_to_odom_converter started");
  nav_odom_publisher = nh.advertise<ros_nav_msgs::Odometry>("/rtabmap/odom", 10);
  ros::Subscriber pose2d_subscriber = nh.subscribe<ros_geometry_msgs::Pose2D>("/pose2D", 10, process_pose_msg);
 
  rosSpin(node);
 
  ROS_INFO_STREAM("pose2d_to_odom_converter finished");
#endif // __ROS_VERSION == 1
  return 0;
}