Merge branch 'ros2' into ros2-new

.github/workflows/ci.yaml

@@ -19,26 +19,27 @@ jobs:
       fail-fast: false
       matrix:
         env:
+          # - IMAGE: rolling-source
+          #   NAME: ccov
+          #   TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_FLAGS="--coverage"
           - IMAGE: rolling-source
-            NAME: ccov
-            TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_FLAGS="--coverage"
           - IMAGE: rolling-source
             CLANG_TIDY: pedantic
-          - IMAGE: rolling-source
+          - IMAGE: jazzy-source
             NAME: asan
             # Add fast_unwind_on_malloc=0 to fix stacktraces being too short or do not make sense
             # see https://github.com/google/sanitizers/wiki/AddressSanitizer
             # Disable alloc/dealloc mismatch warnings: https://github.com/ros2/rclcpp/pull/1324
             DOCKER_RUN_OPTS: >-
-              -e PRELOAD=libasan.so.5
+              -e PRELOAD=libasan.so.8
               -e LSAN_OPTIONS="suppressions=$PWD/.github/workflows/lsan.suppressions,fast_unwind_on_malloc=0"
               -e ASAN_OPTIONS="new_delete_type_mismatch=0,alloc_dealloc_mismatch=0"
             TARGET_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer -O1 -g"
 
     env:
       CLANG_TIDY_ARGS: -quiet -export-fixes ${{ github.workspace }}/.work/clang-tidy-fixes.yaml
       DOCKER_IMAGE: moveit/moveit2:${{ matrix.env.IMAGE }}
-      UNDERLAY: /root/ws_moveit/install
+      UNDERLAY: ${{ endsWith(matrix.env.IMAGE, '-source') && '/root/ws_moveit/install' || ''}}
       # TODO: Port to ROS2
       # DOWNSTREAM_WORKSPACE: "github:ubi-agni/mtc_demos#master github:TAMS-Group/mtc_pour#master"
       TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Release

.github/workflows/prerelease.yaml

@@ -5,24 +5,24 @@ name: pre-release
 
 on:
   workflow_dispatch:
+    inputs:
+      ROS_DISTRO:
+        type: string
+        required: true
+        description: 'ROS distribution codename:'
+        default: noetic
 
 permissions:
   contents: read # to fetch code (actions/checkout)
 
 jobs:
   default:
-    strategy:
-      matrix:
-        distro: [noetic]
-
     env:
-      # https://github.com/ros-industrial/industrial_ci/issues/666
-      BUILDER: catkin_make_isolated
-      ROS_DISTRO: ${{ matrix.distro }}
+      ROS_DISTRO: ${{ inputs.ROS_DISTRO }}
       PRERELEASE: true
       BASEDIR: ${{ github.workspace }}/.work
 
-    name: "${{ matrix.distro }}"
+    name: "${{ inputs.ROS_DISTRO }}"
     runs-on: ubuntu-latest
     steps:
       - name: "Free up disk space"

.gitmodules

@@ -3,3 +3,6 @@
 	url = https://github.com/pybind/pybind11
 	branch = smart_holder
 	shallow = true
+[submodule "core/src/scope_guard"]
+	path = core/src/scope_guard
+	url = https://github.com/ricab/scope_guard

README.md

@@ -24,7 +24,7 @@ This repository provides the following branches:
 
 ## Tutorial
 
