Merge remote-tracking branch 'upstream/ros2' into ros2

sjahr · Oct 13, 2023 · cf5cf4a · cf5cf4a
2 parents a16a334 + b25d2ba
commit cf5cf4a
Show file tree

Hide file tree

Showing 3 changed files with 15 additions and 28 deletions.
diff --git 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<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>> goal_handle) {
+	    [this](const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteTaskSolutionAction>>& 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<moveit_msgs::msg::PlanningScene>& 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
@@ -78,8 +78,6 @@ void export_solvers(py::module& m) {
 	    .property<double>("goal_joint_tolerance", "float: Tolerance for reaching joint goals")
 	    .property<double>("goal_position_tolerance", "float: Tolerance for reaching position goals")
 	    .property<double>("goal_orientation_tolerance", "float: Tolerance for reaching orientation goals")
-	    .property<bool>("display_motion_plans", "bool: Publish generated solutions via a topic")
-	    .property<bool>("publish_planning_requests", "bool: Publish motion planning requests via a topic")
 	    .def(py::init<const std::string&>(), "pipeline"_a = std::string("ompl"));
 
 	properties::class_<JointInterpolationPlanner, PlannerInterface>(

diff --git a/core/src/solvers/pipeline_planner.cpp b/core/src/solvers/pipeline_planner.cpp
@@ -85,12 +85,6 @@ PipelinePlanner::PipelinePlanner(
 	properties().declare<double>("goal_position_tolerance", 1e-4, "tolerance for reaching position goals");
 	properties().declare<double>("goal_orientation_tolerance", 1e-4, "tolerance for reaching orientation goals");
 	// Declare properties that configure the planning pipeline
-	properties().declare<bool>("display_motion_plans", false,
-	                           "publish generated solutions on topic " +
-	                               planning_pipeline::PlanningPipeline::DISPLAY_PATH_TOPIC);
-	properties().declare<bool>("publish_planning_requests", false,
-	                           "publish motion planning requests on topic " +
-	                               planning_pipeline::PlanningPipeline::MOTION_PLAN_REQUEST_TOPIC);
 	properties().declare<std::unordered_map<std::string, std::string>>(
 	    "pipeline_id_planner_id_map", std::unordered_map<std::string, std::string>(),
 	    "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<bool>("display_motion_plans"));
-		name_pipeline_pair.second->publishReceivedRequests(properties().get<bool>("publish_planning_requests"));
-	}
 }
 
 bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,