Merge branches 'cleanup-planner-interface', 'connect', 'fix-visualization' and 'rviz-createMarker' into master

This commit is contained in:
v4hn 2018-10-22 17:31:01 +02:00
27 changed files with 152 additions and 136 deletions

View File

@ -106,6 +106,8 @@ private:
void fillSolution(moveit_task_constructor_msgs::Solution &msg, const SolutionBase &s);
/// retrieve or set id of given stage
uint32_t stageId(const moveit::task_constructor::Stage * const s);
/// retrieve solution with given id
const SolutionBase* solutionFromId(uint id) const;
};
} }

View File

@ -47,9 +47,6 @@ class CartesianPath : public PlannerInterface {
public:
CartesianPath();
void setGroup(const std::string &group) { setProperty("group", group); }
void setTimeout(double timeout) { setProperty("timeout", timeout); }
void setStepSize(double step_size) { setProperty("step_size", step_size); }
void setJumpThreshold(double jump_threshold) { setProperty("jump_threshold", jump_threshold); }
void setMinFraction(double min_fraction) { setProperty("min_fraction", min_fraction); }

View File

@ -50,9 +50,6 @@ class JointInterpolationPlanner : public PlannerInterface {
public:
JointInterpolationPlanner();
void setGroup(const std::string &group) { setProperty("group", group); }
void setTimeout(double timeout) { setProperty("timeout", timeout); }
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
bool plan(const planning_scene::PlanningSceneConstPtr& from,

View File

@ -52,9 +52,7 @@ class PipelinePlanner : public PlannerInterface {
public:
PipelinePlanner();
void setGroup(const std::string &group) { setProperty("group", group); }
void setPlannerId(const std::string &planner) { setProperty("planner", planner); }
void setTimeout(double timeout) { setProperty("timeout", timeout); }
void init(const moveit::core::RobotModelConstPtr& robot_model) override;

View File

@ -63,7 +63,8 @@ class PlannerInterface {
PropertyMap properties_;
public:
PlannerInterface() {}
PlannerInterface();
virtual ~PlannerInterface() {}
PropertyMap& properties() { return properties_; }
const PropertyMap& properties() const { return properties_; }

View File

@ -62,6 +62,8 @@ protected:
bool compatible(const InterfaceState &from_state, const InterfaceState &to_state) const override;
public:
enum MergeMode { SEQUENTIAL = 0, WAYPOINTS = 1 };
typedef std::vector<std::pair<std::string, solvers::PlannerInterfacePtr>> GroupPlannerVector;
Connect(const std::string& name, const GroupPlannerVector& planners);
@ -75,9 +77,10 @@ public:
protected:
SolutionSequencePtr makeSequential(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes, const InterfaceState& from, const InterfaceState& to);
const std::vector<planning_scene::PlanningSceneConstPtr>& intermediate_scenes,
const InterfaceState& from, const InterfaceState& to);
SubTrajectoryPtr merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes,
const std::vector<planning_scene::PlanningSceneConstPtr>& intermediate_scenes,
const moveit::core::RobotState& state);
protected:

View File

@ -99,6 +99,7 @@ public:
/// create an InterfaceState from a planning scene
InterfaceState(const planning_scene::PlanningScenePtr& ps);
InterfaceState(const planning_scene::PlanningSceneConstPtr& ps);
/// copy an existing InterfaceState, but not including incoming/outgoing trajectories
InterfaceState(const InterfaceState& other);

View File

@ -26,6 +26,7 @@ add_library(${PROJECT_NAME}
storage.cpp
task.cpp
solvers/planner_interface.cpp
solvers/cartesian_path.cpp
solvers/pipeline_planner.cpp
solvers/joint_interpolation.cpp

View File

@ -164,14 +164,20 @@ void Introspection::publishAllSolutions(bool wait)
};
}
const SolutionBase* Introspection::solutionFromId(uint id) const {
auto it = impl->id_solution_bimap_.left.find(id);
if (it == impl->id_solution_bimap_.left.end())
return nullptr;
return it->second;
}
bool Introspection::getSolution(moveit_task_constructor_msgs::GetSolution::Request &req,
moveit_task_constructor_msgs::GetSolution::Response &res)
{
auto it = impl->id_solution_bimap_.left.find(req.solution_id);
if (it == impl->id_solution_bimap_.left.end())
return false;
const SolutionBase* solution = solutionFromId(req.solution_id);
if (!solution) return false;
fillSolution(res.solution, *it->second);
fillSolution(res.solution, *solution);
return true;
}

View File

@ -48,8 +48,6 @@ CartesianPath::CartesianPath()
p.declare<double>("step_size", 0.01, "step size between consecutive waypoints");
p.declare<double>("jump_threshold", 1.5, "acceptable fraction of mean joint motion per step");
p.declare<double>("min_fraction", 1.0, "fraction of motion required for success");
p.declare<double>("max_velocity_scaling_factor", 1.0, "scale down max velocity by this factor");
p.declare<double>("max_acceleration_scaling_factor", 1.0, "scale down max acceleration by this factor");
}
void CartesianPath::init(const core::RobotModelConstPtr &robot_model)

View File

@ -46,8 +46,6 @@ JointInterpolationPlanner::JointInterpolationPlanner()
{
auto& p = properties();
p.declare<double>("max_step", 0.1, "max joint step");
p.declare<double>("max_velocity_scaling_factor", 1.0, "scale down max velocity by this factor");
p.declare<double>("max_acceleration_scaling_factor", 1.0, "scale down max acceleration by this factor");
}
void JointInterpolationPlanner::init(const core::RobotModelConstPtr &robot_model)

View File

@ -52,8 +52,6 @@ PipelinePlanner::PipelinePlanner()
p.declare<std::string>("planner", "", "planner id");
p.declare<uint>("num_planning_attempts", 1u, "number of planning attempts");
p.declare<double>("max_velocity_scaling_factor", 1.0, "scale down max velocity by this factor");
p.declare<double>("max_acceleration_scaling_factor", 1.0, "scale down max acceleration by this factor");
p.declare<moveit_msgs::WorkspaceParameters>("workspace_parameters", moveit_msgs::WorkspaceParameters(), "allowed workspace of mobile base?");
p.declare<double>("goal_joint_tolerance", 1e-4, "tolerance for reaching joint goals");

View File

@ -0,0 +1,50 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Bielefeld University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Bielefeld University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Authors: Robert Haschke
Desc: Planner Interface: implementation details shared across different planners
*/
#include <moveit/task_constructor/solvers/planner_interface.h>
namespace moveit { namespace task_constructor { namespace solvers {
PlannerInterface::PlannerInterface()
{
auto& p = properties();
p.declare<double>("max_velocity_scaling_factor", 1.0, "scale down max velocity by this factor");
p.declare<double>("max_acceleration_scaling_factor", 1.0, "scale down max acceleration by this factor");
}
} } }

View File

@ -695,19 +695,28 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
const planning_scene::PlanningSceneConstPtr& from = from_state.scene();
const planning_scene::PlanningSceneConstPtr& to = to_state.scene();
if (from->getWorld()->size() != to->getWorld()->size())
return false; // different number of collision objects
if (from->getWorld()->size() != to->getWorld()->size()) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different number of collision objects");
return false;
}
// both scenes should have the same set of collision objects, at the same location
for (const auto& from_object_pair : *from->getWorld()) {
const collision_detection::World::ObjectPtr& from_object = from_object_pair.second;
const collision_detection::World::ObjectConstPtr& to_object = to->getWorld()->getObject(from_object_pair.first);
if (!to_object) return false; // object missing
if (from_object->shape_poses_.size() != to_object->shape_poses_.size()) return false; // shapes not matching
if (!to_object) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": object missing: " << from_object_pair.first);
return false;
}
if (from_object->shape_poses_.size() != to_object->shape_poses_.size()) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object shapes: " << from_object_pair.first);
return false; // shapes not matching
}
for (auto from_it = from_object->shape_poses_.cbegin(), from_end = from_object->shape_poses_.cend(),
to_it = to_object->shape_poses_.cbegin(); from_it != from_end; ++from_it, ++to_it)
if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4)){
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object pose: " << from_object_pair.first);
return false; // transforms do not match
}
}

