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 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
