chrissunny94
commented
9 months ago I wrote the following python script to combine the /fix and /fix_velocity Then i used geodesy library to convert lat ,long to UTM coordinate .That was very straight forward .
Now i am confused how to get the heading .
# ROS Client Library for Python
import rclpy
from geodesy import utm
from nav_msgs.msg import Odometry
# Handles the creation of nodes
from rclpy.node import Node
from sensor_msgs.msg import Imu, NavSatFix
# Enables usage of the String message type
from std_msgs.msg import String
class MinimalPublisher(Node):
"""
Create a MinimalPublisher class, which is a subclass of the Node class.
"""
def __init__(self):
"""
Class constructor to set up the node
"""
# Initiate the Node class's constructor and give it a name
super().__init__('minimal_publisher')
self.publisher_odom_ = self.create_publisher(Odometry, '/odometry/filtered', 10)
# Initialize a counter variable
self.i = 0
self.temp_odom = Odometry()
self.temp_odom.header.frame_id = "map"
self.temp_odom.child_frame_id ="odom"
self.subscriber_fix_ = self.create_subscription(
NavSatFix,
'/gps/fix',
self.listener_callback_fix,
10)
self.subscriber_imu_ = self.create_subscription(
Imu,
'/roveo_mid_imu/sensor_imu',
self.listener_callback_imu,
10)
def listener_callback_fix(self, msg):
lat = msg.latitude
lon = msg.longitude
point = utm.fromLatLong(lat, lon)
print(point)
self.temp_odom.pose.pose.position.x = point.easting
self.temp_odom.pose.pose.position.y = point.northing
self.publisher_odom_.publish(self.temp_odom)
def listener_callback_imu(self,msg):
self.temp_odom.pose.pose.orientation = msg.orientation
# Populate linear and angular velocity from IMU
self.temp_odom.twist.twist.linear.x = msg.linear_acceleration.x
self.temp_odom.twist.twist.linear.y = msg.linear_acceleration.y
self.temp_odom.twist.twist.linear.z = msg.linear_acceleration.z
self.temp_odom.twist.twist.angular.x = msg.angular_velocity.x
self.temp_odom.twist.twist.angular.y = msg.angular_velocity.y
self.temp_odom.twist.twist.angular.z = msg.angular_velocity.z
def main(args=None):
rclpy.init(args=args)
minimal_publisher = MinimalPublisher()
rclpy.spin(minimal_publisher)
minimal_publisher.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
Hi all , how do i combine the following topics
My goal is to generate Nav_msgs/Odometry (odometry , odom ) using the above and it should be in UTM coordinate space .
Kindly help me out .