Gazebo | Ignition | Community
Ask Your Question
0

How to get the position of certain object in gazebo and publish it on a topic to use it in px4 or ROS?

asked 2020-10-12 06:12:07 -0600

David Wright gravatar image

I am trying to get the position of Aruco Marker in the gazebo and publish it on the topic so that I can use it for different processes. Can anyone help me to sort out the solution? I found a similar question in the links below

https://answers.gazebosim.org/questio...

https://answers.gazebosim.org/questio...

But I can not apply the solution given in the link above. Can someone guide me on how can I get the position and publish it on a topic so I can use it outside the gazebo e.g., PX4 or ROS?

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-10-13 20:35:16 -0600

wongrufus gravatar image

Here's a simple rospy script that reads from gazebo/model_states and publishes all transforms as static transform

#!/usr/bin/env python  
import rospy
import tf2_ros
import gazebo_msgs.msg
import geometry_msgs.msg
import time

import pdb

if __name__ == '__main__':
    rospy.init_node('gazebo_tf_broadcaster')

    broadcaster = tf2_ros.StaticTransformBroadcaster()

    publish_frequency = rospy.get_param("publish_frequency", 1)

    last_published = None
    def callback(data):
        global last_published
        if last_published and publish_frequency > 0.0 and time.time() - last_published <= 1.0 / publish_frequency:
            return
        transforms = []
        for i in range(len(data.name)):
            transform = geometry_msgs.msg.TransformStamped()
            transform.header.stamp = rospy.Time.now()
            transform.header.frame_id = "world"
            transform.child_frame_id = data.name[i]
            transform.transform.translation.x = data.pose[i].position.x
            transform.transform.translation.y = data.pose[i].position.y
            transform.transform.translation.z = data.pose[i].position.z
            transform.transform.rotation.w = data.pose[i].orientation.w
            transform.transform.rotation.x = data.pose[i].orientation.x
            transform.transform.rotation.y = data.pose[i].orientation.y
            transform.transform.rotation.z = data.pose[i].orientation.z
            transforms.append(transform)
        broadcaster.sendTransform(transforms)
        last_published = time.time()

    rospy.Subscriber("/gazebo/model_states", gazebo_msgs.msg.ModelStates, callback)

    rospy.spin()
edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2020-10-12 04:11:15 -0600

Seen: 2,618 times

Last updated: Oct 13 '20