Creating control for limo robot

ROS
1

Im trying to move LIMO robot from point A to point B in straight line with linear speed 0.5m/s and distance of 0.5m. After receiving feedback using odometry. Make a 0.8rad/s of turn with the speed of 0.5m/s from point B to point C and have odometry pickup the data again.

Heres some of the code I wrote but it does not work as intended hoping that someone can help correct this

import rospy
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from math import radians

class LIMORobotController:
def init(self):
rospy.init_node(‘limo_robot_controller’, anonymous=True)
self.cmd_vel_pub = rospy.Publisher(‘/cmd_vel’, Twist, queue_size=10)
self.odom_sub = rospy.Subscriber(‘/odom’, Odometry, self.odom_callback)
self.odometry_data = None
self.distance_to_point_B = 0.5
self.distance_to_point_C = 0.5
self.linear_speed = 0.5
self.angular_speed = 0.8
self.rate = rospy.Rate(10) # 10 Hz

def odom_callback(self, data):
    self.odometry_data = data

def move_to_point_B(self):
    initial_position_x = self.odometry_data.pose.pose.position.x
    while abs(initial_position_x - self.odometry_data.pose.pose.position.x) < self.distance_to_point_B:
        twist = Twist()
        twist.linear.x = self.linear_speed
        self.cmd_vel_pub.publish(twist)
        self.rate.sleep()
    self.stop_robot()

def turn_to_point_C(self):
    initial_orientation_z = self.odometry_data.pose.pose.orientation.z
    target_angle = initial_orientation_z + radians(90)  # Turning 90 degrees
    while abs(self.odometry_data.pose.pose.orientation.z - target_angle) > 0.05:
        twist = Twist()
        twist.angular.z = self.angular_speed
        self.cmd_vel_pub.publish(twist)
        self.rate.sleep()
    self.stop_robot()

def move_to_point_C(self):
    initial_position_x = self.odometry_data.pose.pose.position.x
    while abs(initial_position_x - self.odometry_data.pose.pose.position.x) < self.distance_to_point_C:
        twist = Twist()
        twist.linear.x = self.linear_speed
        self.cmd_vel_pub.publish(twist)
        self.rate.sleep()
    self.stop_robot()

def stop_robot(self):
    twist = Twist()
    self.cmd_vel_pub.publish(twist)

def run(self):
    rospy.loginfo("Moving to point B")
    self.move_to_point_B()
    rospy.loginfo("Reached point B, turning towards point C")
    self.turn_to_point_C()
    rospy.loginfo("Turned towards point C, moving to point C")
    self.move_to_point_C()
    rospy.loginfo("Reached point C, task complete")

if name == ‘main’:
limo_controller = LIMORobotController()
limo_controller.run()

3

This is probably not the right forum to ask questions about non-UR Robots in ROS.
Check the ROS discourse forum: Search results for 'Limo' - ROS Discourse