class ControlSphero(object):
def __init__(self, goal_distance):
self._goal_distance = goal_distance
self.init_direction_service_client()
self.init_rec_odom_action_client()
self.init_move_sphero_publisher()
def init_direction_service_client(self,
service_name="/crash_direction_service"):
rospy.loginfo('Waiting for Service Server')
rospy.wait_for_service(
service_name
) # wait for the service client /gazebo/delete_model to be running
rospy.loginfo('Service Server Found...')
self._direction_service = rospy.ServiceProxy(
service_name, Trigger) # create the connection to the service
self._request_object = TriggerRequest()
def make_direction_request(self):
result = self._direction_service(
self._request_object
) # send the name of the object to be deleted by the service through the connection
return result.message
def init_rec_odom_action_client(self):
self._rec_odom_action_client = actionlib.SimpleActionClient(
'/rec_odom_as', record_odomAction)
# waits until the action server is up and running
rospy.loginfo('Waiting for action Server')
self._rec_odom_action_client.wait_for_server()
rospy.loginfo('Action Server Found...')
self._rec_odom_action_goal = record_odomGoal()
def send_goal_to_rec_odom_action_server(self):
self._rec_odom_action_client.send_goal(
self._rec_odom_action_goal,
feedback_cb=self.rec_odom_feedback_callback)
def rec_odom_feedback_callback(self, feedback):
rospy.loginfo("Rec Odom Feedback feedback ==>" + str(feedback))
def rec_odom_finished(self):
has_finished = (self._rec_odom_action_client.get_state() >= 2)
return has_finished
def get_result_rec_odom(self):
return self._rec_odom_action_client.get_result()
def init_move_sphero_publisher(self):
self._cmdvelpub_object = CmdVelPub()
def move_sphero(self, direction):
self._cmdvelpub_object.move_robot(direction)
def got_out_maze(self, odom_result_array):
return check_if_out_maze(self._goal_distance, odom_result_array)
def __init__(self):
self.rotated = False
self.positioned = False
self.complete = False # Service is complete
self._turning_direction = "right"
self._turning_selected = False
self._rate = rospy.Rate(SRV_RATE)
self._cmdvelpub_object = CmdVelPub()
self._lasersub_object = LaserTopicReader()
self.srv = rospy.Service('/find_wall', FindWall, self.srv_cb)
rospy.loginfo("Find Wall Service Server Up")
def init_robot_navigation(self):
self._cmdvelpub_object = CmdVelPub()
self._lasersub_object = LaserTopicReader()
time.sleep(1) # time to initialize
class RobotControl(object):
def __init__(self):
rospy.loginfo("Initializing Robot Control")
self._rate = rospy.Rate(NAV_RATE)
self.init_findwall_service_client()
self.init_rec_odom_action_client()
self.init_robot_navigation()
def init_findwall_service_client(self):
service_name = '/find_wall'
rospy.loginfo('Waiting for Service Server')
rospy.wait_for_service(service_name)
rospy.loginfo('Service Server Found...')
self._findwall_service = rospy.ServiceProxy(service_name, FindWall)
self._request_object = FindWallRequest()
def make_findwall_request(self):
rospy.loginfo('Service request done')
result = self._findwall_service(self._request_object)
return result
def init_rec_odom_action_client(self):
self._rec_odom_action_client = actionlib.SimpleActionClient('/record_odom',OdomRecordAction)
# waits until the action server is up and running
rospy.loginfo('Waiting for action Server')
self._rec_odom_action_client.wait_for_server()
rospy.loginfo('Action Server Found...')
self._rec_odom_action_goal = OdomRecordGoal()
def send_goal_to_rec_odom_action_server(self):
self._rec_odom_action_client.send_goal(self._rec_odom_action_goal, feedback_cb=self.rec_odom_feedback_callback)
def rec_odom_feedback_callback(self,feedback):
rospy.loginfo("Rec Odom Feedback feedback ==>"+str(feedback))
def rec_odom_finished(self):
has_finished = ( self._rec_odom_action_client.get_state() >= 2 )
return has_finished
def get_result_rec_odom(self):
return self._rec_odom_action_client.get_result()
def init_robot_navigation(self):
self._cmdvelpub_object = CmdVelPub()
self._lasersub_object = LaserTopicReader()
time.sleep(1) # time to initialize
def navigate_robot(self):
while(not self.rec_odom_finished()):
laser_reading = self._lasersub_object.wall_detector()
if laser_reading['front'] < NAV_FRONT_MARGIN:
self.move_robot('forwardsleft', linearspeed=NAV_LINEAR_SPEED, angularspeed=NAV_ANGULAR_SPEED_HIGH)
elif laser_reading['right'] >= NAV_RIGHT_MARGIN_MAX:
self.move_robot('forwardsright', linearspeed=NAV_LINEAR_SPEED, angularspeed=NAV_ANGULAR_SPEED_LOW)
elif laser_reading['right'] < NAV_RIGHT_MARGIN_MIN:
self.move_robot('forwardsleft', linearspeed=NAV_LINEAR_SPEED, angularspeed=NAV_ANGULAR_SPEED_LOW)
else:
self.move_robot('forwards', linearspeed=NAV_LINEAR_SPEED)
self._rate.sleep()
robot_control_object.move_robot('stop')
self._rate.sleep()
def move_robot(self, direction, linearspeed=0,angularspeed=0):
self._cmdvelpub_object.move_robot(direction,linearspeed=linearspeed, angularspeed=angularspeed)
def init_move_sphero_publisher(self):
self._cmdvelpub_object = CmdVelPub()
class FindWallService(object):
def __init__(self):
self.rotated = False
self.positioned = False
self.complete = False # Service is complete
self._turning_direction = "right"
self._turning_selected = False
self._rate = rospy.Rate(SRV_RATE)
self._cmdvelpub_object = CmdVelPub()
self._lasersub_object = LaserTopicReader()
self.srv = rospy.Service('/find_wall', FindWall, self.srv_cb)
rospy.loginfo("Find Wall Service Server Up")
# Service Callback
def srv_cb(self,request):
self.complete = False # Service can be called again
# rospy.loginfo("Complete: " + str(self.complete))
response = False
rospy.loginfo("Find Wall Service started")
# Subsriber definition
response = FindWallResponse()
while not self.complete:
self.srv_processing()
self._rate.sleep()
response.wallfound = True
return response
def srv_processing(self):
## message of type LaserScan
msg = self._lasersub_object.get_laserdata()
current_position = np.argmin(msg.ranges)
if not self.rotated and not self.positioned: ## Get the correct rotation
rospy.loginfo("Rotating #1: %s", current_position)
target_position = SRV_TARGET_FRONT_ANGLE # front should face the wall
margin_rotation = SRV_ROTATION_MARGIN
if not self._turning_selected: # Avoids strange behaviour
self._turning_direction = 'right' if (target_position - current_position >= 0) else 'left'
self._turning_selected = True
if np.abs(target_position - current_position) > margin_rotation:
self.move_robot(self._turning_direction, angularspeed=SRV_ROTATION_SPEED)
else:
self.move_robot('stop')
self.rotated = True
rospy.loginfo("Rotating #1 Completed")
if self.rotated and not self.positioned: # get close to the wall
target_position = SRV_TARGET_FRONT_ANGLE
distance_front = msg.ranges[target_position]
margin_distance = SRV_TARGET_FRONT_DISTANCE
rospy.loginfo("Positioning %s", distance_front)
if distance_front > margin_distance:
self.move_robot('forwards', linearspeed=SRV_LINEAR_SPEED)
else:
self.move_robot('stop')
self.positioned = True
self.rotated = False
rospy.loginfo("Positioning Completed")
if not self.rotated and self.positioned:
rospy.loginfo("Rotating #2: %s", current_position)
target_position = SRV_TARGET_RIGHT_ANGLE # 90 # front should face the wall
margin_rotation = SRV_ROTATION_MARGIN
if np.abs(target_position - current_position) > margin_rotation:
self.move_robot('left', angularspeed=SRV_ROTATION_SPEED)
else:
self.move_robot('stop')
self.rotated = True
rospy.loginfo("Rotating #2 Completed")
if self.rotated and self.positioned:
# rospy.loginfo("Service Completed")
self.move_robot('stop')
time.sleep(1)
# self.sub.unregister()
self.complete = True
def move_robot(self, direction, linearspeed=0,angularspeed=0):
self._cmdvelpub_object.move_robot(direction,linearspeed=linearspeed, angularspeed=angularspeed)