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!