def test_2(self):
# Test 2: move turtlebot to a designated spot using amcl, map and move_base topic
rospy.loginfo("Begin test 2")
# Intialize the waypoint goal
goal = MoveBaseGoal()
# Use the map frame to define goal poses
goal.target_pose.header.frame_id = 'map'
rospy.loginfo("Define position relative to map")
# Set pose
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = 0.840401411057
goal.target_pose.pose.position.y = 0.674615263939
goal.target_pose.pose.orientation = Quaternion(0.0,0.0,-0.631627191685, 0.77527226877)
# Move to goal
move_to_goal.move_to_goal(goal)
rospy.loginfo("End test 2")
def stage_1(self):
# Move to initial position using move base goal
rospy.loginfo("Begin stage 1")
# Move turtlebot 1 to starting position
# Intialize the waypoint goal
goal = MoveBaseGoal()
# Use the map frame to define goal poses
goal.target_pose.header.frame_id = 'map'
# Set pose
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = 0.840401411057
goal.target_pose.pose.position.y = 0.674615263939
goal.target_pose.pose.orientation = Quaternion(0.0, 0.0,
-0.631627191685,
0.77527226877)
# Move to goal
move_to_goal.move_to_goal(goal)
# Scan for ar marker and get position + offset to reposition turtlebot
# Set pose THIS IS WRONG, based upon AR marker which has frame id rgb camera
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = 0.0927959606051
goal.target_pose.pose.position.y = -0.157733559608
goal.target_pose.pose.orientation = Quaternion(0.0, 0.0,
-0.0746215653308,
0.0235089268487)
# Move to goal
move_to_goal.move_to_goal(goal, 'turtlebot_1')
# Move turtlebot 2 to starting position
# Intialize the waypoint goal
goal = MoveBaseGoal()
# Use the map frame to define goal poses
goal.target_pose.header.frame_id = 'map'
# Set pose
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = 0.840401411057
goal.target_pose.pose.position.y = 0.674615263939
goal.target_pose.pose.orientation = Quaternion(0.0, 0.0,
-0.631627191685,
0.77527226877)
# Move to goal
move_to_goal.move_to_goal(goal)
# Scan for ar marker and get position + offset to reposition turtlebot
# Set pose THIS IS WRONG, based upon AR marker which has frame id rgb camera
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = 0.0927959606051
goal.target_pose.pose.position.y = -0.157733559608
goal.target_pose.pose.orientation = Quaternion(0.0, 0.0,
-0.0746215653308,
0.0235089268487)
# Move to goal
move_to_goal.move_to_goal(goal, 'turtlebot_2')
def stage_1(self):
# Move to initial position using move base goal
rospy.loginfo("Begin stage 1")
# Move turtlebot to starting position
# Intialize the waypoint goal
goal = MoveBaseGoal()
# Use the map frame to define goal poses
goal.target_pose.header.frame_id = 'map'
# Set pose
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = 0.840401411057
goal.target_pose.pose.position.y = 0.674615263939
goal.target_pose.pose.orientation = Quaternion(0.0,0.0,-0.631627191685, 0.77527226877)
# Move to goal
move_to_goal.move_to_goal(goal)
# Scan for ar marker and get position + offset to reposition turtlebot
# Set pose THIS IS WRONG, based upon AR marker which has frame id rgb camera
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = 0.0927959606051
goal.target_pose.pose.position.y = -0.157733559608
goal.target_pose.pose.orientation = Quaternion(0.0,0.0,-0.0746215653308, 0.0235089268487)
# Move to goal
move_to_goal.move_to_goal(goal)