Merge pull request #353 from jrgnicho/fix/update-pick-and-place-demo-…

…in-code-comments

Fix/update pick and place demo in code comments

exercises/Perception-Driven_Manipulation/ros2/solution_ws/src/.gitignore

@@ -0,0 +1,2 @@
+__pycache__/
+ *.pyc

exercises/Perception-Driven_Manipulation/ros2/solution_ws/src/pick_and_place_application/include/pick_and_place_application/pick_and_place.h

@@ -61,7 +61,7 @@ class PickAndPlaceApp
   std::shared_ptr<tf2_ros::TransformListener> transform_listener;
 
   // =============================== Task Functions ===============================
-  bool initialize();
+  void initialize();
 
   void moveToWaitPosition();
 

exercises/Perception-Driven_Manipulation/ros2/solution_ws/src/pick_and_place_application/include/pick_and_place_application/pick_and_place_utilities.h

@@ -37,15 +37,19 @@ moveit_msgs::msg::Constraints createPathOrientationConstraints(const geometry_ms
                                                                float z_tolerance,
                                                                std::string link_name);
 
+/**
+ * @class PickAndPlaceConfig
+ * @details Contains the configuration parameters for the pick and place application
+ */
 class PickAndPlaceConfig
 {
 public:
   // =============================== Parameters ===============================
   std::string ARM_GROUP_NAME;             // MoveIt Planning Group associated with the robot arm
-  std::string TCP_LINK_NAME;              // Link / frame name for the suction gripper tool-tip
-  std::string ATTACHED_OBJECT_LINK_NAME;  // attached object link in robot
+  std::string TCP_LINK_NAME;              // Name of the gripper tool-tip link
+  std::string ATTACHED_OBJECT_LINK_NAME;  // Name of the object to be attached to the robot TCP
   std::string WORLD_FRAME_ID;             // Frame name for the fixed world reference frame
-  std::string AR_TAG_FRAME_ID;            // Frame name for the "AR Tag" mounted to the target box
+  std::string AR_TAG_FRAME_ID;            // Frame name for the "AR Tag" pasted on the box
   std::string HOME_POSE_NAME;             // Named pose for robot Home position (set in SRDF)
   std::string WAIT_POSE_NAME;             // Named pose for robot WAIT position (set in SRDF)
   tf2::Vector3 BOX_SIZE;                  // Size of the target box
@@ -61,12 +65,12 @@ class PickAndPlaceConfig
   std::string GRASP_ACTION_NAME;        // Action name used to control suction gripper
   std::string MARKER_TOPIC;             // Topic for publishing visualization of attached object.
   std::string PLANNING_SCENE_TOPIC;     // Topic for publishing the planning scene
-  std::string GET_TARGET_POSE_SERVICE;  // service for requesting box pick pose
-  std::string MOTION_PLAN_SERVICE;      // service for requesting moveit for a motion plan
+  std::string GET_TARGET_POSE_SERVICE;  // Service for requesting box pick pose
+  std::string MOTION_PLAN_SERVICE;      // Service for requesting Moveit for a motion plan
 
   // =============================== Messages and variables ===============================
-  visualization_msgs::msg::Marker MARKER_MESSAGE;     // visual representation of target object
-  moveit_msgs::msg::CollisionObject ATTACHED_OBJECT;  // attached object message
+  visualization_msgs::msg::Marker MARKER_MESSAGE;     // Visual representation of target object
+  moveit_msgs::msg::CollisionObject ATTACHED_OBJECT;  // Attached object message
   geometry_msgs::msg::Pose TCP_TO_BOX_POSE;
 
   PickAndPlaceConfig()

exercises/Perception-Driven_Manipulation/ros2/solution_ws/src/pick_and_place_application/src/nodes/pick_and_place_node.cpp

@@ -3,6 +3,7 @@
 
 using namespace pick_and_place_application;
 
+// create a logger to print messages into the terminal
 static const rclcpp::Logger LOGGER = rclcpp::get_logger("pick_and_place_node");
 
 // =============================== Main Thread ===============================
@@ -15,8 +16,7 @@ int main(int argc, char** argv)
   /*	INITIALIZING ROS NODE
       Goal:
       - Observe all steps needed to properly initialize a ros node.
-      - Look into the 'cfg' member of PickAndPlace to take notice of the parameters that
-        are available for the rest of the program. */
+      - Take notice of how the node's spin() method is invoked from within a thread.  This avoids locking the application.
   /* =========================================================================================*/
 
   RCLCPP_INFO(LOGGER, "Initialize node");
@@ -36,10 +36,7 @@ int main(int argc, char** argv)
   PickAndPlaceApp application(node);
 
   // initializing application
-  if (!application.initialize())
-  {
-    return false;
-  }
+  application.initialize();
 
   /* ========================================*/
   /* Pick & Place Tasks                      */

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

@@ -2,12 +2,9 @@
 
 /* CREATE MOTION PLAN
   Goal:
-    - Creates a motion plan request using the desired end-effector pose and the current
-        robot state (joint configuration and payload status)
-    - Calls the moveit motion planning service and returns the motion plan if a valid one is
-        found.
-
-  Hints:
+    - Learn how to use the moveit_cpp::PlanningComponent class to plan a trajectory that moves the robot from
+       the current robot state (joint configuration and payload status) to a desired pose of the tcp.
+    - Understand how to use the kinematic_constraints::constructGoalConstraints to create a goal cartesian pose for the tcp.
 
 */
 
@@ -20,11 +17,11 @@ bool PickAndPlaceApp::doMotionPlanning(const geometry_msgs::msg::Pose& pose_targ
   // constructing motion plan goal constraints
   std::vector<double> position_tolerances(3, 0.01f);
   std::vector<double> orientation_tolerances(3, 0.01f);
-  geometry_msgs::msg::PoseStamped p;
-  p.header.frame_id = cfg.WORLD_FRAME_ID;
-  p.pose = pose_target;
-  moveit_msgs::msg::Constraints pose_goal = kinematic_constraints::constructGoalConstraints(
-      cfg.TCP_LINK_NAME, p, position_tolerances, orientation_tolerances);
+  geometry_msgs::msg::PoseStamped goal_pose;
+  goal_pose.header.frame_id = cfg.WORLD_FRAME_ID;
+  goal_pose.pose = pose_target;
+  moveit_msgs::msg::Constraints robot_goal = kinematic_constraints::constructGoalConstraints(
+      cfg.TCP_LINK_NAME, goal_pose, position_tolerances, orientation_tolerances);
 
   if (!moveit_cpp->getPlanningSceneMonitor()->requestPlanningSceneState())
   {
@@ -43,7 +40,7 @@ bool PickAndPlaceApp::doMotionPlanning(const geometry_msgs::msg::Pose& pose_targ
   moveit::core::RobotState start_robot_state(moveit_cpp->getRobotModel());
   moveit::core::robotStateMsgToRobotState(start_robot_state_msg, start_robot_state);
   planning_component.setStartState(start_robot_state);
-  planning_component.setGoal({ pose_goal });
+  planning_component.setGoal({ robot_goal });
 
   // planning for goal
   plan_solution = planning_component.plan(plan_parameters);

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

@@ -3,36 +3,30 @@
 
 /* CREATE PICK MOVES
   Goal:
-    - Set the pose for the tcp at the box pick.
-    - Create tcp poses for the pick motions (approach, target, retreat).
-    - Find transform of the wrist in tcp coordinates
-    - Convert tcp pick poses to wrist poses.
-         * Moveit's kinematics require the target position to be specified relative to
-           one of the kinematic links of the manipulator arm (as defined in the SRDF)
+    - Set the pose for the TCP at the box pick.
+    - Create tcp poses for the pick positions (approach, target, retreat).
 
   Hints:
     - You can manipulate the "world_to_tcp_tf" transform through the "setOrigin" and "setRotation".
     - Look into the "create_manipulation_poses" function and observe how each pick pose is created.
-    - Use the "transform_from_tcp_to_wrist" function to populate the "wrist_pick_poses" array.
 */
 
 std::vector<geometry_msgs::msg::Pose>
 pick_and_place_application::PickAndPlaceApp::computePickToolPoses(geometry_msgs::msg::Pose& box_pose)
 {
-  // ROS_ERROR_STREAM("create_pick_moves is not implemented yet.  Aborting."); exit(1);
+  // RCLCPP_ERROR_STREAM(node->get_logger(), "create_pick_moves is not implemented yet.  Aborting."); exit(1);
 
   // transforms relative to world
-  tf2::Transform tcp_at_box_tf;  // transform of tcp at box relative to world
+  tf2::Transform tcp_at_box_tf;  // transform of tcp at box pick location relative to world
   tf2::Transform box_tf;         // transform of box at box relative to world
   std::vector<geometry_msgs::msg::Pose> tcp_pick_poses;
 
   /* Fill Code:
    * Goal:
-   * - Create tcp pose at box pick.
+   * - 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".
    */
-  // tf2::poseMsgToTF(box_pose,world_to_box_tf);
   box_tf = tf2::poseMsgToTransform(box_pose);
   tf2::Vector3 box_position(box_pose.position.x, box_pose.position.y, box_pose.position.z);
   tcp_at_box_tf.setOrigin(box_position);

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

@@ -4,21 +4,16 @@
 /*    CREATE PLACE MOVES
   Goal:
     - Set the pose of the tcp at the box place pose
-    - Create tcp poses for the place motion (approach, release, retreat).
-    - Find transform of the wrist in tcp coordinates
-    - Convert tcp pick poses to wrist poses.
-         * MoveIt's kinematics require the target position to be specified relative to
-           one of the kinematic links of the manipulator arm (as defined in the SRDF)
+    - Create tcp poses for the place locations (approach, release, retreat).
 
   Hints:
     - You can manipulate the "world_to_tcp_tf" transform through the "setOrigin" and "setRotation".
     - Use the "create_manipulation_poses" function to create the tcp poses between each place move
-    - Use the "transform_from_tcp_to_wrist" function to populate the "wrist_place_poses" array.
 */
 
 std::vector<geometry_msgs::msg::Pose> pick_and_place_application::PickAndPlaceApp::computePlaceToolPoses()
 {
-  // ROS_ERROR_STREAM("create_place_moves is not implemented yet.  Aborting."); exit(1);
+  // RCLCPP_ERROR_STREAM(node->get_logger(), "create_place_moves is not implemented yet.  Aborting."); exit(1);
 
   // task variables
   tf2::Transform tcp_at_box_tf, tcp_to_wrist_tf;
@@ -27,9 +22,9 @@ std::vector<geometry_msgs::msg::Pose> pick_and_place_application::PickAndPlaceAp
 
   /* Fill Code:
    * Objective:
-   * - Find the desired tcp pose at box place
+   * - Compute the TCP pose at the box place location
    * Hints:
-   * - Use the "setOrigin" method to set the position of "world_to_tcp_tf"
+   * - Use the "setOrigin" method to set the position of "tcp_at_box_tf"
    * 	using cfg.BOX_PLACE_TF.
    * - cfg.BOX_PLACE_TF is a tf::Transform object so it provides a getOrigin() method.
    */
@@ -39,7 +34,7 @@ std::vector<geometry_msgs::msg::Pose> pick_and_place_application::PickAndPlaceAp
    * Goal:
    * - Reorient the tool so that the tcp points towards the box.
    * Hints:
-   * - Use the "setRotation" to set the orientation of "world_to_tcp_tf".
+   * - Use the "setRotation" to set the orientation of "tcp_at_box_tf".
    * - The quaternion value "tf::Quaternion(0.707, 0.707, 0, 0)" will point
    * 	the tcp's direction towards the box.
    */
@@ -50,21 +45,10 @@ std::vector<geometry_msgs::msg::Pose> pick_and_place_application::PickAndPlaceAp
    * - Create place poses for tcp.   *
    * Hints:
    * - Use the "create_manipulation_poses" and save results to "tcp_place_poses".
-   * - Look in the "cfg" object to find the corresponding retreat and approach distance
-   * 	values.
+   * - The RETREAT_DISTANCE and APPROACH_DISTANCE values were populated from a configuration yaml file passed to the executable in the launch file.
    */
   tcp_place_poses = createManipulationPoses(cfg.RETREAT_DISTANCE, cfg.APPROACH_DISTANCE, tcp_at_box_tf);
 
-  /* Fill Code:
-   * Goal:
-   * - Transform list of place poses from the tcp to the wrist coordinate frame.
-   * Hints:
-   * - Use the "transform_from_tcp_to_wrist" function and save results into
-   * 	"wrist_place_poses".
-   * - The "tcp_to_wrist_tf" is the transform that will help convert "tcp_place_poses"
-   * 	into "wrist_place_poses".
-   */
-
   // printing results
   RCLCPP_INFO_STREAM(node->get_logger(),
                      "tcp position at place: "

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

@@ -3,16 +3,15 @@
 
 /* DETECTING BOX PICK POSE
   Goal:
-    - Find the box's position in the world frame using the transform listener.
-        * this transform is published by the kinect AR-tag perception node
-    - Save the pose into 'box_pose'.
+    - Get the box location in the world frame by calling the target recognition service.
+    - Return the box pose.
 */
 geometry_msgs::msg::Pose pick_and_place_application::PickAndPlaceApp::detectBox()
 {
   using RequestType = pick_and_place_msgs::srv::GetTargetPose::Request;
   using ResponseType = pick_and_place_msgs::srv::GetTargetPose::Response;
 
-  // RCLCPP_ERROR_STREAM(node,"detect_box_pick is not implemented yet.  Aborting."); exit(1);
+  // RCLCPP_ERROR_STREAM(node->get_logger(),"detect_box_pick is not implemented yet.  Aborting."); exit(1);
 
   // creating shape for recognition
   shape_msgs::msg::SolidPrimitive shape;
@@ -30,34 +29,38 @@ geometry_msgs::msg::Pose pick_and_place_application::PickAndPlaceApp::detectBox(
 
   /* Fill Code:
    * Goal:
-   * - Call target recognition service and save results.
+   * - Call target recognition service and save the result.
    * Hint:
-   * - Use the service response member to access the
-   * 	detected pose "srv.response.target_pose".
-   * - Assign the target_pose in the response to the box_pose variable in
-   * 	order to save the results.
+   * - 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.
    */
   geometry_msgs::msg::Pose box_pose;
-  std::shared_future<ResponseType::SharedPtr> response_fut = target_recognition_client->async_send_request(req);
+
+  // send request asynchronously
+  std::shared_future<ResponseType::SharedPtr> response_fut;
+  response_fut = target_recognition_client->async_send_request(req);
+
+  // now wait for result using the "wait_for" method of the future object
   std::future_status st = response_fut.wait_for(rclcpp::Duration::from_seconds(20.0).to_chrono<std::chrono::seconds>());
   if (st == std::future_status ::ready)
   {
     ResponseType::SharedPtr response = response_fut.get();
     if (response->succeeded)
     {
+      // save target pose into the box pose variable
       box_pose = response->target_pose;
       RCLCPP_INFO_STREAM(node->get_logger(), "target recognition succeeded");
     }
     else
     {
-      RCLCPP_ERROR_STREAM(node->get_logger(), "target recognition failed");
-      exit(0);
+      RCLCPP_ERROR_STREAM(node->get_logger(), "Target recognition failed");
+      throw std::runtime_error("Service call failure");
     }
   }
   else
   {
     RCLCPP_ERROR_STREAM(node->get_logger(), "Service call for target recognition failed with response timed out");
-    exit(0);
+    throw std::runtime_error("Service call failure");
   }
 
   // updating box marker for visualization in rviz

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

@@ -9,12 +9,13 @@
 
 /* INITIALIZE
   Goal:
-    - Loads node parameters and initializes ROS2 interfaces (actions, subscribers, publishers, services)
+    - Understand how parameters get loaded into the application with the application specific "cfg.init()" method.
+    - Understand how to initialize the various ROS2 interfaces (actions, subscribers, publishers, services) used by this application.
 */
 
 namespace pick_and_place_application
 {
-bool PickAndPlaceApp::initialize()
+void PickAndPlaceApp::initialize()
 {
   // reading parameters
   if (this->cfg.init(node))
@@ -24,7 +25,7 @@ bool PickAndPlaceApp::initialize()
   else
   {
     RCLCPP_ERROR(node->get_logger(), "Parameters failed to load");
-    return false;
+    throw std::runtime_error("Parameter lookup failure");
   }
 
   // marker publisher
@@ -42,12 +43,6 @@ bool PickAndPlaceApp::initialize()
   // instantiate the moveit cpp object for motion planing
   moveit_cpp = std::make_shared<moveit_cpp::MoveItCpp>(node);
 
-  // initializing planning scene monitor
-  moveit_cpp->getPlanningSceneMonitor()->startSceneMonitor();
-  moveit_cpp->getPlanningSceneMonitor()->startStateMonitor();
-  // moveit_cpp->getPlanningSceneMonitor()->startWorldGeometryMonitor(); // allows updating the scene from sensor data,
-  // etc. moveit_cpp->getPlanningSceneMonitor()->providePlanningSceneService();
-
   // waiting to establish connections
   if (rclcpp::ok() && grasp_action_client->wait_for_action_server(
                           rclcpp::Duration::from_seconds(10.0).to_chrono<std::chrono::seconds>()))
@@ -68,7 +63,5 @@ bool PickAndPlaceApp::initialize()
   {
     throw std::runtime_error("Failed to find server");
   }
-
-  return true;
 }
 }  // namespace pick_and_place_application