mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
cb85e1b864
commit
622603268f
@ -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);
|
||||
|
||||
@ -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);
|
||||
};
|
||||
|
||||
} } }
|
||||
|
||||
@ -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;
|
||||
};
|
||||
|
||||
} } }
|
||||
|
||||
@ -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
|
||||
};
|
||||
|
||||
|
||||
@ -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());
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
} }
|
||||
|
||||
@ -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<Stage::SolutionCallback> solution_cbs_;
|
||||
|
||||
private:
|
||||
// !! items write-accessed only by ContainerBasePrivate to maintain hierarchy !!
|
||||
ContainerBase* parent_; // owning parent
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
} } }
|
||||
|
||||
@ -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));
|
||||
|
||||
14
src/task.cpp
14
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);
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user