Skip to content

Commit 6805c26

Browse files
sjahrsjahr
committed
Make code readable
1 parent a0befc5 commit 6805c26

File tree

2 files changed

+70
-68
lines changed

2 files changed

+70
-68
lines changed

‎core/include/moveit/task_constructor/solvers/pipeline_planner.h‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -86,11 +86,11 @@ class PipelinePlanner : public PlannerInterface
8686
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
8787

8888
bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
89-
const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
89+
const core::JointModelGroup* joint_model_group, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
9090
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
9191

9292
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
93-
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
93+
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* joint_model_group,
9494
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
9595
const moveit_msgs::msg::Constraints& path_constraints = moveit_msgs::msg::Constraints()) override;
9696

‎core/src/solvers/pipeline_planner.cpp‎

Lines changed: 68 additions & 66 deletions
Original file line numberDiff line numberDiff line change
@@ -60,7 +60,7 @@ struct PlannerCache
6060
using ModelList = std::list<std::pair<std::weak_ptr<const moveit::core::RobotModel>, PlannerMap> >;
6161
ModelList cache_;
6262

63-
PlannerMap::mapped_type& retrieve(const moveit::core::RobotModelConstPtr& model, const PlannerID& id) {
63+
PlannerMap::mapped_type& retrieve(const moveit::core::RobotModelConstPtr& model, const PlannerID& planner_id) {
6464
// find model in cache_ and remove expired entries while doing so
6565
ModelList::iterator model_it = cache_.begin();
6666
while (model_it != cache_.end()) {
@@ -75,17 +75,17 @@ struct PlannerCache
7575
if (model_it == cache_.end()) // if not found, create a new PlannerMap for this model
7676
model_it = cache_.insert(cache_.begin(), std::make_pair(model, PlannerMap()));
7777

78-
return model_it->second.insert(std::make_pair(id, PlannerMap::mapped_type())).first->second;
78+
return model_it->second.insert(std::make_pair(planner_id, PlannerMap::mapped_type())).first->second;
7979
}
8080
};
8181

8282
planning_pipeline::PlanningPipelinePtr PipelinePlanner::create(const rclcpp::Node::SharedPtr& node,
83-
const PipelinePlanner::Specification& spec) {
83+
const PipelinePlanner::Specification& specification) {
8484
static PlannerCache cache;
8585

8686
static constexpr char const* PLUGIN_PARAMETER_NAME = "planning_plugin";
8787

88-
std::string pipeline_ns = spec.ns;
88+
std::string pipeline_ns = specification.ns;
8989
const std::string parameter_name = pipeline_ns + "." + PLUGIN_PARAMETER_NAME;
9090
// fallback to old structure for pipeline parameters in MoveIt
9191
if (!node->has_parameter(parameter_name)) {
@@ -97,14 +97,14 @@ planning_pipeline::PlanningPipelinePtr PipelinePlanner::create(const rclcpp::Nod
9797
pipeline_ns = "move_group";
9898
}
9999

100-
PlannerCache::PlannerID id(pipeline_ns, spec.adapter_param);
100+
PlannerCache::PlannerID id(pipeline_ns, specification.adapter_param);
101101

102-
std::weak_ptr<planning_pipeline::PlanningPipeline>& entry = cache.retrieve(spec.model, id);
102+
std::weak_ptr<planning_pipeline::PlanningPipeline>& entry = cache.retrieve(specification.model, id);
103103
planning_pipeline::PlanningPipelinePtr planner = entry.lock();
104104
if (!planner) {
105105
// create new entry
106-
planner = std::make_shared<planning_pipeline::PlanningPipeline>(spec.model, node, pipeline_ns,
107-
PLUGIN_PARAMETER_NAME, spec.adapter_param);
106+
planner = std::make_shared<planning_pipeline::PlanningPipeline>(
107+
specification.model, node, pipeline_ns, PLUGIN_PARAMETER_NAME, specification.adapter_param);
108108
// store in cache
109109
entry = planner;
110110
}
@@ -113,22 +113,23 @@ planning_pipeline::PlanningPipelinePtr PipelinePlanner::create(const rclcpp::Nod
113113

114114
PipelinePlanner::PipelinePlanner(const rclcpp::Node::SharedPtr& node, const std::string& pipeline_name)
115115
: pipeline_name_{ pipeline_name }, node_(node) {
116-
auto& p = properties();
117-
p.declare<std::string>("planner", "", "planner id");
118-
119-
p.declare<uint>("num_planning_attempts", 1u, "number of planning attempts");
120-
p.declare<moveit_msgs::msg::WorkspaceParameters>("workspace_parameters", moveit_msgs::msg::WorkspaceParameters(),
121-
"allowed workspace of mobile base?");
122-
123-
p.declare<double>("goal_joint_tolerance", 1e-4, "tolerance for reaching joint goals");
124-
p.declare<double>("goal_position_tolerance", 1e-4, "tolerance for reaching position goals");
125-
p.declare<double>("goal_orientation_tolerance", 1e-4, "tolerance for reaching orientation goals");
126-
127-
p.declare<bool>("display_motion_plans", false,
128-
"publish generated solutions on topic " + planning_pipeline::PlanningPipeline::DISPLAY_PATH_TOPIC);
129-
p.declare<bool>("publish_planning_requests", false,
130-
"publish motion planning requests on topic " +
131-
planning_pipeline::PlanningPipeline::MOTION_PLAN_REQUEST_TOPIC);
116+
auto& planner_properties = properties();
117+
planner_properties.declare<std::string>("planner", "", "planner id");
118+
119+
planner_properties.declare<uint>("num_planning_attempts", 1u, "number of planning attempts");
120+
planner_properties.declare<moveit_msgs::msg::WorkspaceParameters>(
121+
"workspace_parameters", moveit_msgs::msg::WorkspaceParameters(), "allowed workspace of mobile base?");
122+
123+
planner_properties.declare<double>("goal_joint_tolerance", 1e-4, "tolerance for reaching joint goals");
124+
planner_properties.declare<double>("goal_position_tolerance", 1e-4, "tolerance for reaching position goals");
125+
planner_properties.declare<double>("goal_orientation_tolerance", 1e-4, "tolerance for reaching orientation goals");
126+
127+
planner_properties.declare<bool>("display_motion_plans", false,
128+
"publish generated solutions on topic " +
129+
planning_pipeline::PlanningPipeline::DISPLAY_PATH_TOPIC);
130+
planner_properties.declare<bool>("publish_planning_requests", false,
131+
"publish motion planning requests on topic " +
132+
planning_pipeline::PlanningPipeline::MOTION_PLAN_REQUEST_TOPIC);
132133
}
133134

134135
PipelinePlanner::PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& planning_pipeline)
@@ -138,11 +139,11 @@ PipelinePlanner::PipelinePlanner(const planning_pipeline::PlanningPipelinePtr& p
138139

139140
void PipelinePlanner::init(const core::RobotModelConstPtr& robot_model) {
140141
if (!planner_) {
141-
Specification spec;
142-
spec.model = robot_model;
143-
spec.pipeline = pipeline_name_;
144-
spec.ns = pipeline_name_;
145-
planner_ = create(node_, spec);
142+
Specification specification;
143+
specification.model = robot_model;
144+
specification.pipeline = pipeline_name_;
145+
specification.ns = pipeline_name_;
146+
planner_ = create(node_, specification);
146147
} else if (robot_model != planner_->getRobotModel()) {
147148
throw std::runtime_error(
148149
"The robot model of the planning pipeline isn't the same as the task's robot model -- "
@@ -152,60 +153,61 @@ void PipelinePlanner::init(const core::RobotModelConstPtr& robot_model) {
152153
planner_->publishReceivedRequests(properties().get<bool>("publish_planning_requests"));
153154
}
154155

155-
void initMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& req, const PropertyMap& p,
156-
const moveit::core::JointModelGroup* jmg, double timeout) {
157-
req.group_name = jmg->getName();
158-
req.planner_id = p.get<std::string>("planner");
159-
req.allowed_planning_time = timeout;
160-
req.start_state.is_diff = true; // we don't specify an extra start state
161-
162-
req.num_planning_attempts = p.get<uint>("num_planning_attempts");
163-
req.max_velocity_scaling_factor = p.get<double>("max_velocity_scaling_factor");
164-
req.max_acceleration_scaling_factor = p.get<double>("max_acceleration_scaling_factor");
165-
req.workspace_parameters = p.get<moveit_msgs::msg::WorkspaceParameters>("workspace_parameters");
156+
void initMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& request, const PropertyMap& p,
157+
const moveit::core::JointModelGroup* joint_model_group, double timeout) {
158+
request.group_name = joint_model_group->getName();
159+
request.planner_id = p.get<std::string>("planner");
160+
request.allowed_planning_time = timeout;
161+
request.start_state.is_diff = true; // we don't specify an extra start state
162+
163+
request.num_planning_attempts = p.get<uint>("num_planning_attempts");
164+
request.max_velocity_scaling_factor = p.get<double>("max_velocity_scaling_factor");
165+
request.max_acceleration_scaling_factor = p.get<double>("max_acceleration_scaling_factor");
166+
request.workspace_parameters = p.get<moveit_msgs::msg::WorkspaceParameters>("workspace_parameters");
166167
}
167168

168169
bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
169-
const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg,
170-
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
170+
const planning_scene::PlanningSceneConstPtr& to,
171+
const moveit::core::JointModelGroup* joint_model_group, double timeout,
172+
robot_trajectory::RobotTrajectoryPtr& result,
171173
const moveit_msgs::msg::Constraints& path_constraints) {
172-
const auto& props = properties();
173-
moveit_msgs::msg::MotionPlanRequest req;
174-
initMotionPlanRequest(req, props, jmg, timeout);
175-
176-
req.goal_constraints.resize(1);
177-
req.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(to->getCurrentState(), jmg,
178-
props.get<double>("goal_joint_tolerance"));
179-
req.path_constraints = path_constraints;
180-
181-
::planning_interface::MotionPlanResponse res;
182-
bool success = planner_->generatePlan(from, req, res);
183-
result = res.trajectory;
174+
const auto& planner_properties = properties();
175+
moveit_msgs::msg::MotionPlanRequest request;
176+
initMotionPlanRequest(request, planner_properties, joint_model_group, timeout);
177+
178+
request.goal_constraints.resize(1);
179+
request.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(
180+
to->getCurrentState(), joint_model_group, planner_properties.get<double>("goal_joint_tolerance"));
181+
request.path_constraints = path_constraints;
182+
183+
::planning_interface::MotionPlanResponse response;
184+
bool success = planner_->generatePlan(from, request, response);
185+
result = response.trajectory;
184186
return success;
185187
}
186188

187189
bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
188190
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target_eigen,
189-
const moveit::core::JointModelGroup* jmg, double timeout,
191+
const moveit::core::JointModelGroup* joint_model_group, double timeout,
190192
robot_trajectory::RobotTrajectoryPtr& result,
191193
const moveit_msgs::msg::Constraints& path_constraints) {
192-
const auto& props = properties();
193-
moveit_msgs::msg::MotionPlanRequest req;
194-
initMotionPlanRequest(req, props, jmg, timeout);
194+
const auto& planner_properties = properties();
195+
moveit_msgs::msg::MotionPlanRequest request;
196+
initMotionPlanRequest(request, planner_properties, joint_model_group, timeout);
195197

196198
geometry_msgs::msg::PoseStamped target;
197199
target.header.frame_id = from->getPlanningFrame();
198200
target.pose = tf2::toMsg(target_eigen * offset.inverse());
199201

200-
req.goal_constraints.resize(1);
201-
req.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(
202-
link.getName(), target, props.get<double>("goal_position_tolerance"),
203-
props.get<double>("goal_orientation_tolerance"));
204-
req.path_constraints = path_constraints;
202+
request.goal_constraints.resize(1);
203+
request.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(
204+
link.getName(), target, planner_properties.get<double>("goal_position_tolerance"),
205+
planner_properties.get<double>("goal_orientation_tolerance"));
206+
request.path_constraints = path_constraints;
205207

206-
::planning_interface::MotionPlanResponse res;
207-
bool success = planner_->generatePlan(from, req, res);
208-
result = res.trajectory;
208+
::planning_interface::MotionPlanResponse response;
209+
bool success = planner_->generatePlan(from, request, response);
210+
result = response.trajectory;
209211
return success;
210212
}
211213
} // namespace solvers

0 commit comments

Comments
 (0)