Build on jazzy

.github/workflows/ci-format.yml

@@ -12,19 +12,18 @@ jobs:
     name: Format
     runs-on: ubuntu-latest
     container:
-      image: osrf/ros:humble-desktop
+      image: osrf/ros:jazzy-desktop
 
     steps:
     - uses: actions/checkout@v2
 
     - name: Install clang-format
       run: |
        apt update
-       apt install -y python3-pip clang-format ros-humble-ament-cppcheck
+       apt install -y python3-pip clang-format ros-jazzy-ament-cppcheck pre-commit
        git config --global --add safe.directory `pwd`
-       pip install pre-commit
     - name: pre-commit
       shell: bash
       run: |
-        source /opt/ros/humble/setup.bash
+        source /opt/ros/jazzy/setup.bash
         pre-commit run -a

.github/workflows/doxygen-deploy.yml

@@ -2,7 +2,7 @@ name: "Doxygen Deployment"
 on:
   workflow_dispatch:
   push:
-    branches: [humble]
+    branches: [jazzy]
 
 jobs:
   deploy-doc:

.github/workflows/humble-binary-build.yml → .github/workflows/jazzy-binary-build.yml

@@ -1,20 +1,20 @@
-name: Humble Binary Build
+name: Jazzy Binary Build
 on:
    pull_request:
       branches:
-         - humble
+         - jazzy
    push:
       branches:
-         - humble
+         - jazzy
    schedule: # Run every morning to detect flakiness and broken dependencies
          - cron: '43 3 * * *'
 
 jobs:
   ci:
-    name: humble binary build
+    name: jazzy binary build
     runs-on: ubuntu-22.04
     container:
-        image: osrf/ros:humble-desktop
+        image: osrf/ros:jazzy-desktop
 
     steps:
         - uses: actions/checkout@v1
@@ -27,5 +27,5 @@ jobs:
                 rosdep init
                 rosdep update
                 rosdep install --from-paths . --ignore-src -r -y
-                source /opt/ros/humble/setup.bash
+                source /opt/ros/jazzy/setup.bash
                 colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-skip sm_multi_ur5_sim sm_panda_moveit2z_cb_inventory sm_atomic_hierarchy  sm_aws_warehouse_navigation  sm_dance_bot_warehouse_2 sm_multi_ur5_sim sm_three_some sm_atomic_lifecycle sm_branching sm_dance_bot_warehouse_3 sm_pack_ml sm_advanced_recovery_1 sm_atomic_mode_states sm_dance_bot sm_ferrari sm_pubsub_1 sm_atomic sm_atomic_services sm_dance_bot_strikes_back sm_multi_panda_sim sm_respira_1 sm_atomic_24hr sm_autoware_avp sm_dance_bot_warehouse sm_multi_stage_1 sm_panda_moveit2z_cb_inventory sm_dancebot_ue sm_dancebot_artgallery_ue sm_dancebot_fashion_ue sm_dancebot_mine_ue sm_dancebot_office_ue

.github/workflows/humble-bloom-release.yml → .github/workflows/jazzy-bloom-release.yml

@@ -1,20 +1,20 @@
-name: Humble bloom-release
+name: Jazzy bloom-release
 
 on:
   push:
     paths:
        - package.xml
        - '*/package.xml'
-    branches: humble
+    branches: jazzy
   workflow_dispatch:
 
 jobs:
   bloom-release:
     runs-on: ubuntu-latest
     container:
-      image: osrf/ros:humble-desktop
+      image: osrf/ros:jazzy-desktop
     env:
-      ROS_DISTRO: humble
+      ROS_DISTRO: jazzy
       PRERELEASE: true
       BASEDIR: ${{ github.workspace }}/.work
 
@@ -26,7 +26,7 @@ jobs:
         run: |
           apt-get update
           apt-get install -y python3-colcon-* python3-colcon-coveragepy-result
-          source /opt/ros/humble/setup.bash
+          source /opt/ros/jazzy/setup.bash
           rosdep update
           rosdep install --from-paths . --ignore-src -r -y --skip-keys warehouse_ros_mongo
           colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-up-to smacc2 smacc2_msgs

.github/workflows/humble-semi-binary-build.yml → .github/workflows/jazzy-semi-binary-build.yml

@@ -1,20 +1,20 @@
-name: Humble Semi-Binary Build
+name: Jazzy Semi-Binary Build
 on:
    pull_request:
       branches:
-         - humble
+         - jazzy
    push:
       branches:
-         - humble
+         - jazzy
    schedule: # Run every morning to detect flakiness and broken dependencies
          - cron: '43 3 * * *'
 
 jobs:
   ci:
-    name: humble semi-binary build
+    name: jazzy semi-binary build
     runs-on: ubuntu-22.04
     container:
