Skip to content

Commit

Permalink
AP_DDS: Implement joystick support
Browse files Browse the repository at this point in the history
Signed-off-by: Ryan Friedman <[email protected]>
Co-Authored-by: Tiziano Fiorenzani
  • Loading branch information
Ryan Friedman authored and peterbarker committed Oct 1, 2024
1 parent b6f20e3 commit 1bdc635
Show file tree
Hide file tree
Showing 3 changed files with 277 additions and 9 deletions.
Original file line number Diff line number Diff line change
@@ -0,0 +1,229 @@
# Copyright 2023 ArduPilot.org.
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <https://www.gnu.org/licenses/>.

"""
Bring up ArduPilot SITL and check the Joy message is being published.
Arms the copter and commands a throttle up. Check that the vehicle climbs.
colcon test --packages-select ardupilot_dds_tests \
--event-handlers=console_cohesion+ --pytest-args -k test_joy_msg_received
"""

import rclpy
import time
import launch_pytest
from rclpy.node import Node
from builtin_interfaces.msg import Time
from geographic_msgs.msg import GeoPoseStamped
from geometry_msgs.msg import TwistStamped
from geopy import point
from ardupilot_msgs.srv import ArmMotors
from ardupilot_msgs.srv import ModeSwitch
from sensor_msgs.msg import Joy
from scipy.spatial.transform import Rotation as R
import pytest
from launch_pytest.tools import process as process_tools
from launch import LaunchDescription
import threading

GUIDED = 4
LOITER = 5

FRAME_GLOBAL_INT = 5

# Hard code some known locations
# Note - Altitude in geopy is in km!
GRAYHOUND_TRACK = point.Point(latitude=-35.345996, longitude=149.159017, altitude=0.575)
CMAC = point.Point(latitude=-35.3627010, longitude=149.1651513, altitude=0.585)


class PlaneFbwbJoyControl(Node):
"""Plane follow waypoints using guided control."""

def __init__(self):
"""Initialise the node."""
super().__init__("copter_joy_listener")

self.arming_event_object = threading.Event()
self.mode_event_object = threading.Event()
self.climbing_event_object = threading.Event()

self._client_arm = self.create_client(ArmMotors, "/ap/arm_motors")

self._client_mode_switch = self.create_client(ModeSwitch, "/ap/mode_switch")

self._joy_pub = self.create_publisher(Joy, "/ap/joy", 1)

# Create subscriptions after services are up
qos = rclpy.qos.QoSProfile(
reliability=rclpy.qos.ReliabilityPolicy.BEST_EFFORT, durability=rclpy.qos.DurabilityPolicy.VOLATILE, depth=1
)
self._subscription_geopose = self.create_subscription(GeoPoseStamped, "/ap/geopose/filtered", self.geopose_cb, qos)
self._cur_geopose = GeoPoseStamped()

self._subscription_twist = self.create_subscription(TwistStamped, "/ap/twist/filtered", self.twist_cb, qos)
self._climb_rate = 0.0

# Add a spin thread.
self.ros_spin_thread = threading.Thread(target=lambda node: rclpy.spin(node), args=(self,))
self.ros_spin_thread.start()

def geopose_cb(self, msg: GeoPoseStamped):
"""Process a GeoPose message."""
stamp = msg.header.stamp
if stamp.sec:
# Store current state
self._cur_geopose = msg

def twist_cb(self, msg: TwistStamped):
"""Process a Twist message."""
linear = msg.twist.linear
self._climb_rate = linear.z

def arm(self):
req = ArmMotors.Request()
req.arm = True
future = self._client_arm.call_async(req)
time.sleep(0.2)
try:
return future.result().result
except:
return False

def arm_with_timeout(self, timeout: rclpy.duration.Duration):
"""Try to arm. Returns true on success, or false if arming fails or times out."""
armed = False
start = self.get_clock().now()
while not armed and self.get_clock().now() - start < timeout:
armed = self.arm()
time.sleep(1)
return armed

def switch_mode(self, mode):
req = ModeSwitch.Request()
req.mode = mode
future = self._client_mode_switch.call_async(req)
time.sleep(1)
try:
return future.result().status and future.result().curr_mode == mode
except:
return False