-We provide a tutorial for a pick-and-place pipeline without bells & whistles [as part of the MoveIt tutorials](https://ros-planning.github.io/moveit_tutorials/doc/moveit_task_constructor/moveit_task_constructor_tutorial.html).
+We provide a tutorial for a pick-and-place pipeline without bells & whistles [as part of the MoveIt tutorials](https://moveit.github.io/moveit_tutorials/doc/moveit_task_constructor/moveit_task_constructor_tutorial.html).
 
 ## Roadmap
 

capabilities/CMakeLists.txt

@@ -3,11 +3,14 @@ project(moveit_task_constructor_capabilities)
 
 find_package(ament_cmake REQUIRED)
 
+find_package(fmt REQUIRED)
 find_package(Boost REQUIRED)
 find_package(moveit_common REQUIRED)
 moveit_package()
 find_package(moveit_core REQUIRED)
 find_package(moveit_ros_move_group REQUIRED)
+find_package(moveit_ros_planning REQUIRED)
+find_package(moveit_task_constructor_core REQUIRED)
 find_package(moveit_task_constructor_msgs REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp_action REQUIRED)
@@ -18,9 +21,12 @@ add_library(${PROJECT_NAME} SHARED
 )
 ament_target_dependencies(${PROJECT_NAME}
                           Boost
+                          fmt
                           rclcpp_action
                           moveit_core
                           moveit_ros_move_group
+                          moveit_ros_planning
+                          moveit_task_constructor_core
                           moveit_task_constructor_msgs
 )
 
@@ -31,6 +37,8 @@ install(TARGETS ${PROJECT_NAME}
 pluginlib_export_plugin_description_file(moveit_ros_move_group capabilities_plugin_description.xml)
 ament_export_dependencies(moveit_core)
 ament_export_dependencies(moveit_ros_move_group)
+ament_export_dependencies(moveit_ros_planning)
+ament_export_dependencies(moveit_task_constructor_core)
 ament_export_dependencies(moveit_task_constructor_msgs)
 ament_export_dependencies(pluginlib)
 ament_export_dependencies(rclcpp_action)

capabilities/package.xml

@@ -12,6 +12,7 @@
 
 	<buildtool_depend>ament_cmake</buildtool_depend>
 
+	<depend>fmt</depend>
 	<depend>moveit_core</depend>
 	<depend>moveit_ros_move_group</depend>
 	<depend>moveit_ros_planning</depend>

capabilities/src/execute_task_solution_capability.cpp

@@ -43,9 +43,7 @@
 #include <moveit/move_group/capability_names.h>
 #include <moveit/robot_state/conversions.h>
 #include <moveit/utils/message_checks.h>
-#include <moveit/moveit_cpp/moveit_cpp.h>
-
-#include <boost/algorithm/string/join.hpp>
+#include <fmt/format.h>
 
 namespace {
 
@@ -164,8 +162,8 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
 			if (!joint_names.empty()) {
 				group = findJointModelGroup(*model, joint_names);
 				if (!group) {
-					RCLCPP_ERROR_STREAM(LOGGER, "Could not find JointModelGroup that actuates {"
-					                                << boost::algorithm::join(joint_names, ", ") << "}");
+					RCLCPP_ERROR_STREAM(LOGGER, fmt::format("Could not find JointModelGroup that actuates {{{}}}",
+					                                        fmt::join(joint_names, ", ")));
 					return false;
 				}
 				RCLCPP_DEBUG(LOGGER, "Using JointModelGroup '%s' for execution", group->getName().c_str());
@@ -184,11 +182,12 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
 		exec_traj.controller_name = sub_traj.execution_info.controller_names;
 
 		/* TODO add action feedback and markers */
-		exec_traj.effect_on_success = [this,
-		                               &scene_diff = const_cast<::moveit_msgs::msg::PlanningScene&>(sub_traj.scene_diff),
+		exec_traj.effect_on_success = [this, &scene_diff = const_cast<::moveit_msgs::msg::PlanningScene&>(sub_traj.scene_diff),
 		                               description](const plan_execution::ExecutableMotionPlan* /*plan*/) {
+			// Never modify joint state directly (only via robot trajectories)
 			scene_diff.robot_state.joint_state = sensor_msgs::msg::JointState();
 			scene_diff.robot_state.multi_dof_joint_state = sensor_msgs::msg::MultiDOFJointState();
+			scene_diff.robot_state.is_diff = true;  // silent empty JointState msg error
 
 			if (!moveit::core::isEmpty(scene_diff)) {
 				RCLCPP_DEBUG_STREAM(LOGGER, "apply effect of " << description);

core/CMakeLists.txt

@@ -4,13 +4,15 @@ project(moveit_task_constructor_core)
 find_package(ament_cmake REQUIRED)
 
 find_package(Boost REQUIRED)
+find_package(fmt REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(moveit_common REQUIRED)
 moveit_package()
 find_package(moveit_core REQUIRED)
 find_package(moveit_ros_planning REQUIRED)
 find_package(moveit_ros_planning_interface REQUIRED)
 find_package(moveit_task_constructor_msgs REQUIRED)
+find_package(py_binding_tools REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rviz_marker_tools REQUIRED)
 find_package(tf2_eigen REQUIRED)
@@ -25,8 +27,8 @@ add_compile_options(-fvisibility-inlines-hidden)
 set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/moveit/task_constructor)
 
 add_subdirectory(src)
-# TODO: enable python wrapper
-#add_subdirectory(python)
+ament_python_install_package(moveit PACKAGE_DIR python/moveit)
+add_subdirectory(python)
 add_subdirectory(test)
 
 install(DIRECTORY include/ DESTINATION include
@@ -41,6 +43,7 @@ ament_export_dependencies(moveit_core)
 ament_export_dependencies(moveit_ros_planning)
 ament_export_dependencies(moveit_ros_planning_interface)
 ament_export_dependencies(moveit_task_constructor_msgs)
+ament_export_dependencies(py_binding_tools)
 ament_export_dependencies(rclcpp)
 ament_export_dependencies(rviz_marker_tools)
 ament_export_dependencies(tf2_eigen)

core/doc/tutorials/properties.rst

@@ -97,11 +97,6 @@ You can obtain a reference to the the PropertyMap of a stage like so
     :start-after: [propertyTut10]
     :end-before: [propertyTut10]
 
-.. literalinclude:: ../../../demo/scripts/properties.py
-    :language: python
-    :start-after: [propertyTut11]
-    :end-before: [propertyTut11]
-
 
 As mentioned, each stage contains a PropertyMap.
 Stages communicate to each other via their interfaces.

core/include/moveit/python/task_constructor/properties.h

@@ -1,7 +1,7 @@
 #pragma once
 
 #include <pybind11/smart_holder.h>
-#include <moveit/python/pybind_rosmsg_typecasters.h>
+#include <py_binding_tools/ros_msg_typecasters.h>
 #include <moveit/task_constructor/properties.h>
 #include <boost/any.hpp>
 #include <typeindex>
@@ -35,12 +35,12 @@ class PropertyConverter : protected PropertyConverterBase
 	static boost::any fromPython(const pybind11::object& po) { return pybind11::cast<T>(po); }
 
 	template <class Q = T>
-	typename std::enable_if<ros::message_traits::IsMessage<Q>::value, std::string>::type rosMsgName() {
-		return ros::message_traits::DataType<T>::value();
+	typename std::enable_if<rosidl_generator_traits::is_message<Q>::value, std::string>::type rosMsgName() {
+		return rosidl_generator_traits::name<Q>();
 	}
 
 	template <class Q = T>
-	typename std::enable_if<!ros::message_traits::IsMessage<Q>::value, std::string>::type rosMsgName() {
+	typename std::enable_if<!rosidl_generator_traits::is_message<Q>::value, std::string>::type rosMsgName() {
 		return std::string();
 	}
 };

core/include/moveit/task_constructor/container.h

@@ -51,6 +51,10 @@ class ContainerBase : public Stage
 	PRIVATE_CLASS(ContainerBase)
 	using pointer = std::unique_ptr<ContainerBase>;
 
+	/// Explicitly enable/disable pruning
+	void setPruning(bool pruning) { setProperty("pruning", pruning); }
+	bool pruning() const { return properties().get<bool>("pruning"); }
+
 	size_t numChildren() const;
 	Stage* findChild(const std::string& name) const;
 	Stage* operator[](int index) const;

core/include/moveit/task_constructor/cost_terms.h

@@ -95,6 +95,7 @@ class LambdaCostTerm : public TrajectoryCostTerm
 	LambdaCostTerm(const SubTrajectorySignature& term);
 	LambdaCostTerm(const SubTrajectoryShortSignature& term);
 
+	using TrajectoryCostTerm::operator();
 	double operator()(const SubTrajectory& s, std::string& comment) const override;
 
 protected:
@@ -132,6 +133,8 @@ class PathLength : public TrajectoryCostTerm
 	PathLength(std::vector<std::string> joints);
 	/// Limit measurements to given joints and use given weighting
 	PathLength(std::map<std::string, double> j) : joints(std::move(j)) {}
+
+	using TrajectoryCostTerm::operator();
 	double operator()(const SubTrajectory& s, std::string& comment) const override;
 
 	std::map<std::string, double> joints;  //< joint weights
@@ -145,6 +148,8 @@ class DistanceToReference : public TrajectoryCostTerm
 	                    std::map<std::string, double> w = std::map<std::string, double>());
 	DistanceToReference(const std::map<std::string, double>& ref, Mode m = Mode::AUTO,
 	                    std::map<std::string, double> w = std::map<std::string, double>());
+
+	using TrajectoryCostTerm::operator();
 	double operator()(const SubTrajectory& s, std::string& comment) const override;
 
 	moveit_msgs::msg::RobotState reference;
@@ -156,6 +161,7 @@ class DistanceToReference : public TrajectoryCostTerm
 class TrajectoryDuration : public TrajectoryCostTerm
 {
 public:
+	using TrajectoryCostTerm::operator();
 	double operator()(const SubTrajectory& s, std::string& comment) const override;
 };
 
@@ -167,6 +173,7 @@ class LinkMotion : public TrajectoryCostTerm
 
 	std::string link_name;
 
+	using TrajectoryCostTerm::operator();
 	double operator()(const SubTrajectory& s, std::string& comment) const override;
 };
 
@@ -191,6 +198,7 @@ class Clearance : public TrajectoryCostTerm
 
 	std::function<double(double)> distance_to_cost;
 
+	using TrajectoryCostTerm::operator();
 	double operator()(const SubTrajectory& s, std::string& comment) const override;
 };
 

core/include/moveit/task_constructor/solvers/cartesian_path.h

@@ -52,6 +52,10 @@ class CartesianPath : public PlannerInterface
 public:
 	CartesianPath();
 
+	void setIKFrame(const geometry_msgs::msg::PoseStamped& pose) { setProperty("ik_frame", pose); }
+	void setIKFrame(const Eigen::Isometry3d& pose, const std::string& link);
+	void setIKFrame(const std::string& link) { setIKFrame(Eigen::Isometry3d::Identity(), link); }
+
 	void setStepSize(double step_size) { setProperty("step_size", step_size); }
 	void setJumpThreshold(double jump_threshold) { setProperty("jump_threshold", jump_threshold); }
 	void setMinFraction(double min_fraction) { setProperty("min_fraction", min_fraction); }