ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
2

How can we create a fake gps publisher node in ROS2 ?

asked 2020-04-15 23:56:31 -0500

petal gravatar image

updated 2020-04-15 23:58:31 -0500

Hello!

I want to create a fake GPS ros node to publish the GPS data so that the robot_localization package can subscribe to it for localization? What information would I be requiring for this task? Any suggestion would be helpful.

Thanks,

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-04-16 02:30:27 -0500

DanielRobotics gravatar image

I have put a node below that publishes static GPS data in a sensor_msgs/NavSatFix message to the topic 'gps/fix' since the navsat_transform_node of robot_localization subscribes to gps/fix. If you want the GPS data to update you can change the values on the attributes you need e.g. latitude/longitude over time in the pattern you want.

import rclpy
import os
from rclpy.node import Node

from sensor_msgs.msg import NavSatFix
from sensor_msgs.msg import NavSatStatus
from std_msgs.msg import Header


class GpsNode(Node):

    def __init__(self):
        super().__init__('gps_node')
        self.publisher_ = self.create_publisher(NavSatFix, 'gps/fix', 10)
        timer_period = 0.5  # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)

    def timer_callback(self):
            msg = NavSatFix()
            msg.header = Header()
            msg.header.stamp = self.get_clock().now().to_msg()
            msg.header.frame_id = "gps"

            msg.status.status = NavSatStatus.STATUS_FIX
            msg.status.service = NavSatStatus.SERVICE_GPS

            # Position in degrees.
            msg.latitude = 57.047218
            msg.longitude = 9.920100

            # Altitude in metres.
            msg.altitude = 1.15

            msg.position_covariance[0] = 0
            msg.position_covariance[4] = 0
            msg.position_covariance[8] = 0
            msg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN

            self.publisher_.publish(msg)
            self.best_pos_a = None


def main(args=None):
    rclpy.init(args=args)

    gps_node = GpsNode()

    rclpy.spin(gps_node)

    # Destroy the node explicitly
    # (optional - otherwise it will be done automatically
    # when the garbage collector destroys the node object)
    gps_node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()
edit flag offensive delete link more

Comments

1

@DanielRobotics thanks for catching the duplicate post. Recently spammers are getting more sophisticated and will duplicate posts like this to make their accounts look more active.

tfoote gravatar image tfoote  ( 2020-04-16 12:46:08 -0500 )edit

Hi, thanks a lot for this but do you happen to know if this also works in ROS1?

Rika gravatar image Rika  ( 2021-08-07 07:13:53 -0500 )edit

Question Tools

3 followers

Stats

Asked: 2020-04-15 23:56:31 -0500

Seen: 13,101 times

Last updated: Apr 16 '20