Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_DDS: Add defines for experimental topics such as IMU

Browse Source 
* Experimental topics, such as IMU, should have an easy way to be
  disabled at compile time
* This demonstrates a pattern to add in new topics at compile time

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>
This commit is contained in:
Ryan Friedman 2024-09-13 17:02:38 -06:00 committed by Andrew Tridgell
parent 94f2eb4999
commit dcfbf0ab69
4 changed files with 33 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_DDS/AP_DDS_Client.cpp
View File

@ -32,7 +32,9 @@
static constexpr uint8_t ENABLED_BY_DEFAULT = 1;
static constexpr uint16_t DELAY_TIME_TOPIC_MS = 10;
static constexpr uint16_t DELAY_BATTERY_STATE_TOPIC_MS = 1000;
#if AP_DDS_IMU_PUB_ENABLED
static constexpr uint16_t DELAY_IMU_TOPIC_MS = 5;
#endif // AP_DDS_IMU_PUB_ENABLED
static constexpr uint16_t DELAY_LOCAL_POSE_TOPIC_MS = 33;
static constexpr uint16_t DELAY_LOCAL_VELOCITY_TOPIC_MS = 33;
static constexpr uint16_t DELAY_GEO_POSE_TOPIC_MS = 33;
@ -440,6 +442,7 @@ void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPoseStamped& msg)
}
}
#if AP_DDS_IMU_PUB_ENABLED
void AP_DDS_Client::update_topic(sensor_msgs_msg_Imu& msg)
{
update_topic(msg.header.stamp);
@ -477,6 +480,7 @@ void AP_DDS_Client::update_topic(sensor_msgs_msg_Imu& msg)
msg.angular_velocity_covariance[0] = -1;
msg.linear_acceleration_covariance[0] = -1;
}
#endif // AP_DDS_IMU_PUB_ENABLED
void AP_DDS_Client::update_topic(rosgraph_msgs_msg_Clock& msg)
{
@ -1039,6 +1043,7 @@ void AP_DDS_Client::write_tx_local_velocity_topic()
}
}
#if AP_DDS_IMU_PUB_ENABLED
void AP_DDS_Client::write_imu_topic()
{
WITH_SEMAPHORE(csem);
@ -1053,6 +1058,7 @@ void AP_DDS_Client::write_imu_topic()
}
}
}
#endif // AP_DDS_IMU_PUB_ENABLED
void AP_DDS_Client::write_geo_pose_topic()
{
@ -1132,12 +1138,13 @@ void AP_DDS_Client::update()
last_local_velocity_time_ms = cur_time_ms;
write_tx_local_velocity_topic();
}
#if AP_DDS_IMU_PUB_ENABLED
if (cur_time_ms - last_imu_time_ms > DELAY_IMU_TOPIC_MS) {
update_topic(imu_topic);
last_imu_time_ms = cur_time_ms;
write_imu_topic();
}
#endif
if (cur_time_ms - last_geo_pose_time_ms > DELAY_GEO_POSE_TOPIC_MS) {
update_topic(geo_pose_topic);

10
libraries/AP_DDS/AP_DDS_Client.h
View File

@ -13,7 +13,9 @@
#include "sensor_msgs/msg/NavSatFix.h"
#include "tf2_msgs/msg/TFMessage.h"
#include "sensor_msgs/msg/BatteryState.h"
#if AP_DDS_IMU_PUB_ENABLED
#include "sensor_msgs/msg/Imu.h"
#endif // AP_DDS_IMU_PUB_ENABLED
#include "sensor_msgs/msg/Joy.h"
#include "geometry_msgs/msg/PoseStamped.h"
#include "geometry_msgs/msg/TwistStamped.h"
@ -65,7 +67,9 @@ private:
geometry_msgs_msg_TwistStamped tx_local_velocity_topic;
sensor_msgs_msg_BatteryState battery_state_topic;
sensor_msgs_msg_NavSatFix nav_sat_fix_topic;
#if AP_DDS_IMU_PUB_ENABLED
sensor_msgs_msg_Imu imu_topic;
#endif // AP_DDS_IMU_PUB_ENABLED
rosgraph_msgs_msg_Clock clock_topic;
// incoming joystick data
static sensor_msgs_msg_Joy rx_joy_topic;
@ -91,7 +95,9 @@ private:
static void update_topic(geometry_msgs_msg_PoseStamped& msg);
static void update_topic(geometry_msgs_msg_TwistStamped& msg);
static void update_topic(geographic_msgs_msg_GeoPoseStamped& msg);
#if AP_DDS_IMU_PUB_ENABLED
static void update_topic(sensor_msgs_msg_Imu& msg);
#endif // AP_DDS_IMU_PUB_ENABLED
static void update_topic(rosgraph_msgs_msg_Clock& msg);
static void update_topic(geographic_msgs_msg_GeoPointStamped& msg);
@ -117,8 +123,10 @@ private:
uint64_t last_nav_sat_fix_time_ms;
// The last ms timestamp AP_DDS wrote a BatteryState message
uint64_t last_battery_state_time_ms;
#if AP_DDS_IMU_PUB_ENABLED
// The last ms timestamp AP_DDS wrote an IMU message
uint64_t last_imu_time_ms;
#endif // AP_DDS_IMU_PUB_ENABLED
// The last ms timestamp AP_DDS wrote a Local Pose message
uint64_t last_local_pose_time_ms;
// The last ms timestamp AP_DDS wrote a Local Velocity message
@ -197,8 +205,10 @@ public:
void write_tx_local_velocity_topic();
//! @brief Serialize the current geo_pose and publish to the IO stream(s)
void write_geo_pose_topic();
#if AP_DDS_IMU_PUB_ENABLED
//! @brief Serialize the current IMU data and publish to the IO stream(s)
void write_imu_topic();
#endif // AP_DDS_IMU_PUB_ENABLED
//! @brief Serialize the current clock and publish to the IO stream(s)
void write_clock_topic();
//! @brief Serialize the current gps global origin and publish to the IO stream(s)

6
libraries/AP_DDS/AP_DDS_Topic_Table.h
View File

@ -3,7 +3,9 @@
#include "tf2_msgs/msg/TFMessage.h"
#include "sensor_msgs/msg/BatteryState.h"
#include "geographic_msgs/msg/GeoPoseStamped.h"
#if AP_DDS_IMU_PUB_ENABLED
#include "sensor_msgs/msg/Imu.h"
#endif //AP_DDS_IMU_PUB_ENABLED
#include "uxr/client/client.h"
@ -16,7 +18,9 @@ enum class TopicIndex: uint8_t {
NAV_SAT_FIX_PUB,
STATIC_TRANSFORMS_PUB,
BATTERY_STATE_PUB,
#if AP_DDS_IMU_PUB_ENABLED
IMU_PUB,
#endif //AP_DDS_IMU_PUB_ENABLED
LOCAL_POSE_PUB,
LOCAL_VELOCITY_PUB,
GEOPOSE_PUB,
@ -100,6 +104,7 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
.depth = 5,
},
},
#if AP_DDS_IMU_PUB_ENABLED
{
.topic_id = to_underlying(TopicIndex::IMU_PUB),
.pub_id = to_underlying(TopicIndex::IMU_PUB),
@ -116,6 +121,7 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
.depth = 5,
},
},
#endif //AP_DDS_IMU_PUB_ENABLED
{
.topic_id = to_underlying(TopicIndex::LOCAL_POSE_PUB),
.pub_id = to_underlying(TopicIndex::LOCAL_POSE_PUB),

9
libraries/AP_DDS/AP_DDS_config.h
View File

@ -17,6 +17,15 @@
#define AP_DDS_VISUALODOM_ENABLED HAL_VISUALODOM_ENABLED && AP_DDS_ENABLED
#endif
// Whether experimental interfaces are enabled.
#ifndef AP_DDS_EXPERIMENTAL_ENABLED
#define AP_DDS_EXPERIMENTAL_ENABLED 1
#endif
#ifndef AP_DDS_IMU_PUB_ENABLED
#define AP_DDS_IMU_PUB_ENABLED AP_DDS_EXPERIMENTAL_ENABLED
#endif
#ifndef AP_DDS_DEFAULT_UDP_IP_ADDR
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
#define AP_DDS_DEFAULT_UDP_IP_ADDR "192.168.13.2"