diff --git a/mars_helicopter/CMakeLists.txt b/mars_helicopter/CMakeLists.txt new file mode 100644 index 00000000..136a89bd --- /dev/null +++ b/mars_helicopter/CMakeLists.txt @@ -0,0 +1,52 @@ +cmake_minimum_required(VERSION 3.8) +project(mars_helicopter) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclpy REQUIRED) +find_package(simulation REQUIRED) +find_package(std_msgs REQUIRED) +find_package(ros_gz_bridge REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +install(DIRECTORY + launch + config + worlds + DESTINATION share/${PROJECT_NAME} +) + +install( + DIRECTORY src/utils # Install the utils directory + DESTINATION lib/${PROJECT_NAME} +) + +ament_python_install_package(${PROJECT_NAME}) + +install(PROGRAMS + nodes/teleop_helicopter + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() \ No newline at end of file diff --git a/mars_helicopter/LICENSE b/mars_helicopter/LICENSE new file mode 100644 index 00000000..7a4a3ea2 --- /dev/null +++ b/mars_helicopter/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. \ No newline at end of file diff --git a/mars_helicopter/README.md b/mars_helicopter/README.md new file mode 100644 index 00000000..280cd277 --- /dev/null +++ b/mars_helicopter/README.md @@ -0,0 +1,109 @@ +# NASA Space ROS Sim Summer Sprint Challenge + +Team Lead Freelancer Username: @exp99generator + +Submission Title: Ingenuity coaxial helicopter model + +# Description and purpose of package + +This package aims to accelerate the development of guidance, navigation, and control algorithms for autonomous flying vehicles utilizing a helicopter architecture. + +![Ingenuity](/mars_helicopter/assets/ingenuity.png) + +The package consists of the following: +- A Gazebo world modeled after the Jezero Mars crater, and a coaxial helicopter SDF model of Ingenuity (available in the space-ros/simulation repository). +- A Gazebo plugin that simulates rotor disk flapping dynamics, rotor inflow, and rotor forces. The rotor can be controlled by varying the collective pitch, longitudinal and lateral cyclic pitch, and angular velocity through messages sent via Gazebo topics. +- A Gazebo plugin for the atmosphere, which starts a service that allows the rotors in the simulation to poll relevant information, such as density at a certain height (currently modeled with uniform density). + +The implementation uses mathematical models from: +- Principles of Helicopter Aerodynamics by J. Gordon Leishman +- Helicopter Flight Dynamics by Gareth D. Padfield + +# Definition and description of the public API + +The rotor plugin subscribes to the following gazebo topics: +- /{link_name}/angular_velocity (ignition.msgs.Double) +- /{link_name}/collective (ignition.msgs.Double) +- /{link_name}/lateral_cyclic (ignition.msgs.Double) +- /{link_name}/longitudinal_cyclic (ignition.msgs.Double) + +The Ingenuity helicopter publishes sensor data on: +- /ingenuity/sensors/imu +- /ingenuity/sensors/laser_altimeter/range +- /ingenuity/sensors/laser_altimeter/range/points +- /ingenuity/sensors/mono_camera/camera_info +- /ingenuity/sensors/mono_camera/image_raw +- /ingenuity/sensors/rgb_camera/camera_info +- /ingenuity/sensors/rgb_camera/image_raw + +The atmosphere plugin opens a service at: +- /atmo/density + +The plugins can be configured by using the following SDF elements: +- rotor plugin: +```xml + + + fuselage_link + gazebo_joint_name + 0 0 0 0 0 0 + base + gordon + harmonic_1 + + + 0.1029 + 0.514334 + 5.73 + 0.14875 + 0.0738 + 0.01 + 0.005 + + cw + 4.5680 + 2 + 10 + + +``` + +- atmosphere plugin: + +```xml + + uniform_density + 0.020 + +``` + +# How to build and install + +To build the docker image and start a docker container follow these passages + +1. Clone this repository, change directory to demos\mars_helicopter\docker + +2. Run the build script ```./build.sh``` + +3. Start the container by executing ```./run.sh``` + +4. (optional) additional terminals can be opened executing ```./open_cmd.sh``` + +# Run Ingenuity demo + +The Ingenuity helicopter demo can be started by executing the following commands: + +1. Run ```ros2 launch mars_helicopter mars_helicopter.launch.py``` this launch file will start the gazebo simulator with the Jezero crater world and will initiate the gazebo/ros2 bridge + +2. In another terminal run ```ros2 run mars_helicopter teleop_helicopter``` this will start a simple ros2 teleop node. + +3. Command the helicopter: + * Enter for rotor on/off + * Space bar for collective + * WASD for cyclic input + * HL for yaw control by differential collective input + + +# License + +Apache License 2.0 \ No newline at end of file diff --git a/mars_helicopter/assets/ingenuity.png b/mars_helicopter/assets/ingenuity.png new file mode 100644 index 00000000..19cfc773 Binary files /dev/null and b/mars_helicopter/assets/ingenuity.png differ diff --git a/mars_helicopter/config/gz_ros2_bridge.yaml b/mars_helicopter/config/gz_ros2_bridge.yaml new file mode 100644 index 00000000..9e3760f2 --- /dev/null +++ b/mars_helicopter/config/gz_ros2_bridge.yaml @@ -0,0 +1,48 @@ +- ros_topic_name: "/rotor_bottom_revolute/angular_velocity" + gz_topic_name: "/rotor_bottom_revolute/angular_velocity" + ros_type_name: "std_msgs/msg/Float64" + gz_type_name: "ignition.msgs.Double" + direction: ROS_TO_GZ + +- ros_topic_name: "/rotor_top_revolute/angular_velocity" + gz_topic_name: "/rotor_top_revolute/angular_velocity" + ros_type_name: "std_msgs/msg/Float64" + gz_type_name: "ignition.msgs.Double" + direction: ROS_TO_GZ + +- ros_topic_name: "/rotor_bottom_revolute/collective" + gz_topic_name: "/rotor_bottom_revolute/collective" + ros_type_name: "std_msgs/msg/Float64" + gz_type_name: "ignition.msgs.Double" + direction: ROS_TO_GZ + +- ros_topic_name: "/rotor_top_revolute/collective" + gz_topic_name: "/rotor_top_revolute/collective" + ros_type_name: "std_msgs/msg/Float64" + gz_type_name: "ignition.msgs.Double" + direction: ROS_TO_GZ + +- ros_topic_name: "/rotor_bottom_revolute/longitudinal_cyclic" + gz_topic_name: "/rotor_bottom_revolute/longitudinal_cyclic" + ros_type_name: "std_msgs/msg/Float64" + gz_type_name: "ignition.msgs.Double" + direction: ROS_TO_GZ + +- ros_topic_name: "/rotor_top_revolute/longitudinal_cyclic" + gz_topic_name: "/rotor_top_revolute/longitudinal_cyclic" + ros_type_name: "std_msgs/msg/Float64" + gz_type_name: "ignition.msgs.Double" + direction: ROS_TO_GZ + + +- ros_topic_name: "/rotor_bottom_revolute/lateral_cyclic" + gz_topic_name: "/rotor_bottom_revolute/lateral_cyclic" + ros_type_name: "std_msgs/msg/Float64" + gz_type_name: "ignition.msgs.Double" + direction: ROS_TO_GZ + +- ros_topic_name: "/rotor_top_revolute/lateral_cyclic" + gz_topic_name: "/rotor_top_revolute/lateral_cyclic" + ros_type_name: "std_msgs/msg/Float64" + gz_type_name: "ignition.msgs.Double" + direction: ROS_TO_GZ \ No newline at end of file diff --git a/mars_helicopter/docker/Dockerfile b/mars_helicopter/docker/Dockerfile new file mode 100644 index 00000000..db800298 --- /dev/null +++ b/mars_helicopter/docker/Dockerfile @@ -0,0 +1,84 @@ +# Copyright 2021 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. +# +# A Docker configuration script to build the Space ROS image. +# +# The script provides the following build arguments: +# +# VCS_REF - The git revision of the Space ROS source code (no default value). +# VERSION - The version of Space ROS (default: "preview") + +FROM osrf/space-ros:latest AS base + +# Define arguments used in the metadata definition +ARG VCS_REF +ARG VERSION="preview" + +# Specify the docker image metadata +LABEL org.label-schema.schema-version="1.0" +LABEL org.label-schema.name="Mars Helicopter" +LABEL org.label-schema.description="Simulation of the Ingenuity mars helicopter in a Gazebo environment" +LABEL org.label-schema.vendor="Open Robotics" +LABEL org.label-schema.version=${VERSION} +LABEL org.label-schema.url="https://github.com/space-ros" +LABEL org.label-schema.vcs-url="https://github.com/space-ros/demos" +LABEL org.label-schema.vcs-ref=${VCS_REF} + +# Disable prompting during package installation +ARG DEBIAN_FRONTEND=noninteractive + +# Define a few key variables +ENV DEMO_DIR=${HOME_DIR}/demo_ws +ENV IGNITION_VERSION=fortress +ENV GZ_VERSION=fortress + +# Define key locations +RUN mkdir -p ${DEMO_DIR}/src +WORKDIR ${DEMO_DIR} + +# Define the XDG_RUNTIME_DIR environment variable +ENV XDG_RUNTIME_DIR=/run/${USERNAME}/1000 + +# Create the runtime directory, set appropriate ownership, and adjust permissions to 0700 +RUN sudo mkdir -p ${XDG_RUNTIME_DIR} && \ + sudo chown 1000:1000 ${XDG_RUNTIME_DIR} && \ + sudo chmod 0700 ${XDG_RUNTIME_DIR} + +RUN sudo curl https://packages.osrfoundation.org/gazebo.gpg --output /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg \ + && echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null \ + && sudo apt-get update && sudo apt-get install -y ignition-fortress + +# development version +RUN sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' \ + && wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add - \ + && sudo apt-get update && sudo apt-get install -y libignition-gazebo6-dev ros-humble-ros2-control ros-humble-ros2-controllers ros-humble-ros-ign ros-humble-ign-ros2-control + +FROM base AS workspace + +# Build argument to bust cache +ARG CACHEBUST=1 + +ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp +# Get the source files +COPY --chown=${USERNAME}:${USERNAME} demo_manual_pkgs.repos /tmp/ +RUN vcs import src < /tmp/demo_manual_pkgs.repos && /bin/bash -c 'source "${SPACEROS_DIR}/install/setup.bash"' +RUN cp -r src/demos/mars_helicopter src && rm -rf src/demos + +RUN /bin/bash -c 'source "${SPACEROS_DIR}/install/setup.bash" && source /opt/ros/humble/setup.bash && \ + rosdep install --from-paths src --ignore-src -r -y && colcon build' + +# Setup the entrypoint +COPY ./entrypoint.sh / +ENTRYPOINT ["/entrypoint.sh"] +CMD ["bash"] \ No newline at end of file diff --git a/mars_helicopter/docker/build.sh b/mars_helicopter/docker/build.sh new file mode 100755 index 00000000..d5b7901e --- /dev/null +++ b/mars_helicopter/docker/build.sh @@ -0,0 +1,22 @@ +#!/usr/bin/env bash + +ORG=openrobotics +IMAGE=mars_helicopter +TAG=latest + +VCS_REF="" +VERSION=preview + +# Exit script with failure if build fails +set -eo pipefail + +echo "" +echo "##### Building Space ROS Demo Docker Image #####" +echo "" + +docker build --build-arg CACHEBUST=$(date +%s) -t $ORG/$IMAGE:$TAG \ + --build-arg VCS_REF="$VCS_REF" \ + --build-arg VERSION="$VERSION" . \ + +echo "" +echo "##### Done! #####" \ No newline at end of file diff --git a/mars_helicopter/docker/demo-pkgs.txt b/mars_helicopter/docker/demo-pkgs.txt new file mode 100644 index 00000000..727e06e6 --- /dev/null +++ b/mars_helicopter/docker/demo-pkgs.txt @@ -0,0 +1,2 @@ +xacro +yaml_cpp_vendor \ No newline at end of file diff --git a/mars_helicopter/docker/demo_manual_pkgs.repos b/mars_helicopter/docker/demo_manual_pkgs.repos new file mode 100644 index 00000000..a48e301d --- /dev/null +++ b/mars_helicopter/docker/demo_manual_pkgs.repos @@ -0,0 +1,9 @@ +repositories: + demos: + type: git + url: https://github.com/Govax99/demos.git + version: feature/ingenuity-demo + simulation: + type: git + url: https://github.com/Govax99/simulation.git + version: feature/mars_helicopter \ No newline at end of file diff --git a/mars_helicopter/docker/entrypoint.sh b/mars_helicopter/docker/entrypoint.sh new file mode 100755 index 00000000..20384d1c --- /dev/null +++ b/mars_helicopter/docker/entrypoint.sh @@ -0,0 +1,8 @@ +#!/bin/bash +set -e + +# Setup the Demo environment +source "${SPACEROS_DIR}/install/setup.bash" +source /opt/ros/humble/setup.bash +source "${DEMO_DIR}/install/setup.bash" +exec "$@" \ No newline at end of file diff --git a/mars_helicopter/docker/excluded-pkgs.txt b/mars_helicopter/docker/excluded-pkgs.txt new file mode 100644 index 00000000..c9d845c9 --- /dev/null +++ b/mars_helicopter/docker/excluded-pkgs.txt @@ -0,0 +1,9 @@ +fastrtps +fastcdr +rmw_fastrtps_cpp +rmw_fastrtps_dynamic_cpp +rmw_fastrtps_shared_cpp +rmw_connextdds +rosidl_typesupport_fastrtps_c +rosidl_typesupport_fastrtps_cpp +fastrtps_cmake_module \ No newline at end of file diff --git a/mars_helicopter/docker/open_cmd.sh b/mars_helicopter/docker/open_cmd.sh new file mode 100755 index 00000000..2b2380a1 --- /dev/null +++ b/mars_helicopter/docker/open_cmd.sh @@ -0,0 +1,14 @@ +#!/usr/bin/env bash + +# Name of the container +CONTAINER_NAME="openrobotics_mars_helicopter" + +# Check if the container is running +if ! docker ps --format '{{.Names}}' | grep -q "$CONTAINER_NAME"; then + echo "Container $CONTAINER_NAME is not running." + exit 1 +fi + +# Connect to the running container and set up the environment +# Assuming `entrypoint.sh` handles setting up the environment inside the container +docker exec -it $CONTAINER_NAME /bin/bash -c "source /home/spaceros-user/demo_ws/src/mars_helicopter/docker/entrypoint.sh && /bin/bash" \ No newline at end of file diff --git a/mars_helicopter/docker/run.sh b/mars_helicopter/docker/run.sh new file mode 100755 index 00000000..6995dcfc --- /dev/null +++ b/mars_helicopter/docker/run.sh @@ -0,0 +1,16 @@ +#!/usr/bin/env bash + +# Runs a docker container with the image created by build.bash +# Requires: +# docker +# an X server + +IMG_NAME=openrobotics/mars_helicopter + +# Replace `/` with `_` to comply with docker container naming +# And append `_runtime` +CONTAINER_NAME="$(tr '/' '_' <<< "$IMG_NAME")" + +# Start the container +docker run --rm -it --name $CONTAINER_NAME --network host \ + -e DISPLAY -e TERM -v /tmp/.X11-unix:/tmp/.X11-unix -e QT_X11_NO_MITSHM=1 $IMG_NAME \ No newline at end of file diff --git a/mars_helicopter/launch/mars_helicopter.launch.py b/mars_helicopter/launch/mars_helicopter.launch.py new file mode 100644 index 00000000..88632299 --- /dev/null +++ b/mars_helicopter/launch/mars_helicopter.launch.py @@ -0,0 +1,69 @@ +from http.server import executable +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, RegisterEventHandler, IncludeLaunchDescription +from launch.substitutions import TextSubstitution, PathJoinSubstitution, LaunchConfiguration, Command +from launch_ros.actions import Node, SetParameter +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.event_handlers import OnProcessExit, OnExecutionComplete +import os +from os import environ + +from ament_index_python.packages import get_package_share_directory + + + +# . ../spaceros_ws/install/setup.bash && . ../depends_ws/install/setup.bash +# rm -rf build install log && colcon build && . install/setup.bash + +def generate_launch_description(): + + mars_helicopter_demos_path = get_package_share_directory('mars_helicopter') + mars_helicopter_models_path = get_package_share_directory('simulation') + + env = {'IGN_GAZEBO_SYSTEM_PLUGIN_PATH': + ':'.join([environ.get('IGN_GAZEBO_SYSTEM_PLUGIN_PATH', default=''), + environ.get('LD_LIBRARY_PATH', default='')]), + 'IGN_GAZEBO_RESOURCE_PATH': + ':'.join([environ.get('IGN_GAZEBO_RESOURCE_PATH', default=''), mars_helicopter_demos_path])} + + urdf_model_path = os.path.join(mars_helicopter_models_path, 'models', 'ingenuity', 'urdf', 'model.urdf') + mars_world_model = os.path.join(mars_helicopter_demos_path, 'worlds', 'jezero_crater.world') + + #doc = xacro.process_file(urdf_model_path) + #robot_description = {'robot_description': doc.toxml()} + + # hover_node = Node( + # package="mars_helicopter", + # executable="teleop_helicopter", + # output='screen' + # ) + + start_world = ExecuteProcess( + cmd=['ign gazebo', mars_world_model, '-r'], + output='screen', + additional_env=env, + shell=True + ) + + gz_ros2_bridge_yaml = os.path.join(mars_helicopter_demos_path, 'config', 'gz_ros2_bridge.yaml') + + # ROS 2 to Ignition bridge for joint states and TF + bridge_node = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + parameters=[{'config_file': gz_ros2_bridge_yaml}], + output='screen' + ) + + + return LaunchDescription([ + SetParameter(name='use_sim_time', value=True), + start_world, + bridge_node, + DeclareLaunchArgument( + 'use_sim_time', + default_value='true', + description='Use simulation (Gazebo) clock if true' + ), + ]) \ No newline at end of file diff --git a/mars_helicopter/mars_helicopter/__init__.py b/mars_helicopter/mars_helicopter/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/mars_helicopter/nodes/teleop_helicopter b/mars_helicopter/nodes/teleop_helicopter new file mode 100644 index 00000000..28dbf2b2 --- /dev/null +++ b/mars_helicopter/nodes/teleop_helicopter @@ -0,0 +1,245 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from builtin_interfaces.msg import Duration + +from std_msgs.msg import String, Float64 +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from std_srvs.srv import Empty + +import sys, select, os +if os.name == 'nt': + import msvcrt, time +else: + import tty, termios + +import threading +import time +import copy +import shutil + +from utils.key_capture import KeyCapture +from utils.display_cmd import CommandDisplay + +ROTOR_OPERATIONAL_SPEED = 270.0 +DELTA_INCREASE_COLL = 0.05 +DELTA_DECREASE_COLL = -0.01 +DELTA_CYCLIC = 0.01 +DELTA_YAW = 0.1 + + +class CommandLimits(): + def __init__(self, lim_thrust_coll, lim_lat_cycl, lim_long_cycl, lim_yaw): + self.lim_thrust_coll = lim_thrust_coll + self.lim_lat_cycl = lim_lat_cycl + self.lim_long_cycl = lim_long_cycl + +class HeliCommands(): + def __init__(self): + self.cmd_speed = 0.0 + self.cmd_thrust_coll = 0.0 + self.cmd_yaw_coll = 0.0 + self.cmd_lat_cycl = 0.0 + self.cmd_long_cycl = 0.0 + + def print_status(self): + """Prints the current status of all command attributes.""" + print(f"Speed Command: {self.cmd_speed:.2f}") + print(f"Thrust Collective Command: {self.cmd_thrust_coll:.2f}") + print(f"Yaw Collective Command: {self.cmd_yaw_coll:.2f}") + print(f"Lateral Cyclic Command: {self.cmd_lat_cycl:.2f}") + print(f"Longitudinal Cyclic Command: {self.cmd_long_cycl:.2f}") + +def clamp(n, smallest, largest): return max(smallest, min(n, largest)) + +def limit_add(val, delta, xinf, xsup): + return clamp(val + delta, xinf, xsup) + +class TeleopHelicopter(Node): + + def __init__(self, limits: CommandLimits): + super().__init__('teleop_helicopter_node') + self.ang_vel_T_pub_ = self.create_publisher(Float64, '/rotor_top_revolute/angular_velocity', 10) + self.ang_vel_B_pub_ = self.create_publisher(Float64, '/rotor_bottom_revolute/angular_velocity', 10) + self.coll_T_pub_ = self.create_publisher(Float64, '/rotor_top_revolute/collective', 10) + self.coll_B_pub_ = self.create_publisher(Float64, '/rotor_bottom_revolute/collective', 10) + self.lon_cyc_T_pub_ = self.create_publisher(Float64, '/rotor_top_revolute/longitudinal_cyclic', 10) + self.lon_cyc_B_pub_ = self.create_publisher(Float64, '/rotor_bottom_revolute/longitudinal_cyclic', 10) + self.lat_cyc_T_pub_ = self.create_publisher(Float64, '/rotor_top_revolute/lateral_cyclic', 10) + self.lat_cyc_B_pub_ = self.create_publisher(Float64, '/rotor_bottom_revolute/lateral_cyclic', 10) + self.commands = HeliCommands() + self.lock = threading.Lock() # to use when accessing commands + self.lims = limits + + def parse_commands(self, keys): + with self.lock: + coll = self.commands.cmd_thrust_coll + long_cycl = self.commands.cmd_long_cycl + lat_cycl = self.commands.cmd_lat_cycl + coll_lims = self.lims.lim_thrust_coll + long_cycl_lims = self.lims.lim_long_cycl + lat_cycl_lims = self.lims.lim_lat_cycl + if (' ' in keys): + self.commands.cmd_thrust_coll = limit_add(coll, DELTA_INCREASE_COLL, coll_lims[0], coll_lims[1]) + else: + self.commands.cmd_thrust_coll = limit_add(coll, DELTA_DECREASE_COLL, coll_lims[0], coll_lims[1]) + + + if ('w' in keys and 's' in keys): + pass + elif ('w' in keys): + if (long_cycl > 0): + self.commands.cmd_long_cycl = limit_add(long_cycl, DELTA_CYCLIC, long_cycl_lims[0], long_cycl_lims[1]) + else: + self.commands.cmd_long_cycl = DELTA_CYCLIC + elif ('s' in keys): + if (long_cycl < 0): + self.commands.cmd_long_cycl = limit_add(long_cycl, -DELTA_CYCLIC, long_cycl_lims[0], long_cycl_lims[1]) + else: + self.commands.cmd_long_cycl = -DELTA_CYCLIC + else: + self.commands.cmd_long_cycl = 0.0 + + if ('d' in keys and 'a' in keys): + pass + elif ('d' in keys): + if (lat_cycl > 0): + self.commands.cmd_lat_cycl = limit_add(lat_cycl, DELTA_CYCLIC, lat_cycl_lims[0], lat_cycl_lims[1]) + else: + self.commands.cmd_lat_cycl = DELTA_CYCLIC + elif ('a' in keys): + if (lat_cycl < 0): + self.commands.cmd_lat_cycl = limit_add(lat_cycl, -DELTA_CYCLIC, lat_cycl_lims[0], lat_cycl_lims[1]) + else: + self.commands.cmd_lat_cycl = -DELTA_CYCLIC + else: + self.commands.cmd_lat_cycl = 0.0 + + if ('\r' in keys): + if (self.commands.cmd_speed > 0): + self.commands.cmd_speed = 0.0 + else: + self.commands.cmd_speed = ROTOR_OPERATIONAL_SPEED + + if ('h' in keys and 'l' in keys): + pass + elif ('l' in keys): + self.commands.cmd_yaw_coll = DELTA_YAW + elif ('h' in keys): + self.commands.cmd_yaw_coll = -DELTA_YAW + else: + self.commands.cmd_yaw_coll = 0.0 + + commands = copy.copy(self.commands) + + return commands + + def get_commands(self): + with self.lock: + commands = copy.copy(self.commands) # Make a copy to safely access + return commands + + def publish_commands(self): + # angular velocity + msg = Float64() + msg.data = self.commands.cmd_speed + self.ang_vel_T_pub_.publish(msg) + self.ang_vel_B_pub_.publish(msg) + + # collective pitch (thrust and yaw) + top_coll = self.commands.cmd_thrust_coll + self.commands.cmd_yaw_coll + bottom_coll = self.commands.cmd_thrust_coll - self.commands.cmd_yaw_coll + msg.data = top_coll + self.coll_T_pub_.publish(msg) + msg.data = bottom_coll + self.coll_B_pub_.publish(msg) + + # longitudinal pitch + long_cycl = self.commands.cmd_long_cycl + msg.data = long_cycl + self.lon_cyc_T_pub_.publish(msg) + msg.data = -long_cycl + self.lon_cyc_B_pub_.publish(msg) + + # lateral pitch + lat_cycl = self.commands.cmd_lat_cycl + msg.data = lat_cycl + self.lat_cyc_T_pub_.publish(msg) + msg.data = -lat_cycl + self.lat_cyc_B_pub_.publish(msg) + + +def clear_screen(): + sys.stdout.write('\033[H\033[J') # ANSI escape code to clear the terminal screen + sys.stdout.flush() + +def print_command_descriptions(): + # Get terminal size + terminal_size = shutil.get_terminal_size() + terminal_width = terminal_size.columns + + # Define the width for the descriptions + description_width = terminal_width - 40 # Adjust based on grid width + + # Command descriptions + command_descriptions = [ + "Commands: (Simultaneous commands not yet supported)", + "Longitudinal Cyclic: W (Pitch down), S (Pitch up)", + "Lateral Cyclic: A (Roll left), D (Roll right)", + "Differential Collective: H (Yaw Left), L (Yaw Right)", + "Thrust: SPACEBAR (Increase)", + "Switch Rotors On/Off: ENTER (Toggle)" + ] + + # Print command descriptions + for p in command_descriptions: + formatted_line = p.ljust(description_width) + print(formatted_line) + +def main(args=None): + + rclpy.init(args=args) + limits = CommandLimits((0.0, 0.4),(-0.05, 0.05),(-0.05, 0.05),(-0.1,0.1)) + teleop_node = TeleopHelicopter(limits) + grid_size = 50 + cmd_display = CommandDisplay(limits, grid_size) + print("Teleop node online") + print_command_descriptions() + input("Press Enter to continue...") + + key_capture = KeyCapture() + key_capture.start_listening() # start a thread for key detection + + #command_display = CommandDisplay() + + # main loop + try: + while True: + keys = key_capture.get_pressed_keys() + if '\x03' in keys: + print(f"Teleoperation node is closing.") + break + + commands = teleop_node.parse_commands(keys) + #clear_screen() + cmd_display.draw_commands(commands) + + teleop_node.publish_commands() + + + time.sleep(0.1) # Check for key presses every 100 ms + except KeyboardInterrupt: + print("Stopping key capture...") + finally: + key_capture.stop_listening() + + + # Cleanup and shutdown + teleop_node.destroy_node() + rclpy.shutdown() + #thread.join() # Wait for the thread to finish + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/mars_helicopter/package.xml b/mars_helicopter/package.xml new file mode 100644 index 00000000..861d07b5 --- /dev/null +++ b/mars_helicopter/package.xml @@ -0,0 +1,30 @@ + + + + mars_helicopter + 0.0.1 + Mars helicopter based on Ingenuity simulated in Gazebo for the Space-ROS project + Davide Zamblera + Apache-2.0 + + ament_cmake + ament_cmake_python + + rclcpp + rclpy + + simulation + ament_index_python + launch + launch_ros + ros_ign_gazebo + std_msgs + xacro + + ament_lint_auto + ament_lint_common + + + ament_cmake + + \ No newline at end of file diff --git a/mars_helicopter/src/utils/__init__.py b/mars_helicopter/src/utils/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/mars_helicopter/src/utils/display_cmd.py b/mars_helicopter/src/utils/display_cmd.py new file mode 100644 index 00000000..ebcc1965 --- /dev/null +++ b/mars_helicopter/src/utils/display_cmd.py @@ -0,0 +1,90 @@ +import sys +import os +import shutil + +class CommandDisplay(): + def __init__(self, limits, grid_size): + self.size = grid_size + self.lims = limits + self.grid = [[' ' for i in range(grid_size)] for i in range(grid_size)] + self.empty_box = "+----------+\n| |\n| |\n| |\n| |\n+----------+" + self.half_box = "+----------+\n| |\n| ++++ |\n| ++++ |\n| |\n+----------+" + self.full_box = "+----------+\n|++++++++++|\n|++++++++++|\n|++++++++++|\n|++++++++++|\n+----------+" + self.yaw_left = "+---\n| \nv " + self.yaw_right = "---+\n |\n v" + + def clear_grid(self): + self.grid = [[' ' for _ in range(len(self.grid))] for _ in range(len(self.grid))] + + + def bar(self, progress): + max_lines = 14.0 + border = "+------+\n" + empty = "| |\n" + full = "|++++++|\n" + + n_full = int(progress*max_lines) + n_empty = int(max_lines - n_full) + return border + n_empty*empty + n_full*full + border + + def put_in_grid(self, start_position, text): + p = list(start_position) + x_reset = p[1] + for c in text: + if (c == '\n'): + p[0] += 1 + p[1] = x_reset + else: + y = p[0] + x = p[1] + self.grid[y][x] = c + p[1] += 1 + + def print_grid(self): + sys.stdout.write('\033[H\033[J') + for row in self.grid: + sys.stdout.write(''.join(row)) + sys.stdout.write("\n\r") + sys.stdout.flush() + self.clear_grid() + + def box_type(self, val, limit): + if (val == 0.0): + return self.empty_box + elif (val == limit): + return self.full_box + else: + return self.half_box + + def draw_commands(self, commands): + # Clear the screen to update in place + + rotor_on = commands.cmd_speed > 0.0 + is_yaw_right = commands.cmd_yaw_coll > 0.0 + is_yaw_left = commands.cmd_yaw_coll < 0.0 + long_cycl = commands.cmd_long_cycl + if (long_cycl >= 0.0): + self.put_in_grid((2,12),self.box_type(long_cycl, self.lims.lim_long_cycl[1])) + self.put_in_grid((14,12),self.empty_box) + else: + self.put_in_grid((2,12),self.empty_box) + self.put_in_grid((14,12),self.box_type(long_cycl, self.lims.lim_long_cycl[0])) + + lat_cycl = commands.cmd_lat_cycl + if (lat_cycl >= 0.0): + self.put_in_grid((8,24),self.box_type(lat_cycl, self.lims.lim_lat_cycl[1])) + self.put_in_grid((8,0),self.empty_box) + else: + self.put_in_grid((8,24),self.empty_box) + self.put_in_grid((8,0),self.box_type(lat_cycl, self.lims.lim_lat_cycl[0])) + + coll = commands.cmd_thrust_coll + progress = coll/(self.lims.lim_thrust_coll[1]-self.lims.lim_thrust_coll[0]) + self.put_in_grid((3,37),self.bar(progress)) + + if (is_yaw_right): + self.put_in_grid((3,29),self.yaw_right) + if (is_yaw_left): + self.put_in_grid((3,3),self.yaw_left) + + self.print_grid() \ No newline at end of file diff --git a/mars_helicopter/src/utils/key_capture.py b/mars_helicopter/src/utils/key_capture.py new file mode 100644 index 00000000..1577c052 --- /dev/null +++ b/mars_helicopter/src/utils/key_capture.py @@ -0,0 +1,47 @@ +import sys, select, os +if os.name == 'nt': + import msvcrt, time +else: + import tty, termios + +import threading +import time + +class KeyCapture: + def __init__(self): + self.pressed_keys = set() # Use a set to store unique key presses + self.stop_thread = False + + def getKey(self): + if os.name == 'nt': # Windows + while not self.stop_thread: + if msvcrt.kbhit(): + key = msvcrt.getch().decode() if sys.version_info[0] >= 3 else msvcrt.getch() + if key == '\r': # Handle Enter + key = '\n' + self.pressed_keys.add(key) + time.sleep(0.01) # Slight delay to avoid hogging CPU + else: # Unix-like + settings = termios.tcgetattr(sys.stdin) + tty.setraw(sys.stdin.fileno()) + try: + while not self.stop_thread: + rlist, _, _ = select.select([sys.stdin], [], [], 0.1) + if rlist: + key = sys.stdin.read(1) + self.pressed_keys.add(key) + finally: + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) + + def start_listening(self): + self.thread = threading.Thread(target=self.getKey) + self.thread.start() + + def stop_listening(self): + self.stop_thread = True + self.thread.join() + + def get_pressed_keys(self): + keys = list(self.pressed_keys) # Make a copy to safely access + self.pressed_keys.clear() # Clear the set after accessing + return keys \ No newline at end of file diff --git a/mars_helicopter/worlds/jezero_crater.world b/mars_helicopter/worlds/jezero_crater.world new file mode 100644 index 00000000..bcf3bd0d --- /dev/null +++ b/mars_helicopter/worlds/jezero_crater.world @@ -0,0 +1,146 @@ + + + + + model://jezero_crater + jezero_crater + 0 0 3 0 0 0 + + + + + uniform_density + 0.020 + + + + + + + + + ogre2 + + + + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + 0 1 1 0 0.5 -1.57 + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + + + + ingenuity/sensors/mono_camera/image_raw + 0 + + navigation camera + floating + 300 + 300 + 1 + 1 + 1 + + + + ingenuity/sensors/rgb_camera/image_raw + 0 + + front camera + floating + 300 + 300 + 300 + 1 + 1 + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + 0 0 -3.711 + + + model://ingenuity/sdf + mars_helicopter + 0 0 5.5 0 0 0 + + + + + \ No newline at end of file