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
