First attempt to test moveit_cpp

moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h

@@ -68,13 +68,15 @@ class MoveitCpp
                planning_scene_monitor::PlanningSceneMonitor::MONITORED_PLANNING_SCENE_TOPIC);
       nh.param(ns + "publish_planning_scene_topic", publish_planning_scene_topic,
                planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_TOPIC);
-    };
+      nh.param<double>(ns + "wait_for_initial_state_timeout", wait_for_initial_state_timeout, 0.0);
+    }
     std::string name;
     std::string robot_description;
     std::string joint_state_topic;
     std::string attached_collision_object_topic;
     std::string monitored_planning_scene_topic;
     std::string publish_planning_scene_topic;
+    double wait_for_initial_state_timeout;
   };
   struct PlanningPipelineOptions
   {
@@ -83,7 +85,7 @@ class MoveitCpp
       std::string ns = "planning_pipelines/";
       nh.getParam(ns + "pipeline_names", pipeline_names);
       nh.getParam(ns + "namespace", parent_namespace);
-    };
+    }
     std::vector<std::string> pipeline_names;
     std::string parent_namespace;
   };
@@ -95,7 +97,7 @@ class MoveitCpp
       nh.getParam(ns + "planning_attempts", planning_attempts);
       nh.getParam(ns + "max_velocity_scaling_factor", max_velocity_scaling_factor);
       nh.getParam(ns + "max_acceleration_scaling_factor", max_acceleration_scaling_factor);
-    };
+    }
     int planning_attempts;
     double planning_time;
     double max_velocity_scaling_factor;

moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h

@@ -42,6 +42,7 @@
 #include <moveit/robot_state/robot_state.h>
 #include <geometry_msgs/PoseStamped.h>
 #include <moveit/robot_state/conversions.h>
+#include <moveit_msgs/MoveItErrorCodes.h>
 
 namespace moveit
 {
@@ -54,6 +55,35 @@ class PlanningComponent
 public:
   MOVEIT_STRUCT_FORWARD(PlanSolution);
 
+  class MoveItErrorCode : public moveit_msgs::MoveItErrorCodes
+  {
+  public:
+    MoveItErrorCode()
+    {
+      val = 0;
+    }
+    MoveItErrorCode(int code)
+    {
+      val = code;
+    }
+    MoveItErrorCode(const moveit_msgs::MoveItErrorCodes& code)
+    {
+      val = code.val;
+    }
+    explicit operator bool() const
+    {
+      return val == moveit_msgs::MoveItErrorCodes::SUCCESS;
+    }
+    bool operator==(const int c) const
+    {
+      return val == c;
+    }
+    bool operator!=(const int c) const
+    {
+      return val != c;
+    }
+  };
+
   /// The representation of a plan solution
   struct PlanSolution
   {
@@ -62,6 +92,14 @@ class PlanningComponent
 
     /// The trajectory of the robot (may not contain joints that are the same as for the start_state_)
     robot_trajectory::RobotTrajectoryPtr trajectory;
+
+    /// Error code
+    MoveItErrorCode error_code;
+
+    explicit operator bool() const
+    {
+      return bool(error_code);
+    }
   };
 
   /// Planner parameters provided with the MotionPlanRequest
@@ -133,21 +171,25 @@ class PlanningComponent
 
   /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using
    * default parameters. */
-  bool plan();
+  PlanSolution plan();
   /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using the
    * provided PlanRequestParameters. */
-  bool plan(const PlanRequestParameters& parameters);
+  PlanSolution plan(const PlanRequestParameters& parameters);
 
   /** \brief Execute the latest computed solution trajectory computed by plan(). By default this function terminates
    * after the execution is complete. The execution can be run in background by setting blocking to false. */
   bool execute(bool blocking = true);
 
+  /** \brief Return the last plan solution*/
+  const PlanSolutionPtr getLastPlanSolution();
+
 private:
   // Core properties and instances
   ros::NodeHandle nh_;
   MoveitCppPtr moveit_cpp_;
   const std::string group_name_;
-  moveit::core::JointModelGroupConstPtr joint_model_group_;
+  // The robot_model_ member variable of MoveItCpp class will manually free the joint_model_group_ resources
+  const moveit::core::JointModelGroup* joint_model_group_;
 
   // Planning
   std::set<std::string> planning_pipeline_names_;

moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp

@@ -147,12 +147,10 @@ bool MoveitCpp::loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& opt)
   ros::Duration(0.5).sleep();  // when at 0.1, i believe sometimes vjoint not properly loaded
 
   // Wait for complete state to be recieved
-  // TODO(henningkayser): parameterize
-  double wait_for_complete_state_timeout = 10.0;
-  // Break early
-  if (wait_for_complete_state_timeout > 0.0)
+  if (opt.wait_for_initial_state_timeout > 0.0)
   {
-    return planning_scene_monitor_->getStateMonitor()->waitForCurrentState();
+    return planning_scene_monitor_->getStateMonitor()->waitForCurrentState(ros::Time::now(),
+                                                                           opt.wait_for_initial_state_timeout);
   }
 
   return true;

moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp

@@ -73,7 +73,7 @@ constexpr char LOGNAME[] = "planning_component";
 PlanningComponent::PlanningComponent(const std::string& group_name, const MoveitCppPtr& moveit_context)
   : group_name_(group_name), nh_(moveit_context->getNodeHandle()), moveit_cpp_(moveit_context)
 {
-  joint_model_group_.reset(moveit_cpp_->getRobotModel()->getJointModelGroup(group_name));
+  joint_model_group_ = moveit_cpp_->getRobotModel()->getJointModelGroup(group_name);
   if (!joint_model_group_)
   {
     std::string error = "Could not find joint model group '" + group_name + "'.";
@@ -126,12 +126,14 @@ const std::string& PlanningComponent::getName() const
   return group_name_;
 }
 
-bool PlanningComponent::plan(const PlanRequestParameters& parameters)
+PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParameters& parameters)
 {
+  last_plan_solution_.reset(new PlanSolution());
   if (!joint_model_group_)
   {
     ROS_ERROR_NAMED(LOGNAME, "Failed to retrieve joint model group for name '%s'.", group_name_.c_str());
-    return false;
+    last_plan_solution_->error_code = MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME);
+    return *last_plan_solution_;
   }
 
   // Clone current planning scene
