How to use navsat_transform_node along with this ?

[chrissunny94]

Hi all , how do i combine the following topics

• /ublox_gps_node/fix
• /ublox_gps_node/fix_velocity
• /ublox_gps_node/navpvt

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 .

[chrissunny94]

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()