From bff6cc569c34ba9dfd32dcd78c29d79b29c2794c Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 16 Feb 2018 23:36:11 +0100 Subject: [PATCH 01/13] pick: reusable stage for picking up an object --- core/demo/plan_pick_pa10.cpp | 2 +- core/demo/plan_pick_trixi.cpp | 96 ++++---------- core/demo/plan_pick_ur5.cpp | 2 +- .../stages/generate_grasp_pose.h | 3 +- .../stages/modify_planning_scene.h | 2 +- .../moveit/task_constructor/stages/pick.h | 79 ++++++++++++ .../task_constructor/stages/simple_grasp.h | 101 +++++++++++++++ core/src/stages/CMakeLists.txt | 6 + core/src/stages/generate_grasp_pose.cpp | 16 ++- core/src/stages/pick.cpp | 118 +++++++++++++++++ core/src/stages/simple_grasp.cpp | 120 ++++++++++++++++++ 11 files changed, 461 insertions(+), 84 deletions(-) create mode 100644 core/include/moveit/task_constructor/stages/pick.h create mode 100644 core/include/moveit/task_constructor/stages/simple_grasp.h create mode 100644 core/src/stages/pick.cpp create mode 100644 core/src/stages/simple_grasp.cpp diff --git a/core/demo/plan_pick_pa10.cpp b/core/demo/plan_pick_pa10.cpp index c813f8fb..00948ca2 100644 --- a/core/demo/plan_pick_pa10.cpp +++ b/core/demo/plan_pick_pa10.cpp @@ -105,7 +105,7 @@ int main(int argc, char** argv){ { auto gengrasp = std::make_unique("generate grasp pose"); gengrasp->properties().configureInitFrom(Stage::PARENT); - gengrasp->setGripperGraspPose("open"); + gengrasp->setNamedPose("open"); gengrasp->setObject("object"); gengrasp->setToolToGraspTF(Eigen::Translation3d(0,0,.05)* Eigen::AngleAxisd(-0.5*M_PI, Eigen::Vector3d::UnitY()), diff --git a/core/demo/plan_pick_trixi.cpp b/core/demo/plan_pick_trixi.cpp index e0cf26ad..340b4b42 100644 --- a/core/demo/plan_pick_trixi.cpp +++ b/core/demo/plan_pick_trixi.cpp @@ -1,11 +1,8 @@ #include #include -#include -#include -#include -#include -#include +#include +#include #include #include @@ -44,78 +41,31 @@ int main(int argc, char** argv){ Task t; Stage* initial_stage = nullptr; + auto initial = std::make_unique("current state"); + initial_stage = initial.get(); + t.add(std::move(initial)); - { - auto initial = std::make_unique("current state"); - initial_stage = initial.get(); - t.add(std::move(initial)); - } + auto grasp_generator = std::make_unique(); + grasp_generator->setToolToGraspTF(Eigen::Affine3d::Identity(), "l_gripper_tool_frame"); + grasp_generator->setAngleDelta(.2); + grasp_generator->setPreGraspPose("open"); + grasp_generator->setGraspPose("closed"); + grasp_generator->setMonitoredStage(initial_stage); - { - auto move= std::make_unique("open gripper"); - move->setEndEffector("left_gripper"); - move->setTo("open"); - t.add(std::move(move)); - } + auto pick = std::make_unique(std::move(grasp_generator)); + pick->setProperty("eef", std::string("left_gripper")); + pick->setProperty("object", std::string("object")); + geometry_msgs::TwistStamped approach; + approach.header.frame_id = "l_gripper_tool_frame"; + approach.twist.linear.x = 1.0; + pick->setApproachMotion(approach, 0.03, 0.1); - { - auto move= std::make_unique("move to pre-grasp"); - move->setGroup("left_arm"); - move->setPlannerId("RRTConnectkConfigDefault"); - move->setTimeout(8.0); - t.add(std::move(move)); - } + geometry_msgs::TwistStamped lift; + lift.header.frame_id = "base_link"; + lift.twist.linear.z = 1.0; + pick->setLiftMotion(lift, 0.03, 0.05); - { - auto move= std::make_unique("proceed to grasp pose"); - move->addSolutionCallback(std::bind(&Introspection::publishSolution, &t.introspection(), std::placeholders::_1)); - move->setGroup("left_arm"); - move->setLink("l_gripper_tool_frame"); - move->setMinMaxDistance(.03, 0.1); - move->setCartesianStepSize(0.02); - - geometry_msgs::PointStamped target; - target.header.frame_id= "object"; - move->towards(target); - t.add(std::move(move)); - } - - { - auto gengrasp= std::make_unique("generate grasp pose"); - gengrasp->setEndEffector("left_gripper"); - gengrasp->setGripperGraspPose("open"); - gengrasp->setObject("object"); - gengrasp->setToolToGraspTF(Eigen::Affine3d::Identity(), "l_gripper_tool_frame"); - gengrasp->setAngleDelta(.2); - gengrasp->setMonitoredStage(initial_stage); - - auto ik = std::make_unique("compute ik", std::move(gengrasp)); - ik->setEndEffector("left_gripper"); - t.add(std::move(ik)); - } - - { - auto move= std::make_unique("grasp"); - move->setEndEffector("left_gripper"); - move->setAttachLink("l_gripper_tool_frame"); - move->setTo("closed"); - move->graspObject("object"); - t.add(std::move(move)); - } - - { - auto move= std::make_unique("lift object"); - move->setGroup("left_arm"); - move->setLink("l_gripper_tool_frame"); - move->setMinMaxDistance(0.03, 0.05); - move->setCartesianStepSize(0.01); - - geometry_msgs::Vector3Stamped direction; - direction.header.frame_id= "base_link"; - direction.vector.z= 1.0; - move->along(direction); - t.add(std::move(move)); - } + t.add(std::move(pick)); try { t.plan(); diff --git a/core/demo/plan_pick_ur5.cpp b/core/demo/plan_pick_ur5.cpp index 92e2e1f6..c18f35a9 100644 --- a/core/demo/plan_pick_ur5.cpp +++ b/core/demo/plan_pick_ur5.cpp @@ -84,7 +84,7 @@ int main(int argc, char** argv){ { auto gengrasp = std::make_unique("generate grasp pose"); gengrasp->properties().configureInitFrom(Stage::PARENT); - gengrasp->setGripperGraspPose("open"); + gengrasp->setNamedPose("open"); gengrasp->setObject("object"); gengrasp->setToolToGraspTF(Eigen::Translation3d(.03,0,0), "s_model_tool0"); gengrasp->setAngleDelta(-.2); diff --git a/core/include/moveit/task_constructor/stages/generate_grasp_pose.h b/core/include/moveit/task_constructor/stages/generate_grasp_pose.h index 9f4b4885..d6559312 100644 --- a/core/include/moveit/task_constructor/stages/generate_grasp_pose.h +++ b/core/include/moveit/task_constructor/stages/generate_grasp_pose.h @@ -39,6 +39,7 @@ #pragma once #include +#include #include namespace moveit { namespace task_constructor { namespace stages { @@ -52,7 +53,7 @@ public: bool compute() override; void setEndEffector(const std::string &eef); - void setGripperGraspPose(const std::string &pose_name); + void setNamedPose(const std::string &pose_name); void setObject(const std::string &object); void setToolToGraspTF(const geometry_msgs::TransformStamped &transform); diff --git a/core/include/moveit/task_constructor/stages/modify_planning_scene.h b/core/include/moveit/task_constructor/stages/modify_planning_scene.h index a30e1c7c..b92aaebf 100644 --- a/core/include/moveit/task_constructor/stages/modify_planning_scene.h +++ b/core/include/moveit/task_constructor/stages/modify_planning_scene.h @@ -60,7 +60,7 @@ namespace moveit { namespace task_constructor { namespace stages { class ModifyPlanningScene : public PropagatingEitherWay { public: typedef std::vector Names; - typedef std::function ApplyCallback; + typedef std::function ApplyCallback; ModifyPlanningScene(const std::string& name = "modify planning scene"); bool computeForward(const InterfaceState& from) override; diff --git a/core/include/moveit/task_constructor/stages/pick.h b/core/include/moveit/task_constructor/stages/pick.h new file mode 100644 index 00000000..c1ebd54e --- /dev/null +++ b/core/include/moveit/task_constructor/stages/pick.h @@ -0,0 +1,79 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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 */ + +#pragma once + +#include +#include + +namespace moveit { namespace task_constructor { namespace stages { + +/** Pick wraps a complete pipeline to pick up an object with a given end effector. + * + * Picking consist of the following sub stages: + * - reaching to the object + "pre-grasp" end effector posture + * - linearly approaching the object along an approach direction/twist + * - "grasp" end effector posture + * - attach object + * - lift along along a given direction/twist + * + * The end effector postures corresponding to pre-grasp and grasp as well as + * the end effector's Cartesian pose needs to be provided by an external grasp stage. + */ +class Pick : public SerialContainer { + Stage* grasp_stage_ = nullptr; + Stage* approach_stage_ = nullptr; + Stage* lift_stage_ = nullptr; + +public: + Pick(Stage::pointer &&grasp_stage, const std::string& name = "pick"); + + void init(const moveit::core::RobotModelConstPtr& robot_model) override; + + void setEndEffector(const std::string& eef) { + properties().set("eef", eef); + } + void setObject(const std::string& object) { + properties().set("object", object); + } + + void setApproachMotion(const geometry_msgs::TwistStamped& motion, + double min_distance, double max_distance); + void setLiftMotion(const geometry_msgs::TwistStamped& motion, + double min_distance, double max_distance); +}; + +} } } diff --git a/core/include/moveit/task_constructor/stages/simple_grasp.h b/core/include/moveit/task_constructor/stages/simple_grasp.h new file mode 100644 index 00000000..160ff4e1 --- /dev/null +++ b/core/include/moveit/task_constructor/stages/simple_grasp.h @@ -0,0 +1,101 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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 */ + +#pragma once + +#include +#include +#include +#include + +namespace moveit { namespace core { MOVEIT_CLASS_FORWARD(RobotModel) } } +namespace moveit { namespace task_constructor { namespace stages { + +class GenerateGraspPose; + +/** Simple Grasp Stage + * + * Given a named pre-grasp and grasp posture the stage generates + * fully-qualified pre-grasp and grasp robot states, connected + * by a grasping trajectory of the end-effector. + */ +class SimpleGrasp : public SerialContainer { + moveit::core::RobotModelConstPtr model_; + GenerateGraspPose* grasp_generator_ = nullptr; + +public: + SimpleGrasp(const std::string& name = "grasp"); + + void init(const moveit::core::RobotModelConstPtr& robot_model) override; + void setMonitoredStage(Stage* monitored); + + void setEndEffector(const std::string& eef) { + properties().set("eef", eef); + } + void setObject(const std::string& object) { + properties().set("object", object); + } + + void setPreGraspPose(const std::string& pregrasp) { + properties().set("pregrasp", pregrasp); + } + void setGraspPose(const std::string& grasp) { + properties().set("grasp", grasp); + } + + void setToolToGraspTF(const geometry_msgs::TransformStamped &transform) { + properties().set("tool_to_grasp_tf", transform); + } + void setToolToGraspTF(const Eigen::Affine3d& transform, const std::string& link = ""); + template + void setToolToGraspTF(const T& t, const std::string& link = "") { + Eigen::Affine3d transform; transform = t; + setToolToGraspTF(transform, link); + } + + void setAngleDelta(double angle_delta) { + properties().set("angle_delta", angle_delta); + } + + void setMaxIKSolutions(uint32_t max_ik_solutions) { + properties().set("max_ik_solutions", max_ik_solutions); + } + void setTimeout(double timeout) { + properties().set("timeout", timeout); + } +}; + +} } } diff --git a/core/src/stages/CMakeLists.txt b/core/src/stages/CMakeLists.txt index 02915ad2..03959900 100644 --- a/core/src/stages/CMakeLists.txt +++ b/core/src/stages/CMakeLists.txt @@ -11,6 +11,9 @@ add_library(${PROJECT_NAME}_stages ${PROJECT_INCLUDE}/stages/move_to.h ${PROJECT_INCLUDE}/stages/move_relative.h + ${PROJECT_INCLUDE}/stages/simple_grasp.h + ${PROJECT_INCLUDE}/stages/pick.h + ${PROJECT_INCLUDE}/stages/cartesian_position_motion.h ${PROJECT_INCLUDE}/stages/gripper.h ${PROJECT_INCLUDE}/stages/move.h @@ -27,6 +30,9 @@ add_library(${PROJECT_NAME}_stages move_to.cpp move_relative.cpp + simple_grasp.cpp + pick.cpp + cartesian_position_motion.cpp gripper.cpp move.cpp diff --git a/core/src/stages/generate_grasp_pose.cpp b/core/src/stages/generate_grasp_pose.cpp index 6bd164a3..ddf17d38 100644 --- a/core/src/stages/generate_grasp_pose.cpp +++ b/core/src/stages/generate_grasp_pose.cpp @@ -50,8 +50,8 @@ GenerateGraspPose::GenerateGraspPose(const std::string& name) : MonitoringGenerator(name) { auto& p = properties(); - p.declare("eef", "name of end-effector group"); - p.declare("eef_named_pose"); + p.declare("eef", "name of end-effector"); + p.declare("pregrasp", "name of end-effector's pregrasp pose"); p.declare("object"); p.declare("tool_to_grasp_tf", geometry_msgs::TransformStamped(), "transform from robot tool frame to grasp frame"); p.declare("angle_delta", 0.1, "angular steps (rad)"); @@ -61,8 +61,8 @@ void GenerateGraspPose::setEndEffector(const std::string &eef){ setProperty("eef", eef); } -void GenerateGraspPose::setGripperGraspPose(const std::string &pose_name){ - setProperty("eef_named_pose", pose_name); +void GenerateGraspPose::setNamedPose(const std::string &pose_name){ + setProperty("pregrasp", pose_name); } void GenerateGraspPose::setObject(const std::string &object){ @@ -111,11 +111,13 @@ bool GenerateGraspPose::compute(){ const moveit::core::JointModelGroup* jmg = scene_->getRobotModel()->getEndEffector(eef); robot_state::RobotState &robot_state = scene_->getCurrentStateNonConst(); - const std::string& eef_named_pose = props.get("eef_named_pose"); - robot_state.setToDefaultValues(jmg , eef_named_pose); + const std::string& joint_pose = props.get("pregrasp"); + if(!joint_pose.empty()){ + robot_state.setToDefaultValues(jmg , joint_pose); + } geometry_msgs::TransformStamped tool2grasp_msg = props.get("tool_to_grasp_tf"); - const std::string &link_name = jmg ->getEndEffectorParentGroup().second; + const std::string &link_name = jmg->getEndEffectorParentGroup().second; if (tool2grasp_msg.header.frame_id.empty()) // if no frame_id is given tool2grasp_msg.header.frame_id = link_name; // interpret the transform w.r.t. eef link frame Eigen::Affine3d to_grasp; diff --git a/core/src/stages/pick.cpp b/core/src/stages/pick.cpp new file mode 100644 index 00000000..0fdae2c6 --- /dev/null +++ b/core/src/stages/pick.cpp @@ -0,0 +1,118 @@ +#include + +#include +#include + +#include +#include +#include +#include +#include + +#include + +namespace moveit { namespace task_constructor { namespace stages { + +Pick::Pick(Stage::pointer&& grasp_stage, const std::string& name) + : SerialContainer(name) +{ + PropertyMap& p = properties(); + p.declare("eef", "end effector name"); + p.declare("object", "name of object to grasp"); + // internal properties (cannot be marked as such yet) + p.declare("eef_group", "JMG of eef"); + p.declare("eef_parent_group", "JMG of eef's parent"); + + auto cartesian = std::make_shared(); + auto pipeline = std::make_shared(); + pipeline->setTimeout(8.0); + pipeline->setPlannerId("RRTConnectkConfigDefault"); + + { + auto move = std::make_unique("open gripper", pipeline); + PropertyMap& p = move->properties(); + p.property("group").configureInitFrom(Stage::PARENT, "eef_group"); + move->setGoal("open"); // TODO: retrieve from grasp stage + insert(std::move(move)); + } + + { + auto move = std::make_unique("move to object", pipeline); + PropertyMap& p = move->properties(); + p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group"); + insert(std::move(move)); + } + + { + auto approach = std::make_unique("approach object", cartesian); + PropertyMap& p = approach->properties(); + p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group"); + p.set("marker_ns", std::string("approach")); + approach_stage_ = approach.get(); + insert(std::move(approach)); + } + + grasp_stage_ = grasp_stage.get(); + grasp_stage->properties().configureInitFrom(Stage::PARENT, {"eef", "object"}); + insert(std::move(grasp_stage)); + + { + auto attach = std::make_unique("attach object"); + PropertyMap& p = attach->properties(); + p.declare("eef"); + p.declare("object"); + p.configureInitFrom(Stage::PARENT, { "eef", "object" }); + attach->setCallback([this](const planning_scene::PlanningScenePtr& scene, const PropertyMap& p){ + const std::string& eef = p.get("eef"); + moveit_msgs::AttachedCollisionObject obj; + obj.object.operation = moveit_msgs::CollisionObject::ADD; + obj.link_name = scene->getRobotModel()->getEndEffector(eef)->getEndEffectorParentGroup().second; + obj.object.id = p.get("object"); + scene->processAttachedCollisionObjectMsg(obj); + }); + insert(std::move(attach)); + } + + { + auto lift = std::make_unique("lift object", cartesian); + PropertyMap& p = lift->properties(); + p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group"); + p.set("marker_ns", std::string("lift")); + lift_stage_ = lift.get(); + insert(std::move(lift)); + } +} + +void Pick::init(const moveit::core::RobotModelConstPtr& robot_model) +{ + // inherit properties from parent + PropertyMap* p = &properties(); + p->performInitFrom(Stage::PARENT, parent()->properties()); + + // init internal properties + const std::string &eef = p->get("eef"); + const moveit::core::JointModelGroup *jmg = robot_model->getEndEffector(eef); + p->set("eef_group", jmg->getName()); + p->set("eef_parent_group", jmg->getEndEffectorParentGroup().first); + + // propagate my properties to children (and do standard init) + SerialContainer::init(robot_model); +} + +void Pick::setApproachMotion(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) +{ + auto& p = approach_stage_->properties(); + p.set("twist", motion); + p.set("min_distance", min_distance); + p.set("max_distance", max_distance); +} + +void Pick::setLiftMotion(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance) +{ + auto& p = lift_stage_->properties(); + p.set("twist", motion); + p.set("min_distance", min_distance); + p.set("max_distance", max_distance); +} + +} } } diff --git a/core/src/stages/simple_grasp.cpp b/core/src/stages/simple_grasp.cpp new file mode 100644 index 00000000..64ba316e --- /dev/null +++ b/core/src/stages/simple_grasp.cpp @@ -0,0 +1,120 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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 */ + +#include + +#include +#include +#include +#include +#include + +#include + +#include +#include + +namespace moveit { namespace task_constructor { namespace stages { + +SimpleGrasp::SimpleGrasp(const std::string& name) + : SerialContainer(name) +{ + { + auto gengrasp = std::make_unique("generate grasp pose"); + grasp_generator_ = gengrasp.get(); + + auto ik = std::make_unique("compute ik", std::move(gengrasp)); + const std::initializer_list& grasp_prop_names = { "eef", "pregrasp", "object", "angle_delta", "tool_to_grasp_tf" }; + ik->exposePropertiesOfChild(0, grasp_prop_names); + insert(std::move(ik)); + + exposePropertiesOfChild(-1, grasp_prop_names); + exposePropertiesOfChild(-1, { "max_ik_solutions", "timeout" }); + } + { + auto allow_touch = std::make_unique("enable object collision"); + PropertyMap& p = allow_touch->properties(); + p.declare("eef"); + p.declare("object"); + p.configureInitFrom(Stage::PARENT, { "eef", "object" }); + + allow_touch->setCallback([this](const planning_scene::PlanningScenePtr& scene, const PropertyMap& p){ + collision_detection::AllowedCollisionMatrix& acm = scene->getAllowedCollisionMatrixNonConst(); + const std::string& eef = p.get("eef"); + const std::string& object = p.get("object"); + acm.setEntry(object, scene->getRobotModel()->getEndEffector(eef) + ->getLinkModelNamesWithCollisionGeometry(), true); + }); + insert(std::move(allow_touch)); + } + { + auto pipeline = std::make_shared(); + pipeline->setTimeout(8.0); + pipeline->setPlannerId("RRTConnectkConfigDefault"); + + auto move = std::make_unique("close gripper", pipeline); + move->restrictDirection(MoveTo::FORWARD); + PropertyMap& p = move->properties(); + p.property("group").configureInitFrom(Stage::PARENT, [this](const PropertyMap& parent_map){ + const std::string& eef = parent_map.get("eef"); + const moveit::core::JointModelGroup* jmg = model_->getEndEffector(eef); + return boost::any(jmg->getName()); + }); + insert(std::move(move)); + exposePropertyOfChildAs(-1, "joint_pose", "grasp"); + } +} + +void SimpleGrasp::init(const moveit::core::RobotModelConstPtr& robot_model) +{ + model_ = robot_model; + SerialContainer::init(robot_model); +} + +void SimpleGrasp::setMonitoredStage(Stage* monitored) +{ + grasp_generator_->setMonitoredStage(monitored); +} + +void SimpleGrasp::setToolToGraspTF(const Eigen::Affine3d& transform, const std::string& link) { + geometry_msgs::TransformStamped stamped; + stamped.header.frame_id = link; + stamped.child_frame_id = "grasp_frame"; + tf::transformEigenToMsg(transform, stamped.transform); + setToolToGraspTF(stamped); +} + +} } } From ea6cc4b6bf0bc43aaff5fd83191fe27cfdf24382 Mon Sep 17 00:00:00 2001 From: llach Date: Sat, 24 Feb 2018 16:51:04 +0100 Subject: [PATCH 02/13] moved "attach object" from "pick" to "grasp" stage --- core/src/stages/pick.cpp | 17 ----------------- core/src/stages/simple_grasp.cpp | 17 ++++++++++++++++- 2 files changed, 16 insertions(+), 18 deletions(-) diff --git a/core/src/stages/pick.cpp b/core/src/stages/pick.cpp index 0fdae2c6..800b95c4 100644 --- a/core/src/stages/pick.cpp +++ b/core/src/stages/pick.cpp @@ -56,23 +56,6 @@ Pick::Pick(Stage::pointer&& grasp_stage, const std::string& name) grasp_stage->properties().configureInitFrom(Stage::PARENT, {"eef", "object"}); insert(std::move(grasp_stage)); - { - auto attach = std::make_unique("attach object"); - PropertyMap& p = attach->properties(); - p.declare("eef"); - p.declare("object"); - p.configureInitFrom(Stage::PARENT, { "eef", "object" }); - attach->setCallback([this](const planning_scene::PlanningScenePtr& scene, const PropertyMap& p){ - const std::string& eef = p.get("eef"); - moveit_msgs::AttachedCollisionObject obj; - obj.object.operation = moveit_msgs::CollisionObject::ADD; - obj.link_name = scene->getRobotModel()->getEndEffector(eef)->getEndEffectorParentGroup().second; - obj.object.id = p.get("object"); - scene->processAttachedCollisionObjectMsg(obj); - }); - insert(std::move(attach)); - } - { auto lift = std::make_unique("lift object", cartesian); PropertyMap& p = lift->properties(); diff --git a/core/src/stages/simple_grasp.cpp b/core/src/stages/simple_grasp.cpp index 64ba316e..74c25fa2 100644 --- a/core/src/stages/simple_grasp.cpp +++ b/core/src/stages/simple_grasp.cpp @@ -86,7 +86,6 @@ SimpleGrasp::SimpleGrasp(const std::string& name) pipeline->setPlannerId("RRTConnectkConfigDefault"); auto move = std::make_unique("close gripper", pipeline); - move->restrictDirection(MoveTo::FORWARD); PropertyMap& p = move->properties(); p.property("group").configureInitFrom(Stage::PARENT, [this](const PropertyMap& parent_map){ const std::string& eef = parent_map.get("eef"); @@ -96,6 +95,22 @@ SimpleGrasp::SimpleGrasp(const std::string& name) insert(std::move(move)); exposePropertyOfChildAs(-1, "joint_pose", "grasp"); } + { + auto attach = std::make_unique("attach object"); + PropertyMap& p = attach->properties(); + p.declare("eef"); + p.declare("object"); + p.configureInitFrom(Stage::PARENT, { "eef", "object" }); + attach->setCallback([this](const planning_scene::PlanningScenePtr& scene, const PropertyMap& p){ + const std::string& eef = p.get("eef"); + moveit_msgs::AttachedCollisionObject obj; + obj.object.operation = moveit_msgs::CollisionObject::ADD; + obj.link_name = scene->getRobotModel()->getEndEffector(eef)->getEndEffectorParentGroup().second; + obj.object.id = p.get("object"); + scene->processAttachedCollisionObjectMsg(obj); + }); + insert(std::move(attach)); + } } void SimpleGrasp::init(const moveit::core::RobotModelConstPtr& robot_model) From 1c61fb3b8562d9ce5f650b83c90b696af21224c7 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 25 Feb 2018 22:07:47 +0100 Subject: [PATCH 03/13] expose solvers --- .../moveit/task_constructor/stages/pick.h | 15 ++++++++++++++- core/src/stages/pick.cpp | 16 ++++++++-------- 2 files changed, 22 insertions(+), 9 deletions(-) diff --git a/core/include/moveit/task_constructor/stages/pick.h b/core/include/moveit/task_constructor/stages/pick.h index c1ebd54e..0b828304 100644 --- a/core/include/moveit/task_constructor/stages/pick.h +++ b/core/include/moveit/task_constructor/stages/pick.h @@ -36,10 +36,18 @@ #pragma once +#include #include #include -namespace moveit { namespace task_constructor { namespace stages { +namespace moveit { namespace task_constructor { + +namespace solvers { +MOVEIT_CLASS_FORWARD(CartesianPath) +MOVEIT_CLASS_FORWARD(PipelinePlanner) +} + +namespace stages { /** Pick wraps a complete pipeline to pick up an object with a given end effector. * @@ -54,6 +62,8 @@ namespace moveit { namespace task_constructor { namespace stages { * the end effector's Cartesian pose needs to be provided by an external grasp stage. */ class Pick : public SerialContainer { + solvers::CartesianPathPtr cartesian_solver_; + solvers::PipelinePlannerPtr pipeline_solver_; Stage* grasp_stage_ = nullptr; Stage* approach_stage_ = nullptr; Stage* lift_stage_ = nullptr; @@ -70,6 +80,9 @@ public: properties().set("object", object); } + solvers::CartesianPathPtr cartesianSolver() { return cartesian_solver_; } + solvers::PipelinePlannerPtr pipelineSolver() { return pipeline_solver_; } + void setApproachMotion(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance); void setLiftMotion(const geometry_msgs::TwistStamped& motion, diff --git a/core/src/stages/pick.cpp b/core/src/stages/pick.cpp index 800b95c4..8f234046 100644 --- a/core/src/stages/pick.cpp +++ b/core/src/stages/pick.cpp @@ -23,13 +23,13 @@ Pick::Pick(Stage::pointer&& grasp_stage, const std::string& name) p.declare("eef_group", "JMG of eef"); p.declare("eef_parent_group", "JMG of eef's parent"); - auto cartesian = std::make_shared(); - auto pipeline = std::make_shared(); - pipeline->setTimeout(8.0); - pipeline->setPlannerId("RRTConnectkConfigDefault"); + cartesian_solver_ = std::make_shared(); + pipeline_solver_ = std::make_shared(); + pipeline_solver_->setTimeout(8.0); + pipeline_solver_->setPlannerId("RRTConnectkConfigDefault"); { - auto move = std::make_unique("open gripper", pipeline); + auto move = std::make_unique("open gripper", pipeline_solver_); PropertyMap& p = move->properties(); p.property("group").configureInitFrom(Stage::PARENT, "eef_group"); move->setGoal("open"); // TODO: retrieve from grasp stage @@ -37,14 +37,14 @@ Pick::Pick(Stage::pointer&& grasp_stage, const std::string& name) } { - auto move = std::make_unique("move to object", pipeline); + auto move = std::make_unique("move to object", pipeline_solver_); PropertyMap& p = move->properties(); p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group"); insert(std::move(move)); } { - auto approach = std::make_unique("approach object", cartesian); + auto approach = std::make_unique("approach object", cartesian_solver_); PropertyMap& p = approach->properties(); p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group"); p.set("marker_ns", std::string("approach")); @@ -57,7 +57,7 @@ Pick::Pick(Stage::pointer&& grasp_stage, const std::string& name) insert(std::move(grasp_stage)); { - auto lift = std::make_unique("lift object", cartesian); + auto lift = std::make_unique("lift object", cartesian_solver_); PropertyMap& p = lift->properties(); p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group"); p.set("marker_ns", std::string("lift")); From 2331c9aef247db87ab0a0c4aacd87f0c0e89f27e Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 27 Feb 2018 23:05:28 +0100 Subject: [PATCH 04/13] pick: expose IK link frame as eef_frame --- core/src/stages/pick.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/core/src/stages/pick.cpp b/core/src/stages/pick.cpp index 8f234046..122c35d8 100644 --- a/core/src/stages/pick.cpp +++ b/core/src/stages/pick.cpp @@ -17,8 +17,10 @@ Pick::Pick(Stage::pointer&& grasp_stage, const std::string& name) : SerialContainer(name) { PropertyMap& p = properties(); - p.declare("eef", "end effector name"); p.declare("object", "name of object to grasp"); + p.declare("eef", "end effector name"); + p.declare("eef_frame", "name of end effector frame"); + // internal properties (cannot be marked as such yet) p.declare("eef_group", "JMG of eef"); p.declare("eef_parent_group", "JMG of eef's parent"); @@ -47,6 +49,7 @@ Pick::Pick(Stage::pointer&& grasp_stage, const std::string& name) auto approach = std::make_unique("approach object", cartesian_solver_); PropertyMap& p = approach->properties(); p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group"); + p.property("link").configureInitFrom(Stage::PARENT, "eef_frame"); p.set("marker_ns", std::string("approach")); approach_stage_ = approach.get(); insert(std::move(approach)); @@ -60,6 +63,7 @@ Pick::Pick(Stage::pointer&& grasp_stage, const std::string& name) auto lift = std::make_unique("lift object", cartesian_solver_); PropertyMap& p = lift->properties(); p.property("group").configureInitFrom(Stage::PARENT, "eef_parent_group"); + p.property("link").configureInitFrom(Stage::PARENT, "eef_frame"); p.set("marker_ns", std::string("lift")); lift_stage_ = lift.get(); insert(std::move(lift)); From 81d88911a9a290c4d7d2e04e5a5f03febca3930c Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 27 Feb 2018 17:22:53 +0100 Subject: [PATCH 05/13] cosmetic fixes --- core/src/properties.cpp | 4 ++-- core/src/stages/compute_ik.cpp | 4 +++- core/src/stages/fixed_state.cpp | 2 +- core/src/stages/simple_grasp.cpp | 2 +- 4 files changed, 7 insertions(+), 5 deletions(-) diff --git a/core/src/properties.cpp b/core/src/properties.cpp index c922dc04..db38e7e3 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -215,8 +215,8 @@ boost::any fromName(const PropertyMap& other, const std::string& other_name) Property::error::error(const std::string& msg) - : std::runtime_error("Property: " + msg) - , msg_(msg) + : std::runtime_error(msg) + , msg_("Property: " + msg) {} void Property::error::setName(const std::string& name) diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 6a50871b..48f1733d 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -156,7 +156,7 @@ void ComputeIK::onNewSolution(const SolutionBase &s) // validate placed link for collisions bool colliding = isTargetPoseColliding(sandbox_scene, eef_jmg, target_pose, link_name); if (colliding && !storeFailures()) { - ROS_ERROR("eeg in collision"); + ROS_ERROR("eef in collision"); return; } @@ -174,6 +174,7 @@ void ComputeIK::onNewSolution(const SolutionBase &s) generateCollisionMarkers(sandbox_state, appender, visualize_links); std::copy(placed_link_markers.begin(), placed_link_markers.end(), std::back_inserter(solution.markers())); solution.setCost(std::numeric_limits::infinity()); // mark solution as failure + solution.setName("eef in collision"); spawn(InterfaceState(sandbox_scene), std::move(solution)); return; } else @@ -260,6 +261,7 @@ void ComputeIK::onNewSolution(const SolutionBase &s) // mark solution as invalid solution.setCost(std::numeric_limits::infinity()); + solution.setName("no IK found"); // ik target link placement std::copy(placed_link_markers.begin(), placed_link_markers.end(), std::back_inserter(solution.markers())); diff --git a/core/src/stages/fixed_state.cpp b/core/src/stages/fixed_state.cpp index 1af35363..99ba79dc 100644 --- a/core/src/stages/fixed_state.cpp +++ b/core/src/stages/fixed_state.cpp @@ -54,7 +54,7 @@ bool FixedState::canCompute() const{ bool FixedState::compute() { spawn(InterfaceState(scene_), 0.0); - return ran_= true; + return ran_ = true; } } } } diff --git a/core/src/stages/simple_grasp.cpp b/core/src/stages/simple_grasp.cpp index 74c25fa2..7de51a36 100644 --- a/core/src/stages/simple_grasp.cpp +++ b/core/src/stages/simple_grasp.cpp @@ -65,7 +65,7 @@ SimpleGrasp::SimpleGrasp(const std::string& name) exposePropertiesOfChild(-1, { "max_ik_solutions", "timeout" }); } { - auto allow_touch = std::make_unique("enable object collision"); + auto allow_touch = std::make_unique("allow object collision"); PropertyMap& p = allow_touch->properties(); p.declare("eef"); p.declare("object"); From 1f6bea9438a7eee073595bbd37efe466651559c0 Mon Sep 17 00:00:00 2001 From: janEbert Date: Thu, 22 Feb 2018 15:39:42 +0100 Subject: [PATCH 06/13] icons to visualize logic flow --- .../motion_planning_tasks/src/CMakeLists.txt | 1 + .../motion_planning_tasks/src/icons.cpp | 19 ++ .../motion_planning_tasks/src/icons.h | 13 + .../src/icons/connectarrow.png | Bin 0 -> 180 bytes .../src/icons/downarrow.png | Bin 0 -> 125 bytes .../src/icons/generatearrow.png | Bin 0 -> 141 bytes .../src/icons/uparrow.png | Bin 0 -> 123 bytes .../src/icons/updownarrow.png | Bin 0 -> 183 bytes .../src/remote_task_model.cpp | 4 + .../motion_planning_tasks/src/resources.qrc | 5 + .../src/task_list_model.cpp | 20 ++ .../src/task_list_model.h | 1 + .../utils/CMakeLists.txt | 1 + .../motion_planning_tasks/utils/icon.cpp | 236 ++++++++++++++++++ .../motion_planning_tasks/utils/icon.h | 85 +++++++ 15 files changed, 385 insertions(+) create mode 100644 visualization/motion_planning_tasks/src/icons.cpp create mode 100644 visualization/motion_planning_tasks/src/icons.h create mode 100644 visualization/motion_planning_tasks/src/icons/connectarrow.png create mode 100644 visualization/motion_planning_tasks/src/icons/downarrow.png create mode 100644 visualization/motion_planning_tasks/src/icons/generatearrow.png create mode 100644 visualization/motion_planning_tasks/src/icons/uparrow.png create mode 100644 visualization/motion_planning_tasks/src/icons/updownarrow.png create mode 100644 visualization/motion_planning_tasks/utils/icon.cpp create mode 100644 visualization/motion_planning_tasks/utils/icon.h diff --git a/visualization/motion_planning_tasks/src/CMakeLists.txt b/visualization/motion_planning_tasks/src/CMakeLists.txt index c64d6114..b1dc4178 100644 --- a/visualization/motion_planning_tasks/src/CMakeLists.txt +++ b/visualization/motion_planning_tasks/src/CMakeLists.txt @@ -8,6 +8,7 @@ qt_wrap_ui(UIC_FILES add_library(${MOVEIT_LIB_NAME} factory_model.cpp + icons.cpp job_queue.cpp local_task_model.cpp meta_task_list_model.cpp diff --git a/visualization/motion_planning_tasks/src/icons.cpp b/visualization/motion_planning_tasks/src/icons.cpp new file mode 100644 index 00000000..1d05a297 --- /dev/null +++ b/visualization/motion_planning_tasks/src/icons.cpp @@ -0,0 +1,19 @@ +#include "icons.h" +#include + +using namespace moveit_rviz_plugin::utils; + +namespace moveit_rviz_plugin { namespace icons { + +const Icon CONNECT({ + {QLatin1String(":icons/connectarrow.png"), QColor::fromRgba(0xff000000)}}, Icon::Tint); +const Icon FORWARD({ + {QLatin1String(":icons/downarrow.png"), QColor::fromRgba(0xff000000)}}, Icon::Tint); +const Icon BACKWARD({ + {QLatin1String(":icons/uparrow.png"), QColor::fromRgba(0xff000000)}}, Icon::Tint); +const Icon BOTHWAY({ + {QLatin1String(":icons/updownarrow.png"), QColor::fromRgba(0xff000000)}}, Icon::Tint); +const Icon GENERATE({ + {QLatin1String(":icons/generatearrow.png"), QColor::fromRgba(0xff000000)}}, Icon::Tint); + +} } diff --git a/visualization/motion_planning_tasks/src/icons.h b/visualization/motion_planning_tasks/src/icons.h new file mode 100644 index 00000000..aa696adb --- /dev/null +++ b/visualization/motion_planning_tasks/src/icons.h @@ -0,0 +1,13 @@ +#pragma once + +#include + +namespace moveit_rviz_plugin { namespace icons { + +extern const moveit_rviz_plugin::utils::Icon CONNECT; +extern const moveit_rviz_plugin::utils::Icon FORWARD; +extern const moveit_rviz_plugin::utils::Icon BACKWARD; +extern const moveit_rviz_plugin::utils::Icon BOTHWAY; +extern const moveit_rviz_plugin::utils::Icon GENERATE; + +} } diff --git a/visualization/motion_planning_tasks/src/icons/connectarrow.png b/visualization/motion_planning_tasks/src/icons/connectarrow.png new file mode 100644 index 0000000000000000000000000000000000000000..b709b9c9578bb37f8eea675b4f5fcca8820b8be8 GIT binary patch literal 180 zcmeAS@N?(olHy`uVBq!ia0vp^0wB!60wlNoGJgf6I14-?iy0WWg+Z8+Vb&Z8pkSJ( zi(`n!`K=Q+@-`R^8M&G*h7I4a%Jq!Rr9kkH1TY$v&AZq~eP@bKm-9`<(OBGWe3<1zOa{w;sC ayzE6wb<=i_$1Ok`89ZJ6T-G@yGywoIct5NF literal 0 HcmV?d00001 diff --git a/visualization/motion_planning_tasks/src/icons/downarrow.png b/visualization/motion_planning_tasks/src/icons/downarrow.png new file mode 100644 index 0000000000000000000000000000000000000000..222c1ddf98992b4f1677eb11815298667ee39503 GIT binary patch literal 125 zcmeAS@N?(olHy`uVBq!ia0vp^0wBx*Bp9q_EZ7UAI14-?iy0WWg+Z8+Vb&Z8prF2| zi(`ny<>VjyjFLQ@0&K#}QW927JOcd=4u}5#|1bGZ*+uBzVb*`?2YwhZwtQ#wGO;){ SON}`kWUi;HpUXO@geCwW0UuHT literal 0 HcmV?d00001 diff --git a/visualization/motion_planning_tasks/src/icons/generatearrow.png b/visualization/motion_planning_tasks/src/icons/generatearrow.png new file mode 100644 index 0000000000000000000000000000000000000000..8df7a986d6a84b298400f881a350bd72e33842f7 GIT binary patch literal 141 zcmeAS@N?(olHy`uVBq!ia0vp^0wBx*Bp9q_EZ7UAI14-?iy0WWg+Z8+Vb&Z8prE~{ zi(`ny<>VjyjDkFj0&L7Z=NUv38$K^o;^DA!VOjOGp3Rx5<=~M9win6=7z$ZfR1U~a kV|;!%W)|b)WGh*Q^Fhg0tDf340F7esboFyt=akR{075t<*#H0l literal 0 HcmV?d00001 diff --git a/visualization/motion_planning_tasks/src/icons/uparrow.png b/visualization/motion_planning_tasks/src/icons/uparrow.png new file mode 100644 index 0000000000000000000000000000000000000000..0781d0e5c39f8d6659fa81569e4174dc462e9912 GIT binary patch literal 123 zcmeAS@N?(olHy`uVBq!ia0vp^0wBx*Bp9q_EZ7UAI14-?iy0WWg+Z8+Vb&Z8prEd& zi(`ny<>VjyjDkFj0&L7Z=NUv38$K^o;^DA!VOjOGp3Rx5<=~M9wzfV7hF!cCUQ*9* Qb%0FuboFyt=akR{0K*L(WB>pF literal 0 HcmV?d00001 diff --git a/visualization/motion_planning_tasks/src/icons/updownarrow.png b/visualization/motion_planning_tasks/src/icons/updownarrow.png new file mode 100644 index 0000000000000000000000000000000000000000..46da5a23a65581e184088a57c6e098648f47d696 GIT binary patch literal 183 zcmeAS@N?(olHy`uVBq!ia0vp^0wB!60wlNoGJgf6I14-?iy0WWg+Z8+Vb&Z8pkSt_ zi(`n!`MVbk`3^boxLn*`?et{d73wz~zq(SY`#qdAR^jkMC+VNnOhTr@{85Or@ty~{5HSN~E e#5wZzjp7Y9`cs)URowtu%HZkh=d#Wzp$P!KP(^+K literal 0 HcmV?d00001 diff --git a/visualization/motion_planning_tasks/src/remote_task_model.cpp b/visualization/motion_planning_tasks/src/remote_task_model.cpp index 59a14801..b6e07f98 100644 --- a/visualization/motion_planning_tasks/src/remote_task_model.cpp +++ b/visualization/motion_planning_tasks/src/remote_task_model.cpp @@ -200,6 +200,10 @@ QVariant RemoteTaskModel::data(const QModelIndex &index, int role) const if (index.column() == 0 && !index.parent().isValid()) return (flags_ & IS_DESTROYED) ? QColor(Qt::red) : QApplication::palette().text().color(); break; + case Qt::DecorationRole: + if (index.column() == 0 && index.parent().isValid()) + return flowIcon(n->interface_flags_); + break; } return BaseTaskModel::data(index, role); diff --git a/visualization/motion_planning_tasks/src/resources.qrc b/visualization/motion_planning_tasks/src/resources.qrc index d08caa5c..499e5a2c 100644 --- a/visualization/motion_planning_tasks/src/resources.qrc +++ b/visualization/motion_planning_tasks/src/resources.qrc @@ -6,5 +6,10 @@ icons/new-stage.png icons/success.png icons/tasks.png + icons/connectarrow.png + icons/downarrow.png + icons/generatearrow.png + icons/uparrow.png + icons/updownarrow.png diff --git a/visualization/motion_planning_tasks/src/task_list_model.cpp b/visualization/motion_planning_tasks/src/task_list_model.cpp index e4b81f6d..f95268d4 100644 --- a/visualization/motion_planning_tasks/src/task_list_model.cpp +++ b/visualization/motion_planning_tasks/src/task_list_model.cpp @@ -38,6 +38,7 @@ #include "local_task_model.h" #include "remote_task_model.h" #include "factory_model.h" +#include "icons.h" #include #include @@ -48,6 +49,8 @@ #include #include +using namespace moveit::task_constructor; + namespace moveit_rviz_plugin { static const std::string LOGNAME("TaskListModel"); @@ -109,6 +112,23 @@ Qt::ItemFlags BaseTaskModel::flags(const QModelIndex &index) const return flags; } +QVariant BaseTaskModel::flowIcon(moveit::task_constructor::InterfaceFlags f) +{ + static const QIcon CONNECT_ICON = icons::CONNECT.icon(); + static const QIcon FORWARD_ICON = icons::FORWARD.icon(); + static const QIcon BACKWARD_ICON = icons::BACKWARD.icon(); + static const QIcon BOTHWAY_ICON = icons::BOTHWAY.icon(); + static const QIcon GENERATE_ICON = icons::GENERATE.icon(); + + if (f == InterfaceFlags(CONNECT)) return CONNECT_ICON; + if (f == InterfaceFlags(PROPAGATE_FORWARDS)) return FORWARD_ICON; + if (f == InterfaceFlags(PROPAGATE_BACKWARDS)) return BACKWARD_ICON; + if (f == PROPAGATE_BOTHWAYS) return BOTHWAY_ICON; + if (f == InterfaceFlags(GENERATE)) return GENERATE_ICON; + + return QVariant(); +} + StageFactoryPtr getStageFactory() { diff --git a/visualization/motion_planning_tasks/src/task_list_model.h b/visualization/motion_planning_tasks/src/task_list_model.h index 29950221..753e5f4a 100644 --- a/visualization/motion_planning_tasks/src/task_list_model.h +++ b/visualization/motion_planning_tasks/src/task_list_model.h @@ -84,6 +84,7 @@ public: virtual void setStageFactory(const StageFactoryPtr &factory) {} Qt::ItemFlags flags(const QModelIndex &index) const override; unsigned int taskFlags() const { return flags_; } + static QVariant flowIcon(moveit::task_constructor::InterfaceFlags f); /// get solution model for given stage index virtual QAbstractItemModel* getSolutionModel(const QModelIndex& index) = 0; diff --git a/visualization/motion_planning_tasks/utils/CMakeLists.txt b/visualization/motion_planning_tasks/utils/CMakeLists.txt index c9de4164..ae3533c8 100644 --- a/visualization/motion_planning_tasks/utils/CMakeLists.txt +++ b/visualization/motion_planning_tasks/utils/CMakeLists.txt @@ -3,6 +3,7 @@ set(MOVEIT_LIB_NAME motion_planning_tasks_utils) set(SOURCES flat_merge_proxy_model.cpp tree_merge_proxy_model.cpp + icon.cpp ) add_library(${MOVEIT_LIB_NAME} SHARED ${SOURCES}) diff --git a/visualization/motion_planning_tasks/utils/icon.cpp b/visualization/motion_planning_tasks/utils/icon.cpp new file mode 100644 index 00000000..245c4980 --- /dev/null +++ b/visualization/motion_planning_tasks/utils/icon.cpp @@ -0,0 +1,236 @@ +/**************************************************************************** +** +** Copyright (C) 2016 The Qt Company Ltd. +** Contact: https://www.qt.io/licensing/ +** +** This file is adapted from Qt Creator (replacing theme stuff by QColor) +** +** Commercial License Usage +** Licensees holding valid commercial Qt licenses may use this file in +** accordance with the commercial license agreement provided with the +** Software or, alternatively, in accordance with the terms contained in +** a written agreement between you and The Qt Company. For licensing terms +** and conditions see https://www.qt.io/terms-conditions. For further +** information use the contact form at https://www.qt.io/contact-us. +** +** GNU General Public License Usage +** Alternatively, this file may be used under the terms of the GNU +** General Public License version 3 as published by the Free Software +** Foundation with exceptions as appearing in the file LICENSE.GPL3-EXCEPT +** included in the packaging of this file. Please review the following +** information to ensure the GNU General Public License requirements will +** be met: https://www.gnu.org/licenses/gpl-3.0.html. +** +****************************************************************************/ + +#include "icon.h" + +#include +#include +#include +#include +#include +#include +#include + +namespace moveit_rviz_plugin { namespace utils { + +static const qreal PunchEdgeWidth = 0.5; +static const qreal PunchEdgeIntensity = 0.6; + +static QPixmap maskToColorAndAlpha(const QPixmap &mask, const QColor &color) +{ + QImage result(mask.toImage().convertToFormat(QImage::Format_ARGB32)); + result.setDevicePixelRatio(mask.devicePixelRatio()); + QRgb *bitsStart = reinterpret_cast(result.bits()); + const QRgb *bitsEnd = bitsStart + result.width() * result.height(); + const QRgb tint = color.rgb() & 0x00ffffff; + const QRgb alpha = QRgb(color.alpha()); + for (QRgb *pixel = bitsStart; pixel < bitsEnd; ++pixel) { + QRgb pixelAlpha = (((~*pixel) & 0xff) * alpha) >> 8; + *pixel = (pixelAlpha << 24) | tint; + } + return QPixmap::fromImage(result); +} + +typedef QPair MaskAndColor; +typedef QList MasksAndColors; +static MasksAndColors masksAndColors(const Icon &icon, int dpr) +{ + MasksAndColors result; + for (const IconMaskAndColor &i: icon) { + const QString &fileName = i.first; + const QColor color = i.second; + result.append(qMakePair(QPixmap(fileName), color)); + } + return result; +} + +static void smearPixmap(QPainter *painter, const QPixmap &pixmap, qreal radius) +{ + const qreal nagative = -radius - 0.01; // Workaround for QPainter rounding behavior + const qreal positive = radius; + painter->drawPixmap(QPointF(nagative, nagative), pixmap); + painter->drawPixmap(QPointF(0, nagative), pixmap); + painter->drawPixmap(QPointF(positive, nagative), pixmap); + painter->drawPixmap(QPointF(positive, 0), pixmap); + painter->drawPixmap(QPointF(positive, positive), pixmap); + painter->drawPixmap(QPointF(0, positive), pixmap); + painter->drawPixmap(QPointF(nagative, positive), pixmap); + painter->drawPixmap(QPointF(nagative, 0), pixmap); +} + +static QPixmap combinedMask(const MasksAndColors &masks, Icon::IconStyleOptions style) +{ + if (masks.count() == 1) + return masks.first().first; + + QPixmap result(masks.first().first); + QPainter p(&result); + p.setCompositionMode(QPainter::CompositionMode_Darken); + auto maskImage = masks.constBegin(); + maskImage++; + for (;maskImage != masks.constEnd(); ++maskImage) { + if (style & Icon::PunchEdges) { + p.save(); + p.setOpacity(PunchEdgeIntensity); + p.setCompositionMode(QPainter::CompositionMode_Lighten); + smearPixmap(&p, maskToColorAndAlpha((*maskImage).first, Qt::white), PunchEdgeWidth); + p.restore(); + } + p.drawPixmap(0, 0, (*maskImage).first); + } + p.end(); + return result; +} + +static QPixmap masksToIcon(const MasksAndColors &masks, const QPixmap &combinedMask, Icon::IconStyleOptions style) +{ + QPixmap result(combinedMask.size()); + result.setDevicePixelRatio(combinedMask.devicePixelRatio()); + result.fill(Qt::transparent); + QPainter p(&result); + + for (MasksAndColors::const_iterator maskImage = masks.constBegin(); + maskImage != masks.constEnd(); ++maskImage) { + if (style & Icon::PunchEdges && maskImage != masks.constBegin()) { + // Punch a transparent outline around an overlay. + p.save(); + p.setOpacity(PunchEdgeIntensity); + p.setCompositionMode(QPainter::CompositionMode_DestinationOut); + smearPixmap(&p, maskToColorAndAlpha((*maskImage).first, Qt::white), PunchEdgeWidth); + p.restore(); + } + p.drawPixmap(0, 0, maskToColorAndAlpha((*maskImage).first, (*maskImage).second)); + } + + if (style & Icon::DropShadow) { + const QPixmap shadowMask = maskToColorAndAlpha(combinedMask, Qt::black); + p.setCompositionMode(QPainter::CompositionMode_DestinationOver); + p.setOpacity(0.05); + p.drawPixmap(QPointF(0, -0.501), shadowMask); + p.drawPixmap(QPointF(-0.501, 0), shadowMask); + p.drawPixmap(QPointF(0.5, 0), shadowMask); + p.drawPixmap(QPointF(0.5, 0.5), shadowMask); + p.drawPixmap(QPointF(-0.501, 0.5), shadowMask); + p.setOpacity(0.2); + p.drawPixmap(0, 1, shadowMask); + } + + p.end(); + + return result; +} + +static QPixmap combinedPlainPixmaps(const QVector &images) +{ + QPixmap result(images.first().first); + auto pixmap = images.constBegin(); + pixmap++; + for (;pixmap != images.constEnd(); ++pixmap) { + const QPixmap overlay((*pixmap).first); + result.paintEngine()->painter()->drawPixmap(0, 0, overlay); + } + return result; +} + +Icon::Icon() +{ +} + +Icon::Icon(std::initializer_list args, Icon::IconStyleOptions style) + : QVector(args) + , m_style(style) +{ +} + +Icon::Icon(const QString &imageFileName) + : m_style(None) +{ + append({imageFileName, QColor()}); +} + +QIcon Icon::icon() const +{ + if (isEmpty()) { + return QIcon(); + } else if (m_style == None) { + return QIcon(combinedPlainPixmaps(*this)); + } else { + QIcon result; + const int maxDpr = qRound(qApp->devicePixelRatio()); + for (int dpr = 1; dpr <= maxDpr; dpr++) { + const MasksAndColors masks = masksAndColors(*this, dpr); + const QPixmap combined_mask = combinedMask(masks, m_style); + result.addPixmap(masksToIcon(masks, combined_mask, m_style)); + + const QColor disabledColor = QColor::fromRgba(0x60a4a6a8); + result.addPixmap(maskToColorAndAlpha(combined_mask, disabledColor), QIcon::Disabled); + } + return result; + } +} + +QPixmap Icon::pixmap() const +{ + if (isEmpty()) { + return QPixmap(); + } else if (m_style == None) { + return combinedPlainPixmaps(*this); + } else { + const MasksAndColors masks = + masksAndColors(*this, qRound(qApp->devicePixelRatio())); + const QPixmap combined_mask = combinedMask(masks, m_style); + return masksToIcon(masks, combined_mask, m_style); + } +} + +QString Icon::imageFileName() const +{ + return first().first; +} + +QIcon Icon::sideBarIcon(const Icon &classic, const Icon &flat) +{ + return flat.icon(); +} + +QIcon Icon::modeIcon(const Icon &classic, const Icon &flat, const Icon &flatActive) +{ + QIcon result = sideBarIcon(classic, flat); + result.addPixmap(flatActive.pixmap(), QIcon::Active); + return result; +} + +QIcon Icon::combinedIcon(const QList &icons) +{ + QIcon result; + QWindow *window = QApplication::allWidgets().first()->windowHandle(); + for (const QIcon &icon: icons) + for (const QIcon::Mode mode: {QIcon::Disabled, QIcon::Normal}) + for (const QSize &size: icon.availableSizes(mode)) + result.addPixmap(icon.pixmap(window, size, mode), mode); + return result; +} + +} } diff --git a/visualization/motion_planning_tasks/utils/icon.h b/visualization/motion_planning_tasks/utils/icon.h new file mode 100644 index 00000000..547a72be --- /dev/null +++ b/visualization/motion_planning_tasks/utils/icon.h @@ -0,0 +1,85 @@ +/**************************************************************************** +** +** Copyright (C) 2016 The Qt Company Ltd. +** Contact: https://www.qt.io/licensing/ +** +** This file is adapted from Qt Creator (replacing theme stuff by QColor) +** +** Commercial License Usage +** Licensees holding valid commercial Qt licenses may use this file in +** accordance with the commercial license agreement provided with the +** Software or, alternatively, in accordance with the terms contained in +** a written agreement between you and The Qt Company. For licensing terms +** and conditions see https://www.qt.io/terms-conditions. For further +** information use the contact form at https://www.qt.io/contact-us. +** +** GNU General Public License Usage +** Alternatively, this file may be used under the terms of the GNU +** General Public License version 3 as published by the Free Software +** Foundation with exceptions as appearing in the file LICENSE.GPL3-EXCEPT +** included in the packaging of this file. Please review the following +** information to ensure the GNU General Public License requirements will +** be met: https://www.gnu.org/licenses/gpl-3.0.html. +** +****************************************************************************/ + +#pragma once + +#include +#include + +class QColor; +class QIcon; +class QPixmap; +class QString; + +namespace moveit_rviz_plugin { namespace utils { + +typedef QPair IconMaskAndColor; + +// Returns a recolored icon with shadow and custom disabled state for a +// series of grayscalemask|Theme::Color mask pairs +class Icon : public QVector +{ +public: + enum IconStyleOption { + None = 0, + Tint = 1, + DropShadow = 2, + PunchEdges = 4, + + ToolBarStyle = Tint | DropShadow | PunchEdges, + MenuTintedStyle = Tint | PunchEdges + }; + + Q_DECLARE_FLAGS(IconStyleOptions, IconStyleOption) + + Icon(); + Icon(std::initializer_list args, IconStyleOptions style = ToolBarStyle); + Icon(const QString &imageFileName); + Icon(const Icon &other) = default; + + QIcon icon() const; + // Same as icon() but without disabled state. + QPixmap pixmap() const; + + // Try to avoid it. it is just there for special API cases in Qt Creator + // where icons are still defined as filename. + QString imageFileName() const; + + // Returns either the classic or a themed icon depending on + // the current Theme::FlatModeIcons flag. + static QIcon sideBarIcon(const Icon &classic, const Icon &flat); + // Like sideBarIcon plus added action mode for the flat icon + static QIcon modeIcon(const Icon &classic, const Icon &flat, const Icon &flatActive); + + // Combined icon pixmaps in Normal and Disabled states from several QIcons + static QIcon combinedIcon(const QList &icons); + +private: + IconStyleOptions m_style = None; +}; + +} } + +Q_DECLARE_OPERATORS_FOR_FLAGS(moveit_rviz_plugin::utils::Icon::IconStyleOptions) From 3ce0bc7dee8c44db20431185880753ef14f4ce2b Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 25 Feb 2018 23:49:42 +0100 Subject: [PATCH 07/13] MetaTaskListModel: don't remove top-level items --- .../motion_planning_tasks/src/meta_task_list_model.cpp | 7 +++++++ .../motion_planning_tasks/src/meta_task_list_model.h | 3 ++- visualization/motion_planning_tasks/src/task_panel.cpp | 4 ++-- 3 files changed, 11 insertions(+), 3 deletions(-) diff --git a/visualization/motion_planning_tasks/src/meta_task_list_model.cpp b/visualization/motion_planning_tasks/src/meta_task_list_model.cpp index 12d6d460..582f6bcf 100644 --- a/visualization/motion_planning_tasks/src/meta_task_list_model.cpp +++ b/visualization/motion_planning_tasks/src/meta_task_list_model.cpp @@ -95,6 +95,13 @@ bool MetaTaskListModel::setData(const QModelIndex &index, const QVariant &value, return result; } +bool MetaTaskListModel::removeRows(int row, int count, const QModelIndex &parent) +{ + if (!parent.isValid()) // forbid removal of top-level items (displays) + return false; + TreeMergeProxyModel::removeRows(row, count, parent); +} + std::pair MetaTaskListModel::getTaskListModel(const QModelIndex &index) const { diff --git a/visualization/motion_planning_tasks/src/meta_task_list_model.h b/visualization/motion_planning_tasks/src/meta_task_list_model.h index 91188289..4ee96143 100644 --- a/visualization/motion_planning_tasks/src/meta_task_list_model.h +++ b/visualization/motion_planning_tasks/src/meta_task_list_model.h @@ -69,7 +69,7 @@ class MetaTaskListModel : public utils::TreeMergeProxyModel { // hide this, as we want to offer another API using utils::TreeMergeProxyModel::insertModel; - private Q_SLOTS: +private Q_SLOTS: void onRowsRemoved(const QModelIndex &parent, int first, int last); void onDisplayNameChanged(const QString &name); @@ -80,6 +80,7 @@ public: bool insertModel(TaskListModel* model, TaskDisplay* display); bool setData(const QModelIndex &index, const QVariant &value, int role) override; + bool removeRows(int row, int count, const QModelIndex &parent) override; /// retrieve TaskListModel and TaskDisplay corresponding to given index std::pair getTaskListModel(const QModelIndex &index) const; diff --git a/visualization/motion_planning_tasks/src/task_panel.cpp b/visualization/motion_planning_tasks/src/task_panel.cpp index 2a008bda..6fba1a6a 100644 --- a/visualization/motion_planning_tasks/src/task_panel.cpp +++ b/visualization/motion_planning_tasks/src/task_panel.cpp @@ -245,8 +245,8 @@ void TaskView::onCurrentStageChanged(const QModelIndex ¤t, const QModelInd // adding task is allowed on top-level items and sub-top-level items d_ptr->actionAddLocalTask->setEnabled(current.isValid() && (!current.parent().isValid() || !current.parent().parent().isValid())); - // removing stuff is allowed if there is any selection / any curren item - d_ptr->actionRemoveTaskTreeRows->setEnabled(current.isValid()); + // removing stuff is allowed any valid selection except top-level items + d_ptr->actionRemoveTaskTreeRows->setEnabled(current.isValid() && current.parent().isValid()); BaseTaskModel *task; QModelIndex task_index; From 0fd5f3c1b34cd710bceaa70ebe17d0fbb0866ccd Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 26 Feb 2018 19:17:19 +0100 Subject: [PATCH 08/13] auto-expand newly inserted task-pipelines --- .../motion_planning_tasks/src/task_panel.cpp | 30 +++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/visualization/motion_planning_tasks/src/task_panel.cpp b/visualization/motion_planning_tasks/src/task_panel.cpp index 6fba1a6a..9b254fd4 100644 --- a/visualization/motion_planning_tasks/src/task_panel.cpp +++ b/visualization/motion_planning_tasks/src/task_panel.cpp @@ -156,6 +156,21 @@ void TaskPanel::showStageDockWidget() } +// expand all children up to given depth +void setExpanded(QTreeView *view, const QModelIndex &index, bool expand, int depth = -1) +{ + if (!index.isValid()) + return; + + // recursively expand all children + if (depth != 0) { + for (int row = 0, rows = index.model()->rowCount(index); row < rows; ++row) + setExpanded(view, index.child(row, 0), expand, depth-1); + } + + view->setExpanded(index, expand); +} + TaskViewPrivate::TaskViewPrivate(TaskView *q_ptr) : q_ptr(q_ptr) { @@ -165,6 +180,21 @@ TaskViewPrivate::TaskViewPrivate(TaskView *q_ptr) StageFactoryPtr factory = getStageFactory(); if (factory) meta_model->setMimeTypes( { factory->mimeType() } ); tasks_view->setModel(meta_model); + // auto-expand newly-inserted top-level items + QObject::connect(meta_model, &QAbstractItemModel::rowsInserted, [this](const QModelIndex &parent, int first, int last){ + if (parent.isValid() && !parent.parent().isValid()) { + for (int row = first; row <= last; ++row) { + QModelIndex child = parent.child(row, 0); + // expand inserted items + setExpanded(tasks_view, child, true); + // collapse up to first level + setExpanded(tasks_view, child, false, 1); + // expand inserted item + setExpanded(tasks_view, child, true, 0); + } + tasks_view->setExpanded(parent, true); // expand parent group item + } + }); tasks_view->setSelectionMode(QAbstractItemView::ExtendedSelection); tasks_view->setAcceptDrops(true); From 3d07e345224ec46b19ff0630751c0c7bc747b9f9 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 25 Feb 2018 23:49:06 +0100 Subject: [PATCH 09/13] highlight currently active solution part --- .../src/local_task_model.cpp | 6 ++++ .../src/local_task_model.h | 2 ++ .../src/meta_task_list_model.cpp | 2 +- .../src/remote_task_model.cpp | 6 ++++ .../src/remote_task_model.h | 1 + .../src/task_display.cpp | 2 ++ .../src/task_list_model.cpp | 29 +++++++++++++++++++ .../src/task_list_model.h | 12 ++++++++ .../utils/flat_merge_proxy_model.cpp | 11 +++++++ .../utils/flat_merge_proxy_model.h | 3 ++ .../visualization_tools/display_solution.h | 5 ++++ .../task_solution_visualization.h | 3 ++ .../src/display_solution.cpp | 6 ++++ .../src/task_solution_visualization.cpp | 6 +++- 14 files changed, 92 insertions(+), 2 deletions(-) diff --git a/visualization/motion_planning_tasks/src/local_task_model.cpp b/visualization/motion_planning_tasks/src/local_task_model.cpp index b756d1c6..5563739c 100644 --- a/visualization/motion_planning_tasks/src/local_task_model.cpp +++ b/visualization/motion_planning_tasks/src/local_task_model.cpp @@ -221,6 +221,12 @@ bool LocalTaskModel::dropMimeData(const QMimeData *mime, Qt::DropAction action, return true; } +QModelIndex LocalTaskModel::indexFromStageId(size_t id) const +{ + // TODO implement + return QModelIndex(); +} + QAbstractItemModel *LocalTaskModel::getSolutionModel(const QModelIndex &index) { // TODO implement diff --git a/visualization/motion_planning_tasks/src/local_task_model.h b/visualization/motion_planning_tasks/src/local_task_model.h index b9873a80..dbedd86f 100644 --- a/visualization/motion_planning_tasks/src/local_task_model.h +++ b/visualization/motion_planning_tasks/src/local_task_model.h @@ -72,6 +72,8 @@ public: void setStageFactory(const StageFactoryPtr &factory) override; bool dropMimeData(const QMimeData *data, Qt::DropAction action, int row, int column, const QModelIndex &parent) override; + QModelIndex indexFromStageId(size_t id) const override; + QAbstractItemModel* getSolutionModel(const QModelIndex& index) override; DisplaySolutionPtr getSolution(const QModelIndex &index) override; diff --git a/visualization/motion_planning_tasks/src/meta_task_list_model.cpp b/visualization/motion_planning_tasks/src/meta_task_list_model.cpp index 582f6bcf..3584ee52 100644 --- a/visualization/motion_planning_tasks/src/meta_task_list_model.cpp +++ b/visualization/motion_planning_tasks/src/meta_task_list_model.cpp @@ -99,7 +99,7 @@ bool MetaTaskListModel::removeRows(int row, int count, const QModelIndex &parent { if (!parent.isValid()) // forbid removal of top-level items (displays) return false; - TreeMergeProxyModel::removeRows(row, count, parent); + return TreeMergeProxyModel::removeRows(row, count, parent); } std::pair diff --git a/visualization/motion_planning_tasks/src/remote_task_model.cpp b/visualization/motion_planning_tasks/src/remote_task_model.cpp index b6e07f98..666a6114 100644 --- a/visualization/motion_planning_tasks/src/remote_task_model.cpp +++ b/visualization/motion_planning_tasks/src/remote_task_model.cpp @@ -220,6 +220,12 @@ bool RemoteTaskModel::setData(const QModelIndex &index, const QVariant &value, i return true; } +QModelIndex RemoteTaskModel::indexFromStageId(size_t id) const +{ + Node *n = node(id); + return n ? index(n) : QModelIndex(); +} + void RemoteTaskModel::processStageDescriptions(const moveit_task_constructor_msgs::TaskDescription::_stages_type &msg) { // iterate over descriptions and create new / update existing nodes where needed diff --git a/visualization/motion_planning_tasks/src/remote_task_model.h b/visualization/motion_planning_tasks/src/remote_task_model.h index dd770cec..2a07dec4 100644 --- a/visualization/motion_planning_tasks/src/remote_task_model.h +++ b/visualization/motion_planning_tasks/src/remote_task_model.h @@ -81,6 +81,7 @@ public: QVariant data(const QModelIndex &index, int role = Qt::DisplayRole) const override; bool setData(const QModelIndex &index, const QVariant &value, int role = Qt::EditRole) override; + QModelIndex indexFromStageId(size_t id) const override; void processStageDescriptions(const moveit_task_constructor_msgs::TaskDescription::_stages_type &msg); void processStageStatistics(const moveit_task_constructor_msgs::TaskStatistics::_stages_type &msg); DisplaySolutionPtr processSolutionMessage(const moveit_task_constructor_msgs::Solution &msg); diff --git a/visualization/motion_planning_tasks/src/task_display.cpp b/visualization/motion_planning_tasks/src/task_display.cpp index ced3f388..92da2f82 100644 --- a/visualization/motion_planning_tasks/src/task_display.cpp +++ b/visualization/motion_planning_tasks/src/task_display.cpp @@ -83,6 +83,8 @@ TaskDisplay::TaskDisplay() : Display() this, SLOT(changedTaskSolutionTopic()), this); trajectory_visual_.reset(new TaskSolutionVisualization(this, this)); + connect(trajectory_visual_.get(), SIGNAL(activeStageChanged(size_t)), + task_list_model_.get(), SLOT(highlightStage(size_t))); marker_visual_ = new MarkerVisualizationProperty("Markers", this); diff --git a/visualization/motion_planning_tasks/src/task_list_model.cpp b/visualization/motion_planning_tasks/src/task_list_model.cpp index f95268d4..e8ac2a52 100644 --- a/visualization/motion_planning_tasks/src/task_list_model.cpp +++ b/visualization/motion_planning_tasks/src/task_list_model.cpp @@ -212,6 +212,23 @@ Qt::ItemFlags TaskListModel::flags(const QModelIndex &index) const return f; } +void TaskListModel::highlightStage(size_t id) +{ + if (!active_task_model_) return; + QModelIndex old_index = highlighted_row_index_; + QModelIndex new_index = active_task_model_->indexFromStageId(id); + if (new_index.isValid()) + highlighted_row_index_ = new_index = mapFromSource(new_index); + else + highlighted_row_index_ = QModelIndex(); + + if (old_index == new_index) return; + if (old_index.isValid()) + Q_EMIT dataChanged(old_index, old_index.sibling(old_index.row(), columnCount()-1)); + if (new_index.isValid()) + Q_EMIT dataChanged(new_index, new_index.sibling(new_index.row(), columnCount()-1)); +} + QVariant TaskListModel::headerData(int section, Qt::Orientation orientation, int role) const { if (orientation == Qt::Horizontal) @@ -220,6 +237,15 @@ QVariant TaskListModel::headerData(int section, Qt::Orientation orientation, int return QAbstractItemModel::headerData(section, orientation, role); } +QVariant TaskListModel::data(const QModelIndex &index, int role) const +{ + if (role == Qt::BackgroundRole && index.isValid() && + index.row() == highlighted_row_index_.row() && + index.parent() == highlighted_row_index_.parent()) + return QColor(Qt::yellow); + return FlatMergeProxyModel::data(index, role); +} + // process a task description message: // update existing RemoteTask, create a new one, or (if msg.stages is empty) delete an existing one void TaskListModel::processTaskDescriptionMessage(const std::string& id, @@ -247,6 +273,9 @@ void TaskListModel::processTaskDescriptionMessage(const std::string& id, // the model is managed by this instance via Qt's parent-child mechanism remote_task = new RemoteTaskModel(scene_, this); remote_task->setSolutionClient(get_solution_client_); + + // HACK: always use the last created model as active + active_task_model_ = remote_task; } if (!remote_task) return; // task is not in use anymore diff --git a/visualization/motion_planning_tasks/src/task_list_model.h b/visualization/motion_planning_tasks/src/task_list_model.h index 753e5f4a..3a51b40c 100644 --- a/visualization/motion_planning_tasks/src/task_list_model.h +++ b/visualization/motion_planning_tasks/src/task_list_model.h @@ -48,6 +48,7 @@ #include #include #include +#include namespace ros { class ServiceClient; } namespace rviz { class PropertyTreeModel; } @@ -86,6 +87,9 @@ public: unsigned int taskFlags() const { return flags_; } static QVariant flowIcon(moveit::task_constructor::InterfaceFlags f); + /// retrieve model index associated with given stage id + virtual QModelIndex indexFromStageId(size_t id) const = 0; + /// get solution model for given stage index virtual QAbstractItemModel* getSolutionModel(const QModelIndex& index) = 0; /// get solution for given solution index @@ -119,6 +123,9 @@ class TaskListModel : public utils::FlatMergeProxyModel { // factory used to create stages StageFactoryPtr stage_factory_; + QPointer active_task_model_; + QPersistentModelIndex highlighted_row_index_; + void onRemoveModel(QAbstractItemModel *model) override; public: @@ -127,10 +134,12 @@ public: void setScene(const planning_scene::PlanningSceneConstPtr& scene); void setSolutionClient(ros::ServiceClient* client); + void setActiveTaskModel(BaseTaskModel* model) { active_task_model_ = model; } int columnCount(const QModelIndex &parent = QModelIndex()) const override { return 3; } static QVariant horizontalHeader(int column, int role); QVariant headerData(int section, Qt::Orientation orientation, int role) const override; + QVariant data(const QModelIndex &index, int role) const override; /// process an incoming task description message - only call in Qt's main loop void processTaskDescriptionMessage(const std::string &id, const moveit_task_constructor_msgs::TaskDescription &msg); @@ -153,6 +162,9 @@ public: bool dropMimeData(const QMimeData *mime, Qt::DropAction action, int row, int column, const QModelIndex &parent) override; Qt::DropActions supportedDropActions() const override; Qt::ItemFlags flags(const QModelIndex &index) const override; + +protected Q_SLOTS: + void highlightStage(size_t id); }; diff --git a/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.cpp b/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.cpp index 614b6fb2..b6d818ee 100644 --- a/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.cpp +++ b/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.cpp @@ -261,6 +261,17 @@ QModelIndex FlatMergeProxyModel::parent(const QModelIndex &child) const return d_ptr->mapFromSource(src_parent, data); } +QModelIndex FlatMergeProxyModel::mapToSource(const QModelIndex &proxy_index) const +{ + FlatMergeProxyModelPrivate::ModelData* data; + return d_ptr->mapToSource(proxy_index, data); +} + +QModelIndex FlatMergeProxyModel::mapFromSource(const QModelIndex &src_index) const +{ + return d_ptr->mapFromSource(src_index, nullptr); +} + Qt::ItemFlags FlatMergeProxyModel::flags(const QModelIndex &index) const { if (!index.isValid()) diff --git a/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.h b/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.h index 3c5f7ab5..4fd5b2e4 100644 --- a/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.h +++ b/visualization/motion_planning_tasks/utils/flat_merge_proxy_model.h @@ -75,6 +75,9 @@ public: QModelIndex index(int row, int column, const QModelIndex &parent = QModelIndex()) const override; QModelIndex parent(const QModelIndex &index) const override; + QModelIndex mapToSource(const QModelIndex &proxy_index) const; + QModelIndex mapFromSource(const QModelIndex &src_index) const; + Qt::ItemFlags flags(const QModelIndex & index) const override; QVariant data(const QModelIndex &index, int role = Qt::DisplayRole) const override; bool setData(const QModelIndex &index, const QVariant &value, int role = Qt::EditRole) override; diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h b/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h index 17d2c50f..2970a130 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h @@ -76,6 +76,8 @@ class DisplaySolution robot_trajectory::RobotTrajectoryPtr trajectory_; /// optional name of the trajectory std::string name_; + /// id of creating stage + uint32_t creator_id_; /// rviz markers MarkerVisualizationPtr markers_; }; @@ -91,6 +93,7 @@ public: size_t getWayPointCount() const { return steps_; } bool empty() const { return steps_ == 0; } + /// pair of trajectory part and way point index within part typedef std::pair IndexPair; IndexPair indexPair(size_t index) const; @@ -113,6 +116,8 @@ public: const std::string& name(size_t index) const { return name(indexPair(index)); } + uint32_t creatorId(const IndexPair& idx_pair) const; + const MarkerVisualizationPtr markers(const IndexPair& idx_pair) const; const MarkerVisualizationPtr markers(size_t index) const { return markers(indexPair(index)); diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h index c3a3718a..f4f70cd1 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h @@ -127,6 +127,9 @@ private Q_SLOTS: void changedSceneEnabled(); void renderCurrentScene(); +Q_SIGNALS: + void activeStageChanged(size_t); + protected: void setVisibility(); // set visibility of main scene node void setVisibility(Ogre::SceneNode* node, Ogre::SceneNode* parent, bool visible); diff --git a/visualization/visualization_tools/src/display_solution.cpp b/visualization/visualization_tools/src/display_solution.cpp index d0280884..7b214c7c 100644 --- a/visualization/visualization_tools/src/display_solution.cpp +++ b/visualization/visualization_tools/src/display_solution.cpp @@ -84,6 +84,11 @@ const std::string &DisplaySolution::name(const IndexPair &idx_pair) const return data_[idx_pair.first].name_; } +uint32_t DisplaySolution::creatorId(const DisplaySolution::IndexPair &idx_pair) const +{ + return data_[idx_pair.first].creator_id_; +} + const MarkerVisualizationPtr DisplaySolution::markers(const DisplaySolution::IndexPair &idx_pair) const { return data_[idx_pair.first].markers_; @@ -112,6 +117,7 @@ void DisplaySolution::setFromMessage(const planning_scene::PlanningScenePtr& sta data_[i].trajectory_.reset(new robot_trajectory::RobotTrajectory(ref_scene->getRobotModel(), "")); data_[i].trajectory_->setRobotTrajectoryMsg(ref_scene->getCurrentState(), sub.trajectory); data_[i].name_ = sub.name; + data_[i].creator_id_ = sub.stage_id; steps_ += data_[i].trajectory_->getWayPointCount(); ref_scene->setPlanningSceneDiffMsg(sub.scene_diff); diff --git a/visualization/visualization_tools/src/task_solution_visualization.cpp b/visualization/visualization_tools/src/task_solution_visualization.cpp index f2aebdbd..5560ea7a 100644 --- a/visualization/visualization_tools/src/task_solution_visualization.cpp +++ b/visualization/visualization_tools/src/task_solution_visualization.cpp @@ -446,6 +446,7 @@ void TaskSolutionVisualization::renderWayPoint(size_t index, int previous_index) moveit::core::RobotStateConstPtr robot_state; planning_scene::PlanningSceneConstPtr scene; if (index == displaying_solution_->getWayPointCount()) { + // render last state scene = displaying_solution_->scene(index); renderPlanningScene (scene); robot_state.reset(new moveit::core::RobotState(scene->getCurrentState())); @@ -454,8 +455,11 @@ void TaskSolutionVisualization::renderWayPoint(size_t index, int previous_index) scene = displaying_solution_->scene(idx_pair); if (previous_index < 0 || - 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 renderPlanningScene (scene); + Q_EMIT activeStageChanged(displaying_solution_->creatorId(idx_pair)); + } robot_state = displaying_solution_->getWayPointPtr(idx_pair); } From 2451d0c4d5cb5e66c5d939ecec236256a6f97ae2 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 28 Feb 2018 00:52:14 +0100 Subject: [PATCH 10/13] save panel settings --- .../src/task_display.cpp | 10 ++++ .../motion_planning_tasks/src/task_display.h | 6 ++- .../motion_planning_tasks/src/task_panel.cpp | 53 +++++++++++++++++++ .../motion_planning_tasks/src/task_panel.h | 3 ++ 4 files changed, 70 insertions(+), 2 deletions(-) diff --git a/visualization/motion_planning_tasks/src/task_display.cpp b/visualization/motion_planning_tasks/src/task_display.cpp index 92da2f82..abc0746f 100644 --- a/visualization/motion_planning_tasks/src/task_display.cpp +++ b/visualization/motion_planning_tasks/src/task_display.cpp @@ -146,6 +146,16 @@ void TaskDisplay::reset() trajectory_visual_->reset(); } +void TaskDisplay::save(rviz::Config config) const +{ + Display::save(config); +} + +void TaskDisplay::load(const rviz::Config &config) +{ + Display::load(config); +} + void TaskDisplay::onEnable() { Display::onEnable(); diff --git a/visualization/motion_planning_tasks/src/task_display.h b/visualization/motion_planning_tasks/src/task_display.h index d9d34ffa..4a834f68 100644 --- a/visualization/motion_planning_tasks/src/task_display.h +++ b/visualization/motion_planning_tasks/src/task_display.h @@ -77,8 +77,10 @@ public: void loadRobotModel(); - virtual void update(float wall_dt, float ros_dt); - virtual void reset(); + void update(float wall_dt, float ros_dt) override; + void reset() override; + void save(rviz::Config config) const; + void load(const rviz::Config &config); // overrides from Display virtual void onInitialize(); diff --git a/visualization/motion_planning_tasks/src/task_panel.cpp b/visualization/motion_planning_tasks/src/task_panel.cpp index 9b254fd4..b164be26 100644 --- a/visualization/motion_planning_tasks/src/task_panel.cpp +++ b/visualization/motion_planning_tasks/src/task_panel.cpp @@ -142,11 +142,13 @@ void TaskPanel::onInitialize() void TaskPanel::save(rviz::Config config) const { rviz::Panel::save(config); + d_ptr->tasks_widget->save(config.mapMakeChild("tasks_view")); } void TaskPanel::load(const rviz::Config& config) { rviz::Panel::load(config); + d_ptr->tasks_widget->load(config.mapGetChild("tasks_view")); } void TaskPanel::showStageDockWidget() @@ -247,6 +249,57 @@ TaskView::TaskView(QWidget *parent) onCurrentStageChanged(d->tasks_view->currentIndex(), QModelIndex()); } +void TaskView::save(rviz::Config config) +{ + auto writeSplitterSizes = [&config](QSplitter* splitter, const QString& key) { + rviz::Config group = config.mapMakeChild(key); + for (int s : splitter->sizes()) { + rviz::Config item = group.listAppendNew(); + item.setValue(s); + } + }; + writeSplitterSizes(d_ptr->tasks_property_splitter, "property_splitter"); + writeSplitterSizes(d_ptr->tasks_solutions_splitter, "solutions_splitter"); + + auto writeColumnSizes = [&config](QTreeView* view, const QString& key) { + rviz::Config group = config.mapMakeChild(key); + for (int c = 0, end = view->header()->count(); c != end; ++c) { + rviz::Config item = group.listAppendNew(); + item.setValue(view->columnWidth(c)); + } + }; + writeColumnSizes(d_ptr->tasks_view, "tasks_view_columns"); + writeColumnSizes(d_ptr->solutions_view, "solutions_view_columns"); +} + +void TaskView::load(const rviz::Config &config) +{ + if (!config.isValid()) return; + + auto readSizes = [&config](const QString& key) { + rviz::Config group = config.mapGetChild(key); + QList sizes, empty; + for (int i = 0; i < group.listLength(); ++i) { + rviz::Config item = group.listChildAt(i); + if (item.getType() != rviz::Config::Value) return empty; + QVariant value = item.getValue(); + bool ok = false; + int int_value = value.toInt(&ok); + if (!ok) return empty; + sizes << int_value; + } + return sizes; + }; + d_ptr->tasks_property_splitter->setSizes(readSizes("property_splitter")); + d_ptr->tasks_solutions_splitter->setSizes(readSizes("solutions_splitter")); + + int column = 0; + for (int w : readSizes("tasks_view_columns")) + d_ptr->tasks_view->setColumnWidth(++column, w); + for (int w : readSizes("solutions_view_columns")) + d_ptr->tasks_view->setColumnWidth(++column, w); +} + void TaskView::addTask() { QModelIndex current = d_ptr->tasks_view->currentIndex(); diff --git a/visualization/motion_planning_tasks/src/task_panel.h b/visualization/motion_planning_tasks/src/task_panel.h index 845712f9..5323e1ec 100644 --- a/visualization/motion_planning_tasks/src/task_panel.h +++ b/visualization/motion_planning_tasks/src/task_panel.h @@ -97,6 +97,9 @@ class TaskView : public QWidget { public: TaskView(QWidget* parent = 0); + void save(rviz::Config config); + void load(const rviz::Config& config); + public Q_SLOTS: void addTask(); From cb38a87a7346b960bf44de1c0a8f8bcc460fcf2a Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 13 Mar 2018 02:16:35 +0100 Subject: [PATCH 11/13] CartesianPath: return a partial trajectory in case of failure --- core/src/solvers/cartesian_path.cpp | 7 ++++--- core/src/stages/move_to.cpp | 4 ++-- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp index 163d0af2..f0c98d01 100644 --- a/core/src/solvers/cartesian_path.cpp +++ b/core/src/solvers/cartesian_path.cpp @@ -97,7 +97,8 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr from, props.get("step_size"), props.get("jump_threshold"), isValid); - if (achieved_fraction >= props.get("min_fraction")) { + + if (!trajectory.empty()) { result.reset(new robot_trajectory::RobotTrajectory(sandbox_scene->getRobotModel(), jmg)); for (const auto& waypoint : trajectory) result->addSuffixWayPoint(waypoint, 0.0); @@ -106,9 +107,9 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr from, timing.computeTimeStamps(*result, props.get("max_velocity_scaling_factor"), props.get("max_acceleration_scaling_factor")); - return true; } - return false; + + return achieved_fraction >= props.get("min_fraction"); } } } } diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index 6b547605..79662370 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -86,7 +86,7 @@ void MoveTo::init(const moveit::core::RobotModelConstPtr& robot_model) } bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene, - SubTrajectory &trajectory, Direction dir) { + SubTrajectory &solution, Direction dir) { scene = state.scene()->diff(); assert(scene->getRobotModel()); @@ -163,7 +163,7 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP if (success || (robot_trajectory && storeFailures())) { scene->setCurrentState(robot_trajectory->getLastWayPoint()); if (dir == BACKWARD) robot_trajectory->reverse(); - trajectory.setTrajectory(robot_trajectory); + solution.setTrajectory(robot_trajectory); } return success; From a4fed6a18ad000affe4dbb8655c79031beeaadeb Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 13 Mar 2018 02:47:44 +0100 Subject: [PATCH 12/13] MoveTo Cartesian: create marker frame at current and goal pose --- core/src/stages/move_to.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index 79662370..46f3f0af 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -38,6 +38,7 @@ #include #include +#include #include namespace moveit { namespace task_constructor { namespace stages { @@ -156,6 +157,16 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP target_eigen.translation() = target_point; } + // frame at current link pose + geometry_msgs::PoseStamped pose_msg; + pose_msg.header.frame_id = scene->getPlanningFrame(); + tf::poseEigenToMsg(scene->getCurrentState().getGlobalLinkTransform(link_name), pose_msg.pose); + rviz_marker_tools::appendFrame(solution.markers(), pose_msg, 0.1, "ik frame"); + + // frame at target pose + tf::poseEigenToMsg(target_eigen, pose_msg.pose); + rviz_marker_tools::appendFrame(solution.markers(), pose_msg, 0.1, "ik frame"); + success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory); } From 56c2ef3fbbd509b7a0bf49b8a629aab3ccc4aac9 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 12 Mar 2018 13:54:03 +0100 Subject: [PATCH 13/13] save settings for solution sorting --- .../motion_planning_tasks/src/task_panel.cpp | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/visualization/motion_planning_tasks/src/task_panel.cpp b/visualization/motion_planning_tasks/src/task_panel.cpp index b164be26..65c26fce 100644 --- a/visualization/motion_planning_tasks/src/task_panel.cpp +++ b/visualization/motion_planning_tasks/src/task_panel.cpp @@ -270,6 +270,11 @@ void TaskView::save(rviz::Config config) }; writeColumnSizes(d_ptr->tasks_view, "tasks_view_columns"); writeColumnSizes(d_ptr->solutions_view, "solutions_view_columns"); + + const QHeaderView *view = d_ptr->solutions_view->header(); + rviz::Config group = config.mapMakeChild("solution_sorting"); + group.mapSetValue("column", view->sortIndicatorSection()); + group.mapSetValue("order", view->sortIndicatorOrder()); } void TaskView::load(const rviz::Config &config) @@ -296,8 +301,15 @@ void TaskView::load(const rviz::Config &config) int column = 0; for (int w : readSizes("tasks_view_columns")) d_ptr->tasks_view->setColumnWidth(++column, w); + column = 0; for (int w : readSizes("solutions_view_columns")) d_ptr->tasks_view->setColumnWidth(++column, w); + + QTreeView *view = d_ptr->solutions_view; + rviz::Config group = config.mapGetChild("solution_sorting"); + int order; + if (group.mapGetInt("column", &column) && group.mapGetInt("order", &order)) + view->sortByColumn(column, static_cast(order)); } void TaskView::addTask() @@ -339,10 +351,13 @@ void TaskView::onCurrentStageChanged(const QModelIndex ¤t, const QModelInd // update the SolutionModel QTreeView *view = d_ptr->solutions_view; + int sort_column = view->header()->sortIndicatorSection(); + Qt::SortOrder sort_order = view->header()->sortIndicatorOrder(); + QItemSelectionModel *sm = view->selectionModel(); QAbstractItemModel *m = task ? task->getSolutionModel(task_index) : nullptr; - view->sortByColumn(-1); view->setModel(m); + view->sortByColumn(sort_column, sort_order); if (sm) delete sm; // we don't store the selection model sm = view->selectionModel();