move solution callbacks from task to stage

... allowing for solution callbacks to be registered at each stage
Using Introspection::publishSolution(), the individual solutions in some
stages could be replace by a general one.
This commit is contained in:
Robert Haschke 2017-11-09 18:13:16 +01:00
parent cb85e1b864
commit 622603268f
12 changed files with 49 additions and 75 deletions

View File

@ -100,6 +100,13 @@ public:
virtual void processSolutions(const SolutionProcessor &processor) const = 0;
virtual void processFailures(const SolutionProcessor &processor) const {}
typedef std::function<void(const SolutionBase &s)> SolutionCallback;
typedef std::list<SolutionCallback> SolutionCallbackList;
/// add function to be called for every newly found solution
SolutionCallbackList::const_iterator addSolutionCallback(SolutionCallback &&cb);
/// remove function callback
void erase(SolutionCallbackList::const_iterator which);
protected:
/// Stage can only be instantiated through derived classes
Stage(StagePrivate *impl);

View File

@ -4,8 +4,6 @@
#include <moveit_task_constructor/stage.h>
#include <ros/ros.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <geometry_msgs/PointStamped.h>
@ -52,10 +50,6 @@ protected:
geometry_msgs::Vector3Stamped along_;
double step_size_;
ros::Publisher pub;
void _publishTrajectory(const planning_scene::PlanningSceneConstPtr &scene, const robot_trajectory::RobotTrajectory& trajectory, const moveit::core::RobotState& start);
};
} } }

View File

@ -4,10 +4,6 @@
#include <moveit_task_constructor/stage.h>
#include <ros/ros.h>
#include <moveit/macros/class_forward.h>
namespace moveit { namespace task_constructor { namespace stages {
class GenerateGraspPose : public Generator {
@ -70,8 +66,6 @@ protected:
bool tried_current_state_as_seed_ = false;
std::vector< std::vector<double> > previous_solutions_;
ros::Publisher pub;
};
} } }

View File

@ -49,17 +49,11 @@ public:
/// enable introspection publishing for use with rviz
void enableIntrospection(bool enable = true);
typedef std::function<void(const SolutionBase &s)> SolutionCallback;
typedef std::list<SolutionCallback> SolutionCallbackList;
/// add function to be called for every newly found solution
SolutionCallbackList::const_iterator addSolutionCallback(SolutionCallback &&cb);
/// remove function callback
void erase(SolutionCallbackList::const_iterator which);
Introspection &introspection();
typedef std::function<void(const Task &t)> TaskCallback;
typedef std::list<TaskCallback> TaskCallbackList;
/// add function to be called for every newly found solution
/// add function to be called after each top-level iteration
TaskCallbackList::const_iterator addTaskCallback(TaskCallback &&cb);
/// remove function callback
void erase(TaskCallbackList::const_iterator which);
@ -112,7 +106,6 @@ private:
// introspection and monitoring
std::unique_ptr<Introspection> introspection_;
std::list<Task::SolutionCallback> solution_cbs_; // functions called for each new solution
std::list<Task::TaskCallback> task_cbs_; // functions to monitor task's planning progress
};

View File

@ -260,8 +260,8 @@ void SerialContainerPrivate::storeNewSolution(SerialContainer::solution_containe
nextStarts()->add(InterfaceState(*internal_to), &solution, NULL);
}
// inform parent about new solution
parent()->onNewSolution(solutions_.back());
// perform default stage action on new solution
newSolution(solutions_.back());
}
@ -449,8 +449,8 @@ void ParallelContainerBase::onNewSolution(SolutionBase &s)
// store solution in our own list
impl->solutions_.emplace_back(std::move(wrapped));
// inform parent
impl->parent()->onNewSolution(impl->solutions_.back());
// perform default stage action on new solution
impl->newSolution(impl->solutions_.back());
}

View File

