[SOLVED] ROS2 Subscriber Callback Not Working

girishkumar.kannan · September 17, 2022, 6:38am

Hi The Construct Team,

I am currently learning ROS2 Basics with Python. I am currently on the Topics Quiz and stuck there unable to get any message from /odom topic.

What I have tried so far:

Printed out values in odom callback function - THEY WORK FINE !
Tried ros2 topic echo /odom - THIS ALSO WORKS FINE !
Tried changing QoS reliability from BEST_EFFORT to RELIABLE and back to BEST_EFFORT - Does not seem to have any effect on my program.
Moved rclpy.spin(...) function into a while loop with try-except - still the problem exists.

At this point I have run out of ideas! [Probably I am making a basic mistake that I don’t seem to know!]

ros2 topic info /odom -v prints out saying reliability is RELIABLE, but in both cases, RELIABLE or BEST_EFFORT, I still have the problem.

Type: nav_msgs/msg/Odometry

Publisher count: 1

Node name: turtlebot3_diff_drive
Node namespace: /
Topic type: nav_msgs/msg/Odometry
Endpoint type: PUBLISHER
GID: 01.0f.f7.c7.0b.02.00.00.01.00.00.00.00.00.63.03.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RMW_QOS_POLICY_RELIABILITY_RELIABLE
  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

BUT,
When I run my code (after compiling) - the odom callback does not receive any message and values do not get updated. I cannot figure out why.

Here is my code:

import time
import rclpy
import numpy as np
from rclpy.node import Node
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
from rclpy.qos import ReliabilityPolicy, QoSProfile


class MoveTurtleBot3(Node):

    def __init__(self):
        super().__init__('topics_quiz_node')
        timer_period = 0.5
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.publisher_ = self.create_publisher(Twist, 'cmd_vel', 10)
        self.subscriber = self.create_subscription(
            Odometry, '/odom', self.odom_callback,
            QoSProfile(depth=10, reliability=ReliabilityPolicy.BEST_EFFORT)) # RELIABLE
        self.twist_cmd = Twist()
        self.roll = 0.0
        self.pitch = 0.0
        self.yaw = 0.0

        self.get_logger().info("Moving front a little bit...")
        self.twist_cmd.linear.x = 0.1
        self.twist_cmd.angular.z = 0.0
        self.publisher_.publish(self.twist_cmd)
        time.sleep(0.5)
        self.twist_cmd.linear.x = 0.0
        self.twist_cmd.angular.z = 0.0
        self.publisher_.publish(self.twist_cmd)
        self.get_logger().info("done")

        self.move()
        # return None

    def move(self):
        # move straight till left opening
        self.get_logger().info("Moving Straight...")
        self.twist_cmd.linear.x = 0.5
        self.twist_cmd.angular.z = 0.0
        self.publisher_.publish(self.twist_cmd)
        time.sleep(7.00)
        self.twist_cmd.linear.x = 0.0
        self.twist_cmd.angular.z = 0.0
        self.publisher_.publish(self.twist_cmd)
        # turn left 90 degs or 1.57 rads
        self.get_logger().info("Turning 90 degs...")
        while (self.yaw < 1.569):
            # print(self.odom_msg)
            self.get_logger().info('Yaw: %s' % str(self.yaw))
            self.twist_cmd.linear.x = 0.0
            self.twist_cmd.angular.z = 0.1
            self.publisher_.publish(self.twist_cmd)
            time.sleep(0.5)
        # self.twist_cmd.linear.x = 0.0
        # self.twist_cmd.angular.z = 0.0
        # self.publisher_.publish(self.twist_cmd)
        # return None

    def timer_callback(self):
        # self.move()
        pass
        # return None

    def odom_callback(self, odom_msg):
        # self.odom_msg = odom_msg
        quat = odom_msg.pose.pose.orientation
        # self.get_logger().info("Quaternion: %s" % str(quat))
        # self.get_logger().info("QuatX: %s" % quat.x)
        # self.get_logger().info("QuatY: %s" % quat.y)
        # self.get_logger().info("QuatZ: %s" % quat.z)
        # self.get_logger().info("QuatW: %s" % quat.w)
        self.roll, self.pitch, self.yaw = self.euler_from_quaternion(
            [quat.x, quat.y, quat.z, quat.w])
        self.get_logger().info("Roll: %s, Pitch: %s, Yaw: %s" %
                               (str(self.roll), str(self.pitch), str(self.yaw)))
        # return None

    def euler_from_quaternion(self, quaternion):
        """
        Converts quaternion (w in last place) to euler roll, pitch, yaw
        quaternion = [x, y, z, w]
        Below should be replaced when porting for ROS2 Python tf_conversions is done.
        """
        x = quaternion[0]
        y = quaternion[1]
        z = quaternion[2]
        w = quaternion[3]

        sinr_cosp = 2 * (w * x + y * z)
        cosr_cosp = 1 - 2 * (x * x + y * y)
        roll = np.arctan2(sinr_cosp, cosr_cosp)

        sinp = 2 * (w * y - z * x)
        pitch = np.arcsin(sinp)

        siny_cosp = 2 * (w * z + x * y)
        cosy_cosp = 1 - 2 * (y * y + z * z)
        yaw = np.arctan2(siny_cosp, cosy_cosp)

        return roll, pitch, yaw


