diff --git a/AirLib/include/vehicles/car/firmwares/ardurover/ArduRoverApi.hpp b/AirLib/include/vehicles/car/firmwares/ardurover/ArduRoverApi.hpp index 37ce927c36..c6e80e6e04 100644 --- a/AirLib/include/vehicles/car/firmwares/ardurover/ArduRoverApi.hpp +++ b/AirLib/include/vehicles/car/firmwares/ardurover/ArduRoverApi.hpp @@ -122,8 +122,8 @@ class ArduRoverApi : public CarApiBase { protected: void closeConnections() { - if (udpSocket_ != nullptr) - udpSocket_->close(); + if (udp_socket_ != nullptr) + udp_socket_->close(); } void connect() @@ -144,8 +144,8 @@ 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(); - udpSocket_->bind(connection_info_.local_host_ip, connection_info_.control_port); + udp_socket_ = std::make_unique(); + udp_socket_->bind(connection_info_.local_host_ip, connection_info_.control_port); } private: @@ -153,7 +153,7 @@ class ArduRoverApi : public CarApiBase { { // 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); @@ -161,7 +161,7 @@ class ArduRoverApi : public CarApiBase { 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; @@ -186,7 +186,34 @@ class ArduRoverApi : public CarApiBase { std::ostringstream oss; - const uint count_lidars = getSensors().size(SensorBase::SensorType::Lidar); + // 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\": ["; + + // Add the first sensor separately since otherwise there's an extra "," + // Which leads to AP counting an extra sensor, and if there are 10 sensors (AP supports upto 10), last one will get ignored + auto distance_output = getDistanceSensorData(""); + oss << distance_output.distance; + + // Add remaining sensors in the array + for (uint i=1; i(sensors_->getByType(SensorBase::SensorType::Distance, i)); + if (distance_sensor != nullptr) { + distance_output = distance_sensor->getOutput(); + // AP uses meters so no need to convert here + oss << "," << distance_output.distance; + } + } + + // Close JSON array & element + oss << "]}"; + } + + const uint count_lidars = sensors_->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) if (count_lidars != 0) { @@ -256,8 +283,8 @@ class ArduRoverApi : public CarApiBase { } // Send data - if (udpSocket_ != nullptr) { - udpSocket_->sendto(buf, strlen(buf), ip_, port_); + if (udp_socket_ != nullptr) { + udp_socket_->sendto(buf, strlen(buf), ip_, port_); } } @@ -269,7 +296,7 @@ class ArduRoverApi : public CarApiBase { AirSimSettings::MavLinkConnectionInfo connection_info_; - std::shared_ptr udpSocket_; + std::unique_ptr udp_socket_; uint16_t port_; std::string ip_; diff --git a/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp index 400ac408ed..99b1fc7c73 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp @@ -299,8 +299,8 @@ class ArduCopterApi : public MultirotorApiBase { protected: void closeConnections() { - if (udpSocket_ != nullptr) - udpSocket_->close(); + if (udp_socket_ != nullptr) + udp_socket_->close(); } void connect() @@ -320,8 +320,8 @@ class ArduCopterApi : public MultirotorApiBase { 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(); - udpSocket_->bind(connection_info_.local_host_ip, connection_info_.control_port); + udp_socket_ = std::make_unique(); + udp_socket_->bind(connection_info_.local_host_ip, connection_info_.control_port); } private: @@ -351,15 +351,45 @@ class ArduCopterApi : public MultirotorApiBase { << (last_rcData_.roll + 1) * 0.5f << "," << (last_rcData_.yaw + 1) * 0.5f << "," << (last_rcData_.throttle + 1) * 0.5f << "," - << (-last_rcData_.pitch + 1) * 0.5f << "," - << static_cast(last_rcData_.getSwitch(0)) << "," - << static_cast(last_rcData_.getSwitch(1)) << "," - << static_cast(last_rcData_.getSwitch(2)) << "," - << static_cast(last_rcData_.getSwitch(3)) - << "]}"; + << (-last_rcData_.pitch + 1) * 0.5f; + + // Add switches to RC channels array, 8 switches + for (uint8_t i=0; i<8; ++i) { + oss << "," << static_cast(last_rcData_.getSwitch(i)); + } + + // Close JSON array & element + oss << "]}"; + } + + // 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\": ["; + + // Add the first sensor separately since otherwise there's an extra "," + // Which leads to AP counting an extra sensor, and if there are 10 sensors (AP supports upto 10), last one will get ignored + auto distance_output = getDistanceSensorData(""); + oss << distance_output.distance; + + // Add remaining sensors in the array + for (uint i=1; i(sensors_->getByType(SensorBase::SensorType::Distance, i)); + if (distance_sensor != nullptr) { + distance_output = distance_sensor->getOutput(); + // AP uses meters so no need to convert here + oss << "," << distance_output.distance; + } + } + + // Close JSON array & element + oss << "]}"; } - const uint count_lidars = getSensors().size(SensorBase::SensorType::Lidar); + const uint count_lidars = sensors_->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) if (count_lidars != 0) { @@ -429,8 +459,8 @@ class ArduCopterApi : public MultirotorApiBase { } // Send data - if (udpSocket_ != nullptr) { - udpSocket_->sendto(buf, strlen(buf), ip_, port_); + if (udp_socket_ != nullptr) { + udp_socket_->sendto(buf, strlen(buf), ip_, port_); } } @@ -438,7 +468,7 @@ class ArduCopterApi : public MultirotorApiBase { { // Receive motor data RotorControlMessage 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); @@ -446,7 +476,7 @@ class ArduCopterApi : public MultirotorApiBase { 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); } for (auto i = 0; i < kArduCopterRotorControlCount; ++i) { @@ -463,7 +493,7 @@ class ArduCopterApi : public MultirotorApiBase { uint16_t pwm[kArduCopterRotorControlCount]; }; - std::shared_ptr udpSocket_; + std::unique_ptr udp_socket_; AirSimSettings::MavLinkConnectionInfo connection_info_; uint16_t port_; diff --git a/Unity/AirLibWrapper/AirsimWrapper/cmake/rpc-setup.cmake b/Unity/AirLibWrapper/AirsimWrapper/cmake/rpc-setup.cmake index b637eab622..2c94c36016 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/cmake/rpc-setup.cmake +++ b/Unity/AirLibWrapper/AirsimWrapper/cmake/rpc-setup.cmake @@ -35,7 +35,7 @@ set(RPCLIB_DEFAULT_PORT 8080 CACHE STRING "Default port used for running tests and examples") set(RPCLIB_DEFAULT_BUFFER_SIZE "1024 << 10" CACHE STRING "Default buffer size") -set(RPCLIB_CXX_STANDARD 11 CACHE STRING +set(RPCLIB_CXX_STANDARD 14 CACHE STRING "C++ version used to build rpclib (Currently: Only 11 and 14 supported)") if(RPCLIB_GENERATE_COMPDB)