Launch file improvement and standardization

smacc2/include/smacc2/client_behaviors/cb_call_service.hpp

@@ -86,14 +86,13 @@ class CbServiceCall : public smacc2::SmaccAsyncClientBehavior
 
   std::shared_future<std::shared_ptr<typename ServiceType::Response>> resultFuture_;
 
-  typename std::shared_ptr<typename ServiceType::Response> result_;
-  std::chrono::milliseconds pollRate_;
-
 protected:
   //rclcpp::NodeHandle nh_;
   std::shared_ptr<rclcpp::Client<ServiceType>> client_;
   std::string serviceName_;
   std::shared_ptr<typename ServiceType::Request> request_;
+  typename std::shared_ptr<typename ServiceType::Response> result_;
+  std::chrono::milliseconds pollRate_;
 
   virtual void onServiceResponse(std::shared_ptr<typename ServiceType::Response> /*result*/)
   {

smacc2/package.xml

@@ -8,8 +8,7 @@
 
   <author email="pablo@ibrobotics.com">Pablo Inigo Blasco</author>
   <author email="brett@robosoft.ai">Brett Aldrich</author>
-  <maintainer email="pablo@ibrobotics.com">Pablo Inigo Blasco</maintainer>
-  <maintainer email="denis@stoglrobotics.de">Denis Stogl</maintainer>
+  <maintainer email="brett@robosoft.ai">Brett Aldrich</maintainer>
   <license>Apache-2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>

smacc2_client_library/cl_nav2z/src/cl_nav2z/client_behaviors/cb_position_control_free_space.cpp

@@ -50,8 +50,6 @@ void CbPositionControlFreeSpace::onEntry()
   geometry_msgs::msg::Pose currentPose = pose->toPoseMsg();
 
   rclcpp::Rate loop_rate(10);
-  double countAngle = 0;
-  auto prevyaw = tf2::getYaw(currentPose.orientation);
 
   // PID controller gains (proportional, integral, and derivative)
   double kp_linear = 0.5;

smacc2_client_library/cl_nav2z/src/cl_nav2z/client_behaviors/cb_spiral_motion.cpp

@@ -99,7 +99,6 @@ void CbSpiralMotion::onEntry()
       cmd_vel.linear.x = maxLinearVelocity;
     }
 
-    float signVal = (cmd_vel.angular.z >= 0.0f) ? 1.0f : -1.0f;
     // cmd_vel.angular.z -= signVal * angularAcceleration * dt;
 
     float ellapsedTimeFactor = current_time.seconds() / spiralMotionDuration.seconds();

smacc2_client_library/cl_nav2z/src/cl_nav2z/components/waypoints_navigator/cp_waypoints_navigator.cpp

