Example #1
0
    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
Example #4
0
    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)