Skip to content

Commit

Permalink
AP_DDS: Added Mode Switch Service
Browse files Browse the repository at this point in the history
  • Loading branch information
arshPratap authored and peterbarker committed Sep 10, 2023
1 parent 6a998fd commit 8c2627c
Show file tree
Hide file tree
Showing 5 changed files with 61 additions and 21 deletions.
40 changes: 35 additions & 5 deletions libraries/AP_DDS/AP_DDS_Client.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
#include <AP_BattMonitor/AP_BattMonitor.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Arming/AP_Arming.h>
#include <AP_Vehicle/AP_Vehicle.h>
#include <AP_ExternalControl/AP_ExternalControl_config.h>
#if AP_EXTERNAL_CONTROL_ENABLED
#include "AP_DDS_ExternalControl.h"
Expand Down Expand Up @@ -443,7 +444,6 @@ void AP_DDS_Client::on_topic(uxrSession* uxr_session, uxrObjectId object_id, uin
break;
}

subscribe_sample_count++;
if (rx_joy_topic.axes_size >= 4) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Received sensor_msgs/Joy: %f, %f, %f, %f",
rx_joy_topic.axes[0], rx_joy_topic.axes[1], rx_joy_topic.axes[2], rx_joy_topic.axes[3]);
Expand All @@ -459,7 +459,6 @@ void AP_DDS_Client::on_topic(uxrSession* uxr_session, uxrObjectId object_id, uin
break;
}

subscribe_sample_count++;
if (rx_dynamic_transforms_topic.transforms_size > 0) {
#if AP_DDS_VISUALODOM_ENABLED
AP_DDS_External_Odom::handle_external_odom(rx_dynamic_transforms_topic);
Expand All @@ -476,7 +475,6 @@ void AP_DDS_Client::on_topic(uxrSession* uxr_session, uxrObjectId object_id, uin
break;
}

subscribe_sample_count++;
#if AP_EXTERNAL_CONTROL_ENABLED
if (!AP_DDS_External_Control::handle_velocity_control(rx_velocity_control_topic)) {
// TODO #23430 handle velocity control failure through rosout, throttled.
Expand Down Expand Up @@ -533,8 +531,6 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u
break;
}

request_sample_count++;

uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub));
if (result) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"DDS Client: Request for Arming/Disarming : SUCCESS");
Expand All @@ -543,6 +539,40 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u
}
break;
}
case services[to_underlying(ServiceIndex::MODE_SWITCH)].rep_id: {
uint8_t mode;
const bool deserialize_success = ucdr_deserialize_uint8_t(ub,&mode);
if (deserialize_success == false) {
break;
}
bool status = AP::vehicle()->set_mode(mode, ModeReason::DDS_COMMAND);
uint8_t curr_mode = AP::vehicle()->get_mode();

const uxrObjectId replier_id = {
.id = services[to_underlying(ServiceIndex::MODE_SWITCH)].rep_id,
.type = UXR_REPLIER_ID
};

//Todo : Fix the size-handling of services with the help of the functions autogenerated via Micro-XRCE-DDS Gen
uint8_t reply_buffer[8] {};
ucdrBuffer reply_ub;

ucdr_init_buffer(&reply_ub, reply_buffer, sizeof(reply_buffer));
bool serialize_success = true;
serialize_success &= ucdr_serialize_bool(&reply_ub, status);
serialize_success &= ucdr_serialize_uint8_t(&reply_ub, curr_mode);
if (serialize_success == false || reply_ub.error) {
break;
}

uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub));
if (status) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"DDS Client: Request for Mode Switch : SUCCESS");
} else {
GCS_SEND_TEXT(MAV_SEVERITY_INFO,"DDS Client: Request for Mode Switch : FAIL");
}
break;
}
}
}

Expand Down
7 changes: 0 additions & 7 deletions libraries/AP_DDS/AP_DDS_Client.h
Original file line number Diff line number Diff line change
Expand Up @@ -87,14 +87,10 @@ class AP_DDS_Client
// subscription callback function
static void on_topic_trampoline(uxrSession* session, uxrObjectId object_id, uint16_t request_id, uxrStreamId stream_id, struct ucdrBuffer* ub, uint16_t length, void* args);
void on_topic(uxrSession* session, uxrObjectId object_id, uint16_t request_id, uxrStreamId stream_id, struct ucdrBuffer* ub, uint16_t length);
// count of subscribed samples
uint32_t subscribe_sample_count;