def switch_mode_with_timeout(self, desired_mode: int, timeout: rclpy.duration.Duration):
"""Try to switch mode. Returns true on success, or false if mode switch fails or times out."""
is_in_desired_mode = False
start = self.get_clock().now()
while not is_in_desired_mode:
is_in_desired_mode = self.switch_mode(desired_mode)
time.sleep(1)

return is_in_desired_mode

def get_cur_geopose(self):
"""Return latest geopose."""
return self._cur_geopose

def send_joy_value(self, joy_val):
self._joy_pub.publish(joy_val)

#-------------- PROCESSES -----------------
def process_arm(self):
if self.arm_with_timeout(rclpy.duration.Duration(seconds=30)):
self.arming_event_object.set()

def start_arm(self):
self.arm_thread = threading.Thread(target=self.process_arm)
self.arm_thread.start()

def process_climb(self):
joy_msg = Joy()
joy_msg.axes.append(0.0)
joy_msg.axes.append(0.0)
joy_msg.axes.append(0.8) #- straight up
joy_msg.axes.append(0.0)

while True:
self.send_joy_value(joy_msg)
self.arm() #- Keep the system armed (sometimes it disarms and the test fails)
time.sleep(0.1)
self.get_logger().info("From AP : Climb rate {}".format(self._climb_rate))
if self._climb_rate > 0.5:
self.climbing_event_object.set()
return

def climb(self):
self.climb_thread = threading.Thread(target=self.process_climb)
self.climb_thread.start()

def arm_and_takeoff_process(self):
self.process_arm()
self.climb()

def arm_and_takeoff(self):
self.climb_thread = threading.Thread(target=self.arm_and_takeoff_process)
self.climb_thread.start()

@launch_pytest.fixture
def launch_sitl_copter_dds_serial(sitl_copter_dds_serial):
"""Fixture to create the launch description."""
sitl_ld, sitl_actions = sitl_copter_dds_serial

ld = LaunchDescription(
[
sitl_ld,
launch_pytest.actions.ReadyToTest(),
]
)
actions = sitl_actions
yield ld, actions

@launch_pytest.fixture
def launch_sitl_copter_dds_udp(sitl_copter_dds_udp):
"""Fixture to create the launch description."""
sitl_ld, sitl_actions = sitl_copter_dds_udp

ld = LaunchDescription(
[
sitl_ld,
launch_pytest.actions.ReadyToTest(),
]
)
actions = sitl_actions
yield ld, actions

@pytest.mark.launch(fixture=launch_sitl_copter_dds_udp)
def test_dds_udp_joy_msg_recv(launch_context, launch_sitl_copter_dds_udp):
"""Test joy messages are published by AP_DDS."""
_, actions = launch_sitl_copter_dds_udp
micro_ros_agent = actions["micro_ros_agent"].action
mavproxy = actions["mavproxy"].action
sitl = actions["sitl"].action

# Wait for process to start.
process_tools.wait_for_start_sync(launch_context, micro_ros_agent, timeout=2)
process_tools.wait_for_start_sync(launch_context, mavproxy, timeout=2)
process_tools.wait_for_start_sync(launch_context, sitl, timeout=2)

rclpy.init()
try:
node = PlaneFbwbJoyControl()
node.arm_and_takeoff()
climb_flag = node.climbing_event_object.wait(10)
assert climb_flag, "Could not climb"
finally:
rclpy.shutdown()
yield
23 changes: 18 additions & 5 deletions libraries/AP_DDS/AP_DDS_Client.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@

#include <AP_GPS/AP_GPS.h>
#include <AP_HAL/AP_HAL.h>
#include <RC_Channel/RC_Channel.h>
#include <AP_RTC/AP_RTC.h>
#include <AP_Math/AP_Math.h>
#include <AP_InertialSensor/AP_InertialSensor.h>
Expand Down Expand Up @@ -573,11 +574,23 @@ void AP_DDS_Client::on_topic(uxrSession* uxr_session, uxrObjectId object_id, uin
}

