Merge branches 'simplify-node-creation' and 'master' into ros2

Author: rhaschke

.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"

capabilities/CMakeLists.txt

@@ -8,6 +8,8 @@ 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)
@@ -21,6 +23,8 @@ ament_target_dependencies(${PROJECT_NAME}
                           rclcpp_action
                           moveit_core
                           moveit_ros_move_group
+                          moveit_ros_planning
+                          moveit_task_constructor_core
                           moveit_task_constructor_msgs
 )
 
@@ -31,6 +35,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)

core/CMakeLists.txt

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

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>

core/include/moveit/task_constructor/stage.h

@@ -365,6 +365,7 @@ class Generator : public ComputeBase
 
 	virtual bool canCompute() const = 0;
 	virtual void compute() = 0;
+	void spawn(InterfaceState&& from, InterfaceState&& to, SubTrajectory&& trajectory);
 	void spawn(InterfaceState&& state, SubTrajectory&& trajectory);
 	void spawn(InterfaceState&& state, double cost) {
 		SubTrajectory trajectory;

core/include/moveit/task_constructor/stage_p.h

@@ -136,6 +136,7 @@ class StagePrivate
 	void sendBackward(InterfaceState&& from, const InterfaceState& to, const SolutionBasePtr& solution);
 	template <Interface::Direction>
 	inline void send(const InterfaceState& start, InterfaceState&& end, const SolutionBasePtr& solution);
+	void spawn(InterfaceState&& from, InterfaceState&& to, const SolutionBasePtr& solution);
 	void spawn(InterfaceState&& state, const SolutionBasePtr& solution);
 	void connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution);
 

core/include/moveit/task_constructor/stages/noop.h

@@ -0,0 +1,64 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2024, Sherbrooke 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: Captain Yoshi */
+
+#pragma once
+
+#include <moveit/task_constructor/stage.h>
+#include <moveit/planning_scene/planning_scene.h>
+
+namespace moveit {
+namespace task_constructor {
+namespace stages {
+
+/** no-op stage, which doesn't modify the interface state nor adds a trajectory.
+ *  However, it can be used to store custom stage properties,
+ *  which in turn can be queried post-planning to steer the execution.
+ */
+
+class NoOp : public PropagatingEitherWay
+{
+public:
+	NoOp(const std::string& name = "no-op") : PropagatingEitherWay(name){};
+
+private:
+	bool compute(const InterfaceState& state, planning_scene::PlanningScenePtr& scene, SubTrajectory& /*trajectory*/,
+	             Interface::Direction /*dir*/) override {
+		scene = state.scene()->diff();
+		return true;
+	};
+};
+}  // namespace stages
+}  // namespace task_constructor
+}  // namespace moveit

core/include/moveit/task_constructor/storage.h

@@ -95,10 +95,9 @@ class InterfaceState
 	 */
 	struct Priority : std::tuple<Status, unsigned int, double>
 	{
-		Priority(unsigned int depth, double cost, Status status = ENABLED)
-		  : std::tuple<Status, unsigned int, double>(status, depth, cost) {
-			assert(std::isfinite(cost));
-		}
+		Priority(unsigned int depth, double cost, Status status)
+		  : std::tuple<Status, unsigned int, double>(status, depth, cost) {}
+		Priority(unsigned int depth, double cost) : Priority(depth, cost, std::isfinite(cost) ? ENABLED : PRUNED) {}
 		// Constructor copying depth and cost, but modifying its status
 		Priority(const Priority& other, Status status) : Priority(other.depth(), other.cost(), status) {}
 

core/package.xml

@@ -19,6 +19,7 @@
 	<depend>moveit_ros_planning</depend>
 	<depend>moveit_ros_planning_interface</depend>
 	<depend>moveit_task_constructor_msgs</depend>
+	<depend>py_binding_tools</depend>
 	<depend>rclcpp</depend>
 	<depend>rviz_marker_tools</depend>
 	<depend>tf2_eigen</depend>

core/python/bindings/src/core.cpp

@@ -86,9 +86,8 @@ void setForwardedProperties(Stage& self, const py::object& names) {
 			for (auto item : names)
 				s.emplace(item.cast<std::string>());
 	} catch (const py::cast_error& e) {
-		// manually translate cast_error to type error
-		PyErr_SetString(PyExc_TypeError, e.what());
-		throw py::error_already_set();
+		// translate cast_error to type_error with an informative message
+		throw py::type_error("Expecting a string or a list of strings");
 	}
 	self.setForwardedProperties(s);
 }