Esempio n. 1
0
def main():
    robot = turtlebot.robot.build_real()
    db = location_db.build_real()
    api = RobotApi(robot, db)
    rospy.Service('code_it/api/display_message', DisplayMessage,
                  api.on_display_message)
    rospy.Service('code_it/api/ask_multiple_choice', AskMultipleChoice,
                  api.on_ask_multiple_choice)
    rospy.Service('code_it/api/go_to', GoTo, api.on_go_to)
    rospy.Service('code_it/api/go_to_dock', GoToDock, api.on_go_to_dock)
    rospy.Subscriber("code_it/is_program_running", Bool, api.on_is_program_running)
            return GetPoseByNameResponse()
        else:
            return GetPoseByNameResponse(exists=True, pose_stamped=pose_stamped)

    def on_list(self, request):
        return ListPosesResponse(names=self._db.list())

    def on_upsert(self, request):
        self._db.upsert(request.name, request.pose_stamped)
        return UpsertPoseResponse()

    def on_save_current(self, request):
        pose_stamped = get_current_location(self._tf_listener, 'base_footprint', 'map')
        if pose_stamped is None:
            raise rospy.ServiceException('Failed to save current location.')
        self._db.upsert(request.name, pose_stamped)
        return SaveCurrentPoseResponse()


if __name__ == '__main__':
    rospy.init_node('location_server')
    db = build_real()
    server = Server(db)
    rospy.Service('location_db/delete', DeletePose, server.on_delete)
    rospy.Service('location_db/get_by_name', GetPoseByName,
                  server.on_get_by_name)
    rospy.Service('location_db/list', ListPoses, server.on_list)
    rospy.Service('location_db/upsert', UpsertPose, server.on_upsert)
    rospy.Service('location_db/save_current', SaveCurrentPose, server.on_save_current)
    rospy.spin()