-        image: osrf/ros:humble-desktop
+        image: osrf/ros:jazzy-desktop
 
     steps:
         - uses: actions/checkout@v1
@@ -26,8 +26,8 @@ jobs:
                 rm /etc/ros/rosdep/sources.list.d/20-default.list
                 rosdep init
                 rosdep update
-                vcs import < .github/SMACC2.humble.repos
+                vcs import < .github/SMACC2.jazzy.repos
                 rosdep install --from-paths . --ignore-src -r -y
-                source /opt/ros/humble/setup.bash
+                source /opt/ros/jazzy/setup.bash
 
                 colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-skip sm_multi_ur5_sim sm_atomic_hierarchy sm_aws_warehouse_navigation  sm_dance_bot_warehouse_2 sm_multi_ur5_sim sm_three_some sm_atomic_lifecycle sm_branching sm_dance_bot_warehouse_3 sm_pack_ml sm_advanced_recovery_1 sm_atomic_mode_states sm_dance_bot sm_ferrari sm_pubsub_1 sm_atomic sm_atomic_services sm_dance_bot_strikes_back sm_multi_panda_sim sm_respira_1 sm_atomic_24hr sm_autoware_avp sm_dance_bot_warehouse sm_multi_stage_1 sm_panda_moveit2z_cb_inventory

smacc2/include/smacc2/smacc_tracing/smacc_tracing.hpp

