planning_component.h

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2019, PickNik Inc.
 *  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 Inc. 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 <rclcpp/rclcpp.hpp>
#include <moveit/moveit_cpp/moveit_cpp.h>
#include <moveit/robot_state/robot_state.h>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <moveit/robot_state/conversions.h>
#include <moveit/utils/moveit_error_code.h>
 
namespace moveit_cpp
{
MOVEIT_CLASS_FORWARD(PlanningComponent);  // Defines PlanningComponentPtr, ConstPtr, WeakPtr... etc
 
class PlanningComponent
{
public:
  MOVEIT_STRUCT_FORWARD(PlanSolution);
 
  using MoveItErrorCode [[deprecated("Use moveit::core::MoveItErrorCode")]] = moveit::core::MoveItErrorCode;
 
  struct PlanSolution
  {
    moveit_msgs::msg::RobotState start_state;
 
    robot_trajectory::RobotTrajectoryPtr trajectory;
 
    moveit::core::MoveItErrorCode error_code;
 
    explicit operator bool() const
    {
      return bool(error_code);
    }
  };
 
  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 rclcpp::Node::SharedPtr& node)
    {
      std::string ns = "plan_request_params.";
      node->get_parameter_or(ns + "planner_id", planner_id, std::string(""));
      node->get_parameter_or(ns + "planning_pipeline", planning_pipeline, std::string(""));
      node->get_parameter_or(ns + "planning_time", planning_time, 1.0);
      node->get_parameter_or(ns + "planning_attempts", planning_attempts, 5);
      node->get_parameter_or(ns + "max_velocity_scaling_factor", max_velocity_scaling_factor, 1.0);
      node->get_parameter_or(ns + "max_acceleration_scaling_factor", max_acceleration_scaling_factor, 1.0);
    }
  };
 
  PlanningComponent(const std::string& group_name, const rclcpp::Node::SharedPtr& node);
  PlanningComponent(const std::string& group_name, const MoveItCppPtr& moveit_cpp);
 
  PlanningComponent(const PlanningComponent&) = delete;
  PlanningComponent& operator=(const PlanningComponent&) = delete;
 
  PlanningComponent(PlanningComponent&& other) = delete;
  PlanningComponent& operator=(PlanningComponent&& other) = delete;
 
  ~PlanningComponent() = default;
 
  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::msg::Constraints>& goal_constraints);
  bool setGoal(const moveit::core::RobotState& goal_state);
  bool setGoal(const geometry_msgs::msg::PoseStamped& goal_pose, const std::string& link_name);
  bool setGoal(const std::string& named_target);
 
  bool setPathConstraints(const moveit_msgs::msg::Constraints& path_constraints);
 
  PlanSolution plan();
  PlanSolution plan(const PlanRequestParameters& parameters);
 
  bool execute(bool blocking = true);
 
  const PlanSolutionPtr getLastPlanSolution();
 
private:
  // Core properties and instances
  rclcpp::Node::SharedPtr node_;
  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
  std::set<std::string> planning_pipeline_names_;
  // The start state used in the planning motion request
  moveit::core::RobotStatePtr considered_start_state_;
  std::vector<moveit_msgs::msg::Constraints> current_goal_constraints_;
  moveit_msgs::msg::Constraints current_path_constraints_;
  PlanRequestParameters plan_request_parameters_;
  moveit_msgs::msg::WorkspaceParameters workspace_parameters_;
  bool workspace_parameters_set_ = false;
  PlanSolutionPtr 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): implement path/trajectory constraints
  // std::unique_ptr<moveit_msgs::msg::Constraints> path_constraints_;
  // std::unique_ptr<moveit_msgs::msg::TrajectoryConstraints> trajectory_constraints_;
};
}  // 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