mirror of https://github.com/ArduPilot/ardupilot
AP_DDS: Rally Interface to get-set rally points.
This commit is contained in:
Tiziano Fiorenzani
parent
dd37065bb5
commit
92ce630cda
|
|
@ -0,0 +1,200 @@
|
|||
# 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/>.
|
||||
|
||||
"""
|
||||
Tests the Rally Point interface.
|
||||
|
||||
Clear the list, adds points and verifies that they are correctly updated.
|
||||
|
||||
colcon test --packages-select ardupilot_dds_tests \
|
||||
--event-handlers=console_cohesion+ --pytest-args -k test_rally_point
|
||||
|
||||
"""
|
||||
|
||||
import rclpy
|
||||
import time
|
||||
import launch_pytest
|
||||
from rclpy.node import Node
|
||||
from builtin_interfaces.msg import Time
|
||||
from ardupilot_msgs.msg import Rally
|
||||
from ardupilot_msgs.srv import RallyGet
|
||||
from ardupilot_msgs.srv import RallySet
|
||||
import pytest
|
||||
from launch_pytest.tools import process as process_tools
|
||||
from launch import LaunchDescription
|
||||
import threading
|
||||
|
||||
RALLY_0 = Rally()
|
||||
RALLY_0.point.latitude =-35.0
|
||||
RALLY_0.point.longitude = 149.0
|
||||
RALLY_0.point.altitude = 400.0
|
||||
RALLY_0.altitude_frame = 2
|
||||
|
||||
|
||||
class RallyControl(Node):
|
||||
"""Push/Pull Rally points."""
|
||||
|
||||
def __init__(self):
|
||||
"""Initialise the node."""
|
||||
super().__init__("rally_service")
|
||||
self.clear_rally_event = threading.Event()
|
||||
self.add_rally_event = threading.Event()
|
||||
self.get_rally_event = threading.Event()
|
||||
|
||||
self._client_rally_get = self.create_client(RallyGet, "/ap/rally_get")
|
||||
self._client_rally_set = self.create_client(RallySet, "/ap/rally_set")
|
||||
# Add a spin thread.
|
||||
self.ros_spin_thread = threading.Thread(target=lambda node: rclpy.spin(node), args=(self,))
|
||||
self.ros_spin_thread.start()
|
||||
|
||||
def _clear_list(self):
|
||||
req = RallySet.Request()
|
||||
req.clear = True
|
||||
self.future = self._client_rally_set.call_async(req)
|
||||
time.sleep(1.0)
|
||||
if self.future.result() is None:
|
||||
return False
|
||||
else:
|
||||
print(self.future)
|
||||
print(self.future.result())
|
||||
return self.future.result().size == 0
|
||||
|
||||
def _send_rally(self, rally):
|
||||
req = RallySet.Request()
|
||||
req.clear = False
|
||||
req.rally = rally
|
||||
self.future = self._client_rally_set.call_async(req)
|
||||
time.sleep(1.0)
|
||||
return self.future.result()
|
||||
|
||||
def _get_rally(self):
|
||||
req = RallyGet.Request()
|
||||
req.index = 0
|
||||
self.future = self._client_rally_get.call_async(req)
|
||||
time.sleep(1.0)
|
||||
return self.future.result()
|
||||
|
||||
#-------------- PROCESSES -----------------
|
||||
def process_clear(self):
|
||||
print("---> Process start")
|
||||
while True:
|
||||
if self._clear_list():
|
||||
print("---> List Cleared")
|
||||
self.clear_rally_event.set()
|
||||
return
|
||||
time.sleep(1)
|
||||
|
||||
def clear_list(self):
|
||||
try:
|
||||
self.clear_thread.stop()
|
||||
except:
|
||||
print("---> Starting thread")
|
||||
self.clear_thread = threading.Thread(target=self.process_clear)
|
||||
self.clear_thread.start()
|
||||
|
||||
def process_send(self, rally):
|
||||
while True:
|
||||
response = self._send_rally(rally)
|
||||
if response is not None:
|
||||
if response.success:
|
||||
self.add_rally_event.set()
|
||||
return
|
||||
time.sleep(1)
|
||||
|
||||
def send_rally(self, rally):
|
||||
try:
|
||||
self.send_thread.stop()
|
||||
except:
|
||||
self.send_thread = threading.Thread(target=self.process_send, args=[rally])
|
||||
self.send_thread.start()
|
||||
|
||||
def process_compare(self, rally):
|
||||
while True:
|
||||
response = self._get_rally()
|
||||
if response is not None:
|
||||
if response.success:
|
||||
if (abs(response.rally.point.latitude - rally.point.latitude) < 1e-7
|
||||
and abs(response.rally.point.longitude - rally.point.longitude) < 1e-7
|
||||
and response.rally.altitude_frame == rally.altitude_frame):
|
||||
self.get_rally_event.set()
|
||||
return
|
||||
time.sleep(1)
|
||||
|
||||
def get_and_compare_rally(self, rally):
|
||||
try:
|
||||
self.get_thread.stop()
|
||||
except:
|
||||
self.get_thread = threading.Thread(target=self.process_compare, args=[rally])
|
||||
self.get_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_rally(launch_context, launch_sitl_copter_dds_udp):
|
||||
"""Test Rally Points with 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()
|
||||
time.sleep(5)
|
||||
try:
|
||||
node = RallyControl()
|
||||
node.clear_list()
|
||||
clear_flag = node.clear_rally_event.wait(20)
|
||||
assert clear_flag, "Could not clear"
|
||||
node.send_rally(RALLY_0)
|
||||
|
||||
send_flag = node.add_rally_event.wait(10)
|
||||
assert send_flag, "Could not send Rally"
|
||||
|
||||
node.get_and_compare_rally(RALLY_0)
|
||||
send_flag = node.get_rally_event.wait(10)
|
||||
assert send_flag, "Wrong Rally point back"
|
||||
finally:
|
||||
rclpy.shutdown()
|
||||
yield
|
||||
|
|
@ -5,6 +5,7 @@ project(ardupilot_msgs)
|
|||
# Find dependencies.
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(geographic_msgs REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(rosidl_default_generators REQUIRED)
|
||||
|
|
@ -14,11 +15,14 @@ find_package(rosidl_default_generators REQUIRED)
|
|||
|
||||
rosidl_generate_interfaces(${PROJECT_NAME}
|
||||
"msg/GlobalPosition.msg"
|
||||
"msg/Rally.msg"
|
||||
"msg/Status.msg"
|
||||
"srv/ArmMotors.srv"
|
||||
"srv/ModeSwitch.srv"
|
||||
"srv/Takeoff.srv"
|
||||
DEPENDENCIES geometry_msgs std_msgs
|
||||
"srv/RallyGet.srv"
|
||||
"srv/RallySet.srv"
|
||||
DEPENDENCIES geographic_msgs geometry_msgs std_msgs
|
||||
ADD_LINTER_TESTS
|
||||
)
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,11 @@
|
|||
# Rally message provides a geographic point and
|
||||
# its altitude reference frame.
|
||||
|
||||
geographic_msgs/GeoPoint point
|
||||
|
||||
# Altitude reference frame
|
||||
uint8 ALTITUDE_FRAME_ABSOLUTE = 0
|
||||
uint8 ALTITUDE_FRAME_HOME = 1
|
||||
uint8 ALTITUDE_FRAME_ORIGIN = 2
|
||||
uint8 ALTITUDE_FRAME_TERRAIN = 3
|
||||
uint8 altitude_frame
|
||||
|
|
@ -0,0 +1,11 @@
|
|||
# This service requests a Rally point from its ID.
|
||||
|
||||
# Rally Point ID.
|
||||
uint8 index
|
||||
---
|
||||
# True is the retrieved point is valid.
|
||||
bool success
|
||||
# Size of Rally Points list.
|
||||
uint8 size
|
||||
# Rally point.
|
||||
ardupilot_msgs/Rally rally
|
||||
|
|
@ -0,0 +1,12 @@
|
|||
# This service appends a Rally Point to the list or clears the whole list.
|
||||
|
||||
# Rally Point.
|
||||
ardupilot_msgs/Rally rally
|
||||
|
||||
# True to clear the whole list (rally entry is ignored).
|
||||
bool clear
|
||||
---
|
||||
# True if the action is successful.
|
||||
bool success
|
||||
# Size of Rally Points list.
|
||||
uint8 size
|
||||
|
|
@ -32,6 +32,11 @@
|
|||
#if AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED
|
||||
#include "ardupilot_msgs/srv/Takeoff.h"
|
||||
#endif // AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED
|
||||
#if AP_DDS_RALLY_SERVER_ENABLED
|
||||
#include <AP_Rally/AP_Rally.h>
|
||||
#include "ardupilot_msgs/srv/RallyGet.h"
|
||||
#include "ardupilot_msgs/srv/RallySet.h"
|
||||
#endif // AP_DDS_RALLY_SERVER_ENABLED
|
||||
|
||||
#if AP_EXTERNAL_CONTROL_ENABLED
|
||||
#include "AP_DDS_ExternalControl.h"
|
||||
|
|
@ -976,6 +981,110 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u
|
|||
break;
|
||||
}
|
||||
#endif //AP_DDS_ARM_CHECK_SERVER_ENABLED
|
||||
#if AP_DDS_RALLY_SERVER_ENABLED
|
||||
case services[to_underlying(ServiceIndex::GET_RALLY)].rep_id: {
|
||||
ardupilot_msgs_srv_RallyGet_Request rally_get_request;
|
||||
ardupilot_msgs_srv_RallyGet_Response rally_get_response;
|
||||
const bool deserialize_success = ardupilot_msgs_srv_RallyGet_Request_deserialize_topic(ub, &rally_get_request);
|
||||
if (deserialize_success == false) {
|
||||
break;
|
||||
}
|
||||
|
||||
const uxrObjectId replier_id = {
|
||||
.id = services[to_underlying(ServiceIndex::GET_RALLY)].rep_id,
|
||||
.type = UXR_REPLIER_ID
|
||||
};
|
||||
|
||||
RallyLocation rally_location {};
|
||||
const AP_Rally *rally = AP::rally();
|
||||
if (rally->get_rally_point_with_index(rally_get_request.index, rally_location)) {
|
||||
rally_get_response.success = true;
|
||||
rally_get_response.size = rally->get_rally_total();
|
||||
rally_get_response.rally.point.latitude = rally_location.lat * 1e-7;
|
||||
rally_get_response.rally.point.longitude = rally_location.lng * 1e-7;
|
||||
rally_get_response.rally.point.altitude = rally_location.alt;
|
||||
bool alt_frame_valid = rally_location.flags & (1 << 2);
|
||||
if (alt_frame_valid) {
|
||||
rally_get_response.rally.altitude_frame = (rally_location.flags >> 3) & ((1 << 2) - 1);
|
||||
} else {
|
||||
rally_get_response.rally.altitude_frame = static_cast<uint8_t>(Location::AltFrame::ABOVE_HOME);
|
||||
}
|
||||
} else {
|
||||
rally_get_response.success = false;
|
||||
}
|
||||
|
||||
uint8_t reply_buffer[ardupilot_msgs_srv_RallyGet_Response_size_of_topic(&rally_get_response, 0)] {};
|
||||
ucdrBuffer reply_ub;
|
||||
|
||||
ucdr_init_buffer(&reply_ub, reply_buffer, sizeof(reply_buffer));
|
||||
const bool serialize_success = ardupilot_msgs_srv_RallyGet_Response_serialize_topic(&reply_ub, &rally_get_response);
|
||||
if (serialize_success == false) {
|
||||
break;
|
||||
}
|
||||
|
||||
uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub));
|
||||
|
||||
break;
|
||||
}
|
||||
case services[to_underlying(ServiceIndex::SET_RALLY)].rep_id: {
|
||||
|
||||
ardupilot_msgs_srv_RallySet_Request rally_set_request;
|
||||
ardupilot_msgs_srv_RallySet_Response rally_set_response;
|
||||
const bool deserialize_success = ardupilot_msgs_srv_RallySet_Request_deserialize_topic(ub, &rally_set_request);
|
||||
if (deserialize_success == false) {
|
||||
break;
|
||||
}
|
||||
|
||||
const uxrObjectId replier_id = {
|
||||
.id = services[to_underlying(ServiceIndex::SET_RALLY)].rep_id,
|
||||
.type = UXR_REPLIER_ID
|
||||
};
|
||||
|
||||
RallyLocation rally_location{};
|
||||
AP_Rally *rally = AP::rally();
|
||||
|
||||
if (rally_set_request.clear) {
|
||||
rally->truncate(0);
|
||||
rally_set_response.success = rally->get_rally_total() == 0;
|
||||
} else {
|
||||
rally_location.lat = static_cast<int32_t>(rally_set_request.rally.point.latitude * 1e7);
|
||||
rally_location.lng = static_cast<int32_t>(rally_set_request.rally.point.longitude * 1e7);
|
||||
rally_location.alt = static_cast<int16_t>(rally_set_request.rally.point.altitude);
|
||||
rally_location.flags = 0;
|
||||
rally_location.flags |= (1 << 2); // Use the provided frame.
|
||||
rally_location.flags |= (rally_set_request.rally.altitude_frame << 3);
|
||||
|
||||
if (rally_location.lat != 0 && rally_location.lng != 0
|
||||
&& (rally_set_request.rally.altitude_frame == static_cast<uint8_t>(Location::AltFrame::ABOVE_HOME)
|
||||
|| rally_set_request.rally.altitude_frame == static_cast<uint8_t>(Location::AltFrame::ABOVE_ORIGIN)
|
||||
|| rally_set_request.rally.altitude_frame == static_cast<uint8_t>(Location::AltFrame::ABOVE_TERRAIN)
|
||||
|| rally_set_request.rally.altitude_frame == static_cast<uint8_t>(Location::AltFrame::ABSOLUTE))) {
|
||||
if (rally->append(rally_location)) {
|
||||
rally_set_response.success = true;
|
||||
} else {
|
||||
rally_set_response.success = false;
|
||||
}
|
||||
} else {
|
||||
rally_set_response.success = false;
|
||||
}
|
||||
}
|
||||
|
||||
rally_set_response.size = rally->get_rally_total();
|
||||
|
||||
uint8_t reply_buffer[ardupilot_msgs_srv_RallySet_Response_size_of_topic(&rally_set_response, 0)] {};
|
||||
ucdrBuffer reply_ub;
|
||||
|
||||
ucdr_init_buffer(&reply_ub, reply_buffer, sizeof(reply_buffer));
|
||||
const bool serialize_success = ardupilot_msgs_srv_RallySet_Response_serialize_topic(&reply_ub, &rally_set_response);
|
||||
if (serialize_success == false) {
|
||||
break;
|
||||
}
|
||||
|
||||
uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub));
|
||||
|
||||
break;
|
||||
}
|
||||
#endif // AP_DDS_RALLY_SERVER_ENABLED
|
||||
#if AP_DDS_PARAMETER_SERVER_ENABLED
|
||||
case services[to_underlying(ServiceIndex::SET_PARAMETERS)].rep_id: {
|
||||
const bool deserialize_success = rcl_interfaces_srv_SetParameters_Request_deserialize_topic(ub, &set_parameter_request);
|
||||
|
|
|
|||
|
|
@ -14,6 +14,10 @@ enum class ServiceIndex: uint8_t {
|
|||
#if AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED
|
||||
TAKEOFF,
|
||||
#endif // AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED
|
||||
#if AP_DDS_RALLY_SERVER_ENABLED
|
||||
GET_RALLY,
|
||||
SET_RALLY,
|
||||
#endif // AP_DDS_RALLY_SERVER_ENABLED
|
||||
#if AP_DDS_PARAMETER_SERVER_ENABLED
|
||||
SET_PARAMETERS,
|
||||
GET_PARAMETERS
|
||||
|
|
@ -81,6 +85,28 @@ constexpr struct AP_DDS_Client::Service_table AP_DDS_Client::services[] = {
|
|||
.reply_topic_name = "rr/ap/experimental/takeoffReply",
|
||||
},
|
||||
#endif // AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED
|
||||
#if AP_DDS_RALLY_SERVER_ENABLED
|
||||
{
|
||||
.req_id = to_underlying(ServiceIndex::GET_RALLY),
|
||||
.rep_id = to_underlying(ServiceIndex::GET_RALLY),
|
||||
.service_rr = Service_rr::Replier,
|
||||
.service_name = "rs/ap/rally_getService",
|
||||
.request_type = "ardupilot_msgs::srv::dds_::RallyGet_Request_",
|
||||
.reply_type = "ardupilot_msgs::srv::dds_::RallyGet_Response_",
|
||||
.request_topic_name = "rq/ap/rally_getRequest",
|
||||
.reply_topic_name = "rr/ap/rally_getReply",
|
||||
},
|
||||
{
|
||||
.req_id = to_underlying(ServiceIndex::SET_RALLY),
|
||||
.rep_id = to_underlying(ServiceIndex::SET_RALLY),
|
||||
.service_rr = Service_rr::Replier,
|
||||
.service_name = "rs/ap/rally_setService",
|
||||
.request_type = "ardupilot_msgs::srv::dds_::RallySet_Request_",
|
||||
.reply_type = "ardupilot_msgs::srv::dds_::RallySet_Response_",
|
||||
.request_topic_name = "rq/ap/rally_setRequest",
|
||||
.reply_topic_name = "rr/ap/rally_setReply",
|
||||
},
|
||||
#endif // AP_DDS_RALLY_SERVER_ENABLED
|
||||
#if AP_DDS_PARAMETER_SERVER_ENABLED
|
||||
{
|
||||
.req_id = to_underlying(ServiceIndex::SET_PARAMETERS),
|
||||
|
|
|
|||
|
|
@ -145,6 +145,10 @@
|
|||
#define AP_DDS_VTOL_TAKEOFF_SERVER_ENABLED 1
|
||||
#endif
|
||||
|
||||
#ifndef AP_DDS_RALLY_SERVER_ENABLED
|
||||
#define AP_DDS_RALLY_SERVER_ENABLED 1
|
||||
#endif
|
||||
|
||||
#ifndef AP_DDS_PARAMETER_SERVER_ENABLED
|
||||
#define AP_DDS_PARAMETER_SERVER_ENABLED 1
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -0,0 +1,23 @@
|
|||
// generated from rosidl_adapter/resource/msg.idl.em
|
||||
// with input from ardupilot_msgs/msg/Rally.msg
|
||||
// generated code does not contain a copyright notice
|
||||
|
||||
#include "geographic_msgs/msg/GeoPoint.idl"
|
||||
|
||||
module ardupilot_msgs {
|
||||
module msg {
|
||||
module Rally_Constants {
|
||||
@verbatim (language="comment", text=
|
||||
"Altitude reference frame")
|
||||
const uint8 ALTITUDE_FRAME_ABSOLUTE = 0;
|
||||
const uint8 ALTITUDE_FRAME_HOME = 1;
|
||||
const uint8 ALTITUDE_FRAME_ORIGIN = 2;
|
||||
const uint8 ALTITUDE_FRAME_TERRAIN = 3;
|
||||
};
|
||||
struct Rally {
|
||||
geographic_msgs::msg::GeoPoint point;
|
||||
|
||||
uint8 altitude_frame;
|
||||
};
|
||||
};
|
||||
};
|
||||
|
|
@ -0,0 +1,30 @@
|
|||
// generated from rosidl_adapter/resource/srv.idl.em
|
||||
// with input from ardupilot_msgs/srv/RallyGet.srv
|
||||
// generated code does not contain a copyright notice
|
||||
|
||||
#include "ardupilot_msgs/msg/Rally.idl"
|
||||
|
||||
module ardupilot_msgs {
|
||||
module srv {
|
||||
@verbatim (language="comment", text=
|
||||
"This service requests a Rally point from its ID.")
|
||||
struct RallyGet_Request {
|
||||
@verbatim (language="comment", text=
|
||||
"Rally Point ID.")
|
||||
uint8 index;
|
||||
};
|
||||
@verbatim (language="comment", text=
|
||||
"True is the retrieved point is valid.")
|
||||
struct RallyGet_Response {
|
||||
boolean success;
|
||||
|
||||
@verbatim (language="comment", text=
|
||||
"Size of Rally Points list.")
|
||||
uint8 size;
|
||||
|
||||
@verbatim (language="comment", text=
|
||||
"Rally point.")
|
||||
ardupilot_msgs::msg::Rally rally;
|
||||
};
|
||||
};
|
||||
};
|
||||
|
|
@ -0,0 +1,29 @@
|
|||
// generated from rosidl_adapter/resource/srv.idl.em
|
||||
// with input from ardupilot_msgs/srv/RallySet.srv
|
||||
// generated code does not contain a copyright notice
|
||||
|
||||
#include "ardupilot_msgs/msg/Rally.idl"
|
||||
|
||||
module ardupilot_msgs {
|
||||
module srv {
|
||||
struct RallySet_Request {
|
||||
@verbatim (language="comment", text=
|
||||
"This service appends a Rally Point to the list or clears the whole list." "\n"
|
||||
"Rally Point.")
|
||||
ardupilot_msgs::msg::Rally rally;
|
||||
|
||||
@verbatim (language="comment", text=
|
||||
"True to clear the whole list (rally entry is ignored).")
|
||||
boolean clear;
|
||||
};
|
||||
@verbatim (language="comment", text=
|
||||
"True if the action is successful.")
|
||||
struct RallySet_Response {
|
||||
boolean success;
|
||||
|
||||
@verbatim (language="comment", text=
|
||||
"Size of Rally Points list.")
|
||||
uint8 size;
|
||||
};
|
||||
};
|
||||
};
|
||||
|
|
@ -211,6 +211,8 @@ $ ros2 service list
|
|||
/ap/mode_switch
|
||||
/ap/prearm_check
|
||||
/ap/experimental/takeoff
|
||||
/ap/rally_get
|
||||
/ap/rally_set
|
||||
---
|
||||
```
|
||||
|
||||
|
|
@ -238,6 +240,8 @@ $ ros2 service list -t
|
|||
/ap/mode_switch [ardupilot_msgs/srv/ModeSwitch]
|
||||
/ap/prearm_check [std_srvs/srv/Trigger]
|
||||
/ap/experimental/takeoff [ardupilot_msgs/srv/Takeoff]
|
||||
/ap/rally_get [ardupilot_msgs/srv/RallyGet]
|
||||
/ap/rally_set [ardupilot_msgs/srv/RallySet]
|
||||
```
|
||||
|
||||
Call the arm motors service:
|
||||
|
|
@ -284,6 +288,45 @@ response:
|
|||
ardupilot_msgs.srv.Takeoff_Response(status=True)
|
||||
```
|
||||
|
||||
Clear the Rally Points list (the provided Point will be ignored):
|
||||
|
||||
```bash
|
||||
ros2 service call /ap/rally_set ardupilot_msgs/srv/RallySet "{clear: true}"
|
||||
requester: making request: ardupilot_msgs.srv.RallySet_Request(rally=ardupilot_msgs.msg.Rally(point=geographic_msgs.msg.GeoPoint(latitude=0.0, longitude=0.0, altitude=0.0), altitude_frame=0), clear=True)
|
||||
|
||||
response:
|
||||
ardupilot_msgs.srv.RallySet_Response(success=True, size=0)
|
||||
```
|
||||
|
||||
Append a Rally Point:
|
||||
|
||||
Rally points units of measure are:
|
||||
* degrees for the latitude and longitude (up to 7 decimal digits resolution);
|
||||
* meters for the altitude (with no decimals).
|
||||
|
||||
The `altitude_frame` specifies the datum for the altitude, among Home, Absolute, Terrain or EKF Origin
|
||||
(use the enumerator specified in the message definition).
|
||||
|
||||
```bash
|
||||
ros2 service call /ap/rally_set ardupilot_msgs/srv/RallySet "{rally: {altitude_frame: 1, point: {latitude: -35.12345, longitude: 151.12345, altitude: 400}}}"
|
||||
|
||||
requester: making request: ardupilot_msgs.srv.RallySet_Request(rally=ardupilot_msgs.msg.Rally(point=geographic_msgs.msg.GeoPoint(latitude=-35.12345, longitude=151.12345, altitude=400.0), altitude_frame=1), clear=False)
|
||||
|
||||
response:
|
||||
ardupilot_msgs.srv.RallySet_Response(success=True, size=1)
|
||||
```
|
||||
|
||||
Get a specific Rally Point (the same service can be used to request the size of the Rally list):
|
||||
|
||||
```bash
|
||||
ros2 service call /ap/rally_get ardupilot_msgs/srv/RallyGet "index: 0"
|
||||
|
||||
requester: making request: ardupilot_msgs.srv.RallyGet_Request(index=0)
|
||||
|
||||
response:
|
||||
ardupilot_msgs.srv.RallyGet_Response(success=True, size=1, rally=ardupilot_msgs.msg.Rally(point=geographic_msgs.msg.GeoPoint(latitude=35.12345504760742, longitude=151.12345176604336, altitude=400.0), altitude_frame=1))
|
||||
```
|
||||
|
||||
## Commanding using ROS 2 Topics
|
||||
|
||||
The following topic can be used to control the vehicle.
|
||||
|
|
@ -308,7 +351,7 @@ ros2 topic pub /ap/cmd_gps_pose ardupilot_msgs/msg/GlobalPosition "{latitude: 34
|
|||
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