@ -61,6 +61,7 @@ int main(int argc, char** argv){
{
auto move= std::make_unique<stages::CartesianPositionMotion>("proceed to grasp pose");
move->addSolutionCallback(std::ref(t.introspection()));
move->setGroup("left_arm");
move->setLink("l_gripper_tool_frame");
move->setMinMaxDistance(.03, 0.1);

View File

@ -59,6 +59,7 @@ int main(int argc, char** argv){
{
auto move = std::make_unique<stages::CartesianPositionMotion>("proceed to grasp pose");
move->addSolutionCallback(std::ref(t.introspection()));
move->setGroup("arm");
move->setLink("s_model_tool0");
move->setMinMaxDistance(.03, 0.1);

View File

@ -63,6 +63,14 @@ void StagePrivate::validate() const {
if (errors) throw errors;
}
void StagePrivate::newSolution(SolutionBase &solution)
{
for (const auto& cb : solution_cbs_)
cb(solution);
if (parent())
parent()->onNewSolution(solution);
}
Stage::Stage(StagePrivate *impl)
: pimpl_(impl)
{
@ -104,6 +112,18 @@ void Stage::setName(const std::string& name)
pimpl_->name_ = name;
}
Stage::SolutionCallbackList::const_iterator Stage::addSolutionCallback(SolutionCallback &&cb)
{
auto impl = pimpl();
impl->solution_cbs_.emplace_back(std::move(cb));
return --impl->solution_cbs_.cend();
}
void Stage::erase(SolutionCallbackList::const_iterator which)
{
pimpl()->solution_cbs_.erase(which);
}
template<InterfaceFlag own, InterfaceFlag other>
const char* direction(const StagePrivate& stage) {
InterfaceFlags f = stage.interfaceFlags();
@ -321,7 +341,7 @@ void PropagatingEitherWay::sendForward(const InterfaceState& from,
SubTrajectory &trajectory = addTrajectory(t, cost);
trajectory.setStartState(from);
impl->nextStarts()->add(std::move(to), &trajectory, NULL);
impl->parent()->onNewSolution(trajectory);
impl->newSolution(trajectory);
}
void PropagatingEitherWay::sendBackward(InterfaceState&& from,
@ -333,7 +353,7 @@ void PropagatingEitherWay::sendBackward(InterfaceState&& from,
SubTrajectory& trajectory = addTrajectory(t, cost);
trajectory.setEndState(to);
impl->prevEnds()->add(std::move(from), NULL, &trajectory);
impl->parent()->onNewSolution(trajectory);
impl->newSolution(trajectory);
}
@ -404,7 +424,7 @@ void Generator::spawn(InterfaceState&& state, double cost)
SubTrajectory& trajectory = addTrajectory(dummy, cost);
impl->prevEnds()->add(InterfaceState(state), NULL, &trajectory);
impl->nextStarts()->add(std::move(state), &trajectory, NULL);
impl->parent()->onNewSolution(trajectory);
impl->newSolution(trajectory);
}
@ -455,7 +475,7 @@ void Connecting::connect(const InterfaceState& from, const InterfaceState& to,
SubTrajectory& trajectory = addTrajectory(t, cost);
trajectory.setStartState(from);
trajectory.setEndState(to);
impl->parent()->onNewSolution(trajectory);
impl->newSolution(trajectory);
}
} }

View File

@ -56,6 +56,8 @@ public:
inline void setPrevEnds(const InterfacePtr& prev_ends) { prev_ends_ = prev_ends; }
inline void setNextStarts(const InterfacePtr& next_starts) { next_starts_ = next_starts; }
void newSolution(SolutionBase &current);
protected:
Stage* const me_; // associated/owning Stage instance
std::string name_;
@ -63,6 +65,9 @@ protected:
InterfacePtr starts_;
InterfacePtr ends_;
// functions called for each new solution
std::list<Stage::SolutionCallback> solution_cbs_;
private:
// !! items write-accessed only by ContainerBasePrivate to maintain hierarchy !!
ContainerBase* parent_; // owning parent

View File

@ -1,9 +1,6 @@
#include <moveit_task_constructor/stages/cartesian_position_motion.h>
#include <moveit_task_constructor/storage.h>
#include <ros/ros.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_state/conversions.h>
#include <moveit/robot_state/robot_state.h>
@ -19,9 +16,6 @@ CartesianPositionMotion::CartesianPositionMotion(std::string name)
: PropagatingEitherWay(name),
step_size_(0.005)
{
ros::NodeHandle nh;
pub= nh.advertise<moveit_msgs::DisplayTrajectory>("move_group/display_planned_path", 50);
ros::Duration(1.0).sleep();
}
void CartesianPositionMotion::setGroup(std::string group){
@ -148,10 +142,6 @@ bool CartesianPositionMotion::computeForward(const InterfaceState &from){
for( auto& tp : trajectory_steps )
traj->addSuffixWayPoint(tp, 0.0);
sendForward(from, InterfaceState(result_scene), traj);
moveit::core::RobotStatePtr result_state= trajectory_steps.back();
robot_state= *result_state;
_publishTrajectory(result_scene, *traj, *result_state);
}
return succeeded;
@ -214,27 +204,9 @@ bool CartesianPositionMotion::computeBackward(const InterfaceState &to){
for( auto& tp : trajectory_steps )
traj->addPrefixWayPoint(tp, 0.0);
sendBackward(InterfaceState(result_scene), to, traj);
moveit::core::RobotStatePtr result_state= trajectory_steps.back();
robot_state= *result_state;
_publishTrajectory(result_scene, *traj, *result_state);
}
return succeeded;
}
void CartesianPositionMotion::_publishTrajectory(const planning_scene::PlanningSceneConstPtr& scene,
const robot_trajectory::RobotTrajectory& trajectory,
const moveit::core::RobotState& start){
moveit_msgs::DisplayTrajectory dt;
robot_state::robotStateToRobotStateMsg(start, dt.trajectory_start);
dt.model_id= scene->getRobotModel()->getName();
dt.trajectory.resize(1);
trajectory.getRobotTrajectoryMsg(dt.trajectory[0]);
dt.model_id= start.getRobotModel()->getName();
pub.publish(dt);
}
} } }

