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

update

Browse Source
This commit is contained in:
Your Name 2025-01-14 12:39:13 +01:00
parent 5320103fbe
commit 02c4e9d741
1 changed files with 49 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
nodes/odom_publisher_linus.py
View File

@ -1,43 +1,57 @@
#!/usr/bin/env python #include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/TransformStamped.h>
#include <signal.h>
import rospy volatile sig_atomic_t flag = 0;
from nav_msgs.msg import Odometry
import tf
import math
def normalize_quaternion(x, y, z, w): void signal_handler(int signum) {
norm = math.sqrt(x**2 + y**2 + z**2 + w**2) flag = 1;
return x / norm, y / norm, z / norm, w / norm }
def odom_callback(msg): void odomCallback(const nav_msgs::Odometry::ConstPtr& msg, tf::TransformBroadcaster& odom_broadcaster) {
# Extract position and orientation from the odometry message // Create transform message
position = msg.pose.pose.position geometry_msgs::TransformStamped odom_trans;
orientation = msg.pose.pose.orientation 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;
# Normalize the quaternion // Copy translation and rotation
qx, qy, qz, qw = normalize_quaternion( odom_trans.transform.translation.x = msg->pose.pose.position.x;
orientation.x, orientation.y, orientation.z, orientation.w 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;
# Get frame IDs from the message // Publish the transform
parent_frame_id = msg.header.frame_id if msg.header.frame_id else "odom" odom_broadcaster.sendTransform(odom_trans);
child_frame_id = msg.child_frame_id if msg.child_frame_id else "base_link" }
# Broadcast the transformation int main(int argc, char** argv) {
br = tf.TransformBroadcaster() ros::init(argc, argv, "tf_broadcaster");
br.sendTransform( ros::NodeHandle nh;
(position.x, position.y, position.z), # Translation
(qx, qy, qz, qw), # Normalized rotation (quaternion)
rospy.Time.now(), # Timestamp
child_frame_id, # Child frame ID
parent_frame_id # Parent frame ID
)
def main(): // Register the signal handler
rospy.init_node('odom_tf_publisher', anonymous=True) signal(SIGINT, signal_handler);
rospy.Subscriber('/odom', Odometry, odom_callback)
rospy.loginfo("TF Publisher for /odom topic is running...")
rospy.spin()
if __name__ == '__main__': // Create the transform broadcaster
main() tf::TransformBroadcaster odom_broadcaster;
// 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) {
ros::spinOnce();
rate.sleep();
}
ROS_INFO("TF broadcaster node shutting down.");
ros::shutdown();
return 0;
}