53 base_odom_.twist.twist.linear.x = msg->twist.twist.linear.x;
54 base_odom_.twist.twist.linear.y = msg->twist.twist.linear.y;
55 base_odom_.twist.twist.angular.z = msg->twist.twist.angular.z;
56 base_odom_.child_frame_id = msg->child_frame_id;
70 geometry_msgs::Twist global_vel;
73 global_vel.linear.x =
base_odom_.twist.twist.linear.x;
74 global_vel.linear.y =
base_odom_.twist.twist.linear.y;
75 global_vel.angular.z =
base_odom_.twist.twist.angular.z;
77 robot_vel.header.frame_id =
base_odom_.child_frame_id;
79 robot_vel.pose.position.x = global_vel.linear.x;
80 robot_vel.pose.position.y = global_vel.linear.y;
81 robot_vel.pose.position.z = 0;
83 q.
setRPY(0, 0, global_vel.angular.z);
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
#define ROS_INFO_ONCE(...)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void getOdom(nav_msgs::Odometry &base_odom)
nav_msgs::Odometry base_odom_
ros::Subscriber odom_sub_
OdometryHelperRos(std::string odom_topic="")
Constructor.
void odomCallback(const nav_msgs::Odometry::ConstPtr &msg)
Callback for receiving odometry data.
void convert(const A &a, B &b)
void setOdomTopic(std::string odom_topic)
Set the odometry topic. This overrides what was set in the constructor, if anything.
void getRobotVel(geometry_msgs::PoseStamped &robot_vel)