40 #include <rclcpp/rclcpp.hpp>
55 template <
typename GeneratorT>
60 const moveit::core::RobotModelConstPtr& model,
119 template <
typename GeneratorT>
125 if (request_.start_state.joint_state.name.empty())
127 moveit_msgs::msg::RobotState current_state;
129 request_.start_state = current_state;
131 bool result = generator_.generate(getPlanningScene(), request_, res);
137 RCLCPP_ERROR(
rclcpp::get_logger(
"moveit.pilz_industrial_motion_planner.planning_context_base"),
138 "Using solve on a terminated planning context!");
139 res.
error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED;
143 template <
typename GeneratorT>
149 bool result = solve(undetailed_response);
167 template <
typename GeneratorT>
170 RCLCPP_DEBUG_STREAM(
rclcpp::get_logger(
"moveit.pilz_industrial_motion_planner.planning_context_base"),
176 template <
typename GeneratorT>
This class combines CartesianLimit and JointLimits into on single class.
PlanningContext for obtaining trajectories.
pilz_industrial_motion_planner::LimitsContainer limits_
Joint limits to be used during planning.
~PlanningContextBase() override
bool solve(planning_interface::MotionPlanResponse &res) override
Calculates a trajectory for the request this context is currently set for.
std::atomic_bool terminated_
Flag if terminated.
bool solve(planning_interface::MotionPlanDetailedResponse &res) override
Will return the same trajectory as solve(planning_interface::MotionPlanResponse& res) This function j...
void clear() override
Clear the data structures used by the planner.
bool terminate() override
Will terminate solve()
moveit::core::RobotModelConstPtr model_
The robot model.
Representation of a particular planning context – the planning scene and the request are known,...
void robotStateToRobotStateMsg(const RobotState &state, moveit_msgs::msg::RobotState &robot_state, bool copy_attached_bodies=true)
Convert a MoveIt robot state to a robot state message.
rclcpp::Logger get_logger()
moveit_msgs::msg::MoveItErrorCodes error_code
std::vector< std::string > description
std::vector< double > processing_time
std::vector< robot_trajectory::RobotTrajectoryPtr > trajectory
Response to a planning query.
moveit::core::MoveItErrorCode error_code
robot_trajectory::RobotTrajectoryPtr trajectory