// service replier callback function
static void on_request_trampoline(uxrSession* session, uxrObjectId object_id, uint16_t request_id, SampleIdentity* sample_id, ucdrBuffer* ub, uint16_t length, void* args);
void on_request(uxrSession* session, uxrObjectId object_id, uint16_t request_id, SampleIdentity* sample_id, ucdrBuffer* ub, uint16_t length);
// count of request samples
uint32_t request_sample_count;

// delivery control parameters
uxrDeliveryControl delivery_control {
Expand Down Expand Up @@ -207,9 +203,6 @@ class AP_DDS_Client
//! @brief Reply ID for the service
const uint8_t rep_id;

//! @brief Profile Label for the service
const char* srv_profile_label;

//! @brief Profile Label for the service requester
const char* req_profile_label;

Expand Down
12 changes: 9 additions & 3 deletions libraries/AP_DDS/AP_DDS_Service_Table.h
Original file line number Diff line number Diff line change
@@ -1,7 +1,8 @@
#include "uxr/client/client.h"

enum class ServiceIndex: uint8_t {
ARMING_MOTORS
ARMING_MOTORS,
MODE_SWITCH
};

static inline constexpr uint8_t to_underlying(const ServiceIndex index)
Expand All @@ -14,8 +15,13 @@ constexpr struct AP_DDS_Client::Service_table AP_DDS_Client::services[] = {
{
.req_id = to_underlying(ServiceIndex::ARMING_MOTORS),
.rep_id = to_underlying(ServiceIndex::ARMING_MOTORS),
.srv_profile_label = "",
.req_profile_label = "",
.rep_profile_label = "arm_motors__replier",
}
},
{
.req_id = to_underlying(ServiceIndex::MODE_SWITCH),
.rep_id = to_underlying(ServiceIndex::MODE_SWITCH),
.req_profile_label = "",
.rep_profile_label = "mode_switch__replier",
},
};
12 changes: 12 additions & 0 deletions libraries/AP_DDS/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -238,6 +238,7 @@ nanosec: 729410000
```bash
$ ros2 service list
/ap/arm_motors
/ap/mode_switch
---
```

Expand All @@ -262,6 +263,7 @@ List the available services:
```bash
$ ros2 service list -t
/ap/arm_motors [ardupilot_msgs/srv/ArmMotors]
/ap/mode_switch [ardupilot_msgs/srv/ModeSwitch]
```

Call the arm motors service:
Expand All @@ -273,6 +275,16 @@ requester: making request: ardupilot_msgs.srv.ArmMotors_Request(arm=True)
response:
ardupilot_msgs.srv.ArmMotors_Response(result=True)
```

Call the mode switch service:

```bash
$ ros2 service call /ap/mode_switch ardupilot_msgs/srv/ModeSwitch "{mode: 4}"
requester: making request: ardupilot_msgs.srv.ModeSwitch_Request(mode=4)

response:
ardupilot_msgs.srv.ModeSwitch_Response(status=True, curr_mode=4)
```

## Contributing to `AP_DDS` library

Expand Down
11 changes: 5 additions & 6 deletions libraries/AP_DDS/dds_xrce_profile.xml
Original file line number Diff line number Diff line change
Expand Up @@ -268,13 +268,12 @@
<dataType>geometry_msgs::msg::dds_::TwistStamped_</dataType>
</topic>
</data_reader>
<replier
profile_name="arm_motors__replier"
service_name="rs/ap/arm_motorsService"
request_type="ardupilot_msgs::srv::dds_::ArmMotors_Request_"
reply_type="ardupilot_msgs::srv::dds_::ArmMotors_Response_">
<replier profile_name="arm_motors__replier" service_name="rs/ap/arm_motorsService" request_type="ardupilot_msgs::srv::dds_::ArmMotors_Request_" reply_type="ardupilot_msgs::srv::dds_::ArmMotors_Response_">
<request_topic_name>rq/ap/arm_motorsRequest</request_topic_name>
<reply_topic_name>rr/ap/arm_motorsReply</reply_topic_name>
</replier>

<replier profile_name="mode_switch__replier" service_name="rs/ap/mode_switchService" request_type="ardupilot_msgs::srv::dds_::ModeSwitch_Request_" reply_type="ardupilot_msgs::srv::dds_::ModeSwitch_Response_">
<request_topic_name>rq/ap/mode_switchRequest</request_topic_name>
<reply_topic_name>rr/ap/mode_switchReply</reply_topic_name>
</replier>
</profiles>

0 comments on commit 8c2627c

Please sign in to comment.