fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Remove uorb topics exclusively used for avoidance

Browse Source 
- TrajectoryBezier.msg
- TrajectoryWaypoint.msg
- VehicleTrajectoryBezier.msg
- VehicleTrajectoryWaypoint.msg

Additionally remove TRAJECTORY_REPRESENTATION_WAYPOINTS mavlink stream.

Signed-off-by: Silvan <silvan@auterion.com>
This commit is contained in:
Silvan 2025-01-10 13:11:23 +01:00 committed by Silvan Fuhrer
parent b7b6d45e18
commit b916a96e00
14 changed files with 0 additions and 296 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
msg/CMakeLists.txt
View File

@ -214,8 +214,6 @@ set(msg_files
TelemetryStatus.msg
TiltrotorExtraControls.msg
TimesyncStatus.msg
TrajectoryBezier.msg
TrajectoryWaypoint.msg
TransponderReport.msg
TuneControl.msg
UavcanParameterRequest.msg
@ -235,8 +233,6 @@ set(msg_files
VehicleRoi.msg
VehicleThrustSetpoint.msg
VehicleTorqueSetpoint.msg
VehicleTrajectoryBezier.msg
VehicleTrajectoryWaypoint.msg
VelocityLimits.msg
WheelEncoders.msg
Wind.msg

8
msg/TrajectoryBezier.msg
View File

@ -1,8 +0,0 @@
# Bezier Trajectory description. See also Mavlink TRAJECTORY msg
# The topic trajectory_bezier describe each waypoint defined in vehicle_trajectory_bezier
uint64 timestamp # time since system start (microseconds)
float32[3] position # local position x,y,z (metres)
float32 yaw # yaw angle (rad)
float32 delta # time it should take to get to this waypoint, if this is the final waypoint (seconds)

13
msg/TrajectoryWaypoint.msg
View File

@ -1,13 +0,0 @@
# Waypoint Trajectory description. See also Mavlink TRAJECTORY msg
# The topic trajectory_waypoint describe each waypoint defined in vehicle_trajectory_waypoint
uint64 timestamp # time since system start (microseconds)
float32[3] position
float32[3] velocity
float32[3] acceleration
float32 yaw
float32 yaw_speed
bool point_valid
uint8 type

18
msg/VehicleTrajectoryBezier.msg
View File

@ -1,18 +0,0 @@
# Vehicle Waypoints Trajectory description. See also MAVLink MAV_TRAJECTORY_REPRESENTATION msg
# The topic vehicle_trajectory_bezier is used to send a smooth flight path from the
# companion computer / avoidance module to the position controller.
uint64 timestamp # time since system start (microseconds)
uint8 POINT_0 = 0
uint8 POINT_1 = 1
uint8 POINT_2 = 2
uint8 POINT_3 = 3
uint8 POINT_4 = 4
uint8 NUMBER_POINTS = 5
TrajectoryBezier[5] control_points
uint8 bezier_order
# TOPICS vehicle_trajectory_bezier

21
msg/VehicleTrajectoryWaypoint.msg
View File

@ -1,21 +0,0 @@
# Vehicle Waypoints Trajectory description. See also MAVLink MAV_TRAJECTORY_REPRESENTATION msg
# The topic vehicle_trajectory_waypoint_desired is used to send the user desired waypoints from the position controller to the companion computer / avoidance module.
# The topic vehicle_trajectory_waypoint is used to send the adjusted waypoints from the companion computer / avoidance module to the position controller.
uint64 timestamp # time since system start (microseconds)
uint8 MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS = 0
uint8 type # Type from MAV_TRAJECTORY_REPRESENTATION enum.
uint8 POINT_0 = 0
uint8 POINT_1 = 1
uint8 POINT_2 = 2
uint8 POINT_3 = 3
uint8 POINT_4 = 4
uint8 NUMBER_POINTS = 5
TrajectoryWaypoint[5] waypoints
# TOPICS vehicle_trajectory_waypoint vehicle_trajectory_waypoint_desired

2
src/modules/logger/logged_topics.cpp
View File

@ -335,8 +335,6 @@ void LoggedTopics::add_vision_and_avoidance_topics()
add_topic("obstacle_distance_fused");
add_topic("obstacle_distance");
add_topic("vehicle_mocap_odometry", 30);
add_topic("vehicle_trajectory_waypoint", 200);
add_topic("vehicle_trajectory_waypoint_desired", 200);
add_topic("vehicle_visual_odometry", 30);
}

3
src/modules/mavlink/mavlink_main.cpp
View File

@ -1525,7 +1525,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("SYS_STATUS", 5.0f);
configure_stream_local("SYSTEM_TIME", 1.0f);
configure_stream_local("TIME_ESTIMATE_TO_TARGET", 1.0f);
configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f);
configure_stream_local("VFR_HUD", 10.0f);
configure_stream_local("VIBRATION", 0.5f);
configure_stream_local("WIND_COV", 10.0f);
@ -1592,7 +1591,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("RC_CHANNELS", 5.0f);
configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f);
configure_stream_local("SYS_STATUS", 5.0f);
configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f);
configure_stream_local("VFR_HUD", 4.0f);
configure_stream_local("VIBRATION", 0.5f);
configure_stream_local("WIND_COV", 1.0f);
@ -1784,7 +1782,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("SYS_STATUS", 5.0f);
configure_stream_local("SYSTEM_TIME", 2.0f);
configure_stream_local("TIME_ESTIMATE_TO_TARGET", 1.0f);
configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f);
configure_stream_local("VFR_HUD", 4.0f);
configure_stream_local("VIBRATION", 0.5f);
configure_stream_local("WIND_COV", 1.0f);

