# Creating control for limo robot

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

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):
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()