@@ -165,7 +167,8 @@ bool PlanningComponent::plan(const PlanRequestParameters& parameters)
   if (current_goal_constraints_.empty())
   {
     ROS_ERROR_NAMED(LOGNAME, "No goal constraints set for planning request");
-    return false;
+    last_plan_solution_->error_code = MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
+    return *last_plan_solution_;
   }
   req.goal_constraints = current_goal_constraints_;
 
@@ -174,17 +177,18 @@ bool PlanningComponent::plan(const PlanRequestParameters& parameters)
   if (planning_pipeline_names_.find(parameters.planning_pipeline) == planning_pipeline_names_.end())
   {
     ROS_ERROR_NAMED(LOGNAME, "No planning pipeline available for name '%s'", parameters.planning_pipeline.c_str());
-    return false;
+    last_plan_solution_->error_code = MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
+    return *last_plan_solution_;
   }
   const planning_pipeline::PlanningPipelinePtr pipeline =
       moveit_cpp_->getPlanningPipelines().at(parameters.planning_pipeline);
   pipeline->generatePlan(planning_scene, req, res);
+  last_plan_solution_->error_code = res.error_code_.val;
   if (res.error_code_.val != res.error_code_.SUCCESS)
   {
     ROS_ERROR("Could not compute plan successfully");
-    return false;
+    return *last_plan_solution_;
   }
-  last_plan_solution_.reset(new PlanSolution());
   last_plan_solution_->start_state = req.start_state;
   last_plan_solution_->trajectory = res.trajectory_;
   // TODO(henningkayser): Visualize trajectory
@@ -199,10 +203,10 @@ bool PlanningComponent::plan(const PlanRequestParameters& parameters)
   //    visual_tools_->publishRobotState(last_solution_trajectory_->getLastWayPoint(), rviz_visual_tools::TRANSLUCENT);
   //  }
   //}
-  return true;
+  return *last_plan_solution_;
 }
 
-bool PlanningComponent::plan()
+PlanningComponent::PlanSolution PlanningComponent::plan()
 {
   PlanRequestParameters default_parameters;
   default_parameters.planning_attempts = 1;
@@ -270,7 +274,7 @@ bool PlanningComponent::setGoal(const std::vector<moveit_msgs::Constraints>& goa
 
 bool PlanningComponent::setGoal(const robot_state::RobotState& goal_state)
 {
-  current_goal_constraints_ = { kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group_.get()) };
+  current_goal_constraints_ = { kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group_) };
   return true;
 }
 
@@ -296,7 +300,7 @@ bool PlanningComponent::setGoal(const std::string& goal_state_name)
     return false;
   }
   robot_state::RobotState goal_state(moveit_cpp_->getRobotModel());
-  goal_state.setToDefaultValues(joint_model_group_.get(), goal_state_name);
+  goal_state.setToDefaultValues(joint_model_group_, goal_state_name);
   return setGoal(goal_state);
 }
 
@@ -316,15 +320,19 @@ bool PlanningComponent::execute(bool blocking)
   //  ROS_ERROR("Failed to parameterize trajectory");
   //  return false;
   //}
-  moveit_cpp_->execute(group_name_, last_plan_solution_->trajectory, blocking);
+  return moveit_cpp_->execute(group_name_, last_plan_solution_->trajectory, blocking);
+}
+
+const PlanningComponent::PlanSolutionPtr PlanningComponent::getLastPlanSolution()
+{
+  return last_plan_solution_;
 }
 
 void PlanningComponent::clearContents()
 {
   considered_start_state_.reset();
   last_plan_solution_.reset();
   current_goal_constraints_.clear();
-  joint_model_group_.reset();
   moveit_cpp_.reset();
   planning_pipeline_names_.clear();
 }

moveit_ros/planning_interface/test/CMakeLists.txt

@@ -5,6 +5,8 @@ if (CATKIN_ENABLE_TESTING)
   add_executable(test_cleanup test_cleanup.cpp)
   target_link_libraries(test_cleanup moveit_move_group_interface)
 
+  add_rostest_gtest(moveit_cpp_test moveit_cpp_test.test moveit_cpp_test.cpp)
+  target_link_libraries(moveit_cpp_test moveit_cpp ${catkin_LIBRARIES})
   add_rostest(python_move_group.test)
   add_rostest(python_move_group_ns.test)
   add_rostest(robot_state_update.test)

moveit_ros/planning_interface/test/moveit_cpp.yaml

@@ -0,0 +1,17 @@
+planning_scene_monitor_options:
+  name: "planning_scene_monitor"
+  robot_description: "robot_description"
+  joint_state_topic: "/joint_states"
+  attached_collision_object_topic: "/planning_scene_monitor"
+  publish_planning_scene_topic: "/publish_planning_scene"
+  monitored_planning_scene_topic: "/monitored_planning_scene"
+  wait_for_initial_state_timeout: 10.0
+
+planning_pipelines:
+  pipeline_names:
+    - ompl
+
+default_planner_options:
+  planning_attempts: 1
+  max_velocity_scaling_factor: 1.0
+  max_acceleration_scaling_factor: 1.0