planning_component.h

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2019, PickNik LLC
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of PickNik LLC nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
 
/* Author: Henning Kayser
   Desc: API for planning and execution capabilities of a JointModelGroup */
 
#pragma once
 
#include <ros/ros.h>
#include <moveit/moveit_cpp/moveit_cpp.h>
#include <moveit_msgs/MoveItErrorCodes.h>
#include <moveit/utils/moveit_error_code.h>
#include <moveit/planning_interface/planning_response.h>
#include <mutex>
 
namespace moveit_cpp
{
MOVEIT_CLASS_FORWARD(PlanningComponent);  // Defines PlanningComponentPtr, ConstPtr, WeakPtr... etc
 
planning_interface::MotionPlanResponse
getShortestSolution(std::vector<planning_interface::MotionPlanResponse> const& solutions);
 
class PlanningComponent
{
public:
  using MoveItErrorCode [[deprecated("Use moveit::core::MoveItErrorCode")]] = moveit::core::MoveItErrorCode;
 
  class PlanSolutions
  {
  public:
    PlanSolutions(const size_t expected_size = 0)
    {
      solutions_.reserve(expected_size);
    }
 
    void pushBack(const planning_interface::MotionPlanResponse& plan_solution)
    {
      std::lock_guard<std::mutex> lock_guard(solutions_mutex_);
      solutions_.push_back(plan_solution);
    }
 
    std::vector<planning_interface::MotionPlanResponse> const& getSolutions() const
    {
      return solutions_;
    }
 
  private:
    std::vector<planning_interface::MotionPlanResponse> solutions_;
    std::mutex solutions_mutex_;
  };
 
  struct PlanRequestParameters
  {
    std::string planner_id;
    std::string planning_pipeline;
    int planning_attempts;
    double planning_time;
    double max_velocity_scaling_factor;
    double max_acceleration_scaling_factor;
 
    void load(const ros::NodeHandle& nh, const std::string& param_namespace = "")
    {
      std::string ns = "plan_request_params/";
      if (!param_namespace.empty())
      {
        ns = param_namespace + "/plan_request_params/";
      }
 
      nh.param(ns + "planner_id", planner_id, std::string(""));
      nh.param(ns + "planning_pipeline", planning_pipeline, std::string(""));
      nh.param(ns + "planning_time", planning_time, 1.0);
      nh.param(ns + "planning_attempts", planning_attempts, 5);
      nh.param(ns + "max_velocity_scaling_factor", max_velocity_scaling_factor, 1.0);
      nh.param(ns + "max_acceleration_scaling_factor", max_acceleration_scaling_factor, 1.0);
    }
  };
 
  struct MultiPipelinePlanRequestParameters
  {
    MultiPipelinePlanRequestParameters(const ros::NodeHandle& nh,
                                       const std::vector<std::string>& planning_pipeline_names)
    {
      multi_plan_request_parameters.reserve(planning_pipeline_names.size());
 
      for (auto const& planning_pipeline_name : planning_pipeline_names)
      {
        PlanRequestParameters parameters;
        parameters.load(nh, planning_pipeline_name);
        multi_plan_request_parameters.push_back(parameters);
      }
    }
    // Plan request parameters for the individual planning pipelines which run concurrently
    std::vector<PlanRequestParameters> multi_plan_request_parameters;
  };
 
  typedef std::function<planning_interface::MotionPlanResponse(
      std::vector<planning_interface::MotionPlanResponse> const& solutions)>
      SolutionCallbackFunction;
  typedef std::function<bool(PlanSolutions const& solutions,
                             MultiPipelinePlanRequestParameters const& plan_request_parameters)>
      StoppingCriterionFunction;
 
  PlanningComponent(const std::string& group_name, const ros::NodeHandle& nh);
  PlanningComponent(const std::string& group_name, const MoveItCppPtr& moveit_cpp);
 
  PlanningComponent(const PlanningComponent&) = delete;
  PlanningComponent& operator=(const PlanningComponent&) = delete;
 
  PlanningComponent(PlanningComponent&& other) = default;
  PlanningComponent& operator=(PlanningComponent&& other) = delete;
 
  ~PlanningComponent();
 
  const std::string& getPlanningGroupName() const;
 
  const std::vector<std::string> getNamedTargetStates();
 
  std::map<std::string, double> getNamedTargetStateValues(const std::string& name);
 
  void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz);
 
  void unsetWorkspace();
 
  moveit::core::RobotStatePtr getStartState();
 
  bool setStartState(const moveit::core::RobotState& start_state);
  bool setStartState(const std::string& named_state);
 
  void setStartStateToCurrentState();
 
  bool setGoal(const std::vector<moveit_msgs::Constraints>& goal_constraints);
  bool setGoal(const moveit::core::RobotState& goal_state);
  bool setGoal(const geometry_msgs::PoseStamped& goal_pose, const std::string& link_name);
  bool setGoal(const std::string& named_target);
 
  bool setPathConstraints(const moveit_msgs::Constraints& path_constraints);
 
  bool setTrajectoryConstraints(const moveit_msgs::TrajectoryConstraints& trajectory_constraints);
 
  planning_interface::MotionPlanResponse plan();
  planning_interface::MotionPlanResponse plan(const PlanRequestParameters& parameters, const bool store_solution = true);
 
  planning_interface::MotionPlanResponse
  plan(const MultiPipelinePlanRequestParameters& parameters,
       const SolutionCallbackFunction& solution_selection_callback = &getShortestSolution,
       const StoppingCriterionFunction& stopping_criterion_callback = StoppingCriterionFunction());
 
  bool execute(bool blocking = true);
 
  planning_interface::MotionPlanResponse const& getLastMotionPlanResponse();
 
private:
  // Core properties and instances
  ros::NodeHandle nh_;
  MoveItCppPtr moveit_cpp_;
  const std::string group_name_;
  // The robot_model_ member variable of MoveItCpp class will manually free the joint_model_group_ resources
  const moveit::core::JointModelGroup* joint_model_group_;
 
  // Planning
  // The start state used in the planning motion request
  moveit::core::RobotStatePtr considered_start_state_;
  std::vector<moveit_msgs::Constraints> current_goal_constraints_;
  moveit_msgs::TrajectoryConstraints current_trajectory_constraints_;
  moveit_msgs::Constraints current_path_constraints_;
  PlanRequestParameters plan_request_parameters_;
  moveit_msgs::WorkspaceParameters workspace_parameters_;
  bool workspace_parameters_set_ = false;
  planning_interface::MotionPlanResponse last_plan_solution_;
 
  // common properties for goals
  // TODO(henningkayser): support goal tolerances
  // double goal_joint_tolerance_;
  // double goal_position_tolerance_;
  // double goal_orientation_tolerance_;
  // TODO(henningkayser): implment path/trajectory constraints
  // std::unique_ptr<moveit_msgs::Constraints> path_constraints_;
  // std::unique_ptr<moveit_msgs::TrajectoryConstraints> trajectory_constraints_;
 
  void clearContents();
};
}  // namespace moveit_cpp
 
namespace moveit
{
namespace planning_interface
{
using PlanningComponent [[deprecated("use moveit_cpp")]] = moveit_cpp::PlanningComponent;
[[deprecated("use moveit_cpp")]] MOVEIT_DECLARE_PTR(PlanningComponent, moveit_cpp::PlanningComponent);
}  // namespace planning_interface
}  // namespace moveit