Merge branches 'bug-fixes', 'gui' and 'pick-stage'

This commit is contained in:
Robert Haschke 2018-03-24 07:32:20 +01:00
commit 5a55a5949b
46 changed files with 1102 additions and 100 deletions

View File

@ -105,7 +105,7 @@ int main(int argc, char** argv){
{
auto gengrasp = std::make_unique<stages::GenerateGraspPose>("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()),

View File

@ -1,11 +1,8 @@
#include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/stages/current_state.h>
#include <moveit/task_constructor/stages/gripper.h>
#include <moveit/task_constructor/stages/move.h>
#include <moveit/task_constructor/stages/generate_grasp_pose.h>
#include <moveit/task_constructor/stages/cartesian_position_motion.h>
#include <moveit/task_constructor/stages/compute_ik.h>
#include <moveit/task_constructor/stages/simple_grasp.h>
#include <moveit/task_constructor/stages/pick.h>
#include <ros/ros.h>
#include <moveit_msgs/CollisionObject.h>
@ -44,78 +41,31 @@ int main(int argc, char** argv){
Task t;
Stage* initial_stage = nullptr;
auto initial = std::make_unique<stages::CurrentState>("current state");
initial_stage = initial.get();
t.add(std::move(initial));
{
auto initial = std::make_unique<stages::CurrentState>("current state");
initial_stage = initial.get();
t.add(std::move(initial));
}
auto grasp_generator = std::make_unique<stages::SimpleGrasp>();
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<stages::Gripper>("open gripper");
move->setEndEffector("left_gripper");
move->setTo("open");
t.add(std::move(move));
}
auto pick = std::make_unique<stages::Pick>(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<stages::Move>("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<stages::CartesianPositionMotion>("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<stages::GenerateGraspPose>("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<stages::ComputeIK>("compute ik", std::move(gengrasp));
ik->setEndEffector("left_gripper");
t.add(std::move(ik));
}
{
auto move= std::make_unique<stages::Gripper>("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<stages::CartesianPositionMotion>("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();

View File

@ -84,7 +84,7 @@ int main(int argc, char** argv){
{
auto gengrasp = std::make_unique<stages::GenerateGraspPose>("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);

View File

@ -39,6 +39,7 @@
#pragma once
#include <moveit/task_constructor/stage.h>
#include <geometry_msgs/TransformStamped.h>
#include <Eigen/Geometry>
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);

View File

@ -60,7 +60,7 @@ namespace moveit { namespace task_constructor { namespace stages {
class ModifyPlanningScene : public PropagatingEitherWay {
public:
typedef std::vector<std::string> Names;
typedef std::function<void(planning_scene::PlanningScenePtr scene, PropertyMap& properties)> ApplyCallback;
typedef std::function<void(const planning_scene::PlanningScenePtr& scene, const PropertyMap& properties)> ApplyCallback;
ModifyPlanningScene(const std::string& name = "modify planning scene");
bool computeForward(const InterfaceState& from) override;

View File

@ -0,0 +1,92 @@
/*********************************************************************
* 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 <moveit/macros/class_forward.h>
#include <moveit/task_constructor/container.h>
#include <geometry_msgs/TwistStamped.h>
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.
*
* 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 {
solvers::CartesianPathPtr cartesian_solver_;
solvers::PipelinePlannerPtr pipeline_solver_;
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<std::string>("eef", eef);
}
void setObject(const std::string& object) {
properties().set<std::string>("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,
double min_distance, double max_distance);
};
} } }

View File

@ -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 <moveit/task_constructor/container.h>
#include <moveit/macros/class_forward.h>
#include <geometry_msgs/TransformStamped.h>
#include <Eigen/Geometry>
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<std::string>("eef", eef);
}
void setObject(const std::string& object) {
properties().set<std::string>("object", object);
}
void setPreGraspPose(const std::string& pregrasp) {
properties().set<std::string>("pregrasp", pregrasp);
}
void setGraspPose(const std::string& grasp) {
properties().set<std::string>("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 <typename T>
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);
}
};
} } }

View File

@ -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)

View File

@ -97,7 +97,8 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr from,
props.get<double>("step_size"),
props.get<double>("jump_threshold"),
isValid);
if (achieved_fraction >= props.get<double>("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<double>("max_velocity_scaling_factor"),
props.get<double>("max_acceleration_scaling_factor"));
return true;
}
return false;
return achieved_fraction >= props.get<double>("min_fraction");
}
} } }

View File

@ -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

View File

@ -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<double>::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<double>::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()));

View File

@ -54,7 +54,7 @@ bool FixedState::canCompute() const{
bool FixedState::compute() {
spawn(InterfaceState(scene_), 0.0);
return ran_= true;
return ran_ = true;
}
} } }

View File

@ -50,8 +50,8 @@ GenerateGraspPose::GenerateGraspPose(const std::string& name)
: MonitoringGenerator(name)
{
auto& p = properties();
p.declare<std::string>("eef", "name of end-effector group");
p.declare<std::string>("eef_named_pose");
p.declare<std::string>("eef", "name of end-effector");
p.declare<std::string>("pregrasp", "name of end-effector's pregrasp pose");
p.declare<std::string>("object");
p.declare<geometry_msgs::TransformStamped>("tool_to_grasp_tf", geometry_msgs::TransformStamped(), "transform from robot tool frame to grasp frame");
p.declare<double>("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<std::string>("eef_named_pose");
robot_state.setToDefaultValues(jmg , eef_named_pose);
const std::string& joint_pose = props.get<std::string>("pregrasp");
if(!joint_pose.empty()){
robot_state.setToDefaultValues(jmg , joint_pose);
}
geometry_msgs::TransformStamped tool2grasp_msg = props.get<geometry_msgs::TransformStamped>("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;

View File

@ -38,6 +38,7 @@
#include <moveit/task_constructor/stages/move_to.h>
#include <moveit/planning_scene/planning_scene.h>
#include <rviz_marker_tools/marker_creation.h>
#include <eigen_conversions/eigen_msg.h>
namespace moveit { namespace task_constructor { namespace stages {
@ -86,7 +87,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());
@ -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);
}
@ -163,7 +174,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;

105
core/src/stages/pick.cpp Normal file
View File

@ -0,0 +1,105 @@
#include <moveit/task_constructor/stages/pick.h>
#include <moveit/task_constructor/solvers/cartesian_path.h>
#include <moveit/task_constructor/solvers/pipeline_planner.h>
#include <moveit/task_constructor/container.h>
#include <moveit/task_constructor/stages/move_to.h>
#include <moveit/task_constructor/stages/move_relative.h>
#include <moveit/task_constructor/stages/connect.h>
#include <moveit/task_constructor/stages/modify_planning_scene.h>
#include <moveit/planning_scene/planning_scene.h>
namespace moveit { namespace task_constructor { namespace stages {
Pick::Pick(Stage::pointer&& grasp_stage, const std::string& name)
: SerialContainer(name)
{
PropertyMap& p = properties();
p.declare<std::string>("object", "name of object to grasp");
p.declare<std::string>("eef", "end effector name");
p.declare<std::string>("eef_frame", "name of end effector frame");
// internal properties (cannot be marked as such yet)
p.declare<std::string>("eef_group", "JMG of eef");
p.declare<std::string>("eef_parent_group", "JMG of eef's parent");
cartesian_solver_ = std::make_shared<solvers::CartesianPath>();
pipeline_solver_ = std::make_shared<solvers::PipelinePlanner>();
pipeline_solver_->setTimeout(8.0);
pipeline_solver_->setPlannerId("RRTConnectkConfigDefault");
{
auto move = std::make_unique<MoveTo>("open gripper", pipeline_solver_);
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<Connect>("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<MoveRelative>("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));
}
grasp_stage_ = grasp_stage.get();
grasp_stage->properties().configureInitFrom(Stage::PARENT, {"eef", "object"});
insert(std::move(grasp_stage));
{
auto lift = std::make_unique<MoveRelative>("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));
}
}
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<std::string>("eef");
const moveit::core::JointModelGroup *jmg = robot_model->getEndEffector(eef);
p->set<std::string>("eef_group", jmg->getName());
p->set<std::string>("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);
}
} } }

View File

@ -0,0 +1,135 @@
/*********************************************************************
* 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 <moveit/task_constructor/stages/simple_grasp.h>
#include <moveit/task_constructor/stages/generate_grasp_pose.h>
#include <moveit/task_constructor/stages/compute_ik.h>
#include <moveit/task_constructor/stages/modify_planning_scene.h>
#include <moveit/task_constructor/stages/move_to.h>
#include <moveit/task_constructor/solvers/pipeline_planner.h>
#include <moveit/planning_scene/planning_scene.h>
#include <Eigen/Geometry>
#include <eigen_conversions/eigen_msg.h>
namespace moveit { namespace task_constructor { namespace stages {
SimpleGrasp::SimpleGrasp(const std::string& name)
: SerialContainer(name)
{
{
auto gengrasp = std::make_unique<GenerateGraspPose>("generate grasp pose");
grasp_generator_ = gengrasp.get();
auto ik = std::make_unique<ComputeIK>("compute ik", std::move(gengrasp));
const std::initializer_list<std::string>& 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<ModifyPlanningScene>("allow object collision");
PropertyMap& p = allow_touch->properties();
p.declare<std::string>("eef");
p.declare<std::string>("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<std::string>("eef");
const std::string& object = p.get<std::string>("object");
acm.setEntry(object, scene->getRobotModel()->getEndEffector(eef)
->getLinkModelNamesWithCollisionGeometry(), true);
});
insert(std::move(allow_touch));
}
{
auto pipeline = std::make_shared<solvers::PipelinePlanner>();
pipeline->setTimeout(8.0);
pipeline->setPlannerId("RRTConnectkConfigDefault");
auto move = std::make_unique<MoveTo>("close gripper", pipeline);
PropertyMap& p = move->properties();
p.property("group").configureInitFrom(Stage::PARENT, [this](const PropertyMap& parent_map){
const std::string& eef = parent_map.get<std::string>("eef");
const moveit::core::JointModelGroup* jmg = model_->getEndEffector(eef);
return boost::any(jmg->getName());
});
insert(std::move(move));
exposePropertyOfChildAs(-1, "joint_pose", "grasp");
}
{
auto attach = std::make_unique<ModifyPlanningScene>("attach object");
PropertyMap& p = attach->properties();
p.declare<std::string>("eef");
p.declare<std::string>("object");
p.configureInitFrom(Stage::PARENT, { "eef", "object" });
attach->setCallback([this](const planning_scene::PlanningScenePtr& scene, const PropertyMap& p){
const std::string& eef = p.get<std::string>("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<std::string>("object");
scene->processAttachedCollisionObjectMsg(obj);
});
insert(std::move(attach));
}
}
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);
}
} } }

View File

@ -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

View File

@ -0,0 +1,19 @@
#include "icons.h"
#include <QColor>
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);
} }

View File

@ -0,0 +1,13 @@
#pragma once
#include <utils/icon.h>
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;
} }

Binary file not shown.

After

Width:  |  Height:  |  Size: 180 B

Binary file not shown.

After

Width:  |  Height:  |  Size: 125 B

Binary file not shown.

After

Width:  |  Height:  |  Size: 141 B

Binary file not shown.

After

Width:  |  Height:  |  Size: 123 B

Binary file not shown.

After

Width:  |  Height:  |  Size: 183 B

View File

@ -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

View File

@ -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;

View File

@ -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;
return TreeMergeProxyModel::removeRows(row, count, parent);
}
std::pair<TaskListModel*, TaskDisplay*>
MetaTaskListModel::getTaskListModel(const QModelIndex &index) const
{

View File

@ -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<TaskListModel*, TaskDisplay*> getTaskListModel(const QModelIndex &index) const;

View File

@ -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);
@ -216,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

View File

@ -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);

View File

@ -6,5 +6,10 @@
<file>icons/new-stage.png</file>
<file>icons/success.png</file>
<file>icons/tasks.png</file>
<file>icons/connectarrow.png</file>
<file>icons/downarrow.png</file>
<file>icons/generatearrow.png</file>
<file>icons/uparrow.png</file>
<file>icons/updownarrow.png</file>
</qresource>
</RCC>

View File

@ -85,6 +85,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);
@ -146,6 +148,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();

View File

@ -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);
void setName(const QString& name);
void setSolutionStatus(bool ok);

View File

@ -38,6 +38,7 @@
#include "local_task_model.h"
#include "remote_task_model.h"
#include "factory_model.h"
#include "icons.h"
#include <ros/console.h>
#include <ros/service_client.h>
@ -48,6 +49,8 @@
#include <qevent.h>
#include <numeric>
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()
{
@ -192,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)
@ -200,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,
@ -226,6 +272,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

View File

@ -48,6 +48,7 @@
#include <QAbstractItemModel>
#include <QTreeView>
#include <memory>
#include <QPointer>
namespace ros { class ServiceClient; }
namespace rviz { class PropertyTreeModel; }
@ -84,6 +85,10 @@ 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);
/// 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;
@ -118,6 +123,9 @@ class TaskListModel : public utils::FlatMergeProxyModel {
// factory used to create stages
StageFactoryPtr stage_factory_;
QPointer<BaseTaskModel> active_task_model_;
QPersistentModelIndex highlighted_row_index_;
void onRemoveModel(QAbstractItemModel *model) override;
public:
@ -126,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);
@ -152,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);
};

View File

@ -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()
@ -156,6 +158,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 +182,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);
@ -217,6 +249,69 @@ 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");
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)
{
if (!config.isValid()) return;
auto readSizes = [&config](const QString& key) {
rviz::Config group = config.mapGetChild(key);
QList<int> 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);
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<Qt::SortOrder>(order));
}
void TaskView::addTask()
{
QModelIndex current = d_ptr->tasks_view->currentIndex();
@ -245,8 +340,8 @@ void TaskView::onCurrentStageChanged(const QModelIndex &current, 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;
@ -256,10 +351,13 @@ void TaskView::onCurrentStageChanged(const QModelIndex &current, 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();

View File

@ -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();

View File

@ -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})

View File

@ -304,6 +304,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())

View File

@ -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;

View File

@ -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 <QApplication>
#include <QIcon>
#include <QImage>
#include <QMetaEnum>
#include <QPainter>
#include <QPaintEngine>
#include <QWidget>
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<QRgb*>(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<QPixmap, QColor> MaskAndColor;
typedef QList<MaskAndColor> 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<IconMaskAndColor> &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<IconMaskAndColor> args, Icon::IconStyleOptions style)
: QVector<IconMaskAndColor>(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<QIcon> &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;
}
} }

View File

@ -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 <QPair>
#include <QVector>
class QColor;
class QIcon;
class QPixmap;
class QString;
namespace moveit_rviz_plugin { namespace utils {
typedef QPair<QString, QColor> IconMaskAndColor;
// Returns a recolored icon with shadow and custom disabled state for a
// series of grayscalemask|Theme::Color mask pairs
class Icon : public QVector<IconMaskAndColor>
{
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<IconMaskAndColor> 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<QIcon> &icons);
private:
IconStyleOptions m_style = None;
};
} }
Q_DECLARE_OPERATORS_FOR_FLAGS(moveit_rviz_plugin::utils::Icon::IconStyleOptions)

View File

@ -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<size_t, size_t> 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));

View File

@ -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);

View File

@ -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);

View File

@ -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);
}