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-14 12:40:08 +01:00
parent 02c4e9d741
commit edb23af234
1 changed files with 28 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
nodes/odom_publisher_linus.py
View File

@ -1,57 +1,38 @@
#include <ros/ros.h> #!/usr/bin/env python
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/TransformStamped.h>
#include <signal.h>
volatile sig_atomic_t flag = 0; import rospy
from tf.transform_broadcaster import TransformBroadcaster
from nav_msgs.msg import Odometry
from geometry_msgs.msg import TransformStamped
void signal_handler(int signum) { def odom_callback(msg):
flag = 1; # Create TransformStamped message
} odom_trans = TransformStamped()
odom_trans.header.stamp = msg.header.stamp
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, tf::TransformBroadcaster& odom_broadcaster) { # Set translation
// Create 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;
odom_trans.child_frame_id = msg->child_frame_id;
// Copy translation and rotation # Set rotation
odom_trans.transform.translation.x = msg->pose.pose.position.x; odom_trans.transform.rotation = msg.pose.pose.orientation
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 # Broadcast the transform
odom_broadcaster.sendTransform(odom_trans); tf_broadcaster.sendTransformMessage(odom_trans)
}
int main(int argc, char** argv) { if __name__ == "__main__":
ros::init(argc, argv, "tf_broadcaster"); rospy.init_node("tf_broadcaster")
ros::NodeHandle nh;
// Register the signal handler # Create a TransformBroadcaster
signal(SIGINT, signal_handler); tf_broadcaster = TransformBroadcaster()
// Create the transform broadcaster # Subscribe to the /odom topic
tf::TransformBroadcaster odom_broadcaster; rospy.Subscriber("/odom", Odometry, odom_callback)
// Subscribe to the /odom topic rospy.loginfo("TF broadcaster node is running...")
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..."); # Keep the node running
rospy.spin()
// Spin until the node is stopped
ros::Rate rate(10.0); // Adjust rate if necessary
while (ros::ok() && !flag) {
ros::spinOnce();
rate.sleep();
}
ROS_INFO("TF broadcaster node shutting down.");
ros::shutdown();
return 0;
}