Updating sm_simple_action_client launch file

smacc2/include/smacc2/smacc_signal_detector.hpp

@@ -53,6 +53,8 @@ class SignalDetector
 
   void stop();
 
+  void terminateScheduler();
+
   void pollingLoop();
 
   template <typename EventType>
@@ -105,6 +107,14 @@ class SignalDetector
 
 void onSigQuit(int sig);
 
+// Global variables for graceful shutdown handling
+// Declared here, defined in signal_detector.cpp
+extern std::atomic<bool> g_shutdown_requested;
+extern SignalDetector * g_signal_detector;
+
+// Signal handler for graceful shutdown (SIGINT/SIGTERM)
+void onSignalShutdown(int sig);
+
 // Main entry point for any SMACC state machine
 // It instantiates and starts the specified state machine type
 // it uses two threads: a new thread and the current one.
@@ -113,6 +123,9 @@ void onSigQuit(int sig);
 template <typename StateMachineType>
 void run(ExecutionModel executionModel = ExecutionModel::SINGLE_THREAD_SPINNER)
 {
+  // Register signal handlers for graceful shutdown
+  ::signal(SIGINT, onSignalShutdown);
+  ::signal(SIGTERM, onSignalShutdown);
   ::signal(SIGQUIT, onSigQuit);
 
   // create the asynchronous state machine scheduler
@@ -121,6 +134,9 @@ void run(ExecutionModel executionModel = ExecutionModel::SINGLE_THREAD_SPINNER)
   // create the signalDetector component
   SignalDetector signalDetector(&scheduler1, executionModel);
 
+  // Store global reference for signal handler access
+  g_signal_detector = &signalDetector;
+
   // create the asynchronous state machine processor
   SmaccFifoScheduler::processor_handle sm =
     scheduler1.create_processor<StateMachineType>(&signalDetector);
@@ -135,6 +151,19 @@ void run(ExecutionModel executionModel = ExecutionModel::SINGLE_THREAD_SPINNER)
 
   // use the  main thread for the signal detector component (waiting actionclient requests)
   signalDetector.pollingLoop();
+
+  // After polling loop exits (due to shutdown), terminate the scheduler from main thread
+  // This is safe to call here (not from signal handler) as we're in the main thread context
+  RCLCPP_INFO(rclcpp::get_logger("SMACC"), "Polling loop exited. Terminating scheduler...");
+  signalDetector.terminateScheduler();
+
+  // Wait for scheduler thread to finish
+  RCLCPP_INFO(rclcpp::get_logger("SMACC"), "Waiting for scheduler thread to join...");
+  schedulerThread.join();
+  RCLCPP_INFO(rclcpp::get_logger("SMACC"), "Scheduler thread terminated. Shutdown complete.");
+
+  // Clear global reference
+  g_signal_detector = nullptr;
 }
 
 struct SmExecution
