The goal of the program is to use a service to make the Turtlebot move in a square when requested and to give a Response upon completion. Its is also given that the robot needs to stop and the service needs to be stopped if at any point an obstacle is detected.
The service server file :
#! usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
from turtlebot3_move.srv import MoveInSquare,MoveInSquareResponse
class RealRobo:
def __init__(self):
rospy.init_node("Turtlebot_Node")
self.TW=Twist()
self.Resp_obj=MoveInSquareResponse()
self.pub=rospy.Publisher("/cmd_vel",Twist,queue_size=1)
self.sub=rospy.Subscriber("/scan",LaserScan,self.callback)
# self.Obst_detect=False
while self.pub.get_num_connections()<1:
print("waiting for publisher /cmd_vel---------")
print("-------------------------------------")
#start the service:
service_server_sq = rospy.Service('/move_in_square', MoveInSquare , self.my_callback)
def move_forward(self):
self.TW.linear.x=0.1
print("Forward-----")
self.pub.publish(self.TW)
def stop(self):
self.TW.linear.x=0
self.TW.angular.z=0
print("Stop---")
self.pub.publish(self.TW)
def move_straight_15(self):
self.TW.linear.x=0.05
self.pub.publish(self.TW)
rospy.sleep(5)
def turn_90(self):
self.TW.angular.z=0.3
print("Turning 90")
self.pub.publish(self.TW)
rospy.sleep(5.5)
self.stop()
def callback(self,msg):
if (min(msg.ranges)>0.2):
self.Obst_detect=False
else:
print("Obstacle detected",min(msg.ranges))
self.Obst_detect=True
self.stop()
def move_in_square(self):
self.move_straight_15()
self.stop()
self.turn_90()
self.move_straight_15()
self.stop()
self.turn_90()
self.move_straight_15()
self.stop()
self.turn_90()
self.move_straight_15()
self.stop()
def my_callback(self,msg):
print("------------Service Called-------------")
if(not self.Obst_detect):
print("No obstacle when service called")
self.move_in_square()
self.Resp_obj.complete=True
return self.Resp_obj
if self.Obst_detect:
self.stop()
print("Obstacle when service called")
self.Resp_obj.complete=False
return self.Resp_obj
if __name__=="__main__":
while (not rospy.is_shutdown()):
RR=RealRobo()
rospy.spin()
The Service calling file:
#! /usr/bin/env python
import rospy
from turtlebot3_move.srv import MoveInSquare,MoveInSquareRequest
import sys
rospy.init_node('service_client')
rospy.wait_for_service('/move_in_square')
client_pipeline = rospy.ServiceProxy('/move_in_square', MoveInSquare)
result = client_pipeline(MoveInSquareRequest())
print(result)
Upon running the program, it works when an obstacle is detected at the start of the program, then the service is stopped immediately.
But when the robot detects an obstacle mid traveling, it continues to move in square, that is continues the module def move_in_square(self): because the program can only check the control loop or if condition after the program to move the robot in square completes.
Is there a way around this to stop the robot, that is stop the execution of the contents of def move_in_square(self):midway when an obstacle is detected.
Thanks in advace
)