Update motion planning API tutorial

Author: sjahr

doc/examples/motion_planning_api/src/motion_planning_api_tutorial.cpp

@@ -99,8 +99,8 @@ int main(int argc, char** argv)
   // We will get the name of planning plugin we want to load
   // from the ROS parameter server, and then load the planner
   // making sure to catch all exceptions.
-  if (!motion_planning_api_tutorial_node->get_parameter("planning_plugin", planner_plugin_names))
-    RCLCPP_FATAL(LOGGER, "Could not find planner plugin name");
+  if (!motion_planning_api_tutorial_node->get_parameter("planning_plugins", planner_plugin_names))
+    RCLCPP_FATAL(LOGGER, "Could not find a planner plugin name");
   try
   {
     planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>(
@@ -166,14 +166,14 @@ int main(int argc, char** argv)
   geometry_msgs::msg::PoseStamped pose;
   pose.header.frame_id = "panda_link0";
   pose.pose.position.x = 0.3;
-  pose.pose.position.y = 0.4;
+  pose.pose.position.y = 0.0;
   pose.pose.position.z = 0.75;
-  pose.pose.orientation.w = 1.0;
+  pose.pose.orientation.z = 1.0;
 
-  // A tolerance of 0.01 m is specified in position
-  // and 0.01 radians in orientation
-  std::vector<double> tolerance_pose(3, 0.01);
-  std::vector<double> tolerance_angle(3, 0.01);
+  // A tolerance of 0.1 m is specified in position
+  // and 0.1 radians in orientation
+  std::vector<double> tolerance_pose(3, 0.1);
+  std::vector<double> tolerance_angle(3, 0.1);
 
   // We will create the request as a constraint using a helper function available
   // from the
@@ -185,16 +185,28 @@ int main(int argc, char** argv)
   req.group_name = PLANNING_GROUP;
   req.goal_constraints.push_back(pose_goal);
 
+  // Define workspace bounds
+  req.workspace_parameters.min_corner.x = req.workspace_parameters.min_corner.y =
+      req.workspace_parameters.min_corner.z = -5.0;
+  req.workspace_parameters.max_corner.x = req.workspace_parameters.max_corner.y =
+      req.workspace_parameters.max_corner.z = 5.0;
+
   // We now construct a planning context that encapsulate the scene,
   // the request and the response. We call the planner using this
   // planning context
   planning_interface::PlanningContextPtr context =
       planner_instance->getPlanningContext(planning_scene, req, res.error_code);
+
+  if (!context)
+  {
+    RCLCPP_ERROR(LOGGER, "Failed to create planning context");
+    return -1;
+  }
   context->solve(res);
   if (res.error_code.val != res.error_code.SUCCESS)
   {
     RCLCPP_ERROR(LOGGER, "Could not compute plan successfully");
-    return 0;
+    return -1;
   }
 
   // Visualize the result
@@ -246,7 +258,7 @@ int main(int argc, char** argv)
   if (res.error_code.val != res.error_code.SUCCESS)
   {
     RCLCPP_ERROR(LOGGER, "Could not compute plan successfully");
-    return 0;
+    return -1;
   }
   /* Visualize the trajectory */
   res.getMessage(response);