Skip to content

Commit 10f6b7a

Browse files
rhaschkerhaschke
committed
MoveTo/MoveRel: reduce code duplication
... using templated versions for computeForward + computeBackward as they essentially perform the same operations.
1 parent dfffd77 commit 10f6b7a

File tree

7 files changed

+57
-60
lines changed

7 files changed

+57
-60
lines changed

‎core/include/moveit/task_constructor/stage.h‎

Lines changed: 23 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -289,15 +289,33 @@ class PropagatingEitherWay : public ComputeBase
289289
};
290290
void restrictDirection(Direction dir);
291291

292-
virtual void computeForward(const InterfaceState& from) = 0;
293-
virtual void computeBackward(const InterfaceState& to) = 0;
294-
295-
void sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& trajectory);
296-
void sendBackward(InterfaceState&& from, const InterfaceState& to, SubTrajectory&& trajectory);
292+
// Default implementations, using generic compute().
293+
// Override if you want to use different code for FORWARD and BACKWARD directions.
294+
virtual void computeForward(const InterfaceState& from) { computeGeneric<Interface::FORWARD>(from); }
295+
virtual void computeBackward(const InterfaceState& to) { computeGeneric<Interface::BACKWARD>(to); }
297296

298297
protected:
299298
// constructor for use in derived classes
300299
PropagatingEitherWay(PropagatingEitherWayPrivate* impl);
300+
301+
template <Interface::Direction dir>
302+
void send(const InterfaceState& start, InterfaceState&& end, SubTrajectory&& trajectory);
303+
304+
inline void sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& trajectory) {
305+
send<Interface::FORWARD>(from, std::move(to), std::move(trajectory));
306+
}
307+
inline void sendBackward(InterfaceState&& from, const InterfaceState& to, SubTrajectory&& trajectory) {
308+
send<Interface::BACKWARD>(to, std::move(from), std::move(trajectory));
309+
}
310+
311+
private:
312+
virtual bool compute(const InterfaceState& state, planning_scene::PlanningScenePtr& scene, SubTrajectory& trajectory,
313+
Interface::Direction dir) {
314+
throw std::runtime_error("PropagatingEitherWay: Override compute() or compute[Forward|Backward]()");
315+
}
316+
317+
template <Interface::Direction dir>
318+
void computeGeneric(const InterfaceState& start);
301319
};
302320

303321
class PropagatingForwardPrivate;

‎core/include/moveit/task_constructor/stage_p.h‎

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -139,6 +139,8 @@ class StagePrivate
139139
// methods to spawn new solutions
140140
void sendForward(const InterfaceState& from, InterfaceState&& to, const SolutionBasePtr& solution);
141141
void sendBackward(InterfaceState&& from, const InterfaceState& to, const SolutionBasePtr& solution);
142+
template <Interface::Direction>
143+
inline void send(const InterfaceState& start, InterfaceState&& end, const SolutionBasePtr& solution);
142144
void spawn(InterfaceState&& state, const SolutionBasePtr& solution);
143145
void connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution);
144146

@@ -195,6 +197,17 @@ class StagePrivate
195197
PIMPL_FUNCTIONS(Stage)
196198
std::ostream& operator<<(std::ostream& os, const StagePrivate& stage);
197199

200+
template <>
201+
inline void StagePrivate::send<Interface::FORWARD>(const InterfaceState& start, InterfaceState&& end,
202+
const SolutionBasePtr& solution) {
203+
sendForward(start, std::move(end), solution);
204+
}
205+
template <>
206+
inline void StagePrivate::send<Interface::BACKWARD>(const InterfaceState& start, InterfaceState&& end,
207+
const SolutionBasePtr& solution) {
208+
sendBackward(std::move(end), start, solution);
209+
}
210+
198211
// ComputeBasePrivate is the base class for all computing stages, i.e. non-containers.
199212
// It adds the trajectories_ variable.
200213
class ComputeBasePrivate : public StagePrivate

‎core/include/moveit/task_constructor/stages/move_relative.h‎

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -61,8 +61,6 @@ class MoveRelative : public PropagatingEitherWay
6161
const solvers::PlannerInterfacePtr& planner = solvers::PlannerInterfacePtr());
6262

