ellensohn/cps_rmp220_support
1
0
Fork 0
mirror of https://github.com/bjoernellens1/cps_rmp220_support.git synced 2025-01-19 08:06:59 +00:00
Code Issues Packages Projects Releases Wiki Activity

update

Browse Source
This commit is contained in:
Your Name 2025-01-13 15:05:40 +01:00
parent e1ff47b134
commit 1943de705d
1 changed files with 30 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
nodes/odom_publisher.cpp
View File

@ -10,51 +10,48 @@ void signal_handler(int signum) {
flag = 1; flag = 1;
} }
class OdomToBaseLinkPublisher { void odomCallback(const nav_msgs::Odometry::ConstPtr& msg, tf::TransformBroadcaster& odom_broadcaster) {
public: // Create transform message
OdomToBaseLinkPublisher() : nh_() { geometry_msgs::TransformStamped odom_trans;
// Subscribe to the /odom topic odom_trans.header.stamp = msg->header.stamp;
odom_sub_ = nh_.subscribe("/odom", 10, &OdomToBaseLinkPublisher::odomCallback, this); odom_trans.header.frame_id = msg->header.frame_id;
} odom_trans.child_frame_id = msg->child_frame_id;
void odomCallback(const nav_msgs::Odometry::ConstPtr& msg) { // Copy translation and rotation
// Prepare the transform message odom_trans.transform.translation.x = msg->pose.pose.position.x;
geometry_msgs::TransformStamped odom_trans; odom_trans.transform.translation.y = msg->pose.pose.position.y;
odom_trans.header.stamp = msg->header.stamp; odom_trans.transform.translation.z = msg->pose.pose.position.z;
odom_trans.header.frame_id = msg->header.frame_id; // Should be "odom" odom_trans.transform.rotation = msg->pose.pose.orientation;
odom_trans.child_frame_id = msg->child_frame_id; // Should be "base_link"
// Copy translation and rotation // Publish the transform
odom_trans.transform.translation.x = msg->pose.pose.position.x; odom_broadcaster.sendTransform(odom_trans);
odom_trans.transform.translation.y = msg->pose.pose.position.y; }
odom_trans.transform.translation.z = msg->pose.pose.position.z;
odom_trans.transform.rotation = msg->pose.pose.orientation;
// Publish the transform
odom_broadcaster_.sendTransform(odom_trans);
}
private:
ros::NodeHandle nh_;
ros::Subscriber odom_sub_;
tf::TransformBroadcaster odom_broadcaster_;
};
int main(int argc, char** argv) { int main(int argc, char** argv) {
ros::init(argc, argv, "odom_to_base_link_publisher"); ros::init(argc, argv, "tf_broadcaster");
ros::NodeHandle nh;
// Register the signal handler // Register the signal handler
signal(SIGINT, signal_handler); signal(SIGINT, signal_handler);
OdomToBaseLinkPublisher publisher; // Create the transform broadcaster
tf::TransformBroadcaster odom_broadcaster;
ros::Rate r(50); // Set loop rate // Subscribe to the /odom topic
ros::Subscriber odom_sub = nh.subscribe<nav_msgs::Odometry>(
"/odom", 10,
boost::bind(odomCallback, _1, boost::ref(odom_broadcaster)));
ROS_INFO("TF broadcaster node running...");
// Spin until the node is stopped
ros::Rate rate(10.0); // Adjust rate if necessary
while (ros::ok() && !flag) { while (ros::ok() && !flag) {
ros::spinOnce(); // Handle callbacks ros::spinOnce();
r.sleep(); rate.sleep();
} }
ROS_INFO("Odometry to base_link transform node shutting down."); ROS_INFO("TF broadcaster node shutting down.");
ros::shutdown(); ros::shutdown();
return 0; return 0;
} }