@@ -24,39 +24,39 @@
 extern "C"
 {
 #endif
-  DECLARE_TRACEPOINT(spinOnce)
+  _DECLARE_TRACEPOINT(spinOnce)
 
-  DECLARE_TRACEPOINT(smacc2_event, const char * event_type)
+  _DECLARE_TRACEPOINT(smacc2_event, const char * event_type)
 
-  DECLARE_TRACEPOINT(smacc2_state_update_start, const char * updatable_element_name)
+  _DECLARE_TRACEPOINT(smacc2_state_update_start, const char * updatable_element_name)
 
-  DECLARE_TRACEPOINT(smacc2_state_update_end, const char * updatable_element_name)
+  _DECLARE_TRACEPOINT(smacc2_state_update_end, const char * updatable_element_name)
 
-  DECLARE_TRACEPOINT(smacc2_state_onRuntimeConfigure_start, const char * state_name)
+  _DECLARE_TRACEPOINT(smacc2_state_onRuntimeConfigure_start, const char * state_name)
 
-  DECLARE_TRACEPOINT(smacc2_state_onRuntimeConfigure_end, const char * state_name)
+  _DECLARE_TRACEPOINT(smacc2_state_onRuntimeConfigure_end, const char * state_name)
 
-  DECLARE_TRACEPOINT(smacc2_state_onEntry_start, const char * state_name)
+  _DECLARE_TRACEPOINT(smacc2_state_onEntry_start, const char * state_name)
 
-  DECLARE_TRACEPOINT(smacc2_state_onEntry_end, const char * state_name)
+  _DECLARE_TRACEPOINT(smacc2_state_onEntry_end, const char * state_name)
 
-  DECLARE_TRACEPOINT(smacc2_state_onExit_start, const char * state_name)
+  _DECLARE_TRACEPOINT(smacc2_state_onExit_start, const char * state_name)
 
-  DECLARE_TRACEPOINT(smacc2_state_onExit_end, const char * state_name)
+  _DECLARE_TRACEPOINT(smacc2_state_onExit_end, const char * state_name)
 
-  DECLARE_TRACEPOINT(
+  _DECLARE_TRACEPOINT(
     smacc2_client_behavior_on_entry_start, const char * state_name, const char * orthogonal_name,
     const char * client_behavior_name)
 
-  DECLARE_TRACEPOINT(
+  _DECLARE_TRACEPOINT(
     smacc2_client_behavior_on_entry_end, const char * state_name, const char * orthogonal_name,
     const char * client_behavior_name)
 
-  DECLARE_TRACEPOINT(
+  _DECLARE_TRACEPOINT(
     smacc2_client_behavior_on_exit_start, const char * state_name, const char * orthogonal_name,
     const char * client_behavior_name)
 
-  DECLARE_TRACEPOINT(
+  _DECLARE_TRACEPOINT(
     smacc2_client_behavior_on_exit_end, const char * state_name, const char * orthogonal_name,
     const char * client_behavior_name)
 

smacc2/src/smacc2/smacc_state_machine.cpp

@@ -113,8 +113,8 @@ void ISmaccStateMachine::updateStatusMessage()
   if (currentStateInfo_ != nullptr)
   {
     RCLCPP_WARN_STREAM(
-      nh_->get_logger(), "[StateMachine] setting state active "
-                           << ": " << currentStateInfo_->getFullPath());
+      nh_->get_logger(),
+      "[StateMachine] setting state active " << ": " << currentStateInfo_->getFullPath());
 
     if (runMode_ == SMRunMode::DEBUG)
     {

smacc2/src/smacc2/smacc_tracing.cpp

@@ -22,80 +22,83 @@
 
 #ifdef TRACETOOLS_LTTNG_ENABLED
 #include <smacc2/smacc_tracing/trace_provider.hpp>
-#define CONDITIONAL_TP(...) tracepoint(TRACEPOINT_PROVIDER, __VA_ARGS__)
+#define CONDITIONAL_TP(...) tracepoint(TRACETOOLS_TRACEPOINT_PROVIDER, __VA_ARGS__)
 #else
 #define CONDITIONAL_TP(...)
 #endif
 
-void TRACEPOINT(spinOnce) { CONDITIONAL_TP(spinOnce); }
+void TRACETOOLS_TRACEPOINT(spinOnce) { CONDITIONAL_TP(spinOnce); }
 
-void TRACEPOINT(smacc2_event, const char * event_type) { CONDITIONAL_TP(smacc2_event, event_type); }
+void TRACETOOLS_TRACEPOINT(smacc2_event, const char * event_type)
+{
+  CONDITIONAL_TP(smacc2_event, event_type);
+}
 
-void TRACEPOINT(smacc2_state_update_start, const char * updatable_element_name)
+void TRACETOOLS_TRACEPOINT(smacc2_state_update_start, const char * updatable_element_name)
 {
   CONDITIONAL_TP(smacc2_state_update_start, updatable_element_name);
 }
 
-void TRACEPOINT(smacc2_state_update_end, const char * updatable_element_name)
+void TRACETOOLS_TRACEPOINT(smacc2_state_update_end, const char * updatable_element_name)
 {
   CONDITIONAL_TP(smacc2_state_update_end, updatable_element_name);
 }
 
-void TRACEPOINT(smacc2_state_onRuntimeConfigure_start, const char * state_name)
+void TRACETOOLS_TRACEPOINT(smacc2_state_onRuntimeConfigure_start, const char * state_name)
 {
   CONDITIONAL_TP(smacc2_state_onRuntimeConfigure_start, state_name);
 }
 
-void TRACEPOINT(smacc2_state_onRuntimeConfigure_end, const char * state_name)
+void TRACETOOLS_TRACEPOINT(smacc2_state_onRuntimeConfigure_end, const char * state_name)
 {
   CONDITIONAL_TP(smacc2_state_onRuntimeConfigure_end, state_name);
 }
 
-void TRACEPOINT(smacc2_state_onEntry_start, const char * state_name)
+void TRACETOOLS_TRACEPOINT(smacc2_state_onEntry_start, const char * state_name)
 {
   CONDITIONAL_TP(smacc2_state_onEntry_start, state_name);
 }
 
-void TRACEPOINT(smacc2_state_onEntry_end, const char * state_name)
+void TRACETOOLS_TRACEPOINT(smacc2_state_onEntry_end, const char * state_name)
 {
   CONDITIONAL_TP(smacc2_state_onEntry_end, state_name);
 }
 
-void TRACEPOINT(smacc2_state_onExit_start, const char * state_name)
+void TRACETOOLS_TRACEPOINT(smacc2_state_onExit_start, const char * state_name)
 {
   CONDITIONAL_TP(smacc2_state_onExit_start, state_name);
 }
 
-void TRACEPOINT(smacc2_state_onExit_end, const char * state_name)
+void TRACETOOLS_TRACEPOINT(smacc2_state_onExit_end, const char * state_name)
 {
   CONDITIONAL_TP(smacc2_state_onExit_end, state_name);
 }
 
-void TRACEPOINT(
+void TRACETOOLS_TRACEPOINT(
   smacc2_client_behavior_on_entry_start, const char * state_name, const char * orthogonal_name,
   const char * client_behavior_name)
 {
   CONDITIONAL_TP(
     smacc2_client_behavior_on_entry_start, state_name, orthogonal_name, client_behavior_name);
 }
 
-void TRACEPOINT(
+void TRACETOOLS_TRACEPOINT(
   smacc2_client_behavior_on_entry_end, const char * state_name, const char * orthogonal_name,
   const char * client_behavior_name)
 {
   CONDITIONAL_TP(
     smacc2_client_behavior_on_entry_end, state_name, orthogonal_name, client_behavior_name);
 }
 
-void TRACEPOINT(
+void TRACETOOLS_TRACEPOINT(
   smacc2_client_behavior_on_exit_start, const char * state_name, const char * orthogonal_name,
   const char * client_behavior_name)
 {
   CONDITIONAL_TP(
     smacc2_client_behavior_on_exit_start, state_name, orthogonal_name, client_behavior_name);
 }
 
-void TRACEPOINT(
+void TRACETOOLS_TRACEPOINT(
   smacc2_client_behavior_on_exit_end, const char * state_name, const char * orthogonal_name,
   const char * client_behavior_name)
 {

smacc2_client_library/moveit2z_client/CMakeLists.txt

@@ -13,7 +13,7 @@ find_package(smacc2 REQUIRED)
 find_package(moveit_ros_planning_interface REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_ros REQUIRED)
-find_package(yaml_cpp_vendor REQUIRED)
+find_package(yaml-cpp REQUIRED)
 
 #set(YAML_CPP_LIBRARIES yaml-cpp)
 
@@ -23,7 +23,7 @@ tf2
 tf2_ros
 smacc2
 tf2_geometry_msgs
-yaml_cpp_vendor
+yaml-cpp
 )
 
 include_directories(include)
@@ -53,8 +53,6 @@ add_library(${PROJECT_NAME}
   src/moveit2z_client/client_behaviors/cb_move_last_trajectory_initial_state.cpp
 )
 
-target_link_libraries(${PROJECT_NAME} ${YAML_CPP_LIBRARIES})
-
 ament_target_dependencies(${PROJECT_NAME}
                           ${dependencies})
 

smacc2_client_library/moveit2z_client/include/moveit2z_client/cl_moveit2z.hpp

@@ -24,8 +24,8 @@
 //#include <smacc2/smacc_signal.h>
 #include <smacc2/smacc.hpp>
 
-#include <moveit/move_group_interface/move_group_interface.h>
-#include <moveit/planning_scene_interface/planning_scene_interface.h>
+#include <moveit/move_group_interface/move_group_interface.hpp>
+#include <moveit/planning_scene_interface/planning_scene_interface.hpp>
 
 #include <geometry_msgs/msg/transform.hpp>
 #include <geometry_msgs/msg/vector3.hpp>

smacc2_client_library/moveit2z_client/package.xml

@@ -14,7 +14,6 @@
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
   <depend>moveit_ros_planning_interface</depend>
-  <depend>yaml_cpp_vendor</depend>
   <depend>ament_index_cpp</depend>
   <depend>yaml-cpp</depend>
   <depend>tf2_geometry_msgs</depend>

smacc2_client_library/moveit2z_client/src/moveit2z_client/cl_moveit2z.cpp

@@ -40,7 +40,7 @@ ClMoveit2z::~ClMoveit2z() {}
 void ClMoveit2z::onInitialize()
 {
   moveGroupClientInterface = std::make_shared<MoveGroupInterface>(getNode(), options_);
-  planningSceneInterface = std::make_shared<PlanningSceneInterface>(options_.move_group_namespace_);
+  planningSceneInterface = std::make_shared<PlanningSceneInterface>(options_.move_group_namespace);
 }
 
 void ClMoveit2z::postEventMotionExecutionSucceded()

smacc2_client_library/moveit2z_client/src/moveit2z_client/client_behaviors/cb_move_cartesian_relative.cpp

@@ -92,7 +92,6 @@ void CbMoveCartesianRelative::moveRelativeCartesian(
   double fraction = movegroupClient->computeCartesianPath(
     waypoints,
     0.01,  // eef_step
-    0.00,  // jump_threshold
     trajectory);
 
   moveit::core::MoveItErrorCode behaviorResult;
@@ -114,7 +113,7 @@ void CbMoveCartesianRelative::moveRelativeCartesian(
     moveit::planning_interface::MoveGroupInterface::Plan grasp_pose_plan;
 
     // grasp_pose_plan.start_state_ = *(moveGroupInterface.getCurrentState());
-    grasp_pose_plan.trajectory_ = trajectory;
+    grasp_pose_plan.trajectory = trajectory;
     behaviorResult = movegroupClient->execute(grasp_pose_plan);
   }
 

smacc2_client_library/nav2z_client/custom_planners/backward_global_planner/include/backward_global_planner/backward_global_planner.hpp

@@ -71,7 +71,8 @@ class BackwardGlobalPlanner : public nav2_core::GlobalPlanner
    * @return      The sequence of poses to get from start to goal, if any
    */
   virtual nav_msgs::msg::Path createPlan(
-    const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal);
+    const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal,
+    std::function<bool()> cancel_checker);
 
 private:
   // rclcpp::Node::SharedPtr nh_;

smacc2_client_library/nav2z_client/custom_planners/backward_global_planner/src/backward_global_planner/backward_global_planner.cpp

@@ -226,7 +226,8 @@ void BackwardGlobalPlanner::createDefaultBackwardPath(
 ******************************************************************************************************************
 */
 nav_msgs::msg::Path BackwardGlobalPlanner::createPlan(
-  const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal)
+  const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal,
+  std::function<bool()> cancel_checker)
 {
   RCLCPP_INFO_STREAM(
     nh_->get_logger(), "[BackwardGlobalPlanner] goal frame id: "