From aed0dfa5930dbf27921f22ea601f6923bc309a5b Mon Sep 17 00:00:00 2001 From: Rhys Mainwaring Date: Mon, 17 Apr 2023 13:48:14 +0100 Subject: [PATCH] Copter: add gimbal and camera to iris (#2) - Merge gimbal and camera peripheral into model. - Update iris launch file to set SDF_PATH before running the sdformat_urdf converter. - Update bring up launch file to use the iris with gimbal params. - Add bridge entry for the camera and camera info. - Add image display and TF to rviz config. - Add ros2_gz.repos containing dependencies - Update README Signed-off-by: Rhys Mainwaring --- README.md | 188 ++++++++++-------- ardupilot_gz_bringup/config/iris.rviz | 115 ++++++++++- ardupilot_gz_bringup/config/iris_bridge.yaml | 11 +- .../launch/bringup_iris.launch.py | 6 +- ardupilot_gz_bringup/launch/iris.launch.py | 16 +- ardupilot_gz_gazebo/worlds/iris.sdf | 58 +++++- ros2_gz.repos | 37 ++++ 7 files changed, 337 insertions(+), 94 deletions(-) create mode 100644 ros2_gz.repos diff --git a/README.md b/README.md index 4942aa9..8f9aa3c 100644 --- a/README.md +++ b/README.md @@ -1,128 +1,158 @@ # ardupilot_gz -Adapted from the [`ros_gz_project_template`](https://github.com/gazebosim/ros_gz_project_template) project integrating ROS 2 and Gazebo. +This project contains ROS 2 packages for simulating models controlled +by ArduPilot SITL with DDS support in Gazebo. + +The project is adapted from the [`ros_gz_project_template`](https://github.com/gazebosim/ros_gz_project_template) project. ## Included packages -* `ardupilot_gz_description` - Contains the sdf description of the simulated - system and any other assets. +* `ardupilot_gz_description` - Contains the SDFormat description of the simulated + system. -* `ardupilot_gz_gazebo` - Contains Gazebo specific code and configurations such - as system plugins. +* `ardupilot_gz_gazebo` - Contains Gazebo specific code such as system plugins. * `ardupilot_gz_application` - Contains ROS 2 specific code and configuration. * `ardupilot_gz_bringup` - Contains launch files and high level utilities. +## Prerequisites + +- Install [ROS 2 Humble](https://docs.ros.org/en/humble/index.html) +- Install [Gazebo Garden](https://gazebosim.org/docs/garden) + ## Install -### Dependencies +#### 1. Create a workspace folder -1. Install [ROS 2 Humble](https://docs.ros.org/en/humble/index.html) +```bash +mkdir -p ~/ros2_ws/src && cd ~/ros2_ws/src +``` -1. Install [Gazebo Garden](https://gazebosim.org/docs/garden) +#### 2. Get the project source -1. Install necessary tools +```bash +cd ~/ros2_ws/src +wget https://raw.githubusercontent.com/srmainwaring/ardupilot_gz/wips/wip-iris-camera/ros2_gz.repos +vcs import --recursive < ros2_gz.repos +``` - `sudo apt install python3-vcstool python3-colcon-common-extensions git wget` +#### 3. Set the Gazebo version to Garden: -### Usage +```bash +export GZ_VERSION=garden +``` -1. Create a workspace, for example: +#### 4. Update ROS dependencies - ``` - mkdir -p ~/ros2_ws/src - cd ~/ros2_ws/src - ``` +```bash +cd ~/ros_ws +source /opt/ros/humble/setup.bash +sudo apt update +rosdep update +rosdep install --rosdistro humble --from-paths src -i -r -y +``` -1. Clone the project: +#### 5. Build - ``` - TBC - ``` +```bash +cd ~/ros2_ws +colcon build --cmake-args -DBUILD_TESTING=ON +``` -1. Set the Gazebo version to Garden: +#### 6. Test - ``` - export GZ_VERSION=garden - ``` +```bash +source ./install/setup.bash +colcon test --packages-select ardupilot_dds_sitl ardupilot_dds_tests ardupilot_gazebo ardupilot_gz_applications ardupilot_gz_description ardupilot_gz_gazebo ardupilot_gz_bringup +colcon test-result --all --verbose +``` -1. Install ROS dependencies +## Usage - ``` - sudo rosdep init - rosdep update - rosdep install --from-paths src --ignore-src -r -y -i - ``` +#### 1. Source the workspace -1. Build and install +```bash +source ~/ros2_ws/install/setup.sh +``` - ``` - cd ~/ros2_ws - colcon build --cmake-args -DBUILD_TESTING=ON - ``` +#### 2. Launch the simulation -### Run +```bash +ros2 launch ardupilot_gz_bringup bringup_iris.launch.py +``` -1. Source the workspace +#### 3. Launch a GCS (MAVPorxy) - `. ~/ros2_ws/install/setup.sh` +```bash +mavproxy.py --console --map +``` -1. Launch the simulation +#### 4. Inspect topics - `ros2 launch ardupilot_gz_bringup bringup_iris.launch.py` +```bash +$ ros2 topic list +/ROS2_BatteryState0 +/ROS2_NavSatFix0 +/ROS2_Time +/clicked_point +/clock +/goal_pose +/initialpose +/iris/odometry +/joint_states +/parameter_events +/robot_description +/rosout +/tf +/tf_static +``` -1. Launch the a GCS +## Notes - `mavproxy.py --console --map` +#### 1. Additional dependencies -1. Inspect topics +`ros_gz` has a dependency on `gps_msgs` included in - ```bash - $ ros2 topic list - /ROS2_BatteryState0 - /ROS2_NavSatFix0 - /ROS2_Time - /clicked_point - /clock - /goal_pose - /initialpose - /iris/odometry - /joint_states - /parameter_events - /robot_description - /rosout - /tf - /tf_static - ``` +```bash +git clone https://github.com/swri-robotics/gps_umd.git -b ros2-devel +``` +Add `COLCON_IGNORE` to `gpsd_client` as this package is not required and +will not build on macOS. -## Notes +#### 2. `sdformat_urdf` + +On macOS the `robot_state_publisher` node cannot load the +`sdformat_urdf_plugin` plugin unless the +suffix is changed: -1. Additional dependency +```bash +cd ./install/sdformat_urdf/lib +ln -s libsdformat_urdf_plugin.so libsdformat_urdf_plugin.dylib +``` - `ros_gz` has a dependency on `gps_msgs` included in +#### 3. Model URIs - ```bash - git clone https://github.com/swri-robotics/gps_umd.git -b ros2-devel - ``` +The `sdformat_urdf` plugin requires that the `` element use +the `package` prefix for a resource to be located by RViz. - Add `COLCON_IGNORE` to `gpsd_client` as this package is not required and - will not build on macOS. -1. sdformat_urdf +#### 4. SDFormat environment variables - On macOS the `robot_state_publisher` node cannot load the - `sdformat_urdf_plugin` plugin unless the - suffix is changed: +The `sdformat_urdf` plugin uses the `sdformat13` libraries to parse the +model description. `sdformat13` relies on the environment variable +`SDF_PATH` to resolve model resources. This is usually set in `gz-sim7`, +however when using the plugins standalone, for instance in the bring-up +launch files, `SDF_PATH` must be set otherwise the plugin will not resolve +the models and their dependencies. - ```bash - cd ./install/sdformat_urdf/lib - ln -s libsdformat_urdf_plugin.so libsdformat_urdf_plugin.dylib - ``` +```bash +source ~/ros2_ws/install/setup.sh +export SDF_PATH=$GZ_SIM_RESOURCE_PATH +``` -1. Model URIs +This is checked in the launch file as `SDF_PATH` is not usually set +by the `ament` environment hooks. - The `` element must use the `package` prefix for resource to be located - by RViz. diff --git a/ardupilot_gz_bringup/config/iris.rviz b/ardupilot_gz_bringup/config/iris.rviz index cd01149..59e5d06 100644 --- a/ardupilot_gz_bringup/config/iris.rviz +++ b/ardupilot_gz_bringup/config/iris.rviz @@ -1,14 +1,13 @@ Panels: - Class: rviz_common/Displays - Help Height: 78 + Help Height: 36 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - - /RobotModel1 - - /Odometry1 + - /Image1/Topic1 Splitter Ratio: 0.5 - Tree Height: 510 + Tree Height: 253 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -71,10 +70,20 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + gimbal_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true imu_link: Alpha: 1 Show Axes: false Show Trail: false + roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true rotor_0: Alpha: 1 Show Axes: false @@ -95,6 +104,11 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true Mass Properties: Inertia: false Mass: false @@ -103,6 +117,79 @@ Visualization Manager: Update Interval: 0 Value: true Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + base_link: + Value: true + gimbal_link: + Value: true + imu_link: + Value: true + iris: + Value: true + iris/base_link: + Value: true + iris/gimbal_link: + Value: true + iris/imu_link: + Value: true + iris/odom: + Value: true + iris/roll_link: + Value: true + iris/rotor_0: + Value: true + iris/rotor_1: + Value: true + iris/rotor_2: + Value: true + iris/rotor_3: + Value: true + iris/tilt_link: + Value: true + roll_link: + Value: true + rotor_0: + Value: true + rotor_1: + Value: true + rotor_2: + Value: true + rotor_3: + Value: true + tilt_link: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + iris/odom: + iris: + iris/base_link: + {} + iris/gimbal_link: + {} + iris/imu_link: + {} + iris/roll_link: + {} + iris/rotor_0: + {} + iris/rotor_1: + {} + iris/rotor_2: + {} + iris/rotor_3: + {} + iris/tilt_link: + {} + Update Interval: 0 + Value: true - Angle Tolerance: 0.10000000149011612 Class: rviz_default_plugins/Odometry Covariance: @@ -121,7 +208,7 @@ Visualization Manager: Value: true Value: true Enabled: true - Keep: 100 + Keep: 500 Name: Odometry Position Tolerance: 0.10000000149011612 Shape: @@ -142,6 +229,20 @@ Visualization Manager: Reliability Policy: Reliable Value: /iris/odometry Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera/image + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -214,7 +315,9 @@ Window Geometry: Height: 801 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001630000029bfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002c0000029b000000e300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000029bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002c0000029b000000c600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004af0000003efc0100000002fb0000000800540069006d00650100000000000004af0000023d00fffffffb0000000800540069006d006501000000000000045000000000000000000000034b0000029b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001630000029bfc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002c00000170000000e300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000019d0000012a0000003000ffffff000000010000010f0000029bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002c0000029b000000c600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004af0000003efc0100000002fb0000000800540069006d00650100000000000004af0000023d00fffffffb0000000800540069006d006501000000000000045000000000000000000000034b0000029b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: diff --git a/ardupilot_gz_bringup/config/iris_bridge.yaml b/ardupilot_gz_bringup/config/iris_bridge.yaml index 39c95bf..285ab62 100644 --- a/ardupilot_gz_bringup/config/iris_bridge.yaml +++ b/ardupilot_gz_bringup/config/iris_bridge.yaml @@ -24,4 +24,13 @@ ros_type_name: "tf2_msgs/msg/TFMessage" gz_type_name: "gz.msgs.Pose_V" direction: GZ_TO_ROS - +- ros_topic_name: "/camera/image" + gz_topic_name: "/world/demo/model/iris/link/tilt_link/sensor/camera/image" + ros_type_name: "sensor_msgs/msg/Image" + gz_type_name: "gz.msgs.Image" + direction: GZ_TO_ROS +- ros_topic_name: "/camera/camera_info" + gz_topic_name: "/world/demo/model/iris/link/tilt_link/sensor/camera/camera_info" + ros_type_name: "sensor_msgs/msg/CameraInfo" + gz_type_name: "gz.msgs.CameraInfo" + direction: GZ_TO_ROS diff --git a/ardupilot_gz_bringup/launch/bringup_iris.launch.py b/ardupilot_gz_bringup/launch/bringup_iris.launch.py index ba1f1f5..78209cf 100644 --- a/ardupilot_gz_bringup/launch/bringup_iris.launch.py +++ b/ardupilot_gz_bringup/launch/bringup_iris.launch.py @@ -52,6 +52,7 @@ def generate_launch_description(): """Generate a launch description for a iris quadcopter.""" pkg_ardupilot_sitl = get_package_share_directory("ardupilot_sitl") + pkg_ardupilot_gazebo = get_package_share_directory("ardupilot_gazebo") # Include component launch files. iris_gz = IncludeLaunchDescription( @@ -100,10 +101,9 @@ def generate_launch_description(): "instance": "0", "uartC": "uart:./dev/ttyROS1", "defaults": os.path.join( - pkg_ardupilot_sitl, + pkg_ardupilot_gazebo, "config", - "default_params", - "gazebo-iris.parm", + "gazebo-iris-gimbal.parm", ) + "," + os.path.join( diff --git a/ardupilot_gz_bringup/launch/iris.launch.py b/ardupilot_gz_bringup/launch/iris.launch.py index 7a894eb..ca728c9 100644 --- a/ardupilot_gz_bringup/launch/iris.launch.py +++ b/ardupilot_gz_bringup/launch/iris.launch.py @@ -48,15 +48,23 @@ def generate_launch_description(): """Generate a launch description for a iris quadcopter.""" pkg_project_bringup = get_package_share_directory("ardupilot_gz_bringup") pkg_project_gazebo = get_package_share_directory("ardupilot_gz_gazebo") - # pkg_project_description = get_package_share_directory( - # 'ardupilot_gz_description') pkg_ros_gz_sim = get_package_share_directory("ros_gz_sim") - pkg_ardupilot_gazebo = get_package_share_directory("ardupilot_gazebo") + # Ensure `SDF_PATH` is populated as `sdformat_urdf` uses this rather + # than `GZ_SIM_RESOURCE_PATH` to locate resources. + if "GZ_SIM_RESOURCE_PATH" in os.environ: + gz_sim_resource_path = os.environ["GZ_SIM_RESOURCE_PATH"] + + if "SDF_PATH" in os.environ: + sdf_path = os.environ["SDF_PATH"] + os.environ["SDF_PATH"] = sdf_path + ":" + gz_sim_resource_path + else: + os.environ["SDF_PATH"] = gz_sim_resource_path + # Load the SDF file from "description" package sdf_file = os.path.join( - pkg_ardupilot_gazebo, "models", "iris_with_standoffs", "model.sdf" + pkg_ardupilot_gazebo, "models", "iris_with_gimbal", "model.sdf" ) with open(sdf_file, "r") as infp: robot_desc = infp.read() diff --git a/ardupilot_gz_gazebo/worlds/iris.sdf b/ardupilot_gz_gazebo/worlds/iris.sdf index 602c195..b9c97b4 100755 --- a/ardupilot_gz_gazebo/worlds/iris.sdf +++ b/ardupilot_gz_gazebo/worlds/iris.sdf @@ -50,11 +50,30 @@ 0 0 0.194923 0 0 0 + package://ardupilot_gazebo/models/iris_with_standoffs iris + + package://ardupilot_gazebo/models/gimbal_small_2d + gimbal + 0 -0.01 -0.11 1.57 0 1.57 + + + + base_link + gimbal_link + + + 0 + 0 + + 0 0 1 + + + @@ -233,8 +252,23 @@ rotor_3_joint + + roll_joint + /gimbal/cmd_roll + 2 + + + tilt_joint + /gimbal/cmd_tilt + 2 + + + filename="ArduPilotPlugin"> 127.0.0.1 9002 5 @@ -318,6 +352,28 @@ 1 + + roll_joint + 3.14159265 + -0.5 + 1100 + 1900 + COMMAND + /gimbal/cmd_roll + 3 + + + + tilt_joint + 3.14159265 + -0.5 + 1100 + 1900 + COMMAND + /gimbal/cmd_tilt + 3 + + diff --git a/ros2_gz.repos b/ros2_gz.repos new file mode 100644 index 0000000..435ffa3 --- /dev/null +++ b/ros2_gz.repos @@ -0,0 +1,37 @@ +repositories: + ardupilot: + type: git + url: https://github.com/srmainwaring/ardupilot.git + version: wip/dds-launch-gz + ardupilot_gazebo: + type: git + url: https://github.com/srmainwaring/ardupilot_gazebo-1.git + version: wips/wip-colcon-pkg-sdf-mods + ardupilot_gz: + type: git + url: https://github.com/srmainwaring/ardupilot_gz.git + version: wips/wip-iris-camera + gps_umd: + type: git + url: https://github.com/swri-robotics/gps_umd.git + version: ros2-devel + micro_ros_agent: + type: git + url: https://github.com/srmainwaring/micro-ROS-Agent.git + version: cmake-macos + micro_ros_msgs: + type: git + url: https://github.com/micro-ROS/micro_ros_msgs.git + version: humble + microxrcedds_gen: + type: git + url: https://github.com/eProsima/Micro-XRCE-DDS-Gen.git + version: master + ros_gz: + type: git + url: https://github.com/gazebosim/ros_gz.git + version: ros2 + sdformat_urdf: + type: git + url: https://github.com/srmainwaring/sdformat_urdf.git + version: srmainwaring/humble-garden