Backports (#16)

* Prefix node names with can interface

* Wait for interface to be up before launching node (#7)

* Wait for interface to be UP before starting node

* Fix: Multiple IncludeLaunchDescription (#9)

* Use opaque function

* Add import and context

* Type performed context

* Reattempt transitions on failure (#10)

* Reattempt lifecycle transitions on failure

* Unique activator node names

* Removed namespace from node name

* Linting

* Use arguments instead of perform(context) (#11)

* Fix: Use script instead of OpaqueFunction (#12)

* Use script instead of OpaqueFunction

* Add missing line

* Add license header

* Add EOF line

* Add retry in the event lifecycle service is not up yet

* Fix: Spin Timeout (#13)

* Add timeout to spin to prevent script from waiting when node has failed

* Update log to match appropriate action

* Fixed using a global namespace.

---------

Co-authored-by: Roni Kreinin <[email protected]>
Co-authored-by: Roni Kreinin <[email protected]>
Co-authored-by: Tony Baltovski <[email protected]>

Author: 3 people

.gitignore

@@ -0,0 +1 @@
+__pycache__/

CMakeLists.txt

@@ -43,6 +43,12 @@ install(
   RUNTIME DESTINATION bin
 )
 
+install(
+  PROGRAMS
+  scripts/activate_lifecycle
+  DESTINATION lib/${PROJECT_NAME}
+)
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   list(APPEND AMENT_LINT_AUTO_EXCLUDE

launch/receiver.launch.py

@@ -25,33 +25,118 @@
 # 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.
+
 from launch import LaunchDescription
 from launch.actions import (
     DeclareLaunchArgument,
-    EmitEvent,
+    ExecuteProcess,
+    OpaqueFunction,
     RegisterEventHandler)
-from launch.conditions import IfCondition
-from launch.event_handlers import OnProcessStart
-from launch.events import matches_action
-from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import LifecycleNode
-from launch_ros.event_handlers import OnStateTransition
-from launch_ros.events.lifecycle import ChangeState
-from lifecycle_msgs.msg import Transition
+from launch.event_handlers import OnProcessExit, OnProcessStart
+from launch.substitutions import FindExecutable, LaunchConfiguration
+from launch_ros.actions import LifecycleNode, Node
 
 
+def launch_setup(context, *args, **kwargs):
+    # Apply context and type cast all LaunchConfiguration
+    namespace = str(
+        LaunchConfiguration('namespace').perform(context))
 
-def generate_launch_description():
-    namespace = LaunchConfiguration('namespace')
-    interface = LaunchConfiguration('interface')
-    enable_can_fd = LaunchConfiguration('enable_can_fd')
-    interval_sec = LaunchConfiguration('interval_sec')
-    use_bus_time = LaunchConfiguration('use_bus_time')
-    filters = LaunchConfiguration('filters')
-    auto_configure = LaunchConfiguration('auto_configure')
-    auto_activate = LaunchConfiguration('auto_activate')
-    from_can_bus_topic = LaunchConfiguration('from_can_bus_topic')
+    interface = str(
+        LaunchConfiguration('interface').perform(context))
+
+    enable_can_fd = bool(
+        LaunchConfiguration('enable_can_fd').perform(context) == 'true')
+
+    interval_sec = float(
+        LaunchConfiguration('interval_sec').perform(context))
+
+    use_bus_time = bool(
+        LaunchConfiguration('use_bus_time').perform(context) == 'true')
+
+    filters = str(
+        LaunchConfiguration('filters').perform(context))
+
+    from_can_bus_topic = str(
+        LaunchConfiguration('from_can_bus_topic').perform(context))
+
+    auto_configure = bool(
+        LaunchConfiguration('auto_configure').perform(context) == 'true')
+
+    auto_activate = bool(
+        LaunchConfiguration('auto_activate').perform(context) == 'true')
+
+    timeout = float(
+        LaunchConfiguration('timeout').perform(context))
+
+    transition_attempts = int(
+        LaunchConfiguration('transition_attempts').perform(context))
+
+    name = f'{interface}_socket_can_receiver'
+
+    # SocketCAN receiver node
+    node = LifecycleNode(
+        package='ros2_socketcan',
+        executable='socket_can_receiver_node_exe',
+        name=name,
+        namespace=namespace,
+        parameters=[{
+            'interface': interface,
+            'enable_can_fd': enable_can_fd == 'true',
+            'interval_sec': interval_sec,
+            'filters': filters,
+            'use_bus_time': use_bus_time == 'true',
+        }],
+        remappings=[('from_can_bus', from_can_bus_topic)],
+        output='screen')
+
+    # Wait for interface to be up
+    wait_for_can_interface_proc = ExecuteProcess(
+        cmd=[['until ', FindExecutable(name='ip'), ' link show ', interface,
+              ' | ', FindExecutable(name='grep'), ' \"state UP\"', '; do sleep 1; done']],
+        shell=True
+    )
+
+    # Event to launch node once interface is up
+    launch_node = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=wait_for_can_interface_proc,
+            on_exit=node
+        )
+    )
+
+    # Activate launch
+    activate_lifecycle_node = Node(
+        name=f'activate_{name}',
+        package='clearpath_ros2_socketcan_interface',
+        executable='activate_lifecycle',
+        namespace=namespace,
+        arguments=[
+            '--namespace', str(namespace),
+            '--node', str(name),
+            '--auto_configure', str(auto_configure),
+            '--auto_activate', str(auto_activate),
+            '--timeout', str(timeout),
+            '--transition_attempts', str(transition_attempts)
+        ]
+    )
+
+    # Event to configure and activate node once node is up
+    configure_event = RegisterEventHandler(
+        event_handler=OnProcessStart(
+            target_action=node,
+            on_start=[activate_lifecycle_node],
+        ),
+    )
+
+    return [
+        wait_for_can_interface_proc,
+        launch_node,
+        configure_event,
+    ]
 
+
+def generate_launch_description():
     arg_namespace = DeclareLaunchArgument(
       'namespace',
       default_value='')
@@ -88,54 +173,16 @@ def generate_launch_description():
       'from_can_bus_topic',
       default_value='rx')
 
-    node = LifecycleNode(
-        package='ros2_socketcan',
-        executable='socket_can_receiver_node_exe',
-        name='socket_can_receiver',
-        namespace=namespace,
-        parameters=[{
-            'interface': interface,
-            'enable_can_fd': enable_can_fd,
-            'interval_sec': interval_sec,
-            'filters': filters,
-            'use_bus_time': use_bus_time,
-        }],
-        remappings=[('from_can_bus', from_can_bus_topic)],
-        output='screen')
-
-    configure_event = RegisterEventHandler(
-        event_handler=OnProcessStart(
-            target_action=node,
-            on_start=[
-                EmitEvent(
-                    event=ChangeState(
-                        lifecycle_node_matcher=matches_action(node),
-                        transition_id=Transition.TRANSITION_CONFIGURE,
-                    ),
-                ),
-            ],
-        ),
-        condition=IfCondition(auto_configure),
-    )
+    arg_timeout = DeclareLaunchArgument(
+      'timeout',
+      default_value='5.0')
 
-    activate_event = RegisterEventHandler(
-        event_handler=OnStateTransition(
-            target_lifecycle_node=node,
-            start_state='configuring',
-            goal_state='inactive',
-            entities=[
-                EmitEvent(
-                    event=ChangeState(
-                        lifecycle_node_matcher=matches_action(node),
-                        transition_id=Transition.TRANSITION_ACTIVATE,
-                    ),
-                ),
-            ],
-        ),
-        condition=IfCondition(auto_activate),
-    )
+    arg_transition_attempts = DeclareLaunchArgument(
+      'transition_attempts',
+      default_value='3')
 
     ld = LaunchDescription()
+
     ld.add_action(arg_namespace)
     ld.add_action(arg_interface)
     ld.add_action(arg_enable_can_fd)
@@ -145,7 +192,7 @@ def generate_launch_description():
     ld.add_action(arg_auto_configure)
     ld.add_action(arg_auto_activate)
     ld.add_action(arg_from_can_bus_topic)
-    ld.add_action(node)
-    ld.add_action(configure_event)
-    ld.add_action(activate_event)
+    ld.add_action(arg_timeout)
+    ld.add_action(arg_transition_attempts)
+    ld.add_action(OpaqueFunction(function=launch_setup))
     return ld