View File

@ -48,7 +48,7 @@ Connect::Connect(const std::string& name, const GroupPlannerVector& planners)
{
setTimeout(1.0);
auto& p = properties();
p.declare<std::string>("group", "name of planning group");
p.declare<MergeMode>("merge_mode", WAYPOINTS, "merge mode");
p.declare<moveit_msgs::Constraints>("path_constraints", moveit_msgs::Constraints(),
"constraints to maintain during trajectory");
}
@ -138,13 +138,14 @@ bool Connect::compatible(const InterfaceState& from_state, const InterfaceState&
void Connect::compute(const InterfaceState &from, const InterfaceState &to) {
const auto& props = properties();
double timeout = this->timeout();
MergeMode mode = props.get<MergeMode>("merge_mode");
const auto& path_constraints = props.get<moveit_msgs::Constraints>("path_constraints");
const moveit::core::RobotState& final_goal_state = to.scene()->getCurrentState();
std::vector<robot_trajectory::RobotTrajectoryConstPtr> sub_trajectories;
planning_scene::PlanningScenePtr start = from.scene()->diff();
const moveit::core::RobotState& goal_state = to.scene()->getCurrentState();
std::vector<planning_scene::PlanningScenePtr> intermediate_scenes;
std::vector<planning_scene::PlanningSceneConstPtr> intermediate_scenes;
planning_scene::PlanningSceneConstPtr start = from.scene();
intermediate_scenes.push_back(start);
bool success = false;
@ -152,10 +153,12 @@ void Connect::compute(const InterfaceState &from, const InterfaceState &to) {
for (const GroupPlannerVector::value_type& pair : planner_) {
// set intermediate goal state
planning_scene::PlanningScenePtr end = start->diff();
const moveit::core::JointModelGroup *jmg = final_goal_state.getJointModelGroup(pair.first);
final_goal_state.copyJointGroupPositions(jmg, positions);
robot_state::RobotState& goal_state = end->getCurrentStateNonConst();
goal_state.setJointGroupPositions(jmg, positions);
goal_state.update();
intermediate_scenes.push_back(end);
const moveit::core::JointModelGroup *jmg = goal_state.getJointModelGroup(pair.first);
goal_state.copyJointGroupPositions(jmg, positions);
end->getCurrentStateNonConst().setJointGroupPositions(jmg, positions);
robot_trajectory::RobotTrajectoryPtr trajectory;
success = pair.second->plan(start, end, jmg, timeout, trajectory, path_constraints);
@ -169,29 +172,27 @@ void Connect::compute(const InterfaceState &from, const InterfaceState &to) {
}
SolutionBasePtr solution;
if (!success) { // error during sequential planning
solution = makeSequential(sub_trajectories, intermediate_scenes, from, to);
solution->markAsFailure();
} else {
if (success && mode != SEQUENTIAL) // try to merge
solution = merge(sub_trajectories, intermediate_scenes, from.scene()->getCurrentState());
if (!solution) // merging failed, store sequentially
solution = makeSequential(sub_trajectories, intermediate_scenes, from, to);
}
if (!solution) // success == false or merging failed: store sequentially
solution = makeSequential(sub_trajectories, intermediate_scenes, from, to);
if (!success) // error during sequential planning
solution->markAsFailure();
connect(from, to, solution);
}
SolutionSequencePtr Connect::makeSequential(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes,
const std::vector<planning_scene::PlanningSceneConstPtr>& intermediate_scenes,
const InterfaceState &from, const InterfaceState &to)
{
assert(sub_trajectories.size() + 1 == intermediate_scenes.size());
auto scene_it = intermediate_scenes.begin();
planning_scene::PlanningScenePtr start = *scene_it;
planning_scene::PlanningSceneConstPtr start = *scene_it;
const InterfaceState* state = &from;
SolutionSequence::container_type sub_solutions;
for (const auto &sub : sub_trajectories) {
planning_scene::PlanningScenePtr end = *++scene_it;
planning_scene::PlanningSceneConstPtr end = *++scene_it;
auto inserted = subsolutions_.insert(subsolutions_.end(), SubTrajectory(sub));
inserted->setCreator(pimpl_);
@ -214,7 +215,7 @@ SolutionSequencePtr Connect::makeSequential(const std::vector<robot_trajectory::
}
SubTrajectoryPtr Connect::merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes,
const std::vector<planning_scene::PlanningSceneConstPtr>& intermediate_scenes,
const moveit::core::RobotState& state)
{
// no need to merge if there is only a single sub trajectory

View File

@ -99,7 +99,6 @@ void SimpleGraspBase::setup(std::unique_ptr<Stage>&& generator, bool forward)
}
{
auto pipeline = std::make_shared<solvers::PipelinePlanner>();
pipeline->setTimeout(8.0);
pipeline->setPlannerId("RRTConnectkConfigDefault");
auto move = new MoveTo(forward ? "close gripper" : "open gripper", pipeline);

View File

@ -58,6 +58,13 @@ InterfaceState::InterfaceState(const planning_scene::PlanningScenePtr& ps)
{
}
InterfaceState::InterfaceState(const planning_scene::PlanningSceneConstPtr& ps)
: scene_(ps)
{
if (scene_->getCurrentState().dirty())
ROS_ERROR_NAMED("InterfaceState", "Dirty PlanningScene! Please only forward clean ones into InterfaceState.");
}
InterfaceState::InterfaceState(const InterfaceState &other)
: scene_(other.scene_), properties_(other.properties_), priority_(other.priority_)
{

View File

@ -44,7 +44,6 @@ TEST(PA10, pick) {
t.setProperty("gripper", std::string("left_hand"));
auto pipeline = std::make_shared<solvers::PipelinePlanner>();
pipeline->setTimeout(8.0);
pipeline->setPlannerId("RRTConnectkConfigDefault");
auto cartesian = std::make_shared<solvers::CartesianPath>();

View File

@ -40,7 +40,6 @@ TEST(PR2, pick) {
// planner used for connect
auto pipeline = std::make_shared<solvers::PipelinePlanner>();
pipeline->setTimeout(8.0);
pipeline->setPlannerId("RRTConnectkConfigDefault");
// connect to pick
stages::Connect::GroupPlannerVector planners = {{"left_arm", pipeline}, {"left_gripper", pipeline}};

View File

@ -42,7 +42,6 @@ TEST(UR5, pick) {
// planner used for connect
auto pipeline = std::make_shared<solvers::PipelinePlanner>();
pipeline->setTimeout(8.0);
pipeline->setPlannerId("RRTConnectkConfigDefault");
// connect to pick
stages::Connect::GroupPlannerVector planners = {{"arm", pipeline}, {"gripper", pipeline}};

View File

@ -94,21 +94,16 @@ TaskDisplay::TaskDisplay() : Display()
TaskDisplay::~TaskDisplay()
{
if (context_) TaskPanel::decUseCount();
if (context_) TaskPanel::decDisplayCount();
}
void TaskDisplay::onInitialize()
{
Display::onInitialize();
trajectory_visual_->onInitialize(scene_node_, context_);
#if QT_VERSION >= QT_VERSION_CHECK(5, 0, 0)
// displays are loaded before panels, hence wait a little bit for the panel to be loaded
QTimer::singleShot(1000, [this](){ TaskPanel::incUseCount(context_->getWindowManager()); });
#else
// Qt4 doesn't support lambdas: directly instantiate panel instance
TaskPanel::incUseCount(context_->getWindowManager());
#endif
// create a new TaskPanel by default
// by post-poning this to main loop, we can ensure that rviz has loaded everything before
mainloop_jobs_.addJob([this]() { TaskPanel::incDisplayCount(context_->getWindowManager()); });
}
void TaskDisplay::loadRobotModel()

View File

@ -51,6 +51,7 @@
#include <rviz/display_group.h>
#include <rviz/visualization_manager.h>
#include <rviz/window_manager_interface.h>
#include <rviz/visualization_frame.h>
#include <rviz/panel_dock_widget.h>
#include <ros/console.h>
#include <QPointer>
@ -75,6 +76,12 @@ rviz::PanelDockWidget* getStageDockWidget(rviz::WindowManagerInterface* mgr) {
}
// TaskPanel singleton
static QPointer<TaskPanel> singleton_;
// count active TaskDisplays
static uint display_count_ = 0;
TaskPanel::TaskPanel(QWidget* parent)
: rviz::Panel(parent), d_ptr(new TaskPanelPrivate(this))
{
@ -84,20 +91,15 @@ TaskPanel::TaskPanel(QWidget* parent)
d->settings_widget = new TaskSettings(this);
layout()->addWidget(d->tasks_widget);
layout()->addWidget(d->settings_widget);
connect(d->tasks_widget, SIGNAL(configChanged()), this, SIGNAL(configChanged()));
connect(d->button_show_stage_dock_widget, SIGNAL(clicked()), this, SLOT(showStageDockWidget()));
connect(d->button_show_settings, SIGNAL(toggled(bool)), d->settings_widget, SLOT(setVisible(bool)));
d->settings_widget->setVisible(d->button_show_settings->isChecked());
// if still undefined, this becomes the global instance
if (TaskPanelPrivate::global_instance_.isNull()) {
TaskPanelPrivate::global_instance_ = this;
// If an explicitly created instance is explicitly destroyed, we don't notice.
// If there there were displays "using" it, global_use_count_ is still greater zero.
if (TaskPanelPrivate::global_use_count_ > 0); // In this case, don't increment use count
else
++TaskPanelPrivate::global_use_count_; // otherwise increment use count for explicitly created instance
}
if (singleton_.isNull())
singleton_ = this;
}
TaskPanel::~TaskPanel()
@ -105,26 +107,23 @@ TaskPanel::~TaskPanel()
delete d_ptr;
}
QPointer<TaskPanel> TaskPanelPrivate::global_instance_;
uint TaskPanelPrivate::global_use_count_ = 0;
void TaskPanel::incUseCount(rviz::WindowManagerInterface* window_manager)
void TaskPanel::incDisplayCount(rviz::WindowManagerInterface* window_manager)
{
++TaskPanelPrivate::global_use_count_;
if (TaskPanelPrivate::global_instance_ || !window_manager)
++display_count_;
rviz::VisualizationFrame* vis_frame = dynamic_cast<rviz::VisualizationFrame*>(window_manager);
if (singleton_ || !vis_frame)
return; // already define, nothing to do
--TaskPanelPrivate::global_use_count_; // counteract ++ in constructor
TaskPanelPrivate::global_instance_ = new TaskPanel(window_manager->getParentWindow());
TaskPanelPrivate::global_instance_->d_ptr->window_manager_ = window_manager;
window_manager->addPane("Motion Planning Tasks", TaskPanelPrivate::global_instance_);
window_manager->addPane("Motion Planning Tasks", new TaskPanel());
singleton_->initialize(vis_frame->getManager());
}
void TaskPanel::decUseCount()
void TaskPanel::decDisplayCount()
{
Q_ASSERT(TaskPanelPrivate::global_use_count_ > 0);
if (--TaskPanelPrivate::global_use_count_ == 0 && TaskPanelPrivate::global_instance_)
TaskPanelPrivate::global_instance_->deleteLater();
Q_ASSERT(display_count_ > 0);
if (--display_count_ == 0 && singleton_)
singleton_->deleteLater();
}
TaskPanelPrivate::TaskPanelPrivate(TaskPanel *q_ptr)
@ -247,6 +246,13 @@ TaskView::TaskView(QWidget *parent)
this, SLOT(onCurrentStageChanged(QModelIndex,QModelIndex)));
onCurrentStageChanged(d->tasks_view->currentIndex(), QModelIndex());
// propagate infos about config changes
connect(d_ptr->tasks_property_splitter, SIGNAL(splitterMoved(int,int)),this, SIGNAL(configChanged()));
connect(d_ptr->tasks_solutions_splitter, SIGNAL(splitterMoved(int,int)), this, SIGNAL(configChanged()));
connect(d_ptr->tasks_view->header(), SIGNAL(sectionResized(int,int,int)), this, SIGNAL(configChanged()));
connect(d_ptr->solutions_view->header(), SIGNAL(sectionResized(int,int,int)), this, SIGNAL(configChanged()));
connect(d_ptr->solutions_view->header(), SIGNAL(sortIndicatorChanged(int,Qt::SortOrder)), this, SIGNAL(configChanged()));
}
TaskView::~TaskView()

View File

@ -70,13 +70,13 @@ public:
TaskPanel(QWidget* parent = 0);
~TaskPanel();
/** Increment/decrement use count for global task panel instance.
/** Increment/decrement use count of singleton TaskPanel instance.
*
* If not yet done, an instance is created. If use count drops to zero,
* the global instance is destroyed.
*/
static void incUseCount(rviz::WindowManagerInterface *window_manager);
static void decUseCount();
static void incDisplayCount(rviz::WindowManagerInterface *window_manager);
static void decDisplayCount();
void onInitialize() override;
void load(const rviz::Config& config) override;
@ -101,6 +101,9 @@ public:
void save(rviz::Config config);
void load(const rviz::Config& config);
Q_SIGNALS:
void configChanged();
public Q_SLOTS:
void addTask();

View File

@ -62,8 +62,6 @@ public:
TaskSettings* settings_widget;
rviz::WindowManagerInterface* window_manager_;
static QPointer<TaskPanel> global_instance_;
static uint global_use_count_;
};

View File

@ -99,7 +99,7 @@ public:
float getWayPointDurationFromPrevious(const IndexPair& idx_pair) const;
float getWayPointDurationFromPrevious(size_t index) const {
if (index >= steps_) return 0.0;
if (index >= steps_) return 0.1; // display time of last waypoint before switching to final scene
return getWayPointDurationFromPrevious(indexPair(index));
}
const moveit::core::RobotStatePtr& getWayPointPtr(const IndexPair& idx_pair) const;

View File

@ -2,61 +2,16 @@
#include <moveit/planning_scene/planning_scene.h>
#include <rviz/default_plugin/markers/marker_base.h>
#include "rviz/default_plugin/markers/arrow_marker.h"
#include "rviz/default_plugin/markers/line_list_marker.h"
#include "rviz/default_plugin/markers/line_strip_marker.h"
#include "rviz/default_plugin/markers/mesh_resource_marker.h"
#include "rviz/default_plugin/markers/points_marker.h"
#include "rviz/default_plugin/markers/shape_marker.h"
#include "rviz/default_plugin/markers/text_view_facing_marker.h"
#include "rviz/default_plugin/markers/triangle_list_marker.h"
#include <rviz/default_plugin/marker_utils.h>
#include <rviz/display_context.h>
#include <rviz/frame_manager.h>
#include <OgreSceneManager.h>
#include <tf2_msgs/TF2Error.h>
#include <ros/console.h>
#include <eigen_conversions/eigen_msg.h>
#include <OgreSceneManager.h>
namespace moveit_rviz_plugin {
rviz::MarkerBase* createMarker(int marker_type, rviz::DisplayContext* context, Ogre::SceneNode* node)
{
switch (marker_type) {
case visualization_msgs::Marker::CUBE:
case visualization_msgs::Marker::CYLINDER:
case visualization_msgs::Marker::SPHERE:
return new rviz::ShapeMarker(nullptr, context, node);
case visualization_msgs::Marker::ARROW:
return new rviz::ArrowMarker(nullptr, context, node);
case visualization_msgs::Marker::LINE_STRIP:
return new rviz::LineStripMarker(nullptr, context, node);
case visualization_msgs::Marker::LINE_LIST:
return new rviz::LineListMarker(nullptr, context, node);
case visualization_msgs::Marker::SPHERE_LIST:
case visualization_msgs::Marker::CUBE_LIST:
case visualization_msgs::Marker::POINTS:
return new rviz::PointsMarker(nullptr, context, node);
case visualization_msgs::Marker::TEXT_VIEW_FACING:
return new rviz::TextViewFacingMarker(nullptr, context, node);
case visualization_msgs::Marker::MESH_RESOURCE:
return new rviz::MeshResourceMarker(nullptr, context, node);
case visualization_msgs::Marker::TRIANGLE_LIST:
return new rviz::TriangleListMarker(nullptr, context, node);
default:
ROS_ERROR("Unknown marker type: %d", marker_type);
return nullptr;
}
}
// create MarkerData with nil marker_ pointer, just with a copy of message
MarkerVisualization::MarkerData::MarkerData(const visualization_msgs::Marker& marker)
{
@ -151,7 +106,7 @@ bool MarkerVisualization::createMarkers(rviz::DisplayContext *context, Ogre::Sce
frame_it->second = node->createChildSceneNode();
node = frame_it->second;
data.marker_.reset(createMarker(data.msg_->type, context, node));
data.marker_.reset(rviz::createMarker(data.msg_->type, nullptr, context, node));
if (!data.marker_) continue; // failed to create marker
// setMessage() initializes the marker, placing it at the message-specified frame

View File

@ -419,7 +419,6 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt)
} 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;
@ -429,7 +428,6 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt)
current_state_ = 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
while (current_state_ < max_state_index &&
(tm = displaying_solution_->getWayPointDurationFromPrevious(current_state_ + 1)) < current_state_time_) {
@ -439,11 +437,9 @@ void TaskSolutionVisualization::update(float wall_dt, float ros_dt)
} else if (current_state_time_ > tm) { // fixed display time per 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) ||