In this lecture, we will continue with composing a simple pick and place pipeline with different MoveGroup APIs.

Testing our pick and place pipeline:

Start the factory simulation.
$roslaunch hrwros_gazebo hrwros_environment.launch

In a new terminal source our setup files.
$ source $HOME/hrwros_ws/devel/setup.bash
$ roscd hrwros_week4/scripts/

Uncomment the following function in .../hrwros_week4/scripts/simple_pick_place.py (#L84):
robot1_client.wait_for_result()

Make the script executable and run it.
$ chmod +x simple_pick_place.py
$ rosrun hrwros_week4 simple_pick_place.py

Now let’s test the nonblocking execution. Open up .../hrwros_week4/scripts/simple_pick_place.py:

Remove the following function and restart the script (#L84).
robot1_client.wait_for_result()

Results of first robot:

• Preemption of the first goal
• Only the second was executed

Results second robot:

• Two separate clients which send information to the same server
• Blocking

Waypoints:

• Poses of robot end effector
• Timing synchronisation results in an incorrect pose
  • •  Issues with get_current_pose() API
    • Fixed with delay

current_pose = robot1_group.get_current_pose()
rospy.sleep(0.5)
current_pose = robot1_group.get_current_pose()

• Waypoints should only consist of pose messages
• Linear offsets with geometry_msgs.msg.Pose().

Pose stamped messages consists of timing and reference frame information along with the pose message type.  The extraction of the pose message can be done as following: current_pose.pose.position.x +0.10

Finally, we can add the newly created waypoint and the current pose.