if (rx_joy_topic.axes_size >= 4) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Received sensor_msgs/Joy: %f, %f, %f, %f",
msg_prefix, rx_joy_topic.axes[0], rx_joy_topic.axes[1], rx_joy_topic.axes[2], rx_joy_topic.axes[3]);
// TODO implement joystick RC control to AP
} else {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Received sensor_msgs/Joy. Axes size must be >= 4", msg_prefix);
const uint32_t t_now = AP_HAL::millis();

for (uint8_t i = 0; i < MIN(8U, rx_joy_topic.axes_size); i++) {
// Ignore channel override if NaN.
if (std::isnan(rx_joy_topic.axes[i])) {
// Setting the RC override to 0U releases the channel back to the RC.
RC_Channels::set_override(i, 0U, t_now);
} else {
const uint16_t mapped_data = static_cast<uint16_t>(
linear_interpolate(rc().channel(i)->get_radio_min(),
rc().channel(i)->get_radio_max(),
rx_joy_topic.axes[i],
-1.0, 1.0));
RC_Channels::set_override(i, mapped_data, t_now);
}
}

}
break;
}
Expand Down
34 changes: 30 additions & 4 deletions libraries/AP_DDS/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -76,6 +76,7 @@ Run the simulator with the following command. If using UDP, the only parameter y
| DDS_ENABLE | Set to 1 to enable DDS, or 0 to disable | 1 |
| SERIAL1_BAUD | The serial baud rate for DDS | 57 |
| SERIAL1_PROTOCOL | Set this to 45 to use DDS on the serial port | 0 |

```console
# Wipe params till you see "AP: ArduPilot Ready"
# Select your favorite vehicle type
Expand Down Expand Up @@ -220,7 +221,7 @@ In order to consume the transforms, it's highly recommended to [create and run a

## Using ROS 2 services

The `AP_DDS` library exposes services which are automatically mapped to ROS 2
The `AP_DDS` library exposes services which are automatically mapped to ROS 2
services using appropriate naming conventions for topics and message and service
types. An earlier version of `AP_DDS` required the use of the eProsima
[Integration Service](https://github.com/eProsima/Integration-Service) to map
Expand Down Expand Up @@ -253,7 +254,32 @@ requester: making request: ardupilot_msgs.srv.ModeSwitch_Request(mode=4)
response:
ardupilot_msgs.srv.ModeSwitch_Response(status=True, curr_mode=4)
```


## Commanding using ROS 2 Topics

The following topic can be used to control the vehicle.

- `/ap/joy` (type `sensor_msgs/msg/Joy`): overrides a maximum of 8 RC channels,
at least 4 axes must be sent. Values are clamped between -1.0 and 1.0.
Use `NaN` to disable the override of a single channel.
A channel defaults back to RC after 1 second of not receiving commands.

```bash
ros2 topic pub /ap/joy sensor_msgs/msg/Joy "{axes: [0.0, 0.0, 0.0, 0.0]}"

publisher: beginning loop
publishing #1: sensor_msgs.msg.Joy(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id=''), axes=[0.0, 0.0, 0.0, 0.0], buttons=[])
```
- `/ap/cmd_gps_pose` (type `ardupilot_msgs/msg/GlobalPosition`): sends
a waypoint to head to when the selected mode is GUIDED.

```bash
ros2 topic pub /ap/cmd_gps_pose ardupilot_msgs/msg/GlobalPosition "{latitude: 34, longitude: 118, altitude: 1000}"

publisher: beginning loop
publishing #1: ardupilot_msgs.msg.GlobalPosition(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id=''), coordinate_frame=0, type_mask=0, latitude=34.0, longitude=118.0, altitude=1000.0, velocity=geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0)), acceleration_or_force=geometry_msgs.msg.Twist(linear=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0), angular=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0)), yaw=0.0)
```

## Contributing to `AP_DDS` library

### Adding DDS messages to Ardupilot
Expand Down Expand Up @@ -296,7 +322,7 @@ topic and service tables.
ROS 2 message and interface definitions are mangled by the `rosidl_adapter` when
mapping from ROS 2 to DDS to avoid naming conflicts in the C/C++ libraries.
The ROS 2 object `namespace::Struct` is mangled to `namespace::dds_::Struct_`
for DDS. The table below provides some example mappings:
for DDS. The table below provides some example mappings:

| ROS 2 | DDS |
| --- | --- |
Expand All @@ -322,7 +348,7 @@ The request / response pair for services require an additional suffix.
| parameter | rp/ | |
| action | ra/ | |

The table below provides example mappings for topics and services
The table below provides example mappings for topics and services

| ROS 2 | DDS |
| --- | --- |
Expand Down

0 comments on commit 1bdc635

Please sign in to comment.