mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Merge branches 'cleanup-planner-interface', 'connect', 'fix-visualization' and 'rviz-createMarker' into master
This commit is contained in:
commit
9740179f7f
@ -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;
|
||||
};
|
||||
|
||||
} }
|
||||
|
||||
@ -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); }
|
||||
|
||||
@ -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,
|
||||
|
||||
@ -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;
|
||||
|
||||
|
||||
@ -63,7 +63,8 @@ class PlannerInterface {
|
||||
PropertyMap properties_;
|
||||
|
||||
public:
|
||||
PlannerInterface() {}
|
||||
PlannerInterface();
|
||||
virtual ~PlannerInterface() {}
|
||||
|
||||
PropertyMap& properties() { return properties_; }
|
||||
const PropertyMap& properties() const { return properties_; }
|
||||
|
||||
@ -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:
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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");
|
||||
|
||||
50
core/src/solvers/planner_interface.cpp
Normal file
50
core/src/solvers/planner_interface.cpp
Normal 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");
|
||||
}
|
||||
|
||||
} } }
|
||||
@ -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
|
||||
}
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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_)
|
||||
{
|
||||
|
||||
@ -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>();
|
||||
|
||||
|
||||
@ -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}};
|
||||
|
||||
@ -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}};
|
||||
|
||||
@ -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()
|
||||
|
||||
@ -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()
|
||||
|
||||
@ -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();
|
||||
|
||||
|
||||
@ -62,8 +62,6 @@ public:
|
||||
TaskSettings* settings_widget;
|
||||
|
||||
rviz::WindowManagerInterface* window_manager_;
|
||||
static QPointer<TaskPanel> global_instance_;
|
||||
static uint global_use_count_;
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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) ||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user