def execute(self, userdata): move_base_override.move_speed(0.1, duration=2.0) rospy.sleep(0.3) reset_pose.reset(robot_pose.position.x, 0.08, radians(-90)) rospy.sleep(0.2) # wait for reset pose move_base_override.move(-0.03, duration=1.0) goal = MoveBaseGoal() goal.target_pose.header.frame_id = 'odom' goal.target_pose.header.stamp = rospy.get_rostime() goal.target_pose.pose = robot_pose goal.target_pose.pose.orientation = Quaternion( *quaternion_from_euler(0, 0, radians(-180))) client = actionlib.SimpleActionClient('move_base', MoveBaseAction) client.wait_for_server() client.send_goal(goal) client.wait_for_result() if TEAM == Team.GREEN: move_base_override.move(0.05, duration=1.0) else: move_base_override.move(-0.05, duration=1.0) return Transitions.SUCCESS
def init_robot_pose(): # Initialise robot pose x, y = mirror_point(0.105, 0.900 + 0.21 / 2) if TEAM == Team.VIOLET: reset_pose.reset(x, y, radians(0)) else: reset_pose.reset(x, y, radians(-180))
def execute(self, userdata): move_base_override.move_speed(0.1, duration=2.0) rospy.sleep(0.3) reset_pose.reset(robot_pose.position.x, 0.08, radians(-90)) rospy.sleep(0.2) # wait for reset pose move_base_override.move(-0.03, duration=1.0) goal = MoveBaseGoal() goal.target_pose.header.frame_id = 'odom' goal.target_pose.header.stamp = rospy.get_rostime() goal.target_pose.pose = robot_pose goal.target_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, radians(-180))) client = actionlib.SimpleActionClient('move_base', MoveBaseAction) client.wait_for_server() client.send_goal(goal) client.wait_for_result() if TEAM == Team.GREEN: move_base_override.move(0.05, duration=1.0) else: move_base_override.move(-0.05, duration=1.0) return Transitions.SUCCESS