diff --git a/air_moveit_config/scripts/generate_pose.py b/air_moveit_config/scripts/generate_pose.py new file mode 100755 index 0000000..2ada95f --- /dev/null +++ b/air_moveit_config/scripts/generate_pose.py @@ -0,0 +1,51 @@ +#! /usr/bin/env python3 +# -*- coding: utf-8 -*- + +from moveit.task_constructor import core, stages +from geometry_msgs.msg import PoseStamped +from moveit_commander.roscpp_initializer import roscpp_initialize +import time + +roscpp_initialize("mtc_tutorial_compute_ik") + +# Specify the planning group +group = "air" + +# Create a task +task = core.Task() + +# Get the current robot state +currentState = stages.CurrentState("current state") +task.add(currentState) + +# Create a planner instance that is used to connect +# the current state to the grasp approach pose +pipelinePlanner = core.PipelinePlanner() +pipelinePlanner.planner = "RRTConnectkConfigDefault" +planners = [(group, pipelinePlanner)] + +# Connect the two stages +connect = stages.Connect("connect1", planners) +connect.properties.configureInitFrom(core.Stage.PropertyInitializerSource.PARENT) +task.add(connect) + +# [initAndConfigGeneratePose] +# create an example pose wrt. the origin of the +# panda arm link8 +pose = PoseStamped() +pose.header.frame_id = "qbhand_base_link" + +# Calculate the inverse kinematics for the current robot state +generatePose = stages.GeneratePose("generate pose") + +# spwan a pose whenever there is a solution of the monitored stage +generatePose.setMonitoredStage(task["current state"]) +generatePose.pose = pose + +# Add the stage to the task hierarchy +task.add(generatePose) +# [initAndConfigGeneratePose] + +if task.plan(): + task.publish(task.solutions[0]) +time.sleep(1) diff --git a/air_moveit_config/scripts/pickplace.py b/air_moveit_config/scripts/pickplace.py index f35faf9..fc92abf 100755 --- a/air_moveit_config/scripts/pickplace.py +++ b/air_moveit_config/scripts/pickplace.py @@ -30,14 +30,14 @@ # Grasp object properties objectPose = PoseStamped() objectPose.header.frame_id = "world" -objectPose.pose.orientation.x = 1.0 +objectPose.pose.orientation.w = 1.0 objectPose.pose.position.x = 0.55 objectPose.pose.position.y = -0.25 -objectPose.pose.position.z = 0.7 +objectPose.pose.position.z = 0.82 # [initCollisionObject] # Add the grasp object to the planning scene -psi.add_cylinder(object_name, objectPose, height=0.25, radius=0.02) +psi.add_cylinder(object_name, objectPose, height=0.2, radius=0.01) # [pickAndPlaceTut2] # [pickAndPlaceTut3] @@ -54,15 +54,14 @@ # Create a planner instance that is used to connect # the current state to the grasp approach pose pipeline = core.PipelinePlanner() -pipeline.planner = "RRTConnectkConfigDefault" +pipeline.planner = "RRTConnect" planners = [(arm, pipeline)] # Connect the two stages task.add(stages.Connect("connect1", planners)) # [initAndConfigConnect] # [pickAndPlaceTut4] - -# [pickAndPlaceTut5] + # [initAndConfigGenerateGraspPose] # The grasp generator spawns a set of possible grasp poses around the object grasp_generator = stages.GenerateGraspPose("Generate Grasp Pose") @@ -118,61 +117,13 @@ # [pickAndPlaceTut9] # Connect the Pick stage with the following Place stage -task.add(stages.Connect("connect2", planners)) -# [pickAndPlaceTut9] -# [pickAndPlaceTut10] -# [initAndConfigGeneratePlacePose] -# Define the pose that the object should have after placing -placePose = objectPose -placePose.pose.orientation.x = 1 -placePose.pose.position.x += -0.1 -placePose.pose.position.y += 0.15 # shift object by 20cm along y axis - -# Generate Cartesian place poses for the object -place_generator = stages.GeneratePlacePose("Generate Place Pose") -place_generator.setMonitoredStage(task["Pick"]) -place_generator.object = object_name -place_generator.pose = placePose -# [initAndConfigGeneratePlacePose] -# [pickAndPlaceTut10] - -# [initAndConfigSimpleUnGrasp] -# The SimpleUnGrasp container encapsulates releasing the object at the given Cartesian pose -# [pickAndPlaceTut11] -simpleUnGrasp = stages.SimpleUnGrasp(place_generator, "UnGrasp") -# [pickAndPlaceTut11] - -# [pickAndPlaceTut12] -# [initAndConfigPlace] -# Place container comprises placing, ungrasping, and retracting -place = stages.Place(simpleUnGrasp, "Place") -place.eef = eef -place.object = object_name -place.eef_frame = "qbhand_base_link" -#place.eef_frame = "panda_link8" -# [initAndConfigSimpleUnGrasp] - -# Twist to retract from the object -retract = TwistStamped() -retract.header.frame_id = "world" -retract.twist.linear.y = 1.0 -place.setRetractMotion(retract, 0.00, 0.25) #(retract, 0.03, 0.1) - -# Twist to place the object -placeMotion = TwistStamped() -placeMotion.header.frame_id = "qbhand_base_link" -placeMotion.twist.linear.y = 1.0 -place.setPlaceMotion(placeMotion, 0.00, 0.25) #(placeMotion, 0.03, 0.1) - -# Add the place pipeline to the task's hierarchy -task.add(place) -# [initAndConfigPlace] -# [pickAndPlaceTut12] - -# [pickAndPlaceTut13] if task.plan(): - task.publish(task.solutions[0]) + try: + task.publish(task.solutions[0]) + except: + print("Can't publish task") + # avoid ClassLoader warning del pipeline @@ -180,4 +131,4 @@ # [pickAndPlaceTut13] # Prevent the program from exiting, giving you the opportunity to inspect solutions in rviz -time.sleep(3600) +time.sleep(120) diff --git a/moveit_task_constructor/capabilities/CHANGELOG.rst b/moveit_task_constructor/capabilities/CHANGELOG.rst new file mode 100644 index 0000000..70c3ce1 --- /dev/null +++ b/moveit_task_constructor/capabilities/CHANGELOG.rst @@ -0,0 +1,14 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package moveit_task_constructor_capabilities +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.2 (2023-02-24) +------------------ + +0.1.1 (2023-02-15) +------------------ + +0.1.0 (2023-02-02) +------------------ +* Initial release +* Contributors: Michael Görner, Robert Haschke diff --git a/moveit_task_constructor/core/CHANGELOG.rst b/moveit_task_constructor/core/CHANGELOG.rst new file mode 100644 index 0000000..5fe14dc --- /dev/null +++ b/moveit_task_constructor/core/CHANGELOG.rst @@ -0,0 +1,30 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package moveit_task_constructor_core +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.2 (2023-02-24) +------------------ +* Remove moveit/__init__.py during .deb builds to avoid installation conflicts +* MultiPlanner solver (`#429 `_): a planner that tries multiple planners in sequence + + * CartesianPath: Deprecate redundant property setters + * PlannerInterface: provide "timeout" property + * PlannerInterface: provide setters for properties +* JointInterpolation: fix timeout handling +* Contributors: Robert Haschke + +0.1.1 (2023-02-15) +------------------ +* Resort to MoveIt's python tools +* Provide ComputeIK.ik_frame as full PoseStamped +* Fixed build farm issues + + * Removed unused eigen_conversions includes + * Fixed odr compiler warning on Buster + * Fixed missing dependency declarations +* Contributors: Robert Haschke + +0.1.0 (2023-02-02) +------------------ +* Initial release +* Contributors: Michael Görner, Robert Haschke, Captain Yoshi, Christian Petersmeier, Henning Kayser, Jafar Abdi, Tyler Weaver diff --git a/moveit_task_constructor/core/CMakeLists.txt b/moveit_task_constructor/core/CMakeLists.txt index 74e44e1..8568162 100644 --- a/moveit_task_constructor/core/CMakeLists.txt +++ b/moveit_task_constructor/core/CMakeLists.txt @@ -25,7 +25,6 @@ catkin_package( ${PROJECT_NAME} ${PROJECT_NAME}_stages ${PROJECT_NAME}_stage_plugins - moveit_python_tools INCLUDE_DIRS include CATKIN_DEPENDS diff --git a/moveit_task_constructor/core/include/moveit/python/task_constructor/properties.h b/moveit_task_constructor/core/include/moveit/python/task_constructor/properties.h index 5dea8d6..a164f55 100644 --- a/moveit_task_constructor/core/include/moveit/python/task_constructor/properties.h +++ b/moveit_task_constructor/core/include/moveit/python/task_constructor/properties.h @@ -1,11 +1,14 @@ #pragma once -#include -#include +#include +#include #include #include #include +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Property) +PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::PropertyMap) + namespace moveit { namespace python { diff --git a/moveit_task_constructor/core/include/moveit/task_constructor/solvers/cartesian_path.h b/moveit_task_constructor/core/include/moveit/task_constructor/solvers/cartesian_path.h index 732111c..d2c2c06 100644 --- a/moveit_task_constructor/core/include/moveit/task_constructor/solvers/cartesian_path.h +++ b/moveit_task_constructor/core/include/moveit/task_constructor/solvers/cartesian_path.h @@ -56,8 +56,10 @@ class CartesianPath : public PlannerInterface void setJumpThreshold(double jump_threshold) { setProperty("jump_threshold", jump_threshold); } void setMinFraction(double min_fraction) { setProperty("min_fraction", min_fraction); } - void setMaxVelocityScaling(double factor) { setProperty("max_velocity_scaling_factor", factor); } - void setMaxAccelerationScaling(double factor) { setProperty("max_acceleration_scaling_factor", factor); } + [[deprecated("Replace with setMaxVelocityScalingFactor")]] // clang-format off + void setMaxVelocityScaling(double factor) { setMaxVelocityScalingFactor(factor); } + [[deprecated("Replace with setMaxAccelerationScalingFactor")]] // clang-format off + void setMaxAccelerationScaling(double factor) { setMaxAccelerationScalingFactor(factor); } void init(const moveit::core::RobotModelConstPtr& robot_model) override; diff --git a/moveit_task_constructor/core/include/moveit/task_constructor/solvers/multi_planner.h b/moveit_task_constructor/core/include/moveit/task_constructor/solvers/multi_planner.h new file mode 100644 index 0000000..2487dad --- /dev/null +++ b/moveit_task_constructor/core/include/moveit/task_constructor/solvers/multi_planner.h @@ -0,0 +1,77 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Robert Haschke + Desc: meta planner, running multiple planners in parallel or sequence +*/ + +#pragma once + +#include +#include + +namespace moveit { +namespace task_constructor { +namespace solvers { + +MOVEIT_CLASS_FORWARD(MultiPlanner); + +/** A meta planner that runs multiple alternative planners in sequence and returns the first found solution. + * + * This is useful to sequence different planning strategies of increasing complexity, + * e.g. Cartesian or joint-space interpolation first, then OMPL, ... + * This is (slightly) different from the Fallbacks container, as the MultiPlanner directly applies its planners to each + * individual planning job. In contrast, the Fallbacks container first runs the active child to exhaustion before + * switching to the next child, which possibly applies a different planning strategy. + */ +class MultiPlanner : public PlannerInterface, public std::vector +{ +public: + using PlannerList = std::vector; + using PlannerList::PlannerList; // inherit all std::vector constructors + + void init(const moveit::core::RobotModelConstPtr& robot_model) override; + + bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, + const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; + + bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, + double timeout, robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override; +}; +} // namespace solvers +} // namespace task_constructor +} // namespace moveit diff --git a/moveit_task_constructor/core/include/moveit/task_constructor/solvers/planner_interface.h b/moveit_task_constructor/core/include/moveit/task_constructor/solvers/planner_interface.h index 60139b3..67a876e 100644 --- a/moveit_task_constructor/core/include/moveit/task_constructor/solvers/planner_interface.h +++ b/moveit_task_constructor/core/include/moveit/task_constructor/solvers/planner_interface.h @@ -49,6 +49,9 @@ MOVEIT_CLASS_FORWARD(PlanningScene); namespace robot_trajectory { MOVEIT_CLASS_FORWARD(RobotTrajectory); } +namespace trajectory_processing { +MOVEIT_CLASS_FORWARD(TimeParameterization); +} namespace moveit { namespace core { MOVEIT_CLASS_FORWARD(LinkModel); @@ -75,6 +78,12 @@ class PlannerInterface const PropertyMap& properties() const { return properties_; } void setProperty(const std::string& name, const boost::any& value) { properties_.set(name, value); } + void setTimeout(double timeout) { properties_.set("timeout", timeout); } + void setMaxVelocityScalingFactor(double factor) { properties_.set("max_velocity_scaling_factor", factor); } + void setMaxAccelerationScalingFactor(double factor) { properties_.set("max_acceleration_scaling_factor", factor); } + void setTimeParameterization(const trajectory_processing::TimeParameterizationPtr& tp) { + properties_.set("time_parameterization", tp); + } virtual void init(const moveit::core::RobotModelConstPtr& robot_model) = 0; diff --git a/moveit_task_constructor/core/package.xml b/moveit_task_constructor/core/package.xml index 0459fd5..de0b187 100644 --- a/moveit_task_constructor/core/package.xml +++ b/moveit_task_constructor/core/package.xml @@ -1,6 +1,6 @@ moveit_task_constructor_core - 0.0.0 + 0.1.2 MoveIt Task Pipeline https://github.com/ros-planning/moveit_task_constructor @@ -31,6 +31,8 @@ rosunit rostest moveit_resources_fanuc_moveit_config + moveit_planners + moveit_commander diff --git a/moveit_task_constructor/core/python/CMakeLists.txt b/moveit_task_constructor/core/python/CMakeLists.txt index cf87772..48bb60f 100644 --- a/moveit_task_constructor/core/python/CMakeLists.txt +++ b/moveit_task_constructor/core/python/CMakeLists.txt @@ -21,9 +21,8 @@ set(PYBIND11_CMAKECONFIG_INSTALL_DIR ${CATKIN_PACKAGE_SHARE_DESTINATION}/cmake if(NOT EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/pybind11/CMakeLists.txt") message("Missing content of submodule pybind11: Use 'git clone --recurse-submodule' in future.\n" "Checking out content automatically") - execute_process(COMMAND git submodule init - COMMAND git submodule update - WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}) + execute_process(COMMAND git submodule init WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}) + execute_process(COMMAND git submodule update WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}) endif() #catkin_lint: ignore_once subproject duplicate_cmd add_subdirectory(pybind11) diff --git a/moveit_task_constructor/core/python/bindings/CMakeLists.txt b/moveit_task_constructor/core/python/bindings/CMakeLists.txt index b0809ee..d83a10b 100644 --- a/moveit_task_constructor/core/python/bindings/CMakeLists.txt +++ b/moveit_task_constructor/core/python/bindings/CMakeLists.txt @@ -1,31 +1,6 @@ -# python tools support lib -set(TOOLS_LIB_NAME moveit_python_tools) -set(INCLUDES ${PROJECT_SOURCE_DIR}/include/moveit/python/python_tools) -add_library(${TOOLS_LIB_NAME} SHARED - ${INCLUDES}/ros_init.h - ${INCLUDES}/ros_types.h - src/ros_init.cpp - src/ros_types.cpp -) -target_include_directories(${TOOLS_LIB_NAME} - PUBLIC $ - PUBLIC ${catkin_INCLUDE_DIRS} -) -target_link_libraries(${TOOLS_LIB_NAME} PUBLIC pybind11::pybind11 ${Boost_LIBRARIES} ${roscpp_LIBRARIES}) - -install(TARGETS ${TOOLS_LIB_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -) - # catkin_lint cannot detect target declarations in functions, here in pybind11_add_module #catkin_lint: ignore undefined_target -# moveit.python_tools -pybind11_add_module(pymoveit_python_tools src/python_tools.cpp) -target_link_libraries(pymoveit_python_tools PRIVATE ${TOOLS_LIB_NAME}) -set_target_properties(pymoveit_python_tools PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_PYTHON_DESTINATION}) - # moveit.task_constructor set(INCLUDES ${PROJECT_SOURCE_DIR}/include/moveit/python/task_constructor) pybind11_add_module(pymoveit_mtc @@ -42,6 +17,6 @@ target_link_libraries(pymoveit_mtc PUBLIC ${PROJECT_NAME} ${PROJECT_NAME}_stages set_target_properties(pymoveit_mtc PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_PYTHON_DESTINATION}) # install python libs -install(TARGETS pymoveit_python_tools pymoveit_mtc +install(TARGETS pymoveit_mtc LIBRARY DESTINATION ${CATKIN_GLOBAL_PYTHON_DESTINATION} ) diff --git a/moveit_task_constructor/core/python/bindings/src/properties.cpp b/moveit_task_constructor/core/python/bindings/src/properties.cpp index 7f26293..e53d7a2 100644 --- a/moveit_task_constructor/core/python/bindings/src/properties.cpp +++ b/moveit_task_constructor/core/python/bindings/src/properties.cpp @@ -39,9 +39,6 @@ namespace py = pybind11; using namespace py::literals; using namespace moveit::task_constructor; -PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Property) -PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::PropertyMap) - namespace moveit { namespace python { namespace { diff --git a/moveit_task_constructor/core/python/test/rostest_mtc.py b/moveit_task_constructor/core/python/test/rostest_mtc.py index 5a0f5bb..54e38e6 100755 --- a/moveit_task_constructor/core/python/test/rostest_mtc.py +++ b/moveit_task_constructor/core/python/test/rostest_mtc.py @@ -3,7 +3,7 @@ from __future__ import print_function import unittest import rostest -from moveit.python_tools import roscpp_init +from moveit_commander.roscpp_initializer import roscpp_initialize from moveit.task_constructor import core, stages from geometry_msgs.msg import PoseStamped, Pose from geometry_msgs.msg import Vector3Stamped, Vector3 @@ -65,5 +65,5 @@ def createDisplacement(group, displacement): if __name__ == "__main__": - roscpp_init("test_mtc") + roscpp_initialize("test_mtc") rostest.rosrun("mtc", "base", Test) diff --git a/moveit_task_constructor/core/python/test/rostest_trampoline.py b/moveit_task_constructor/core/python/test/rostest_trampoline.py index 277c99b..562adf7 100755 --- a/moveit_task_constructor/core/python/test/rostest_trampoline.py +++ b/moveit_task_constructor/core/python/test/rostest_trampoline.py @@ -4,10 +4,10 @@ from __future__ import print_function import unittest import rostest -from moveit.python_tools import roscpp_init +from moveit_commander.roscpp_initializer import roscpp_initialize from moveit.task_constructor import core, stages from moveit.core.planning_scene import PlanningScene -from geometry_msgs.msg import Vector3Stamped, Vector3 +from geometry_msgs.msg import Vector3Stamped, Vector3, PoseStamped from std_msgs.msg import Header PLANNING_GROUP = "manipulator" @@ -75,7 +75,7 @@ class PyMoveRelX(stages.MoveRelative): def __init__(self, x, planner, name="Move ±x"): stages.MoveRelative.__init__(self, name, planner) self.group = PLANNING_GROUP - self.ik_frame = "tool0" + self.ik_frame = PoseStamped(header=Header(frame_id="tool0")) self.setDirection( Vector3Stamped(header=Header(frame_id="base_link"), vector=Vector3(x, 0, 0)) ) @@ -131,5 +131,5 @@ def test_propagator(self): if __name__ == "__main__": - roscpp_init("test_mtc") + roscpp_initialize("test_mtc") rostest.rosrun("mtc", "trampoline", TestTrampolines) diff --git a/moveit_task_constructor/core/setup.py b/moveit_task_constructor/core/setup.py index 9d20d30..b3582dd 100644 --- a/moveit_task_constructor/core/setup.py +++ b/moveit_task_constructor/core/setup.py @@ -9,4 +9,19 @@ # specify location of root ("") package dir package_dir={"": "python/src"}, ) -setup(**d) +dist = setup(**d) + +# Remove moveit/__init__.py when building .deb packages +# Otherwise, the installation procedure will complain about conflicting files (with moveit_core) +try: + libdir = dist.command_obj[ + "install_lib" + ].install_dir # installation dir (.../lib/python3/dist-packages) + if libdir.startswith("/opt/ros"): + import os + import shutil + + os.remove(os.path.join(libdir, "moveit", "__init__.py")) + shutil.rmtree(os.path.join(libdir, "moveit", "__pycache__")) +except AttributeError as e: + pass diff --git a/moveit_task_constructor/core/src/CMakeLists.txt b/moveit_task_constructor/core/src/CMakeLists.txt index e25a036..45cefe5 100644 --- a/moveit_task_constructor/core/src/CMakeLists.txt +++ b/moveit_task_constructor/core/src/CMakeLists.txt @@ -18,6 +18,7 @@ add_library(${PROJECT_NAME} ${PROJECT_INCLUDE}/solvers/cartesian_path.h ${PROJECT_INCLUDE}/solvers/joint_interpolation.h ${PROJECT_INCLUDE}/solvers/pipeline_planner.h + ${PROJECT_INCLUDE}/solvers/multi_planner.h container.cpp cost_terms.cpp @@ -32,8 +33,9 @@ add_library(${PROJECT_NAME} solvers/planner_interface.cpp solvers/cartesian_path.cpp - solvers/pipeline_planner.cpp solvers/joint_interpolation.cpp + solvers/pipeline_planner.cpp + solvers/multi_planner.cpp ) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) target_include_directories(${PROJECT_NAME} diff --git a/moveit_task_constructor/core/src/marker_tools.cpp b/moveit_task_constructor/core/src/marker_tools.cpp index 6dbb5e5..5f738e3 100644 --- a/moveit_task_constructor/core/src/marker_tools.cpp +++ b/moveit_task_constructor/core/src/marker_tools.cpp @@ -1,7 +1,6 @@ #include #include #include -#include namespace vm = visualization_msgs; @@ -123,6 +122,8 @@ void generateMarkers(const moveit::core::RobotState& robot_state, const MarkerCa auto element_handler = [&](const T& element) { if (element && element->geometry) { createGeometryMarker(m, *element->geometry, element->origin, materialColor(*model, materialName(*element))); + if (m.scale.x == 0 && m.scale.y == 0 && m.scale.z == 0) + return; // skip zero-size marker m.pose = rviz_marker_tools::composePoses(robot_state.getGlobalLinkTransform(name), m.pose); callback(m, name); valid_found = true; diff --git a/moveit_task_constructor/core/src/solvers/cartesian_path.cpp b/moveit_task_constructor/core/src/solvers/cartesian_path.cpp index 9852470..4e9df5d 100644 --- a/moveit_task_constructor/core/src/solvers/cartesian_path.cpp +++ b/moveit_task_constructor/core/src/solvers/cartesian_path.cpp @@ -71,7 +71,7 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, // reach pose of forward kinematics return plan(from, *link, Eigen::Isometry3d::Identity(), to->getCurrentState().getGlobalLinkTransform(link), jmg, - timeout, result, path_constraints); + std::min(timeout, properties().get("timeout")), result, path_constraints); } bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, diff --git a/moveit_task_constructor/core/src/solvers/joint_interpolation.cpp b/moveit_task_constructor/core/src/solvers/joint_interpolation.cpp index 600fd6f..e972752 100644 --- a/moveit_task_constructor/core/src/solvers/joint_interpolation.cpp +++ b/moveit_task_constructor/core/src/solvers/joint_interpolation.cpp @@ -103,7 +103,8 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::Constraints& path_constraints) { - const auto start_time = std::chrono::steady_clock::now(); + timeout = std::min(timeout, properties().get("timeout")); + const auto deadline = std::chrono::steady_clock::now() + std::chrono::duration>(timeout); auto to{ from->diff() }; @@ -125,8 +126,7 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr } to->getCurrentStateNonConst().update(); - timeout = std::chrono::duration(std::chrono::steady_clock::now() - start_time).count(); - if (timeout <= 0.0) + if (std::chrono::steady_clock::now() >= deadline) return false; return plan(from, to, jmg, timeout, result, path_constraints); diff --git a/moveit_task_constructor/core/src/solvers/multi_planner.cpp b/moveit_task_constructor/core/src/solvers/multi_planner.cpp new file mode 100644 index 0000000..7603f76 --- /dev/null +++ b/moveit_task_constructor/core/src/solvers/multi_planner.cpp @@ -0,0 +1,98 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, 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: Michael Goerner, Robert Haschke + Desc: generate and validate a straight-line Cartesian path +*/ + +#include +#include +#include + +namespace moveit { +namespace task_constructor { +namespace solvers { + +void MultiPlanner::init(const core::RobotModelConstPtr& robot_model) { + for (const auto& p : *this) + p->init(robot_model); +} + +bool MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, + const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg, + double timeout, robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints) { + double remaining_time = std::min(timeout, properties().get("timeout")); + auto start_time = std::chrono::steady_clock::now(); + + for (const auto& p : *this) { + if (remaining_time < 0) + return false; // timeout + if (result) + result->clear(); + if (p->plan(from, to, jmg, remaining_time, result, path_constraints)) + return true; + + auto now = std::chrono::steady_clock::now(); + remaining_time -= std::chrono::duration(now - start_time).count(); + start_time = now; + } + return false; +} + +bool MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link, + const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, + const moveit::core::JointModelGroup* jmg, double timeout, + robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints) { + double remaining_time = std::min(timeout, properties().get("timeout")); + auto start_time = std::chrono::steady_clock::now(); + + for (const auto& p : *this) { + if (remaining_time < 0) + return false; // timeout + if (result) + result->clear(); + if (p->plan(from, link, offset, target, jmg, remaining_time, result, path_constraints)) + return true; + + auto now = std::chrono::steady_clock::now(); + remaining_time -= std::chrono::duration(now - start_time).count(); + start_time = now; + } + return false; +} +} // namespace solvers +} // namespace task_constructor +} // namespace moveit diff --git a/moveit_task_constructor/core/src/solvers/pipeline_planner.cpp b/moveit_task_constructor/core/src/solvers/pipeline_planner.cpp index 66d12d5..3337590 100644 --- a/moveit_task_constructor/core/src/solvers/pipeline_planner.cpp +++ b/moveit_task_constructor/core/src/solvers/pipeline_planner.cpp @@ -143,7 +143,7 @@ void initMotionPlanRequest(moveit_msgs::MotionPlanRequest& req, const PropertyMa const moveit::core::JointModelGroup* jmg, double timeout) { req.group_name = jmg->getName(); req.planner_id = p.get("planner"); - req.allowed_planning_time = timeout; + req.allowed_planning_time = std::min(timeout, p.get("timeout")); req.start_state.is_diff = true; // we don't specify an extra start state req.num_planning_attempts = p.get("num_planning_attempts"); diff --git a/moveit_task_constructor/core/src/solvers/planner_interface.cpp b/moveit_task_constructor/core/src/solvers/planner_interface.cpp index 617fb47..a58b110 100644 --- a/moveit_task_constructor/core/src/solvers/planner_interface.cpp +++ b/moveit_task_constructor/core/src/solvers/planner_interface.cpp @@ -47,6 +47,7 @@ namespace solvers { PlannerInterface::PlannerInterface() { auto& p = properties(); + p.declare("timeout", std::numeric_limits::infinity(), "timeout for planner (s)"); p.declare("max_velocity_scaling_factor", 1.0, "scale down max velocity by this factor"); p.declare("max_acceleration_scaling_factor", 1.0, "scale down max acceleration by this factor"); p.declare("time_parameterization", std::make_shared()); diff --git a/moveit_task_constructor/core/src/stages/compute_ik.cpp b/moveit_task_constructor/core/src/stages/compute_ik.cpp index 1f793f7..5ce320b 100644 --- a/moveit_task_constructor/core/src/stages/compute_ik.cpp +++ b/moveit_task_constructor/core/src/stages/compute_ik.cpp @@ -99,7 +99,7 @@ namespace { // TODO: move into MoveIt core, lift active_components_only_ from fcl to common interface bool isTargetPoseCollidingInEEF(const planning_scene::PlanningSceneConstPtr& scene, robot_state::RobotState& robot_state, Eigen::Isometry3d pose, - const robot_model::LinkModel* link, + const robot_model::LinkModel* link, const robot_model::JointModelGroup* jmg = nullptr, collision_detection::CollisionResult* collision_result = nullptr) { // consider all rigidly connected parent links as well const robot_model::LinkModel* parent = robot_model::RobotModel::getRigidlyConnectedParentLinkModel(link); @@ -130,6 +130,8 @@ bool isTargetPoseCollidingInEEF(const planning_scene::PlanningSceneConstPtr& sce collision_detection::CollisionRequest req; collision_detection::CollisionResult result; req.contacts = (collision_result != nullptr); + if (jmg) + req.group_name = jmg->getName(); collision_detection::CollisionResult& res = collision_result ? *collision_result : result; scene->checkCollision(req, res, robot_state, acm); return res.collision; @@ -311,7 +313,7 @@ void ComputeIK::compute() { collision_detection::CollisionResult collisions; robot_state::RobotState sandbox_state{ scene->getCurrentState() }; bool colliding = - !ignore_collisions && isTargetPoseCollidingInEEF(scene, sandbox_state, target_pose, link, &collisions); + !ignore_collisions && isTargetPoseCollidingInEEF(scene, sandbox_state, target_pose, link, jmg, &collisions); // frames at target pose and ik frame std::deque frame_markers; @@ -371,6 +373,7 @@ void ComputeIK::compute() { collision_detection::CollisionResult res; req.contacts = true; req.max_contacts = 1; + req.group_name = jmg->getName(); scene->checkCollision(req, res, *state); solution.feasible = ignore_collisions || !res.collision; if (!res.contacts.empty()) { diff --git a/moveit_task_constructor/core/src/stages/generate_pose.cpp b/moveit_task_constructor/core/src/stages/generate_pose.cpp index 8315bc9..5a0f033 100644 --- a/moveit_task_constructor/core/src/stages/generate_pose.cpp +++ b/moveit_task_constructor/core/src/stages/generate_pose.cpp @@ -70,7 +70,9 @@ void GeneratePose::compute() { if (upstream_solutions_.empty()) return; - planning_scene::PlanningScenePtr scene = upstream_solutions_.pop()->end()->scene()->diff(); + const SolutionBase& s = *upstream_solutions_.pop(); + planning_scene::PlanningSceneConstPtr scene = s.end()->scene()->diff(); + geometry_msgs::PoseStamped target_pose = properties().get("pose"); if (target_pose.header.frame_id.empty()) target_pose.header.frame_id = scene->getPlanningFrame(); @@ -80,6 +82,7 @@ void GeneratePose::compute() { } InterfaceState state(scene); + forwardProperties(*s.end(), state); // forward registered properties from received solution state.properties().set("target_pose", target_pose); SubTrajectory trajectory; diff --git a/moveit_task_constructor/core/src/stages/passthrough.cpp b/moveit_task_constructor/core/src/stages/passthrough.cpp index 786221e..4ca9348 100644 --- a/moveit_task_constructor/core/src/stages/passthrough.cpp +++ b/moveit_task_constructor/core/src/stages/passthrough.cpp @@ -44,7 +44,6 @@ #include #include -#include #include #include #include diff --git a/moveit_task_constructor/demo/package.xml b/moveit_task_constructor/demo/package.xml index c4ccda3..5e69295 100644 --- a/moveit_task_constructor/demo/package.xml +++ b/moveit_task_constructor/demo/package.xml @@ -1,12 +1,13 @@ moveit_task_constructor_demo - 0.0.1 + 0.1.2 demo tasks illustrating various capabilities of MTC. - simon Goldstein + Robert Haschke + Simon Goldstein Henning Kayser - Henning Kayser + Robert Haschke BSD @@ -24,4 +25,5 @@ moveit_resources_panda_moveit_config rostest + moveit_fake_controller_manager diff --git a/moveit_task_constructor/demo/scripts/current_state.py b/moveit_task_constructor/demo/scripts/current_state.py index 35e19ec..e8011db 100755 --- a/moveit_task_constructor/demo/scripts/current_state.py +++ b/moveit_task_constructor/demo/scripts/current_state.py @@ -2,10 +2,10 @@ # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages -from moveit.python_tools import roscpp_init +from moveit_commander.roscpp_initializer import roscpp_initialize import time -roscpp_init("mtc_tutorial_current_state") +roscpp_initialize("mtc_tutorial_current_state") # Create a task task = core.Task() diff --git a/moveit_task_constructor/demo/scripts/fallbacks.py b/moveit_task_constructor/demo/scripts/fallbacks.py index 8538d22..f99016c 100755 --- a/moveit_task_constructor/demo/scripts/fallbacks.py +++ b/moveit_task_constructor/demo/scripts/fallbacks.py @@ -2,10 +2,10 @@ # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages -from moveit.python_tools import roscpp_init +from moveit_commander.roscpp_initializer import roscpp_initialize import time -roscpp_init("mtc_tutorial_fallbacks") +roscpp_initialize("mtc_tutorial_fallbacks") # use cartesian and joint interpolation planners cartesianPlanner = core.CartesianPath() diff --git a/moveit_task_constructor/demo/scripts/fix_collision_objects.py b/moveit_task_constructor/demo/scripts/fix_collision_objects.py index a26e703..3fdd14e 100755 --- a/moveit_task_constructor/demo/scripts/fix_collision_objects.py +++ b/moveit_task_constructor/demo/scripts/fix_collision_objects.py @@ -2,10 +2,10 @@ # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages -from moveit.python_tools import roscpp_init +from moveit_commander.roscpp_initializer import roscpp_initialize import time -roscpp_init("mtc_tutorial_current_state") +roscpp_initialize("mtc_tutorial_current_state") # Create a task task = core.Task() diff --git a/moveit_task_constructor/demo/scripts/fixed_state.py b/moveit_task_constructor/demo/scripts/fixed_state.py index 1df7db7..7ada93e 100755 --- a/moveit_task_constructor/demo/scripts/fixed_state.py +++ b/moveit_task_constructor/demo/scripts/fixed_state.py @@ -3,11 +3,11 @@ from moveit.core import planning_scene from moveit.task_constructor import core, stages -from moveit.python_tools import roscpp_init +from moveit_commander.roscpp_initializer import roscpp_initialize from moveit.core.planning_scene import PlanningScene import time -roscpp_init("mtc_tutorial_current_state") +roscpp_initialize("mtc_tutorial_current_state") # Create a task diff --git a/moveit_task_constructor/demo/scripts/generate_pose.py b/moveit_task_constructor/demo/scripts/generate_pose.py index 5b89a00..2ada95f 100755 --- a/moveit_task_constructor/demo/scripts/generate_pose.py +++ b/moveit_task_constructor/demo/scripts/generate_pose.py @@ -1,15 +1,15 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages from geometry_msgs.msg import PoseStamped -from moveit.python_tools import roscpp_init +from moveit_commander.roscpp_initializer import roscpp_initialize import time -roscpp_init("mtc_tutorial_compute_ik") +roscpp_initialize("mtc_tutorial_compute_ik") # Specify the planning group -group = "panda_arm" +group = "air" # Create a task task = core.Task() @@ -33,7 +33,7 @@ # create an example pose wrt. the origin of the # panda arm link8 pose = PoseStamped() -pose.header.frame_id = "panda_link8" +pose.header.frame_id = "qbhand_base_link" # Calculate the inverse kinematics for the current robot state generatePose = stages.GeneratePose("generate pose") diff --git a/moveit_task_constructor/demo/scripts/modify_planning_scene.py b/moveit_task_constructor/demo/scripts/modify_planning_scene.py index 49bc062..930ee1b 100755 --- a/moveit_task_constructor/demo/scripts/modify_planning_scene.py +++ b/moveit_task_constructor/demo/scripts/modify_planning_scene.py @@ -1,14 +1,14 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages from moveit_msgs.msg import CollisionObject from shape_msgs.msg import SolidPrimitive from geometry_msgs.msg import PoseStamped -from moveit.python_tools import roscpp_init +from moveit_commander.roscpp_initializer import roscpp_initialize import time -roscpp_init("mtc_tutorial_modify_planning_scene") +roscpp_initialize("mtc_tutorial_modify_planning_scene") # Create a task task = core.Task() diff --git a/moveit_task_constructor/demo/scripts/pickplace.py b/moveit_task_constructor/demo/scripts/pickplace.py index 3e769c6..5318a29 100755 --- a/moveit_task_constructor/demo/scripts/pickplace.py +++ b/moveit_task_constructor/demo/scripts/pickplace.py @@ -1,18 +1,19 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 # -*- coding: utf-8 -*- -from moveit.python_tools import roscpp_init +from moveit_commander.roscpp_initializer import roscpp_initialize from moveit.task_constructor import core, stages from moveit_commander import PlanningSceneInterface from geometry_msgs.msg import PoseStamped, TwistStamped import time +import rospy -roscpp_init("pickplace") - +roscpp_initialize("pickplace") +rospy.loginfo("Z`_) +* Contributors: Robert Haschke + +0.1.1 (2023-02-15) +------------------ + +0.1.0 (2023-02-02) +------------------ +* Initial release +* Contributors: Robert Haschke, Michael Görner diff --git a/moveit_task_constructor/rviz_marker_tools/CMakeLists.txt b/moveit_task_constructor/rviz_marker_tools/CMakeLists.txt index 7312cb7..c3e17a5 100644 --- a/moveit_task_constructor/rviz_marker_tools/CMakeLists.txt +++ b/moveit_task_constructor/rviz_marker_tools/CMakeLists.txt @@ -6,6 +6,7 @@ find_package(catkin REQUIRED COMPONENTS visualization_msgs roscpp rviz + tf2_eigen ) catkin_package( diff --git a/moveit_task_constructor/rviz_marker_tools/package.xml b/moveit_task_constructor/rviz_marker_tools/package.xml index 17b0dc4..3bbdd9c 100644 --- a/moveit_task_constructor/rviz_marker_tools/package.xml +++ b/moveit_task_constructor/rviz_marker_tools/package.xml @@ -1,9 +1,10 @@ rviz_marker_tools - 0.0.0 + 0.1.2 Tools for marker creation / handling BSD + Robert Haschke Robert Haschke catkin @@ -12,4 +13,5 @@ geometry_msgs roscpp rviz + tf2_eigen diff --git a/moveit_task_constructor/rviz_marker_tools/src/marker_creation.cpp b/moveit_task_constructor/rviz_marker_tools/src/marker_creation.cpp index 8d68b2f..eae1913 100644 --- a/moveit_task_constructor/rviz_marker_tools/src/marker_creation.cpp +++ b/moveit_task_constructor/rviz_marker_tools/src/marker_creation.cpp @@ -158,12 +158,6 @@ void prepareMarker(vm::Marker& m, int marker_type) { m.points.clear(); m.colors.clear(); - // ensure valid scale - if (m.scale.x == 0 && m.scale.y == 0 && m.scale.z == 0) { - m.scale.x = 1.0; - m.scale.y = 1.0; - m.scale.z = 1.0; - } // ensure non-null orientation if (m.pose.orientation.w == 0 && m.pose.orientation.x == 0 && m.pose.orientation.y == 0 && m.pose.orientation.z == 0) m.pose.orientation.w = 1.0; @@ -188,6 +182,7 @@ vm::Marker& makeXYPlane(vm::Marker& m) { p[3].y = -1.0; p[3].z = 0.0; + m.scale.x = m.scale.y = m.scale.z = 1.0; prepareMarker(m, vm::Marker::TRIANGLE_LIST); m.points.push_back(p[0]); m.points.push_back(p[1]); @@ -217,6 +212,7 @@ vm::Marker& makeYZPlane(vm::Marker& m) { /// create a cone of given angle along the x-axis vm::Marker makeCone(double angle, vm::Marker& m) { + m.scale.x = m.scale.y = m.scale.z = 1.0; prepareMarker(m, vm::Marker::TRIANGLE_LIST); geometry_msgs::Point p[3]; p[0].x = p[0].y = p[0].z = 0.0; @@ -296,6 +292,7 @@ vm::Marker& makeArrow(vm::Marker& m, double scale, bool tip_at_origin) { } vm::Marker& makeText(vm::Marker& m, const std::string& text) { + m.scale.x = m.scale.y = m.scale.z = 1.0; prepareMarker(m, vm::Marker::TEXT_VIEW_FACING); m.text = text; return m; diff --git a/moveit_task_constructor/visualization/CHANGELOG.rst b/moveit_task_constructor/visualization/CHANGELOG.rst new file mode 100644 index 0000000..8b66959 --- /dev/null +++ b/moveit_task_constructor/visualization/CHANGELOG.rst @@ -0,0 +1,16 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package moveit_task_constructor_visualization +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.2 (2023-02-24) +------------------ + +0.1.1 (2023-02-15) +------------------ +* Remove unused eigen_conversions includes +* Contributors: Robert Haschke + +0.1.0 (2023-02-02) +------------------ +* Initial release +* Contributors: Robert Haschke, Michael Görner diff --git a/moveit_task_constructor/visualization/package.xml b/moveit_task_constructor/visualization/package.xml index 196503b..32e01e2 100644 --- a/moveit_task_constructor/visualization/package.xml +++ b/moveit_task_constructor/visualization/package.xml @@ -1,9 +1,10 @@ moveit_task_constructor_visualization - 0.0.0 + 0.1.2 Visualization tools for MoveIt Task Pipeline BSD + Robert Haschke Robert Haschke Michael Goerner diff --git a/moveit_task_constructor/visualization/visualization_tools/src/marker_visualization.cpp b/moveit_task_constructor/visualization/visualization_tools/src/marker_visualization.cpp index 131cd7c..9e3ceae 100644 --- a/moveit_task_constructor/visualization/visualization_tools/src/marker_visualization.cpp +++ b/moveit_task_constructor/visualization/visualization_tools/src/marker_visualization.cpp @@ -12,7 +12,6 @@ #endif #include #include -#include namespace moveit_rviz_plugin {