diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp index 4dca0bba9..010f6f406 100644 --- a/capabilities/src/execute_task_solution_capability.cpp +++ b/capabilities/src/execute_task_solution_capability.cpp @@ -94,7 +94,7 @@ void ExecuteTaskSolutionCapability::initialize() { std::placeholders::_1, std::placeholders::_2)), ActionServerType::CancelCallback( std::bind(&ExecuteTaskSolutionCapability::preemptCallback, this, std::placeholders::_1)), - [this](const std::shared_ptr> goal_handle) { + [this](const std::shared_ptr>& goal_handle) { last_goal_future_ = std::async(std::launch::async, &ExecuteTaskSolutionCapability::goalCallback, this, goal_handle); }); @@ -176,19 +176,20 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr }; auto make_apply_planning_scene_diff_cb = [this](const std::vector& scene_diffs) { - return - [this, scene_diffs = std::move(scene_diffs)](const plan_execution::ExecutableMotionPlan* /*plan*/) mutable { - for (auto& scene_diff : scene_diffs) { - if (!moveit::core::isEmpty(scene_diff)) { - /* RCLCPP_DEBUG_STREAM(LOGGER, "apply effect of " << description); */ - scene_diff.robot_state.joint_state = sensor_msgs::msg::JointState(); - scene_diff.robot_state.multi_dof_joint_state = sensor_msgs::msg::MultiDOFJointState(); - if (!context_->planning_scene_monitor_->newPlanningSceneMessage(scene_diff)) - return false; - } - } - return true; - }; + return [this, + &scene_diffs = std::as_const(scene_diffs)](const plan_execution::ExecutableMotionPlan* /*plan*/) mutable { + for (auto& const_scene_diff : scene_diffs) { + if (!moveit::core::isEmpty(const_scene_diff)) { + /* RCLCPP_DEBUG_STREAM(LOGGER, "apply effect of " << description); */ + moveit_msgs::msg::PlanningScene scene_diff = const_scene_diff; + scene_diff.robot_state.joint_state = sensor_msgs::msg::JointState(); + scene_diff.robot_state.multi_dof_joint_state = sensor_msgs::msg::MultiDOFJointState(); + if (!context_->planning_scene_monitor_->newPlanningSceneMessage(scene_diff)) + return false; + } + } + return true; + }; }; auto make_description = [size = solution.sub_trajectory.size()](const std::size_t index) { return std::to_string(index + 1) + "/" + std::to_string(size); diff --git a/core/python/bindings/src/solvers.cpp b/core/python/bindings/src/solvers.cpp index 6bc251a71..8c9b086d1 100644 --- a/core/python/bindings/src/solvers.cpp +++ b/core/python/bindings/src/solvers.cpp @@ -78,8 +78,6 @@ void export_solvers(py::module& m) { .property("goal_joint_tolerance", "float: Tolerance for reaching joint goals") .property("goal_position_tolerance", "float: Tolerance for reaching position goals") .property("goal_orientation_tolerance", "float: Tolerance for reaching orientation goals") - .property("display_motion_plans", "bool: Publish generated solutions via a topic") - .property("publish_planning_requests", "bool: Publish motion planning requests via a topic") .def(py::init(), "pipeline"_a = std::string("ompl")); properties::class_( diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp index 0c2435320..d658951b6 100644 --- a/core/src/solvers/pipeline_planner.cpp +++ b/core/src/solvers/pipeline_planner.cpp @@ -85,12 +85,6 @@ PipelinePlanner::PipelinePlanner( properties().declare("goal_position_tolerance", 1e-4, "tolerance for reaching position goals"); properties().declare("goal_orientation_tolerance", 1e-4, "tolerance for reaching orientation goals"); // Declare properties that configure the planning pipeline - properties().declare("display_motion_plans", false, - "publish generated solutions on topic " + - planning_pipeline::PlanningPipeline::DISPLAY_PATH_TOPIC); - properties().declare("publish_planning_requests", false, - "publish motion planning requests on topic " + - planning_pipeline::PlanningPipeline::MOTION_PLAN_REQUEST_TOPIC); properties().declare>( "pipeline_id_planner_id_map", std::unordered_map(), "Set of pipelines and planners used for planning"); @@ -144,12 +138,6 @@ void PipelinePlanner::init(const core::RobotModelConstPtr& robot_model) { throw std::runtime_error( "Cannot initialize PipelinePlanner: Could not create any valid entries for planning pipeline maps!"); } - - // Configure all pipelines according to the configuration in properties - for (auto const& name_pipeline_pair : planning_pipelines_) { - name_pipeline_pair.second->displayComputedMotionPlans(properties().get("display_motion_plans")); - name_pipeline_pair.second->publishReceivedRequests(properties().get("publish_planning_requests")); - } } bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,