diff --git a/include/moveit_task_constructor/stage.h b/include/moveit_task_constructor/stage.h index 2c0f40ee..de5f6ea4 100644 --- a/include/moveit_task_constructor/stage.h +++ b/include/moveit_task_constructor/stage.h @@ -100,6 +100,13 @@ public: virtual void processSolutions(const SolutionProcessor &processor) const = 0; virtual void processFailures(const SolutionProcessor &processor) const {} + typedef std::function SolutionCallback; + typedef std::list 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); diff --git a/include/moveit_task_constructor/stages/cartesian_position_motion.h b/include/moveit_task_constructor/stages/cartesian_position_motion.h index 44959781..5b71175c 100644 --- a/include/moveit_task_constructor/stages/cartesian_position_motion.h +++ b/include/moveit_task_constructor/stages/cartesian_position_motion.h @@ -4,8 +4,6 @@ #include -#include - #include #include @@ -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); }; } } } diff --git a/include/moveit_task_constructor/stages/generate_grasp_pose.h b/include/moveit_task_constructor/stages/generate_grasp_pose.h index 3221ff7e..08357170 100644 --- a/include/moveit_task_constructor/stages/generate_grasp_pose.h +++ b/include/moveit_task_constructor/stages/generate_grasp_pose.h @@ -4,10 +4,6 @@ #include -#include - -#include - 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 > previous_solutions_; - - ros::Publisher pub; }; } } } diff --git a/include/moveit_task_constructor/task.h b/include/moveit_task_constructor/task.h index 8e52a982..286c3e5d 100644 --- a/include/moveit_task_constructor/task.h +++ b/include/moveit_task_constructor/task.h @@ -49,17 +49,11 @@ public: /// enable introspection publishing for use with rviz void enableIntrospection(bool enable = true); - - typedef std::function SolutionCallback; - typedef std::list 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 TaskCallback; typedef std::list 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_; - std::list solution_cbs_; // functions called for each new solution std::list task_cbs_; // functions to monitor task's planning progress }; diff --git a/src/container.cpp b/src/container.cpp index c7fc89ca..17f9cdc3 100644 --- a/src/container.cpp +++ b/src/container.cpp @@ -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()); } diff --git a/src/demo/plan_pick_trixi.cpp b/src/demo/plan_pick_trixi.cpp index 28fef053..6ff4490e 100644 --- a/src/demo/plan_pick_trixi.cpp +++ b/src/demo/plan_pick_trixi.cpp @@ -61,6 +61,7 @@ int main(int argc, char** argv){ { auto move= std::make_unique("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); diff --git a/src/demo/plan_pick_ur5.cpp b/src/demo/plan_pick_ur5.cpp index 83bedf4a..9dfc1e1f 100644 --- a/src/demo/plan_pick_ur5.cpp +++ b/src/demo/plan_pick_ur5.cpp @@ -59,6 +59,7 @@ int main(int argc, char** argv){ { auto move = std::make_unique("proceed to grasp pose"); + move->addSolutionCallback(std::ref(t.introspection())); move->setGroup("arm"); move->setLink("s_model_tool0"); move->setMinMaxDistance(.03, 0.1); diff --git a/src/stage.cpp b/src/stage.cpp index 94a761b5..597da4ba 100644 --- a/src/stage.cpp +++ b/src/stage.cpp @@ -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 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); } } } diff --git a/src/stage_p.h b/src/stage_p.h index 5b71381c..1d3fa1d1 100644 --- a/src/stage_p.h +++ b/src/stage_p.h @@ -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 ¤t); + 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 solution_cbs_; + private: // !! items write-accessed only by ContainerBasePrivate to maintain hierarchy !! ContainerBase* parent_; // owning parent diff --git a/src/stages/cartesian_position_motion.cpp b/src/stages/cartesian_position_motion.cpp index 5c8d46a7..0d587685 100644 --- a/src/stages/cartesian_position_motion.cpp +++ b/src/stages/cartesian_position_motion.cpp @@ -1,9 +1,6 @@ #include #include -#include -#include - #include #include #include @@ -19,9 +16,6 @@ CartesianPositionMotion::CartesianPositionMotion(std::string name) : PropagatingEitherWay(name), step_size_(0.005) { - ros::NodeHandle nh; - pub= nh.advertise("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); -} - } } } diff --git a/src/stages/generate_grasp_pose.cpp b/src/stages/generate_grasp_pose.cpp index a2fbd4d5..e5b2fc1b 100644 --- a/src/stages/generate_grasp_pose.cpp +++ b/src/stages/generate_grasp_pose.cpp @@ -21,9 +21,6 @@ namespace moveit { namespace task_constructor { namespace stages { GenerateGraspPose::GenerateGraspPose(std::string name) : Generator(name) { - ros::NodeHandle nh; - pub= nh.advertise("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(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)); diff --git a/src/task.cpp b/src/task.cpp index 6a485af9..5406aedb 100644 --- a/src/task.cpp +++ b/src/task.cpp @@ -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); }