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

Ros2 Turtlebot3 pose topic subscriber isn't triggered. [Python]

asked 2022-03-01 11:45:26 -0500

Hedwin gravatar image

Hello,

I'm trying to catch turtlebot position so I can plot its trajectory.

More precisely, I'm trying to build and run a ros2 subscriber on "/Turtle_2/pose", where "/Turtle_2" is my turtlebot namespace. Using this subscriber, I want to store the turtlebot position in a memory to plot all of them. But as a first step, I wan to print("Hello") when the subscriber is triggered by a new message published on this "/Turtle_2/pose" topic, which is not.

For confidentiality reason, I cannot give you more code than my subscriber. I'm sry about that.

NB: - ros2 topic list contain the topic I'm looking for. - ros2 topic echo /Turtle_2/pose plot indeed real time information about my turtle position. - Running the subscriber don't plot anything (no trigger message nor error message) - OS ubuntu 20.04.2 LTS ; Gnome 3.36.8 - Ros2 Foxy installed - Subscriber code :

import rclpy
from rclpy.node import Node

from nav_msgs.msg import Odometry
from std_msgs.msg import String
from geometry_msgs.msg import PoseStamped

class MinimalSubscriber(Node):

    def __init__(self):
        super().__init__('minimal_subscriber')
        self.subscription = self.create_subscription(
            PoseStamped,
            '/Turtle_2/pose',
            self.listener_callback,
            10)

    def listener_callback(self, msg):
        self.get_logger().info('HELLO')


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

    minimal_subscriber = MinimalSubscriber()

    rclpy.spin(minimal_subscriber)

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


if __name__ == '__main__':
    main()
  • This subscriber work on other topics like "Turtle_2/odom"
  • result of ros2 topic info -v /Turtle_2/pose:

    Type: geometry_msgs/msg/PoseStamped

    Publisher count: 1

    Node name: natnet_client Node namespace: /Turtle_2 Topic type: geometry_msgs/msg/PoseStamped Endpoint type: PUBLISHER GID: 01.0f.69.56.05.b0.02.00.01.00.00.00.00.00.12.03.00.00.00.00.00.00.00.00 QoS profile: Reliability: RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT Durability: RMW_QOS_POLICY_DURABILITY_VOLATILE Lifespan: 2147483651294967295 nanoseconds Deadline: 2147483651294967295 nanoseconds Liveliness: RMW_QOS_POLICY_LIVELINESS_AUTOMATIC Liveliness lease duration: 2147483651294967295 nanoseconds

    Subscription count: 0

Any idea will be appreciated, thanks in advance.

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
1

answered 2022-03-15 09:22:46 -0500

Hedwin gravatar image

A guy from my laboratory found the solution:

There was king of a communication problem, so the subscriber qos profile had to be redefined :

qos_sensor = QoSProfile(
    reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT,
    history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
    depth=1
)


self.get_logger().info('Use QoS Sensor')

self.subscription = self.create_subscription(
    PoseStamped,
    'Turtle_2/pose',
    self.listener_callback,
    qos_profile=qos_sensor)

Thanks to everyone who tried to help

edit flag offensive delete link more
0

answered 2022-03-01 12:57:42 -0500

ljaniec gravatar image

updated 2022-03-01 13:06:36 -0500

Just for your attention - you can try to use rosbag2 to record this kind of things. No need to reinvent the wheel (but if you want, go on! it's a nice way to learn, of course :) )

Your code should be fine, are you sure you have messages on this topic? Maybe the name? There are a lot of / there.

edit flag offensive delete link more

Comments

1

Hi! And thank you for your answer.

About rosbag, thank you for the tips, I didn't knew that, but I'm not sure it can help me in this case because I need my robot to learn about his pose in real time, and not learn his trajectory when it's done.

Yes I have message on this topic because when I do ros2 topic echo /Turtle2/pose, there is a lot of PoseStamped messages in the terminal. About the '/', when I tested my code on '/Turtle_2/odom', it worked with and without the first '/', only the second one is mandatory.

More, when I run the script, ros2 topic info confirm that there is one more subscriber on the topic.

Hedwin gravatar image Hedwin  ( 2022-03-07 07:01:27 -0500 )edit

More, when I run the script, ros2 topic info confirm that there is one more subscriber on the topic.

I'm thinking that maybe the message type is not correct, but according to ros2 topic info:

Type: geometry_msgs/msg/PoseStamped

And to ros2 topic echo /Turtle_2/pose :

---
header:
  stamp:
    sec: 1646656486
    nanosec: 273631294
  frame_id: map
pose:
  position:
    x: 1.000205397605896
    y: 1.0537039041519165
    z: 0.19965964555740356
  orientation:
    x: 0.0108546894043684
    y: 0.01880391500890255
    z: 0.2571524977684021
    w: -0.9661269187927246
---

(This topic echo command print a message similar to the one above many times in a second, I just copy one). It looks like I'm definitely right about the geometry_msgs/msg/PoseStamped Message type ...

Hedwin gravatar image Hedwin  ( 2022-03-07 07:01:53 -0500 )edit

Can you check rqt_graph? Just to see if there is a connection between your subscriber and topic /Turtle2/pose. These nodes are on one PC, right?

ljaniec gravatar image ljaniec  ( 2022-03-08 05:22:14 -0500 )edit

Hi, thank you for your help.

About the rqt_graph, it looks that I can't add images here so here is a link to a screenshot on google drive https://drive.google.com/file/d/1ZJmr...

We can see a node "/check_position" that is the node created by my script.

The node "/Turtle_2/natnet_client", communicate with a second computer that run motive, an optitrack software to get position data. Then it publish them on "/Turtle_2/pose". "/Turtle_2/natnet_client" is run on the same computer than "/check_position".

Hedwin gravatar image Hedwin  ( 2022-03-09 02:15:22 -0500 )edit

Note that "/Turtle_2/natnet_client" is coded in cpp, when my subscriber is coded in python. Do you think that it can create some libraries mismatches in the way they consider PoseStamped message shape, so the published message isn't considered as a PoseStamped one?

Here is the essentials parts of natnet_client cpp code :

In the constructor:

pose_pub = this->create_publisher<geometry_msgs::msg::PoseStamped>("pose", qos);
Hedwin gravatar image Hedwin  ( 2022-03-09 02:36:46 -0500 )edit

And in the callback:

// Pose
auto p = geometry_msgs::msg::PoseStamped();
p.header.frame_id = world_frame;
p.header.stamp = this->get_clock()->now();
p.pose.position.x = data->RigidBodies[i].x;
p.pose.position.y = data->RigidBodies[i].y;
p.pose.position.z = data->RigidBodies[i].z;
p.pose.orientation.x = data->RigidBodies[i].qx;
p.pose.orientation.y = data->RigidBodies[i].qy;
p.pose.orientation.z = data->RigidBodies[i].qz;
p.pose.orientation.w = data->RigidBodies[i].qw;
pose_pub->publish(p);
Hedwin gravatar image Hedwin  ( 2022-03-09 02:37:27 -0500 )edit

Hmmm, RQt graph seems to be okay. The different language of implementation shouldn't be a problem. You can try to check it with C++ subscriber but I wouldn't promise anything there. Just to be sure, try to modify the callback so it's using the received message (something simple, as printing Hello and stamp.sec). Maybe it will show something new..?

ljaniec gravatar image ljaniec  ( 2022-03-09 03:32:00 -0500 )edit

Question Tools

Stats

Asked: 2022-03-01 11:45:26 -0500

Seen: 871 times

Last updated: Mar 15 '22