mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Merge branches 'travis', 'fixes', 'visualization' and 'todos'
This commit is contained in:
commit
728c1b40e2
@ -76,7 +76,7 @@ public:
|
|||||||
protected:
|
protected:
|
||||||
SolutionBase* storeSequential(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
|
SolutionBase* storeSequential(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
|
||||||
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes);
|
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes);
|
||||||
robot_trajectory::RobotTrajectoryPtr merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
|
robot_trajectory::RobotTrajectoryConstPtr merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
|
||||||
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes,
|
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes,
|
||||||
const moveit::core::RobotState& state);
|
const moveit::core::RobotState& state);
|
||||||
|
|
||||||
|
|||||||
@ -64,6 +64,7 @@ MOVEIT_CLASS_FORWARD(Task)
|
|||||||
*/
|
*/
|
||||||
class Task : protected WrapperBase {
|
class Task : protected WrapperBase {
|
||||||
public:
|
public:
|
||||||
|
// +1 TODO: move into MoveIt! core
|
||||||
static planning_pipeline::PlanningPipelinePtr createPlanner(const moveit::core::RobotModelConstPtr &model,
|
static planning_pipeline::PlanningPipelinePtr createPlanner(const moveit::core::RobotModelConstPtr &model,
|
||||||
const std::string &ns = "move_group",
|
const std::string &ns = "move_group",
|
||||||
const std::string &planning_plugin_param_name = "planning_plugin",
|
const std::string &planning_plugin_param_name = "planning_plugin",
|
||||||
@ -81,6 +82,7 @@ public:
|
|||||||
/// load robot model from given parameter
|
/// load robot model from given parameter
|
||||||
void loadRobotModel(const std::string& robot_description = "robot_description");
|
void loadRobotModel(const std::string& robot_description = "robot_description");
|
||||||
|
|
||||||
|
// TODO: use Stage::insert as well?
|
||||||
void add(Stage::pointer &&stage);
|
void add(Stage::pointer &&stage);
|
||||||
void clear() override;
|
void clear() override;
|
||||||
|
|
||||||
@ -111,6 +113,7 @@ public:
|
|||||||
/// publish all top-level solutions
|
/// publish all top-level solutions
|
||||||
void publishAllSolutions(bool wait = true);
|
void publishAllSolutions(bool wait = true);
|
||||||
|
|
||||||
|
// +1 TODO: convenient access to arbitrary stage by name. traverse hierarchy using / separator?
|
||||||
/// access stage tree
|
/// access stage tree
|
||||||
ContainerBase *stages();
|
ContainerBase *stages();
|
||||||
const ContainerBase *stages() const;
|
const ContainerBase *stages() const;
|
||||||
|
|||||||
@ -116,6 +116,8 @@ typedef std::vector<std::vector<double>> IKSolutions;
|
|||||||
|
|
||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
|
// ??? TODO: provide callback methods in PlanningScene class / probably not very useful here though...
|
||||||
|
// move into MoveIt! core
|
||||||
bool isTargetPoseColliding(const planning_scene::PlanningScenePtr& scene,
|
bool isTargetPoseColliding(const planning_scene::PlanningScenePtr& scene,
|
||||||
Eigen::Affine3d pose, const robot_model::LinkModel* link)
|
Eigen::Affine3d pose, const robot_model::LinkModel* link)
|
||||||
{
|
{
|
||||||
@ -215,6 +217,8 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
|
|||||||
assert(s.start()->scene() == s.end()->scene()); // wrapped child should be a generator
|
assert(s.start()->scene() == s.end()->scene()); // wrapped child should be a generator
|
||||||
planning_scene::PlanningScenePtr sandbox_scene = s.start()->scene()->diff();
|
planning_scene::PlanningScenePtr sandbox_scene = s.start()->scene()->diff();
|
||||||
|
|
||||||
|
// -1 TODO: this should not be necessary in my opinion: Why do you think so?
|
||||||
|
// It is, because the properties on the interface might change from call to call...
|
||||||
// enforced initialization from interface ensures that new target_pose is read
|
// enforced initialization from interface ensures that new target_pose is read
|
||||||
properties().performInitFrom(INTERFACE, s.start()->properties(), true);
|
properties().performInitFrom(INTERFACE, s.start()->properties(), true);
|
||||||
const auto& props = properties();
|
const auto& props = properties();
|
||||||
@ -376,6 +380,9 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
|
|||||||
spawn(InterfaceState(scene), std::move(solution));
|
spawn(InterfaceState(scene), std::move(solution));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// TODO: magic constant should be a property instead ("current_seed_only", or equivalent)
|
||||||
|
// Yeah, you are right, these are two different semantic concepts:
|
||||||
|
// One could also have multiple IK solutions derived from the same seed
|
||||||
if (!succeeded && max_ik_solutions == 1)
|
if (!succeeded && max_ik_solutions == 1)
|
||||||
break; // first and only attempt failed
|
break; // first and only attempt failed
|
||||||
}
|
}
|
||||||
|
|||||||
@ -126,7 +126,7 @@ bool Connect::compatible(const InterfaceState& from_state, const InterfaceState&
|
|||||||
const unsigned int num = jm->getVariableCount();
|
const unsigned int num = jm->getVariableCount();
|
||||||
Eigen::Map<const Eigen::VectorXd> positions_from (from.getJointPositions(jm), num);
|
Eigen::Map<const Eigen::VectorXd> positions_from (from.getJointPositions(jm), num);
|
||||||
Eigen::Map<const Eigen::VectorXd> positions_to (to.getJointPositions(jm), num);
|
Eigen::Map<const Eigen::VectorXd> positions_to (to.getJointPositions(jm), num);
|
||||||
if (!positions_from.array().isApprox(positions_to.array())) {
|
if (!positions_from.array().isApprox(positions_to.array(), 1e-4)) {
|
||||||
ROS_INFO_STREAM_ONCE_NAMED("Connect", "Deviation in joint " << jm->getName()
|
ROS_INFO_STREAM_ONCE_NAMED("Connect", "Deviation in joint " << jm->getName()
|
||||||
<< ": [" << positions_from.transpose()
|
<< ": [" << positions_from.transpose()
|
||||||
<< "] != [" << positions_to.transpose() << "]");
|
<< "] != [" << positions_to.transpose() << "]");
|
||||||
@ -158,7 +158,7 @@ bool Connect::compute(const InterfaceState &from, const InterfaceState &to) {
|
|||||||
end->getCurrentStateNonConst().setJointGroupPositions(jmg, positions);
|
end->getCurrentStateNonConst().setJointGroupPositions(jmg, positions);
|
||||||
|
|
||||||
robot_trajectory::RobotTrajectoryPtr trajectory;
|
robot_trajectory::RobotTrajectoryPtr trajectory;
|
||||||
if (!pair.second->plan(start, to.scene(), jmg, timeout, trajectory, path_constraints))
|
if (!pair.second->plan(start, end, jmg, timeout, trajectory, path_constraints))
|
||||||
break;
|
break;
|
||||||
|
|
||||||
sub_trajectories.push_back(trajectory);
|
sub_trajectories.push_back(trajectory);
|
||||||
@ -176,7 +176,7 @@ bool Connect::compute(const InterfaceState &from, const InterfaceState &to) {
|
|||||||
// mark solution as failure
|
// mark solution as failure
|
||||||
solution->setCost(std::numeric_limits<double>::infinity());
|
solution->setCost(std::numeric_limits<double>::infinity());
|
||||||
} else {
|
} else {
|
||||||
robot_trajectory::RobotTrajectoryPtr t = merge(sub_trajectories, intermediate_scenes, from.scene()->getCurrentState());
|
auto t = merge(sub_trajectories, intermediate_scenes, from.scene()->getCurrentState());
|
||||||
if (t) {
|
if (t) {
|
||||||
connect(from, to, SubTrajectory(t));
|
connect(from, to, SubTrajectory(t));
|
||||||
return true;
|
return true;
|
||||||
@ -218,11 +218,16 @@ SolutionBase* Connect::storeSequential(const std::vector<robot_trajectory::Robot
|
|||||||
return &solutions_.back();
|
return &solutions_.back();
|
||||||
}
|
}
|
||||||
|
|
||||||
robot_trajectory::RobotTrajectoryPtr Connect::merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
|
robot_trajectory::RobotTrajectoryConstPtr Connect::merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
|
||||||
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes,
|
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes,
|
||||||
const moveit::core::RobotState& state)
|
const moveit::core::RobotState& state)
|
||||||
{
|
{
|
||||||
|
// no need to merge if there is only a single sub trajectory
|
||||||
|
if (sub_trajectories.size() == 1)
|
||||||
|
return sub_trajectories[0];
|
||||||
|
|
||||||
auto jmg = merged_jmg_.get();
|
auto jmg = merged_jmg_.get();
|
||||||
|
assert(jmg);
|
||||||
robot_trajectory::RobotTrajectoryPtr trajectory = task_constructor::merge(sub_trajectories, state, jmg);
|
robot_trajectory::RobotTrajectoryPtr trajectory = task_constructor::merge(sub_trajectories, state, jmg);
|
||||||
// TODO: check merged trajectory for collisions
|
// TODO: check merged trajectory for collisions
|
||||||
return trajectory;
|
return trajectory;
|
||||||
|
|||||||
@ -122,6 +122,7 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Cartesian targets require the link name
|
// Cartesian targets require the link name
|
||||||
|
// TODO: use ik_frame property as in ComputeIK
|
||||||
std::string link_name = props.get<std::string>("link");
|
std::string link_name = props.get<std::string>("link");
|
||||||
const moveit::core::LinkModel* link;
|
const moveit::core::LinkModel* link;
|
||||||
if (link_name.empty())
|
if (link_name.empty())
|
||||||
@ -241,10 +242,12 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
|
|||||||
|
|
||||||
// add an arrow marker
|
// add an arrow marker
|
||||||
visualization_msgs::Marker m;
|
visualization_msgs::Marker m;
|
||||||
|
// +1 TODO: make "marker" a common property of all stages. However, I would stick with "marker_ns"
|
||||||
m.ns = props.get<std::string>("marker_ns");
|
m.ns = props.get<std::string>("marker_ns");
|
||||||
if (!m.ns.empty()) {
|
if (!m.ns.empty()) {
|
||||||
m.header.frame_id = scene->getPlanningFrame();
|
m.header.frame_id = scene->getPlanningFrame();
|
||||||
if (linear_norm > 1e-3) {
|
if (linear_norm > 1e-3) {
|
||||||
|
// +1 TODO: arrow could be split into "valid" and "invalid" part (as red cylinder)
|
||||||
rviz_marker_tools::setColor(m.color, success ? rviz_marker_tools::LIME_GREEN
|
rviz_marker_tools::setColor(m.color, success ? rviz_marker_tools::LIME_GREEN
|
||||||
: rviz_marker_tools::RED);
|
: rviz_marker_tools::RED);
|
||||||
rviz_marker_tools::makeArrow(m, linear_norm);
|
rviz_marker_tools::makeArrow(m, linear_norm);
|
||||||
|
|||||||
@ -48,7 +48,7 @@ MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& plan
|
|||||||
, planner_(planner)
|
, planner_(planner)
|
||||||
{
|
{
|
||||||
auto& p = properties();
|
auto& p = properties();
|
||||||
p.declare<double>("timeout", 10.0, "planning timeout");
|
p.declare<double>("timeout", 10.0, "planning timeout"); // TODO: make this a common property in Stage
|
||||||
p.declare<std::string>("group", "name of planning group");
|
p.declare<std::string>("group", "name of planning group");
|
||||||
p.declare<std::string>("link", "", "link to move (default is tip of jmg)");
|
p.declare<std::string>("link", "", "link to move (default is tip of jmg)");
|
||||||
|
|
||||||
@ -184,6 +184,8 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP
|
|||||||
return success;
|
return success;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// -1 TODO: move these as default implementation to PropagateEitherWay?
|
||||||
|
// Essentially, here compute() is a class-specific worker function
|
||||||
bool MoveTo::computeForward(const InterfaceState &from){
|
bool MoveTo::computeForward(const InterfaceState &from){
|
||||||
planning_scene::PlanningScenePtr to;
|
planning_scene::PlanningScenePtr to;
|
||||||
SubTrajectory trajectory;
|
SubTrajectory trajectory;
|
||||||
|
|||||||
@ -106,9 +106,10 @@ public:
|
|||||||
const moveit::core::RobotStatePtr& getWayPointPtr(size_t index) const {
|
const moveit::core::RobotStatePtr& getWayPointPtr(size_t index) const {
|
||||||
return getWayPointPtr(indexPair(index));
|
return getWayPointPtr(indexPair(index));
|
||||||
}
|
}
|
||||||
|
const planning_scene::PlanningSceneConstPtr& startScene() const { return start_scene_; }
|
||||||
const planning_scene::PlanningSceneConstPtr& scene(const IndexPair& idx_pair) const;
|
const planning_scene::PlanningSceneConstPtr& scene(const IndexPair& idx_pair) const;
|
||||||
const planning_scene::PlanningSceneConstPtr& scene(size_t index) const {
|
const planning_scene::PlanningSceneConstPtr& scene(size_t index) const {
|
||||||
if (index == steps_)
|
if (index >= steps_)
|
||||||
return data_.back().scene_;
|
return data_.back().scene_;
|
||||||
return scene(indexPair(index));
|
return scene(indexPair(index));
|
||||||
}
|
}
|
||||||
|
|||||||
@ -88,7 +88,7 @@ protected:
|
|||||||
QPushButton* button_;
|
QPushButton* button_;
|
||||||
|
|
||||||
bool paused_;
|
bool paused_;
|
||||||
int last_way_point_;
|
bool empty_;
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace moveit_rviz_plugin
|
} // namespace moveit_rviz_plugin
|
||||||
|
|||||||
@ -156,9 +156,9 @@ protected:
|
|||||||
void unsetRobotColor(rviz::Robot* robot);
|
void unsetRobotColor(rviz::Robot* robot);
|
||||||
|
|
||||||
DisplaySolutionPtr displaying_solution_;
|
DisplaySolutionPtr displaying_solution_;
|
||||||
DisplaySolutionPtr solution_to_display_;
|
DisplaySolutionPtr next_solution_to_display_;
|
||||||
std::vector<rviz::Robot*> trail_;
|
std::vector<rviz::Robot*> trail_;
|
||||||
bool animating_ = false;
|
bool animating_ = false; // auto-progressing the current waypoint?
|
||||||
bool drop_displaying_solution_ = false;
|
bool drop_displaying_solution_ = false;
|
||||||
bool locked_ = false;
|
bool locked_ = false;
|
||||||
int current_state_ = -1;
|
int current_state_ = -1;
|
||||||
|
|||||||
@ -41,6 +41,7 @@ namespace moveit_rviz_plugin
|
|||||||
{
|
{
|
||||||
TaskSolutionPanel::TaskSolutionPanel(QWidget* parent) : Panel(parent)
|
TaskSolutionPanel::TaskSolutionPanel(QWidget* parent) : Panel(parent)
|
||||||
{
|
{
|
||||||
|
empty_ = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
TaskSolutionPanel::~TaskSolutionPanel()
|
TaskSolutionPanel::~TaskSolutionPanel()
|
||||||
@ -92,16 +93,16 @@ void TaskSolutionPanel::onDisable()
|
|||||||
|
|
||||||
void TaskSolutionPanel::update(int way_point_count)
|
void TaskSolutionPanel::update(int way_point_count)
|
||||||
{
|
{
|
||||||
int max_way_point = std::max(0, way_point_count - 1);
|
int max_way_point = std::max(1, way_point_count - 1);
|
||||||
|
empty_ = (way_point_count == 0);
|
||||||
|
|
||||||
slider_->setEnabled(way_point_count != 0);
|
slider_->setEnabled(way_point_count >= 0);
|
||||||
button_->setEnabled(way_point_count != 0);
|
button_->setEnabled(way_point_count >= 0);
|
||||||
|
|
||||||
last_way_point_ = max_way_point;
|
|
||||||
paused_ = false;
|
paused_ = false;
|
||||||
slider_->setSliderPosition(0);
|
|
||||||
slider_->setMaximum(max_way_point);
|
slider_->setMaximum(max_way_point);
|
||||||
maximum_label_->setText(QString::number(max_way_point));
|
maximum_label_->setText(way_point_count >= 0 ? QString::number(way_point_count) : "");
|
||||||
|
slider_->setSliderPosition(0);
|
||||||
}
|
}
|
||||||
|
|
||||||
void TaskSolutionPanel::pauseButton(bool pause)
|
void TaskSolutionPanel::pauseButton(bool pause)
|
||||||
@ -115,7 +116,7 @@ void TaskSolutionPanel::pauseButton(bool pause)
|
|||||||
{
|
{
|
||||||
button_->setText("Pause");
|
button_->setText("Pause");
|
||||||
paused_ = false;
|
paused_ = false;
|
||||||
if (slider_->sliderPosition() == last_way_point_)
|
if (slider_->sliderPosition() == slider_->maximum())
|
||||||
slider_->setSliderPosition(0);
|
slider_->setSliderPosition(0);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -127,7 +128,11 @@ void TaskSolutionPanel::setSliderPosition(int position)
|
|||||||
|
|
||||||
void TaskSolutionPanel::sliderValueChanged(int value)
|
void TaskSolutionPanel::sliderValueChanged(int value)
|
||||||
{
|
{
|
||||||
minimum_label_->setText(QString::number(value));
|
QString text;
|
||||||
|
if (!slider_->isEnabled()) text = "";
|
||||||
|
else if (empty_) text = value == 0 ? "S" : "E";
|
||||||
|
else text = QString::number(value+1);
|
||||||
|
minimum_label_->setText(text);
|
||||||
}
|
}
|
||||||
|
|
||||||
void TaskSolutionPanel::buttonClicked()
|
void TaskSolutionPanel::buttonClicked()
|
||||||
|
|||||||
@ -161,7 +161,7 @@ TaskSolutionVisualization::TaskSolutionVisualization(rviz::Property* parent, rvi
|
|||||||
TaskSolutionVisualization::~TaskSolutionVisualization()
|
TaskSolutionVisualization::~TaskSolutionVisualization()
|
||||||
{
|
{
|
||||||
clearTrail();
|
clearTrail();
|
||||||
solution_to_display_.reset();
|
next_solution_to_display_.reset();
|
||||||
displaying_solution_.reset();
|
displaying_solution_.reset();
|
||||||
|
|
||||||
scene_render_.reset();
|
scene_render_.reset();
|
||||||
@ -233,9 +233,11 @@ void TaskSolutionVisualization::onRobotModelLoaded(robot_model::RobotModelConstP
|
|||||||
void TaskSolutionVisualization::reset()
|
void TaskSolutionVisualization::reset()
|
||||||
{
|
{
|
||||||
clearTrail();
|
clearTrail();
|
||||||
solution_to_display_.reset();
|
next_solution_to_display_.reset();
|
||||||
displaying_solution_.reset();
|
displaying_solution_.reset();
|
||||||
animating_ = false;
|
current_state_ = -1;
|
||||||
|
if (slider_panel_)
|
||||||
|
slider_panel_->update(-1);
|
||||||
|
|
||||||
robot_render_->setVisualVisible(robot_visual_enabled_property_->getBool());
|
robot_render_->setVisualVisible(robot_visual_enabled_property_->getBool());
|
||||||
robot_render_->setCollisionVisible(robot_collision_enabled_property_->getBool());
|
robot_render_->setCollisionVisible(robot_collision_enabled_property_->getBool());
|
||||||
@ -253,16 +255,19 @@ void TaskSolutionVisualization::clearTrail()
|
|||||||
|
|
||||||
void TaskSolutionVisualization::changedLoopDisplay()
|
void TaskSolutionVisualization::changedLoopDisplay()
|
||||||
{
|
{
|
||||||
if (loop_display_property_->getBool() && slider_panel_)
|
// restart animation if current_state_ is at end and looping got activated
|
||||||
|
if (displaying_solution_ && loop_display_property_->getBool() &&
|
||||||
|
slider_panel_ && slider_panel_->isVisible() &&
|
||||||
|
current_state_+1 >= (int)displaying_solution_->getWayPointCount()) {
|
||||||
|
current_state_ = -1;
|
||||||
slider_panel_->pauseButton(false);
|
slider_panel_->pauseButton(false);
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|
||||||
void TaskSolutionVisualization::changedTrail()
|
void TaskSolutionVisualization::changedTrail()
|
||||||
{
|
{
|
||||||
clearTrail();
|
clearTrail();
|
||||||
DisplaySolutionPtr t = solution_to_display_;
|
DisplaySolutionPtr t = displaying_solution_;
|
||||||
if (!t)
|
|
||||||
t = displaying_solution_;
|
|
||||||
|
|
||||||
if (!t || !trail_display_property_->getBool()) {
|
if (!t || !trail_display_property_->getBool()) {
|
||||||
setVisibility(trail_scene_node_, main_scene_node_, false);
|
setVisibility(trail_scene_node_, main_scene_node_, false);
|
||||||
@ -273,8 +278,7 @@ void TaskSolutionVisualization::changedTrail()
|
|||||||
setVisibility(trail_scene_node_, main_scene_node_, true);
|
setVisibility(trail_scene_node_, main_scene_node_, true);
|
||||||
|
|
||||||
int stepsize = trail_step_size_property_->getInt();
|
int stepsize = trail_step_size_property_->getInt();
|
||||||
// always include last trajectory point
|
trail_.resize(t->getWayPointCount() / stepsize);
|
||||||
trail_.resize((int)std::ceil((t->getWayPointCount() + stepsize - 1) / (float)stepsize));
|
|
||||||
for (std::size_t i = 0; i < trail_.size(); i++)
|
for (std::size_t i = 0; i < trail_.size(); i++)
|
||||||
{
|
{
|
||||||
int waypoint_i = std::min(i * stepsize, t->getWayPointCount() - 1); // limit to last trajectory point
|
int waypoint_i = std::min(i * stepsize, t->getWayPointCount() - 1); // limit to last trajectory point
|
||||||
@ -325,7 +329,8 @@ void TaskSolutionVisualization::onDisable()
|
|||||||
parent_scene_node_->removeChild(main_scene_node_);
|
parent_scene_node_->removeChild(main_scene_node_);
|
||||||
|
|
||||||
displaying_solution_.reset();
|
displaying_solution_.reset();
|
||||||
animating_ = false;
|
next_solution_to_display_.reset();
|
||||||
|
current_state_ = -1;
|
||||||
if (slider_panel_) {
|
if (slider_panel_) {
|
||||||
slider_panel_was_visible_ = slider_panel_->isVisible();
|
slider_panel_was_visible_ = slider_panel_->isVisible();
|
||||||
slider_panel_->onDisable();
|
slider_panel_->onDisable();
|
||||||
@ -374,107 +379,121 @@ float TaskSolutionVisualization::getStateDisplayTime()
|
|||||||
|
|
||||||
void TaskSolutionVisualization::update(float wall_dt, float ros_dt)
|
void TaskSolutionVisualization::update(float wall_dt, float ros_dt)
|
||||||
{
|
{
|
||||||
if (drop_displaying_solution_)
|
if (drop_displaying_solution_) {
|
||||||
{
|
current_state_ = -1;
|
||||||
animating_ = false;
|
|
||||||
displaying_solution_.reset();
|
displaying_solution_.reset();
|
||||||
slider_panel_->update(0);
|
if (slider_panel_) slider_panel_->update(-1);
|
||||||
drop_displaying_solution_ = false;
|
drop_displaying_solution_ = false;
|
||||||
}
|
}
|
||||||
if (!animating_)
|
if (current_state_ < 0 || // last animation finished
|
||||||
{ // finished last animation
|
(slider_panel_ && slider_panel_->isPaused())) {
|
||||||
boost::mutex::scoped_lock lock(display_solution_mutex_);
|
boost::mutex::scoped_lock lock(display_solution_mutex_);
|
||||||
|
|
||||||
// new trajectory available to display?
|
// new trajectory available to display?
|
||||||
if (solution_to_display_ && (!locked_ || !displaying_solution_)) {
|
if (next_solution_to_display_ && (!locked_ || !displaying_solution_)) {
|
||||||
|
current_state_ = -1;
|
||||||
animating_ = true;
|
animating_ = true;
|
||||||
displaying_solution_ = solution_to_display_;
|
displaying_solution_ = next_solution_to_display_;
|
||||||
changedTrail();
|
changedTrail();
|
||||||
if (slider_panel_)
|
if (slider_panel_)
|
||||||
slider_panel_->update(solution_to_display_->getWayPointCount());
|
slider_panel_->update(next_solution_to_display_->getWayPointCount());
|
||||||
}
|
}
|
||||||
else if (displaying_solution_) {
|
// also reset if locked_
|
||||||
if (loop_display_property_->getBool()) {
|
next_solution_to_display_.reset();
|
||||||
animating_ = true;
|
}
|
||||||
} else if (slider_panel_ && slider_panel_->isVisible()) {
|
if (!displaying_solution_) {
|
||||||
if (slider_panel_->getSliderPosition() >= (int)displaying_solution_->getWayPointCount() - 1)
|
animating_ = false;
|
||||||
return; // nothing more to do
|
setVisibility();
|
||||||
else
|
|
||||||
animating_ = true;
|
|
||||||
} else if (locked_)
|
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
solution_to_display_.reset();
|
|
||||||
|
|
||||||
if (animating_)
|
|
||||||
{
|
|
||||||
// restart animation
|
|
||||||
current_state_ = -1;
|
|
||||||
trail_scene_node_->setVisible(false);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if (animating_)
|
|
||||||
{
|
|
||||||
int previous_state = current_state_;
|
int previous_state = current_state_;
|
||||||
|
// for an empty trajectory, show the start and end state at least
|
||||||
int waypoint_count = displaying_solution_->getWayPointCount();
|
int waypoint_count = displaying_solution_->getWayPointCount();
|
||||||
|
int max_state_index = std::max<int>(1, waypoint_count);
|
||||||
|
if (slider_panel_ && slider_panel_->isVisible()) {
|
||||||
|
animating_ = !slider_panel_->isPaused();
|
||||||
|
if (current_state_ >= 0)
|
||||||
|
// user can override current_state_ at any time with slider
|
||||||
|
current_state_ = slider_panel_->getSliderPosition();
|
||||||
|
} else if (current_state_ < max_state_index)
|
||||||
|
animating_ = true; // auto-activate animation if slider_panel_ is hidden
|
||||||
|
|
||||||
|
QString msg = "no change";
|
||||||
|
if (animating_ && current_state_ == previous_state) {
|
||||||
|
// auto-advance current_state_ based on time progress
|
||||||
current_state_time_ += wall_dt;
|
current_state_time_ += wall_dt;
|
||||||
|
|
||||||
float tm = getStateDisplayTime();
|
float tm = getStateDisplayTime();
|
||||||
if (slider_panel_ && slider_panel_->isVisible() && slider_panel_->isPaused())
|
if (current_state_ < 0) { // special case indicating restart of animation
|
||||||
current_state_ = slider_panel_->getSliderPosition();
|
|
||||||
else if (current_state_ < 0) { // special case indicating restart of animation
|
|
||||||
current_state_ = 0;
|
current_state_ = 0;
|
||||||
current_state_time_ = 0.0;
|
current_state_time_ = 0.0;
|
||||||
|
trail_scene_node_->setVisible(false);
|
||||||
|
msg = "restart";
|
||||||
} else if (tm < 0.0) { // using realtime: skip to next waypoint based on elapsed display time
|
} else if (tm < 0.0) { // using realtime: skip to next waypoint based on elapsed display time
|
||||||
while (current_state_ < waypoint_count &&
|
while (current_state_ < max_state_index &&
|
||||||
(tm = displaying_solution_->getWayPointDurationFromPrevious(current_state_ + 1)) < current_state_time_) {
|
(tm = displaying_solution_->getWayPointDurationFromPrevious(current_state_ + 1)) < current_state_time_) {
|
||||||
current_state_time_ -= tm;
|
|
||||||
++current_state_;
|
++current_state_;
|
||||||
|
current_state_time_ -= tm;
|
||||||
}
|
}
|
||||||
} else if (current_state_time_ > tm) { // fixed display time per state
|
} else if (current_state_time_ > tm) { // fixed display time per state
|
||||||
current_state_time_ = 0.0;
|
|
||||||
++current_state_;
|
++current_state_;
|
||||||
|
current_state_time_ = 0.0;
|
||||||
|
msg = "progress";
|
||||||
|
}
|
||||||
|
} else if (current_state_ != previous_state) { // current_state_ changed from slider
|
||||||
|
current_state_time_ = 0.0;
|
||||||
|
msg = "slider";
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if ((waypoint_count > 0 && current_state_ >= max_state_index) ||
|
||||||
|
(waypoint_count == 0 && current_state_ > max_state_index)) { // animation finished?
|
||||||
|
if (loop_display_property_->getBool())
|
||||||
|
current_state_ = -1; // restart in next cycle
|
||||||
|
else {
|
||||||
|
current_state_ = max_state_index;
|
||||||
|
if (slider_panel_)
|
||||||
|
slider_panel_->pauseButton(true);
|
||||||
|
}
|
||||||
|
setVisibility();
|
||||||
|
return;
|
||||||
|
}
|
||||||
if (current_state_ == previous_state)
|
if (current_state_ == previous_state)
|
||||||
return;
|
return;
|
||||||
|
|
||||||
if (current_state_ < waypoint_count)
|
|
||||||
{
|
|
||||||
renderWayPoint(current_state_, previous_state);
|
renderWayPoint(current_state_, previous_state);
|
||||||
|
|
||||||
|
// show / hide trail between start .. end
|
||||||
int stepsize = trail_step_size_property_->getInt();
|
int stepsize = trail_step_size_property_->getInt();
|
||||||
for (int i = std::max(0, previous_state / stepsize),
|
int start = std::max(0, previous_state / stepsize);
|
||||||
end = std::min(current_state_ / stepsize, ((int)trail_.size()) - 1); i <= end; ++i)
|
int end = current_state_ / stepsize;
|
||||||
trail_[i]->setVisible(true);
|
bool show = start <= end;
|
||||||
}
|
if (!show) std::swap(start, end);
|
||||||
else
|
end = std::min<int>(end, trail_.size());
|
||||||
{
|
for (; start < end; ++start) trail_[start]->setVisible(show);
|
||||||
animating_ = false; // animation finished
|
|
||||||
if (!loop_display_property_->getBool() && slider_panel_)
|
|
||||||
slider_panel_->pauseButton(true);
|
|
||||||
// ensure to render end state
|
|
||||||
renderWayPoint(waypoint_count, previous_state);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
setVisibility();
|
setVisibility();
|
||||||
}
|
}
|
||||||
|
|
||||||
void TaskSolutionVisualization::renderWayPoint(size_t index, int previous_index)
|
void TaskSolutionVisualization::renderWayPoint(size_t index, int previous_index)
|
||||||
{
|
{
|
||||||
|
assert(index >= 0);
|
||||||
|
size_t waypoint_count = displaying_solution_->getWayPointCount();
|
||||||
moveit::core::RobotStateConstPtr robot_state;
|
moveit::core::RobotStateConstPtr robot_state;
|
||||||
planning_scene::PlanningSceneConstPtr scene;
|
planning_scene::PlanningSceneConstPtr scene;
|
||||||
if (index == displaying_solution_->getWayPointCount()) {
|
if (index+1 >= waypoint_count) {
|
||||||
// render last state
|
if (index == 0 && waypoint_count == 0)
|
||||||
scene = displaying_solution_->scene(index);
|
// special case: render start scene
|
||||||
|
scene = displaying_solution_->startScene();
|
||||||
|
else // render end state
|
||||||
|
scene = displaying_solution_->scene(waypoint_count);
|
||||||
renderPlanningScene (scene);
|
renderPlanningScene (scene);
|
||||||
robot_state.reset(new moveit::core::RobotState(scene->getCurrentState()));
|
robot_state.reset(new moveit::core::RobotState(scene->getCurrentState()));
|
||||||
} else {
|
} else {
|
||||||
auto idx_pair = displaying_solution_->indexPair(index);
|
auto idx_pair = displaying_solution_->indexPair(index);
|
||||||
scene = displaying_solution_->scene(idx_pair);
|
scene = displaying_solution_->scene(idx_pair);
|
||||||
|
|
||||||
if (previous_index < 0 ||
|
if (previous_index < 0 || previous_index >= (int)waypoint_count ||
|
||||||
displaying_solution_->indexPair(previous_index).first != idx_pair.first) {
|
displaying_solution_->indexPair(previous_index).first != idx_pair.first) {
|
||||||
// switch to new stage: show new planning scene
|
// switch to new stage: show new planning scene
|
||||||
renderPlanningScene (scene);
|
renderPlanningScene (scene);
|
||||||
@ -532,7 +551,7 @@ void TaskSolutionVisualization::showTrajectory(const DisplaySolutionPtr& s, bool
|
|||||||
{
|
{
|
||||||
if (lock_display || !s->empty()) {
|
if (lock_display || !s->empty()) {
|
||||||
boost::mutex::scoped_lock lock(display_solution_mutex_);
|
boost::mutex::scoped_lock lock(display_solution_mutex_);
|
||||||
solution_to_display_ = s;
|
next_solution_to_display_ = s;
|
||||||
if (lock_display)
|
if (lock_display)
|
||||||
locked_ = drop_displaying_solution_ = true;
|
locked_ = drop_displaying_solution_ = true;
|
||||||
else if (interrupt_display_property_->getBool())
|
else if (interrupt_display_property_->getBool())
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user