yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

CollisionPrevention: update orb_publish to uORB::Publication<>

Browse Source
This commit is contained in:
Daniel Agar
2019-09-02 15:35:34 -04:00
committed by GitHub
parent 9f5f9c577e
commit b0f82ee8ac
2 changed files with 31 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files

66
src/lib/CollisionPrevention/CollisionPrevention.cpp
View File

@@ -56,51 +56,6 @@ CollisionPrevention::~CollisionPrevention()
if (_mavlink_log_pub != nullptr) {
orb_unadvertise(_mavlink_log_pub);
}
if (_constraints_pub != nullptr) {
orb_unadvertise(_constraints_pub);
}
if (_obstacle_distance_pub != nullptr) {
orb_unadvertise(_obstacle_distance_pub);
}
if (_pub_vehicle_command != nullptr) {
orb_unadvertise(_pub_vehicle_command);
}
}
void CollisionPrevention::_publishConstrainedSetpoint(const Vector2f &original_setpoint,
const Vector2f &adapted_setpoint)
{
collision_constraints_s constraints{}; /**< collision constraints message */
//fill in values
constraints.timestamp = hrt_absolute_time();
constraints.original_setpoint[0] = original_setpoint(0);
constraints.original_setpoint[1] = original_setpoint(1);
constraints.adapted_setpoint[0] = adapted_setpoint(0);
constraints.adapted_setpoint[1] = adapted_setpoint(1);
// publish constraints
if (_constraints_pub != nullptr) {
orb_publish(ORB_ID(collision_constraints), _constraints_pub, &constraints);
} else {
_constraints_pub = orb_advertise(ORB_ID(collision_constraints), &constraints);
}
}
void CollisionPrevention::_publishObstacleDistance(obstacle_distance_s &obstacle)
{
// publish fused obtacle distance message with data from offboard obstacle_distance and distance sensor
if (_obstacle_distance_pub != nullptr) {
orb_publish(ORB_ID(obstacle_distance_fused), _obstacle_distance_pub, &obstacle);
} else {
_obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance_fused), &obstacle);
}
}
void CollisionPrevention::_updateOffboardObstacleDistance(obstacle_distance_s &obstacle)
@@ -197,7 +152,8 @@ void CollisionPrevention::_updateDistanceSensor(obstacle_distance_s &obstacle)
}
}
_publishObstacleDistance(obstacle);
// publish fused obtacle distance message with data from offboard obstacle_distance and distance sensor
_obstacle_distance_pub.publish(obstacle);
}
void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint,
@@ -298,13 +254,21 @@ void CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const floa
}
_interfering = currently_interfering;
_publishConstrainedSetpoint(original_setpoint, new_setpoint);
// publish constraints
collision_constraints_s constraints{};
constraints.timestamp = hrt_absolute_time();
original_setpoint.copyTo(constraints.original_setpoint);
new_setpoint.copyTo(constraints.adapted_setpoint);
_constraints_pub.publish(constraints);
original_setpoint = new_setpoint;
}
void CollisionPrevention::_publishVehicleCmdDoLoiter()
{
vehicle_command_s command{};
command.timestamp = hrt_absolute_time();
command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
command.param1 = (float)1; // base mode
command.param3 = (float)0; // sub mode
@@ -318,11 +282,5 @@ void CollisionPrevention::_publishVehicleCmdDoLoiter()
command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
// publish the vehicle command
if (_pub_vehicle_command == nullptr) {
_pub_vehicle_command = orb_advertise_queue(ORB_ID(vehicle_command), &command,
vehicle_command_s::ORB_QUEUE_LENGTH);
} else {
orb_publish(ORB_ID(vehicle_command), _pub_vehicle_command, &command);
}
_vehicle_command_pub.publish(command);
}

42
src/lib/CollisionPrevention/CollisionPrevention.hpp
View File

@@ -41,20 +41,23 @@
#pragma once
#include <px4_module_params.h>
#include <float.h>
#include <commander/px4_custom_mode.h>
#include <drivers/drv_hrt.h>
#include <mathlib/mathlib.h>
#include <matrix/matrix/math.hpp>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/obstacle_distance.h>
#include <px4_module_params.h>
#include <systemlib/mavlink_log.h>
#include <uORB/Publication.hpp>
#include <uORB/PublicationQueued.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/collision_constraints.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/mavlink_log.h>
#include <uORB/topics/obstacle_distance.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_command.h>
#include <mathlib/mathlib.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <systemlib/mavlink_log.h>
#include <commander/px4_custom_mode.h>
class CollisionPrevention : public ModuleParams
{
@@ -81,10 +84,11 @@ private:
bool _interfering{false}; /**< states if the collision prevention interferes with the user input */
orb_advert_t _constraints_pub{nullptr}; /**< constraints publication */
orb_advert_t _mavlink_log_pub{nullptr}; /**< Mavlink log uORB handle */
orb_advert_t _obstacle_distance_pub{nullptr}; /**< obstacle_distance publication */
orb_advert_t _pub_vehicle_command{nullptr}; /**< vehicle command do publication */
uORB::Publication<collision_constraints_s> _constraints_pub{ORB_ID(collision_constraints)}; /**< constraints publication */
uORB::Publication<obstacle_distance_s> _obstacle_distance_pub{ORB_ID(obstacle_distance_fused)}; /**< obstacle_distance publication */
uORB::PublicationQueued<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)}; /**< vehicle command do publication */
uORB::SubscriptionData<obstacle_distance_s> _sub_obstacle_distance{ORB_ID(obstacle_distance)}; /**< obstacle distances received form a range sensor */
uORB::Subscription _sub_distance_sensor[ORB_MULTI_MAX_INSTANCES] {{ORB_ID(distance_sensor), 0}, {ORB_ID(distance_sensor), 1}, {ORB_ID(distance_sensor), 2}, {ORB_ID(distance_sensor), 3}}; /**< distance data received from onboard rangefinders */
@@ -140,27 +144,19 @@ private:
*/
void _calculateConstrainedSetpoint(matrix::Vector2f &setpoint, const matrix::Vector2f &curr_pos,
const matrix::Vector2f &curr_vel);
/**
* Publishes collision_constraints message
* @param original_setpoint, setpoint before collision prevention intervention
* @param adapted_setpoint, collision prevention adaped setpoint
*/
void _publishConstrainedSetpoint(const matrix::Vector2f &original_setpoint, const matrix::Vector2f &adapted_setpoint);
/**
* Publishes obstacle_distance message with fused data from offboard and from distance sensors
* @param obstacle, obstacle_distance message to be publsihed
*/
void _publishObstacleDistance(obstacle_distance_s &obstacle);
/**
* Updates obstacle distance message with measurement from offboard
* @param obstacle, obstacle_distance message to be updated
*/
void _updateOffboardObstacleDistance(obstacle_distance_s &obstacle);
/**
* Updates obstacle distance message with measurement from distance sensors
* @param obstacle, obstacle_distance message to be updated
*/
void _updateDistanceSensor(obstacle_distance_s &obstacle);
/**
* Publishes vehicle command.
*/