def __init__(self): self.status = map_mode() self.status.mode = self.status.MAP_A self.status_pub = rospy.Publisher("/global_map_status", map_mode) self.set_mode_server = rospy.Service("/set_map_mode", setMapMode, self.set_mode_cb) self.get_mode_server = rospy.Service("/get_map_mode", getMapMode, self.get_mode_cb)
def server_cb(self, transition_req): # pretty much all we have to do is send the message to the mode_mapper # saying that we're now in marker mode, and giving the initial marker ... map_mode_msg = map_mode() set_mode_req = setMapModeRequest() set_mode_req.mode = map_mode_msg.MARKER_MODE set_mode_req.marker = transition_req.marker_id self.map_client(set_mode_req) transition_resp = mapMarkerTransitionResponse() return transition_resp
def server_cb(self, transition_req): map_mode_msg = map_mode() # re-init appropriate amcl estimator ... resetAMCLreq = EmptyRequest() # this is ugly, but the marker_map_transition code uses a diff enum than the various messages going through the mode_mapper.py if transition_req.map_id == map_mode_msg.MAP_A: self.amcl1_client(resetAMCLreq) elif transition_req.map_id == map_mode_msg.MAP_B: self.amcl2_client(resetAMCLreq) else: rospy.logerror("in markerMapTransitioner.server_cb, requested invalid map_id!! request was: %r", transition_req) # give amcl the prior as to where the robot is now ... pose_msg = PoseWithCovarianceStamped() pose_msg.header.stamp = rospy.Time.now() position = Point(transition_req.map_x, transition_req.map_y, 0.0) qq = tf.transformations.quaternion_from_euler(0, 0, transition_req.map_th) orientation = Quaternion(*qq) pose_msg.pose.pose.position = position pose_msg.pose.pose.orientation = orientation # copying covariance estimate from rviz .cpp file: covariance = 36*[0.0] covariance[0] = 0.5**2 covariance[7] = 0.5**2 covariance[21] = (math.pi/12.0)**2 pose_msg.pose.covariance = covariance if transition_req.map_id == map_mode_msg.MAP_A: pose_msg.header.frame_id = "/map1" self.amcl1_pose_pub.publish(pose_msg) elif transition_req.map_id == map_mode_msg.MAP_B: pose_msg.header.frame_id = "/map2" self.amcl2_pose_pub.publish(pose_msg) # send info to the mapper module to keep track of what mode we're in # rosservice call set_map_mode map_frame={0, 1} -1 set_mode_req = setMapModeRequest() if transition_req.map_id == map_mode_msg.MAP_A: set_mode_req.mode = map_mode_msg.MAP_A elif transition_req.map_id == map_mode_msg.MAP_B: set_mode_req.mode = map_mode_msg.MAP_B self.map_client(set_mode_req) # need to instantiate an empty response message transition_resp = markerMapTransitionResponse() return transition_resp
def run_test_script(self): # this script assumes that the robot is currently located at marker 35 # initialize the mode_mapper map_mode_msg = map_mode() init_mapper_req = setMapModeRequest() init_mapper_req.mode = map_mode.MARKER_MODE init_mapper_req.marker = 35 self.marker_transition_service(init_mapper_req) trail_1_2 = [0, 1, 2, 3, 4, 5, 6, 7, 8] trail_2_1 = [10, 11, 12, 13, 14, 15, 16, 17, 18] # send the robot to room 2/B ar_req = markerFollowerRequest() ar_req.ids = trail_1_2 self.ar_nav_service(ar_req) #transition the robot to the map mode in room 2/B map_transition_req = markerMapTransitionRequest() map_transition_req.map_id = map_transition_req.MAP_B map_transition.req.map_x = 0.56 map_transition.req.map_y = 2.03 map_transition.req.map_th = -1.56 self.map_transition_service(map_transition_req) # move the robot around in the room ... map_plan_req = mapPlannerRequest() map_plan_req.frame_id = "/map2" map_plan_req.goal_x = 1 map_plan_req.goal_y = 0 map_plan_req.goal_th = 0 self.map_nav_service(map_plan_req) map_plan_req.goal_x = 0.63 map_plan_req.goal_y = 1.24 map_plan_req.goal_x = 1.63 self.map_nav_service(map_plan_req) # transition into marker-following mode marker_transition_req = mapMarkerTransitionRequest() marker_transition_req.marker_id = 10 self.marker_transition_service(marker_transition_req) # follow markers back to room 1 # send the robot to room 2/B ar_req = markerFollowerRequest() ar_req.ids = trail_2_1 self.ar_nav_service(ar_req) # transition marker->map in room 1/A map_transition_req = markerMapTransitionRequest() map_transition_req.map_id = map_transition_req.MAP_A map_transition.req.map_x = -0.55 map_transition.req.map_y = -0.05 map_transition.req.map_th = 0 self.map_transition_service(map_transition_req) # move the robot around in the room ... map_plan_req = mapPlannerRequest() map_plan_req.frame_id = "/map1" map_plan_req.goal_x = 2 map_plan_req.goal_y = 0 map_plan_req.goal_th = 0 self.map_nav_service(map_plan_req)