6363
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
64-
void computeForward(const InterfaceState& from) override;
65-
void computeBackward(const InterfaceState& to) override;
6664

6765
void setGroup(const std::string& group) { setProperty("group", group); }
6866
/// setters for IK frame
@@ -98,7 +96,7 @@ class MoveRelative : public PropagatingEitherWay
9896
protected:
9997
// return false if trajectory shouldn't be stored
10098
bool compute(const InterfaceState& state, planning_scene::PlanningScenePtr& scene, SubTrajectory& trajectory,
101-
Direction dir);
99+
Interface::Direction dir) override;
102100

103101
protected:
104102
solvers::PlannerInterfacePtr planner_;

‎core/include/moveit/task_constructor/stages/move_to.h‎

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -60,8 +60,6 @@ class MoveTo : public PropagatingEitherWay
6060
const solvers::PlannerInterfacePtr& planner = solvers::PlannerInterfacePtr());
6161

6262
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
63-
void computeForward(const InterfaceState& from) override;
64-
void computeBackward(const InterfaceState& to) override;
6563

6664
void setGroup(const std::string& group) { setProperty("group", group); }
6765
/// setters for IK frame
@@ -97,7 +95,7 @@ class MoveTo : public PropagatingEitherWay
9795
protected:
9896
// return false if trajectory shouldn't be stored
9997
bool compute(const InterfaceState& state, planning_scene::PlanningScenePtr& scene, SubTrajectory& trajectory,
100-
Direction dir);
98+
Interface::Direction dir) override;
10199
bool getJointStateGoal(const boost::any& goal, const core::JointModelGroup* jmg, moveit::core::RobotState& state);
102100
bool getPoseGoal(const boost::any& goal, const geometry_msgs::PoseStamped& ik_pose_msg,
103101
const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen,

‎core/src/stage.cpp‎

Lines changed: 12 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -574,12 +574,20 @@ void PropagatingEitherWay::restrictDirection(PropagatingEitherWay::Direction dir
574574
impl->initInterface(dir);
575575
}
576576

577-
void PropagatingEitherWay::sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& t) {
578-
pimpl()->sendForward(from, std::move(to), std::make_shared<SubTrajectory>(std::move(t)));
577+
template <Interface::Direction dir>
578+
void PropagatingEitherWay::send(const InterfaceState& start, InterfaceState&& end, SubTrajectory&& trajectory) {
579+
pimpl()->send<dir>(start, std::move(end), std::make_shared<SubTrajectory>(std::move(trajectory)));
579580
}
580581

581-
void PropagatingEitherWay::sendBackward(InterfaceState&& from, const InterfaceState& to, SubTrajectory&& t) {
582-
pimpl()->sendBackward(std::move(from), to, std::make_shared<SubTrajectory>(std::move(t)));
582+
template <Interface::Direction dir>
583+
void PropagatingEitherWay::computeGeneric(const InterfaceState& start) {
584+
planning_scene::PlanningScenePtr end;
585+
SubTrajectory trajectory;
586+
587+
if (!compute(start, end, trajectory, dir) && trajectory.comment().empty())
588+
silentFailure(); // there is nothing to report (comment is empty)
589+
else
590+
send<dir>(start, InterfaceState(end), std::move(trajectory));
583591
}
584592

585593
PropagatingForwardPrivate::PropagatingForwardPrivate(PropagatingForward* me, const std::string& name)

‎core/src/stages/move_relative.cpp‎

Lines changed: 5 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -105,7 +105,7 @@ static bool getJointStateFromOffset(const boost::any& direction, const moveit::c
105105
}
106106

107107
bool MoveRelative::compute(const InterfaceState& state, planning_scene::PlanningScenePtr& scene,
108-
SubTrajectory& solution, Direction dir) {
108+
SubTrajectory& solution, Interface::Direction dir) {
109109
scene = state.scene()->diff();
110110
const robot_model::RobotModelConstPtr& robot_model = scene->getRobotModel();
111111
assert(robot_model);
@@ -191,7 +191,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
191191
}
192192

193193
// invert direction?
194-
if (dir == BACKWARD) {
194+
if (dir == Interface::BACKWARD) {
195195
linear *= -1.0;
196196
angular *= -1.0;
197197
}
@@ -220,7 +220,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
220220
linear_norm = linear.norm();
221221

222222
// invert direction?
223-
if (dir == BACKWARD)
223+
if (dir == Interface::BACKWARD)
224224
linear *= -1.0;
225225

226226
// compute absolute transform for link
@@ -275,7 +275,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
275275
rviz_marker_tools::makeArrow(m, linear_norm);
276276
auto quat = Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitX(), linear);
277277
Eigen::Vector3d pos(link_pose.translation());
278-
if (dir == BACKWARD) {
278+
if (dir == Interface::BACKWARD) {
279279
// flip arrow direction
280280
quat = quat * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitY());
281281
// arrow tip at goal pose
@@ -291,7 +291,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
291291
// store result
292292
if (robot_trajectory) {
293293
scene->setCurrentState(robot_trajectory->getLastWayPoint());
294-
if (dir == BACKWARD)
294+
if (dir == Interface::BACKWARD)
295295
robot_trajectory->reverse();
296296
solution.setTrajectory(robot_trajectory);
297297

@@ -302,25 +302,6 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
302302
return false;
303303
}
304304

305-
void MoveRelative::computeForward(const InterfaceState& from) {
306-
planning_scene::PlanningScenePtr to;
307-
SubTrajectory trajectory;
308-
309-
if (!compute(from, to, trajectory, FORWARD) && trajectory.comment().empty())
310-
silentFailure(); // there is nothing to report (comment is empty)
311-
else
312-
sendForward(from, InterfaceState(to), std::move(trajectory));
313-
}
314-
315-
void MoveRelative::computeBackward(const InterfaceState& to) {
316-
planning_scene::PlanningScenePtr from;
317-
SubTrajectory trajectory;
318-
319-
if (!compute(to, from, trajectory, BACKWARD) && trajectory.comment().empty())
320-
silentFailure();
321-
else
322-
sendBackward(InterfaceState(from), to, std::move(trajectory));
323-
}
324305
} // namespace stages
325306
} // namespace task_constructor
326307
} // namespace moveit

