Skip to content

Commit

Permalink
Make clang tidy happy
Browse files Browse the repository at this point in the history
  • Loading branch information
sjahr committed Oct 13, 2023
1 parent f6b1882 commit 1fddae7
Showing 1 changed file with 15 additions and 14 deletions.
29 changes: 15 additions & 14 deletions capabilities/src/execute_task_solution_capability.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
});
Expand Down Expand Up @@ -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(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);
Expand Down

0 comments on commit 1fddae7

Please sign in to comment.