mirror of https://github.com/ArduPilot/ardupilot
AP_DDS: Implement joystick support
Signed-off-by: Ryan Friedman <ryan.friedman+github@avinc.com> Co-Authored-by: Tiziano Fiorenzani
This commit is contained in:
Ryan Friedman
Peter Barker
parent
b6f20e3786
commit
1bdc635ba8
|
|
@ -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
|
||||
|
|
@ -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>
|
||||
|
|
@ -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
|
||||
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 {
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s Received sensor_msgs/Joy. Axes size must be >= 4", msg_prefix);
|
||||
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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -254,6 +255,31 @@ 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
|
||||
|
|
|
|||
Loading…
Reference in New Issue