4
src/modules/mavlink/mavlink_messages.cpp
View File

@ -116,7 +116,6 @@
#include "streams/SYSTEM_TIME.hpp"
#include "streams/TIME_ESTIMATE_TO_TARGET.hpp"
#include "streams/TIMESYNC.hpp"
#include "streams/TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp"
#include "streams/VFR_HUD.hpp"
#include "streams/VIBRATION.hpp"
#include "streams/WIND_COV.hpp"
@ -382,9 +381,6 @@ static const StreamListItem streams_list[] = {
#if defined(MANUAL_CONTROL_HPP)
create_stream_list_item<MavlinkStreamManualControl>(),
#endif // MANUAL_CONTROL_HPP
#if defined(TRAJECTORY_REPRESENTATION_WAYPOINTS_HPP)
create_stream_list_item<MavlinkStreamTrajectoryRepresentationWaypoints>(),
#endif // TRAJECTORY_REPRESENTATION_WAYPOINTS_HPP
#if defined(OPTICAL_FLOW_RAD_HPP)
create_stream_list_item<MavlinkStreamOpticalFlowRad>(),
#endif // OPTICAL_FLOW_RAD_HPP

66
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -256,14 +256,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_tunnel(msg);
break;
case MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER:
handle_message_trajectory_representation_bezier(msg);
break;
case MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS:
handle_message_trajectory_representation_waypoints(msg);
break;
case MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS:
handle_message_onboard_computer_status(msg);
break;
@ -1988,64 +1980,6 @@ MavlinkReceiver::handle_message_tunnel(mavlink_message_t *msg)
}
void
MavlinkReceiver::handle_message_trajectory_representation_bezier(mavlink_message_t *msg)
{
mavlink_trajectory_representation_bezier_t trajectory;
mavlink_msg_trajectory_representation_bezier_decode(msg, &trajectory);
vehicle_trajectory_bezier_s trajectory_bezier{};
trajectory_bezier.timestamp = _mavlink_timesync.sync_stamp(trajectory.time_usec);
for (int i = 0; i < vehicle_trajectory_bezier_s::NUMBER_POINTS; ++i) {
trajectory_bezier.control_points[i].position[0] = trajectory.pos_x[i];
trajectory_bezier.control_points[i].position[1] = trajectory.pos_y[i];
trajectory_bezier.control_points[i].position[2] = trajectory.pos_z[i];
trajectory_bezier.control_points[i].delta = trajectory.delta[i];
trajectory_bezier.control_points[i].yaw = trajectory.pos_yaw[i];
}
trajectory_bezier.bezier_order = math::min(trajectory.valid_points, vehicle_trajectory_bezier_s::NUMBER_POINTS);
_trajectory_bezier_pub.publish(trajectory_bezier);
}
void
MavlinkReceiver::handle_message_trajectory_representation_waypoints(mavlink_message_t *msg)
{
mavlink_trajectory_representation_waypoints_t trajectory;
mavlink_msg_trajectory_representation_waypoints_decode(msg, &trajectory);
vehicle_trajectory_waypoint_s trajectory_waypoint{};
const int number_valid_points = math::min(trajectory.valid_points, vehicle_trajectory_waypoint_s::NUMBER_POINTS);
for (int i = 0; i < number_valid_points; ++i) {
trajectory_waypoint.waypoints[i].position[0] = trajectory.pos_x[i];
trajectory_waypoint.waypoints[i].position[1] = trajectory.pos_y[i];
trajectory_waypoint.waypoints[i].position[2] = trajectory.pos_z[i];
trajectory_waypoint.waypoints[i].velocity[0] = trajectory.vel_x[i];
trajectory_waypoint.waypoints[i].velocity[1] = trajectory.vel_y[i];
trajectory_waypoint.waypoints[i].velocity[2] = trajectory.vel_z[i];
trajectory_waypoint.waypoints[i].acceleration[0] = trajectory.acc_x[i];
trajectory_waypoint.waypoints[i].acceleration[1] = trajectory.acc_y[i];
trajectory_waypoint.waypoints[i].acceleration[2] = trajectory.acc_z[i];
trajectory_waypoint.waypoints[i].yaw = trajectory.pos_yaw[i];
trajectory_waypoint.waypoints[i].yaw_speed = trajectory.vel_yaw[i];
trajectory_waypoint.waypoints[i].point_valid = true;
trajectory_waypoint.waypoints[i].type = UINT8_MAX;
}
trajectory_waypoint.timestamp = hrt_absolute_time();
_trajectory_waypoint_pub.publish(trajectory_waypoint);
}
void
MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg)
{

6
src/modules/mavlink/mavlink_receiver.h
View File

@ -110,8 +110,6 @@
#include <uORB/topics/vehicle_odometry.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_trajectory_bezier.h>
#include <uORB/topics/vehicle_trajectory_waypoint.h>
#include <uORB/topics/velocity_limits.h>
#if !defined(CONSTRAINED_FLASH)
@ -201,8 +199,6 @@ private:
void handle_message_set_position_target_local_ned(mavlink_message_t *msg);
void handle_message_statustext(mavlink_message_t *msg);
void handle_message_tunnel(mavlink_message_t *msg);
void handle_message_trajectory_representation_bezier(mavlink_message_t *msg);
void handle_message_trajectory_representation_waypoints(mavlink_message_t *msg);
void handle_message_utm_global_position(mavlink_message_t *msg);
#if defined(MAVLINK_MSG_ID_SET_VELOCITY_LIMITS) // For now only defined if development.xml is used
void handle_message_set_velocity_limits(mavlink_message_t *msg);
@ -329,8 +325,6 @@ private:
uORB::Publication<vehicle_odometry_s> _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)};
uORB::Publication<vehicle_odometry_s> _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
uORB::Publication<vehicle_rates_setpoint_s> _rates_sp_pub{ORB_ID(vehicle_rates_setpoint)};
uORB::Publication<vehicle_trajectory_bezier_s> _trajectory_bezier_pub{ORB_ID(vehicle_trajectory_bezier)};
uORB::Publication<vehicle_trajectory_waypoint_s> _trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)};
#if !defined(CONSTRAINED_FLASH)
uORB::Publication<debug_array_s> _debug_array_pub {ORB_ID(debug_array)};

