Trouble creating Publisher/Subscriber Node

Hi everybody,

im currently working on changing my diff_drive robot to an ackerman steering robot.
Therefore im trying to use a node, thats a publisher and subscriber simultaneously, to turn the Twist Message of cmd_vel into steering angles for the front wheels and velocity commands for the back wheels.

My Code:

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64
from geometry_msgs.msg import Twist
from std_msgs.msg import String

wheel_distance=400 
wheel_separation=5750
wheel_radius=10

class Subscriber(Node):

    def __init__(self):
        super().__init__('angle_calculator')
        self.subscription = self.create_subscription(Twist,'/cmd_vel', self.listener_callback,10)
        self.subscription  #prevent unused variable warning

    def listener_callback(self, msg:Twist):

        #calculates steering angles
        steering_angle=msg.angular.z

        # model limits
        if steering_angle>0.45:
            steering_angle=0.45
        elif steering_angle<-0.45:
            steering_angle=-0.45

        try:
            # curve radius is considered way higher than wheel distance
            curve_radius=wheel_distance/abs(steering_angle)
        except:
            # avoid division by zero, curve_radius=0 means infinite
            curve_radius=0

        self.calc_angles(steering_angle, msg.linear.x, curve_radius)
    
    def calc_angles(self, steering_angle, speed, curve_radius):
        # standard ackermann geometry
        if curve_radius==0:
            # avoid division by zero
            left_angle=0
            right_angle=0
            left_speed=speed*wheel_radius
            right_speed=speed*wheel_radius
        else:
            # calculate steering angles
            inner_angle=wheel_distance/(curve_radius-wheel_separation/2)
            outer_angle=wheel_distance/(curve_radius+wheel_separation/2)

            # differential drive for the rear wheels
            inner_speed=speed*wheel_radius*(curve_radius-wheel_separation/2)/curve_radius
            outer_speed=speed*wheel_radius*(curve_radius+wheel_separation/2)/curve_radius

            if steering_angle<0:
                # right turn
                left_angle=-outer_angle
                right_angle=-inner_angle
                left_speed=outer_speed
                right_speed=inner_speed
            else:
                # left turn
                left_angle=inner_angle
                right_angle=outer_angle
                left_speed=inner_speed
                right_speed=outer_speed
        
        self.get_logger().info(str(left_angle))
        
        #fl_pub.publish(left_angle)
        #fr_pub.publish(right_angle)
        #rl_pub.publish(left_speed)
        #rr_pub.publish(right_speed)


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

    angle_calculator = Subscriber()
    #fl_pub = angle_calculator.create_publisher(Float64, '/tunnel_bot/front_left_steer_joint_position_controller/command', 10)
    #fr_pub = angle_calculator.create_publisher(Float64, '/tunnel_bot/front_right_steer_joint_position_controller/command', 10)
    #rl_pub = angle_calculator.create_publisher(Float64, '/tunnel_bot/rear_left_wheel_joint_velocity_controller/command', 10)
    #rr_pub = angle_calculator.create_publisher(Float64, '/tunnel_bot/rear_right_wheel_joint_velocity_controller/command', 10)


    rclpy.spin(angle_calculator)

    # Destroy the node explicitly
    angle_calculator.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

How can i add a publisher that publishes e.g. the left angle?
When i create the publishers in the main method (like in the commented sections in the code) i cant access it from inside the Subscriber Class. But thatā€™s where my data is. How can i access that data from the ā€œoutsideā€?

Thanks a lot in advance for any help!

I tried to initialize the publisher in the init function of the Subscriber Class as follows:

class Subscriber(Node):
    def __init__(self):
        super().__init__('angle_calculator')
        self.subscription = self.create_subscription(Twist,'/cmd_vel', self.listener_callback,10)
        self.subscription  #prevent unused variable warning
       
        #-----------NEW-------------------
        self.fl_pub = self.create_publisher(Float64, '/tunnel_bot/front_left_steer_joint_position_controller/command', 10)

im getting an error as soon as ā€˜cmd_velā€™ sends data:

