Merge branch 'main' into kp/job-manager

.github/workflows/nexus_integration_tests.yaml

@@ -40,6 +40,8 @@ jobs:
         rosdep update
         rosdep install --from-paths . -yir
     - name: build
-      run: /ros_entrypoint.sh colcon build --packages-up-to nexus_integration_tests --mixin release lld --cmake-args -DCMAKE_CXX_COMPILER_LAUNCHER=ccache
-    - name: test
+      run: /ros_entrypoint.sh colcon build --packages-up-to nexus_integration_tests nexus_motion_planner --mixin release lld --cmake-args -DCMAKE_CXX_COMPILER_LAUNCHER=ccache
+    - name: Test - Unit Tests
+      run: . ./install/setup.bash && RMW_IMPLEMENTATION=rmw_cyclonedds_cpp /ros_entrypoint.sh colcon test --packages-select nexus_motion_planner --event-handlers=console_direct+
+    - name: Test - Integration Test
       run: . ./install/setup.bash && cd nexus_integration_tests && RMW_IMPLEMENTATION=rmw_cyclonedds_cpp /ros_entrypoint.sh python3 -m unittest

README.md

@@ -1,6 +1,6 @@
 # NEXUS
-![](https://github.com/OpenSourceRobotics/nexus/workflows/style/badge.svg)
-![](https://github.com/OpenSourceRobotics/nexus/workflows/integration_tests/badge.svg)
+![](https://github.com/osrf/nexus/workflows/style/badge.svg)
+![](https://github.com/osrf/nexus/workflows/integration_tests/badge.svg)
 
 ![](./docs/media/nexus_architecture.png)
 
@@ -30,7 +30,7 @@ curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh
 ```bash
 mkdir ~/ws_nexus/src -p
 cd ~/ws_nexus/src/
-git clone [email protected]:OpenSourceRobotics/nexus
+git clone [email protected]:osrf/nexus
 vcs import . < nexus/abb.repos
 cd ~/ws_nexus
 rosdep install --from-paths src --ignore-src --rosdistro iron -y -r
@@ -50,7 +50,7 @@ See sample BTs for [system_orchestrator](nexus_integration_tests/config/system_o
 At present, capabilities are registered as plugins with the `orchestrator` at runtime.
 We support [these capabilities](./nexus_capabilities/src/capabilities/plugins.xml) out of the bo
 Each plugin may register one or more behavior tree nodes which can then be used to define processes as seen in the `place_on_conveyor.xml`.
-The framework to register capabilities and map them to processes that can be performed in ongoing work. See [this discussion](https://github.com/OpenSourceRobotics/nexus/discussions/369) for more details.
+The framework to register capabilities and map them to processes that can be performed in ongoing work. See [this discussion](https://github.com/osrf/nexus/discussions/32) for more details.
 
 ### Visualization [experimental]
 ![](./docs/media/bt_example.png)
@@ -77,7 +77,7 @@ TODO: Add a dedicated demo package.
 
 The `nexus_endpoints` package contains ROS topics, services and actions used by NEXUS. The package is generated from [nexus_endpoints.redf.yaml](./nexus_endpoints.redf.yaml) using `redf`. rust is required to generated the package, the easiest way to install rust is via [rustup](https://rustup.rs/).
 
-With rust installed, clone the redf repo at https://github.com/OpenSourceRobotics/redf, then run
+With rust installed, clone the redf repo at https://github.com/osrf/redf, then run
 ```bash
 cargo install --path .
 ```

nexus_capabilities/CHANGELOG.rst

@@ -0,0 +1,10 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package nexus_capabilities
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.1.1 (2023-11-20)
+------------------
+
+0.1.0 (2023-11-06)
+------------------
+* Provides abstract interfaces for defining new capabilities along with several out-of-the-box capabilities.

nexus_capabilities/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nexus_capabilities</name>
-  <version>0.0.1</version>
+  <version>0.1.1</version>
   <description>Utilities to describe and validate hardware capabilities</description>
   <maintainer email="[email protected]">Yadunund</maintainer>
   <license>Apache License 2.0</license>

nexus_capabilities/src/capabilities/plan_motion.cpp

@@ -120,6 +120,17 @@ make_request()
   req->max_velocity_scaling_factor = scale_speed;
   req->max_acceleration_scaling_factor = scale_speed;
 
+  auto maybe_read_only =
+    this->getInput<bool>("force_cache_mode_execute_read_only");
+  if (maybe_read_only.has_value())
+  {
+    req->force_cache_mode_execute_read_only = maybe_read_only.value();
+  }
+  else
+  {
+    req->force_cache_mode_execute_read_only = false;
+  }
+
   RCLCPP_DEBUG_STREAM(this->_logger,
     "planning to " << req->goal_pose.pose << " at frame " <<
       req->goal_pose.header.frame_id << " with cartesian " << req->cartesian << "and scaled speed of " <<

nexus_capabilities/src/capabilities/plan_motion.hpp

@@ -66,6 +66,9 @@ public: static BT::PortsList providedPorts()
       BT::InputPort<std::vector<moveit_msgs::msg::JointConstraint>>(
         "start_constraints",
         "OPTIONAL. If provided the the joint_constraints are used as the start condition. Else, the current state of the robot will be used as the start."),
+      BT::InputPort<bool>(
+        "force_cache_mode_execute_read_only",
+        "OPTIONAL. Set true to force cache mode to ExecuteReadOnly for this request."),
       BT::OutputPort<moveit_msgs::msg::RobotTrajectory>(
         "result", "The resulting trajectory.")};
   }

nexus_cmake/CHANGELOG.rst

@@ -0,0 +1,10 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package nexus_cmake
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.1.1 (2023-11-20)
+------------------
+
+0.1.0 (2023-11-06)
+------------------
+* Provides cmake macros and utilities for NEXUS.

nexus_cmake/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nexus_cmake</name>
-  <version>0.0.1</version>
+  <version>0.1.1</version>
   <description>cmake helpers for nexus</description>
   <maintainer email="[email protected]">Teo Koon Peng</maintainer>
   <license>Apache License 2.0</license>

nexus_common/CHANGELOG.rst

@@ -0,0 +1,10 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package nexus_common
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.1.1 (2023-11-20)
+------------------
+
+0.1.0 (2023-11-06)
+------------------
+* Provides reusable behavior tree modules and utilities.

nexus_common/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nexus_common</name>
-  <version>0.0.1</version>
+  <version>0.1.1</version>
   <description>Common code shared by NEXUS packages</description>
   <maintainer email="[email protected]">Teo Koon Peng</maintainer>
   <license>Apache License 2.0</license>

nexus_endpoints/CHANGELOG.rst

@@ -0,0 +1,10 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package nexus_endpoints
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.1.1 (2023-11-20)
+------------------
+
+0.1.0 (2023-11-06)
+------------------
+* Provides ``nexus_endpoints.hpp`` header only file generated via ``redf`` which contains APIs to access endpoint related information within NEXUS.

nexus_endpoints/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nexus_endpoints</name>
-  <version>0.0.1</version>
+  <version>0.1.1</version>
   <description>NEXUS</description>
   <maintainer email="[email protected]">Teo Koon Peng</maintainer>
   <license>Apache License 2.0</license>

nexus_integration_tests/CHANGELOG.rst

@@ -0,0 +1,10 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package nexus_integration_tests
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.1.1 (2023-11-20)
+------------------
+
+0.1.0 (2023-11-06)
+------------------
+* Provides test cases and a demo setup to validate core functionalities in NEXUS.

nexus_integration_tests/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nexus_integration_tests</name>
-  <version>0.0.0</version>
+  <version>0.1.1</version>
   <description>Integration test for system and workcell orchestrators</description>
   <maintainer email="[email protected]">Teo Koon Peng</maintainer>
   <license>Apache License 2.0</license>

nexus_lifecycle_manager/CHANGELOG.rst

@@ -0,0 +1,10 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package nexus_lifecycle_manager
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.1.1 (2023-11-20)
+------------------
+
+0.1.0 (2023-11-06)
+------------------
+* Provides ``LifecycleManager`` and ``LifecycleManagerClient`` APIs to manage the lifecycle states of nodes within NEXUS.

nexus_lifecycle_manager/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nexus_lifecycle_manager</name>
-  <version>0.0.1</version>
+  <version>0.1.1</version>
   <description>A controller/manager for the lifecycle nodes of the NEXUS system</description>
   <maintainer email="[email protected]">Alejandro Hernandez Cordero</maintainer>
   <license>Apache-2.0</license>

nexus_motion_planner/CHANGELOG.rst

@@ -0,0 +1,12 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package nexus_motion_planner
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.1.1 (2023-11-20)
+------------------
+* Use parent node options (`#24 <https://github.com/OpenSourceRobotics/nexus/issues/24>`_)
+* Contributors: Yadunund
+
+0.1.0 (2023-11-06)
+------------------
+* Provides ``nexus_motion_planner`` node which can be queried for motion plans for manipulators.

nexus_motion_planner/CMakeLists.txt

@@ -17,6 +17,7 @@ find_package(tf2 REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(trajectory_msgs REQUIRED)
 find_package(warehouse_ros REQUIRED)
+find_package(warehouse_ros_sqlite REQUIRED)
 
 include_directories(include)
 
@@ -27,25 +28,37 @@ set (motion_planner_server_dependencies
   rclcpp
   rclcpp_action
   rclcpp_lifecycle
-  warehouse_ros
   tf2
   tf2_ros
 )
 
+set (motion_plan_cache_dependencies
+  moveit_ros_planning_interface
+  rclcpp
+  warehouse_ros
+  warehouse_ros_sqlite
+)
+
 set (test_request_dependencies
+  moveit_msgs
   nexus_endpoints
   geometry_msgs
   rclcpp
   trajectory_msgs
 )
 
 #===============================================================================
-set(LIBRARY_NAME motion_planner_server_core)
+set(MOTION_PLAN_CACHE_LIBRARY_NAME motion_planner_server_core)
 set(EXECUTABLE_NAME motion_planner_server)
 
+# Motion plan cache library
+add_library(${MOTION_PLAN_CACHE_LIBRARY_NAME} src/motion_plan_cache.cpp)
+ament_target_dependencies(${MOTION_PLAN_CACHE_LIBRARY_NAME} ${motion_plan_cache_dependencies})
+
 # Server executable
 add_executable(${EXECUTABLE_NAME} src/main.cpp src/motion_planner_server.cpp)
 ament_target_dependencies(${EXECUTABLE_NAME} ${motion_planner_server_dependencies})
+target_link_libraries(${EXECUTABLE_NAME} ${MOTION_PLAN_CACHE_LIBRARY_NAME})
 
 # Test executable
 add_executable(test_request src/test_request.cpp)
@@ -64,6 +77,7 @@ install(
 )
 
 if(BUILD_TESTING)
+  find_package(ament_cmake_pytest REQUIRED)
   find_package(ament_cmake_uncrustify REQUIRED)
   find_package(launch_testing_ament_cmake REQUIRED)
   find_package(rmf_utils REQUIRED)
@@ -72,19 +86,37 @@ if(BUILD_TESTING)
     NAMES "rmf_code_style.cfg"
     PATHS "${rmf_utils_DIR}/../../../../share/rmf_utils/")
 
+  add_executable(test_motion_plan_cache src/test_motion_plan_cache.cpp)
+  target_link_libraries(test_motion_plan_cache ${MOTION_PLAN_CACHE_LIBRARY_NAME})
+
+  install(TARGETS
+      test_motion_plan_cache
+    RUNTIME DESTINATION lib/${PROJECT_NAME}
+  )
+
   ament_uncrustify(
     ARGN include src test
     CONFIG_FILE ${uncrustify_config_file}
     MAX_LINE_LENGTH 80
     LANGUAGE CPP
   )
 
-  # TODO(YV): Fix these tests.
-  # add_launch_test(
-  #   test/test_request.test.py
-  #   TIMEOUT 60
-  # )
+  ament_add_pytest_test(test_motion_plan_cache_py "test/test_motion_plan_cache.py"
+   WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  )
+
+  # Motion planner server test with cache mode unset
+  add_launch_test(
+    test/test_request.test.py
+    TIMEOUT 60
+  )
+
+  # Motion planner server test with cache mode set (so it uses the cache)
+  add_launch_test(
+    test/test_request_with_cache.test.py
+    TIMEOUT 60
+  )
 endif()
 
-ament_export_dependencies(${motion_planner_server_dependencies})
+ament_export_dependencies(${motion_planner_server_dependencies} ${motion_plan_cache_dependencies})
 ament_package()

nexus_motion_planner/config/planner_params.yaml

@@ -41,18 +41,4 @@ motion_planner_server:
       # The max_z of workspace bounding box.
     workspace_max_z: 1.0
     # The seconds within which the current robot state should be valid.
-    get_state_wait_seconds: 0.01
-    # Namespaced parameters for each robot when use_namespace is true.
-    # Is important to ensure move_group, robot_state_publisher and
-    # joint_state broadcaster all have the same namespace.
-    # Note: If use_namespace is false, pass these parameters directly.
-    abb_irb1300:
-      robot_description: ""
-      robot_description_semantic: ""
-      group_name: "manipulator"
-      # Group specific kinematic properties.
-      manipulator:
-        kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
-        kinematics_solver_search_resolution: 0.005
-        kinematics_solver_timeout: 0.005
-        kinematics_solver_attempts: 3
+    get_state_wait_seconds: 0.01