mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
display scene corresponding to SubTrajectory
This commit is contained in:
parent
8129caa614
commit
c1d676589a
@ -65,7 +65,6 @@ class PanelDockWidget;
|
|||||||
|
|
||||||
namespace moveit { namespace core {
|
namespace moveit { namespace core {
|
||||||
MOVEIT_CLASS_FORWARD(RobotModel)
|
MOVEIT_CLASS_FORWARD(RobotModel)
|
||||||
MOVEIT_CLASS_FORWARD(RobotState)
|
|
||||||
} }
|
} }
|
||||||
namespace planning_scene { MOVEIT_CLASS_FORWARD(PlanningScene) }
|
namespace planning_scene { MOVEIT_CLASS_FORWARD(PlanningScene) }
|
||||||
namespace robot_trajectory { MOVEIT_CLASS_FORWARD(RobotTrajectory) }
|
namespace robot_trajectory { MOVEIT_CLASS_FORWARD(RobotTrajectory) }
|
||||||
@ -76,6 +75,7 @@ namespace moveit_rviz_plugin
|
|||||||
MOVEIT_CLASS_FORWARD(RobotStateVisualization)
|
MOVEIT_CLASS_FORWARD(RobotStateVisualization)
|
||||||
MOVEIT_CLASS_FORWARD(TaskSolutionVisualization)
|
MOVEIT_CLASS_FORWARD(TaskSolutionVisualization)
|
||||||
MOVEIT_CLASS_FORWARD(PlanningSceneRender)
|
MOVEIT_CLASS_FORWARD(PlanningSceneRender)
|
||||||
|
MOVEIT_CLASS_FORWARD(DisplaySolution)
|
||||||
|
|
||||||
class TaskSolutionPanel;
|
class TaskSolutionPanel;
|
||||||
class TaskSolutionVisualization : public QObject
|
class TaskSolutionVisualization : public QObject
|
||||||
@ -129,6 +129,7 @@ private Q_SLOTS:
|
|||||||
protected:
|
protected:
|
||||||
float getStateDisplayTime();
|
float getStateDisplayTime();
|
||||||
void clearTrail();
|
void clearTrail();
|
||||||
|
void renderWayPoint(size_t index, int previous_index);
|
||||||
void renderPlanningScene(const planning_scene::PlanningSceneConstPtr &scene);
|
void renderPlanningScene(const planning_scene::PlanningSceneConstPtr &scene);
|
||||||
|
|
||||||
// render the planning scene
|
// render the planning scene
|
||||||
@ -140,8 +141,8 @@ protected:
|
|||||||
void setRobotColor(rviz::Robot* robot, const QColor& color);
|
void setRobotColor(rviz::Robot* robot, const QColor& color);
|
||||||
void unsetRobotColor(rviz::Robot* robot);
|
void unsetRobotColor(rviz::Robot* robot);
|
||||||
|
|
||||||
robot_trajectory::RobotTrajectoryPtr displaying_solution_;
|
DisplaySolutionPtr displaying_solution_;
|
||||||
robot_trajectory::RobotTrajectoryPtr solution_to_display_;
|
DisplaySolutionPtr solution_to_display_;
|
||||||
std::vector<rviz::Robot*> trail_;
|
std::vector<rviz::Robot*> trail_;
|
||||||
bool animating_ = false;
|
bool animating_ = false;
|
||||||
bool drop_displaying_solution_ = false;
|
bool drop_displaying_solution_ = false;
|
||||||
|
|||||||
@ -168,15 +168,18 @@ void TaskSolutionVisualization::onInitialize(Ogre::SceneNode* scene_node, rviz::
|
|||||||
// Save pointers for later use
|
// Save pointers for later use
|
||||||
scene_node_ = scene_node;
|
scene_node_ = scene_node;
|
||||||
context_ = context;
|
context_ = context;
|
||||||
|
scene_node_->setVisible(false);
|
||||||
|
|
||||||
// Load trajectory robot
|
// Load trajectory robot
|
||||||
robot_render_.reset(new RobotStateVisualization(scene_node_, context_, "Solution Trajectory", robot_property_));
|
robot_render_.reset(new RobotStateVisualization(scene_node_, context_, "Solution Trajectory", robot_property_));
|
||||||
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());
|
||||||
robot_render_->setVisible(false);
|
changedRobotAlpha();
|
||||||
|
enabledRobotColor();
|
||||||
|
robot_render_->setVisible(true);
|
||||||
|
|
||||||
scene_render_.reset(new PlanningSceneRender(scene_node_, context_, robot_render_));
|
scene_render_.reset(new PlanningSceneRender(scene_node_, context_, RobotStateVisualizationPtr()));
|
||||||
scene_render_->getGeometryNode()->setVisible(false);
|
scene_render_->getGeometryNode()->setVisible(true);
|
||||||
|
|
||||||
rviz::WindowManagerInterface* window_context = context_->getWindowManager();
|
rviz::WindowManagerInterface* window_context = context_->getWindowManager();
|
||||||
if (window_context)
|
if (window_context)
|
||||||
@ -221,19 +224,18 @@ void TaskSolutionVisualization::reset()
|
|||||||
|
|
||||||
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());
|
||||||
robot_render_->setVisible(false);
|
scene_node_->setVisible(false);
|
||||||
}
|
}
|
||||||
|
|
||||||
void TaskSolutionVisualization::clearTrail()
|
void TaskSolutionVisualization::clearTrail()
|
||||||
{
|
{
|
||||||
for (std::size_t i = 0; i < trail_.size(); ++i)
|
qDeleteAll(trail_);
|
||||||
delete trail_[i];
|
|
||||||
trail_.clear();
|
trail_.clear();
|
||||||
}
|
}
|
||||||
|
|
||||||
void TaskSolutionVisualization::changedLoopDisplay()
|
void TaskSolutionVisualization::changedLoopDisplay()
|
||||||
{
|
{
|
||||||
robot_render_->setVisible(display_->isEnabled() && displaying_solution_ && animating_);
|
scene_node_->setVisible(display_->isEnabled() && displaying_solution_ && animating_);
|
||||||
if (loop_display_property_->getBool() && slider_panel_)
|
if (loop_display_property_->getBool() && slider_panel_)
|
||||||
slider_panel_->pauseButton(false);
|
slider_panel_->pauseButton(false);
|
||||||
}
|
}
|
||||||
@ -244,7 +246,7 @@ void TaskSolutionVisualization::changedShowTrail()
|
|||||||
|
|
||||||
if (!trail_display_property_->getBool())
|
if (!trail_display_property_->getBool())
|
||||||
return;
|
return;
|
||||||
robot_trajectory::RobotTrajectoryPtr t = solution_to_display_;
|
DisplaySolutionPtr t = solution_to_display_;
|
||||||
if (!t)
|
if (!t)
|
||||||
t = displaying_solution_;
|
t = displaying_solution_;
|
||||||
if (!t)
|
if (!t)
|
||||||
@ -264,7 +266,7 @@ void TaskSolutionVisualization::changedShowTrail()
|
|||||||
r->update(PlanningLinkUpdater(t->getWayPointPtr(waypoint_i)));
|
r->update(PlanningLinkUpdater(t->getWayPointPtr(waypoint_i)));
|
||||||
if (enable_robot_color_property_->getBool())
|
if (enable_robot_color_property_->getBool())
|
||||||
setRobotColor(r, robot_color_property_->getColor());
|
setRobotColor(r, robot_color_property_->getColor());
|
||||||
r->setVisible(display_->isEnabled() && (!animating_ || waypoint_i <= current_state_));
|
r->setVisible(waypoint_i <= current_state_);
|
||||||
trail_[i] = r;
|
trail_[i] = r;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -284,24 +286,16 @@ void TaskSolutionVisualization::changedRobotAlpha()
|
|||||||
|
|
||||||
void TaskSolutionVisualization::changedRobotVisualEnabled()
|
void TaskSolutionVisualization::changedRobotVisualEnabled()
|
||||||
{
|
{
|
||||||
if (display_->isEnabled())
|
robot_render_->setVisualVisible(robot_visual_enabled_property_->getBool());
|
||||||
{
|
for (std::size_t i = 0; i < trail_.size(); ++i)
|
||||||
robot_render_->setVisualVisible(robot_visual_enabled_property_->getBool());
|
trail_[i]->setVisualVisible(robot_visual_enabled_property_->getBool());
|
||||||
robot_render_->setVisible(display_->isEnabled() && displaying_solution_ && animating_);
|
|
||||||
for (std::size_t i = 0; i < trail_.size(); ++i)
|
|
||||||
trail_[i]->setVisualVisible(robot_visual_enabled_property_->getBool());
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void TaskSolutionVisualization::changedRobotCollisionEnabled()
|
void TaskSolutionVisualization::changedRobotCollisionEnabled()
|
||||||
{
|
{
|
||||||
if (display_->isEnabled())
|
robot_render_->setCollisionVisible(robot_collision_enabled_property_->getBool());
|
||||||
{
|
for (std::size_t i = 0; i < trail_.size(); ++i)
|
||||||
robot_render_->setCollisionVisible(robot_collision_enabled_property_->getBool());
|
trail_[i]->setCollisionVisible(robot_collision_enabled_property_->getBool());
|
||||||
robot_render_->setVisible(display_->isEnabled() && displaying_solution_ && animating_);
|
|
||||||
for (std::size_t i = 0; i < trail_.size(); ++i)
|
|
||||||
trail_[i]->setCollisionVisible(robot_collision_enabled_property_->getBool());
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void TaskSolutionVisualization::changedStateDisplayTime()
|
void TaskSolutionVisualization::changedStateDisplayTime()
|
||||||
@ -310,28 +304,12 @@ void TaskSolutionVisualization::changedStateDisplayTime()
|
|||||||
|
|
||||||
void TaskSolutionVisualization::onEnable()
|
void TaskSolutionVisualization::onEnable()
|
||||||
{
|
{
|
||||||
changedRobotAlpha(); // set alpha property
|
scene_node_->setVisible(displaying_solution_ && animating_);
|
||||||
changedSceneEnabled(); // show/hide planning scene
|
|
||||||
|
|
||||||
robot_render_->setVisualVisible(robot_visual_enabled_property_->getBool());
|
|
||||||
robot_render_->setCollisionVisible(robot_collision_enabled_property_->getBool());
|
|
||||||
robot_render_->setVisible(displaying_solution_ && animating_);
|
|
||||||
for (std::size_t i = 0; i < trail_.size(); ++i)
|
|
||||||
{
|
|
||||||
trail_[i]->setVisualVisible(robot_visual_enabled_property_->getBool());
|
|
||||||
trail_[i]->setCollisionVisible(robot_collision_enabled_property_->getBool());
|
|
||||||
trail_[i]->setVisible(true);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void TaskSolutionVisualization::onDisable()
|
void TaskSolutionVisualization::onDisable()
|
||||||
{
|
{
|
||||||
if (scene_render_)
|
scene_node_->setVisible(false);
|
||||||
scene_render_->getGeometryNode()->setVisible(false);
|
|
||||||
|
|
||||||
robot_render_->setVisible(false);
|
|
||||||
for (std::size_t i = 0; i < trail_.size(); ++i)
|
|
||||||
trail_[i]->setVisible(false);
|
|
||||||
|
|
||||||
displaying_solution_.reset();
|
displaying_solution_.reset();
|
||||||
animating_ = false;
|
animating_ = false;
|
||||||
@ -381,7 +359,7 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt)
|
|||||||
{
|
{
|
||||||
animating_ = false;
|
animating_ = false;
|
||||||
displaying_solution_.reset();
|
displaying_solution_.reset();
|
||||||
robot_render_->setVisible(false);
|
scene_node_->setVisible(false);
|
||||||
slider_panel_->update(0);
|
slider_panel_->update(0);
|
||||||
drop_displaying_solution_ = false;
|
drop_displaying_solution_ = false;
|
||||||
}
|
}
|
||||||
@ -407,10 +385,7 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt)
|
|||||||
else if (slider_panel_ && slider_panel_->isVisible())
|
else if (slider_panel_ && slider_panel_->isVisible())
|
||||||
{
|
{
|
||||||
if (slider_panel_->getSliderPosition() == (int)displaying_solution_->getWayPointCount() - 1)
|
if (slider_panel_->getSliderPosition() == (int)displaying_solution_->getWayPointCount() - 1)
|
||||||
{ // show the last waypoint if the slider is enabled
|
return; // nothing more to do
|
||||||
robot_render_->update(
|
|
||||||
displaying_solution_->getWayPointPtr(displaying_solution_->getWayPointCount() - 1));
|
|
||||||
}
|
|
||||||
else
|
else
|
||||||
animating_ = true;
|
animating_ = true;
|
||||||
}
|
}
|
||||||
@ -421,10 +396,7 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt)
|
|||||||
{
|
{
|
||||||
current_state_ = -1;
|
current_state_ = -1;
|
||||||
current_state_time_ = std::numeric_limits<float>::infinity();
|
current_state_time_ = std::numeric_limits<float>::infinity();
|
||||||
robot_render_->update(displaying_solution_->getFirstWayPointPtr());
|
scene_node_->setVisible(display_->isEnabled());
|
||||||
robot_render_->setVisible(display_->isEnabled());
|
|
||||||
if (slider_panel_)
|
|
||||||
slider_panel_->setSliderPosition(0);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -433,6 +405,8 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt)
|
|||||||
float tm = getStateDisplayTime();
|
float tm = getStateDisplayTime();
|
||||||
if (tm < 0.0) // if we should use realtime
|
if (tm < 0.0) // if we should use realtime
|
||||||
tm = displaying_solution_->getWayPointDurationFromPrevious(current_state_ + 1);
|
tm = displaying_solution_->getWayPointDurationFromPrevious(current_state_ + 1);
|
||||||
|
int previous_state = current_state_;
|
||||||
|
|
||||||
if (current_state_time_ > tm)
|
if (current_state_time_ > tm)
|
||||||
{
|
{
|
||||||
if (slider_panel_ && slider_panel_->isVisible() && slider_panel_->isPaused())
|
if (slider_panel_ && slider_panel_->isVisible() && slider_panel_->isPaused())
|
||||||
@ -442,20 +416,21 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt)
|
|||||||
int waypoint_count = displaying_solution_->getWayPointCount();
|
int waypoint_count = displaying_solution_->getWayPointCount();
|
||||||
if (current_state_ < waypoint_count)
|
if (current_state_ < waypoint_count)
|
||||||
{
|
{
|
||||||
if (slider_panel_)
|
renderWayPoint(current_state_, previous_state);
|
||||||
slider_panel_->setSliderPosition(current_state_);
|
|
||||||
robot_render_->update(displaying_solution_->getWayPointPtr(current_state_));
|
|
||||||
for (std::size_t i = 0; i < trail_.size(); ++i)
|
for (std::size_t i = 0; i < trail_.size(); ++i)
|
||||||
trail_[i]->setVisible(
|
trail_[i]->setVisible(
|
||||||
std::min(waypoint_count - 1, static_cast<int>(i) * trail_step_size_property_->getInt()) <=
|
std::min(waypoint_count - 1,
|
||||||
current_state_);
|
static_cast<int>(i) * trail_step_size_property_->getInt()) <= current_state_);
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
animating_ = false; // animation finished
|
animating_ = false; // animation finished
|
||||||
robot_render_->setVisible(loop_display_property_->getBool());
|
scene_node_->setVisible(loop_display_property_->getBool() ||
|
||||||
|
(slider_panel_ && slider_panel_->isVisible()));
|
||||||
if (!loop_display_property_->getBool() && slider_panel_)
|
if (!loop_display_property_->getBool() && slider_panel_)
|
||||||
slider_panel_->pauseButton(true);
|
slider_panel_->pauseButton(true);
|
||||||
|
// ensure to render last frame
|
||||||
|
renderWayPoint(displaying_solution_->getWayPointCount() - 1, previous_state);
|
||||||
}
|
}
|
||||||
current_state_time_ = 0.0f;
|
current_state_time_ = 0.0f;
|
||||||
}
|
}
|
||||||
@ -463,6 +438,30 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void TaskSolutionVisualization::renderWayPoint(size_t index, int previous_index)
|
||||||
|
{
|
||||||
|
auto idx_pair = displaying_solution_->indexPair(index);
|
||||||
|
const planning_scene::PlanningSceneConstPtr &scene = displaying_solution_->scene(idx_pair);
|
||||||
|
|
||||||
|
if (previous_index < 0 ||
|
||||||
|
displaying_solution_->indexPair(previous_index).first != idx_pair.first)
|
||||||
|
renderPlanningScene (scene);
|
||||||
|
|
||||||
|
QColor attached_color = attached_body_color_property_->getColor();
|
||||||
|
std_msgs::ColorRGBA color;
|
||||||
|
color.r = attached_color.redF();
|
||||||
|
color.g = attached_color.greenF();
|
||||||
|
color.b = attached_color.blueF();
|
||||||
|
color.a = 1.0f;
|
||||||
|
|
||||||
|
planning_scene::ObjectColorMap color_map;
|
||||||
|
scene->getKnownObjectColors(color_map);
|
||||||
|
robot_render_->update(displaying_solution_->getWayPointPtr(idx_pair), color, color_map);
|
||||||
|
|
||||||
|
if (slider_panel_)
|
||||||
|
slider_panel_->setSliderPosition(index);
|
||||||
|
}
|
||||||
|
|
||||||
void TaskSolutionVisualization::renderPlanningScene(const planning_scene::PlanningSceneConstPtr &scene)
|
void TaskSolutionVisualization::renderPlanningScene(const planning_scene::PlanningSceneConstPtr &scene)
|
||||||
{
|
{
|
||||||
if (!scene_render_ || !scene_enabled_property_->getBool())
|
if (!scene_render_ || !scene_enabled_property_->getBool())
|
||||||
@ -492,25 +491,13 @@ void TaskSolutionVisualization::showTrajectory(const moveit_task_constructor::So
|
|||||||
|
|
||||||
scene_->setPlanningSceneMsg(msg.start_scene);
|
scene_->setPlanningSceneMsg(msg.start_scene);
|
||||||
|
|
||||||
robot_trajectory::RobotTrajectoryPtr t(new robot_trajectory::RobotTrajectory(scene_->getRobotModel(), ""));
|
DisplaySolutionPtr s (new DisplaySolution);
|
||||||
for (std::size_t i = 0; i < msg.sub_trajectory.size(); ++i)
|
s->setFromMessage(scene_, msg);
|
||||||
{
|
|
||||||
if (t->empty())
|
|
||||||
{
|
|
||||||
t->setRobotTrajectoryMsg(scene_->getCurrentState(), msg.sub_trajectory[i].trajectory);
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
robot_trajectory::RobotTrajectory tmp(scene_->getRobotModel(), "");
|
|
||||||
tmp.setRobotTrajectoryMsg(t->getLastWayPoint(), msg.sub_trajectory[i].trajectory);
|
|
||||||
t->append(tmp, 0.0);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!t->empty())
|
if (!s->empty())
|
||||||
{
|
{
|
||||||
boost::mutex::scoped_lock lock(display_solution_mutex_);
|
boost::mutex::scoped_lock lock(display_solution_mutex_);
|
||||||
solution_to_display_.swap(t);
|
solution_to_display_.swap(s);
|
||||||
if (interrupt_display_property_->getBool())
|
if (interrupt_display_property_->getBool())
|
||||||
interruptCurrentDisplay();
|
interruptCurrentDisplay();
|
||||||
}
|
}
|
||||||
@ -556,6 +543,8 @@ void TaskSolutionVisualization::sliderPanelVisibilityChange(bool enable)
|
|||||||
slider_panel_->onEnable();
|
slider_panel_->onEnable();
|
||||||
else
|
else
|
||||||
slider_panel_->onDisable();
|
slider_panel_->onDisable();
|
||||||
|
|
||||||
|
scene_node_->setVisible(enable || animating_);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@ -567,6 +556,8 @@ void TaskSolutionVisualization::changedSceneEnabled()
|
|||||||
|
|
||||||
void TaskSolutionVisualization::renderCurrentScene()
|
void TaskSolutionVisualization::renderCurrentScene()
|
||||||
{
|
{
|
||||||
|
if (scene_render_ && scene_enabled_property_->getBool() && current_state_ >= 0)
|
||||||
|
renderPlanningScene(displaying_solution_->scene(current_state_));
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace moveit_rviz_plugin
|
} // namespace moveit_rviz_plugin
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user