124
src/modules/mavlink/streams/TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp
View File

@ -1,124 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#ifndef TRAJECTORY_REPRESENTATION_WAYPOINTS_HPP
#define TRAJECTORY_REPRESENTATION_WAYPOINTS_HPP
#include <uORB/topics/vehicle_trajectory_waypoint.h>
class MavlinkStreamTrajectoryRepresentationWaypoints: public MavlinkStream
{
public:
static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamTrajectoryRepresentationWaypoints(mavlink); }
const char *get_name() const override { return get_name_static(); }
uint16_t get_id() override { return get_id_static(); }
static constexpr const char *get_name_static() { return "TRAJECTORY_REPRESENTATION_WAYPOINTS"; }
static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS; }
unsigned get_size() override
{
if (_traj_wp_avoidance_sub.advertised()) {
return MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
return 0;
}
private:
explicit MavlinkStreamTrajectoryRepresentationWaypoints(Mavlink *mavlink) : MavlinkStream(mavlink) {}
uORB::Subscription _traj_wp_avoidance_sub{ORB_ID(vehicle_trajectory_waypoint_desired)};
bool send() override
{
vehicle_trajectory_waypoint_s traj_wp_avoidance_desired;
if (_traj_wp_avoidance_sub.update(&traj_wp_avoidance_desired)) {
mavlink_trajectory_representation_waypoints_t msg{};
msg.time_usec = traj_wp_avoidance_desired.timestamp;
int number_valid_points = 0;
for (int i = 0; i < vehicle_trajectory_waypoint_s::NUMBER_POINTS; ++i) {
msg.pos_x[i] = traj_wp_avoidance_desired.waypoints[i].position[0];
msg.pos_y[i] = traj_wp_avoidance_desired.waypoints[i].position[1];
msg.pos_z[i] = traj_wp_avoidance_desired.waypoints[i].position[2];
msg.vel_x[i] = traj_wp_avoidance_desired.waypoints[i].velocity[0];
msg.vel_y[i] = traj_wp_avoidance_desired.waypoints[i].velocity[1];
msg.vel_z[i] = traj_wp_avoidance_desired.waypoints[i].velocity[2];
msg.acc_x[i] = traj_wp_avoidance_desired.waypoints[i].acceleration[0];
msg.acc_y[i] = traj_wp_avoidance_desired.waypoints[i].acceleration[1];
msg.acc_z[i] = traj_wp_avoidance_desired.waypoints[i].acceleration[2];
msg.pos_yaw[i] = traj_wp_avoidance_desired.waypoints[i].yaw;
msg.vel_yaw[i] = traj_wp_avoidance_desired.waypoints[i].yaw_speed;
switch (traj_wp_avoidance_desired.waypoints[i].type) {
case position_setpoint_s::SETPOINT_TYPE_TAKEOFF:
msg.command[i] = vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF;
break;
case position_setpoint_s::SETPOINT_TYPE_LOITER:
msg.command[i] = vehicle_command_s::VEHICLE_CMD_NAV_LOITER_UNLIM;
break;
case position_setpoint_s::SETPOINT_TYPE_LAND:
msg.command[i] = vehicle_command_s::VEHICLE_CMD_NAV_LAND;
break;
default:
msg.command[i] = UINT16_MAX;
}
if (traj_wp_avoidance_desired.waypoints[i].point_valid) {
number_valid_points++;
}
}
msg.valid_points = number_valid_points;
mavlink_msg_trajectory_representation_waypoints_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
#endif // TRAJECTORY_REPRESENTATION_WAYPOINTS_HPP

9
src/modules/uxrce_dds_client/dds_topics.yaml
View File

@ -71,9 +71,6 @@ publications:
- topic: /fmu/out/vehicle_status
type: px4_msgs::msg::VehicleStatus
- topic: /fmu/out/vehicle_trajectory_waypoint_desired
type: px4_msgs::msg::VehicleTrajectoryWaypoint
- topic: /fmu/out/airspeed_validated
type: px4_msgs::msg::AirspeedValidated
@ -145,12 +142,6 @@ subscriptions:
- topic: /fmu/in/vehicle_command_mode_executor
type: px4_msgs::msg::VehicleCommand
- topic: /fmu/in/vehicle_trajectory_bezier
type: px4_msgs::msg::VehicleTrajectoryBezier
- topic: /fmu/in/vehicle_trajectory_waypoint
type: px4_msgs::msg::VehicleTrajectoryWaypoint
- topic: /fmu/in/vehicle_thrust_setpoint
type: px4_msgs::msg::VehicleThrustSetpoint

3
src/modules/zenoh/Kconfig
View File

@ -46,9 +46,6 @@ if MODULES_ZENOH
select ZENOH_PUBSUB_VEHICLE_ODOMETRY
select ZENOH_PUBSUB_VEHICLE_RATES_SETPOINT
select ZENOH_PUBSUB_VEHICLE_COMMAND
select ZENOH_PUBSUB_VEHICLE_TRAJECTORY_BEZIER
select ZENOH_PUBSUB_VEHICLE_TRAJECTORY_WAYPOINT
config ZENOH_PUBSUB_ALL
bool "All"

15
src/modules/zenoh/Kconfig.topics
View File

@ -741,10 +741,6 @@ menu "Zenoh publishers/subscribers"
bool "timesync_status"
default n
config ZENOH_PUBSUB_TRAJECTORY_BEZIER
bool "trajectory_bezier"
default n
config ZENOH_PUBSUB_TRAJECTORY_SETPOINT
bool "trajectory_setpoint"
default n
@ -881,14 +877,6 @@ menu "Zenoh publishers/subscribers"
bool "vehicle_torque_setpoint"
default n
config ZENOH_PUBSUB_VEHICLE_TRAJECTORY_BEZIER
bool "vehicle_trajectory_bezier"
default n
config ZENOH_PUBSUB_VEHICLE_TRAJECTORY_WAYPOINT
bool "vehicle_trajectory_waypoint"
default n
config ZENOH_PUBSUB_VELOCITY_LIMITS
bool "velocity_limits"
default n
@ -1099,7 +1087,6 @@ config ZENOH_PUBSUB_ALL_SELECTION
select ZENOH_PUBSUB_TELEMETRY_STATUS
select ZENOH_PUBSUB_TILTROTOR_EXTRA_CONTROLS
select ZENOH_PUBSUB_TIMESYNC_STATUS
select ZENOH_PUBSUB_TRAJECTORY_BEZIER
select ZENOH_PUBSUB_TRAJECTORY_SETPOINT
select ZENOH_PUBSUB_TRAJECTORY_WAYPOINT
select ZENOH_PUBSUB_TRANSPONDER_REPORT
@ -1134,8 +1121,6 @@ config ZENOH_PUBSUB_ALL_SELECTION
select ZENOH_PUBSUB_VEHICLE_STATUS
select ZENOH_PUBSUB_VEHICLE_THRUST_SETPOINT
select ZENOH_PUBSUB_VEHICLE_TORQUE_SETPOINT
select ZENOH_PUBSUB_VEHICLE_TRAJECTORY_BEZIER
select ZENOH_PUBSUB_VEHICLE_TRAJECTORY_WAYPOINT
select ZENOH_PUBSUB_VELOCITY_LIMITS
select ZENOH_PUBSUB_VTOL_VEHICLE_STATUS
select ZENOH_PUBSUB_WHEEL_ENCODERS