42 #include <rclcpp/rclcpp.hpp>
48 #include <tf2_ros/buffer.h>
60 void load(
const rclcpp::Node::SharedPtr& node)
62 const std::string ns =
"planning_scene_monitor_options";
63 node->get_parameter_or(ns +
".name",
name, std::string(
"planning_scene_monitor"));
64 node->get_parameter_or(ns +
".robot_description",
robot_description, std::string(
"robot_description"));
87 void load(
const rclcpp::Node::SharedPtr& node)
89 const std::string ns =
"planning_pipelines.";
111 MoveItCpp(
const rclcpp::Node::SharedPtr& node);
132 const rclcpp::Node::SharedPtr&
getNode()
const;
136 bool getCurrentState(moveit::core::RobotStatePtr& current_state,
double wait_seconds);
140 moveit::core::RobotStatePtr
getCurrentState(
double wait_seconds = 0.0);
143 const std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr>&
getPlanningPipelines()
const;
149 std::shared_ptr<const tf2_ros::Buffer>
getTFBuffer()
const;
165 const std::vector<std::string>& controllers = std::vector<std::string>());
169 const std::vector<std::string>& controllers = std::vector<std::string>());
176 rclcpp::Node::SharedPtr node_;
177 planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
180 std::unordered_map<std::string, planning_pipeline::PlanningPipelinePtr> planning_pipelines_;
181 std::unordered_map<std::string, std::set<std::string>> groups_algorithms_map_;
184 trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_;
186 rclcpp::Logger logger_;
MoveItCpp(MoveItCpp &&other)=default
std::shared_ptr< const tf2_ros::Buffer > getTFBuffer() const
MoveItCpp & operator=(MoveItCpp &&other)=default
moveit_controller_manager::ExecutionStatus execute(const robot_trajectory::RobotTrajectoryPtr &robot_trajectory, bool blocking, const std::vector< std::string > &controllers=std::vector< std::string >())
Execute a trajectory on the planning group specified by the robot's trajectory using the trajectory e...
moveit::core::RobotModelConstPtr getRobotModel() const
Get the RobotModel object.
MoveItCpp & operator=(const MoveItCpp &)=delete
const std::unordered_map< std::string, planning_pipeline::PlanningPipelinePtr > & getPlanningPipelines() const
Get all loaded planning pipeline instances mapped to their reference names.
planning_scene_monitor::PlanningSceneMonitorConstPtr getPlanningSceneMonitor() const
Get the stored instance of the planning scene monitor.
bool getCurrentState(moveit::core::RobotStatePtr ¤t_state, double wait_seconds)
Get the current state queried from the current state monitor.
planning_scene_monitor::PlanningSceneMonitorPtr getPlanningSceneMonitorNonConst()
const rclcpp::Node::SharedPtr & getNode() const
Get the ROS node this instance operates on.
MoveItCpp(const rclcpp::Node::SharedPtr &node)
Constructor.
bool terminatePlanningPipeline(const std::string &pipeline_name)
Utility to terminate the given planning pipeline.
trajectory_execution_manager::TrajectoryExecutionManagerConstPtr getTrajectoryExecutionManager() const
Get the stored instance of the trajectory execution manager.
MoveItCpp(const MoveItCpp &)=delete
This class owns unique resources (e.g. action clients, threads) and its not very meaningful to copy....
trajectory_execution_manager::TrajectoryExecutionManagerPtr getTrajectoryExecutionManagerNonConst()
static const std::string DEFAULT_PLANNING_SCENE_TOPIC
The name of the topic used by default for receiving full planning scenes or planning scene diffs.
static const std::string DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC
The name of the topic used by default for attached collision objects.
static const std::string MONITORED_PLANNING_SCENE_TOPIC
static const std::string DEFAULT_JOINT_STATES_TOPIC
The name of the topic used by default for receiving joint states.
MOVEIT_CLASS_FORWARD(MoveItCpp)
The reported execution status.
Parameter container for initializing MoveItCpp.
PlanningPipelineOptions planning_pipeline_options
Options(const rclcpp::Node::SharedPtr &node)
PlanningSceneMonitorOptions planning_scene_monitor_options
struct contains the the variables used for loading the planning pipeline
void load(const rclcpp::Node::SharedPtr &node)
std::vector< std::string > pipeline_names
std::string parent_namespace
Specification of options to use when constructing the MoveItCpp class.
void load(const rclcpp::Node::SharedPtr &node)
std::string robot_description
std::string publish_planning_scene_topic
std::string joint_state_topic
std::string monitored_planning_scene_topic
std::string attached_collision_object_topic
double wait_for_initial_state_timeout