def main(args=None):
    rclpy.init(args=args)
    move_turtlebot3 = MoveTurtleBot3()
    while True:
        try:
            rclpy.spin(move_turtlebot3)
        except:
            break
    # rclpy.spin(move_turtlebot3)
    move_turtlebot3.destroy_node()
    rclpy.shutdown()
    # return None


if __name__ == '__main__':
    main()

# End of Code

This is the output of my program: The yaw value never changes and stuck at 0.0.

user:~/ros2_ws$ ros2 launch topics_quiz topics_quiz.launch.py
[INFO] [launch]: All log files can be found below /home/user/.ros/log/2022-09-17-06-19-27-649269-4_xterm-10026
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [move_turtlebot3-1]: process started with pid [10028]
[move_turtlebot3-1] [INFO] [1663395568.602659797] [topics_quiz_node]: Moving front a little bit...
[move_turtlebot3-1] [INFO] [1663395569.104318385] [topics_quiz_node]: done
[move_turtlebot3-1] [INFO] [1663395569.104947102] [topics_quiz_node]: Moving Straight...
[move_turtlebot3-1] [INFO] [1663395576.110771928] [topics_quiz_node]: Turning 90 degs...
[move_turtlebot3-1] [INFO] [1663395576.111307290] [topics_quiz_node]: Yaw: 0.0
[move_turtlebot3-1] [INFO] [1663395576.612531274] [topics_quiz_node]: Yaw: 0.0
[move_turtlebot3-1] [INFO] [1663395577.114181184] [topics_quiz_node]: Yaw: 0.0
[move_turtlebot3-1] [INFO] [1663395577.615251677] [topics_quiz_node]: Yaw: 0.0
[move_turtlebot3-1] [INFO] [1663395578.116688925] [topics_quiz_node]: Yaw: 0.0
[move_turtlebot3-1] [INFO] [1663395578.618135039] [topics_quiz_node]: Yaw: 0.0
[move_turtlebot3-1] [INFO] [1663395579.119771268] [topics_quiz_node]: Yaw: 0.0
[move_turtlebot3-1] [INFO] [1663395579.621020665] [topics_quiz_node]: Yaw: 0.0
[move_turtlebot3-1] [INFO] [1663395580.122279405] [topics_quiz_node]: Yaw: 0.0
[move_turtlebot3-1] [INFO] [1663395580.623943868] [topics_quiz_node]: Yaw: 0.0
[move_turtlebot3-1] [INFO] [1663395581.125656957] [topics_quiz_node]: Yaw: 0.0

Please help!

Thanks,
Girish

roalgoal · September 19, 2022, 7:26am

Hi Girish,

Are you sure your code is entering the odom_callback()?

The only Message being printed out is from your method move() being invoked by timer_callback()

Simplify your subscriber creation until you can make sure you get that message, something like this:

