Skip to content

Commit

Permalink
Merge pull request #3364 from rajat2004/ap-sensor-updates
Browse files Browse the repository at this point in the history 
ArduPilot Sensor Updates
  • Loading branch information
@zimmy87
zimmy87 authored Apr 28, 2021
2 parents 815fe8e + 8c5cbb0 commit a2acbf4
Show file tree
Hide file tree
Showing 9 changed files with 206 additions and 101 deletions.
5 changes: 5 additions & 0 deletions AirLib/include/common/AirSimSettings.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -215,6 +215,7 @@ struct AirSimSettings {
Vector3r position = VectorMath::nanVector();
Rotation rotation = Rotation::nanRotation();
bool draw_debug_points = false;
bool external_controller = true;
};

struct LidarSetting : SensorSetting {
Expand All @@ -235,6 +236,8 @@ struct AirSimSettings {

bool draw_debug_points = false;
std::string data_frame = AirSimSettings::kVehicleInertialFrame;

bool external_controller = true;
};

struct VehicleSetting {
Expand Down Expand Up @@ -1237,6 +1240,7 @@ struct AirSimSettings {
distance_setting.min_distance = settings_json.getFloat("MinDistance", distance_setting.min_distance);
distance_setting.max_distance = settings_json.getFloat("MaxDistance", distance_setting.max_distance);
distance_setting.draw_debug_points = settings_json.getBool("DrawDebugPoints", distance_setting.draw_debug_points);
distance_setting.external_controller = settings_json.getBool("ExternalController", distance_setting.external_controller);

distance_setting.position = createVectorSetting(settings_json, distance_setting.position);
distance_setting.rotation = createRotationSetting(settings_json, distance_setting.rotation);
Expand All @@ -1250,6 +1254,7 @@ struct AirSimSettings {
lidar_setting.horizontal_rotation_frequency = settings_json.getInt("RotationsPerSecond", lidar_setting.horizontal_rotation_frequency);
lidar_setting.draw_debug_points = settings_json.getBool("DrawDebugPoints", lidar_setting.draw_debug_points);
lidar_setting.data_frame = settings_json.getString("DataFrame", lidar_setting.data_frame);
lidar_setting.external_controller = settings_json.getBool("ExternalController", lidar_setting.external_controller);

lidar_setting.vertical_FOV_upper = settings_json.getFloat("VerticalFOVUpper", lidar_setting.vertical_FOV_upper);
lidar_setting.vertical_FOV_lower = settings_json.getFloat("VerticalFOVLower", lidar_setting.vertical_FOV_lower);
Expand Down
2 changes: 2 additions & 0 deletions AirLib/include/sensors/distance/DistanceSimpleParams.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@ struct DistanceSimpleParams {
};

bool draw_debug_points = false;
bool external_controller = true;

/*
Ref: A Stochastic Approach to Noise Modeling for Barometric Altimeters
Expand Down Expand Up @@ -52,6 +53,7 @@ struct DistanceSimpleParams {
max_distance = settings.max_distance;

draw_debug_points = settings.draw_debug_points;
external_controller = settings.external_controller;

relative_pose.position = settings.position;
if (std::isnan(relative_pose.position.x()))
Expand Down
3 changes: 3 additions & 0 deletions AirLib/include/sensors/lidar/LidarSimpleParams.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,8 @@ struct LidarSimpleParams {
bool draw_debug_points = false;
std::string data_frame = AirSimSettings::kVehicleInertialFrame;

bool external_controller = true;

real_T update_frequency = 10; // Hz
real_T startup_delay = 0; // sec

Expand Down Expand Up @@ -90,6 +92,7 @@ struct LidarSimpleParams {

draw_debug_points = settings.draw_debug_points;
data_frame = settings.data_frame;
external_controller = settings.external_controller;
}
};

Expand Down
117 changes: 79 additions & 38 deletions AirLib/include/vehicles/car/firmwares/ardurover/ArduRoverApi.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,8 @@
#include "sensors/gps/GpsBase.hpp"
#include "sensors/magnetometer/MagnetometerBase.hpp"
#include "sensors/barometer/BarometerBase.hpp"
#include "sensors/lidar/LidarBase.hpp"
#include "sensors/distance/DistanceSimple.hpp"
#include "sensors/lidar/LidarSimple.hpp"

#include "UdpSocket.hpp"

Expand Down Expand Up @@ -122,8 +123,8 @@ class ArduRoverApi : public CarApiBase {
protected:
void closeConnections()
{
if (udpSocket_ != nullptr)
udpSocket_->close();
if (udp_socket_ != nullptr)
udp_socket_->close();
}

void connect()
Expand All @@ -144,24 +145,24 @@ class ArduRoverApi : public CarApiBase {
Utils::log(Utils::stringf("Using UDP port %d, local IP %s, remote IP %s for sending sensor data", port_, connection_info_.local_host_ip.c_str(), ip_.c_str()), Utils::kLogLevelInfo);
Utils::log(Utils::stringf("Using UDP port %d for receiving rotor power", connection_info_.control_port, connection_info_.local_host_ip.c_str(), ip_.c_str()), Utils::kLogLevelInfo);

udpSocket_ = std::make_shared<mavlinkcom::UdpSocket>();
udpSocket_->bind(connection_info_.local_host_ip, connection_info_.control_port);
udp_socket_ = std::make_unique<mavlinkcom::UdpSocket>();
udp_socket_->bind(connection_info_.local_host_ip, connection_info_.control_port);
}

private:
void recvRoverControl()
{
// Receive motor data
RoverControlMessage pkt;
int recv_ret = udpSocket_->recv(&pkt, sizeof(pkt), 100);
int recv_ret = udp_socket_->recv(&pkt, sizeof(pkt), 100);
while (recv_ret != sizeof(pkt)) {
if (recv_ret <= 0) {
Utils::log(Utils::stringf("Error while receiving rotor control data - ErrorNo: %d", recv_ret), Utils::kLogLevelInfo);
} else {
Utils::log(Utils::stringf("Received %d bytes instead of %zu bytes", recv_ret, sizeof(pkt)), Utils::kLogLevelInfo);
}

recv_ret = udpSocket_->recv(&pkt, sizeof(pkt), 100);
recv_ret = udp_socket_->recv(&pkt, sizeof(pkt), 100);
}

last_controls_.throttle = pkt.throttle;
Expand All @@ -181,83 +182,123 @@ class ArduRoverApi : public CarApiBase {
if (sensors_ == nullptr)
return;

const auto& gps_output = getGpsData("");
const auto& imu_output = getImuData("");

std::ostringstream oss;

const uint count_lidars = getSensors().size(SensorBase::SensorType::Lidar);
// TODO: Add bool value in settings to check whether to send lidar data or not
// Since it's possible that we don't want to send the lidar data to Ardupilot but still have the lidar (maybe as a ROS topic)
const uint count_gps_sensors = sensors_->size(SensorBase::SensorType::Gps);
if (count_gps_sensors != 0) {
const auto& gps_output = getGpsData("");

oss << ","
"\"gps\": {"
<< std::fixed << std::setprecision(7)
<< "\"lat\": " << gps_output.gnss.geo_point.latitude << ","
<< "\"lon\": " << gps_output.gnss.geo_point.longitude << ","
<< std::setprecision(3) << "\"alt\": " << gps_output.gnss.geo_point.altitude
<< "},"

<< "\"velocity\": {"
<< std::setprecision(12)
<< "\"world_linear_velocity\": ["
<< gps_output.gnss.velocity[0] << ","
<< gps_output.gnss.velocity[1] << ","
<< gps_output.gnss.velocity[2] << "]"
"}";
}

// Send Distance Sensors data if present
const uint count_distance_sensors = sensors_->size(SensorBase::SensorType::Distance);
if (count_distance_sensors != 0) {
// Start JSON element
oss << ","
"\"rng\": {"
"\"distances\": [";

// Used to avoid trailing comma
std::string sep = "";

// Add sensor outputs in the array
for (uint i=0; i<count_distance_sensors; ++i) {
const auto* distance_sensor = static_cast<const DistanceSimple*>(
sensors_->getByType(SensorBase::SensorType::Distance, i));
// Don't send the data if sending to external controller is disabled in settings
if (distance_sensor && distance_sensor->getParams().external_controller) {
const auto& distance_output = distance_sensor->getOutput();
// AP uses meters so no need to convert here
oss << sep << distance_output.distance;
sep = ",";
}
}

// Close JSON array & element
oss << "]}";
}

const uint count_lidars = sensors_->size(SensorBase::SensorType::Lidar);
if (count_lidars != 0) {
const auto& lidar_output = getLidarData("");
oss << ","
"\"lidar\": {"
"\"point_cloud\": [";

std::copy(lidar_output.point_cloud.begin(), lidar_output.point_cloud.end(), std::ostream_iterator<real_T>(oss, ","));
// Add sensor outputs in the array
for (uint i=0; i<count_lidars; ++i) {
const auto* lidar = static_cast<const LidarSimple*>(sensors_->getByType(SensorBase::SensorType::Lidar, i));

if (lidar && lidar->getParams().external_controller) {
const auto& lidar_output = lidar->getOutput();
std::copy(lidar_output.point_cloud.begin(), lidar_output.point_cloud.end(), std::ostream_iterator<real_T>(oss, ","));
// AP backend only takes in a single Lidar sensor data currently
break;
}
}

oss << "]}";
}

const auto& imu_output = getImuData("");

float yaw;
float pitch;
float roll;
VectorMath::toEulerianAngle(imu_output.orientation, pitch, roll, yaw);

// UDP packets have a maximum size limit of 65kB
char buf[65000];

// TODO: Split the following sensor packet formation into different parts for individual sensors

// UDP packets have a maximum size limit of 65kB
int ret = snprintf(buf, sizeof(buf),
"{"
"\"timestamp\": %" PRIu64 ","
"\"imu\": {"
"\"angular_velocity\": [%.12f, %.12f, %.12f],"
"\"linear_acceleration\": [%.12f, %.12f, %.12f]"
"},"
"\"gps\": {"
"\"lat\": %.7f,"
"\"lon\": %.7f,"
"\"alt\": %.3f"
"},"
"\"velocity\": {"
"\"world_linear_velocity\": [%.12f, %.12f, %.12f]"
"},"
"\"pose\": {"
"\"roll\": %.12f,"
"\"pitch\": %.12f,"
"\"yaw\": %.12f"
"}"
"%s"
"}\n",
static_cast<uint64_t>(msr::airlib::ClockFactory::get()->nowNanos() / 1.0E3),
static_cast<uint64_t>(ClockFactory::get()->nowNanos() / 1.0E3),
imu_output.angular_velocity[0],
imu_output.angular_velocity[1],
imu_output.angular_velocity[2],
imu_output.linear_acceleration[0],
imu_output.linear_acceleration[1],
imu_output.linear_acceleration[2],
gps_output.gnss.geo_point.latitude,
gps_output.gnss.geo_point.longitude,
gps_output.gnss.geo_point.altitude,
gps_output.gnss.velocity[0],
gps_output.gnss.velocity[1],
gps_output.gnss.velocity[2],
roll, pitch, yaw,
oss.str().c_str());

if (ret == -1) {
Utils::log("Error while allocating memory for sensor message", Utils::kLogLevelInfo);
Utils::log("Error while allocating memory for sensor message", Utils::kLogLevelError);
return;
}
else if (static_cast<uint>(ret) >= sizeof(buf)) {
Utils::log(Utils::stringf("Sensor message truncated, lost %d bytes", ret - sizeof(buf)), Utils::kLogLevelInfo);
Utils::log(Utils::stringf("Sensor message truncated, lost %d bytes", ret - sizeof(buf)), Utils::kLogLevelWarn);
}

// Send data
if (udpSocket_ != nullptr) {
udpSocket_->sendto(buf, strlen(buf), ip_, port_);
if (udp_socket_ != nullptr) {
udp_socket_->sendto(buf, strlen(buf), ip_, port_);
}
}

Expand All @@ -269,7 +310,7 @@ class ArduRoverApi : public CarApiBase {

AirSimSettings::MavLinkConnectionInfo connection_info_;

std::shared_ptr<mavlinkcom::UdpSocket> udpSocket_;
std::unique_ptr<mavlinkcom::UdpSocket> udp_socket_;

uint16_t port_;
std::string ip_;
Expand Down
Loading

0 comments on commit a2acbf4

Please sign in to comment.