[INFO] [launch]: All log files can be found below /home/ima-untow/.ros/log/2022-10-20-13-25-43-985515-fast-ima-robotics01-125978
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [cmd_vel_to_gazebo.py-1]: process started with pid [125984]
[cmd_vel_to_gazebo.py-1] [INFO] [1666265153.206694851] [angle_calculator]: 0
[cmd_vel_to_gazebo.py-1] Traceback (most recent call last):
[cmd_vel_to_gazebo.py-1]   File "/home/ima-untow/dev_ws/install/tunnel_bot/lib/tunnel_bot/cmd_vel_to_gazebo.py", line 99, in <module>
[cmd_vel_to_gazebo.py-1]     main()
[cmd_vel_to_gazebo.py-1]   File "/home/ima-untow/dev_ws/install/tunnel_bot/lib/tunnel_bot/cmd_vel_to_gazebo.py", line 89, in main
[cmd_vel_to_gazebo.py-1]     rclpy.spin(angle_calculator)
[cmd_vel_to_gazebo.py-1]   File "/opt/ros/foxy/lib/python3.8/site-packages/rclpy/__init__.py", line 191, in spin
[cmd_vel_to_gazebo.py-1]     executor.spin_once()
[cmd_vel_to_gazebo.py-1]   File "/opt/ros/foxy/lib/python3.8/site-packages/rclpy/executors.py", line 720, in spin_once
[cmd_vel_to_gazebo.py-1]     raise handler.exception()
[cmd_vel_to_gazebo.py-1]   File "/opt/ros/foxy/lib/python3.8/site-packages/rclpy/task.py", line 239, in __call__
[cmd_vel_to_gazebo.py-1]     self._handler.send(None)
[cmd_vel_to_gazebo.py-1]   File "/opt/ros/foxy/lib/python3.8/site-packages/rclpy/executors.py", line 431, in handler
[cmd_vel_to_gazebo.py-1]     await call_coroutine(entity, arg)
[cmd_vel_to_gazebo.py-1]   File "/opt/ros/foxy/lib/python3.8/site-packages/rclpy/executors.py", line 356, in _execute_subscription
[cmd_vel_to_gazebo.py-1]     await await_or_execute(sub.callback, msg)
[cmd_vel_to_gazebo.py-1]   File "/opt/ros/foxy/lib/python3.8/site-packages/rclpy/executors.py", line 118, in await_or_execute
[cmd_vel_to_gazebo.py-1]     return callback(*args)
[cmd_vel_to_gazebo.py-1]   File "/home/ima-untow/dev_ws/install/tunnel_bot/lib/tunnel_bot/cmd_vel_to_gazebo.py", line 39, in listener_callback
[cmd_vel_to_gazebo.py-1]     self.calc_angles(steering_angle, msg.linear.x, curve_radius)
[cmd_vel_to_gazebo.py-1]   File "/home/ima-untow/dev_ws/install/tunnel_bot/lib/tunnel_bot/cmd_vel_to_gazebo.py", line 73, in calc_angles
[cmd_vel_to_gazebo.py-1]     self.fl_pub.publish(left_angle)
[cmd_vel_to_gazebo.py-1]   File "/opt/ros/foxy/lib/python3.8/site-packages/rclpy/publisher.py", line 74, in publish
[cmd_vel_to_gazebo.py-1]     raise TypeError()
[cmd_vel_to_gazebo.py-1] TypeError
[ERROR] [cmd_vel_to_gazebo.py-1]: process has died [pid 125984, exit code 1, cmd '/home/ima-untow/dev_ws/install/tunnel_bot/lib/tunnel_bot/cmd_vel_to_gazebo.py --ros-args'].

which indicates an Type Error. But i dont understand why that is? I tried casting it to different types without any luck.
Can anyone explain how an Float64 Message has to be?

Thanks

EDIT: Sorry, I wasnā€™t thinking straight. Try the following where your publish was:

left_angle_msg = Float64()
left_angle_msg.data = left_angle
fl_pub.publish(left_angle_msg)

The issue is that the publisher needs to take a ROS msg and it canā€™t auto cast the variable youā€™ve made as it needs to be assigned to the data member. I think thereā€™s also a way you can create the message and assign the data on the one line with something like Float64().set_data(left_angle) or something like that but Iā€™m not certain of the syntax.


Original reply:

Ok Iā€™m not sure if this is it, but the first thing to try is to change the left_angle= assignment to be 0.0 rather than 0.
Because of Pythonā€™s dynamic typing, sometimes it can get a bit stuck because itā€™s trying to do things like assign an integer to a Float64 and failing (which can be very frustrating).

However you did say you have tried casting it with no luck so perhaps that is not the problem. Assuming that doesnā€™t work, can you repost the full code again as it currently stands?

Hi Josh,

first of all thanks alot for your quick reply, i really appreciate it.
Now it makes sense! I had to do both to make it work

  1. change the variable-assignment to left_angle = 0.0
  2. and then declare the actual message like you mentioned:
left_angle_msg = Float64()
left_angle_msg.data = left_angle
fl_pub.publish(left_angle_msg)

You really helped a ton, thanks Josh!

1 Like

Fantastic, Iā€™m glad it works!

1 Like