subscription_ = this->create_subscription<std_msgs::msg::String>(
      "topic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1));

which is taken directly from the ROS2 documentation: Writing a simple publisher and subscriber (C++) — ROS 2 Documentation: Foxy documentation

girishkumar.kannan · September 19, 2022, 7:33am

Hi @roalgoal ,

With the self.move() line in my code commented out, my odom_callback() works.
But, with self.move() uncommented, my odom_callback() does not work !

No, currently timer_callback() has only pass in its code block. self.move() is called in __init__ as the last line.

~~Is there a python equivalent for simplifying Subscription?~~ This is same as creating a subscriber.

girishkumar.kannan · September 19, 2022, 6:11pm

Hi The Construct Team,

I still cannot get my subscriber callback working. I think the program is not entering into my odom_callback() and I really do not know why.

My observations:

With self.move() commented in both __init__ and timer_callback —> Program enters odom_callback and prints roll pitch and yaw correctly - verified by running ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.0, y: 0.0, z: 1.0}, angular: {x: 0.0, y: 0.0, z: 1.0}}"
If self.move() is uncommented in __init__, program never enters odom_callback and printed yaw value is always 0.0.
If self.move() is uncommented in timer_callback, program enters odom_callback only once and yaw values printed out is stuck at a random value that is the same.

@staff , somebody help me out please! I think you can run my code on your computer and check my output.
I can also provide the launch file and setup.py file if you need. I have been stuck for more than 2 days.

Here is my code: (made some changes from the code posted before)

import time
import rclpy
import numpy as np
from rclpy.node import Node
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
from rclpy.qos import ReliabilityPolicy, QoSProfile


class MoveTurtleBot3(Node):

    def __init__(self):
        super().__init__('topics_quiz_node')
        timer_period = 0.5
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.publisher_ = self.create_publisher(Twist, 'cmd_vel', 10)

        self.odom_init_done = False
        self.subscriber = self.create_subscription(
            Odometry, '/odom', self.odom_callback,
            QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE))
        self.twist_cmd = Twist()
        self.roll = 0.0
        self.pitch = 0.0
        self.yaw = 0.0

        # self.move()
        return None

    def move(self):
        # while not self.odom_init_done:
        #     pass
        # move straight till left opening
        self.get_logger().info("Moving Straight...")
        self.twist_cmd.linear.x = 0.5
        self.twist_cmd.angular.z = 0.0
        self.publisher_.publish(self.twist_cmd)
        time.sleep(7.00)
        self.twist_cmd.linear.x = 0.0
        self.twist_cmd.angular.z = 0.0
        self.publisher_.publish(self.twist_cmd)
        # turn left 90 degs or 1.57 rads
        self.get_logger().info("Turning 90 degs Left...")
        while (self.yaw < 1.569):
            # print(self.odom_msg)
            self.get_logger().info('Yaw: %s' % str(self.yaw))
            self.twist_cmd.linear.x = 0.0
            self.twist_cmd.angular.z = 0.1
            self.publisher_.publish(self.twist_cmd)
            time.sleep(0.5)
        self.twist_cmd.linear.x = 0.0
        self.twist_cmd.angular.z = 0.0
        self.publisher_.publish(self.twist_cmd)
        # move straight past through opening
        self.get_logger().info("Moving Straight...")
        self.twist_cmd.linear.x = 0.5
        self.twist_cmd.angular.z = 0.0
        self.publisher_.publish(self.twist_cmd)
        time.sleep(10.00)
        self.twist_cmd.linear.x = 0.0
        self.twist_cmd.angular.z = 0.0
        self.publisher_.publish(self.twist_cmd)
        return None

    def timer_callback(self):
        # self.move()
        # print("@timer:", self.roll, self.pitch, self.yaw)
        pass
        return None

    def odom_callback(self, odom_msg):
        # self.odom_msg = odom_msg
        quat = odom_msg.pose.pose.orientation
        # self.get_logger().info("Quaternion: %s" % str(quat))
        # self.get_logger().info("QuatX: %s" % quat.x)
        # self.get_logger().info("QuatY: %s" % quat.y)
        # self.get_logger().info("QuatZ: %s" % quat.z)
        # self.get_logger().info("QuatW: %s" % quat.w)
        self.roll, self.pitch, self.yaw = self.euler_from_quaternion(
            [quat.x, quat.y, quat.z, quat.w])
        self.get_logger().info("Roll: %s, Pitch: %s, Yaw: %s" %
                               (str(self.roll), str(self.pitch), str(self.yaw)))
        self.odom_init_done = True
        return None

    def euler_from_quaternion(self, quaternion):
        """
        Converts quaternion (w in last place) to euler roll, pitch, yaw
        quaternion = [x, y, z, w]
        Below should be replaced when porting for ROS2 Python tf_conversions is done.
        """
        x = quaternion[0]
        y = quaternion[1]
        z = quaternion[2]
        w = quaternion[3]

        sinr_cosp = 2 * (w * x + y * z)
        cosr_cosp = 1 - 2 * (x * x + y * y)
        roll = np.arctan2(sinr_cosp, cosr_cosp)

        sinp = 2 * (w * y - z * x)
        pitch = np.arcsin(sinp)

        siny_cosp = 2 * (w * z + x * y)
        cosy_cosp = 1 - 2 * (y * y + z * z)
        yaw = np.arctan2(siny_cosp, cosy_cosp)

        return (roll, pitch, yaw)