@@ -149,6 +178,9 @@ struct SmExecution
 template <typename StateMachineType>
 SmExecution * run_async()
 {
+  // Register signal handlers for graceful shutdown
+  ::signal(SIGINT, onSignalShutdown);
+  ::signal(SIGTERM, onSignalShutdown);
   ::signal(SIGQUIT, onSigQuit);
 
   SmExecution * ret = new SmExecution();
@@ -159,6 +191,9 @@ SmExecution * run_async()
   // create the signalDetector component
   ret->signalDetector = new SignalDetector(ret->scheduler1);
 
+  // Store global reference for signal handler access
+  g_signal_detector = ret->signalDetector;
+
   // create the asynchronous state machine processor
   ret->sm = ret->scheduler1->create_processor<StateMachineType>(ret->signalDetector);
 

smacc2/src/smacc2/signal_detector.cpp

@@ -36,6 +36,11 @@
 namespace smacc2
 {
 using namespace std::chrono_literals;
+
+// Define global variables for graceful shutdown handling
+std::atomic<bool> g_shutdown_requested{false};
+SignalDetector * g_signal_detector = nullptr;
+
 /**
 ******************************************************************************************************************
 * SignalDetector()
@@ -218,6 +223,20 @@ void SignalDetector::join() { signalDetectorThread_.join(); }
  */
 void SignalDetector::stop() { end_ = true; }
 
+/**
+ ******************************************************************************************************************
+ * terminateScheduler()
+ ******************************************************************************************************************
+ */
+void SignalDetector::terminateScheduler()
+{
+  if (scheduler_)
+  {
+    RCLCPP_INFO(getLogger(), "[SignalDetector] Terminating scheduler...");
+    scheduler_->terminate();
+  }
+}
+
 /**
  ******************************************************************************************************************
  * poll()
@@ -367,6 +386,27 @@ void SignalDetector::pollingLoop()
   }
 }
 
+void onSignalShutdown(int sig)
+{
+  // IMPORTANT: Signal handlers can only call async-signal-safe functions
+  // We must NOT call complex C++ methods here (like terminateScheduler)
+  // as they may use mutexes/condition variables which are not signal-safe
+
+  // Set global shutdown flag (atomic operation - signal-safe)
+  g_shutdown_requested = true;
+
+  // Stop the signal detector loop (atomic operation - signal-safe)
+  if (g_signal_detector)
+  {
+    g_signal_detector->stop();
+  }
+
+  // Trigger ROS2 shutdown (this handles its own signal safety)
+  rclcpp::shutdown();
+
+  // Note: Scheduler termination will happen from main thread after polling loop exits
+}
+
 void onSigQuit(int)
 {
   RCLCPP_INFO(rclcpp::get_logger("SMACC"), "SignalDetector: SIGQUIT received.");

smacc2_client_library/cl_moveit2z/CMakeLists.txt

@@ -15,8 +15,6 @@ find_package(tf2 REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(yaml_cpp_vendor REQUIRED)
 
-#set(YAML_CPP_LIBRARIES yaml-cpp)
-
 set(dependencies
 moveit_ros_planning_interface
 tf2
@@ -28,35 +26,30 @@ yaml_cpp_vendor
 
 include_directories(include)
 
-## Declare a C++ library
+## Declare a C++ library (header-only implementations with compilation anchor)
 add_library(${PROJECT_NAME}
-  src/cl_moveit2z/cl_moveit2z.cpp
-  src/cl_moveit2z/common.cpp
-  src/cl_moveit2z/components/cp_trajectory_history.cpp
-  src/cl_moveit2z/components/cp_grasping_objects.cpp
-
-  src/cl_moveit2z/client_behaviors/cb_execute_last_trajectory.cpp
-  src/cl_moveit2z/client_behaviors/cb_move_end_effector_relative.cpp
-  src/cl_moveit2z/client_behaviors/cb_move_end_effector.cpp
-  src/cl_moveit2z/client_behaviors/cb_circular_pivot_motion.cpp
-  src/cl_moveit2z/client_behaviors/cb_move_known_state.cpp
-  src/cl_moveit2z/client_behaviors/cb_move_joints.cpp
-  src/cl_moveit2z/client_behaviors/cb_pouring_motion.cpp
-  src/cl_moveit2z/client_behaviors/cb_detach_object.cpp
-  src/cl_moveit2z/client_behaviors/cb_undo_last_trajectory.cpp
-  src/cl_moveit2z/client_behaviors/cb_move_end_effector_trajectory.cpp
-  src/cl_moveit2z/client_behaviors/cb_end_effector_rotate.cpp
-  src/cl_moveit2z/client_behaviors/cb_move_cartesian_relative.cpp
-  src/cl_moveit2z/client_behaviors/cb_move_cartesian_relative2.cpp
-  src/cl_moveit2z/client_behaviors/cb_move_named_target.cpp
-  src/cl_moveit2z/client_behaviors/cb_attach_object.cpp
-  src/cl_moveit2z/client_behaviors/cb_move_last_trajectory_initial_state.cpp
+  src/cl_moveit2z/cl_moveit2z.cpp  # Compilation anchor stub
 )
 
+# All implementations are header-only (inline):
+# src/cl_moveit2z/common.cpp  # PHASE 1: NOW HEADER-ONLY (inline stream operators)
+# src/cl_moveit2z/components/cp_trajectory_history.cpp  # PHASE 2: NOW HEADER-ONLY (inline methods)
+# src/cl_moveit2z/components/cp_grasping_objects.cpp  # PHASE 3: NOW HEADER-ONLY (inline methods)
+
+# ALL CLIENT BEHAVIORS ARE NOW HEADER-ONLY
+# src/cl_moveit2z/client_behaviors/cb_move_end_effector.cpp  # HEADER-ONLY
+# src/cl_moveit2z/client_behaviors/cb_move_known_state.cpp  # HEADER-ONLY
+# src/cl_moveit2z/client_behaviors/cb_move_joints.cpp  # HEADER-ONLY
+# src/cl_moveit2z/client_behaviors/cb_undo_last_trajectory.cpp  # HEADER-ONLY
+# src/cl_moveit2z/client_behaviors/cb_move_end_effector_trajectory.cpp  # HEADER-ONLY
+# src/cl_moveit2z/client_behaviors/cb_end_effector_rotate.cpp  # HEADER-ONLY
+# src/cl_moveit2z/client_behaviors/cb_move_cartesian_relative2.cpp  # HEADER-ONLY
+# src/cl_moveit2z/client_behaviors/cb_circular_pivot_motion.cpp  # HEADER-ONLY
+
 target_link_libraries(${PROJECT_NAME} ${YAML_CPP_LIBRARIES})
 
 ament_target_dependencies(${PROJECT_NAME}
-                          ${dependencies})
+  ${dependencies})
 
 ament_export_include_directories(include)
 ament_export_libraries(${PROJECT_NAME})

smacc2_client_library/cl_moveit2z/include/cl_moveit2z/cl_moveit2z.hpp

@@ -91,11 +91,25 @@ class ClMoveit2z : public smacc2::ISmaccClient
 
   virtual ~ClMoveit2z();
 
-  void onInitialize() override;
+  inline void onInitialize() override
+  {
+    moveGroupClientInterface =
+      std::make_shared<moveit::planning_interface::MoveGroupInterface>(getNode(), options_);
+    planningSceneInterface = std::make_shared<moveit::planning_interface::PlanningSceneInterface>(
+      options_.move_group_namespace_);
+  }
 
-  void postEventMotionExecutionSucceded();
+  inline void postEventMotionExecutionSucceded()
+  {
+    RCLCPP_INFO(getLogger(), "[ClMoveit2z] Post Motion Success Event");
+    postEventMotionExecutionSucceded_();
+  }
 
-  void postEventMotionExecutionFailed();
+  inline void postEventMotionExecutionFailed()
+  {
+    RCLCPP_INFO(getLogger(), "[ClMoveit2z] Post Motion Failure Event");
+    postEventMotionExecutionFailed_();
+  }
 
   template <typename TOrthogonal, typename TSourceObject>
   void onOrthogonalAllocation()
@@ -125,7 +139,10 @@ class ClMoveit2z : public smacc2::ISmaccClient
     return this->getStateMachine()->createSignalConnection(onFailed_, callback, object);
   }
 
-  const moveit::planning_interface::MoveGroupInterface::Options & getOptions() const;
+  inline const moveit::planning_interface::MoveGroupInterface::Options & getOptions() const
+  {
+    return options_;
+  }
 
 private:
   std::function<void()> postEventMotionExecutionSucceded_;

smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors.hpp

@@ -20,23 +20,17 @@
 #pragma once
 
 // BASIC MANIPULATION BEHAVIORSz
-#include "client_behaviors/cb_move_cartesian_relative.hpp"
 #include "client_behaviors/cb_move_end_effector.hpp"
-#include "client_behaviors/cb_move_end_effector_relative.hpp"
 #include "client_behaviors/cb_move_joints.hpp"
 #include "client_behaviors/cb_move_known_state.hpp"
-#include "client_behaviors/cb_move_named_target.hpp"
 
 // ADVANCED MANIPULATION BEHAVIORS
 #include "client_behaviors/cb_circular_pivot_motion.hpp"
 #include "client_behaviors/cb_end_effector_rotate.hpp"
 #include "client_behaviors/cb_move_cartesian_relative2.hpp"
 #include "client_behaviors/cb_move_end_effector_trajectory.hpp"
-#include "client_behaviors/cb_pouring_motion.hpp"
 
 // HISTORY BASED BEHAVIRORS
-#include "client_behaviors/cb_execute_last_trajectory.hpp"
-#include "client_behaviors/cb_move_last_trajectory_initial_state.hpp"
 #include "client_behaviors/cb_undo_last_trajectory.hpp"
 
 // GRASPING BEHAVIORS

smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_attach_object.hpp

@@ -21,23 +21,83 @@
 #pragma once
 
 #include <cl_moveit2z/cl_moveit2z.hpp>
+#include <cl_moveit2z/components/cp_grasping_objects.hpp>
+#include <moveit_msgs/msg/collision_object.hpp>
 #include <smacc2/smacc.hpp>
 
 namespace cl_moveit2z
 {
+/**
+ * @brief Client behavior that attaches a collision object to the robot gripper
+ *
+ * This behavior attaches a previously created grasping object to the robot's
+ * end effector. The object must exist in the CpGraspingComponent's graspingObjects
+ * map. Upon successful attachment, the object is added to the planning scene
+ * and attached to the gripper using the finger tip links.
+ */
 class CbAttachObject : public smacc2::SmaccAsyncClientBehavior
 {
 public:
-  CbAttachObject(std::string targetObjectName);
+  /**
+   * @brief Constructor with object name
+   * @param targetObjectName Name of the object to attach (must exist in CpGraspingComponent)
+   */
+  inline CbAttachObject(std::string targetObjectName) : targetObjectName_(targetObjectName) {}
 
-  CbAttachObject();
+  /**
+   * @brief Default constructor
+   */
+  inline CbAttachObject() {}
 
-  virtual void onEntry() override;
+  /**
+   * @brief Called when the behavior is entered
+   *
+   * Retrieves the target object from CpGraspingComponent, adds it to the planning
+   * scene, and attaches it to the gripper. Posts success or failure event based
+   * on whether the object was found.
+   */
+  inline void onEntry() override
+  {
+    cl_moveit2z::ClMoveit2z * moveGroup;
+    this->requiresClient(moveGroup);
 
-  virtual void onExit() override;
+    cl_moveit2z::CpGraspingComponent * graspingComponent;
+    this->requiresComponent(graspingComponent);
 
-  std::string targetObjectName_;
+    moveit_msgs::msg::CollisionObject targetCollisionObject;
+    bool found = graspingComponent->getGraspingObject(targetObjectName_, targetCollisionObject);
+
+    if (found)
+    {
+      RCLCPP_INFO_STREAM(getLogger(), "[CbAttachObject] Attaching object: " << targetObjectName_);
+
+      targetCollisionObject.operation = moveit_msgs::msg::CollisionObject::ADD;
+      targetCollisionObject.header.stamp = getNode()->now();
+
+      moveGroup->planningSceneInterface->applyCollisionObject(targetCollisionObject);
+      graspingComponent->currentAttachedObjectName = targetObjectName_;
 
-private:
+      moveGroup->moveGroupClientInterface->attachObject(
+        targetObjectName_, graspingComponent->gripperLink_, graspingComponent->fingerTipNames);
+
+      this->postSuccessEvent();
+    }
+    else
+    {
+      RCLCPP_ERROR_STREAM(
+        getLogger(),
+        "[CbAttachObject] Object not found in grasping component: " << targetObjectName_);
+      this->postFailureEvent();
+    }
+  }
+
+  /**
+   * @brief Called when the behavior is exited
+   */
+  inline void onExit() override {}
+
+  /// Name of the target object to attach
+  std::string targetObjectName_;
 };
+
 }  // namespace cl_moveit2z