Compare commits

...

7 Commits

Author SHA1 Message Date
Henning Kayser
af87504a3e
Merge 7f3bdaa607 into ef1cdeea86 2025-10-22 18:39:57 +01:00
Robert Haschke
ef1cdeea86 0.1.4
Some checks failed
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[CLANG_TIDY:true IMAGE:noble-ci-testing TARGET_CMAKE_… (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[DOCKER_RUN_OPTS:-e PRELOAD=libasan.so.8 -e LSAN_OPTI… (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:jammy-ci]) (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:noble-ci NAME:ccov TARGET_CMAKE_ARGS:-DCMAKE_B… (push) Has been cancelled
Format / pre-commit (push) Has been cancelled
CI / doc (push) Has been cancelled
CI / deploy (push) Has been cancelled
2025-10-15 13:53:26 +02:00
Robert Haschke
377d09046f Mandatory yaml dependency
Some checks failed
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[CLANG_TIDY:true IMAGE:noble-ci-testing TARGET_CMAKE_… (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[DOCKER_RUN_OPTS:-e PRELOAD=libasan.so.8 -e LSAN_OPTI… (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:jammy-ci]) (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:noble-ci NAME:ccov TARGET_CMAKE_ARGS:-DCMAKE_B… (push) Has been cancelled
Format / pre-commit (push) Has been cancelled
CI / doc (push) Has been cancelled
CI / deploy (push) Has been cancelled
2025-10-09 15:22:55 +02:00
Henning Kayser
7f3bdaa607 Add parameter struct 2019-11-20 17:32:48 +01:00
Henning Kayser
916d26db25 Create empty plan_pick_place_capability 2019-11-20 16:43:52 +01:00
Henning Kayser
bf78d46715 Add PickPlaceTask to core package 2019-11-20 16:43:29 +01:00
Henning Kayser
95e7256c45 Add action message template 2019-11-20 16:43:08 +01:00
24 changed files with 897 additions and 30 deletions

View File

@ -2,6 +2,21 @@
Changelog for package moveit_task_constructor_capabilities
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.1.4 (2025-10-15)
------------------
* Add missing include fmt/ranges.h (`#712 <https://github.com/moveit/moveit_task_constructor/issues/712>`_)
* Provide action feedback during task execution (`#653 <https://github.com/moveit/moveit_task_constructor/issues/653>`_)
* Increase minimum required CMake version to 3.16 supported by Ubuntu 20.04
* Silent error "Found empty JointState message"
* Simplify formatting code with https://github.com/fmtlib (`#499 <https://github.com/moveit/moveit_task_constructor/issues/499>`_)
* Drop Melodic support
* Fix Solution::fillMessage() (`#432 <https://github.com/moveit/moveit_task_constructor/issues/432>`_)
* Add property trajectory_execution_info (`#355 <https://github.com/moveit/moveit_task_constructor/issues/355>`_, `#502 <https://github.com/moveit/moveit_task_constructor/issues/502>`_)
* ExecuteTaskSolutionCapability: Rename goalCallback() -> execCallback()
* Replace namespace robot\_[model|state] with moveit::core
* Use pluginlib consistently (`#463 <https://github.com/moveit/moveit_task_constructor/issues/463>`_)
* Contributors: Dhruv Patel, Michael Görner, Robert Haschke
0.1.3 (2023-03-06)
------------------

View File

@ -8,6 +8,7 @@ find_package(catkin REQUIRED COMPONENTS
moveit_ros_move_group
moveit_task_constructor_core
moveit_task_constructor_msgs
moveit_task_constructor_core
pluginlib
std_msgs
)
@ -19,6 +20,7 @@ catkin_package(
CATKIN_DEPENDS
actionlib
moveit_task_constructor_msgs
moveit_task_constructor_core
std_msgs
)
@ -30,6 +32,7 @@ include(CheckCXXSymbolExists)
add_library(${PROJECT_NAME}
src/execute_task_solution_capability.cpp
src/plan_pick_place_capability.cpp
)
add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS})

View File

@ -4,4 +4,10 @@
Action server to execute solutions generated through the MoveIt Task Constructor.
</description>
</class>
<class name="move_group/PlanPickPlaceCapability" type="move_group::PlanPickPlaceCapability" base_class_type="move_group::MoveGroupCapability">
<description>
Action server to plan full pick and place motions using the MoveIt Task Constructor.
</description>
</class>
</library>

View File

@ -1,6 +1,6 @@
<package format="2">
<name>moveit_task_constructor_capabilities</name>
<version>0.1.3</version>
<version>0.1.4</version>
<description>
MoveGroupCapabilites to interact with MoveIt
</description>
@ -20,6 +20,7 @@
<depend>std_msgs</depend>
<depend>moveit_task_constructor_core</depend>
<depend>moveit_task_constructor_msgs</depend>
<depend>moveit_task_constructor_core</depend>
<export>
<moveit_ros_move_group plugin="${prefix}/capabilities_plugin_description.xml"/>

View File

@ -0,0 +1,78 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2016, Kentaro Wada.
* 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 Willow Garage 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.
*********************************************************************/
/* Author: Henning Kayser */
#include "plan_pick_place_capability.h"
#include <moveit/move_group/capability_names.h>
#include <moveit/robot_state/conversions.h>
namespace move_group {
PlanPickPlaceCapability::PlanPickPlaceCapability() : MoveGroupCapability("PlanPickPlace") {}
void PlanPickPlaceCapability::initialize() {
// configure the action server
as_.reset(new actionlib::SimpleActionServer<moveit_task_constructor_msgs::PlanPickPlaceAction>(
root_node_handle_, "plan_pick_place",
std::bind(&PlanPickPlaceCapability::goalCallback, this, std::placeholders::_1), false));
as_->registerPreemptCallback(std::bind(&PlanPickPlaceCapability::preemptCallback, this));
as_->start();
pick_place_task_ = std::make_unique<PickPlaceTask>("pick_place_task");
}
void PlanPickPlaceCapability::goalCallback(
const moveit_task_constructor_msgs::PlanPickPlaceGoalConstPtr& goal) {
moveit_task_constructor_msgs::PlanPickPlaceResult result;
// TODO: fill parameters
PickPlaceTask::Parameters parameters;
// initialize task
pick_place_task_->init(parameters);
// run plan
pick_place_task_->plan();
// retrieve and return result
}
void PlanPickPlaceCapability::preemptCallback() {
// TODO(henningkayser): abort planning
}
} // namespace move_group
#include <class_loader/class_loader.hpp>
CLASS_LOADER_REGISTER_CLASS(move_group::PlanPickPlaceCapability, move_group::MoveGroupCapability)

View File

@ -0,0 +1,71 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, Hamburg 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 Hamburg 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.
*********************************************************************/
/*
* Capability to plan pick and place motions using the MoveIt Task Constructor.
*
* Author: Henning Kayser
* */
#pragma once
#include <moveit/move_group/move_group_capability.h>
#include <actionlib/server/simple_action_server.h>
#include <moveit_task_constructor_msgs/PlanPickPlaceAction.h>
#include <moveit/task_constructor/tasks/pick_place_task.h>
#include <memory>
namespace move_group {
using moveit::task_constructor::tasks::PickPlaceTask;
class PlanPickPlaceCapability : public MoveGroupCapability
{
public:
PlanPickPlaceCapability();
virtual void initialize();
private:
void goalCallback(const moveit_task_constructor_msgs::PlanPickPlaceGoalConstPtr& goal);
void preemptCallback();
std::unique_ptr<actionlib::SimpleActionServer<moveit_task_constructor_msgs::PlanPickPlaceAction>> as_;
std::unique_ptr<PickPlaceTask> pick_place_task_;
};
} // namespace move_group

View File

@ -2,6 +2,106 @@
Changelog for package moveit_task_constructor_core
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.1.4 (2025-10-15)
------------------
* Avoid duplicate scenes in Solution.msg from generator stages (`#639 <https://github.com/moveit/moveit_task_constructor/issues/639>`_)
* Allow max Cartesian link speed in PlannerInterface (`#277 <https://github.com/moveit/moveit_task_constructor/issues/277>`_)
* Enable collisions visualizations (`#708 <https://github.com/moveit/moveit_task_constructor/issues/708>`_)
* LimitSolutions wrapper stage (`#710 <https://github.com/moveit/moveit_task_constructor/issues/710>`_)
* Improve code documentation
* CI: Fix Noble builds
* Pick with custom max_velocity_scaling_factor during approach+lift
* Rework pybind11 ABI compatibility checks
* Remove pybind11 submodule
* Upgrade pybind11 to v3
* Modernize declaration of compile options
* Factor out Python property handling to allow for reuse in custom Python wrappers
* Fix clamping of joint constraints (`#665 <https://github.com/moveit/moveit_task_constructor/issues/665>`_)
* Correctly report failures instead of issueing console warnings
* Increase minimum required CMake version to 3.16 supported by Ubuntu 20.04
* Python API: Allow passing a task's introspection object to SolutionBase::toMsg()
* clang-format-14
* Add support for GenerateRandomPose
* python: Add Task::setRobotModel
* Add path_constraints property to Connect stage
* provide a fmt wrapper (`#615 <https://github.com/moveit/moveit_task_constructor/issues/615>`_)
* Update API: JumpThreshold -> CartesianPrecision (`#611 <https://github.com/moveit/moveit_task_constructor/issues/611>`_)
* Reduce stop time due to preempt (`#598 <https://github.com/moveit/moveit_task_constructor/issues/598>`_)
* Add unittest for `#581 <https://github.com/moveit/moveit_task_constructor/issues/581>`_
* Fix early planning preemption (`#597 <https://github.com/moveit/moveit_task_constructor/issues/597>`_)
* MoveRelative: fix segfault on empty trajectory (`#595 <https://github.com/moveit/moveit_task_constructor/issues/595>`_)
* MoveRelative: handle equal min/max distance (`#593 <https://github.com/moveit/moveit_task_constructor/issues/593>`_)
* Cleanup unit tests and allow them to run via both, cmdline and pytest
* Connect: Relax validity check of reached end state
* Unify Python demo scripts
* Switch shebang to python3
* Silence gcc's overloaded-virtual warnings
* Add property to enable/disable pruning at runtime (`#590 <https://github.com/moveit/moveit_task_constructor/issues/590>`_)
* Disable pruning by default
* test_pruning.cpp: Add new test
* test_pruning.cpp: Extend test to ParallelContainer
* PassThrough: cleanup unused headers
* Avoid segfault if TimeParameterization is not set
* CartesianPath: allow ik_frame definition if start and end are given as joint-space poses
* Generalize utils::getRobotTipForFrame() to return error_msg instead of calling markAsFailure() on a solution
* ComputeIK: Allow additional constraints for filtering solutions (`#464 <https://github.com/moveit/moveit_task_constructor/issues/464>`_)
* Expose MultiPlanner to Python (`#474 <https://github.com/moveit/moveit_task_constructor/issues/474>`_)
* Add unittest cartesianCollisionMinMaxDistance (`#538 <https://github.com/moveit/moveit_task_constructor/issues/538>`_)
* Simplify formatting code with https://github.com/fmtlib (`#499 <https://github.com/moveit/moveit_task_constructor/issues/499>`_)
* Add NoOp stage (`#534 <https://github.com/moveit/moveit_task_constructor/issues/534>`_)
* ModifyPlanningScene: check state for collisions
* Improve TypeError exceptions
* Drop Melodic support
* Switch to package py_binding_tools
* Add ability to move CollisionObjects (`#567 <https://github.com/moveit/moveit_task_constructor/issues/567>`_)
* Improve description of max_distance property of Connect stage (`#564 <https://github.com/moveit/moveit_task_constructor/issues/564>`_)
* Add Generator::spawn(from, to, trajectory) variant (`#546 <https://github.com/moveit/moveit_task_constructor/issues/546>`_)
* Cosmetic fixes
* Fix Solution::fillMessage() (`#432 <https://github.com/moveit/moveit_task_constructor/issues/432>`_)
* Fix generation of Solution msg: consider backward operation
* Propagate errors from planners to solution comment (`#525 <https://github.com/moveit/moveit_task_constructor/issues/525>`_)
* JointInterpolationPlanner: Check joint bounds (`#505 <https://github.com/moveit/moveit_task_constructor/issues/505>`_)
* Add property trajectory_execution_info (`#355 <https://github.com/moveit/moveit_task_constructor/issues/355>`_, `#502 <https://github.com/moveit/moveit_task_constructor/issues/502>`_)
* Clear JointStates in scene diff (`#504 <https://github.com/moveit/moveit_task_constructor/issues/504>`_)
* Set a non-infinite default timeout in CurrentState stage (`#491 <https://github.com/moveit/moveit_task_constructor/issues/491>`_)
* Add GenerateRandomPose stage (`#166 <https://github.com/moveit/moveit_task_constructor/issues/166>`_)
* GenerateGraspPose: Expose rotation_axis as property (`#535 <https://github.com/moveit/moveit_task_constructor/issues/535>`_)
* Connect: ensure end-state matches goal state (`#532 <https://github.com/moveit/moveit_task_constructor/issues/532>`_)
* Fix discontinuity in trajectory (`#485 <https://github.com/moveit/moveit_task_constructor/issues/485>`_)
* Adaptions for https://github.com/ros-planning/moveit/pull/3534
* Cleanup debug output
* Fix duplicate solutions
* printPendingPairs(os) -> os<<pendingPairsPrinter()
* Fix leaking of failures into enumerated solutions
* Add more debugging output
* Unit tests for `#485 <https://github.com/moveit/moveit_task_constructor/issues/485>`_
* DelayingWrapper stage to delay solution shipping in unit tests
* Simplify tests
* Skip Fallbacks::replaceImpl() when already correctly initialized (`#494 <https://github.com/moveit/moveit_task_constructor/issues/494>`_)
* Fix demos (`#493 <https://github.com/moveit/moveit_task_constructor/issues/493>`_)
* Limit time to wait for execute_task_solution action server
* Replace namespace robot\_[model|state] with moveit::core
* MPS: fixup processCollisionObject
* Merge PR `#460 <https://github.com/moveit/moveit_task_constructor/issues/460>`_: improvements to ModifyPlanningScene stage
* Gracefully handle NULL robot_trajectory (`#469 <https://github.com/moveit/moveit_task_constructor/issues/469>`_)
* introspection: remove any invalid ROS-name chars from hostname (`#465 <https://github.com/moveit/moveit_task_constructor/issues/465>`_)
* Fix SolutionBase::fillMessage(): also write start_scene
* Fix add/remove object in backward operation
* Add python binding for ModifyPlanningScene::removeObject
* ComputeIK: update RobotState before calling setFromIK()
* Use pluginlib consistently (`#463 <https://github.com/moveit/moveit_task_constructor/issues/463>`_)
* Expose argument of PipelinePlanner's constructor to Python (`#462 <https://github.com/moveit/moveit_task_constructor/issues/462>`_)
* Fix allowCollisions(object, enable_collision)
* TestModifyPlanningScene
* Basic Move test: MoveRelative + MoveTo
* Add python binding for ModifyPlanningScene::allowCollisions(std::string, bool)
* Add python binding for Task::insert
* Add Stage::explainFailure() (`#445 <https://github.com/moveit/moveit_task_constructor/issues/445>`_)
* Improve documentation (`#431 <https://github.com/moveit/moveit_task_constructor/issues/431>`_)
* JointInterpolationPlanner: pass optional max_effort property along to GripperCommand (`#458 <https://github.com/moveit/moveit_task_constructor/issues/458>`_)
* Task: findChild() and operator[] should directly operate on stages() (`#435 <https://github.com/moveit/moveit_task_constructor/issues/435>`_)
* Contributors: Abishalini, Ali Haider, Captain Yoshi, Daniel García López, Gauthier Hentz, Henning Kayser, JafarAbdi, Michael Görner, Michael Wiznitzer, Paul Gesel, Robert Haschke, Sebastian Castro, Sebastian Jahr, VideoSystemsTech
0.1.3 (2023-03-06)
------------------
* MoveRelative: Allow backwards operation for joint-space delta (`#436 <https://github.com/ros-planning/moveit_task_constructor/issues/436>`_)

View File

@ -0,0 +1,128 @@
/*********************************************************************
* BSD 3-Clause License
*
* Copyright (c) 2019 PickNik LLC.
* 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 the copyright holder 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 HOLDER 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.
*********************************************************************/
/* Author: Henning Kayser, Simon Goldstein
Desc: A demo to show MoveIt Task Constructor in action
*/
// ROS
#include <ros/ros.h>
// MoveIt
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
// MTC
#include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/stages/compute_ik.h>
#include <moveit/task_constructor/stages/connect.h>
#include <moveit/task_constructor/stages/current_state.h>
#include <moveit/task_constructor/stages/generate_grasp_pose.h>
#include <moveit/task_constructor/stages/generate_pose.h>
#include <moveit/task_constructor/stages/generate_place_pose.h>
#include <moveit/task_constructor/stages/modify_planning_scene.h>
#include <moveit/task_constructor/stages/move_relative.h>
#include <moveit/task_constructor/stages/move_to.h>
#include <moveit/task_constructor/stages/predicate_filter.h>
#include <moveit/task_constructor/solvers/cartesian_path.h>
#include <moveit/task_constructor/solvers/pipeline_planner.h>
#include <moveit_task_constructor_msgs/ExecuteTaskSolutionAction.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib/server/simple_action_server.h>
#include <eigen_conversions/eigen_msg.h>
#pragma once
namespace moveit {
namespace task_constructor {
namespace tasks {
using namespace moveit::task_constructor;
class PickPlaceTask
{
public:
struct Parameters
{
// planning group properties
std::string arm_group_name_;
std::string eef_name_;
std::string hand_group_name_;
std::string hand_frame_;
// object + surface
std::vector<std::string> support_surfaces_;
std::string object_reference_frame_;
std::string surface_link_;
std::string object_name_;
std::string world_frame_;
std::string grasp_frame_;
std::vector<double> object_dimensions_;
// Predefined pose targets
std::string hand_open_pose_;
std::string hand_close_pose_;
std::string arm_home_pose_;
// Pick metrics
Eigen::Isometry3d grasp_frame_transform_;
double approach_object_min_dist_;
double approach_object_max_dist_;
double lift_object_min_dist_;
double lift_object_max_dist_;
double place_object_min_dist_;
double place_object_max_dist_;
double retreat_object_min_dist_;
double retreat_object_max_dist_;
// Place metrics
geometry_msgs::PoseStamped place_pose_;
double place_surface_offset_;
};
PickPlaceTask(const std::string& task_name);
~PickPlaceTask() = default;
void init(const Parameters& parameters);
bool plan();
private:
moveit::task_constructor::TaskPtr task_;
const std::string task_name_;
};
} // namespace tasks
} // namespace task_constructor
} // namespace moveit

View File

@ -1,6 +1,6 @@
<package format="3">
<name>moveit_task_constructor_core</name>
<version>0.1.3</version>
<version>0.1.4</version>
<description>MoveIt Task Pipeline</description>
<url type="website">https://github.com/moveit/moveit_task_constructor</url>

View File

@ -46,6 +46,7 @@ target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS})
add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
add_subdirectory(stages)
add_subdirectory(tasks)
install(TARGETS ${PROJECT_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}

View File

@ -0,0 +1,9 @@
add_library(${PROJECT_NAME}_tasks
${PROJECT_INCLUDE}/tasks/pick_place_task.h
pick_place_task.cpp
)
target_link_libraries(${PROJECT_NAME}_tasks ${PROJECT_NAME} ${catkin_LIBRARIES})
install(TARGETS ${PROJECT_NAME}_tasks
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})

View File

@ -0,0 +1,401 @@
/*********************************************************************
* BSD 3-Clause License
*
* Copyright (c) 2019 PickNik LLC.
* 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 the copyright holder 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 HOLDER 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.
*********************************************************************/
/* Author: Henning Kayser, Simon Goldstein
Desc: A demo to show MoveIt Task Constructor in action
*/
#include <moveit/task_constructor/tasks/pick_place_task.h>
#include <rosparam_shortcuts/rosparam_shortcuts.h>
namespace moveit {
namespace task_constructor {
namespace tasks {
constexpr char LOGNAME[] = "pick_place_task";
PickPlaceTask::PickPlaceTask(const std::string& task_name)
: task_name_(task_name) {}
void PickPlaceTask::init(const Parameters& parameters)
{
ROS_INFO_NAMED(LOGNAME, "Initializing task pipeline");
// Reset ROS introspection before constructing the new object
// TODO(henningkayser): verify this is a bug, fix if possible
task_.reset();
task_.reset(new moveit::task_constructor::Task(task_name_));
Task& t = *task_;
t.loadRobotModel();
// Sampling planner
// TODO(henningkayser): Setup and parameterize alternative planners
auto sampling_planner = std::make_shared<solvers::PipelinePlanner>();
sampling_planner->setProperty("goal_joint_tolerance", 1e-5);
// Cartesian planner
auto cartesian_planner = std::make_shared<solvers::CartesianPath>();
cartesian_planner->setMaxVelocityScaling(1.0);
cartesian_planner->setMaxAccelerationScaling(1.0);
cartesian_planner->setStepSize(.01);
// Set task properties
t.setProperty("group", parameters.arm_group_name_);
t.setProperty("eef", parameters.eef_name_);
t.setProperty("hand", parameters.hand_group_name_);
t.setProperty("ik_frame", parameters.hand_frame_);
/****************************************************
* *
* Current State *
* *
***************************************************/
Stage* current_state = nullptr; // Forward current_state on to grasp pose generator
{
auto _current_state = std::make_unique<stages::CurrentState>("current state");
_current_state->setTimeout(10);
// Verify that object is not attachd
auto applicability_filter =
std::make_unique<stages::PredicateFilter>("applicability test", std::move(_current_state));
applicability_filter->setPredicate([&](const SolutionBase& s, std::string& comment) {
s.start()->scene()->printKnownObjects(std::cout);
if (s.start()->scene()->getCurrentState().hasAttachedBody(parameters.object_name_))
{
comment = "object with id '" + parameters.object_name_ + "' is already attached and cannot be picked";
return false;
}
return true;
});
current_state = applicability_filter.get();
t.add(std::move(applicability_filter));
}
/****************************************************
* *
* Open Hand *
* *
***************************************************/
{ // Open Hand
auto stage = std::make_unique<stages::MoveTo>("open hand", sampling_planner);
stage->setGroup(parameters.hand_group_name_);
stage->setGoal(parameters.hand_open_pose_);
t.add(std::move(stage));
}
/****************************************************
* *
* Move to Pick *
* *
***************************************************/
{ // Move-to pre-grasp
auto stage = std::make_unique<stages::Connect>(
"move to pick", stages::Connect::GroupPlannerVector{ { parameters.arm_group_name_, sampling_planner } });
stage->setTimeout(5.0);
stage->properties().configureInitFrom(Stage::PARENT);
t.add(std::move(stage));
}
/****************************************************
* *
* Pick Object *
* *
***************************************************/
Stage* attach_object_stage = nullptr; // Forward attach_object_stage to place pose generator
{
auto grasp = std::make_unique<SerialContainer>("pick object");
t.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" });
grasp->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group", "ik_frame" });
/****************************************************
---- * Approach Object *
***************************************************/
{
auto stage = std::make_unique<stages::MoveRelative>("approach object", cartesian_planner);
stage->properties().set("marker_ns", "approach_object");
stage->properties().set("link", parameters.hand_frame_);
stage->properties().configureInitFrom(Stage::PARENT, { "group" });
stage->setMinMaxDistance(parameters.approach_object_min_dist_, parameters.approach_object_max_dist_);
// Set hand forward direction
geometry_msgs::Vector3Stamped vec;
vec.header.frame_id = parameters.object_name_;
vec.vector.z = -1.0;
stage->setDirection(vec);
grasp->insert(std::move(stage));
}
/****************************************************
---- * Generate Grasp Pose *
***************************************************/
{
// Sample grasp pose
auto stage = std::make_unique<stages::GenerateGraspPose>("generate grasp pose");
stage->properties().configureInitFrom(Stage::PARENT);
stage->properties().set("marker_ns", "grasp_pose");
stage->setPreGraspPose(parameters.hand_open_pose_);
stage->setObject(parameters.object_name_);
stage->setAngleDelta(M_PI / 12);
stage->setMonitoredStage(current_state); // Hook into current state
// Compute IK
auto wrapper = std::make_unique<stages::ComputeIK>("grasp pose IK", std::move(stage));
wrapper->setMaxIKSolutions(2);
wrapper->setMinSolutionDistance(1.0);
wrapper->setIKFrame(parameters.grasp_frame_transform_, parameters.hand_frame_);
//wrapper->setIgnoreCollisions(true);
wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" });
wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" });
grasp->insert(std::move(wrapper));
}
/****************************************************
---- * Allow Collision (hand object) *
***************************************************/
{
auto stage = std::make_unique<stages::ModifyPlanningScene>("allow collision (hand,object)");
stage->allowCollisions(
parameters.object_name_,
t.getRobotModel()->getJointModelGroup(parameters.hand_group_name_)->getLinkModelNamesWithCollisionGeometry(),
true);
grasp->insert(std::move(stage));
}
/****************************************************
---- * Close Hand *
***************************************************/
{
auto stage = std::make_unique<stages::MoveTo>("close hand", sampling_planner);
stage->properties().property("group").configureInitFrom(Stage::PARENT, parameters.hand_group_name_);
stage->setGoal(parameters.hand_close_pose_);
grasp->insert(std::move(stage));
}
/****************************************************
.... * Attach Object *
***************************************************/
{
auto stage = std::make_unique<stages::ModifyPlanningScene>("attach object");
stage->attachObject(parameters.object_name_, parameters.hand_frame_);
attach_object_stage = stage.get();
grasp->insert(std::move(stage));
}
/****************************************************
.... * Allow collision (object support) *
***************************************************/
{
auto stage = std::make_unique<stages::ModifyPlanningScene>("allow collision (object,support)");
stage->allowCollisions({ parameters.object_name_ }, "source_container", true); // TODO(henningkayser): parameterize
grasp->insert(std::move(stage));
}
/****************************************************
.... * Lift object *
***************************************************/
{
auto stage = std::make_unique<stages::MoveRelative>("lift object", cartesian_planner);
stage->properties().configureInitFrom(Stage::PARENT, { "group" });
stage->setMinMaxDistance(parameters.lift_object_min_dist_, parameters.lift_object_max_dist_);
stage->setIKFrame(parameters.hand_frame_);
stage->properties().set("marker_ns", "lift_object");
// Set upward direction
geometry_msgs::Vector3Stamped vec;
vec.header.frame_id = parameters.world_frame_;
vec.vector.z = 1.0;
stage->setDirection(vec);
grasp->insert(std::move(stage));
}
/****************************************************
.... * Forbid collision (object support) *
***************************************************/
{
auto stage = std::make_unique<stages::ModifyPlanningScene>("forbid collision (object,surface)");
stage->allowCollisions({ parameters.object_name_ }, "source_container", false); // TODO(henningkayser): parameterize
grasp->insert(std::move(stage));
}
// Add grasp container to task
t.add(std::move(grasp));
}
/******************************************************
* *
* Move to Place *
* *
*****************************************************/
{
auto stage = std::make_unique<stages::Connect>(
"move to place", stages::Connect::GroupPlannerVector{ { parameters.arm_group_name_, sampling_planner } });
stage->setTimeout(5.0);
stage->properties().configureInitFrom(Stage::PARENT);
t.add(std::move(stage));
}
/******************************************************
* *
* Place Object *
* *
*****************************************************/
{
auto place = std::make_unique<SerialContainer>("place object");
t.properties().exposeTo(place->properties(), { "eef", "hand", "group" });
place->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group" });
/******************************************************
---- * Lower Object *
*****************************************************/
{
auto stage = std::make_unique<stages::MoveRelative>("lower object", cartesian_planner);
stage->properties().set("marker_ns", "lower_object");
stage->properties().set("link", parameters.hand_frame_);
stage->properties().configureInitFrom(Stage::PARENT, { "group" });
stage->setMinMaxDistance(parameters.place_object_min_dist_, parameters.place_object_max_dist_);
// Set downward direction
geometry_msgs::Vector3Stamped vec;
vec.header.frame_id = parameters.world_frame_;
vec.vector.z = -1.0;
stage->setDirection(vec);
place->insert(std::move(stage));
}
/******************************************************
---- * Generate Place Pose *
*****************************************************/
{
// Generate Place Pose
auto stage = std::make_unique<stages::GeneratePlacePose>("generate place pose");
stage->properties().configureInitFrom(Stage::PARENT, { "ik_frame" });
stage->properties().set("marker_ns", "place_pose");
stage->setObject(parameters.object_name_);
stage->setPose(parameters.place_pose_);
// Hook into attach_object_stage which allows us to use the attached object as IK frame
stage->setMonitoredStage(attach_object_stage);
// Compute IK
auto wrapper = std::make_unique<stages::ComputeIK>("place pose IK", std::move(stage));
wrapper->setMaxIKSolutions(2);
wrapper->setIKFrame(parameters.grasp_frame_transform_, parameters.hand_frame_);
// TODO(henningkayser): Enable collisions
wrapper->setIgnoreCollisions(true);
wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" });
wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" });
place->insert(std::move(wrapper));
}
/******************************************************
---- * Open Hand *
*****************************************************/
{
auto stage = std::make_unique<stages::MoveTo>("open hand", sampling_planner);
stage->properties().property("group").configureInitFrom(Stage::PARENT, parameters.hand_group_name_);
stage->setGoal(parameters.hand_open_pose_);
place->insert(std::move(stage));
}
/******************************************************
---- * Forbid collision (hand, object) *
*****************************************************/
{
// TODO(henningkayser): Forbid collision after retreat?
auto stage = std::make_unique<stages::ModifyPlanningScene>("forbid collision (hand,object)");
stage->allowCollisions(
parameters.object_name_,
t.getRobotModel()->getJointModelGroup(parameters.hand_group_name_)->getLinkModelNamesWithCollisionGeometry(),
false);
place->insert(std::move(stage));
}
/******************************************************
---- * Detach Object *
*****************************************************/
{
auto stage = std::make_unique<stages::ModifyPlanningScene>("detach object");
stage->detachObject(parameters.object_name_, parameters.hand_frame_);
place->insert(std::move(stage));
}
/******************************************************
---- * Retreat Motion *
*****************************************************/
{
// TODO(henningkayser): Do we need this if items are dropped?
auto stage = std::make_unique<stages::MoveRelative>("retreat after place", cartesian_planner);
stage->properties().configureInitFrom(Stage::PARENT, { "group" });
stage->setMinMaxDistance(parameters.retreat_object_min_dist_, parameters.retreat_object_max_dist_);
stage->setIKFrame(parameters.hand_frame_);
stage->properties().set("marker_ns", "retreat");
geometry_msgs::Vector3Stamped vec;
vec.header.frame_id = parameters.hand_frame_;
vec.vector.z = -1.0;
stage->setDirection(vec);
place->insert(std::move(stage));
}
// Add place container to task
t.add(std::move(place));
}
/******************************************************
* *
* Move to Home *
* *
*****************************************************/
{
auto stage = std::make_unique<stages::MoveTo>("move home", sampling_planner);
stage->properties().configureInitFrom(Stage::PARENT, { "group" });
stage->setGoal(parameters.arm_home_pose_);
stage->restrictDirection(stages::MoveTo::FORWARD);
t.add(std::move(stage));
}
}
bool PickPlaceTask::plan() {
ROS_INFO_NAMED(LOGNAME, "Start searching for task solutions");
try {
task_->plan(10); // TODO: parameterize
} catch (InitStageException& e) {
ROS_ERROR_STREAM_NAMED(LOGNAME, "Initialization failed: " << e);
return false;
}
if (task_->numSolutions() == 0) {
ROS_ERROR_NAMED(LOGNAME, "Planning failed");
return false;
}
return true;
}
} // namespace tasks
} // namespace task_constructor
} // namespace moveit

View File

@ -2,6 +2,28 @@
Changelog for package moveit_task_constructor_demo
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.1.4 (2025-10-15)
------------------
* Allow max Cartesian link speed in PlannerInterface (`#277 <https://github.com/moveit/moveit_task_constructor/issues/277>`_)
* Add missing dependency
* CI: Fix Noble builds
* Pick with custom max_velocity_scaling_factor during approach+lift
* Fix pick+place: connect should plan both, arm and hand motion
* Increase minimum required CMake version to 3.16 supported by Ubuntu 20.04
* clang-tidy fixes: std::endl -> '\n'
* examples: add orientation path constraint
* Update API: JumpThreshold -> CartesianPrecision (`#611 <https://github.com/moveit/moveit_task_constructor/issues/611>`_)
* Unify Python demo scripts
* Switch shebang to python3
* Improve comments for pick-and-place task (`#238 <https://github.com/moveit/moveit_task_constructor/issues/238>`_)
* Expose MultiPlanner to Python (`#474 <https://github.com/moveit/moveit_task_constructor/issues/474>`_)
* Example of constrained orientation planning
* Switch to package py_binding_tools
* Fix demos (`#493 <https://github.com/moveit/moveit_task_constructor/issues/493>`_)
* Merge PR `#460 <https://github.com/moveit/moveit_task_constructor/issues/460>`_: improvements to ModifyPlanningScene stage
* Fix SolutionBase::fillMessage(): also write start_scene
* Contributors: Fabian Schuetze, Gauthier Hentz, Michael Görner, Robert Haschke, VideoSystemsTech
0.1.3 (2023-03-06)
------------------
* Use const reference instead of reference for ros::NodeHandle (`#437 <https://github.com/ros-planning/moveit_task_constructor/issues/437>`_)

View File

@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="2">
<name>moveit_task_constructor_demo</name>
<version>0.1.3</version>
<version>0.1.4</version>
<description>demo tasks illustrating various capabilities of MTC.</description>
<author email="rhaschke@techfak.uni-bielefeld.de">Robert Haschke</author>

View File

@ -2,6 +2,12 @@
Changelog for package moveit_task_constructor_msgs
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.1.4 (2025-10-15)
------------------
* Increase minimum required CMake version to 3.16 supported by Ubuntu 20.04
* Add property trajectory_execution_info (`#355 <https://github.com/moveit/moveit_task_constructor/issues/355>`_, `#502 <https://github.com/moveit/moveit_task_constructor/issues/502>`_)
* Contributors: Robert Haschke, Luca Lach
0.1.3 (2023-03-06)
------------------

View File

@ -28,6 +28,7 @@ add_service_files(DIRECTORY srv FILES
add_action_files(DIRECTORY action FILES
ExecuteTaskSolution.action
PlanPickPlace.action
)
generate_messages(DEPENDENCIES ${MSG_DEPS})

View File

@ -0,0 +1,16 @@
# goal
string object_id
string end_effector
moveit_msgs/PlaceLocation place_location
---
# result
moveit_msgs/MoveItErrorCodes error_code
Solution solution
---
# feedback
string feedback

View File

@ -1,6 +1,6 @@
<package format="2">
<name>moveit_task_constructor_msgs</name>
<version>0.1.3</version>
<version>0.1.4</version>
<description>Messages for MoveIt Task Pipeline</description>
<license>BSD</license>

View File

@ -2,6 +2,16 @@
Changelog for package rviz_marker_tools
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.1.4 (2025-10-15)
------------------
* Increase minimum required CMake version to 3.16 supported by Ubuntu 20.04
* clang-tidy fixes: use uint8_t enums
* Ignore Debian-specific catkin_lint error around urdfdom_headers (`#614 <https://github.com/moveit/moveit_task_constructor/issues/614>`_)
* clean up dependencies for rviz_marker_tools (`#610 <https://github.com/moveit/moveit_task_constructor/issues/610>`_)
* rviz_marker_tools: add missing dependency on urdfdom
* rviz_marker_tools: drop rviz dependency
* Contributors: Michael Görner, Robert Haschke
0.1.3 (2023-03-06)
------------------

View File

@ -1,6 +1,6 @@
<package format="2">
<name>rviz_marker_tools</name>
<version>0.1.3</version>
<version>0.1.4</version>
<description>Tools for marker creation / handling</description>
<license>BSD</license>

View File

@ -2,6 +2,24 @@
Changelog for package moveit_task_constructor_visualization
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.1.4 (2025-10-15)
------------------
* Mandatory yaml dependency
* Increase minimum required CMake version to 3.16 supported by Ubuntu 20.04
* clang-format-14
* clang-tidy fixes: use uint8_t enums
* Simplify formatting code with https://github.com/fmtlib (`#499 <https://github.com/moveit/moveit_task_constructor/issues/499>`_)
* Drop Kinetic support
* Fix discontinuity in trajectory (`#485 <https://github.com/moveit/moveit_task_constructor/issues/485>`_)
* Cleanup debug output
* Add more debugging output
* Hide button to show rviz-based task construction (`#492 <https://github.com/moveit/moveit_task_constructor/issues/492>`_)
* Fix Qt 5.15 deprecation warnings
* Limit time to wait for execute_task_solution action server
* Replace namespace robot\_[model|state] with moveit::core
* Use pluginlib consistently (`#463 <https://github.com/moveit/moveit_task_constructor/issues/463>`_)
* Contributors: Michael Görner, Robert Haschke
0.1.3 (2023-03-06)
------------------

View File

@ -2,16 +2,13 @@ set(MOVEIT_LIB_NAME motion_planning_tasks_properties)
set(SOURCES
property_factory.cpp
property_from_yaml.cpp
)
find_package(PkgConfig REQUIRED)
pkg_check_modules(YAML yaml-0.1)
if (YAML_FOUND)
# Only cmake > 3.12 provides XXX_LINK_LIBRARIES. Find the absolute path manually
find_library(YAML_LIBRARIES ${YAML_LIBRARIES} PATHS ${YAML_LIBRARY_DIRS})
list(APPEND SOURCES property_from_yaml.cpp)
add_definitions(-DHAVE_YAML)
endif()
pkg_check_modules(YAML REQUIRED yaml-0.1)
# Only cmake > 3.12 provides XXX_LINK_LIBRARIES. Find the absolute path manually
find_library(YAML_LIBRARIES ${YAML_LIBRARIES} PATHS ${YAML_LIBRARY_DIRS})
add_library(${MOVEIT_LIB_NAME} SHARED ${SOURCES})

View File

@ -147,21 +147,4 @@ void PropertyFactory::addRemainingProperties(rviz::Property* root, mtc::Property
new rviz::Property("no properties", QVariant(), QString(), root);
}
#ifndef HAVE_YAML
rviz::Property* PropertyFactory::createDefault(const std::string& name, const std::string& /*type*/,
const std::string& description, const std::string& value,
rviz::Property* old) {
if (old) { // reuse existing Property?
assert(old->getNameStd() == name);
old->setDescription(QString::fromStdString(description));
old->setValue(QString::fromStdString(value));
return old;
} else { // create new Property?
rviz::Property* result = new rviz::StringProperty(QString::fromStdString(name), QString::fromStdString(value),
QString::fromStdString(description));
result->setReadOnly(true);
return result;
}
}
#endif
} // namespace moveit_rviz_plugin

View File

@ -1,6 +1,6 @@
<package format="2">
<name>moveit_task_constructor_visualization</name>
<version>0.1.3</version>
<version>0.1.4</version>
<description>Visualization tools for MoveIt Task Pipeline</description>
<license>BSD</license>
@ -18,6 +18,7 @@
<depend>moveit_ros_visualization</depend>
<depend>roscpp</depend>
<depend>rviz</depend>
<depend>yaml</depend>
<test_depend>rosunit</test_depend>
<test_depend>rostest</test_depend>