def main(args=None):
    rclpy.init(args=args)
    move_turtlebot3 = MoveTurtleBot3()
    # rclpy.spin(move_turtlebot3)
    while True:
        try:
            rclpy.spin(move_turtlebot3)
        except:
            break
    move_turtlebot3.destroy_node()
    rclpy.shutdown()
    return None


if __name__ == '__main__':
    main()

# End of Code

Thanks in advance,
Girish

duckfrost2 · September 20, 2022, 7:49am

Could it be a thread issue? Have you tried adding callback groups and at least two threads?

girishkumar.kannan · September 20, 2022, 7:53am

Hi @duckfrost2 ,

Thanks for the reply.

At the present moment in the course chapter, multi threading is not yet introduced.
I tried to use MultiThreadedExecutor, but I have no idea where to begin or how to implement.
In the same chapter, there is an example tutorial on using publisher and subscriber in the same code and that works fine.
I am not yet introduced to callback groups and I do not know how to implement one.

Is there any other way to solve this without using more than one thread at the moment?

Thanks,
Girish

duckfrost2 · September 20, 2022, 7:58am

I see, good point there.

I see in the code that you have a while inside the move which is executed by the timer every 0.5 seconds, which is more or less the time that the while will be there executing minimum, so basically it might not allow the odom callback to enter in the single thread.

I would suggest that the move function is NOT stopping with while loops. Normally you would do the evaluation and everything inside the odom if you want to control the single thread and not block anything. Use internal variable to check and store the state of movement of the robot so that inside the callback we can access that data and take decisions for the movement.

girishkumar.kannan · September 20, 2022, 8:02am

Hi @duckfrost2 ,

I thought of the exact same solution as the worst case scenario. I just wanted to know if there was any other way to get this working.
Anyways, I will try this and keep you (all) informed.

Thanks,
Girish

girishkumar.kannan · September 20, 2022, 10:27am

Hi @duckfrost2 ,

Tried it out and succeeded ! Topics quiz passed at the first attempt!

To others who get to check this post: Do not try to use my above codes. They will not work !
Did not post my final code here for obvious reasons.

Thanks,
Girish

girishkumar.kannan · September 20, 2022, 10:55am

@duckfrost2 ,

Sorry to post after marking this issue as solved, but I have a doubt.

I have finished ROS Basics in both Python and C++. I have never faced a problem like this in ROS1.
Why is this a problem in ROS2?

I am just curious, if there is a program with more than 2 publishers and more than 2 subscribers, will there be any deadlocks when you subscribe for more than 2 topics?
Although, I think there will not be any problem with publishers.

Thanks in advance,
Girish

bayodesegun · September 24, 2022, 7:00am

This topic was automatically closed after 3 days. New replies are no longer allowed.

duckfrost2 · January 2, 2023, 9:07am

Hi,

Very good question. The fact is that ROS2 has been overcomplicated from my point of view. In theory this way of managing callback gives you much more control, but I think that it make it much more complex to manage it. is like saying that C++ is better than python. In theory it is and for certain applications it sure is but at the end of the day, python is much faster when we want to create stuff.

The reason behind this new architecture only the people of OSRF know it I’m affraid