Merge pull request #370 from jrgnicho/fix/demo-motion-planning-config…

…uration

Fixes for Pick and Place Demo motion planning

.check_training_config.bash

@@ -83,6 +83,7 @@ function check_debs() {
   check_deb libglu1-mesa-dev
   check_deb pcl-tools
   check_deb python3-argcomplete
+  check_deb python3-vcstool
    echo "Checking ROS1 packages:"
   check_deb python3-catkin-tools
   check_deb ros-$ROS_RELEASE-desktop

exercises/Perception-Driven_Manipulation/ros2/solution_ws/src/pick_and_place_application/config/pick_and_place_parameters.yaml

@@ -14,6 +14,7 @@ box_place_x: -0.4
 box_place_y: 0.6
 box_place_z: 0.17
 touch_links: [wrist_1_link, wrist_2_link, wrist_3_link, tcp_frame]
-planner_id: BFMT # RRTConnect
+planner_id: BFMT
+#planner_id: RRTConnect
 planning_time: 30.0
 planning_attempts: 5

exercises/Perception-Driven_Manipulation/ros2/solution_ws/src/pick_and_place_application/src/tasks/detect_box_pick.cpp

@@ -31,8 +31,7 @@ geometry_msgs::msg::Pose pick_and_place_application::PickAndPlaceApp::detectBox(
    * Goal:
    * - Call target recognition service and save the result.
    * Hint:
-   * - Observe how to check that the service response is ready.
-   * - See to access the target_pose in the response in order to copy it into box_pose variable.
+   * - use the service client to send the request object to the service server
    */
   geometry_msgs::msg::Pose box_pose;
 

exercises/Perception-Driven_Manipulation/ros2/solution_ws/src/pick_and_place_application/src/tasks/move_to_wait_position.cpp

@@ -24,24 +24,23 @@ void pick_and_place_application::PickAndPlaceApp::moveToWaitPosition()
   // setting up planning configuration
   moveit_cpp::PlanningComponent planning_component(cfg.ARM_GROUP_NAME, moveit_cpp);
   moveit_cpp::PlanningComponent::PlanRequestParameters plan_parameters;
-  plan_parameters.planner_id = "RRTConnectkConfigDefault";
   plan_parameters.load(node);
-  plan_parameters.planning_time = 20.0f;
-  plan_parameters.planning_attempts = 4;
+  plan_parameters.planner_id = cfg.PLANNER_ID;
+  plan_parameters.planning_time = cfg.PLANNING_TIME;
+  plan_parameters.planning_attempts = cfg.PLANNING_ATTEMPTS;
 
   /*
 
      Fill Code:
      * Goal:
      * - Set robot wait target
      * Hints:
-     * - Use the "setGoal" method in the "planning_component" object.
      * - The "WAIT_POSE_NAME" variable is a member of the "cfg" object.
      */
   planning_component.setGoal(cfg.WAIT_POSE_NAME);
 
   // now plan the trajectory
-  moveit_cpp::PlanningComponent::PlanSolution plan_solution = planning_component.plan();
+  moveit_cpp::PlanningComponent::PlanSolution plan_solution = planning_component.plan(plan_parameters);
   if (plan_solution)
   {
     RCLCPP_INFO_STREAM(node->get_logger(), "Move " << cfg.WAIT_POSE_NAME << " Succeeded");
@@ -57,8 +56,8 @@ void pick_and_place_application::PickAndPlaceApp::moveToWaitPosition()
    * Goal:
    * - Execute the trajectory on the robot
    * Hints:
-   * - Use the "execute" method in the "moveit_cpp" object and save the result
-   *  in the "succeeded" variable
+   * - The "execute" method takes 3 parameters: planning group name, trajectory, and blocking.
+   * - The robot trajectory is the primary result of the "plan()" call above, along with other data.
    */
 
   bool succeeded = false;

exercises/Perception-Driven_Manipulation/ros2/solution_ws/src/pick_and_place_application/src/tasks/pickup_box.cpp

@@ -29,7 +29,7 @@ void pick_and_place_application::PickAndPlaceApp::doBoxPickup(std::vector<geomet
      * - Check that the robot state is valid.
      * Hints:
      * - Use the "moveit_cpp->getCurrentState(...)" method to get the current state from the environment.
-     * - It is possible to evaluate the "robot_state" object inside an if statement.
+     * - getCurrentState takes a timeout input parameter.  A timeout of ~2 seconds is probably fine.
      */
     moveit::core::RobotStatePtr robot_state = nullptr;
     robot_state = moveit_cpp->getCurrentState(2.0);
@@ -63,6 +63,7 @@ void pick_and_place_application::PickAndPlaceApp::doBoxPickup(std::vector<geomet
      * - Execute the planned trajectory
      * Hints:
      * - Use the "moveit_cpp->execute(...)" method to execute the trajectory on the robot
+     * - You'll need to pass the arm group name and trajectory
      */
     success = false;
     success = moveit_cpp->execute(cfg.ARM_GROUP_NAME, plan_solution.trajectory, true);

exercises/Perception-Driven_Manipulation/ros2/solution_ws/src/pick_and_place_application/src/tasks/set_gripper.cpp

@@ -37,12 +37,12 @@ void pick_and_place_application::PickAndPlaceApp::actuateGripper(bool do_grasp)
    * - Confirm that a valid result was sent by the action server.
    * Hints:
    * - Use the "wait_for" method of the goal_fut future object to wait for completion.
-   * - Give "wait_for" a timeout value of 4 seconds
-   * - The "wait_for" method takes an argument of the "std::chrono::duration<double>(4)" type.
-   * - Save the output of wait for in the status variable
+   * - 4 seconds is a reasonable timeout to wait for the gripper action result
+   * - "wait_for" uses std::chrono::duration to specify the timeout
+   * - look up std::chrono::seconds or std::chrono_literals to specify the time
    */
   std::future_status status = std::future_status::deferred;
-  status = goal_fut.wait_for(std::chrono::duration<double>(4));
+  status = goal_fut.wait_for(std::chrono::seconds(4));
 
   if (status == std::future_status::ready)
   {

exercises/Perception-Driven_Manipulation/ros2/solution_ws/src/ur5_workcell_moveit2_config/config/ompl_planning.yaml

@@ -69,14 +69,14 @@ planner_configs:
     extended_fmt: 1  # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1
   BFMT:
     type: geometric::BFMT
-    num_samples: 100  # number of states that the planner should sample. default: 1000
-    radius_multiplier: 2.0  # multiplier used for the nearest neighbors search radius. default: 1.0
-    nearest_k: 1  # use the Knearest strategy. default: 1
+    num_samples: 30  # number of states that the planner should sample. default: 1000
+    radius_multiplier: 100.0  # multiplier used for the nearest neighbors search radius. default: 1.0
+    nearest_k: 0  # use the Knearest strategy. default: 1
     balanced: 1  # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1
     optimality: 0  # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1
-    heuristics: 1  # activates cost to go heuristics. default: 1
-    cache_cc: 1  # use the collision checking cache. default: 1
-    extended_fmt: 1  # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1
+    heuristics: 0  # activates cost to go heuristics. default: 1
+    cache_cc: 0  # use the collision checking cache. default: 1
+    extended_fmt: 0  # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1
   PDST:
     type: geometric::PDST
   STRIDE:
@@ -153,7 +153,7 @@ manipulator:
     - LazyPRMstar
     - SPARS
     - SPARStwo
-  projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint, elbow_joint)
+  projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint)
   longest_valid_segment_fraction: 0.001
 
 vacuum_gripper:

exercises/Perception-Driven_Manipulation/ros2/template_ws/src/dependencies.repos

@@ -15,5 +15,3 @@ repositories:
     type: git
     url: https://github.com/ros-perception/perception_pcl.git
     version: foxy-devel
-
-

exercises/Perception-Driven_Manipulation/ros2/template_ws/src/pick_and_place_application/config/pick_and_place_parameters.yaml

@@ -14,6 +14,7 @@ box_place_x: -0.4
 box_place_y: 0.6
 box_place_z: 0.17
 touch_links: [wrist_1_link, wrist_2_link, wrist_3_link, tcp_frame]
-planner_id: BFMT # RRTConnect
+planner_id: BFMT
+#planner_id: RRTConnect
 planning_time: 30.0
 planning_attempts: 5

exercises/Perception-Driven_Manipulation/ros2/template_ws/src/pick_and_place_application/src/tasks/create_pick_moves.cpp

@@ -25,7 +25,7 @@ pick_and_place_application::PickAndPlaceApp::computePickToolPoses(geometry_msgs:
    * Goal:
    * - Compute the TCP pose at the box pick location.
    * Hints:
-   * - Use the "setOrigin" to set the position of "world_to_tcp_tf".
+   * - Use the "setOrigin" to set the position of "tcp_at_box_tf".
    */
   box_tf = tf2::poseMsgToTransform(box_pose);
   tf2::Vector3 box_position(box_pose.position.x, box_pose.position.y, box_pose.position.z);

exercises/Perception-Driven_Manipulation/ros2/template_ws/src/pick_and_place_application/src/tasks/detect_box_pick.cpp

@@ -31,8 +31,7 @@ geometry_msgs::msg::Pose pick_and_place_application::PickAndPlaceApp::detectBox(
    * Goal:
    * - Call target recognition service and save the result.
    * Hint:
-   * - Observe how to check that the service response is ready.
-   * - See to access the target_pose in the response in order to copy it into box_pose variable.
+   * - use the service client to send the request object to the service server
    */
   geometry_msgs::msg::Pose box_pose;
 

exercises/Perception-Driven_Manipulation/ros2/template_ws/src/pick_and_place_application/src/tasks/move_to_wait_position.cpp

@@ -24,24 +24,23 @@ void pick_and_place_application::PickAndPlaceApp::moveToWaitPosition()
   // setting up planning configuration
   moveit_cpp::PlanningComponent planning_component(cfg.ARM_GROUP_NAME, moveit_cpp);
   moveit_cpp::PlanningComponent::PlanRequestParameters plan_parameters;
-  plan_parameters.planner_id = "RRTConnectkConfigDefault";
   plan_parameters.load(node);
-  plan_parameters.planning_time = 20.0f;
-  plan_parameters.planning_attempts = 4;
+  plan_parameters.planner_id = cfg.PLANNER_ID;
+  plan_parameters.planning_time = cfg.PLANNING_TIME;
+  plan_parameters.planning_attempts = cfg.PLANNING_ATTEMPTS;
 
   /*
 
      Fill Code:
      * Goal:
      * - Set robot wait target
      * Hints:
-     * - Use the "setGoal" method in the "planning_component" object.
      * - The "WAIT_POSE_NAME" variable is a member of the "cfg" object.
      */
   // UNCOMMENT AND COMPLETE: planning_component.setGoal(...);
 
   // now plan the trajectory
-  moveit_cpp::PlanningComponent::PlanSolution plan_solution = planning_component.plan();
+  moveit_cpp::PlanningComponent::PlanSolution plan_solution = planning_component.plan(plan_parameters);
   if (plan_solution)
   {
     RCLCPP_INFO_STREAM(node->get_logger(), "Move " << cfg.WAIT_POSE_NAME << " Succeeded");
@@ -57,8 +56,8 @@ void pick_and_place_application::PickAndPlaceApp::moveToWaitPosition()
    * Goal:
    * - Execute the trajectory on the robot
    * Hints:
-   * - Use the "execute" method in the "moveit_cpp" object and save the result
-   *  in the "succeeded" variable
+   * - The "execute" method takes 3 parameters: planning group name, trajectory, and blocking.
+   * - The robot trajectory is the primary result of the "plan()" call above, along with other data.
    */
 
   bool succeeded = false;

exercises/Perception-Driven_Manipulation/ros2/template_ws/src/pick_and_place_application/src/tasks/pickup_box.cpp

@@ -29,7 +29,7 @@ void pick_and_place_application::PickAndPlaceApp::doBoxPickup(std::vector<geomet
      * - Check that the robot state is valid.
      * Hints:
      * - Use the "moveit_cpp->getCurrentState(...)" method to get the current state from the environment.
-     * - It is possible to evaluate the "robot_state" object inside an if statement.
+     * - getCurrentState takes a timeout input parameter.  A timeout of ~2 seconds is probably fine.
      */
     moveit::core::RobotStatePtr robot_state = nullptr;
     // UNCOMMENT AND COMPLETE: robot_state = moveit_cpp->...;

exercises/Perception-Driven_Manipulation/ros2/template_ws/src/pick_and_place_application/src/tasks/set_gripper.cpp

@@ -38,9 +38,9 @@ void pick_and_place_application::PickAndPlaceApp::actuateGripper(bool do_grasp)
    * - Confirm that a valid result was sent by the action server.
    * Hints:
    * - Use the "wait_for" method of the goal_fut future object to wait for completion.
-   * - Give "wait_for" a timeout value of 4 seconds
-   * - The "wait_for" method takes an argument of the "std::chrono::duration<double>(4)" type.
-   * - Save the output of wait for in the status variable
+   * - 4 seconds is a reasonable timeout to wait for the gripper action result
+   * - "wait_for" uses std::chrono::duration to specify the timeout
+   * - look up std::chrono::seconds or std::chrono_literals to specify the time
    */
   std::future_status status = std::future_status::deferred;
   // UNCOMMENT AND COMPLETE: status = goal_fut.wait_for(...);

exercises/Perception-Driven_Manipulation/ros2/template_ws/src/ur5_workcell_moveit2_config/config/ompl_planning.yaml

@@ -69,14 +69,14 @@ planner_configs:
     extended_fmt: 1  # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1
   BFMT:
     type: geometric::BFMT
-    num_samples: 100  # number of states that the planner should sample. default: 1000
-    radius_multiplier: 2.0  # multiplier used for the nearest neighbors search radius. default: 1.0
-    nearest_k: 1  # use the Knearest strategy. default: 1
+    num_samples: 30  # number of states that the planner should sample. default: 1000
+    radius_multiplier: 100.0  # multiplier used for the nearest neighbors search radius. default: 1.0
+    nearest_k: 0  # use the Knearest strategy. default: 1
     balanced: 1  # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1
     optimality: 0  # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1
-    heuristics: 1  # activates cost to go heuristics. default: 1
-    cache_cc: 1  # use the collision checking cache. default: 1
-    extended_fmt: 1  # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1
+    heuristics: 0  # activates cost to go heuristics. default: 1
+    cache_cc: 0  # use the collision checking cache. default: 1
+    extended_fmt: 0  # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1
   PDST:
     type: geometric::PDST
   STRIDE:
@@ -153,7 +153,7 @@ manipulator:
     - LazyPRMstar
     - SPARS
     - SPARStwo
-  projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint, elbow_joint)
+  projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint)
   longest_valid_segment_fraction: 0.001
 
 vacuum_gripper:

gh_pages/_downloads/ros-industrial-training-setup.sh

@@ -5,7 +5,7 @@ IS_AWS=${IS_AWS:-"$(expr "`head -c 3 /sys/hypervisor/uuid 2>/dev/null`" == "ec2"
 
 sudo apt update -y
 sudo apt upgrade -y
-sudo apt install -y curl gnupg2 lsb-release git meld build-essential libfontconfig1 mesa-common-dev libglu1-mesa-dev
+sudo apt install -y curl gnupg2 lsb-release git meld build-essential libfontconfig1 mesa-common-dev libglu1-mesa-dev python3-vcstool
 
 cd $HOME
 
@@ -63,4 +63,7 @@ if [ $IS_AWS -eq 1 ]; then
 
   # disable terminal auto-sourcing
   sed -E -i 's/^([^#].*source \/opt\/ros\/.*\/setup\..*)$/#\1/' ~/.bashrc
+
+  # enable bash auto-completion
+  echo "[[ -e /etc/profile.d/bash_completion.sh ]] && source /etc/profile.d/bash_completion.sh" >> ~/.bashrc
 fi

gh_pages/_source/demo1/10-Create-place-moves.rst

@@ -13,15 +13,15 @@ Locate Function
   ``application.computePlaceToolPoses()``.
 * Go to the source file of that function by clicking in any part of the
   function and pressing :kbd:`F2` in QtCreator.
-* Remove the first line containing the following ``ROS_ERROR_STREAM ...`` so
+* Remove the first line containing the following ``RCLCPP_ERROR_STREAM ...`` so
   that the program runs.
 
 
 Complete Code
 -------------
 
 * Find every line that begins with the comment ``Fill Code:`` and read the
-  description. Then, replace every instance of the comment ``ENTER CODE HERE``
+  description. Then, replace every instance of the comment ``UNCOMMENT AND COMPLETE``
   with the appropriate line of code.
 
   .. code-block:: cpp
@@ -31,7 +31,7 @@ Complete Code
         .
         .
     */
-    /* ========  ENTER CODE HERE ======== */
+    /* UNCOMMENT AND COMPLETE: */
 
 * The position of the box at the place location is saved in the global variable
   ``cfg.BOX_PLACE_TF``.
@@ -53,7 +53,7 @@ Build Code and Run
 
       colcon build
 
-* Run the supporting nodes with the launch file:
+* Run the supporting nodes with the launch file (only if needed):
 
   .. code-block:: shell
 

gh_pages/_source/demo1/11-Place-box.rst

@@ -13,15 +13,15 @@ Locate Function
 * In the main program, locate the function call to ``application.doBoxPlace()``.
 * Go to the source file of that function by clicking in any part of the
   function and pressing :kbd:`F2` in QtCreator.
-* Remove the first line containing the following ``ROS_ERROR_STREAM ...`` so
+* Remove the first line containing the following ``RCLCPP_ERROR_STREAM ...`` so
   that the program runs.
 
 
 Complete Code
 -------------
 
 * Find every line that begins with the comment ``Fill Code:`` and read the
-  description. Then, replace every instance of the comment ``ENTER CODE HERE``
+  description. Then, replace every instance of the comment ``UNCOMMENT AND COMPLETE``
   with the appropriate line of code.
 
   .. code-block:: cpp
@@ -31,7 +31,7 @@ Complete Code
         .
         .
     */
-    /* ========  ENTER CODE HERE ======== */
+    /* UNCOMMENT AND COMPLETE: ... */
 
 * The |execute()| method sends a motion plan to the robot.
 
@@ -49,7 +49,7 @@ Build Code and Run
 
       colcon build
 
-* Run the supporting nodes with the launch file:
+* Run the supporting nodes with the launch file (only if needed):
 
   .. code-block:: shell
 

gh_pages/_source/demo1/2-Package-Setup.rst

@@ -65,6 +65,8 @@ select ``colcon`` as the build system, and provide the path to your ``perception
 directory as the workspace path. Click :guilabel:`Next` and :guilabel:`Finish` to 
 complete the setup.
 
+Rebuild the workspace inside of QTCreator, to create the metadata required for IDE code completion.
+
 
 Open the Main Thread Source File
 --------------------------------

gh_pages/_source/demo1/3-Start-in-simulation-mode.rst

@@ -11,7 +11,8 @@ In a terminal enter:
 
 .. code-block:: shell
 
-  ros2 launch pick_and_place_application application_setup.launch
+  source ~/perception_driven_ws/install/setup.bash
+  ros2 launch pick_and_place_application application_setup.launch.py
 
 Rviz will display all the workcell components including the robot in its default position; at this stage your system is ready. However, no motion will take place until we run the pick and place node.
 
@@ -24,5 +25,5 @@ additional arguments to the launch file.
 
 .. code-block:: shell
 
-  ros2 launch pick_and_place_application application_setup.launch \
+  ros2 launch pick_and_place_application application_setup.launch.py \
       use_sim_robot:=false robot_ip:={robot IP address}