def manual_mode_callback(self, mode_msg):
self.manual_mode = mode_msg.data
if self.manual_mode:
control_msg = AckermannDrive()
control_msg.speed = 0
control_msg.steering_angle = 0
self.pub_control.publish(control_msg)
def pos_err_callback(self, pos_err_msg):
pos_err = self.target - pos_err_msg.position.x
rospy.loginfo("Current error: pos_err = " + str(pos_err))
servo_pos = self.control_servo(pos_err)
motor_speed = self.motor_speed
rospy.loginfo("Control command: servo_pos = " + str(servo_pos) + ", motor_speed = " + str(motor_speed))
control_msg = AckermannDrive()
control_msg.speed = motor_speed
control_msg.steering_angle = servo_pos
self.pub_control.publish(control_msg)
def pos_callback(self, pos_msg):
if self.manual_mode:
rospy.loginfo("Mannual mode is on. Not running control")
else:
pos_err = self.target - pos_msg.position.x
rospy.loginfo("Current error: pos = {:.2f}".format(pos_err))
servo_pos = self.control_servo(pos_err)
motor_speed = self.motor_speed if servo_pos == 0 else self.motor_speed+20
rospy.loginfo("Control command: servo_pos = " + str(servo_pos) + ", motor_speed = " + str(motor_speed))
control_msg = AckermannDrive()
control_msg.speed = motor_speed
control_msg.steering_angle = servo_pos
self.pub_control.publish(control_msg)