Merge branch 'main' into connection-lost-behavior

Author: nathan-gt

README.md

@@ -107,6 +107,24 @@ View the gripper in RViz:
 ros2 launch robotiq_description view_gripper.launch.py
 ```
 
+## Foxglove usage (user interface)
+
+You can find the json configuration file in utils/ui/capra_ui.json. You need to load it into foxglove. On the device that you want to be connected (a.k.a. the jetson), you need to run the following command : 
+
+
+You can make it as a service and start at boot : 
+
+```bash
+./utils/install.sh
+```
+
+Otherwise, to start use the user interface with a development laptop, you can run it by only launching the launchfile :
+
+```bash
+ros2 launch rove_launch_handler launch_handler.py
+```
+
+
 ## Adding New Packages
 
 To add a package for Rove, create it using the ROS2 command ([Creating Your First ROS2 Package](https://docs.ros.org/en/foxy/Tutorials/Beginner-Client-Libraries/Creating-Your-First-ROS2-Package.html)). Name it starting with `rove_` to ensure Git tracking. For non-Rove specific packages, create a separate repository and add it to `rove.repos`.

src/rove_description/urdf/flipper.urdf.xacro

@@ -42,6 +42,13 @@
 
 
         <link name="flipper_${suffix}">
+            <visual>
+                <origin xyz="0 0 0" rpy="${mesh_rotation}" />
+                <geometry>
+                    <mesh filename="https://raw.githubusercontent.com/clubcapra/rove/main/src/rove_description/meshes/flipper.stl"/>
+                </geometry>
+                <material name="rubber_color" />
+            </visual>
             <visual>
                 <origin xyz="0 0 0" rpy="${mesh_rotation}" />
                 <geometry>

src/rove_description/urdf/rove.urdf.xacro

@@ -20,6 +20,13 @@
     <!-- <xacro:include filename="$(find ovis_description)/urdf/ovis.urdf.xacro" /> -->
 
     <link name="base_link">
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <geometry>
+                <mesh filename="https://raw.githubusercontent.com/clubcapra/rove/main/src/rove_description/meshes/base.stl"/>
+            </geometry>
+            <material name="frame_color"/>
+        </visual>
         <visual>
             <origin xyz="0 0 0" rpy="0 0 0"/>
             <geometry>
@@ -40,11 +47,10 @@
         </inertial>
     </link>
 
-
     <!-- <xacro:rove_ros2_control/> -->
      <!-- Put these at -1 to mock the motors if working without the motors -->
+    <!-- <xacro:odrive_control fl_id="21" rl_id="22" fr_id="23" rr_id="24"/> -->
     <xacro:odrive_control fl_id="21" rl_id="22" fr_id="23" rr_id="24"/>
-    <!-- <xacro:odrive_control fl_id="-1" rl_id="-1" fr_id="-1" rr_id="-1"/> -->
 
 
     <!-- Insert ovis on top of rove -->

src/rove_description/urdf/track.urdf.xacro

@@ -32,6 +32,13 @@
         </joint>
 
         <link name="track_${suffix}">
+            <visual>
+                <origin xyz="0 0 0"/>
+                <geometry>
+                    <mesh filename="https://raw.githubusercontent.com/clubcapra/rove/main/src/rove_description/meshes/track.stl"/>
+                </geometry>
+                <material name="rubber_color" />
+            </visual>
             <visual>
                 <origin xyz="0 0 0" rpy="0 0 0"/>
                 <geometry>

src/rove_launch_handler/CMakeLists.txt

@@ -0,0 +1,29 @@
+cmake_minimum_required(VERSION 3.5)
+project(rove_launch_handler)
+
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(rclpy REQUIRED)
+
+# Convert message files to be used in python
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "srv/LaunchRequest.srv"
+  "srv/LaunchListRequest.srv"
+)
+
+# Install Python executables
+install(PROGRAMS
+  scripts/launch_handler.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(
+  DIRECTORY launch srv
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_export_dependencies(rosidl_default_runtime)
+
+ament_package()

src/rove_launch_handler/launch/launch_handler.launch.py

@@ -0,0 +1,25 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import IncludeLaunchDescription
+from launch_xml.launch_description_sources import XMLLaunchDescriptionSource
+from ament_index_python.packages import get_package_share_directory
+import os
+
+def generate_launch_description():
+    rosbridge_launch_file = os.path.join(
+        get_package_share_directory('foxglove_bridge'),
+        'launch',
+        'foxglove_bridge_launch.xml'
+    )
+
+    return LaunchDescription([
+        IncludeLaunchDescription(
+            XMLLaunchDescriptionSource(rosbridge_launch_file)
+        ),
+        Node(
+            package='rove_launch_handler',
+            executable='launch_handler.py',
+            name='launch_handler',
+            output='screen'
+        )
+    ])

src/rove_launch_handler/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>rove_launch_handler</name>
+  <version>0.0.0</version>
+  <description>The rove_launch_handler package</description>
+  <maintainer email="[email protected]">Capra</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>rclpy</build_depend>
+  <build_depend>rosidl_default_generators</build_depend>
+  
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/rove_launch_handler/resource/rove_launch_handler

@@ -0,0 +1,99 @@
+#!/usr/bin/env python3
+
+import time
+import rclpy
+from rclpy.node import Node
+import signal
+import subprocess
+import os
+from rove_launch_handler.srv import LaunchRequest, LaunchListRequest
+
+launched_files = {}
+
+class LaunchFile: 
+    def __init__(self, package, file_name, pid):
+        self.package = package
+        self.file_name = file_name
+        self.pid = pid
+
+class LaunchMsg:
+    def __init__(self):
+        self.message = ""
+        self.is_launched = False
+        self.file_name = ""
+
+def launch_file(package, file_name):
+    command = f"ros2 launch {package} {file_name}"
+
+    p = subprocess.Popen(command, shell=True, preexec_fn=os.setsid)
+
+    launch_msg = LaunchMsg()
+    # Sleep to make sure the launch command has time to fail if there's an error
+    time.sleep(1)
+    state = p.poll()
+    launch_msg.file_name = file_name
+    if state is None:
+        launch_msg.message = f"{file_name} was launched"
+        launched_files[file_name] = LaunchFile(package, file_name, p.pid)
+        launch_msg.is_launched = True
+    else:
+        launch_msg.message = f"{file_name} was not launched"
+        launch_msg.is_launched = False
+    return launch_msg
+
+def kill_launch_file(file_name):
+    launch_msg = LaunchMsg()
+    launch_msg.file_name = file_name
+    if file_name in launched_files:
+        pid = launched_files[file_name].pid
+        try:
+            os.killpg(os.getpgid(pid), signal.SIGINT)
+            del launched_files[file_name]
+            launch_msg.message = f"{file_name} was killed"
+            launch_msg.is_launched = False
+        except ProcessLookupError as e:
+            launch_msg.message = f"Failed to kill {file_name}: {str(e)}"
+            launch_msg.is_launched = False
+    else:
+        launch_msg.message = f"{file_name} was not launched"
+        launch_msg.is_launched = False
+    return launch_msg
+
+def kill_all():
+    for file_name in list(launched_files.keys()):
+        kill_launch_file(file_name)
+
+class LaunchHandlerService(Node):
+    def __init__(self):
+        super().__init__('launch_handler_service')
+        self.srv_launch = self.create_service(LaunchRequest, 'launchHandler/launchFile', self.launch_callback)
+        self.srv_list = self.create_service(LaunchListRequest, 'launchHandler/getAllLaunchedFiles', self.get_launched_files)
+        self.get_logger().info("LaunchHandlerService node has been started.")
+
+    def launch_callback(self, request, response):
+        package = request.package
+        file_name = request.file_name
+        if file_name not in launched_files:
+            launch_msg = launch_file(package, file_name)
+        else:
+            launch_msg = kill_launch_file(file_name)
+        response.message = launch_msg.message
+        response.is_launched = launch_msg.is_launched
+        response.file_name = launch_msg.file_name
+        return response
+
+    def get_launched_files(self, request, response):
+        response.packages = [lf.package for lf in launched_files.values()]
+        response.files = list(launched_files.keys())
+        return response
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = LaunchHandlerService()
+    rclpy.get_default_context().on_shutdown(kill_all)
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/rove_launch_handler/scripts/launch_handler.py

@@ -0,0 +1,13 @@
+[Unit]
+Descript=Robot launch script
+After=network.target
+
+[Service]
+Environment="ROS_LOG_DIR=/var/log/ros2; ROS_DOMAIN_ID=96"
+ExecStart=/bin/bash -c 'source /home/simon/Workspace/capra/rove/install/setup.bash; ros2 launch rove_launch_handler launch_handler.launch.py;'
+RemainAfterExit=no
+Restart=on-failure
+RestartSec=2s
+
+[Install]
+WantedBy=multi-user.target

src/rove_launch_handler/scripts/launch_script.service

@@ -0,0 +1,3 @@
+---
+string[] packages
+string[] files

src/rove_launch_handler/srv/LaunchListRequest.srv

@@ -0,0 +1,6 @@
+string package
+string file_name
+---
+string message
+bool is_launched
+string file_name

src/rove_launch_handler/srv/LaunchRequest.srv

@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, 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.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

src/rove_launch_handler/test/test_copyright.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, 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.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

src/rove_launch_handler/test/test_flake8.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, 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.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'

src/rove_launch_handler/test/test_pep257.py

@@ -0,0 +1,7 @@
+#!/usr/bin/env bash
+sudo mkdir -p /var/log/ros2
+sudo chmod 775 /var/log/ros2
+sudo cp ./src/rove_launch_handler/scripts/launch_script.service /lib/systemd/system/launch_script.service
+sudo systemctl daemon-reload
+sudo systemctl enable launch_script.service
+sudo systemctl start launch_script.service