adding cost calculations to connect and move_to

This commit is contained in:
Tyler Weaver 2019-07-11 14:44:28 -06:00 committed by v4hn
parent 1783587f41
commit 7ca49bc29b
2 changed files with 24 additions and 3 deletions

View File

@ -190,6 +190,13 @@ SolutionSequencePtr Connect::makeSequential(const std::vector<robot_trajectory::
planning_scene::PlanningSceneConstPtr start_ps = *scene_it;
const InterfaceState* state = &from;
// calculate cost
double cost = 0;
for (const auto& trajectory: sub_trajectories) {
for (const double& distance: trajectory->getWayPointDurations())
cost += distance;
}
SolutionSequence::container_type sub_solutions;
for (const auto &sub : sub_trajectories) {
planning_scene::PlanningSceneConstPtr end_ps = *++scene_it;
@ -212,16 +219,23 @@ SolutionSequencePtr Connect::makeSequential(const std::vector<robot_trajectory::
start_ps = end_ps;
}
return std::make_shared<SolutionSequence>(std::move(sub_solutions));
return std::make_shared<SolutionSequence>(std::move(sub_solutions), cost);
}
SubTrajectoryPtr Connect::merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
const std::vector<planning_scene::PlanningSceneConstPtr>& intermediate_scenes,
const moveit::core::RobotState& state)
{
// calculate cost
double cost = 0;
for (const auto& trajectory: sub_trajectories) {
for (const double& distance: trajectory->getWayPointDurations())
cost += distance;
}
// no need to merge if there is only a single sub trajectory
if (sub_trajectories.size() == 1)
return std::make_shared<SubTrajectory>(sub_trajectories[0]);
return std::make_shared<SubTrajectory>(sub_trajectories[0], cost);
auto jmg = merged_jmg_.get();
assert(jmg);
@ -233,7 +247,7 @@ SubTrajectoryPtr Connect::merge(const std::vector<robot_trajectory::RobotTraject
if (!intermediate_scenes.front()->isPathValid(*trajectory, properties().get<moveit_msgs::Constraints>("path_constraints")))
return SubTrajectoryPtr();
return std::make_shared<SubTrajectory>(trajectory);
return std::make_shared<SubTrajectory>(trajectory, cost);
}
} } }

View File

@ -234,11 +234,18 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP
success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints);
}
// set cost
double cost = 0;
for (const double& distance: robot_trajectory->getWayPointDurations()) {
cost += distance;
}
// store result
if (robot_trajectory) {
scene->setCurrentState(robot_trajectory->getLastWayPoint());
if (dir == BACKWARD) robot_trajectory->reverse();
solution.setTrajectory(robot_trajectory);
solution.setCost(cost);
if (!success)
solution.markAsFailure();
}