@@ -478,7 +478,7 @@ void CpWaypointNavigatorBase::loadWayPointsFromFile(std::string filepath)
 
     if (wp_node != NULL)
     {
-      for (int64_t i = 0; i < wp_node->size(); ++i)
+      for (std::size_t i = 0; i < wp_node->size(); ++i)
       {
         // Parse waypoint entries on YAML
         geometry_msgs::msg::Pose wp;
@@ -551,7 +551,7 @@ void CpWaypointNavigatorBase::loadWayPointsFromFile2(std::string filepath)
 
     if (wp_node != NULL)
     {
-      for (int64_t i = 0; i < wp_node->size(); ++i)
+      for (std::size_t i = 0; i < wp_node->size(); ++i)
       {
         // Parse waypoint entries on YAML
         geometry_msgs::msg::Pose wp;

smacc2_dev_tools/sm_reference_lib_run_commands.md

@@ -1,23 +1,55 @@
 ```
-ros2 launch sm_cl_ros2_timer_unit_test_1 sm_cl_ros2_timer_unit_test_1.py
+ros2 launch sm_advanced_recovery_1 sm_advanced_recovery_1.py
+```
+
+```
+ros2 launch sm_atomic sm_atomic.py
+```
+
+```
+ros2 launch sm_atomic_lifecycle sm_atomic_lifecycle.py
+```
+
+```
+ros2 launch sm_atomic_mode_states sm_atomic_mode_states.py
+```
+
+```
+ros2 launch sm_branching sm_branching.py
 ```
 
 ```
 ros2 launch sm_cl_keyboard_unit_test_1 sm_cl_keyboard_unit_test_1.py
 ```
 
 ```
-ros2 launch sm_panda_cl_moveit2z_cb_inventory sm_panda_cl_moveit2z_cb_inventory.launch.py
+ros2 launch sm_cl_ros2_timer_unit_test_1 sm_cl_ros2_timer_unit_test_1.py
 ```
 
 ```
-ros2 launch sm_multi_stage_1 sm_multi_stage_1.launch
+ros2 launch sm_multi_stage_1 sm_multi_stage_1.py
 ```
 
 ```
-ros2 launch sm_atomic_mode_states sm_atomic_mode_states.py
+ros2 launch sm_multithread_test_1 sm_multithread_test_1.py
 ```
 
 ```
-ros2 launch sm_atomic_lifecycle sm_atomic_lifecycle.py
+ros2 launch sm_multithread_test_1 sm_multithread_test_1_single.py
+```
+
+```
+ros2 launch sm_pack_ml sm_pack_ml.py
+```
+
+```
+ros2 launch sm_panda_cl_moveit2z_cb_inventory sm_panda_cl_moveit2z_cb_inventory.py
+```
+
+```
+ros2 launch sm_simple_action_client sm_simple_action_client.py
+```
+
+```
+ros2 launch sm_three_some sm_three_some.py
 ```

smacc2_sm_reference_library/sm_advanced_recovery_1/README.md

@@ -32,7 +32,7 @@ source install/setup.bash
 And then run the launch file...
 
 ```
-ros2 launch sm_advanced_recovery_1 sm_advanced_recovery_1.launch
+ros2 launch sm_advanced_recovery_1 sm_advanced_recovery_1.py
 ```
 
  <h2>Viewer Instructions</h2>

smacc2_sm_reference_library/sm_advanced_recovery_1/launch/sm_advanced_recovery_1.py

@@ -59,17 +59,17 @@ def generate_launch_description():
 
     # Construct logging prefix for state machine node
     if log_dir:
-        state_machine_log = os.path.join(log_dir, f"sm_advanced_recovery_1_node_{timestamp}.log")
-        state_machine_prefix = f"konsole --hold -p tabtitle='SM Advanced Recovery 1' -e bash -c 'RCUTILS_COLORIZED_OUTPUT=1 \"$@\" 2>&1 | tee {state_machine_log}; exec bash' -- "
+        state_machine_log = os.path.join(log_dir, f"state_machine_{timestamp}.log")
+        state_machine_prefix = f"konsole --hold -p tabtitle='SM Advanced Recovery' -e bash -c 'RCUTILS_COLORIZED_OUTPUT=1 \"$@\" 2>&1 | tee {state_machine_log}; exec bash' -- "
     else:
-        state_machine_prefix = "konsole --hold -p tabtitle='SM Advanced Recovery 1' -e"
+        state_machine_prefix = "konsole --hold -p tabtitle='SM Advanced Recovery' -e"
 
-    # Construct logging prefix for keyboard client node
+    # Construct logging prefix for keyboard server node
     if log_dir:
-        keyboard_log = os.path.join(log_dir, f"keyboard_server_node_{timestamp}.log")
-        keyboard_prefix = f"konsole --hold -p tabtitle='Keyboard Client' -e bash -c 'RCUTILS_COLORIZED_OUTPUT=1 \"$@\" 2>&1 | tee {keyboard_log}; exec bash' -- "
+        keyboard_log = os.path.join(log_dir, f"keyboard_server_{timestamp}.log")
+        keyboard_prefix = f"konsole --hold -p tabtitle='Keyboard Server' -e bash -c 'RCUTILS_COLORIZED_OUTPUT=1 \"$@\" 2>&1 | tee {keyboard_log}; exec bash' -- "
     else:
-        keyboard_prefix = "konsole --hold -p tabtitle='Keyboard Client' -e"
+        keyboard_prefix = "konsole --hold -p tabtitle='Keyboard Server' -e"
 
     return LaunchDescription(
         [
@@ -79,13 +79,15 @@ def generate_launch_description():
                 name="sm_advanced_recovery_1",
                 output="screen",
                 prefix=state_machine_prefix,
+                arguments=["--ros-args", "--log-level", "DEBUG"],
             ),
             Node(
                 package="cl_keyboard",
                 executable="keyboard_server_node.py",
                 name="keyboard_server_node",
                 output="screen",
                 prefix=keyboard_prefix,
+                arguments=["--ros-args", "--log-level", "INFO"],
             ),
-        ]
+        ],
     )

smacc2_sm_reference_library/sm_atomic_lifecycle/launch/sm_atomic_lifecycle.py

@@ -67,9 +67,9 @@ def generate_launch_description():
     # Construct logging prefix for lifecycle example node
     if log_dir:
         lifecycle_log = os.path.join(log_dir, f"lifecycle_example_node_{timestamp}.log")
-        lifecycle_prefix = f"konsole --hold -p tabtitle='Lifecycle Example' -e bash -c 'RCUTILS_COLORIZED_OUTPUT=1 \"$@\" 2>&1 | tee {lifecycle_log}; exec bash' -- "
+        lifecycle_prefix = f"konsole --hold -p tabtitle='Lifecycle Example Node' -e bash -c 'RCUTILS_COLORIZED_OUTPUT=1 \"$@\" 2>&1 | tee {lifecycle_log}; exec bash' -- "
     else:
-        lifecycle_prefix = "konsole --hold -p tabtitle='Lifecycle Example' -e"
+        lifecycle_prefix = "konsole --hold -p tabtitle='Lifecycle Example Node' -e"
 
     return LaunchDescription(
         [

smacc2_sm_reference_library/sm_branching/README.md

@@ -30,10 +30,9 @@ source install/setup.bash
 ```
 
 And then run the launch file...
-! refactor state machine name
 
 ```
-ros2 launch sm_branching sm_branching.launch
+ros2 launch sm_branching sm_branching.py
 ```
 
  <h2>Viewer Instructions</h2>

smacc2_sm_reference_library/sm_branching/launch/sm_branching.py

@@ -0,0 +1,81 @@
+# Copyright 2021 RobosoftAI Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+from datetime import datetime
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+
+
+def setup_log_directory():
+    """
+    Creates timestamped log directory with error handling.
+    Returns: (log_dir_path, timestamp) tuple
+    """
+    timestamp = datetime.now().strftime("%Y-%m-%d-%H-%M-%S")
+
+    # Primary log directory location
+    log_dir = os.path.join(os.path.expanduser("~"), ".ros", "log", f"{timestamp}-sm_branching")
+
+    try:
+        os.makedirs(log_dir, mode=0o755, exist_ok=True)
+        print(f"[Launch] Log directory created: {log_dir}")
+        return log_dir, timestamp
+    except PermissionError as e:
+        # Fallback to /tmp if ~/.ros is not writable
+        fallback_dir = os.path.join("/tmp", "sm_branching_logs", timestamp)
+        print(f"[Launch] WARNING: Cannot create log directory at {log_dir}")
+        print(f"[Launch] Permission denied: {e}")
+        print(f"[Launch] Using fallback directory: {fallback_dir}")
+        try:
+            os.makedirs(fallback_dir, mode=0o755, exist_ok=True)
+            return fallback_dir, timestamp
+        except Exception as fallback_error:
+            print(f"[Launch] ERROR: Cannot create fallback directory: {fallback_error}")
+            print(f"[Launch] Logs will only be displayed in konsole terminals")
+            return None, timestamp
+    except OSError as e:
+        print(f"[Launch] ERROR: Failed to create log directory: {e}")
+        print(f"[Launch] Logs will only be displayed in konsole terminals")
+        return None, timestamp
+
+
+def generate_launch_description():
+    # Setup logging directory
+    log_dir, timestamp = setup_log_directory()
+
+    # Get package share directory for config file
+    package_share_dir = get_package_share_directory("sm_branching")
+    config_file = os.path.join(package_share_dir, "config", "sm_branching_config.yaml")
+
+    # Construct logging prefix for state machine node
+    if log_dir:
+        state_machine_log = os.path.join(log_dir, f"state_machine_{timestamp}.log")
+        state_machine_prefix = f"konsole --hold -p tabtitle='SM Branching' -e bash -c 'RCUTILS_COLORIZED_OUTPUT=1 \"$@\" 2>&1 | tee {state_machine_log}; exec bash' -- "
+    else:
+        state_machine_prefix = "konsole --hold -p tabtitle='SM Branching' -e"
+
+    return LaunchDescription(
+        [
+            Node(
+                package="sm_branching",
+                executable="sm_branching_node",
+                name="sm_branching",
+                output="screen",
+                prefix=state_machine_prefix,
+                parameters=[config_file],
+            ),
+        ],
+    )

smacc2_sm_reference_library/sm_cl_keyboard_unit_test_1/launch/sm_cl_keyboard_unit_test_1.py

@@ -27,9 +27,9 @@ def generate_launch_description():
             Node(
                 package="cl_keyboard",
                 executable="keyboard_server_node.py",
-                name="cl_keyboard",
+                name="keyboard_server_node",
                 output="screen",
-                prefix="xterm -hold -e",
+                prefix="konsole --hold -p tabtitle='Keyboard Server' -e",
                 arguments=["--ros-args", "--log-level", "INFO"],
             ),
         ]

smacc2_sm_reference_library/sm_cl_keyboard_unit_test_1/package.xml

@@ -4,7 +4,7 @@
   <name>sm_cl_keyboard_unit_test_1</name>
   <version>3.0.1</version>
   <description>The sm_cl_keyboard_unit_test_1 package</description>
-  <maintainer email="pablo@ibrobotics.com">Pablo Inigo Blasco</maintainer>
+  <maintainer email="brett@robosoft.ai">Brett Aldrich</maintainer>
   <license>Apache-2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
@@ -13,7 +13,7 @@
   <depend>cl_keyboard</depend>
   <depend>smacc2</depend>
 
-  <exec_depend>xterm</exec_depend>
+  <exec_depend>konsole</exec_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

smacc2_sm_reference_library/sm_cl_ros2_timer_unit_test_1/package.xml

@@ -4,16 +4,14 @@
   <name>sm_cl_ros2_timer_unit_test_1</name>
   <version>3.0.1</version>
   <description>The sm_cl_ros2_timer_unit_test_1 package</description>
-  <maintainer email="pablo@ibrobotics.com">Pablo Inigo Blasco</maintainer>
+  <maintainer email="brett@robosoft.ai">Brett Aldrich</maintainer>
   <license>Apache-2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>cl_ros2_timer</depend>
   <depend>smacc2</depend>
 
-  <exec_depend>xterm</exec_depend>
-
   <export>
     <build_type>ament_cmake</build_type>
   </export>

smacc2_sm_reference_library/sm_multi_stage_1/README.md

@@ -32,7 +32,7 @@ source install/setup.bash
 And then run the launch file...
 
 ```
-ros2 launch sm_multi_stage_1 sm_multi_stage_1.launch
+ros2 launch sm_multi_stage_1 sm_multi_stage_1.py
 ```
 
  <h2>Viewer Instructions</h2>