mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Merge branches 'bug-fixes', 'gui' and 'pick-stage'
This commit is contained in:
commit
5a55a5949b
@ -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()),
|
||||
|
||||
@ -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();
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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;
|
||||
|
||||
92
core/include/moveit/task_constructor/stages/pick.h
Normal file
92
core/include/moveit/task_constructor/stages/pick.h
Normal 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);
|
||||
};
|
||||
|
||||
} } }
|
||||
101
core/include/moveit/task_constructor/stages/simple_grasp.h
Normal file
101
core/include/moveit/task_constructor/stages/simple_grasp.h
Normal 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);
|
||||
}
|
||||
};
|
||||
|
||||
} } }
|
||||
@ -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)
|
||||
|
||||
@ -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");
|
||||
}
|
||||
|
||||
} } }
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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()));
|
||||
|
||||
@ -54,7 +54,7 @@ bool FixedState::canCompute() const{
|
||||
|
||||
bool FixedState::compute() {
|
||||
spawn(InterfaceState(scene_), 0.0);
|
||||
return ran_= true;
|
||||
return ran_ = true;
|
||||
}
|
||||
|
||||
} } }
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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
105
core/src/stages/pick.cpp
Normal 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);
|
||||
}
|
||||
|
||||
} } }
|
||||
135
core/src/stages/simple_grasp.cpp
Normal file
135
core/src/stages/simple_grasp.cpp
Normal 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);
|
||||
}
|
||||
|
||||
} } }
|
||||
@ -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
|
||||
|
||||
19
visualization/motion_planning_tasks/src/icons.cpp
Normal file
19
visualization/motion_planning_tasks/src/icons.cpp
Normal 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);
|
||||
|
||||
} }
|
||||
13
visualization/motion_planning_tasks/src/icons.h
Normal file
13
visualization/motion_planning_tasks/src/icons.h
Normal 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;
|
||||
|
||||
} }
|
||||
BIN
visualization/motion_planning_tasks/src/icons/connectarrow.png
Normal file
BIN
visualization/motion_planning_tasks/src/icons/connectarrow.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 180 B |
BIN
visualization/motion_planning_tasks/src/icons/downarrow.png
Normal file
BIN
visualization/motion_planning_tasks/src/icons/downarrow.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 125 B |
BIN
visualization/motion_planning_tasks/src/icons/generatearrow.png
Normal file
BIN
visualization/motion_planning_tasks/src/icons/generatearrow.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 141 B |
BIN
visualization/motion_planning_tasks/src/icons/uparrow.png
Normal file
BIN
visualization/motion_planning_tasks/src/icons/uparrow.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 123 B |
BIN
visualization/motion_planning_tasks/src/icons/updownarrow.png
Normal file
BIN
visualization/motion_planning_tasks/src/icons/updownarrow.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 183 B |
@ -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
|
||||
|
||||
@ -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;
|
||||
|
||||
|
||||
@ -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
|
||||
{
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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>
|
||||
|
||||
@ -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();
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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);
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -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 ¤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;
|
||||
@ -256,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();
|
||||
|
||||
|
||||
@ -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();
|
||||
|
||||
|
||||
@ -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})
|
||||
|
||||
|
||||
@ -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())
|
||||
|
||||
@ -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;
|
||||
|
||||
236
visualization/motion_planning_tasks/utils/icon.cpp
Normal file
236
visualization/motion_planning_tasks/utils/icon.cpp
Normal 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;
|
||||
}
|
||||
|
||||
} }
|
||||
85
visualization/motion_planning_tasks/utils/icon.h
Normal file
85
visualization/motion_planning_tasks/utils/icon.h
Normal 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)
|
||||
@ -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));
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user