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