diff --git a/.gitmodules b/.gitmodules index fbfe76753a51d6..11cc7809d58b0f 100644 --- a/.gitmodules +++ b/.gitmodules @@ -39,3 +39,6 @@ path = modules/Micro-CDR url = https://github.com/ardupilot/Micro-CDR.git branch = master +[submodule "modules/ardupilot_uros"] + path = modules/ardupilot_uros + url = https://github.com/srmainwaring/ardupilot_uros.git diff --git a/Tools/ardupilotwaf/ardupilotwaf.py b/Tools/ardupilotwaf/ardupilotwaf.py index 75c9975b826bd7..f706b177ca8605 100644 --- a/Tools/ardupilotwaf/ardupilotwaf.py +++ b/Tools/ardupilotwaf/ardupilotwaf.py @@ -138,6 +138,7 @@ def get_legacy_defines(sketch_name, bld): 'doc', 'AP_Scripting', # this gets explicitly included when it is needed and should otherwise never be globbed in 'AP_DDS', + 'AP_UROS', ] diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index faf500cc24ee30..5f3c41f6b9d021 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -84,6 +84,18 @@ def srcpath(path): env.ENABLE_DDS = False env.DEFINES.update(AP_DDS_ENABLED = 0) + # configurations for UROS + if cfg.options.enable_uros: + cfg.recurse('libraries/AP_UROS') + env.ENABLE_UROS = True + env.AP_LIBRARIES += [ + 'AP_UROS' + ] + env.DEFINES.update(AP_UROS_ENABLED = 1) + else: + env.ENABLE_UROS = False + env.DEFINES.update(AP_UROS_ENABLED = 0) + # setup for supporting onvif cam control if cfg.options.enable_onvif: cfg.recurse('libraries/AP_ONVIF') diff --git a/Tools/autotest/sim_vehicle.py b/Tools/autotest/sim_vehicle.py index 3e0fb79eb4c566..96522c30226333 100755 --- a/Tools/autotest/sim_vehicle.py +++ b/Tools/autotest/sim_vehicle.py @@ -411,6 +411,9 @@ def do_build(opts, frame_options): if opts.enable_dds: cmd_configure.append("--enable-dds") + if opts.enable_uros: + cmd_configure.append("--enable-uros") + pieces = [shlex.split(x) for x in opts.waf_configure_args] for piece in pieces: cmd_configure.extend(piece) @@ -1326,6 +1329,8 @@ def generate_frame_help(): help="IP address of the simulator. Defaults to localhost") group_sim.add_option("--enable-dds", action='store_true', help="Enable the dds client to connect with ROS2/DDS") +group_sim.add_option("--enable-uros", action='store_true', + help="Enable the micro-ros client to connect with ROS2/DDS") parser.add_option_group(group_sim) diff --git a/Tools/ros2/README.md b/Tools/ros2/README.md index bdd055e3f13291..691c9c5ac08c1c 100644 --- a/Tools/ros2/README.md +++ b/Tools/ros2/README.md @@ -239,8 +239,28 @@ master:=tcp:127.0.0.1:5760 \ sitl:=127.0.0.1:5501 ``` -UDP version +### DDS UDP version -``` +```bash ros2 launch ardupilot_sitl sitl_dds_udp.launch.py transport:=udp4 refs:=$(ros2 pkg prefix ardupilot_sitl)/share/ardupilot_sitl/config/dds_xrce_profile.xml synthetic_clock:=True wipe:=False model:=quad speedup:=1 slave:=0 instance:=0 defaults:=$(ros2 pkg prefix ardupilot_sitl)/share/ardupilot_sitl/config/default_params/copter.parm,$(ros2 pkg prefix ardupilot_sitl)/share/ardupilot_sitl/config/default_params/dds_udp.parm sim_address:=127.0.0.1 master:=tcp:127.0.0.1:5760 sitl:=127.0.0.1:5501 ``` + +### UROS UDP + +Launch agent: + +```bash +ros2 launch ardupilot_sitl micro_ros_agent.launch.py transport:=udp4 port:=2019 verbose:=6 +``` + +Launch SITL: + +```bash +ros2 launch ardupilot_sitl sitl.launch.py synthetic_clock:=True wipe:=False model:=quad speedup:=1 slave:=0 instance:=0 defaults:=$(ros2 pkg prefix ardupilot_sitl)/share/ardupilot_sitl/config/default_params/copter.parm,$(ros2 pkg prefix ardupilot_sitl)/share/ardupilot_sitl/config/default_params/dds_udp.parm sim_address:=127.0.0.1 +``` + +Launch non-interactive MAVProxy: + +```bash +ros2 launch ardupilot_sitl mavproxy.launch.py master:=tcp:127.0.0.1:5760 sitl:=127.0.0.1:5501 +``` diff --git a/Tools/ros2/ardupilot_sitl/CMakeLists.txt b/Tools/ros2/ardupilot_sitl/CMakeLists.txt index 9d40ea746d43b6..8992b6c758045b 100644 --- a/Tools/ros2/ardupilot_sitl/CMakeLists.txt +++ b/Tools/ros2/ardupilot_sitl/CMakeLists.txt @@ -24,12 +24,12 @@ message(STATUS "WAF_COMMAND: ${WAF_COMMAND}") set(WAF_CONFIG $<$,$>:"--debug">) add_custom_target(ardupilot_configure ALL - ${WAF_COMMAND} configure --board sitl --enable-dds ${WAF_CONFIG} + ${WAF_COMMAND} configure --board sitl --enable-uros ${WAF_CONFIG} WORKING_DIRECTORY ${ARDUPILOT_ROOT} ) add_custom_target(ardupilot_build ALL - ${WAF_COMMAND} build --enable-dds ${WAF_CONFIG} + ${WAF_COMMAND} build --enable-uros ${WAF_CONFIG} WORKING_DIRECTORY ${ARDUPILOT_ROOT} ) add_dependencies(ardupilot_build ardupilot_configure) diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index d5315e829705e1..86cef43b115369 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -12,15 +12,19 @@ #include #include #include + +#include + #if AP_EXTERNAL_CONTROL_ENABLED #include "AP_DDS_ExternalControl.h" #endif -#include "AP_DDS_Frames.h" +// #include "AP_DDS_Frames.h" #include "AP_DDS_Client.h" #include "AP_DDS_Topic_Table.h" #include "AP_DDS_Service_Table.h" #include "AP_DDS_External_Odom.h" +#include "AP_DDS_Type_Conversions.h" // Enable DDS at runtime by default static constexpr uint8_t ENABLED_BY_DEFAULT = 1; @@ -65,335 +69,42 @@ const AP_Param::GroupInfo AP_DDS_Client::var_info[] { void AP_DDS_Client::update_topic(builtin_interfaces_msg_Time& msg) { - uint64_t utc_usec; - if (!AP::rtc().get_utc_usec(utc_usec)) { - utc_usec = AP_HAL::micros64(); - } - msg.sec = utc_usec / 1000000ULL; - msg.nanosec = (utc_usec % 1000000ULL) * 1000UL; - + AP_ROS_Client::update_time(msg); } bool AP_DDS_Client::update_topic(sensor_msgs_msg_NavSatFix& msg, const uint8_t instance) { - // Add a lambda that takes in navsatfix msg and populates the cov - // Make it constexpr if possible - // https://www.fluentcpp.com/2021/12/13/the-evolutions-of-lambdas-in-c14-c17-and-c20/ - // constexpr auto times2 = [] (sensor_msgs_msg_NavSatFix* msg) { return n * 2; }; - - // assert(instance >= GPS_MAX_RECEIVERS); - if (instance >= GPS_MAX_RECEIVERS) { - return false; - } - - auto &gps = AP::gps(); - WITH_SEMAPHORE(gps.get_semaphore()); - - if (!gps.is_healthy(instance)) { - msg.status.status = -1; // STATUS_NO_FIX - msg.status.service = 0; // No services supported - msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN - return false; - } - - // No update is needed - const auto last_fix_time_ms = gps.last_fix_time_ms(instance); - if (last_nav_sat_fix_time_ms == last_fix_time_ms) { - return false; - } else { - last_nav_sat_fix_time_ms = last_fix_time_ms; - } - - - update_topic(msg.header.stamp); - strcpy(msg.header.frame_id, WGS_84_FRAME_ID); - msg.status.service = 0; // SERVICE_GPS - msg.status.status = -1; // STATUS_NO_FIX - - - //! @todo What about glonass, compass, galileo? - //! This will be properly designed and implemented to spec in #23277 - msg.status.service = 1; // SERVICE_GPS - - const auto status = gps.status(instance); - switch (status) { - case AP_GPS::NO_GPS: - case AP_GPS::NO_FIX: - msg.status.status = -1; // STATUS_NO_FIX - msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN - return true; - case AP_GPS::GPS_OK_FIX_2D: - case AP_GPS::GPS_OK_FIX_3D: - msg.status.status = 0; // STATUS_FIX - break; - case AP_GPS::GPS_OK_FIX_3D_DGPS: - msg.status.status = 1; // STATUS_SBAS_FIX - break; - case AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT: - case AP_GPS::GPS_OK_FIX_3D_RTK_FIXED: - msg.status.status = 2; // STATUS_SBAS_FIX - break; - default: - //! @todo Can we not just use an enum class and not worry about this condition? - break; - } - const auto loc = gps.location(instance); - msg.latitude = loc.lat * 1E-7; - msg.longitude = loc.lng * 1E-7; - - int32_t alt_cm; - if (!loc.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_cm)) { - // With absolute frame, this condition is unlikely - msg.status.status = -1; // STATUS_NO_FIX - msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN - return true; - } - msg.altitude = alt_cm * 0.01; - - // ROS allows double precision, ArduPilot exposes float precision today - Matrix3f cov; - msg.position_covariance_type = (uint8_t)gps.position_covariance(instance, cov); - msg.position_covariance[0] = cov[0][0]; - msg.position_covariance[1] = cov[0][1]; - msg.position_covariance[2] = cov[0][2]; - msg.position_covariance[3] = cov[1][0]; - msg.position_covariance[4] = cov[1][1]; - msg.position_covariance[5] = cov[1][2]; - msg.position_covariance[6] = cov[2][0]; - msg.position_covariance[7] = cov[2][1]; - msg.position_covariance[8] = cov[2][2]; - - return true; + return AP_ROS_Client::update_nav_sat_fix(msg, instance, last_nav_sat_fix_time_ms); } void AP_DDS_Client::populate_static_transforms(tf2_msgs_msg_TFMessage& msg) { - msg.transforms_size = 0; - - auto &gps = AP::gps(); - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { - const auto gps_type = gps.get_type(i); - if (gps_type == AP_GPS::GPS_Type::GPS_TYPE_NONE) { - continue; - } - update_topic(msg.transforms[i].header.stamp); - char gps_frame_id[16]; - //! @todo should GPS frame ID's be 0 or 1 indexed in ROS? - hal.util->snprintf(gps_frame_id, sizeof(gps_frame_id), "GPS_%u", i); - strcpy(msg.transforms[i].header.frame_id, BASE_LINK_FRAME_ID); - strcpy(msg.transforms[i].child_frame_id, gps_frame_id); - // The body-frame offsets - // X - Forward - // Y - Right - // Z - Down - // https://ardupilot.org/copter/docs/common-sensor-offset-compensation.html#sensor-position-offset-compensation - - const auto offset = gps.get_antenna_offset(i); - - // In ROS REP 103, it follows this convention - // X - Forward - // Y - Left - // Z - Up - // https://www.ros.org/reps/rep-0103.html#axis-orientation - - msg.transforms[i].transform.translation.x = offset[0]; - msg.transforms[i].transform.translation.y = -1 * offset[1]; - msg.transforms[i].transform.translation.z = -1 * offset[2]; - - msg.transforms_size++; - } - + AP_ROS_Client::update_static_transforms(msg); } void AP_DDS_Client::update_topic(sensor_msgs_msg_BatteryState& msg, const uint8_t instance) { - if (instance >= AP_BATT_MONITOR_MAX_INSTANCES) { - return; - } - - update_topic(msg.header.stamp); - auto &battery = AP::battery(); - - if (!battery.healthy(instance)) { - msg.power_supply_status = 3; //POWER_SUPPLY_HEALTH_DEAD - msg.present = false; - return; - } - msg.present = true; - - msg.voltage = battery.voltage(instance); - - float temperature; - msg.temperature = (battery.get_temperature(temperature, instance)) ? temperature : NAN; - - float current; - msg.current = (battery.current_amps(current, instance)) ? -1 * current : NAN; - - const float design_capacity = (float)battery.pack_capacity_mah(instance) * 0.001; - msg.design_capacity = design_capacity; - - uint8_t percentage; - if (battery.capacity_remaining_pct(percentage, instance)) { - msg.percentage = percentage * 0.01; - msg.charge = (percentage * design_capacity) * 0.01; - } else { - msg.percentage = NAN; - msg.charge = NAN; - } - - msg.capacity = NAN; - - if (battery.current_amps(current, instance)) { - if (percentage == 100) { - msg.power_supply_status = 4; //POWER_SUPPLY_STATUS_FULL - } else if (current < 0.0) { - msg.power_supply_status = 1; //POWER_SUPPLY_STATUS_CHARGING - } else if (current > 0.0) { - msg.power_supply_status = 2; //POWER_SUPPLY_STATUS_DISCHARGING - } else { - msg.power_supply_status = 3; //POWER_SUPPLY_STATUS_NOT_CHARGING - } - } else { - msg.power_supply_status = 0; //POWER_SUPPLY_STATUS_UNKNOWN - } - - msg.power_supply_health = (battery.overpower_detected(instance)) ? 4 : 1; //POWER_SUPPLY_HEALTH_OVERVOLTAGE or POWER_SUPPLY_HEALTH_GOOD - - msg.power_supply_technology = 0; //POWER_SUPPLY_TECHNOLOGY_UNKNOWN - - if (battery.has_cell_voltages(instance)) { - const uint16_t* cellVoltages = battery.get_cell_voltages(instance).cells; - std::copy(cellVoltages, cellVoltages + AP_BATT_MONITOR_CELLS_MAX, msg.cell_voltage); - } + AP_ROS_Client::update_battery_state(msg, instance); } void AP_DDS_Client::update_topic(geometry_msgs_msg_PoseStamped& msg) { - update_topic(msg.header.stamp); - strcpy(msg.header.frame_id, BASE_LINK_FRAME_ID); - - auto &ahrs = AP::ahrs(); - WITH_SEMAPHORE(ahrs.get_semaphore()); - - // ROS REP 103 uses the ENU convention: - // X - East - // Y - North - // Z - Up - // https://www.ros.org/reps/rep-0103.html#axis-orientation - // AP_AHRS uses the NED convention - // X - North - // Y - East - // Z - Down - // As a consequence, to follow ROS REP 103, it is necessary to switch X and Y, - // as well as invert Z - - Vector3f position; - if (ahrs.get_relative_position_NED_home(position)) { - msg.pose.position.x = position[1]; - msg.pose.position.y = position[0]; - msg.pose.position.z = -position[2]; - } - - // In ROS REP 103, axis orientation uses the following convention: - // X - Forward - // Y - Left - // Z - Up - // https://www.ros.org/reps/rep-0103.html#axis-orientation - // As a consequence, to follow ROS REP 103, it is necessary to switch X and Y, - // as well as invert Z (NED to ENU convertion) as well as a 90 degree rotation in the Z axis - // for x to point forward - Quaternion orientation; - if (ahrs.get_quaternion(orientation)) { - Quaternion aux(orientation[0], orientation[2], orientation[1], -orientation[3]); //NED to ENU transformation - Quaternion transformation (sqrtF(2) * 0.5,0,0,sqrtF(2) * 0.5); // Z axis 90 degree rotation - orientation = aux * transformation; - msg.pose.orientation.w = orientation[0]; - msg.pose.orientation.x = orientation[1]; - msg.pose.orientation.y = orientation[2]; - msg.pose.orientation.z = orientation[3]; - } + AP_ROS_Client::update_pose_stamped(msg); } void AP_DDS_Client::update_topic(geometry_msgs_msg_TwistStamped& msg) { - update_topic(msg.header.stamp); - strcpy(msg.header.frame_id, BASE_LINK_FRAME_ID); - - auto &ahrs = AP::ahrs(); - WITH_SEMAPHORE(ahrs.get_semaphore()); - - // ROS REP 103 uses the ENU convention: - // X - East - // Y - North - // Z - Up - // https://www.ros.org/reps/rep-0103.html#axis-orientation - // AP_AHRS uses the NED convention - // X - North - // Y - East - // Z - Down - // As a consequence, to follow ROS REP 103, it is necessary to switch X and Y, - // as well as invert Z - Vector3f velocity; - if (ahrs.get_velocity_NED(velocity)) { - msg.twist.linear.x = velocity[1]; - msg.twist.linear.y = velocity[0]; - msg.twist.linear.z = -velocity[2]; - } - - // In ROS REP 103, axis orientation uses the following convention: - // X - Forward - // Y - Left - // Z - Up - // https://www.ros.org/reps/rep-0103.html#axis-orientation - // The gyro data is received from AP_AHRS in body-frame - // X - Forward - // Y - Right - // Z - Down - // As a consequence, to follow ROS REP 103, it is necessary to invert Y and Z - Vector3f angular_velocity = ahrs.get_gyro(); - msg.twist.angular.x = angular_velocity[0]; - msg.twist.angular.y = -angular_velocity[1]; - msg.twist.angular.z = -angular_velocity[2]; + AP_ROS_Client::update_twist_stamped(msg); } void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPoseStamped& msg) { - update_topic(msg.header.stamp); - strcpy(msg.header.frame_id, BASE_LINK_FRAME_ID); - - auto &ahrs = AP::ahrs(); - WITH_SEMAPHORE(ahrs.get_semaphore()); - - Location loc; - if (ahrs.get_location(loc)) { - msg.pose.position.latitude = loc.lat * 1E-7; - msg.pose.position.longitude = loc.lng * 1E-7; - msg.pose.position.altitude = loc.alt * 0.01; // Transform from cm to m - } - - // In ROS REP 103, axis orientation uses the following convention: - // X - Forward - // Y - Left - // Z - Up - // https://www.ros.org/reps/rep-0103.html#axis-orientation - // As a consequence, to follow ROS REP 103, it is necessary to switch X and Y, - // as well as invert Z (NED to ENU convertion) as well as a 90 degree rotation in the Z axis - // for x to point forward - Quaternion orientation; - if (ahrs.get_quaternion(orientation)) { - Quaternion aux(orientation[0], orientation[2], orientation[1], -orientation[3]); //NED to ENU transformation - Quaternion transformation(sqrtF(2) * 0.5, 0, 0, sqrtF(2) * 0.5); // Z axis 90 degree rotation - orientation = aux * transformation; - msg.pose.orientation.w = orientation[0]; - msg.pose.orientation.x = orientation[1]; - msg.pose.orientation.y = orientation[2]; - msg.pose.orientation.z = orientation[3]; - } + AP_ROS_Client::update_geopose_stamped(msg); } void AP_DDS_Client::update_topic(rosgraph_msgs_msg_Clock& msg) { - update_topic(msg.clock); + AP_ROS_Client::update_clock(msg); } /* @@ -839,6 +550,7 @@ void AP_DDS_Client::write_battery_state_topic() } } } + void AP_DDS_Client::write_local_pose_topic() { WITH_SEMAPHORE(csem); diff --git a/libraries/AP_DDS/AP_DDS_ExternalControl.cpp b/libraries/AP_DDS/AP_DDS_ExternalControl.cpp index a72536d6ffde84..f8fbf7a3e6fc27 100644 --- a/libraries/AP_DDS/AP_DDS_ExternalControl.cpp +++ b/libraries/AP_DDS/AP_DDS_ExternalControl.cpp @@ -1,44 +1,13 @@ #if AP_DDS_ENABLED #include "AP_DDS_ExternalControl.h" -#include "AP_DDS_Frames.h" #include - #include +#include bool AP_DDS_External_Control::handle_velocity_control(geometry_msgs_msg_TwistStamped& cmd_vel) { - auto *external_control = AP::externalcontrol(); - if (external_control == nullptr) { - return false; - } - - if (strcmp(cmd_vel.header.frame_id, BASE_LINK_FRAME_ID) == 0) { - // Convert commands from body frame (x-forward, y-left, z-up) to NED. - Vector3f linear_velocity; - Vector3f linear_velocity_base_link { - float(cmd_vel.twist.linear.x), - float(cmd_vel.twist.linear.y), - float(-cmd_vel.twist.linear.z) }; - const float yaw_rate = -cmd_vel.twist.angular.z; - - auto &ahrs = AP::ahrs(); - linear_velocity = ahrs.body_to_earth(linear_velocity_base_link); - return external_control->set_linear_velocity_and_yaw_rate(linear_velocity, yaw_rate); - } - - else if (strcmp(cmd_vel.header.frame_id, MAP_FRAME) == 0) { - // Convert commands from ENU to NED frame - Vector3f linear_velocity { - float(cmd_vel.twist.linear.y), - float(cmd_vel.twist.linear.x), - float(-cmd_vel.twist.linear.z) }; - const float yaw_rate = -cmd_vel.twist.angular.z; - return external_control->set_linear_velocity_and_yaw_rate(linear_velocity, yaw_rate); - } - - return false; + return AP_ROS_External_Control::handle_velocity_control(cmd_vel); } - #endif // AP_DDS_ENABLED \ No newline at end of file diff --git a/libraries/AP_DDS/AP_DDS_External_Odom.cpp b/libraries/AP_DDS/AP_DDS_External_Odom.cpp index cf121f408521a6..9694d2add4f1f7 100644 --- a/libraries/AP_DDS/AP_DDS_External_Odom.cpp +++ b/libraries/AP_DDS/AP_DDS_External_Odom.cpp @@ -1,76 +1,23 @@ - - #include "AP_DDS_External_Odom.h" #include "AP_DDS_Type_Conversions.h" #if AP_DDS_VISUALODOM_ENABLED -#include -#include +#include void AP_DDS_External_Odom::handle_external_odom(const tf2_msgs_msg_TFMessage& msg) { - auto *visual_odom = AP::visualodom(); - if (visual_odom == nullptr) { - return; - } - - for (size_t i = 0; i < msg.transforms_size; i++) { - const auto& ros_transform_stamped = msg.transforms[i]; - if (!is_odometry_frame(ros_transform_stamped)) { - continue; - } - const uint64_t remote_time_us {AP_DDS_Type_Conversions::time_u64_micros(ros_transform_stamped.header.stamp)}; - - Vector3f ap_position; - Quaternion ap_rotation; - - convert_transform(ros_transform_stamped.transform, ap_position, ap_rotation); - // Although ROS convention states quaternions in ROS messages should be normalized, it's not guaranteed. - // Before propogating a potentially inaccurate quaternion to the rest of AP, normalize it here. - // TODO what if the quaternion is NaN? - ap_rotation.normalize(); - - // No error is available in TF, trust the data as-is - const float posErr {0.0}; - const float angErr {0.0}; - // The odom to base_link transform used is locally consistent per ROS REP-105. - // https://www.ros.org/reps/rep-0105.html#id16 - // Thus, there will not be any resets. - const uint8_t reset_counter {0}; - // TODO imlement jitter correction similar to GCS_MAVLINK::correct_offboard_timestamp_usec_to_ms(remote_time_us, sizeof(msg)); - const uint32_t time_ms {static_cast(remote_time_us * 1E-3)}; - visual_odom->handle_pose_estimate(remote_time_us, time_ms, ap_position.x, ap_position.y, ap_position.z, ap_rotation, posErr, angErr, reset_counter); - - } + AP_ROS_External_Odom::handle_external_odom(msg); } bool AP_DDS_External_Odom::is_odometry_frame(const geometry_msgs_msg_TransformStamped& msg) { - char odom_parent[] = "odom"; - char odom_child[] = "base_link"; - // Assume the frame ID's are null terminated. - return (strcmp(msg.header.frame_id, odom_parent) == 0) && - (strcmp(msg.child_frame_id, odom_child) == 0); + return AP_ROS_External_Odom::is_odometry_frame(msg); } void AP_DDS_External_Odom::convert_transform(const geometry_msgs_msg_Transform& ros_transform, Vector3f& translation, Quaternion& rotation) { - // convert from x-forward, y-left, z-up to NED - // https://github.com/mavlink/mavros/issues/49#issuecomment-51614130 - translation = { - static_cast(ros_transform.translation.x), - static_cast(-ros_transform.translation.y), - static_cast(-ros_transform.translation.z) - }; - - // In AP, q1 is the quaternion's scalar component. - // In ROS, w is the quaternion's scalar component. - // https://docs.ros.org/en/humble/Tutorials/Intermediate/Tf2/Quaternion-Fundamentals.html#components-of-a-quaternion - rotation.q1 = ros_transform.rotation.w; - rotation.q2 = ros_transform.rotation.x; - rotation.q3 = -ros_transform.rotation.y; - rotation.q4 = -ros_transform.rotation.z; + AP_ROS_External_Odom::convert_transform(ros_transform, translation, rotation); } #endif // AP_DDS_VISUALODOM_ENABLED diff --git a/libraries/AP_DDS/AP_DDS_Type_Conversions.cpp b/libraries/AP_DDS/AP_DDS_Type_Conversions.cpp index 2aa4caf543f89b..7508acec1389ac 100644 --- a/libraries/AP_DDS/AP_DDS_Type_Conversions.cpp +++ b/libraries/AP_DDS/AP_DDS_Type_Conversions.cpp @@ -1,13 +1,52 @@ #include "AP_DDS_Type_Conversions.h" #if AP_DDS_ENABLED -#include "builtin_interfaces/msg/Time.h" - - uint64_t AP_DDS_Type_Conversions::time_u64_micros(const builtin_interfaces_msg_Time& ros_time) { - return (uint64_t(ros_time.sec) * 1000000ULL) + (ros_time.nanosec / 1000ULL); + return AP_ROS_TypeConversions::time_u64_micros(ros_time); +} + +// string specialisations +template <> +const char* string_data(const char* str) { + return str; +} + +template <> +char* mutable_string_data(char* str) { + return str; +} + +// transform specialisations +template <> +typename transforms_size_type::type +transforms_size(const tf2_msgs_msg_TFMessage& msg) { + return msg.transforms_size; } +template <> +typename mutable_transforms_size_type::type +mutable_transforms_size(tf2_msgs_msg_TFMessage& msg) { + return msg.transforms_size; +} + +template <> +typename transforms_type::type +transforms_data(const tf2_msgs_msg_TFMessage& msg) { + return msg.transforms; +} + +template <> +typename mutable_transforms_type::type +mutable_transforms_data(tf2_msgs_msg_TFMessage& msg) { + return msg.transforms; +} + +// cell_voltage specialisations +template <> +typename mutable_cell_voltage_type::type +mutable_cell_voltage_data(sensor_msgs_msg_BatteryState& msg) { + return msg.cell_voltage; +} #endif // AP_DDS_ENABLED diff --git a/libraries/AP_DDS/AP_DDS_Type_Conversions.h b/libraries/AP_DDS/AP_DDS_Type_Conversions.h index 71b2e3182de299..096821b0c51cf5 100644 --- a/libraries/AP_DDS/AP_DDS_Type_Conversions.h +++ b/libraries/AP_DDS/AP_DDS_Type_Conversions.h @@ -4,7 +4,12 @@ #if AP_DDS_ENABLED +#include + #include "builtin_interfaces/msg/Time.h" +#include "geometry_msgs/msg/TransformStamped.h" +#include "sensor_msgs/msg/BatteryState.h" +#include "tf2_msgs/msg/TFMessage.h" class AP_DDS_Type_Conversions { @@ -14,4 +19,58 @@ class AP_DDS_Type_Conversions static uint64_t time_u64_micros(const builtin_interfaces_msg_Time& ros_time); }; +// string specialisations +template <> +const char* string_data(const char* str); + +template <> +char* mutable_string_data(char* str); + +// transform specialisations +template <> +struct transforms_size_type { + typedef uint32_t type; +}; + +template <> +struct mutable_transforms_size_type { + typedef uint32_t& type; +}; + +template <> +typename transforms_size_type::type +transforms_size(const tf2_msgs_msg_TFMessage& msg); + +template <> +typename mutable_transforms_size_type::type +mutable_transforms_size(tf2_msgs_msg_TFMessage& msg); + +template <> +struct transforms_type { + typedef const geometry_msgs_msg_TransformStamped* type; +}; + +template <> +struct mutable_transforms_type { + typedef geometry_msgs_msg_TransformStamped* type; +}; + +template <> +typename transforms_type::type +transforms_data(const tf2_msgs_msg_TFMessage& msg); + +template <> +typename mutable_transforms_type::type +mutable_transforms_data(tf2_msgs_msg_TFMessage& msg); + +// cell_voltage specialisations +template <> +struct mutable_cell_voltage_type { + typedef float* type; +}; + +template <> +typename mutable_cell_voltage_type::type +mutable_cell_voltage_data(sensor_msgs_msg_BatteryState& msg); + #endif // AP_DDS_ENABLED diff --git a/libraries/AP_ROS/AP_ROS_Client.h b/libraries/AP_ROS/AP_ROS_Client.h new file mode 100644 index 00000000000000..192eea9d2b3dff --- /dev/null +++ b/libraries/AP_ROS/AP_ROS_Client.h @@ -0,0 +1,393 @@ +#pragma once + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include "AP_ROS_Frames.h" +#include "AP_ROS_TypeConversions.h" + + +class AP_ROS_Client +{ +public: + + template + static void update_battery_state(BatteryState& msg, + const uint8_t instance); + + template + static void update_clock(Clock& msg); + + template + static void update_geopose_stamped(GeoPoseStamped& msg); + + template + static void update_pose_stamped(PoseStamped& msg); + + template + static void update_twist_stamped(TwistStamped& msg); + + template + static void update_static_transforms(TFMessage& msg); + + template + static bool update_nav_sat_fix(NavSatFix& msg, + const uint8_t instance, uint64_t& last_nav_sat_fix_time_ms); + + template + static void update_time(Time& msg); + +}; + +template +void AP_ROS_Client::update_battery_state(BatteryState& msg, + const uint8_t instance) +{ + if (instance >= AP_BATT_MONITOR_MAX_INSTANCES) { + return; + } + + update_time(msg.header.stamp); + auto &battery = AP::battery(); + + if (!battery.healthy(instance)) { + msg.power_supply_status = 3; //POWER_SUPPLY_HEALTH_DEAD + msg.present = false; + return; + } + msg.present = true; + + msg.voltage = battery.voltage(instance); + + float temperature; + msg.temperature = (battery.get_temperature(temperature, instance)) ? temperature : NAN; + + float current; + msg.current = (battery.current_amps(current, instance)) ? -1 * current : NAN; + + const float design_capacity = (float)battery.pack_capacity_mah(instance) * 0.001; + msg.design_capacity = design_capacity; + + uint8_t percentage; + if (battery.capacity_remaining_pct(percentage, instance)) { + msg.percentage = percentage * 0.01; + msg.charge = (percentage * design_capacity) * 0.01; + } else { + msg.percentage = NAN; + msg.charge = NAN; + } + + msg.capacity = NAN; + + if (battery.current_amps(current, instance)) { + if (percentage == 100) { + msg.power_supply_status = 4; //POWER_SUPPLY_STATUS_FULL + } else if (current < 0.0) { + msg.power_supply_status = 1; //POWER_SUPPLY_STATUS_CHARGING + } else if (current > 0.0) { + msg.power_supply_status = 2; //POWER_SUPPLY_STATUS_DISCHARGING + } else { + msg.power_supply_status = 3; //POWER_SUPPLY_STATUS_NOT_CHARGING + } + } else { + msg.power_supply_status = 0; //POWER_SUPPLY_STATUS_UNKNOWN + } + + msg.power_supply_health = (battery.overpower_detected(instance)) ? 4 : 1; //POWER_SUPPLY_HEALTH_OVERVOLTAGE or POWER_SUPPLY_HEALTH_GOOD + + msg.power_supply_technology = 0; //POWER_SUPPLY_TECHNOLOGY_UNKNOWN + + if (battery.has_cell_voltages(instance)) { + const uint16_t* cellVoltages = battery.get_cell_voltages(instance).cells; + std::copy(cellVoltages, cellVoltages + AP_BATT_MONITOR_CELLS_MAX, + mutable_cell_voltage_data(msg)); + } +} + +template +void AP_ROS_Client::update_clock(Clock& msg) +{ + update_time(msg.clock); +} + +template +void AP_ROS_Client::update_geopose_stamped(GeoPoseStamped& msg) +{ + update_time(msg.header.stamp); + strcpy(mutable_string_data(msg.header.frame_id), BASE_LINK_FRAME_ID); + + auto &ahrs = AP::ahrs(); + WITH_SEMAPHORE(ahrs.get_semaphore()); + + Location loc; + if (ahrs.get_location(loc)) { + msg.pose.position.latitude = loc.lat * 1E-7; + msg.pose.position.longitude = loc.lng * 1E-7; + msg.pose.position.altitude = loc.alt * 0.01; // Transform from cm to m + } + + // In ROS REP 103, axis orientation uses the following convention: + // X - Forward + // Y - Left + // Z - Up + // https://www.ros.org/reps/rep-0103.html#axis-orientation + // As a consequence, to follow ROS REP 103, it is necessary to switch X and Y, + // as well as invert Z (NED to ENU convertion) as well as a 90 degree rotation in the Z axis + // for x to point forward + Quaternion orientation; + if (ahrs.get_quaternion(orientation)) { + Quaternion aux(orientation[0], orientation[2], orientation[1], -orientation[3]); //NED to ENU transformation + Quaternion transformation(sqrtF(2) * 0.5, 0, 0, sqrtF(2) * 0.5); // Z axis 90 degree rotation + orientation = aux * transformation; + msg.pose.orientation.w = orientation[0]; + msg.pose.orientation.x = orientation[1]; + msg.pose.orientation.y = orientation[2]; + msg.pose.orientation.z = orientation[3]; + } +} + +template +void AP_ROS_Client::update_pose_stamped(PoseStamped& msg) +{ + update_time(msg.header.stamp); + strcpy(mutable_string_data(msg.header.frame_id), BASE_LINK_FRAME_ID); + + auto &ahrs = AP::ahrs(); + WITH_SEMAPHORE(ahrs.get_semaphore()); + + // ROS REP 103 uses the ENU convention: + // X - East + // Y - North + // Z - Up + // https://www.ros.org/reps/rep-0103.html#axis-orientation + // AP_AHRS uses the NED convention + // X - North + // Y - East + // Z - Down + // As a consequence, to follow ROS REP 103, it is necessary to switch X and Y, + // as well as invert Z + + Vector3f position; + if (ahrs.get_relative_position_NED_home(position)) { + msg.pose.position.x = position[1]; + msg.pose.position.y = position[0]; + msg.pose.position.z = -position[2]; + } + + // In ROS REP 103, axis orientation uses the following convention: + // X - Forward + // Y - Left + // Z - Up + // https://www.ros.org/reps/rep-0103.html#axis-orientation + // As a consequence, to follow ROS REP 103, it is necessary to switch X and Y, + // as well as invert Z (NED to ENU convertion) as well as a 90 degree rotation in the Z axis + // for x to point forward + Quaternion orientation; + if (ahrs.get_quaternion(orientation)) { + Quaternion aux(orientation[0], orientation[2], orientation[1], -orientation[3]); //NED to ENU transformation + Quaternion transformation (sqrtF(2) * 0.5,0,0,sqrtF(2) * 0.5); // Z axis 90 degree rotation + orientation = aux * transformation; + msg.pose.orientation.w = orientation[0]; + msg.pose.orientation.x = orientation[1]; + msg.pose.orientation.y = orientation[2]; + msg.pose.orientation.z = orientation[3]; + } +} + +template +void AP_ROS_Client::update_twist_stamped(TwistStamped& msg) +{ + update_time(msg.header.stamp); + strcpy(mutable_string_data(msg.header.frame_id), BASE_LINK_FRAME_ID); + + auto &ahrs = AP::ahrs(); + WITH_SEMAPHORE(ahrs.get_semaphore()); + + // ROS REP 103 uses the ENU convention: + // X - East + // Y - North + // Z - Up + // https://www.ros.org/reps/rep-0103.html#axis-orientation + // AP_AHRS uses the NED convention + // X - North + // Y - East + // Z - Down + // As a consequence, to follow ROS REP 103, it is necessary to switch X and Y, + // as well as invert Z + Vector3f velocity; + if (ahrs.get_velocity_NED(velocity)) { + msg.twist.linear.x = velocity[1]; + msg.twist.linear.y = velocity[0]; + msg.twist.linear.z = -velocity[2]; + } + + // In ROS REP 103, axis orientation uses the following convention: + // X - Forward + // Y - Left + // Z - Up + // https://www.ros.org/reps/rep-0103.html#axis-orientation + // The gyro data is received from AP_AHRS in body-frame + // X - Forward + // Y - Right + // Z - Down + // As a consequence, to follow ROS REP 103, it is necessary to invert Y and Z + Vector3f angular_velocity = ahrs.get_gyro(); + msg.twist.angular.x = angular_velocity[0]; + msg.twist.angular.y = -angular_velocity[1]; + msg.twist.angular.z = -angular_velocity[2]; +} + +template +void AP_ROS_Client::update_static_transforms(TFMessage& msg) +{ + mutable_transforms_size(msg) = 0; + + auto &gps = AP::gps(); + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + const auto gps_type = gps.get_type(i); + if (gps_type == AP_GPS::GPS_Type::GPS_TYPE_NONE) { + continue; + } + auto* transforms = mutable_transforms_data(msg); + update_time(transforms[i].header.stamp); + char gps_frame_id[16]; + //! @todo should GPS frame ID's be 0 or 1 indexed in ROS? + hal.util->snprintf(gps_frame_id, sizeof(gps_frame_id), "GPS_%u", i); + strcpy(mutable_string_data(transforms[i].header.frame_id), BASE_LINK_FRAME_ID); + strcpy(mutable_string_data(transforms[i].child_frame_id), gps_frame_id); + // The body-frame offsets + // X - Forward + // Y - Right + // Z - Down + // https://ardupilot.org/copter/docs/common-sensor-offset-compensation.html#sensor-position-offset-compensation + + const auto offset = gps.get_antenna_offset(i); + + // In ROS REP 103, it follows this convention + // X - Forward + // Y - Left + // Z - Up + // https://www.ros.org/reps/rep-0103.html#axis-orientation + + transforms[i].transform.translation.x = offset[0]; + transforms[i].transform.translation.y = -1 * offset[1]; + transforms[i].transform.translation.z = -1 * offset[2]; + + mutable_transforms_size(msg)++; + } +} + +template +bool AP_ROS_Client::update_nav_sat_fix(NavSatFix& msg, + const uint8_t instance, uint64_t& last_nav_sat_fix_time_ms) +{ + // Add a lambda that takes in navsatfix msg and populates the cov + // Make it constexpr if possible + // https://www.fluentcpp.com/2021/12/13/the-evolutions-of-lambdas-in-c14-c17-and-c20/ + // constexpr auto times2 = [] (sensor_msgs_msg_NavSatFix* msg) { return n * 2; }; + + // assert(instance >= GPS_MAX_RECEIVERS); + if (instance >= GPS_MAX_RECEIVERS) { + return false; + } + + auto &gps = AP::gps(); + WITH_SEMAPHORE(gps.get_semaphore()); + + if (!gps.is_healthy(instance)) { + msg.status.status = -1; // STATUS_NO_FIX + msg.status.service = 0; // No services supported + msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN + return false; + } + + // No update is needed + const auto last_fix_time_ms = gps.last_fix_time_ms(instance); + if (last_nav_sat_fix_time_ms == last_fix_time_ms) { + return false; + } else { + last_nav_sat_fix_time_ms = last_fix_time_ms; + } + + + update_time(msg.header.stamp); + strcpy(mutable_string_data(msg.header.frame_id), WGS_84_FRAME_ID); + msg.status.service = 0; // SERVICE_GPS + msg.status.status = -1; // STATUS_NO_FIX + + + //! @todo What about glonass, compass, galileo? + //! This will be properly designed and implemented to spec in #23277 + msg.status.service = 1; // SERVICE_GPS + + const auto status = gps.status(instance); + switch (status) { + case AP_GPS::NO_GPS: + case AP_GPS::NO_FIX: + msg.status.status = -1; // STATUS_NO_FIX + msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN + return true; + case AP_GPS::GPS_OK_FIX_2D: + case AP_GPS::GPS_OK_FIX_3D: + msg.status.status = 0; // STATUS_FIX + break; + case AP_GPS::GPS_OK_FIX_3D_DGPS: + msg.status.status = 1; // STATUS_SBAS_FIX + break; + case AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT: + case AP_GPS::GPS_OK_FIX_3D_RTK_FIXED: + msg.status.status = 2; // STATUS_SBAS_FIX + break; + default: + //! @todo Can we not just use an enum class and not worry about this condition? + break; + } + const auto loc = gps.location(instance); + msg.latitude = loc.lat * 1E-7; + msg.longitude = loc.lng * 1E-7; + + int32_t alt_cm; + if (!loc.get_alt_cm(Location::AltFrame::ABSOLUTE, alt_cm)) { + // With absolute frame, this condition is unlikely + msg.status.status = -1; // STATUS_NO_FIX + msg.position_covariance_type = 0; // COVARIANCE_TYPE_UNKNOWN + return true; + } + msg.altitude = alt_cm * 0.01; + + // ROS allows double precision, ArduPilot exposes float precision today + Matrix3f cov; + msg.position_covariance_type = (uint8_t)gps.position_covariance(instance, cov); + msg.position_covariance[0] = cov[0][0]; + msg.position_covariance[1] = cov[0][1]; + msg.position_covariance[2] = cov[0][2]; + msg.position_covariance[3] = cov[1][0]; + msg.position_covariance[4] = cov[1][1]; + msg.position_covariance[5] = cov[1][2]; + msg.position_covariance[6] = cov[2][0]; + msg.position_covariance[7] = cov[2][1]; + msg.position_covariance[8] = cov[2][2]; + + return true; +} + +template +void AP_ROS_Client::update_time(Time& msg) +{ + uint64_t utc_usec; + if (!AP::rtc().get_utc_usec(utc_usec)) { + utc_usec = AP_HAL::micros64(); + } + msg.sec = utc_usec / 1000000ULL; + msg.nanosec = (utc_usec % 1000000ULL) * 1000UL; +} + diff --git a/libraries/AP_ROS/AP_ROS_ExternalControl.h b/libraries/AP_ROS/AP_ROS_ExternalControl.h new file mode 100644 index 00000000000000..d92deba5cd0f1a --- /dev/null +++ b/libraries/AP_ROS/AP_ROS_ExternalControl.h @@ -0,0 +1,49 @@ +#pragma once + +#include +#include + +#include "AP_ROS_Frames.h" +#include "AP_ROS_TypeConversions.h" + +class AP_ROS_External_Control +{ +public: + template + static bool handle_velocity_control(const TTwistStamped& cmd_vel); + +}; + +template +bool AP_ROS_External_Control::handle_velocity_control(const TTwistStamped& cmd_vel) +{ + auto *external_control = AP::externalcontrol(); + if (external_control == nullptr) { + return false; + } + + if (strcmp(string_data(cmd_vel.header.frame_id), BASE_LINK_FRAME_ID) == 0) { + // Convert commands from body frame (x-forward, y-left, z-up) to NED. + Vector3f linear_velocity; + Vector3f linear_velocity_base_link { + float(cmd_vel.twist.linear.x), + float(cmd_vel.twist.linear.y), + float(-cmd_vel.twist.linear.z) }; + const float yaw_rate = -cmd_vel.twist.angular.z; + + auto &ahrs = AP::ahrs(); + linear_velocity = ahrs.body_to_earth(linear_velocity_base_link); + return external_control->set_linear_velocity_and_yaw_rate(linear_velocity, yaw_rate); + } + + else if (strcmp(string_data(cmd_vel.header.frame_id), MAP_FRAME) == 0) { + // Convert commands from ENU to NED frame + Vector3f linear_velocity { + float(cmd_vel.twist.linear.y), + float(cmd_vel.twist.linear.x), + float(-cmd_vel.twist.linear.z) }; + const float yaw_rate = -cmd_vel.twist.angular.z; + return external_control->set_linear_velocity_and_yaw_rate(linear_velocity, yaw_rate); + } + return false; +} diff --git a/libraries/AP_ROS/AP_ROS_ExternalOdom.h b/libraries/AP_ROS/AP_ROS_ExternalOdom.h new file mode 100644 index 00000000000000..76d3b36bfe4f73 --- /dev/null +++ b/libraries/AP_ROS/AP_ROS_ExternalOdom.h @@ -0,0 +1,97 @@ +#pragma once + +#include +#include +#include +#include + +#include "AP_ROS_TypeConversions.h" + +class AP_ROS_External_Odom +{ +public: + // Handler for external position localization + template + static void handle_external_odom(const TFMessage& msg); + + // Checks the child and parent frames match a set needed for external odom. + // Since multiple different transforms can be sent, this validates the specific transform is + // for odometry. + template + static bool is_odometry_frame(const TransformStamped& msg); + + // Helper to convert from ROS transform to AP datatypes + // ros_transform is in ENU + // translation is in NED + template + static void convert_transform(const Transform& ros_transform, Vector3f& translation, Quaternion& rotation); + +}; + +template +void AP_ROS_External_Odom::handle_external_odom(const TFMessage& msg) +{ + auto *visual_odom = AP::visualodom(); + if (visual_odom == nullptr) { + return; + } + + for (size_t i = 0; i < transforms_size(msg); i++) { + const auto& ros_transform_stamped = transforms_data(msg)[i]; + if (!is_odometry_frame(ros_transform_stamped)) { + continue; + } + const uint64_t remote_time_us {AP_ROS_TypeConversions::time_u64_micros(ros_transform_stamped.header.stamp)}; + + Vector3f ap_position; + Quaternion ap_rotation; + + convert_transform(ros_transform_stamped.transform, ap_position, ap_rotation); + // Although ROS convention states quaternions in ROS messages should be normalized, it's not guaranteed. + // Before propogating a potentially inaccurate quaternion to the rest of AP, normalize it here. + // TODO what if the quaternion is NaN? + ap_rotation.normalize(); + + // No error is available in TF, trust the data as-is + const float posErr {0.0}; + const float angErr {0.0}; + // The odom to base_link transform used is locally consistent per ROS REP-105. + // https://www.ros.org/reps/rep-0105.html#id16 + // Thus, there will not be any resets. + const uint8_t reset_counter {0}; + // TODO imlement jitter correction similar to GCS_MAVLINK::correct_offboard_timestamp_usec_to_ms(remote_time_us, sizeof(msg)); + const uint32_t time_ms {static_cast(remote_time_us * 1E-3)}; + visual_odom->handle_pose_estimate(remote_time_us, time_ms, ap_position.x, ap_position.y, ap_position.z, ap_rotation, posErr, angErr, reset_counter); + + } +} + +template +bool AP_ROS_External_Odom::is_odometry_frame(const TransformStamped& msg) +{ + char odom_parent[] = "odom"; + char odom_child[] = "base_link"; + // Assume the frame ID's are null terminated. + return (strcmp(string_data(msg.header.frame_id), odom_parent) == 0) && + (strcmp(string_data(msg.child_frame_id), odom_child) == 0); +} + +template +void AP_ROS_External_Odom::convert_transform(const Transform& ros_transform, Vector3f& translation, Quaternion& rotation) +{ + // convert from x-forward, y-left, z-up to NED + // https://github.com/mavlink/mavros/issues/49#issuecomment-51614130 + translation = { + static_cast(ros_transform.translation.x), + static_cast(-ros_transform.translation.y), + static_cast(-ros_transform.translation.z) + }; + + // In AP, q1 is the quaternion's scalar component. + // In ROS, w is the quaternion's scalar component. + // https://docs.ros.org/en/humble/Tutorials/Intermediate/Tf2/Quaternion-Fundamentals.html#components-of-a-quaternion + rotation.q1 = ros_transform.rotation.w; + rotation.q2 = ros_transform.rotation.x; + rotation.q3 = -ros_transform.rotation.y; + rotation.q4 = -ros_transform.rotation.z; +} diff --git a/libraries/AP_ROS/AP_ROS_Frames.h b/libraries/AP_ROS/AP_ROS_Frames.h new file mode 100644 index 00000000000000..0bdaf354ec116f --- /dev/null +++ b/libraries/AP_ROS/AP_ROS_Frames.h @@ -0,0 +1,7 @@ +#pragma once + +static constexpr char WGS_84_FRAME_ID[] = "WGS-84"; +// https://www.ros.org/reps/rep-0105.html#base-link +static constexpr char BASE_LINK_FRAME_ID[] = "base_link"; +// https://www.ros.org/reps/rep-0105.html#map +static constexpr char MAP_FRAME[] = "map"; diff --git a/libraries/AP_ROS/AP_ROS_TypeConversions.h b/libraries/AP_ROS/AP_ROS_TypeConversions.h new file mode 100644 index 00000000000000..bab8c9a8d3d37b --- /dev/null +++ b/libraries/AP_ROS/AP_ROS_TypeConversions.h @@ -0,0 +1,68 @@ +// Class for handling type conversions for ROS. + +#pragma once + +#include + +class AP_ROS_TypeConversions +{ +public: + + // Convert ROS time to a uint64_t [μS] + template + static uint64_t time_u64_micros(const TTime& ros_time); +}; + +template +uint64_t AP_ROS_TypeConversions::time_u64_micros(const TTime& ros_time) +{ + return (uint64_t(ros_time.sec) * 1000000ULL) + (ros_time.nanosec / 1000ULL); +} + +// string accessor templates +template +const char* string_data(const S* str); + +template +char* mutable_string_data(S* str); + +template +const char* string_data(const S& str); + +template +char* mutable_string_data(S& str); + +// sequence accessor templates +// see: https://stackoverflow.com/questions/15911890/overriding-return-type-in-function-template-specialization + +template +struct transforms_size_type { typedef uint32_t type; }; + +template +struct mutable_transforms_size_type { typedef uint32_t& type; }; + +template +typename transforms_size_type::type transforms_size(const T& msg); + +template +typename mutable_transforms_size_type::type mutable_transforms_size(T& msg); + +template +struct transforms_type { typedef void* type; }; + +template +struct mutable_transforms_type { typedef void* type; }; + +template +typename transforms_type::type transforms_data(const T& msg); + +template +typename mutable_transforms_type::type mutable_transforms_data(T& msg); + +// battery state : cell_voltage + +template +struct mutable_cell_voltage_type { typedef void* type; }; + +template +typename mutable_cell_voltage_type::type mutable_cell_voltage_data(T& msg); diff --git a/libraries/AP_UROS/AP_UROS_Client.cpp b/libraries/AP_UROS/AP_UROS_Client.cpp new file mode 100644 index 00000000000000..82b5f875560bff --- /dev/null +++ b/libraries/AP_UROS/AP_UROS_Client.cpp @@ -0,0 +1,1087 @@ +#include + +#if AP_UROS_ENABLED + +#include "AP_UROS_Client.h" +#include "AP_UROS_Type_Conversions.h" + +#include +#include + +// micro-ROS +#include +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_ESP32 +#include +#endif + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#if AP_EXTERNAL_CONTROL_ENABLED +#include "AP_UROS_ExternalControl.h" +#endif +// #include "AP_UROS_Frames.h" +#include "AP_UROS_External_Odom.h" + +#define UROS_DEBUG 1 +#define UROS_INFO 1 +#define UROS_ERROR 1 + +#if UROS_DEBUG +#define uros_debug(fmt, args...) do {\ + hal.console->printf(fmt "\n", ##args);\ + GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, fmt, ##args);\ +} while(0) +#else +#define uros_debug(fmt, args...) do {} while(0) +#endif + +#if UROS_INFO +#define uros_info(fmt, args...) do {\ + hal.console->printf(fmt "\n", ##args);\ + GCS_SEND_TEXT(MAV_SEVERITY_INFO, fmt, ##args);\ +} while(0) +#else +#define uros_debug(fmt, args...) do {} while(0) +#endif + +#if UROS_ERROR +#define uros_error(fmt, args...) do {\ + hal.console->printf(fmt "\n", ##args);\ + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, fmt, ##args);\ +} while(0) +#else +#define uros_error(fmt, args...) do {} while(0) +#endif + +#define UROS_CHECK(fn, msg) {\ + rcl_ret_t temp_rc = fn;\ + if ((temp_rc != RCL_RET_OK)) {\ + uros_error("UROS: " msg "... FAILED, line %d: code: %d",\ + __LINE__, (int)temp_rc);\ + /* will be empty when RCUTILS_AVOID_DYNAMIC_ALLOCATION is defined */\ + const rcutils_error_state_t *err = rcutils_get_error_state();\ + uros_error("UROS: error: %s, file: %s, line: %u, ",\ + err->message, err->file, (uint32_t)err->line_number);\ + return false;\ + } else {\ + uros_debug("UROS: " msg"... OK");\ + }\ +} + +#define UROS_SOFTCHECK(fn) {\ + rcl_ret_t temp_rc = fn;\ + if ((temp_rc != RCL_RET_OK)) {\ + uros_info("UROS: failed status, line %d: code: %d",\ + __LINE__, (int)temp_rc);\ + }\ +} + +// publishers +constexpr uint16_t DELAY_BATTERY_STATE_TOPIC_MS = 1000; +constexpr uint16_t DELAY_CLOCK_TOPIC_MS = 10; +constexpr uint16_t DELAY_GEO_POSE_TOPIC_MS = 33; +constexpr uint16_t DELAY_LOCAL_POSE_TOPIC_MS = 33; +constexpr uint16_t DELAY_LOCAL_TWIST_TOPIC_MS = 33; +constexpr uint16_t DELAY_STATIC_TRANSFORM_TOPIC_MS = 1000; +constexpr uint16_t DELAY_TIME_TOPIC_MS = 10; + +// constructor +AP_UROS_Client::AP_UROS_Client() { + if (_singleton) { + uros_error("PANIC: Too many AP_UROS_Client instances"); + } + _singleton = this; +} + +// singleton +AP_UROS_Client *AP_UROS_Client::_singleton = nullptr; + +AP_UROS_Client *AP_UROS_Client::get_singleton() { + return AP_UROS_Client::_singleton; +} + +// update published topics + +void AP_UROS_Client::update_topic(sensor_msgs__msg__BatteryState& msg) +{ + const uint8_t instance = 0; + AP_ROS_Client::update_battery_state(msg, instance); +} + +void AP_UROS_Client::update_topic(rosgraph_msgs__msg__Clock& msg) +{ + AP_ROS_Client::update_clock(msg); +} + +void AP_UROS_Client::update_topic(geographic_msgs__msg__GeoPoseStamped& msg) +{ + AP_ROS_Client::update_geopose_stamped(msg); +} + +void AP_UROS_Client::update_topic(geometry_msgs__msg__PoseStamped& msg) +{ + AP_ROS_Client::update_pose_stamped(msg); +} + +void AP_UROS_Client::update_topic(geometry_msgs__msg__TwistStamped& msg) +{ + AP_ROS_Client::update_twist_stamped(msg); +} + +void AP_UROS_Client::update_topic(tf2_msgs__msg__TFMessage& msg) +{ + AP_ROS_Client::update_static_transforms(msg); +} + +bool AP_UROS_Client::update_topic(sensor_msgs__msg__NavSatFix& msg) +{ + const uint8_t instance = 0; + return AP_ROS_Client::update_nav_sat_fix(msg, instance, last_nav_sat_fix_time_ms); +} + +void AP_UROS_Client::update_topic(builtin_interfaces__msg__Time& msg) +{ + AP_ROS_Client::update_time(msg); +} + +// call publishers +void AP_UROS_Client::timer_callback_trampoline(rcl_timer_t * timer, int64_t last_call_time) +{ + AP_UROS_Client *uros = AP_UROS_Client::get_singleton(); + uros->timer_callback(timer, last_call_time); +} + +void AP_UROS_Client::timer_callback(rcl_timer_t * timer, int64_t last_call_time) +{ + (void) last_call_time; + + WITH_SEMAPHORE(csem); + + // get the timer clock + rcl_clock_t * clock; + UROS_SOFTCHECK(rcl_timer_clock(timer, &clock)); + + //! @todo(srmainwaring) - use the rcl_clock? + const auto cur_time_ms = AP_HAL::millis64(); + + if (timer != NULL) { + + if (battery_state_pub_init && + cur_time_ms - last_battery_state_time_ms > DELAY_BATTERY_STATE_TOPIC_MS) { + update_topic(battery_state_msg); + last_battery_state_time_ms = cur_time_ms; + UROS_SOFTCHECK(rcl_publish(&battery_state_publisher, &battery_state_msg, NULL)); + } + + if (clock_pub_init && + cur_time_ms - last_clock_time_ms > DELAY_CLOCK_TOPIC_MS) { + update_topic(clock_msg); + last_clock_time_ms = cur_time_ms; + UROS_SOFTCHECK(rcl_publish(&clock_publisher, &clock_msg, NULL)); + } + + if (local_pose_pub_init && + cur_time_ms - last_local_pose_time_ms > DELAY_LOCAL_POSE_TOPIC_MS) { + update_topic(local_pose_msg); + last_local_pose_time_ms = cur_time_ms; + UROS_SOFTCHECK(rcl_publish(&local_pose_publisher, &local_pose_msg, NULL)); + } + + if (local_twist_pub_init && + cur_time_ms - last_local_twist_time_ms > DELAY_LOCAL_TWIST_TOPIC_MS) { + update_topic(local_twist_msg); + last_local_twist_time_ms = cur_time_ms; + UROS_SOFTCHECK(rcl_publish(&local_twist_publisher, &local_twist_msg, NULL)); + } + + if (nav_sat_fix_pub_init && + update_topic(nav_sat_fix_msg)) { + UROS_SOFTCHECK(rcl_publish(&nav_sat_fix_publisher, &nav_sat_fix_msg, NULL)); + } + + if (tx_static_transforms_pub_init && + cur_time_ms - last_tx_static_transforms_time_ms > DELAY_STATIC_TRANSFORM_TOPIC_MS) { + update_topic(tx_static_transforms_msg); + last_tx_static_transforms_time_ms = cur_time_ms; + UROS_SOFTCHECK(rcl_publish(&tx_static_transforms_publisher, &tx_static_transforms_msg, NULL)); + } + + if (geo_pose_pub_init && + cur_time_ms - last_geo_pose_time_ms > DELAY_GEO_POSE_TOPIC_MS) { + update_topic(geo_pose_msg); + last_geo_pose_time_ms = cur_time_ms; + UROS_SOFTCHECK(rcl_publish(&geo_pose_publisher, &geo_pose_msg, NULL)); + } + + if (time_pub_init && + cur_time_ms - last_time_time_ms > DELAY_TIME_TOPIC_MS) { + update_topic(time_msg); + last_time_time_ms = cur_time_ms; + UROS_SOFTCHECK(rcl_publish(&time_publisher, &time_msg, NULL)); + // hal.console->printf("UROS: sent time: %d, %d\n", time_msg.sec, time_msg.nanosec); + } + } +} + +// subscriber callbacks +void AP_UROS_Client::on_joy_msg_trampoline(const void * msgin, void* context) +{ + AP_UROS_Client *uros = (AP_UROS_Client*)context; + const sensor_msgs__msg__Joy *msg = (const sensor_msgs__msg__Joy *)msgin; + uros->on_joy_msg(msg); +} + +void AP_UROS_Client::on_joy_msg(const sensor_msgs__msg__Joy *msg) +{ + //! @todo(srmainwaring) implement joystick RC control to AP + if (msg->axes.size >= 4) { + uros_info("UROS: sensor_msgs/Joy: %f, %f, %f, %f", + msg->axes.data[0], msg->axes.data[1], msg->axes.data[2], msg->axes.data[3]); + } else { + uros_error("UROS: sensor_msgs/Joy must have axes size >= 4"); + } +} + +void AP_UROS_Client::on_velocity_control_msg_trampoline(const void * msgin, void* context) +{ + AP_UROS_Client *uros = (AP_UROS_Client*)context; + const geometry_msgs__msg__TwistStamped *msg = (const geometry_msgs__msg__TwistStamped *)msgin; + uros->on_velocity_control_msg(msg); +} + +void AP_UROS_Client::on_velocity_control_msg(const geometry_msgs__msg__TwistStamped *msg) +{ +#if AP_EXTERNAL_CONTROL_ENABLED + if (!AP_UROS_External_Control::handle_velocity_control(*msg)) { + // TODO #23430 handle velocity control failure through rosout, throttled. + } +#endif // AP_EXTERNAL_CONTROL_ENABLED +} + +void AP_UROS_Client::on_tf_msg_trampoline(const void * msgin, void* context) +{ + AP_UROS_Client *uros = (AP_UROS_Client*)context; + const tf2_msgs__msg__TFMessage *msg = (const tf2_msgs__msg__TFMessage *)msgin; + uros->on_tf_msg(msg); +} + +void AP_UROS_Client::on_tf_msg(const tf2_msgs__msg__TFMessage * msg) +{ + if (msg->transforms.size > 0) { +#if AP_UROS_VISUALODOM_ENABLED + AP_UROS_External_Odom::handle_external_odom(*msg); +#endif // AP_UROS_VISUALODOM_ENABLED + } else { + uros_error("UROS: tf2_msgs/TFMessage with no content"); + } +} + +// service callbacks +void AP_UROS_Client::arm_motors_callback_trampoline( + const void *req, void *res, void *context) +{ + AP_UROS_Client *uros = (AP_UROS_Client*)context; + ardupilot_msgs__srv__ArmMotors_Request *req_in = + (ardupilot_msgs__srv__ArmMotors_Request *) req; + ardupilot_msgs__srv__ArmMotors_Response *res_in = + (ardupilot_msgs__srv__ArmMotors_Response *) res; + uros->arm_motors_callback(req_in, res_in); +} + +void AP_UROS_Client::arm_motors_callback( + const ardupilot_msgs__srv__ArmMotors_Request *req, + ardupilot_msgs__srv__ArmMotors_Response *res) +{ + if (req->arm) { + uros_info("UROS: Request for arming received"); + res->result = AP::arming().arm(AP_Arming::Method::DDS); + } else { + uros_info("UROS: Request for disarming received"); + res->result = AP::arming().disarm(AP_Arming::Method::DDS); + } + + if (res->result) { + uros_info("UROS: Request for Arming/Disarming : SUCCESS"); + } else { + uros_info("UROS: Request for Arming/Disarming : FAIL"); + } +} + +void AP_UROS_Client::mode_switch_callback_trampoline( + const void *req, void *res, void *context) +{ + AP_UROS_Client *uros = (AP_UROS_Client*)context; + ardupilot_msgs__srv__ModeSwitch_Request *req_in = + (ardupilot_msgs__srv__ModeSwitch_Request *) req; + ardupilot_msgs__srv__ModeSwitch_Response *res_in = + (ardupilot_msgs__srv__ModeSwitch_Response *) res; + uros->mode_switch_callback(req_in, res_in); +} + +void AP_UROS_Client::mode_switch_callback( + const ardupilot_msgs__srv__ModeSwitch_Request *req, + ardupilot_msgs__srv__ModeSwitch_Response *res) +{ + res->status = AP::vehicle()->set_mode(req->mode, ModeReason::DDS_COMMAND); + res->curr_mode = AP::vehicle()->get_mode(); + + if (res->status) { + uros_info("UROS: Request for Mode Switch : SUCCESS"); + } else { + uros_info("UROS: Request for Mode Switch : FAIL"); + } +} + +#if AP_UROS_PARAM_SRV_ENABLED +// parameter server callback +bool AP_UROS_Client::on_parameter_changed_trampoline( + const Parameter * old_param, const Parameter * new_param, void * context) +{ + AP_UROS_Client *uros = (AP_UROS_Client*)context; + return uros->on_parameter_changed(old_param, new_param); +} + +bool AP_UROS_Client::on_parameter_changed( + const Parameter * old_param, const Parameter * new_param) +{ + //! @note copied from rcl_examples/src/example_parameter_server.c + if (old_param == NULL && new_param == NULL) { + uros_error("UROS: error updating parameters"); + return false; + } + + if (old_param == NULL) { + uros_info("UROS: creating new parameter %s", + new_param->name.data); + } else if (new_param == NULL) { + uros_info("UROS: deleting parameter %s", + old_param->name.data); + } else { + switch (old_param->value.type) { + case RCLC_PARAMETER_BOOL: + uros_info( + "UROS: parameter %s modified: " + "old value: %d, new value: %d (bool)", + old_param->name.data, + old_param->value.bool_value, + new_param->value.bool_value); + break; + case RCLC_PARAMETER_INT: + uros_info( + "UROS: parameter %s modified: " + "old value: %d, new value: %d (int)", + old_param->name.data, + (int32_t)old_param->value.integer_value, + (int32_t)new_param->value.integer_value); + break; + case RCLC_PARAMETER_DOUBLE: + uros_info( + "UROS: parameter %s modified: " + "old value: %f, new value: %f (double)", + old_param->name.data, + old_param->value.double_value, + new_param->value.double_value); + break; + default: + break; + } + } + + return true; +} +#endif + +// parameter group +const AP_Param::GroupInfo AP_UROS_Client::var_info[] { + + // @Param: _ENABLE + // @DisplayName: UROS enable + // @Description: Enable UROS subsystem + // @Values: 0:Disabled,1:Enabled + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO_FLAGS("_ENABLE", 1, AP_UROS_Client, enabled, 0, + AP_PARAM_FLAG_ENABLE), + +#if AP_UROS_UDP_ENABLED + // @Param: _UDP_PORT + // @DisplayName: UROS UDP port + // @Description: UDP port number for UROS + // @Range: 1 65535 + // @RebootRequired: True + // @User: Standard + AP_GROUPINFO("_PORT", 2, AP_UROS_Client, udp.port, 2019), +#endif + + AP_GROUPEND +}; + +/* + start the UROS thread + */ +bool AP_UROS_Client::start(void) +{ + AP_Param::setup_object_defaults(this, var_info); + AP_Param::load_object_from_eeprom(this, var_info); + + if (enabled == 0) { + return true; + } + + uros_debug("UROS: creating uros_thread..."); + +#if CONFIG_HAL_BOARD == HAL_BOARD_ESP32 + BaseType_t rc = xTaskCreatePinnedToCore( + &AP_UROS_Client::main_loop_trampoline, + "APM_UROS", + 16384, + this, + 10, //UROS_PRIO, + &uros_task_handle, 0); + + if (rc != pdPASS) { + uros_error("UROS: uros_thread failed to start"); + return false; + } else { + uros_info("UROS: uros_thread started"); + return true; + } +#else + if (!hal.scheduler->thread_create( + FUNCTOR_BIND_MEMBER(&AP_UROS_Client::main_loop, void), + "APM_UROS", + 16384, + AP_HAL::Scheduler::PRIORITY_IO, + 1)) { + uros_error("UROS: thread create failed"); + return false; + } + return true; +#endif +} + +/* + trampoline for main loop for UROS thread + */ +void AP_UROS_Client::main_loop_trampoline(void *arg) { + AP_UROS_Client* uros = (AP_UROS_Client*)arg; + uros->main_loop(); + + // if main_loop returns something has gone wrong + uros_error("UROS: main_thread failed"); + while (true) { + hal.scheduler->delay(1000); + } +} + +/* + main loop for UROS thread + */ +void AP_UROS_Client::main_loop(void) +{ + uros_debug("UROS: started main loop"); + + if (!init() || !create()) { + uros_error("UROS: init or create failed"); + return; + } + uros_info("UROS: init and create succeed"); + + // one-time actions + + // periodic actions + rclc_executor_spin(&executor); + +#if AP_UROS_PARAM_SRV_ENABLED + UROS_SOFTCHECK(rclc_parameter_server_fini(¶m_server, &node)); +#endif + + UROS_SOFTCHECK(rcl_service_fini(&arm_motors_service, &node)); + + UROS_SOFTCHECK(rcl_subscription_fini(&rx_joy_subscriber, &node)); + UROS_SOFTCHECK(rcl_subscription_fini(&rx_dynamic_transforms_subscriber, &node)); + + UROS_SOFTCHECK(rcl_publisher_fini(&battery_state_publisher, &node)); + UROS_SOFTCHECK(rcl_publisher_fini(&clock_publisher, &node)); + UROS_SOFTCHECK(rcl_publisher_fini(&geo_pose_publisher, &node)); + UROS_SOFTCHECK(rcl_publisher_fini(&local_pose_publisher, &node)); + UROS_SOFTCHECK(rcl_publisher_fini(&local_twist_publisher, &node)); + UROS_SOFTCHECK(rcl_publisher_fini(&nav_sat_fix_publisher, &node)); + UROS_SOFTCHECK(rcl_publisher_fini(&tx_static_transforms_publisher, &node)); + UROS_SOFTCHECK(rcl_publisher_fini(&time_publisher, &node)); + + UROS_SOFTCHECK(rcl_node_fini(&node)); +} + +bool AP_UROS_Client::init() +{ + WITH_SEMAPHORE(csem); + + // initialize transport + bool initTransportStatus = false; + +// #if defined(RMW_UXRCE_TRANSPORT_CUSTOM) + // serial init will fail if the SERIALn_PROTOCOL is not setup + if (!initTransportStatus) { + initTransportStatus = urosSerialInit(); + } +// #else +// #error micro-ROS transports misconfigured +// #endif + +#if AP_UROS_UDP_ENABLED + // fallback to UDP if available + if (!initTransportStatus) { + initTransportStatus = urosUdpInit(); + } +#endif + + if (initTransportStatus) { + uros_info("UROS: transport initializated"); + } + else { + uros_error("UROS: transport initialization failed"); + return false; + } + + // create allocator + allocator = rcl_get_default_allocator(); + + // either: custom initialisation + // uros_debug("UROS: create init options"); + // rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + // UROS_CHECK(rcl_init_options_init(&init_options, allocator), "create init options"); + + //! @todo add conditional check if using UDP + // uros_debug("UROS: set rmw init options"); + // rmw_init_options_t* rmw_options = rcl_init_options_get_rmw_init_options(&init_options); + // UROS_CHECK(rmw_uros_options_set_udp_address("192.168.1.31", "2019", rmw_options), + // "set rmw init options"); + + // create init_options + // uros_debug("UROS: initialise support"); + // UROS_CHECK(rclc_support_init_with_options(&support, 0, NULL, &init_options, &allocator), + // "initialise support"); + + // or: default initialisation + // create init_options + // uros_debug("UROS: initialise support"); + UROS_CHECK(rclc_support_init(&support, 0, NULL, &allocator), + "initialise support"); + + // create node + // uros_debug("UROS: initialise node"); + UROS_CHECK(rclc_node_init_default(&node, "ardupilot_uros", "", &support), + "initialise node"); + + uros_info("UROS: init complete"); + + return true; +} + +bool AP_UROS_Client::create() +{ + uros_debug("UROS: create entities"); + + WITH_SEMAPHORE(csem); + + // initialise strings and sequences + { + battery_state_conf.max_string_capacity = 32; + battery_state_conf.max_ros2_type_sequence_capacity = AP_BATT_MONITOR_CELLS_MAX; + battery_state_conf.max_basic_type_sequence_capacity = AP_BATT_MONITOR_CELLS_MAX; + + battery_state_mem_init = micro_ros_utilities_create_message_memory( + ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, BatteryState), + &battery_state_msg, + battery_state_conf + ); + if (!battery_state_mem_init) { + uros_error("UROS: configure battery state msg... FAILED"); + return false; + } + uros_debug("UROS: configure battery state msg... OK"); + } + { + geo_pose_conf.max_string_capacity = 32; + geo_pose_conf.max_ros2_type_sequence_capacity = 2; + geo_pose_conf.max_basic_type_sequence_capacity = 2; + + geo_pose_mem_init = micro_ros_utilities_create_message_memory( + ROSIDL_GET_MSG_TYPE_SUPPORT(geographic_msgs, msg, GeoPoseStamped), + &geo_pose_msg, + geo_pose_conf + ); + if (!geo_pose_mem_init) { + uros_error("UROS: configure geo pose msg... FAILED"); + return false; + } + uros_debug("UROS: configure geo pose msg... OK"); + } + { + local_pose_conf.max_string_capacity = 32; + local_pose_conf.max_ros2_type_sequence_capacity = 2; + local_pose_conf.max_basic_type_sequence_capacity = 2; + + local_pose_mem_init = micro_ros_utilities_create_message_memory( + ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, PoseStamped), + &local_pose_msg, + local_pose_conf + ); + if (!local_pose_mem_init) { + uros_error("UROS: configure local pose msg... FAILED"); + return false; + } + uros_debug("UROS: configure local pose msg... OK"); + } + { + local_twist_conf.max_string_capacity = 32; + local_twist_conf.max_ros2_type_sequence_capacity = 2; + local_twist_conf.max_basic_type_sequence_capacity = 2; + + local_twist_mem_init = micro_ros_utilities_create_message_memory( + ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, TwistStamped), + &local_twist_msg, + local_twist_conf + ); + if (!local_twist_mem_init) { + uros_error("UROS: configure local twist msg... FAILED"); + return false; + } + uros_debug("UROS: configure local twist msg... OK"); + } + { + nav_sat_fix_conf.max_string_capacity = 32; + nav_sat_fix_conf.max_ros2_type_sequence_capacity = 2; + nav_sat_fix_conf.max_basic_type_sequence_capacity = 2; + + nav_sat_fix_mem_init = micro_ros_utilities_create_message_memory( + ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, NavSatFix), + &nav_sat_fix_msg, + nav_sat_fix_conf + ); + if (!nav_sat_fix_mem_init) { + uros_error("UROS: configure nav sat fix msg... FAILED"); + return false; + } + uros_debug("UROS: configure nav sat fix msg... OK"); + } + { + tx_static_transforms_conf.max_string_capacity = 32; + tx_static_transforms_conf.max_ros2_type_sequence_capacity = GPS_MAX_RECEIVERS; + tx_static_transforms_conf.max_basic_type_sequence_capacity = GPS_MAX_RECEIVERS; + + tx_static_transforms_mem_init = micro_ros_utilities_create_message_memory( + ROSIDL_GET_MSG_TYPE_SUPPORT(tf2_msgs, msg, TFMessage), + &tx_static_transforms_msg, + tx_static_transforms_conf + ); + if (!nav_sat_fix_mem_init) { + uros_error("UROS: configure static transform msg... FAILED"); + return false; + } + uros_debug("UROS: configure static transform msg... OK"); + } + { + rx_joy_conf.max_string_capacity = 32; + rx_joy_conf.max_ros2_type_sequence_capacity = 32; + rx_joy_conf.max_basic_type_sequence_capacity = 32; + + rx_joy_mem_init = micro_ros_utilities_create_message_memory( + ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Joy), + &rx_joy_msg, + rx_joy_conf + ); + if (!rx_joy_mem_init) { + uros_error("UROS: configure joy msg... FAILED"); + return false; + } + uros_debug("UROS: configure joy msg... OK"); + } + { + rx_velocity_control_conf.max_string_capacity = 32; + rx_velocity_control_conf.max_ros2_type_sequence_capacity = 32; + rx_velocity_control_conf.max_basic_type_sequence_capacity = 2; + + rx_velocity_control_mem_init = micro_ros_utilities_create_message_memory( + ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, TwistStamped), + &rx_velocity_control_msg, + rx_velocity_control_conf + ); + if (!rx_velocity_control_mem_init) { + uros_error("UROS: configure velocity control msg... FAILED"); + return false; + } + uros_debug("UROS: configure velocity control msg... OK"); + } + { + rx_dynamic_transforms_conf.max_string_capacity = 32; + rx_dynamic_transforms_conf.max_ros2_type_sequence_capacity = 16; + rx_dynamic_transforms_conf.max_basic_type_sequence_capacity = 16; + + rx_dynamic_transforms_mem_init = micro_ros_utilities_create_message_memory( + ROSIDL_GET_MSG_TYPE_SUPPORT(tf2_msgs, msg, TFMessage), + &rx_dynamic_transforms_msg, + rx_dynamic_transforms_conf + ); + if (!rx_dynamic_transforms_mem_init) { + uros_error("UROS: configure tf msg... FAILED"); + return false; + } + uros_debug("UROS: configure tf msg... OK"); + } + + // create publishers + uint8_t number_of_publishers = 0; + + if (battery_state_mem_init) { + rcl_ret_t rc = rclc_publisher_init_default( + &battery_state_publisher, + &node, + ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, BatteryState), + "ap/battery/battery0"); + if (!(battery_state_pub_init = (rc == RCL_RET_OK))) { + uros_error("UROS: create battery state publisher... FAILED (%d)", (int16_t)rc); + return false; + } + number_of_publishers++; + uros_debug("UROS: create battery state publisher... OK"); + } + + { + rcl_ret_t rc = rclc_publisher_init_default( + &clock_publisher, + &node, + ROSIDL_GET_MSG_TYPE_SUPPORT(rosgraph_msgs, msg, Clock), + "ap/clock"); + if (!(clock_pub_init = (rc == RCL_RET_OK))) { + uros_error("UROS: create clock publisher... FAILED (%d)", (int16_t)rc); + return false; + } + number_of_publishers++; + uros_debug("UROS: create clock publisher... OK"); + } + + if (geo_pose_mem_init) { + rcl_ret_t rc = rclc_publisher_init_default( + &geo_pose_publisher, + &node, + ROSIDL_GET_MSG_TYPE_SUPPORT(geographic_msgs, msg, GeoPoseStamped), + "ap/geopose/filtered"); + if (!(geo_pose_pub_init = (rc == RCL_RET_OK))) { + uros_error("UROS: create geo pose publisher... FAILED (%d)",(int16_t)rc); + return false; + } + number_of_publishers++; + uros_debug("UROS: create geo pose publisher... OK"); + } + + if (local_pose_mem_init) { + rcl_ret_t rc = rclc_publisher_init_default( + &local_pose_publisher, + &node, + ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, PoseStamped), + "ap/pose/filtered"); + if (!(local_pose_pub_init = (rc == RCL_RET_OK))) { + uros_error("UROS: create local pose publisher... FAILED (%d)", (int16_t)rc); + return false; + } + number_of_publishers++; + uros_debug("UROS: create local pose publisher... OK"); + } + + if (local_twist_mem_init) { + rcl_ret_t rc = rclc_publisher_init_default( + &local_twist_publisher, + &node, + ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, TwistStamped), + "ap/twist/filtered"); + if (!(local_twist_pub_init = (rc == RCL_RET_OK))) { + uros_error("UROS: create local twist publisher... FAILED (%d)", (int16_t)rc); + return false; + } + number_of_publishers++; + uros_debug("UROS: create local twist publisher... OK"); + } + + if (nav_sat_fix_mem_init) { + rcl_ret_t rc = rclc_publisher_init_default( + &nav_sat_fix_publisher, + &node, + ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, NavSatFix), + "ap/navsat/navsat0"); + if (!(nav_sat_fix_pub_init = (rc == RCL_RET_OK))) { + uros_error("UROS: create nav sat fix publisher... FAILED (%d)", (int16_t)rc); + return false; + } + number_of_publishers++; + uros_debug("UROS: create nav sat fix publisher... OK"); + } + + if (tx_static_transforms_mem_init) { + rcl_ret_t rc = rclc_publisher_init_default( + &tx_static_transforms_publisher, + &node, + ROSIDL_GET_MSG_TYPE_SUPPORT(tf2_msgs, msg, TFMessage), + "ap/tf_static"); + if (!(tx_static_transforms_pub_init = (rc == RCL_RET_OK))) { + uros_error("UROS: create static transform publisher... FAILED (%d)", (int16_t)rc); + return false; + } + number_of_publishers++; + uros_debug("UROS: create static transform publisher... OK"); + } + + { + rcl_ret_t rc = rclc_publisher_init_default( + &time_publisher, + &node, + ROSIDL_GET_MSG_TYPE_SUPPORT(builtin_interfaces, msg, Time), + "ap/time"); + if (!(time_pub_init = (rc == RCL_RET_OK))) { + uros_error("UROS: create time publisher... FAILED (%d)", (int16_t)rc); + return false; + } + number_of_publishers++; + uros_debug("UROS: create time publisher... OK"); + } + + // create subscribers + uint8_t number_of_subscribers = 0; + + if (rx_joy_mem_init) { + rcl_ret_t rc = rclc_subscription_init_default( + &rx_joy_subscriber, + &node, + ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Joy), + "ap/joy"); + if (!(rx_joy_sub_init = (rc == RCL_RET_OK))) { + uros_error("UROS: create joy subscriber... FAILED (%d)", (int16_t)rc); + return false; + } + number_of_subscribers++; + uros_debug("UROS: create joy subscriber... OK"); + } + + if (rx_velocity_control_mem_init) { + rcl_ret_t rc = rclc_subscription_init_default( + &rx_velocity_control_subscriber, + &node, + ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, TwistStamped), + "ap/cmd_vel"); + if (!(rx_velocity_control_sub_init = (rc == RCL_RET_OK))) { + uros_error("UROS: create velocity control subscriber... FAILED (%d)", (int16_t)rc); + return false; + } + number_of_subscribers++; + uros_debug("UROS: create velocity control subscriber... OK"); + } + + if (rx_dynamic_transforms_mem_init) { + rcl_ret_t rc = rclc_subscription_init_default( + &rx_dynamic_transforms_subscriber, + &node, + ROSIDL_GET_MSG_TYPE_SUPPORT(tf2_msgs, msg, TFMessage), + "ap/tf"); + if (!(rx_dynamic_transforms_sub_init = (rc == RCL_RET_OK))) { + uros_error("UROS: create tf subscriber... FAILED (%d)", (int16_t)rc); + return false; + } + number_of_subscribers++; + uros_debug("UROS: create tf subscriber... OK"); + } + + // create services + uint8_t number_of_services = 0; + + { + rcl_ret_t rc = rclc_service_init_default( + &arm_motors_service, + &node, + ROSIDL_GET_SRV_TYPE_SUPPORT(ardupilot_msgs, srv, ArmMotors), + "/ap/arm_motors"); + if (!(arm_motors_srv_init = (rc == RCL_RET_OK))) { + uros_error("UROS: create arm_motors service... FAILED (%d)", (int16_t)rc); + return false; + } + number_of_services++; + uros_debug("UROS: create arm_motors service... OK"); + } + + { + rcl_ret_t rc = rclc_service_init_default( + &mode_switch_service, + &node, + ROSIDL_GET_SRV_TYPE_SUPPORT(ardupilot_msgs, srv, ModeSwitch), + "/ap/mode_switch"); + if (!(mode_switch_srv_init = (rc == RCL_RET_OK))) { + uros_error("UROS: create mode_switch service... FAILED (%d)", (int16_t)rc); + return false; + } + number_of_services++; + uros_debug("UROS: create mode_switch service... OK"); + } + +#if AP_UROS_PARAM_SRV_ENABLED + // create parameter server + { + hal.console->printf("UROS: create parameter server... "); + + // create parameter options + // defaults: + // notify_changed_over_dds: true + // max_params: 4 + // allow_undeclared_parameters: false + // low_mem_mode: false + const rclc_parameter_options_t options = { + .notify_changed_over_dds = true, + .max_params = 4, + .allow_undeclared_parameters = true, + .low_mem_mode = true + }; + + rcl_ret_t rc = rclc_parameter_server_init_with_option( + ¶m_server, &node, &options); + if ((param_srv_init = (rc == RCL_RET_OK))) { + //! @note the parameter server requires 5 services + 1 publisher + // https://micro.ros.org/docs/tutorials/programming_rcl_rclc/parameters/ + number_of_publishers++; + number_of_services += RCLC_EXECUTOR_PARAMETER_SERVER_HANDLES; + hal.console->printf("OK\n"); + } else { + hal.console->printf("FAIL: %d\n", (int16_t)rc); + } + } +#endif + + // create timer + // uros_debug("UROS: create timer"); + UROS_CHECK(rclc_timer_init_default( + &_timer, + &support, + RCL_MS_TO_NS(timer_timeout_ms), + timer_callback_trampoline), + "create timer"); + + // number of entities + uros_debug("UROS: number of publishers: %d", number_of_publishers); + uros_debug("UROS: number of subscribers: %d", number_of_subscribers); + uros_debug("UROS: number of services: %d", number_of_services); + uint8_t number_of_handles = + number_of_publishers + number_of_subscribers + number_of_services; + + // create executor + // uros_debug("UROS: initialise executor"); + executor = rclc_executor_get_zero_initialized_executor(); + UROS_CHECK(rclc_executor_init(&executor, &support.context, + number_of_handles, &allocator), + "initialise executor"); + + // uros_debug("UROS: add timer to executor"); + UROS_CHECK(rclc_executor_add_timer(&executor, &_timer), + "add timer to executor"); + + // uros_debug("UROS: add subscriptions to executor"); + if (rx_joy_sub_init) { + UROS_CHECK(rclc_executor_add_subscription_with_context(&executor, &rx_joy_subscriber, + &rx_joy_msg, &AP_UROS_Client::on_joy_msg_trampoline, this, ON_NEW_DATA), + "add subscription joy to executor"); + } + if (rx_velocity_control_sub_init) { + UROS_CHECK(rclc_executor_add_subscription_with_context(&executor, &rx_velocity_control_subscriber, + &rx_velocity_control_msg, &AP_UROS_Client::on_velocity_control_msg_trampoline, this, ON_NEW_DATA), + "add subscription velocity control to executor"); + } + if (rx_dynamic_transforms_sub_init) { + UROS_CHECK(rclc_executor_add_subscription_with_context(&executor, &rx_dynamic_transforms_subscriber, + &rx_dynamic_transforms_msg, &AP_UROS_Client::on_tf_msg_trampoline, this, ON_NEW_DATA), + "add subscription dynamic transforms to executor"); + } + + // uros_debug("UROS: add services to executor"); + if (arm_motors_srv_init) { + UROS_CHECK(rclc_executor_add_service_with_context(&executor, &arm_motors_service, + &arm_motors_req, &arm_motors_res, + &AP_UROS_Client::arm_motors_callback_trampoline, this), + "add service arm motors to executor"); + } + if (mode_switch_srv_init) { + UROS_CHECK(rclc_executor_add_service_with_context(&executor, &mode_switch_service, + &mode_switch_req, &mode_switch_res, + &AP_UROS_Client::mode_switch_callback_trampoline, this), + "add service mode switch to executor"); + } + +#if AP_UROS_PARAM_SRV_ENABLED + // uros_debug("UROS: add parameter server to executor"); + if (param_srv_init) { + UROS_CHECK(rclc_executor_add_parameter_server_with_context(&executor, ¶m_server, + &AP_UROS_Client::on_parameter_changed_trampoline, this), + "add parameter server to executor"); + } +#endif + + uros_debug("UROS: create complete"); + + return true; +} + +#if CONFIG_HAL_BOARD != HAL_BOARD_SITL && CONFIG_HAL_BOARD != HAL_BOARD_ESP32 + +extern "C" { + int clock_gettime(clockid_t clockid, struct timespec *ts); +} + +int clock_gettime(clockid_t clockid, struct timespec *ts) +{ + //! @todo the value of clockid is ignored here. + //! A fallback mechanism is employed against the caller's choice of clock. + uint64_t utc_usec; + if (!AP::rtc().get_utc_usec(utc_usec)) { + utc_usec = AP_HAL::micros64(); + } + ts->tv_sec = utc_usec / 1000000ULL; + ts->tv_nsec = (utc_usec % 1000000ULL) * 1000UL; + return 0; +} + + +// Placeholder for fprintf() usage in: +// +// rosidl_runtime_c/src/string_functions.c +// rosidl_runtime_c__String__fini(rosidl_runtime_c__String * str) +// +// rosidl_runtime_c/src/u16string_functions.c +// rosidl_runtime_c__U16String__fini(rosidl_runtime_c__U16String * str) +// +#include + +extern "C" { + size_t __wrap_fwrite(const void *ptr, size_t size, size_t nmemb, FILE *stream); +} + +size_t __wrap_fwrite(const void *ptr, size_t size, size_t nmemb, FILE *stream) +{ + // return dll_fwrite(ptr, size, nmemb, stream); + return 0; +} + + +#endif // CONFIG_HAL_BOARD + +#endif // AP_UROS_ENABLED + + diff --git a/libraries/AP_UROS/AP_UROS_Client.h b/libraries/AP_UROS/AP_UROS_Client.h new file mode 100644 index 00000000000000..88a47c69d41b2f --- /dev/null +++ b/libraries/AP_UROS/AP_UROS_Client.h @@ -0,0 +1,268 @@ +#pragma once + +#if AP_UROS_ENABLED + +// micro-xrce-dds +#include +#include + +// micro-ROS +#include +#include +#include +#include +#include +#include + +// ROS msgs +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "fcntl.h" + +#include +#include "AP_UROS_config.h" + +#if AP_UROS_UDP_ENABLED +#include +#endif + +extern const AP_HAL::HAL& hal; + +class AP_UROS_Client +{ +private: + + AP_Int8 enabled; + + HAL_Semaphore csem; + + // micro-ROS + rcl_allocator_t allocator; + rclc_support_t support; + rcl_node_t node; + rclc_executor_t executor; + rcl_timer_t _timer; + const unsigned int timer_timeout_ms = 1; + + // publishers + rcl_publisher_t battery_state_publisher; + sensor_msgs__msg__BatteryState battery_state_msg; + uint64_t last_battery_state_time_ms; + micro_ros_utilities_memory_conf_t battery_state_conf; + bool battery_state_mem_init = false; + bool battery_state_pub_init = false; + + rcl_publisher_t clock_publisher; + rosgraph_msgs__msg__Clock clock_msg; + uint64_t last_clock_time_ms; + bool clock_pub_init = false; + + rcl_publisher_t geo_pose_publisher; + geographic_msgs__msg__GeoPoseStamped geo_pose_msg; + uint64_t last_geo_pose_time_ms; + micro_ros_utilities_memory_conf_t geo_pose_conf; + bool geo_pose_mem_init = false; + bool geo_pose_pub_init = false; + + rcl_publisher_t local_pose_publisher; + geometry_msgs__msg__PoseStamped local_pose_msg; + uint64_t last_local_pose_time_ms; + micro_ros_utilities_memory_conf_t local_pose_conf; + bool local_pose_mem_init = false; + bool local_pose_pub_init = false; + + rcl_publisher_t local_twist_publisher; + geometry_msgs__msg__TwistStamped local_twist_msg; + uint64_t last_local_twist_time_ms; + micro_ros_utilities_memory_conf_t local_twist_conf; + bool local_twist_mem_init = false; + bool local_twist_pub_init = false; + + rcl_publisher_t nav_sat_fix_publisher; + sensor_msgs__msg__NavSatFix nav_sat_fix_msg; + uint64_t last_nav_sat_fix_time_ms; + micro_ros_utilities_memory_conf_t nav_sat_fix_conf; + bool nav_sat_fix_mem_init = false; + bool nav_sat_fix_pub_init = false; + + rcl_publisher_t time_publisher; + builtin_interfaces__msg__Time time_msg; + uint64_t last_time_time_ms; + bool time_pub_init = false; + + // outgoing transforms + rcl_publisher_t tx_static_transforms_publisher; + tf2_msgs__msg__TFMessage tx_static_transforms_msg; + uint64_t last_tx_static_transforms_time_ms; + micro_ros_utilities_memory_conf_t tx_static_transforms_conf; + bool tx_static_transforms_mem_init = false; + bool tx_static_transforms_pub_init = false; + + // subscribers + rcl_subscription_t rx_joy_subscriber; + sensor_msgs__msg__Joy rx_joy_msg; + micro_ros_utilities_memory_conf_t rx_joy_conf; + bool rx_joy_mem_init = false; + bool rx_joy_sub_init = false; + + // velocity control (ROS REP 147) + rcl_subscription_t rx_velocity_control_subscriber; + geometry_msgs__msg__TwistStamped rx_velocity_control_msg; + micro_ros_utilities_memory_conf_t rx_velocity_control_conf; + bool rx_velocity_control_mem_init = false; + bool rx_velocity_control_sub_init = false; + + // incoming transforms + rcl_subscription_t rx_dynamic_transforms_subscriber; + tf2_msgs__msg__TFMessage rx_dynamic_transforms_msg; + micro_ros_utilities_memory_conf_t rx_dynamic_transforms_conf; + bool rx_dynamic_transforms_mem_init = false; + bool rx_dynamic_transforms_sub_init = false; + + // services + rcl_service_t arm_motors_service; + ardupilot_msgs__srv__ArmMotors_Request arm_motors_req; + ardupilot_msgs__srv__ArmMotors_Response arm_motors_res; + bool arm_motors_srv_init = false; + + rcl_service_t mode_switch_service; + ardupilot_msgs__srv__ModeSwitch_Request mode_switch_req; + ardupilot_msgs__srv__ModeSwitch_Response mode_switch_res; + bool mode_switch_srv_init = false; + +#if AP_UROS_PARAM_SRV_ENABLED + // parameter server + rclc_parameter_server_t param_server; + bool param_srv_init = false; +#endif + +#if CONFIG_HAL_BOARD == HAL_BOARD_ESP32 + // thread handle + TaskHandle_t uros_task_handle; +#endif + + // singleton + static AP_UROS_Client *_singleton; + + // publishers + void update_topic(sensor_msgs__msg__BatteryState& msg); + void update_topic(rosgraph_msgs__msg__Clock& msg); + void update_topic(geographic_msgs__msg__GeoPoseStamped& msg); + void update_topic(geometry_msgs__msg__PoseStamped& msg); + void update_topic(geometry_msgs__msg__TwistStamped& msg); + bool update_topic(sensor_msgs__msg__NavSatFix& msg); + void update_topic(builtin_interfaces__msg__Time& msg); + void update_topic(tf2_msgs__msg__TFMessage& msg); + + // subscribers + static void on_joy_msg_trampoline(const void * msgin, void *context); + void on_joy_msg(const sensor_msgs__msg__Joy * msg); + + static void on_velocity_control_msg_trampoline(const void * msgin, void *context); + void on_velocity_control_msg(const geometry_msgs__msg__TwistStamped * msg); + + static void on_tf_msg_trampoline(const void * msgin, void *context); + void on_tf_msg(const tf2_msgs__msg__TFMessage * msg); + + // service callbacks + static void arm_motors_callback_trampoline(const void *req, void *res, void *context); + void arm_motors_callback( + const ardupilot_msgs__srv__ArmMotors_Request *req, + ardupilot_msgs__srv__ArmMotors_Response *res); + + static void mode_switch_callback_trampoline(const void *req, void *res, void *context); + void mode_switch_callback( + const ardupilot_msgs__srv__ModeSwitch_Request *req, + ardupilot_msgs__srv__ModeSwitch_Response *res); + +#if AP_UROS_PARAM_SRV_ENABLED + // parameter server callback + static bool on_parameter_changed_trampoline( + const Parameter * old_param, const Parameter * new_param, void * context); + bool on_parameter_changed( + const Parameter * old_param, const Parameter * new_param); +#endif + + // timer callbacks + static void timer_callback_trampoline(rcl_timer_t * timer, int64_t last_call_time); + void timer_callback(rcl_timer_t * timer, int64_t last_call_time); + + static void main_loop_trampoline(void *arg); + + // functions for serial transport + bool urosSerialInit(); + static bool serial_transport_open(uxrCustomTransport* transport); + static bool serial_transport_close(uxrCustomTransport* transport); + static size_t serial_transport_write(uxrCustomTransport* transport, + const uint8_t* buf, size_t len, uint8_t* error); + static size_t serial_transport_read(uxrCustomTransport* transport, + uint8_t* buf, size_t len, int timeout, uint8_t* error); + + struct { + AP_HAL::UARTDriver *port; + uxrCustomTransport transport; + } serial; + +#if AP_UROS_UDP_ENABLED + // functions for udp transport + bool urosUdpInit(); + static bool udp_transport_open(uxrCustomTransport* transport); + static bool udp_transport_close(uxrCustomTransport* transport); + static size_t udp_transport_write(uxrCustomTransport* transport, + const uint8_t* buf, size_t len, uint8_t* error); + static size_t udp_transport_read(uxrCustomTransport* transport, + uint8_t* buf, size_t len, int timeout, uint8_t* error); + + struct { + AP_Int32 port; + // UDP endpoint + const char* ip = "127.0.0.1"; + // UDP Allocation + uxrCustomTransport transport; + SocketAPM *socket; + } udp; +#endif + + // client key we present + static constexpr uint32_t uniqueClientKey = 0xAAAABBBB; + +public: + AP_UROS_Client(); + + bool start(void); + void main_loop(void); + + //! @brief Initialize the client. + //! @return True on successful initialization, false on failure. + bool init() WARN_IF_UNUSED; + + //! @brief Set up the client. + //! @return True on successful creation, false on failure + bool create() WARN_IF_UNUSED; + + //! @brief Parameter storage. + static const struct AP_Param::GroupInfo var_info[]; + + //! @note A singleton is needed for the timer_callback_trampoline as + //! rcl and rlcl do not provide a function to initialise a timer with + //! a context as is case for the subscriber callbacks. + static AP_UROS_Client *get_singleton(); +}; + +#endif // AP_UROS_ENABLED diff --git a/libraries/AP_UROS/AP_UROS_ExternalControl.cpp b/libraries/AP_UROS/AP_UROS_ExternalControl.cpp new file mode 100644 index 00000000000000..e9eb97c3288f1b --- /dev/null +++ b/libraries/AP_UROS/AP_UROS_ExternalControl.cpp @@ -0,0 +1,13 @@ +#if AP_UROS_ENABLED + +#include "AP_UROS_ExternalControl.h" +#include +#include +#include + +bool AP_UROS_External_Control::handle_velocity_control(const geometry_msgs__msg__TwistStamped& cmd_vel) +{ + return AP_ROS_External_Control::handle_velocity_control(cmd_vel); +} + +#endif // AP_UROS_ENABLED diff --git a/libraries/AP_UROS/AP_UROS_ExternalControl.h b/libraries/AP_UROS/AP_UROS_ExternalControl.h new file mode 100644 index 00000000000000..23e958a3fb5135 --- /dev/null +++ b/libraries/AP_UROS/AP_UROS_ExternalControl.h @@ -0,0 +1,11 @@ +#pragma once + +#if AP_UROS_ENABLED +#include + +class AP_UROS_External_Control +{ +public: + static bool handle_velocity_control(const geometry_msgs__msg__TwistStamped& cmd_vel); +}; +#endif // AP_UROS_ENABLED diff --git a/libraries/AP_UROS/AP_UROS_External_Odom.cpp b/libraries/AP_UROS/AP_UROS_External_Odom.cpp new file mode 100644 index 00000000000000..26aebfbf60d0f4 --- /dev/null +++ b/libraries/AP_UROS/AP_UROS_External_Odom.cpp @@ -0,0 +1,23 @@ +#include "AP_UROS_External_Odom.h" +#include "AP_UROS_Type_Conversions.h" + +#if AP_UROS_VISUALODOM_ENABLED + +#include + +void AP_UROS_External_Odom::handle_external_odom(const tf2_msgs__msg__TFMessage& msg) +{ + AP_ROS_External_Odom::handle_external_odom(msg); +} + +bool AP_UROS_External_Odom::is_odometry_frame(const geometry_msgs__msg__TransformStamped& msg) +{ + return AP_ROS_External_Odom::is_odometry_frame(msg); +} + +void AP_UROS_External_Odom::convert_transform(const geometry_msgs__msg__Transform& ros_transform, Vector3f& translation, Quaternion& rotation) +{ + AP_ROS_External_Odom::convert_transform(ros_transform, translation, rotation); +} + +#endif // AP_UROS_VISUALODOM_ENABLED diff --git a/libraries/AP_UROS/AP_UROS_External_Odom.h b/libraries/AP_UROS/AP_UROS_External_Odom.h new file mode 100644 index 00000000000000..fed3c2cff8b6e8 --- /dev/null +++ b/libraries/AP_UROS/AP_UROS_External_Odom.h @@ -0,0 +1,34 @@ +// Class for handling external localization data. +// For historical reasons, it's called odometry to match AP_VisualOdom. + +#pragma once + +#include "AP_UROS_config.h" +#if AP_UROS_VISUALODOM_ENABLED + +#include +#include + +#include +#include + +class AP_UROS_External_Odom +{ +public: + + // Handler for external position localization + static void handle_external_odom(const tf2_msgs__msg__TFMessage& msg); + + // Checks the child and parent frames match a set needed for external odom. + // Since multiple different transforms can be sent, this validates the specific transform is + // for odometry. + static bool is_odometry_frame(const geometry_msgs__msg__TransformStamped& msg); + + // Helper to convert from ROS transform to AP datatypes + // ros_transform is in ENU + // translation is in NED + static void convert_transform(const geometry_msgs__msg__Transform& ros_transform, Vector3f& translation, Quaternion& rotation); + +}; + +#endif // AP_UROS_VISUALODOM_ENABLED diff --git a/libraries/AP_UROS/AP_UROS_Frames.h b/libraries/AP_UROS/AP_UROS_Frames.h new file mode 100644 index 00000000000000..0bdaf354ec116f --- /dev/null +++ b/libraries/AP_UROS/AP_UROS_Frames.h @@ -0,0 +1,7 @@ +#pragma once + +static constexpr char WGS_84_FRAME_ID[] = "WGS-84"; +// https://www.ros.org/reps/rep-0105.html#base-link +static constexpr char BASE_LINK_FRAME_ID[] = "base_link"; +// https://www.ros.org/reps/rep-0105.html#map +static constexpr char MAP_FRAME[] = "map"; diff --git a/libraries/AP_UROS/AP_UROS_Serial.cpp b/libraries/AP_UROS/AP_UROS_Serial.cpp new file mode 100644 index 00000000000000..d95ae28bddf90e --- /dev/null +++ b/libraries/AP_UROS/AP_UROS_Serial.cpp @@ -0,0 +1,108 @@ +#include "AP_UROS_Client.h" + +#include + +#include + +#include + +/* + open connection on a serial port + */ +bool AP_UROS_Client::serial_transport_open(uxrCustomTransport *t) +{ + AP_UROS_Client *uros = (AP_UROS_Client *)t->args; + AP_SerialManager *serial_manager = AP_SerialManager::get_singleton(); + AP_HAL::UARTDriver *uros_port = serial_manager->find_serial(AP_SerialManager::SerialProtocol_DDS_XRCE, 0); + if (uros_port == nullptr) { + hal.console->printf("UROS: serial transport open... FAIL\n"); + return false; + } + + // ensure we own the UART + uros_port->begin(0); + uros->serial.port = uros_port; + + return true; +} + +/* + close serial transport + */ +bool AP_UROS_Client::serial_transport_close(uxrCustomTransport *t) +{ + // we don't actually close the UART + return true; +} + +/* + write on serial transport + */ +size_t AP_UROS_Client::serial_transport_write(uxrCustomTransport *t, const uint8_t* buf, size_t len, uint8_t* error) +{ + AP_UROS_Client *uros = (AP_UROS_Client *)t->args; + if (uros->serial.port == nullptr) { + *error = EINVAL; + return 0; + } + ssize_t bytes_written = uros->serial.port->write(buf, len); + if (bytes_written <= 0) { + *error = 1; + return 0; + } + //! @todo populate the error code correctly + *error = 0; + return bytes_written; +} + +/* + read from a serial transport + */ +size_t AP_UROS_Client::serial_transport_read(uxrCustomTransport *t, uint8_t* buf, size_t len, int timeout_ms, uint8_t* error) +{ + AP_UROS_Client *uros = (AP_UROS_Client *)t->args; + if (uros->serial.port == nullptr) { + *error = EINVAL; + return 0; + } + const uint32_t tstart = AP_HAL::millis(); + while (AP_HAL::millis() - tstart < uint32_t(timeout_ms) && + uros->serial.port->available() < len) { + hal.scheduler->delay_microseconds(100); // TODO select or poll this is limiting speed (100us) + } + ssize_t bytes_read = uros->serial.port->read(buf, len); + if (bytes_read <= 0) { + *error = 1; + return 0; + } + //! @todo Add error reporting + *error = 0; + return bytes_read; +} + +/* + initialise serial connection + */ +bool AP_UROS_Client::urosSerialInit() +{ + // fail transport initialisation if port not configured + AP_SerialManager *serial_manager = AP_SerialManager::get_singleton(); + auto *uros_port = serial_manager->find_serial(AP_SerialManager::SerialProtocol_DDS_XRCE, 0); + if (uros_port == nullptr) { + hal.console->printf("UROS: serial port not configured\n"); + return false; + } + + // setup a framed transport for serial + hal.console->printf("UROS: initialising custom transport\n"); + rmw_ret_t rcl_ret = rmw_uros_set_custom_transport( + true, + (void *) &serial.transport, + serial_transport_open, + serial_transport_close, + serial_transport_write, + serial_transport_read + ); + + return (rcl_ret == RCL_RET_OK); +} diff --git a/libraries/AP_UROS/AP_UROS_Type_Conversions.cpp b/libraries/AP_UROS/AP_UROS_Type_Conversions.cpp new file mode 100644 index 00000000000000..cefbb7839b613f --- /dev/null +++ b/libraries/AP_UROS/AP_UROS_Type_Conversions.cpp @@ -0,0 +1,104 @@ +#include "AP_UROS_Type_Conversions.h" +#if AP_UROS_ENABLED + +uint64_t AP_UROS_Type_Conversions::time_u64_micros(const builtin_interfaces__msg__Time& ros_time) +{ + return AP_ROS_TypeConversions::time_u64_micros(ros_time); +} + +// string specialisations +template <> +const char* string_data(const rosidl_runtime_c__String& str) { + return str.data; +} + +template <> +char* mutable_string_data(rosidl_runtime_c__String& str) { + return str.data; +} + +// transform specialisations +template <> +typename transforms_size_type::type +transforms_size(const tf2_msgs__msg__TFMessage& msg) { + return msg.transforms.size; +} + +template <> +typename mutable_transforms_size_type::type +mutable_transforms_size(tf2_msgs__msg__TFMessage& msg) { + return msg.transforms.size; +} + +template <> +typename transforms_type::type +transforms_data(const tf2_msgs__msg__TFMessage& msg) { + return msg.transforms.data; +} + +template <> +typename mutable_transforms_type::type +mutable_transforms_data(tf2_msgs__msg__TFMessage& msg) { + return msg.transforms.data; +} + +// cell_voltage specialisations +template <> +typename mutable_cell_voltage_type::type +mutable_cell_voltage_data(sensor_msgs__msg__BatteryState& msg) { + return msg.cell_voltage.data; +} + +#if 0 +//! @todo(srmainwaring) move to test / examples sub-folder. +// tests for the accessor templates +#include + +extern const AP_HAL::HAL& hal; + +template +void test_read(const TFMessage& msg) { + // read msg.transforms.size + hal.console->printf("%u", (uint32_t)transforms_size(msg)); + + // read msg.transforms[i] + double sum_x = 0.0; + for (size_t i = 0; i < transforms_size(msg); ++i) { + const auto& transform_stamped = transforms_data(msg)[i]; + sum_x += transform_stamped.transform.translation.x; + } + + hal.console->printf("%f", sum_x); +} + +template +void test_write(TFMessage& msg) { + // write msg.transforms.size + mutable_transforms_size(msg) = 0; + for (size_t i = 0; i < 4; ++i) { + mutable_transforms_size(msg)++; + } + hal.console->printf("%u", (uint32_t)transforms_size(msg)); + + // write msg.transforms[i] + auto* transform_stamped = mutable_transforms_data(msg); + transform_stamped[0].transform.translation.x = 10.0; + + hal.console->printf("%f", + transforms_data(msg)[0].transform.translation.x); +} + +void test_read(const tf2_msgs__msg__TFMessage& msg) { + test_read(msg); +} + +void test_write(tf2_msgs__msg__TFMessage& msg) { + test_write(msg); +} + +void test_write2(tf2_msgs__msg__TFMessage& msg) { + test_write(msg); +} +#endif + +#endif // AP_UROS_ENABLED diff --git a/libraries/AP_UROS/AP_UROS_Type_Conversions.h b/libraries/AP_UROS/AP_UROS_Type_Conversions.h new file mode 100644 index 00000000000000..34b873dbcbf52c --- /dev/null +++ b/libraries/AP_UROS/AP_UROS_Type_Conversions.h @@ -0,0 +1,76 @@ +// Class for handling type conversions for UROS. + +#pragma once + +#if AP_UROS_ENABLED + +#include + +#include +#include +#include +#include + +class AP_UROS_Type_Conversions +{ +public: + + // Convert ROS time to a uint64_t [μS] + static uint64_t time_u64_micros(const builtin_interfaces__msg__Time& ros_time); +}; + +// string specialisations +template <> +const char* string_data(const rosidl_runtime_c__String& str); + +template <> +char* mutable_string_data(rosidl_runtime_c__String& str); + +// transform specialisations +template <> +struct transforms_size_type { + typedef size_t type; +}; + +template <> +struct mutable_transforms_size_type { + typedef size_t& type; +}; + +template <> +typename transforms_size_type::type +transforms_size(const tf2_msgs__msg__TFMessage& msg); + +template <> +typename mutable_transforms_size_type::type +mutable_transforms_size(tf2_msgs__msg__TFMessage& msg); + +template <> +struct transforms_type { + typedef const geometry_msgs__msg__TransformStamped* type; +}; + +template <> +struct mutable_transforms_type { + typedef geometry_msgs__msg__TransformStamped* type; +}; + +template <> +typename transforms_type::type +transforms_data(const tf2_msgs__msg__TFMessage& msg); + +template <> +typename mutable_transforms_type::type +mutable_transforms_data(tf2_msgs__msg__TFMessage& msg); + +// cell_voltage specialisations +template <> +struct mutable_cell_voltage_type { + typedef float* type; +}; + +template <> +typename mutable_cell_voltage_type::type +mutable_cell_voltage_data(sensor_msgs__msg__BatteryState& msg); + +#endif // AP_UROS_ENABLED diff --git a/libraries/AP_UROS/AP_UROS_UDP.cpp b/libraries/AP_UROS/AP_UROS_UDP.cpp new file mode 100644 index 00000000000000..4ac902a10ec8e8 --- /dev/null +++ b/libraries/AP_UROS/AP_UROS_UDP.cpp @@ -0,0 +1,92 @@ +#include "AP_UROS_Client.h" + +#if AP_UROS_UDP_ENABLED + +#include + +#include + +/* + open connection on UDP + */ +bool AP_UROS_Client::udp_transport_open(uxrCustomTransport *t) +{ + AP_UROS_Client *uros = (AP_UROS_Client *)t->args; + auto *sock = new SocketAPM(true); + if (sock == nullptr) { + return false; + } + if (!sock->connect(uros->udp.ip, uros->udp.port.get())) { + return false; + } + uros->udp.socket = sock; + return true; +} + +/* + close UDP connection + */ +bool AP_UROS_Client::udp_transport_close(uxrCustomTransport *t) +{ + AP_UROS_Client *uros = (AP_UROS_Client *)t->args; + delete uros->udp.socket; + uros->udp.socket = nullptr; + return true; +} + +/* + write on UDP + */ +size_t AP_UROS_Client::udp_transport_write(uxrCustomTransport *t, + const uint8_t* buf, size_t len, uint8_t* error) +{ + AP_UROS_Client *uros = (AP_UROS_Client *)t->args; + if (uros->udp.socket == nullptr) { + *error = EINVAL; + return 0; + } + const ssize_t ret = uros->udp.socket->send(buf, len); + if (ret <= 0) { + *error = errno; + return 0; + } + return ret; +} + +/* + read from UDP + */ +size_t AP_UROS_Client::udp_transport_read(uxrCustomTransport *t, + uint8_t* buf, size_t len, int timeout_ms, uint8_t* error) +{ + AP_UROS_Client *uros = (AP_UROS_Client *)t->args; + if (uros->udp.socket == nullptr) { + *error = EINVAL; + return 0; + } + const ssize_t ret = uros->udp.socket->recv(buf, len, timeout_ms); + if (ret <= 0) { + *error = errno; + return 0; + } + return ret; +} + +/* + initialise UDP connection + */ +bool AP_UROS_Client::urosUdpInit() +{ + // setup a non-framed transport for UDP + rmw_ret_t rcl_ret = rmw_uros_set_custom_transport( + false, + (void*)this, + udp_transport_open, + udp_transport_close, + udp_transport_write, + udp_transport_read); + + return (rcl_ret == RCL_RET_OK); +} + +#endif // AP_UROS_UDP_ENABLED diff --git a/libraries/AP_UROS/AP_UROS_config.h b/libraries/AP_UROS/AP_UROS_config.h new file mode 100644 index 00000000000000..7b7da3a4afed93 --- /dev/null +++ b/libraries/AP_UROS/AP_UROS_config.h @@ -0,0 +1,33 @@ +#pragma once + +#include + +#ifndef AP_UROS_ENABLED +#define AP_UROS_ENABLED 1 +#endif // AP_UROS_ENABLED + +// UDP only on SITL for now +#ifndef AP_UROS_UDP_ENABLED +#define AP_UROS_UDP_ENABLED AP_UROS_ENABLED && (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#endif // AP_UROS_UDP_ENABLED + +#include +#ifndef AP_UROS_VISUALODOM_ENABLED +#define AP_UROS_VISUALODOM_ENABLED HAL_VISUALODOM_ENABLED && AP_UROS_ENABLED +#endif // AP_UROS_VISUALODOM_ENABLED + +// Only enable parameter server on SITL or EPS32 +#ifndef AP_UROS_PARAM_SRV_ENABLED +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_ESP32 +#define AP_UROS_PARAM_SRV_ENABLED 1 +#else +#define AP_UROS_PARAM_SRV_ENABLED 0 +#endif +#endif // AP_UROS_PARAM_SRV_ENABLED + +// esp32 - free-rtos +#if CONFIG_HAL_BOARD == HAL_BOARD_ESP32 +#include +#include +#include +#endif diff --git a/libraries/AP_UROS/README.md b/libraries/AP_UROS/README.md new file mode 100644 index 00000000000000..c44409d3c9bc85 --- /dev/null +++ b/libraries/AP_UROS/README.md @@ -0,0 +1,273 @@ +# AP_UROS: micro-ROS client library + +The `AP_UROS` library is an alternative to `AP_DDS` for direct ROS +integration in ArduPilot. It uses the micro-ROS client library rather +than the lower level XRCE-DDS-Client library. + +The library implements the same features as `AP_DDS` and may be used as +drop in replacement. + +## Build + +Build the ArduPilot binaries for SITL: + +```bash +./waf configure --board sitl --enable-uros +./waf build +``` + +Build as part of a `colcon` workspace: + +```bash +colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_CXX_STANDARD=17 --packages-select ardupilot_sitl +``` + +## Usage + +The most convenient method to bringup a simulation session using the `AP_UROS` +library is to launch the runway world available in [`ardupilot_gz`](https://github.com/ArduPilot/ardupilot_gz): + +```bash +ros2 launch ardupilot_gz_bringup iris_runway.launch.py rviz:=true use_gz_tf:=true +``` + +Attach a MAVProxy session and enable `UROS`: + +```bash +mavproxy.py --master udp:127.0.0.1:14550 --console --map +STABILIZE> param set UROS_ENABLE 1 +STABILIZE> param show UROS* +STABILIZE> UROS_ENABLE 1.0 +UROS_PORT 2019.0 +``` + +The console should display various status messages prefixed with `AP: UROS: ` +as the library is initialised. + +### ROS 2 features + +#### Nodes + +```bash +$ ros2 node list +/ardupilot_uros +``` + +```bash +$ ros2 node info /ardupilot_uros +/ardupilot_uros + Subscribers: + /ap/cmd_vel: geometry_msgs/msg/TwistStamped + /ap/joy: sensor_msgs/msg/Joy + /ap/tf: tf2_msgs/msg/TFMessage + Publishers: + /ap/battery/battery0: sensor_msgs/msg/BatteryState + /ap/clock: rosgraph_msgs/msg/Clock + /ap/geopose/filtered: geographic_msgs/msg/GeoPoseStamped + /ap/navsat/navsat0: sensor_msgs/msg/NavSatFix + /ap/pose/filtered: geometry_msgs/msg/PoseStamped + /ap/tf_static: tf2_msgs/msg/TFMessage + /ap/time: builtin_interfaces/msg/Time + /ap/twist/filtered: geometry_msgs/msg/TwistStamped + /parameter_events: rcl_interfaces/msg/ParameterEvent + Service Servers: + /ap/arm_motors: ardupilot_msgs/srv/ArmMotors + /ap/mode_switch: ardupilot_msgs/srv/ModeSwitch + /ardupilot_uros/describe_parameters: rcl_interfaces/srv/DescribeParameters + /ardupilot_uros/get_parameter_types: rcl_interfaces/srv/GetParameterTypes + /ardupilot_uros/get_parameters: rcl_interfaces/srv/GetParameters + /ardupilot_uros/list_parameters: rcl_interfaces/srv/ListParameters + /ardupilot_uros/set_parameters: rcl_interfaces/srv/SetParameters + Service Clients: + + Action Servers: + + Action Clients: + ``` + +#### Topics + +List topics published by `ardupilot_uros`: + +```bash +% ros2 topic list | grep /ap +/ap/battery/battery0 +/ap/clock +/ap/cmd_vel +/ap/geopose/filtered +/ap/joy +/ap/navsat/navsat0 +/ap/pose/filtered +/ap/tf +/ap/tf_static +/ap/time +/ap/twist/filtered +``` + +Subscribe to a filtered pose: + +```bash +% ros2 topic echo /ap/pose/filtered --once +header: + stamp: + sec: 1696260581 + nanosec: 150658000 + frame_id: base_link +pose: + position: + x: -0.009078041650354862 + y: 0.011131884530186653 + z: 0.10999999940395355 + orientation: + x: -0.0008264112402684987 + y: 6.37705234112218e-05 + z: 0.7135183215141296 + w: 0.700636088848114 +--- +``` + +Publish a `Joy` message: + +```bash +$ ros2 topic pub /ap/joy sensor_msgs/msg/Joy "{axes: [1, 1, 1, 1]}" --once +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=[1.0, 1.0, 1.0, 1.0], buttons=[]) +``` + +The MAVProxy console should show: + +```console +AP: UROS: sensor_msgs/Joy: 1.000000, 1.000000, 1.000000, 1.000000 +``` + +#### Services + +```bash +$ ros2 service list | grep /ap +/ap/arm_motors +/ap/mode_switch +``` + +Switch to `GUIDED` mode (`mode=4`): + +```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) +``` + +```console +Mode GUIDED +AP: UROS: Request for Mode Switch : SUCCESS +``` + +Arm motors: + +```bash +ros2 service call /ap/arm_motors ardupilot_msgs/srv/ArmMotors "{arm: true}" +requester: making request: ardupilot_msgs.srv.ArmMotors_Request(arm=True) + +response: +ardupilot_msgs.srv.ArmMotors_Response(result=True) +``` + +```console +AP: UROS: Request for arming received +AP: Arming motors +AP: UROS: Request for Arming/Disarming : SUCCESS +ARMED +``` + +#### Parameters + +The parameter server is proof of concept only and is not integrated +with ArduPilot's parameter system. + +Create: + +```bash +ros2 param set /ardupilot_uros GPS_TYPE 11 +``` + +Dump: + +```bash +ros2 param dump /ardupilot_uros +/ardupilot_uros: + ros__parameters: + GPS_TYPE: 11 +``` + +Get: + +```bash +$ ros2 param get /ardupilot_uros GPS_TYPE +Integer value is: 11 +``` + +Set: + +```bash +$ ros2 param get /ardupilot_uros GPS_TYPE +Integer value is: 11 +``` + +```bash +$ ros2 param set /ardupilot_uros GPS_TYPE 1 +Set parameter successful + +```bash +$ ros2 param get /ardupilot_uros GPS_TYPE +Integer value is: 1 +``` + + +## Appendix A: ESP32 + +Notes for building the micro-ROS client library using the package +`micro_ros_espidf_component`. + +### Config + +- Update `app-colcon.meta` to allow more publishers and subscribers. + - 1 node + - 16 pub/sub/service/client +- Update `libmicroros.mk` to add message and service interface packages. + - Added `ardupilot` for `ardupilot_msgs` + - Added `geographic_info` for `geographic_msgs` + - Added `geometry2` for `tf2_msgs` + - Added `ardupilot` for `ardupilot_msgs` + +### Build (standalone example) + +This refers to a standalone example used to test the micro-ROS client build. + +```bash +cd ./firmware/toolchain +. ./esp-idf/export.sh +``` + +```bash +cd ./firmware/esp32_examples/ardupilot_uros +idf.py set-target esp32 +idf.py build +idf.py -p /dev/cu.usbserial-0001 flash +``` + +### Run + +```bash +cd ./firmware/esp32_examples/ardupilot_uros +idf.py monitor +``` + +```bash +cd ./firmware/esp32_examples/ardupilot_uros +idf.py monitor +``` + +```bash +ros2 run micro_ros_agent micro_ros_agent udp4 --port 2019 -v6 +``` \ No newline at end of file diff --git a/libraries/AP_UROS/wscript b/libraries/AP_UROS/wscript new file mode 100644 index 00000000000000..fb1362bffe7011 --- /dev/null +++ b/libraries/AP_UROS/wscript @@ -0,0 +1,82 @@ +#!/usr/bin/env python3 + +import pathlib +import platform + + +def configure(cfg): + host_system = platform.system() + host_processor = platform.processor() + host_machine = platform.machine() + host_release = platform.release() # kernel version + toolchain = cfg.env.TOOLCHAIN + + print('host_system: {}'.format(host_system)) + print('host_processor: {}'.format(host_processor)) + print('host_machine: {}'.format(host_machine)) + print('host_release: {}'.format(host_release)) + print('toolchain: {}'.format(toolchain)) + + # suggestion for alternative package names + # - uros-darwin-22.6.0-x86_64 + # - uros-darwin-22.5.0-arm64 + + distro = None + if toolchain == 'native': + if host_system == 'Darwin': + if host_machine == 'x86_64': + distro = 'uros-macos-13.4-x86_64' + elif host_machine == 'arm64': + distro = 'uros-macos-13.4-arm64' + elif host_system == 'Linux': + if host_machine == 'x86_64': + distro = 'uros-ubuntu-22-x86_64' + elif host_machine == 'aarch64': + distro = 'uros-ubuntu-22-aarch64' + else: + print('Cannot configure AP_UROS. Unsupported host: {}'.format(host_system)) + return + elif toolchain == 'arm-none-eabi': + distro = 'uros-stm32-m7-custom' + elif toolchain == 'xtensa-esp32-elf': + distro = 'uros-esp-idf-4.4-custom' + else: + print('Cannot configure AP_UROS. Unsupported toolchain: {}'.format(toolchain)) + return + + uros_include_dir = str(pathlib.PurePath('modules', 'ardupilot_uros', distro, 'uros', 'include')) + uros_library_dir = str(pathlib.PurePath('modules', 'ardupilot_uros', distro, 'uros', 'lib')) + cfg.env.UROS_INCLUDE_DIR = uros_include_dir + cfg.env.UROS_LIBRARY_DIR = uros_library_dir + + extra_src_inc = [ + f'{uros_include_dir}', + f'{uros_include_dir}/rcl', + f'{uros_include_dir}/rcl_action', + f'{uros_include_dir}/rcl_interfaces', + f'{uros_include_dir}/rcutils', + f'{uros_include_dir}/rmw', + f'{uros_include_dir}/rosidl_runtime_c', + f'{uros_include_dir}/rosidl_typesupport_interface', + f'{uros_include_dir}/rosidl_typesupport_introspection_c', + f'{uros_include_dir}/action_msgs', + f'{uros_include_dir}/ardupilot_msgs', + f'{uros_include_dir}/builtin_interfaces', + f'{uros_include_dir}/geographic_msgs', + f'{uros_include_dir}/geometry_msgs', + f'{uros_include_dir}/rosgraph_msgs', + f'{uros_include_dir}/sensor_msgs', + f'{uros_include_dir}/std_msgs', + f'{uros_include_dir}/tf2_msgs', + f'{uros_include_dir}/unique_identifier_msgs', + ] + for inc in extra_src_inc: + cfg.env.INCLUDES += [str(cfg.srcnode.make_node(inc))] + + # auto update submodules + cfg.env.append_value('GIT_SUBMODULES', 'ardupilot_uros') + + +def build(bld): + bld.env.LIB += ['microros'] + bld.env.LIBPATH += [str(pathlib.PurePath(bld.env.SRCROOT, bld.env.UROS_LIBRARY_DIR))] diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index ff333b0a424ba3..ec2deaaeb998dd 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -18,6 +18,7 @@ #include #endif #include +#include #if HAL_WITH_IO_MCU #include extern AP_IOMCU iomcu; @@ -205,6 +206,11 @@ const AP_Param::GroupInfo AP_Vehicle::var_info[] = { AP_GROUPINFO("FLTMODE_GCSBLOCK", 20, AP_Vehicle, flight_mode_GCS_block, 0), #endif // APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_Rover) +#if AP_UROS_ENABLED + // @Group: UROS + // @Path: ../AP_UROS/AP_UROS_Client.cpp + AP_SUBGROUPPTR(uros_client, "UROS", 21, AP_Vehicle, AP_UROS_Client), +#endif // AP_UROS_ENABLED #if AP_NETWORKING_ENABLED // @Group: NET_ @@ -407,6 +413,12 @@ void AP_Vehicle::setup() GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "DDS Client: Failed to Initialize"); } #endif + +#if AP_UROS_ENABLED + if (!init_uros_client()) { + gcs().send_text(MAV_SEVERITY_ERROR, "UROS: Failed to initialize"); + } +#endif } void AP_Vehicle::loop() @@ -945,6 +957,17 @@ bool AP_Vehicle::init_dds_client() } #endif // AP_DDS_ENABLED +#if AP_UROS_ENABLED +bool AP_Vehicle::init_uros_client() +{ + uros_client = new AP_UROS_Client(); + if (uros_client == nullptr) { + return false; + } + return uros_client->start(); +} +#endif // AP_UROS_ENABLED + // Check if this mode can be entered from the GCS #if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_Rover) bool AP_Vehicle::block_GCS_mode_change(uint8_t mode_num, const uint8_t *mode_list, uint8_t mode_list_length) const diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index 18b43fc0209ca8..faafa119c2cb86 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -64,6 +64,7 @@ #include class AP_DDS_Client; +class AP_UROS_Client; class AP_Vehicle : public AP_HAL::HAL::Callbacks { @@ -427,6 +428,12 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { bool init_dds_client() WARN_IF_UNUSED; #endif +#if AP_UROS_ENABLED + // Declare the micro-ros client for communication with ROS 2 and DDS (common for all vehicles) + AP_UROS_Client *uros_client; + bool init_uros_client() WARN_IF_UNUSED; +#endif + // Check if this mode can be entered from the GCS bool block_GCS_mode_change(uint8_t mode_num, const uint8_t *mode_list, uint8_t mode_list_length) const; diff --git a/modules/ardupilot_uros b/modules/ardupilot_uros new file mode 160000 index 00000000000000..a898c6a836147a --- /dev/null +++ b/modules/ardupilot_uros @@ -0,0 +1 @@ +Subproject commit a898c6a836147afbc1400d4029119f70f561d9df diff --git a/wscript b/wscript index db52f181a7ddd4..7441451a1c1338 100644 --- a/wscript +++ b/wscript @@ -267,8 +267,10 @@ submodules at specific revisions. help="Enables GPS logging") g.add_option('--enable-dds', action='store_true', - help="Enable the dds client to connect with ROS2/DDS" - ) + help="Enable the dds client to connect with ROS2/DDS") + + g.add_option('--enable-uros', action='store_true', + help="Enable the micro-ros client to connect with ROS2/DDS") g.add_option('--enable-dronecan-tests', action='store_true', default=False, @@ -720,6 +722,9 @@ def _build_dynamic_sources(bld): if bld.env.ENABLE_DDS: bld.recurse("libraries/AP_DDS") + if bld.env.ENABLE_UROS: + bld.recurse("libraries/AP_UROS") + def write_version_header(tsk): bld = tsk.generator.bld return bld.write_version_header(tsk.outputs[0].abspath())