‎core/src/stages/move_to.cpp‎

Lines changed: 2 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -187,7 +187,7 @@ bool MoveTo::getPointGoal(const boost::any& goal, const moveit::core::LinkModel*
187187
}
188188

189189
bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningScenePtr& scene, SubTrajectory& solution,
190-
Direction dir) {
190+
Interface::Direction dir) {
191191
scene = state.scene()->diff();
192192
const robot_model::RobotModelConstPtr& robot_model = scene->getRobotModel();
193193
assert(robot_model);
@@ -259,7 +259,7 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
259259
}
260260
if (robot_trajectory) {
261261
scene->setCurrentState(robot_trajectory->getLastWayPoint());
262-
if (dir == BACKWARD)
262+
if (dir == Interface::BACKWARD)
263263
robot_trajectory->reverse();
264264
solution.setTrajectory(robot_trajectory);
265265

@@ -271,25 +271,6 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
271271
return false;
272272
}
273273

274-
void MoveTo::computeForward(const InterfaceState& from) {
275-
planning_scene::PlanningScenePtr to;
276-
SubTrajectory trajectory;
277-
278-
if (!compute(from, to, trajectory, FORWARD) && trajectory.comment().empty())
279-
silentFailure(); // there is nothing to report (comment is empty)
280-
else
281-
sendForward(from, InterfaceState(to), std::move(trajectory));
282-
}
283-
284-
void MoveTo::computeBackward(const InterfaceState& to) {
285-
planning_scene::PlanningScenePtr from;
286-
SubTrajectory trajectory;
287-
288-
if (!compute(to, from, trajectory, BACKWARD) && trajectory.comment().empty())
289-
silentFailure();
290-
else
291-
sendBackward(InterfaceState(from), to, std::move(trajectory));
292-
}
293274
} // namespace stages
294275
} // namespace task_constructor
295276
} // namespace moveit

0 commit comments

Comments
 (0)