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 processSolutions(const SolutionProcessor &processor) const = 0;
|
||||||
virtual void processFailures(const SolutionProcessor &processor) const {}
|
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:
|
protected:
|
||||||
/// Stage can only be instantiated through derived classes
|
/// Stage can only be instantiated through derived classes
|
||||||
Stage(StagePrivate *impl);
|
Stage(StagePrivate *impl);
|
||||||
|
|||||||
@ -4,8 +4,6 @@
|
|||||||
|
|
||||||
#include <moveit_task_constructor/stage.h>
|
#include <moveit_task_constructor/stage.h>
|
||||||
|
|
||||||
#include <ros/ros.h>
|
|
||||||
|
|
||||||
#include <geometry_msgs/Vector3Stamped.h>
|
#include <geometry_msgs/Vector3Stamped.h>
|
||||||
#include <geometry_msgs/PointStamped.h>
|
#include <geometry_msgs/PointStamped.h>
|
||||||
|
|
||||||
@ -52,10 +50,6 @@ protected:
|
|||||||
geometry_msgs::Vector3Stamped along_;
|
geometry_msgs::Vector3Stamped along_;
|
||||||
|
|
||||||
double step_size_;
|
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 <moveit_task_constructor/stage.h>
|
||||||
|
|
||||||
#include <ros/ros.h>
|
|
||||||
|
|
||||||
#include <moveit/macros/class_forward.h>
|
|
||||||
|
|
||||||
namespace moveit { namespace task_constructor { namespace stages {
|
namespace moveit { namespace task_constructor { namespace stages {
|
||||||
|
|
||||||
class GenerateGraspPose : public Generator {
|
class GenerateGraspPose : public Generator {
|
||||||
@ -70,8 +66,6 @@ protected:
|
|||||||
bool tried_current_state_as_seed_ = false;
|
bool tried_current_state_as_seed_ = false;
|
||||||
|
|
||||||
std::vector< std::vector<double> > previous_solutions_;
|
std::vector< std::vector<double> > previous_solutions_;
|
||||||
|
|
||||||
ros::Publisher pub;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
} } }
|
} } }
|
||||||
|
|||||||
@ -49,17 +49,11 @@ public:
|
|||||||
|
|
||||||
/// enable introspection publishing for use with rviz
|
/// enable introspection publishing for use with rviz
|
||||||
void enableIntrospection(bool enable = true);
|
void enableIntrospection(bool enable = true);
|
||||||
|
Introspection &introspection();
|
||||||
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);
|
|
||||||
|
|
||||||
typedef std::function<void(const Task &t)> TaskCallback;
|
typedef std::function<void(const Task &t)> TaskCallback;
|
||||||
typedef std::list<TaskCallback> TaskCallbackList;
|
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);
|
TaskCallbackList::const_iterator addTaskCallback(TaskCallback &&cb);
|
||||||
/// remove function callback
|
/// remove function callback
|
||||||
void erase(TaskCallbackList::const_iterator which);
|
void erase(TaskCallbackList::const_iterator which);
|
||||||
@ -112,7 +106,6 @@ private:
|
|||||||
|
|
||||||
// introspection and monitoring
|
// introspection and monitoring
|
||||||
std::unique_ptr<Introspection> introspection_;
|
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
|
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);
|
nextStarts()->add(InterfaceState(*internal_to), &solution, NULL);
|
||||||
}
|
}
|
||||||
|
|
||||||
// inform parent about new solution
|
// perform default stage action on new solution
|
||||||
parent()->onNewSolution(solutions_.back());
|
newSolution(solutions_.back());
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@ -449,8 +449,8 @@ void ParallelContainerBase::onNewSolution(SolutionBase &s)
|
|||||||
|
|
||||||
// store solution in our own list
|
// store solution in our own list
|
||||||
impl->solutions_.emplace_back(std::move(wrapped));
|
impl->solutions_.emplace_back(std::move(wrapped));
|
||||||
// inform parent
|
// perform default stage action on new solution
|
||||||
impl->parent()->onNewSolution(impl->solutions_.back());
|
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");
|
auto move= std::make_unique<stages::CartesianPositionMotion>("proceed to grasp pose");
|
||||||
|
move->addSolutionCallback(std::ref(t.introspection()));
|
||||||
move->setGroup("left_arm");
|
move->setGroup("left_arm");
|
||||||
move->setLink("l_gripper_tool_frame");
|
move->setLink("l_gripper_tool_frame");
|
||||||
move->setMinMaxDistance(.03, 0.1);
|
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");
|
auto move = std::make_unique<stages::CartesianPositionMotion>("proceed to grasp pose");
|
||||||
|
move->addSolutionCallback(std::ref(t.introspection()));
|
||||||
move->setGroup("arm");
|
move->setGroup("arm");
|
||||||
move->setLink("s_model_tool0");
|
move->setLink("s_model_tool0");
|
||||||
move->setMinMaxDistance(.03, 0.1);
|
move->setMinMaxDistance(.03, 0.1);
|
||||||
|
|||||||
@ -63,6 +63,14 @@ void StagePrivate::validate() const {
|
|||||||
if (errors) throw errors;
|
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)
|
Stage::Stage(StagePrivate *impl)
|
||||||
: pimpl_(impl)
|
: pimpl_(impl)
|
||||||
{
|
{
|
||||||
@ -104,6 +112,18 @@ void Stage::setName(const std::string& name)
|
|||||||
pimpl_->name_ = 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>
|
template<InterfaceFlag own, InterfaceFlag other>
|
||||||
const char* direction(const StagePrivate& stage) {
|
const char* direction(const StagePrivate& stage) {
|
||||||
InterfaceFlags f = stage.interfaceFlags();
|
InterfaceFlags f = stage.interfaceFlags();
|
||||||
@ -321,7 +341,7 @@ void PropagatingEitherWay::sendForward(const InterfaceState& from,
|
|||||||
SubTrajectory &trajectory = addTrajectory(t, cost);
|
SubTrajectory &trajectory = addTrajectory(t, cost);
|
||||||
trajectory.setStartState(from);
|
trajectory.setStartState(from);
|
||||||
impl->nextStarts()->add(std::move(to), &trajectory, NULL);
|
impl->nextStarts()->add(std::move(to), &trajectory, NULL);
|
||||||
impl->parent()->onNewSolution(trajectory);
|
impl->newSolution(trajectory);
|
||||||
}
|
}
|
||||||
|
|
||||||
void PropagatingEitherWay::sendBackward(InterfaceState&& from,
|
void PropagatingEitherWay::sendBackward(InterfaceState&& from,
|
||||||
@ -333,7 +353,7 @@ void PropagatingEitherWay::sendBackward(InterfaceState&& from,
|
|||||||
SubTrajectory& trajectory = addTrajectory(t, cost);
|
SubTrajectory& trajectory = addTrajectory(t, cost);
|
||||||
trajectory.setEndState(to);
|
trajectory.setEndState(to);
|
||||||
impl->prevEnds()->add(std::move(from), NULL, &trajectory);
|
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);
|
SubTrajectory& trajectory = addTrajectory(dummy, cost);
|
||||||
impl->prevEnds()->add(InterfaceState(state), NULL, &trajectory);
|
impl->prevEnds()->add(InterfaceState(state), NULL, &trajectory);
|
||||||
impl->nextStarts()->add(std::move(state), &trajectory, NULL);
|
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);
|
SubTrajectory& trajectory = addTrajectory(t, cost);
|
||||||
trajectory.setStartState(from);
|
trajectory.setStartState(from);
|
||||||
trajectory.setEndState(to);
|
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 setPrevEnds(const InterfacePtr& prev_ends) { prev_ends_ = prev_ends; }
|
||||||
inline void setNextStarts(const InterfacePtr& next_starts) { next_starts_ = next_starts; }
|
inline void setNextStarts(const InterfacePtr& next_starts) { next_starts_ = next_starts; }
|
||||||
|
|
||||||
|
void newSolution(SolutionBase ¤t);
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
Stage* const me_; // associated/owning Stage instance
|
Stage* const me_; // associated/owning Stage instance
|
||||||
std::string name_;
|
std::string name_;
|
||||||
@ -63,6 +65,9 @@ protected:
|
|||||||
InterfacePtr starts_;
|
InterfacePtr starts_;
|
||||||
InterfacePtr ends_;
|
InterfacePtr ends_;
|
||||||
|
|
||||||
|
// functions called for each new solution
|
||||||
|
std::list<Stage::SolutionCallback> solution_cbs_;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// !! items write-accessed only by ContainerBasePrivate to maintain hierarchy !!
|
// !! items write-accessed only by ContainerBasePrivate to maintain hierarchy !!
|
||||||
ContainerBase* parent_; // owning parent
|
ContainerBase* parent_; // owning parent
|
||||||
|
|||||||
@ -1,9 +1,6 @@
|
|||||||
#include <moveit_task_constructor/stages/cartesian_position_motion.h>
|
#include <moveit_task_constructor/stages/cartesian_position_motion.h>
|
||||||
#include <moveit_task_constructor/storage.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/planning_scene/planning_scene.h>
|
||||||
#include <moveit/robot_state/conversions.h>
|
#include <moveit/robot_state/conversions.h>
|
||||||
#include <moveit/robot_state/robot_state.h>
|
#include <moveit/robot_state/robot_state.h>
|
||||||
@ -19,9 +16,6 @@ CartesianPositionMotion::CartesianPositionMotion(std::string name)
|
|||||||
: PropagatingEitherWay(name),
|
: PropagatingEitherWay(name),
|
||||||
step_size_(0.005)
|
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){
|
void CartesianPositionMotion::setGroup(std::string group){
|
||||||
@ -148,10 +142,6 @@ bool CartesianPositionMotion::computeForward(const InterfaceState &from){
|
|||||||
for( auto& tp : trajectory_steps )
|
for( auto& tp : trajectory_steps )
|
||||||
traj->addSuffixWayPoint(tp, 0.0);
|
traj->addSuffixWayPoint(tp, 0.0);
|
||||||
sendForward(from, InterfaceState(result_scene), traj);
|
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;
|
return succeeded;
|
||||||
@ -214,27 +204,9 @@ bool CartesianPositionMotion::computeBackward(const InterfaceState &to){
|
|||||||
for( auto& tp : trajectory_steps )
|
for( auto& tp : trajectory_steps )
|
||||||
traj->addPrefixWayPoint(tp, 0.0);
|
traj->addPrefixWayPoint(tp, 0.0);
|
||||||
sendBackward(InterfaceState(result_scene), to, traj);
|
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;
|
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)
|
GenerateGraspPose::GenerateGraspPose(std::string name)
|
||||||
: Generator(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)
|
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);
|
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();
|
remaining_time_-= std::chrono::duration<double>(std::chrono::steady_clock::now()- now).count();
|
||||||
|
|
||||||
if(succeeded){
|
if(succeeded) {
|
||||||
moveit_msgs::DisplayRobotState drs;
|
|
||||||
robot_state::robotStateToRobotStateMsg(grasp_state, drs.state);
|
|
||||||
pub.publish(drs);
|
|
||||||
|
|
||||||
previous_solutions_.emplace_back();
|
previous_solutions_.emplace_back();
|
||||||
grasp_state.copyJointGroupPositions(jmg_active, previous_solutions_.back());
|
grasp_state.copyJointGroupPositions(jmg_active, previous_solutions_.back());
|
||||||
spawn(InterfaceState(grasp_scene));
|
spawn(InterfaceState(grasp_scene));
|
||||||
|
|||||||
14
src/task.cpp
14
src/task.cpp
@ -112,15 +112,10 @@ void Task::enableIntrospection(bool enable)
|
|||||||
introspection_.reset();
|
introspection_.reset();
|
||||||
}
|
}
|
||||||
|
|
||||||
Task::SolutionCallbackList::const_iterator Task::addSolutionCallback(SolutionCallback &&cb)
|
Introspection &Task::introspection()
|
||||||
{
|
{
|
||||||
solution_cbs_.emplace_back(std::move(cb));
|
enableIntrospection(true);
|
||||||
return --solution_cbs_.cend();
|
return *introspection_;
|
||||||
}
|
|
||||||
|
|
||||||
void Task::erase(SolutionCallbackList::const_iterator which)
|
|
||||||
{
|
|
||||||
solution_cbs_.erase(which);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
Task::TaskCallbackList::const_iterator Task::addTaskCallback(TaskCallback &&cb)
|
Task::TaskCallbackList::const_iterator Task::addTaskCallback(TaskCallback &&cb)
|
||||||
@ -216,8 +211,7 @@ void Task::publishAllSolutions(bool wait)
|
|||||||
|
|
||||||
void Task::onNewSolution(SolutionBase &s)
|
void Task::onNewSolution(SolutionBase &s)
|
||||||
{
|
{
|
||||||
for (const auto& cb : solution_cbs_)
|
pimpl()->newSolution(s);
|
||||||
cb(s);
|
|
||||||
if (introspection_)
|
if (introspection_)
|
||||||
introspection_->publishSolution(s);
|
introspection_->publishSolution(s);
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user