diff --git a/README.md b/README.md index 80970cd..febccd5 100644 --- a/README.md +++ b/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`. diff --git a/src/rove_launch_handler/CMakeLists.txt b/src/rove_launch_handler/CMakeLists.txt new file mode 100644 index 0000000..06f6299 --- /dev/null +++ b/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() diff --git a/src/rove_launch_handler/launch/launch_handler.launch.py b/src/rove_launch_handler/launch/launch_handler.launch.py new file mode 100644 index 0000000..e0f6178 --- /dev/null +++ b/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' + ) + ]) diff --git a/src/rove_launch_handler/package.xml b/src/rove_launch_handler/package.xml new file mode 100644 index 0000000..ec61d5b --- /dev/null +++ b/src/rove_launch_handler/package.xml @@ -0,0 +1,25 @@ + + + rove_launch_handler + 0.0.0 + The rove_launch_handler package + Capra + MIT + + ament_cmake + + rclpy + rosidl_default_generators + + rclpy + rosidl_default_runtime + + ament_lint_auto + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + + diff --git a/src/rove_launch_handler/resource/rove_launch_handler b/src/rove_launch_handler/resource/rove_launch_handler new file mode 100644 index 0000000..e69de29 diff --git a/src/rove_launch_handler/scripts/launch_handler.py b/src/rove_launch_handler/scripts/launch_handler.py new file mode 100755 index 0000000..333a5d3 --- /dev/null +++ b/src/rove_launch_handler/scripts/launch_handler.py @@ -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() diff --git a/src/rove_launch_handler/scripts/launch_script.service b/src/rove_launch_handler/scripts/launch_script.service new file mode 100755 index 0000000..6cd0e14 --- /dev/null +++ b/src/rove_launch_handler/scripts/launch_script.service @@ -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 \ No newline at end of file diff --git a/src/rove_launch_handler/srv/LaunchListRequest.srv b/src/rove_launch_handler/srv/LaunchListRequest.srv new file mode 100644 index 0000000..4481e2f --- /dev/null +++ b/src/rove_launch_handler/srv/LaunchListRequest.srv @@ -0,0 +1,3 @@ +--- +string[] packages +string[] files diff --git a/src/rove_launch_handler/srv/LaunchRequest.srv b/src/rove_launch_handler/srv/LaunchRequest.srv new file mode 100644 index 0000000..687d578 --- /dev/null +++ b/src/rove_launch_handler/srv/LaunchRequest.srv @@ -0,0 +1,6 @@ +string package +string file_name +--- +string message +bool is_launched +string file_name diff --git a/src/rove_launch_handler/test/test_copyright.py b/src/rove_launch_handler/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/src/rove_launch_handler/test/test_copyright.py @@ -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' diff --git a/src/rove_launch_handler/test/test_flake8.py b/src/rove_launch_handler/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/src/rove_launch_handler/test/test_flake8.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) diff --git a/src/rove_launch_handler/test/test_pep257.py b/src/rove_launch_handler/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/src/rove_launch_handler/test/test_pep257.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' diff --git a/utils/install.sh b/utils/install.sh new file mode 100755 index 0000000..afd033a --- /dev/null +++ b/utils/install.sh @@ -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 \ No newline at end of file diff --git a/utils/ui/capra_ui.json b/utils/ui/capra_ui.json index 2105a1f..d7758fd 100644 --- a/utils/ui/capra_ui.json +++ b/utils/ui/capra_ui.json @@ -4,22 +4,31 @@ "buttonText": "Front LED ON", "buttonTooltip": "", "advancedView": false, - "value": "{}", - "foxglovePanelTitle": "Front LED ON" + "value": "{\n \"linear\": {\n \"x\": 0,\n \"y\": 0,\n \"z\": 0\n },\n \"angular\": {\n \"x\": 0,\n \"y\": 0,\n \"z\": 0\n }\n}", + "foxglovePanelTitle": "Front LED ON", + "topicName": "/cmd_vel.linear.x", + "datatype": "geometry_msgs/msg/Twist" }, "Indicator!360e2e8": { - "path": "", + "path": "/odom.twist.twist.linear.x", "style": "bulb", - "fallbackColor": "#a0a0a0", - "fallbackLabel": "False", + "fallbackColor": "#e0e2df", + "fallbackLabel": "Speed good", "rules": [ { - "operator": "=", - "rawValue": "true", + "operator": "<=", + "rawValue": "1", "color": "#68e24a", - "label": "True" + "label": "Speed good" + }, + { + "operator": ">", + "rawValue": "1", + "color": "#ede466", + "label": "Slower" } - ] + ], + "foxglovePanelTitle": "lidar state" }, "Publish!gkhilm": { "buttonText": "Front LED OFF", @@ -218,32 +227,33 @@ "poseEstimateThetaDeviation": 0.26179939 }, "imageMode": { - "imageTopic": "/zed/zed_node/rgb/image_rect_color", + "imageTopic": "/zed/zed_node/depth/depth_registered", "calibrationTopic": "/zed/zed_node/depth/camera_info" } }, + "Joystick Panel.Joystick!f5fjj6": {}, "3D!wd5ayb": { "cameraState": { - "distance": 15.475618749968437, - "perspective": true, - "phi": 53.72454709424579, + "perspective": false, + "distance": 25.847108697940985, + "phi": 33.02243748196572, + "thetaOffset": -106.2487931391679, + "targetOffset": [ + 1.4574957054508668, + -1.872533894914948, + 5.047356121098524e-16 + ], "target": [ 0, 0, 0 ], - "targetOffset": [ - -1.0352265201029487, - 0.690125189613747, - -6.717353809730597e-17 - ], "targetOrientation": [ 0, 0, 0, 1 ], - "thetaOffset": 39.94897959183675, "fovy": 45, "near": 0.5, "far": 5000 @@ -397,26 +407,164 @@ "imageMode": {}, "followTf": "base_link" }, + "3D!2euzghd": { + "cameraState": { + "perspective": true, + "distance": 25.847108697995846, + "phi": 59.99999999999988, + "thetaOffset": 44.99999999999998, + "targetOffset": [ + -0.6765570540972538, + 1.1691389716638279, + -5.075832289577129e-17 + ], + "target": [ + 0, + 0, + 0 + ], + "targetOrientation": [ + 0, + 0, + 0, + 1 + ], + "fovy": 45, + "near": 0.5, + "far": 5000 + }, + "followMode": "follow-pose", + "followTf": "map", + "scene": {}, + "transforms": { + "frame:map": { + "visible": true + } + }, + "topics": { + "/marker": { + "visible": true + } + }, + "layers": {}, + "publish": { + "type": "point", + "poseTopic": "/move_base_simple/goal", + "pointTopic": "/clicked_point", + "poseEstimateTopic": "/initialpose", + "poseEstimateXDeviation": 0.5, + "poseEstimateYDeviation": 0.5, + "poseEstimateThetaDeviation": 0.26179939 + }, + "imageMode": {} + }, "map!60yjuv": { "center": { - "lat": 38.16148014673053, - "lon": -122.45464324951172 + "lat": 38.16148858251846, + "lon": -122.45468616485597 }, "customTileUrl": "", "disabledTopics": [], "followTopic": "/gps", "layer": "map", "topicColors": {}, - "zoomLevel": 16, + "zoomLevel": 14, "maxNativeZoom": 18 }, - "Table!1xg0s4v": { - "topicPath": "" + "3D!ac8om2": {}, + "CallService!2b4k70w": { + "requestPayload": "{\n \"package\" : \"rove_bringup\",\n \"file_name\" : \"sim.launch.py\"\n}", + "layout": "vertical", + "timeoutSeconds": 20, + "serviceName": "/launchHandler/launchFile", + "editingMode": false, + "foxglovePanelTitle": "simulation", + "buttonText": "Simulation", + "buttonColor": "#ff0000" + }, + "CallService!3utg1bx": { + "requestPayload": "{\n \"package\" : \"rove_bringup\",\n \"file_name\" : \"real.launch.py\"\n}", + "layout": "vertical", + "timeoutSeconds": 20, + "serviceName": "/launchHandler/launchFile", + "editingMode": false, + "foxglovePanelTitle": "simulation", + "buttonText": "Real", + "buttonColor": "#ff0000" }, - "RosOut!14blizg": { - "searchTerms": [], - "minLogLevel": 1, - "nameFilter": {} + "Blank Panel.Blank!4iybw25": { + "showLogo": false, + "foxglovePanelTitle": " " + }, + "CallService!1z631qn": { + "requestPayload": "{\n \"package\" : \"rove_navigation\",\n \"file_name\" : \"navigation.launch.py\"\n}", + "layout": "vertical", + "timeoutSeconds": 20, + "serviceName": "/launchHandler/launchFile", + "editingMode": false, + "foxglovePanelTitle": "navigation", + "buttonText": "navigation", + "buttonColor": "#ff0000" + }, + "CallService!2kij9dp": { + "requestPayload": "{\n \"package\" : \"rove_slam\",\n \"file_name\" : \"slam3d_full.launch.py\"\n}", + "layout": "vertical", + "timeoutSeconds": 20, + "serviceName": "/launchHandler/launchFile", + "editingMode": false, + "foxglovePanelTitle": "slam", + "buttonText": "slam3d", + "buttonColor": "#ff0000" + }, + "CallService!90e1se": { + "requestPayload": "{\n \"package\" : \"rove_slam\",\n \"file_name\" : \"velodyne_3d.launch.py\"\n}", + "layout": "vertical", + "timeoutSeconds": 20, + "serviceName": "/launchHandler/launchFile", + "editingMode": false, + "foxglovePanelTitle": "slam", + "buttonText": "slam3d - lidar", + "buttonColor": "#ff0000" + }, + "CallService!6jl54y": { + "requestPayload": "{\n \"package\" : \"rove_slam\",\n \"file_name\" : \"icp_zed.launch.py\"\n}", + "layout": "vertical", + "timeoutSeconds": 20, + "serviceName": "/launchHandler/launchFile", + "editingMode": false, + "foxglovePanelTitle": "simulation", + "buttonText": "slam3d - zed", + "buttonColor": "#ff0000" + }, + "CallService!43dz0yr": { + "requestPayload": "{\n \"package\" : \"rove_slam\",\n \"file_name\" : \"slam3d_full.launch.py\"\n}", + "layout": "vertical", + "timeoutSeconds": 20, + "serviceName": "/launchHandler/launchFile", + "editingMode": false, + "foxglovePanelTitle": "Scenario", + "buttonText": "Reconnoitring of structures", + "buttonColor": "#ff0000" + }, + "CallService!2qnnpsv": { + "requestPayload": "{\n \"package\" : \"rove_slam\",\n \"file_name\" : \"slam3d_full.launch.py\"\n}", + "layout": "vertical", + "timeoutSeconds": 20, + "serviceName": "/launchHandler/launchFile", + "editingMode": false, + "foxglovePanelTitle": "Scenario", + "buttonText": "Transport – Mule", + "buttonColor": "#ff0000" + }, + "CallService!6ywrf4": { + "requestPayload": "{\n \"package\" : \"rove_slam\",\n \"file_name\" : \"slam3d_full.launch.py\"\n}", + "layout": "vertical", + "timeoutSeconds": 20, + "serviceName": "/launchHandler/launchFile", + "editingMode": false, + "foxglovePanelTitle": "Scenario", + "buttonText": "Search & Rescue", + "buttonColor": "#ff0000" }, "Tab!2iobm2s": { "activeTabIdx": 0, @@ -429,7 +577,7 @@ "first": "Publish!1b7sy1a", "second": "Indicator!360e2e8", "direction": "column", - "splitPercentage": 55.757575757575765 + "splitPercentage": 55.6420233463035 }, "second": { "first": "Publish!gkhilm", @@ -486,6 +634,10 @@ "second": "Image!3593v6f", "direction": "row" } + }, + { + "title": "Joy", + "layout": "Joystick Panel.Joystick!f5fjj6" } ] }, @@ -496,18 +648,62 @@ "title": "Nav", "layout": { "first": "3D!wd5ayb", + "second": { + "first": "3D!2euzghd", + "second": "map!60yjuv", + "direction": "column", + "splitPercentage": 45.103857566765576 + }, + "direction": "row", + "splitPercentage": 61.47652076855616 + } + }, + { + "title": "Arm", + "layout": "3D!ac8om2" + }, + { + "title": "launch", + "layout": { + "first": { + "first": { + "first": "CallService!2b4k70w", + "second": "CallService!3utg1bx", + "direction": "column" + }, + "second": { + "first": "Blank Panel.Blank!4iybw25", + "second": { + "first": { + "first": "CallService!1z631qn", + "second": "CallService!2kij9dp", + "direction": "column" + }, + "second": { + "first": "CallService!90e1se", + "second": "CallService!6jl54y", + "direction": "column" + }, + "direction": "column" + }, + "direction": "row", + "splitPercentage": 69.34679409175952 + }, + "direction": "row", + "splitPercentage": 22.561529751795238 + }, "second": { "first": { - "first": "map!60yjuv", - "second": "Table!1xg0s4v", - "direction": "row" + "first": "CallService!43dz0yr", + "second": "CallService!2qnnpsv", + "direction": "column" }, - "second": "RosOut!14blizg", + "second": "CallService!6ywrf4", "direction": "column", - "splitPercentage": 48.263254113345525 + "splitPercentage": 64.83679525222553 }, "direction": "row", - "splitPercentage": 50.911458333333336 + "splitPercentage": 85.62162162162163 } } ] @@ -522,6 +718,6 @@ "first": "Tab!2iobm2s", "second": "Tab!1dud6cr", "direction": "column", - "splitPercentage": 25.259067357512954 + "splitPercentage": 30.22794846382557 } } \ No newline at end of file