Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Port parallel planning reviews from MoveIt1 #1699

Merged
merged 2 commits into from
Nov 8, 2022
Merged

Port parallel planning reviews from MoveIt1 #1699

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
a7af4f8
Port reviews from MoveIt1 to 2
sjahr Nov 7, 2022
83039e4
Update moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/movei…
sjahr Nov 7, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -172,7 +172,7 @@ class MoveItCpp
const robot_trajectory::RobotTrajectoryPtr& robot_trajectory,
bool blocking = true);

/** \brief Utility to terminate all active planning pipelines */
/** \brief Utility to terminate the given planning pipeline */
bool terminatePlanningPipeline(const std::string& pipeline_name);

private:
Expand Down
3 changes: 2 additions & 1 deletion moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/plan_solutions.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@

#include <algorithm>
#include <moveit/planning_interface/planning_response.h>
#include <mutex>
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why is that required now (and not before)?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It build before, probably because we're lucky and the required header was included from somewhere else but I think it is nicer to ensure that everything is included.

#include <thread>

namespace moveit_cpp
Expand All @@ -56,7 +57,7 @@ class PlanSolutions

/** \brief Thread safe method to add PlanSolutions to this data structure TODO(sjahr): Refactor this method to an
* insert method similar to https://github.com/ompl/ompl/blob/main/src/ompl/base/src/ProblemDefinition.cpp#L54-L161.
* This way, it is possible to created a sorted container e.g. according to a user specified criteria
* This way, it is possible to create a sorted container e.g. according to a user specified criteria
*/
void pushBack(planning_interface::MotionPlanResponse plan_solution)
{
Expand Down
7 changes: 4 additions & 3 deletions moveit_ros/planning/planning_pipeline/include/moveit/planning_pipeline/planning_pipeline.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -133,7 +133,8 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline
\param req The request for motion planning
\param res The motion planning response */
bool generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene,
const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res);
const planning_interface::MotionPlanRequest& req,
planning_interface::MotionPlanResponse& res) const;

/** \brief Call the motion planner plugin and the sequence of planning request adapters (if any).
\param planning_scene The planning scene where motion planning is to be done
Expand All @@ -145,7 +146,7 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline
invalid in all situations. */
bool generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene,
const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
std::vector<std::size_t>& adapter_added_state_index);
std::vector<std::size_t>& adapter_added_state_index) const;

/** \brief Request termination, if a generatePlan() function is currently computing plans */
void terminate() const;
Expand Down Expand Up @@ -184,7 +185,7 @@ class MOVEIT_PLANNING_PIPELINE_EXPORT PlanningPipeline
void configure();

// Flag that indicates whether or not the planning pipeline is currently solving a planning problem
std::atomic<bool> active_;
mutable std::atomic<bool> active_;

std::shared_ptr<rclcpp::Node> node_;
std::string parameter_namespace_;
Expand Down
4 changes: 2 additions & 2 deletions moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -232,7 +232,7 @@ void planning_pipeline::PlanningPipeline::checkSolutionPaths(bool flag)

bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene,
const planning_interface::MotionPlanRequest& req,
planning_interface::MotionPlanResponse& res)
planning_interface::MotionPlanResponse& res) const
{
std::vector<std::size_t> dummy;
return generatePlan(planning_scene, req, res, dummy);
Expand All @@ -241,7 +241,7 @@ bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::Pla
bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene,
const planning_interface::MotionPlanRequest& req,
planning_interface::MotionPlanResponse& res,
std::vector<std::size_t>& adapter_added_state_index)
std::vector<std::size_t>& adapter_added_state_index) const
{
// Set planning pipeline active
active_ = true;
Expand Down