How to run robot from web UI
I have my robot working with hardcoded instruction of pickup and drop and when I run the node it works properly but now I want to pass the same instruction through web so I have used pub-sub method where the webpage publishes the instruction and subscriber node gets the instruction and runs the robot.
The problem here is robot is running on callback functions and subscriber callback looses the control after running the first request and it does not know if the robot is running or stopped and when second request comes in, so it starts executing the another request parallelly.
I am not the right way to do this
Here's my subscriber code which runs for single request but fails if multiple request comes because subscriber callback function does not know if robot is running or stopped
import math
import actionlib
import rospy
import tf
from geometry_msgs.msg import Pose, Point, Quaternion, Twist
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from std_msgs.msg import Bool, String
from tf.transformations import quaternion_from_euler, euler_from_quaternion
class MoveBase:
def __init__(self):
self.goal_cnt = 0
self.pickup_load = Bool()
self.int_status = Bool()
self.quat_seq = list()
self.pose_seq = list()
self.rad_seq = list()
self.disable_interrupt_flag = False
self.pause_flag = False
self.lf_flag = True
self.orient_correct_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
self.pickup_status_pub = rospy.Publisher('pickup_load_status', Bool, queue_size=1)
self.int_status_pub = rospy.Publisher('interrupt_status', Bool, queue_size=1)
self.finish_status = rospy.Publisher('finish_status', Bool, queue_size=1)
self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
def reverse(self, interval):
cmd = Twist()
rate = rospy.Rate(10)
current_millis = rospy.get_time()
previous_millis = rospy.get_time()
while not (current_millis - previous_millis > interval):
cmd.linear.x = -0.2
cmd.angular.z = 0
self.orient_correct_pub.publish(cmd)
current_millis = rospy.get_time()
rospy.loginfo("%f %f", current_millis, previous_millis)
def pause_routine_cb(self, data):
if data.data:
self.pause_flag = True
else:
self.pause_flag = False
def lf_status_cb(self, data):
if data.data:
self.lf_flag = True
else:
self.lf_flag = False
def servo_control(self, goal_count):
rates = rospy.Rate(10)
if goal_count == len(self.pose_seq) - 1:
self.pickup_load.data = False
elif goal_count % 2 == 0:
self.pickup_load.data = True
elif goal_count % 2 != 0:
self.pickup_load.data = False
for z in range(10):
self.pickup_status_pub.publish(self.pickup_load)
rates.sleep()
def send_goal_routine(self, pose_seq, goal_count, result=True):
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "odom"
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose = pose_seq[goal_count]
rospy.loginfo("Sending goal pose " + str(goal_count + 1) + " to Action Server")
rospy.loginfo(str(pose_seq[goal_count]))
if result:
self.client.send_goal(goal, self.done_cb, self.active_cb)
else:
self.client.send_goal(goal)
wait = self.client.wait_for_result()
if not wait:
rospy.logerr("Action server not available!")
rospy.signal_shutdown("Action server not available!")
else:
return self.client.get_result()
def orientation_correction(self, pose_seq, target_angle, goal_count):
cmd = Twist()
listener = tf.TransformListener()
rate = rospy.Rate(10)
rate2 = rospy.Rate(100)
while not rospy.is_shutdown():
# rospy.loginfo(line_follower.pause_flag)
while self.pause_flag:
self.disable_interrupt_flag = True
rospy.loginfo("inside pauseflag while loop")
rate2.sleep()
if not self.lf_flag:
self.lf_flag = True
rospy.loginfo("break of orientation correction's while loop")
break
try:
(trans, rot) = listener.lookupTransform('/map', '/base_link', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
(roll, pitch, yaw) = euler_from_quaternion(rot)
act_angle_deg = yaw * (180 / math.pi)
tar_angle_deg = (target_angle * 180 / math.pi)
error = tar_angle_deg - act_angle_deg
rospy.loginfo(error)
if error >= 0:
# anti_clockwise()
cmd.linear.x = 0
cmd.angular.z = 0.6
self.orient_correct_pub.publish(cmd)
elif error < 0:
# clockwise()
cmd.linear.x = 0
cmd.angular.z = -0.6
self.orient_correct_pub.publish(cmd)
if -0.6 < error < 0.6:
if goal_count == len(pose_seq) - 1:
rospy.loginfo("in break1")
cmd.linear.x = 0
cmd.angular.z = 0
for z in range(10):
self.orient_correct_pub.publish(cmd)
rate.sleep()
break
if (goal_count % 2 == 0) and trans[1] > (pose_seq[goal_count].position.y - 0.30):
for temp in range(10):
cmd.linear.x = 0.2
cmd.angular.z = 0
self.orient_correct_pub.publish(cmd)
rate.sleep()
elif (goal_count % 2 != 0) and trans[1] < (pose_seq[goal_count].position.y + 0.25):
for temp in range(10):
cmd.linear.x = 0.2
cmd.angular.z = 0
self.orient_correct_pub.publish(cmd)
rate.sleep()
else:
rospy.loginfo("in break2")
cmd.linear.x = 0
cmd.angular.z = 0
for z in range(10):
self.orient_correct_pub.publish(cmd)
rate.sleep()
break
rate.sleep()
def done_cb(self, status, result):
rate = rospy.Rate(0.1)
rospy.loginfo("IN DONE CB")
disable_interrupt_flag = False
self.goal_cnt += 1
if status == 3:
if self.goal_cnt < len(self.pose_seq):
# enable_interrupt()
self.int_status.data = True
self.int_status_pub.publish(self.int_status)
self.orientation_correction(self.pose_seq, self.rad_seq[self.goal_cnt - 1], self.goal_cnt - 1)
if (not self.disable_interrupt_flag) and (self.goal_cnt < len(self.pose_seq)):
self.int_status.data = False
self.int_status_pub.publish(self.int_status)
self.servo_control(self.goal_cnt - 1)
rospy.loginfo("Goal pose " + str(self.goal_cnt) + " reached")
if self.goal_cnt < len(self.pose_seq):
self.reverse(1)
self.send_goal_routine(self.pose_seq, self.goal_cnt)
else:
self.finish_status.publish(True)
rospy.loginfo("Final goal pose reached!")
rospy.signal_shutdown("Final goal pose reached!")
return True
def active_cb(self):
rospy.loginfo("IN ACTIVE CB")
def feedback_cb(self, feedback):
rospy.loginfo("IN FEEDBACK CB")
def move_base(self, points_seq):
self.finish_status.publish(False)
rospy.Subscriber("pause_status", Bool, self.pause_routine_cb)
rospy.Subscriber("lf_status", Bool, self.lf_status_cb)
for i, temp in enumerate(points_seq):
if (i + 1) % 3 == 0:
self.quat_seq.append(Quaternion(*(quaternion_from_euler(0, 0, points_seq[i], axes='sxyz'))))
self.rad_seq.append(points_seq[i])
n = 3
points = [points_seq[i:i + n] for i in range(0, len(points_seq), n)]
for point in points:
self.pose_seq.append(Pose(Point(*point), self.quat_seq[n - 3]))
n += 1
try:
rospy.loginfo("Waiting for move_base action server...")
wait = self.client.wait_for_server(rospy.Duration(5.0))
if not wait:
rospy.logerr("Action server not available!")
rospy.signal_shutdown("Action server not available!")
self.send_goal_routine(self.pose_seq, self.goal_cnt)
rospy.spin()
except rospy.ROSInterruptException:
rospy.loginfo("Navigation finished.")
def callback(data):
rospy.loginfo(rospy.get_caller_id() + "message %s", data.data)
points_seq = eval(data.data)
obj = MoveBase()
obj.move_base(points_seq)
def listener():
rospy.init_node('move_base_sequence', anonymous=True)
rospy.Subscriber('aiv_ui', String, callback)
rospy.spin()
if __name__ == '__main__':
listener()
Asked by abhishekj on 2022-11-08 02:22:13 UTC
Comments