View File

@ -21,9 +21,6 @@ namespace moveit { namespace task_constructor { namespace stages {
GenerateGraspPose::GenerateGraspPose(std::string name)
: Generator(name)
{
ros::NodeHandle nh;
pub= nh.advertise<moveit_msgs::DisplayRobotState>("display_robot_state", 50);
ros::Duration(1.0).sleep();
}
void GenerateGraspPose::init(const planning_scene::PlanningSceneConstPtr &scene)
@ -162,11 +159,7 @@ bool GenerateGraspPose::compute(){
bool succeeded= grasp_state.setFromIK(jmg_active, grasp_pose, ik_link, 1, remaining_time_, is_valid);
remaining_time_-= std::chrono::duration<double>(std::chrono::steady_clock::now()- now).count();
if(succeeded){
moveit_msgs::DisplayRobotState drs;
robot_state::robotStateToRobotStateMsg(grasp_state, drs.state);
pub.publish(drs);
if(succeeded) {
previous_solutions_.emplace_back();
grasp_state.copyJointGroupPositions(jmg_active, previous_solutions_.back());
spawn(InterfaceState(grasp_scene));

View File

@ -112,15 +112,10 @@ void Task::enableIntrospection(bool enable)
introspection_.reset();
}
Task::SolutionCallbackList::const_iterator Task::addSolutionCallback(SolutionCallback &&cb)
Introspection &Task::introspection()
{
solution_cbs_.emplace_back(std::move(cb));
return --solution_cbs_.cend();
}
void Task::erase(SolutionCallbackList::const_iterator which)
{
solution_cbs_.erase(which);
enableIntrospection(true);
return *introspection_;
}
Task::TaskCallbackList::const_iterator Task::addTaskCallback(TaskCallback &&cb)
@ -216,8 +211,7 @@ void Task::publishAllSolutions(bool wait)
void Task::onNewSolution(SolutionBase &s)
{
for (const auto& cb : solution_cbs_)
cb(s);
pimpl()->newSolution(s);
if (introspection_)
introspection_->publishSolution(s);
}