|
1 |
| -#include "messages.hpp" |
| 1 | +#include "airsim_ros_pkgs/messages.hpp" |
| 2 | + |
| 3 | +#include "nav_msgs/Odometry.h" |
| 4 | +#include "sensor_msgs/Imu.h" |
| 5 | +#include "sensor_msgs/MagneticField.h" |
| 6 | +#include "sensor_msgs/PointCloud2.h" |
| 7 | +#include "sensor_msgs/Range.h" |
| 8 | + |
| 9 | +#include "common/CommonStructs.hpp" |
| 10 | + |
| 11 | +namespace { |
| 12 | + |
| 13 | +ros::Time ChronoTimeToRosTime( |
| 14 | + const std::chrono::system_clock::time_point &stamp) { |
| 15 | + auto dur = std::chrono::duration<double>(stamp.time_since_epoch()); |
| 16 | + ros::Time cur_time; |
| 17 | + cur_time.fromSec(dur.count()); |
| 18 | + return cur_time; |
| 19 | +} |
| 20 | + |
| 21 | +ros::Time AirsimTimeToRosTime(const msr::airlib::TTimePoint &stamp) { |
| 22 | + std::chrono::nanoseconds dur(stamp); |
| 23 | + std::chrono::time_point<std::chrono::system_clock> tp(dur); |
| 24 | + ros::Time cur_time = ChronoTimeToRosTime(tp); |
| 25 | + return cur_time; |
| 26 | +} |
| 27 | + |
| 28 | +} // namespace |
2 | 29 |
|
3 | 30 | namespace airsim_ros {
|
4 | 31 | namespace messages {
|
5 | 32 | namespace helper_functions {
|
6 | 33 |
|
7 | 34 | airsim_ros_pkgs::GPSYaw GetGpsMsgFromAirsimGeoPoint(
|
8 |
| - const msr::airlib::GeoPoint& geo_point) { |
| 35 | + const msr::airlib::GeoPoint &geo_point) { |
9 | 36 | airsim_ros_pkgs::GPSYaw gps_msg;
|
10 | 37 | gps_msg.latitude = geo_point.latitude;
|
11 | 38 | gps_msg.longitude = geo_point.longitude;
|
12 | 39 | gps_msg.altitude = geo_point.altitude;
|
13 | 40 | return gps_msg;
|
14 | 41 | }
|
15 | 42 |
|
16 |
| -msr::airlib::Quaternionr GetAirlibQuatFromRos(const tf2::Quaternion& tf2_quat) { |
| 43 | +msr::airlib::Quaternionr GetAirlibQuatFromRos(const tf2::Quaternion &tf2_quat) { |
17 | 44 | return msr::airlib::Quaternionr(tf2_quat.w(), tf2_quat.x(), tf2_quat.y(),
|
18 | 45 | tf2_quat.z());
|
19 | 46 | }
|
20 | 47 |
|
| 48 | +nav_msgs::Odometry GetOdomMsgFromMultirotorState( |
| 49 | + const msr::airlib::MultirotorState &drone_state, bool isEnu) { |
| 50 | + nav_msgs::Odometry odom_msg; |
| 51 | + |
| 52 | + odom_msg.pose.pose.position.x = drone_state.getPosition().x(); |
| 53 | + odom_msg.pose.pose.position.y = drone_state.getPosition().y(); |
| 54 | + odom_msg.pose.pose.position.z = drone_state.getPosition().z(); |
| 55 | + odom_msg.pose.pose.orientation.x = drone_state.getOrientation().x(); |
| 56 | + odom_msg.pose.pose.orientation.y = drone_state.getOrientation().y(); |
| 57 | + odom_msg.pose.pose.orientation.z = drone_state.getOrientation().z(); |
| 58 | + odom_msg.pose.pose.orientation.w = drone_state.getOrientation().w(); |
| 59 | + |
| 60 | + odom_msg.twist.twist.linear.x = |
| 61 | + drone_state.kinematics_estimated.twist.linear.x(); |
| 62 | + odom_msg.twist.twist.linear.y = |
| 63 | + drone_state.kinematics_estimated.twist.linear.y(); |
| 64 | + odom_msg.twist.twist.linear.z = |
| 65 | + drone_state.kinematics_estimated.twist.linear.z(); |
| 66 | + odom_msg.twist.twist.angular.x = |
| 67 | + drone_state.kinematics_estimated.twist.angular.x(); |
| 68 | + odom_msg.twist.twist.angular.y = |
| 69 | + drone_state.kinematics_estimated.twist.angular.y(); |
| 70 | + odom_msg.twist.twist.angular.z = |
| 71 | + drone_state.kinematics_estimated.twist.angular.z(); |
| 72 | + |
| 73 | + if (isEnu) { |
| 74 | + std::swap(odom_msg.pose.pose.position.x, odom_msg.pose.pose.position.y); |
| 75 | + odom_msg.pose.pose.position.z = -odom_msg.pose.pose.position.z; |
| 76 | + |
| 77 | + std::swap(odom_msg.pose.pose.orientation.x, |
| 78 | + odom_msg.pose.pose.orientation.y); |
| 79 | + odom_msg.pose.pose.orientation.z = -odom_msg.pose.pose.orientation.z; |
| 80 | + double roll, pitch, yaw; |
| 81 | + Eigen::Quaterniond attitude_quater; |
| 82 | + attitude_quater.w() = odom_msg.pose.pose.orientation.w; |
| 83 | + attitude_quater.x() = odom_msg.pose.pose.orientation.x; |
| 84 | + attitude_quater.y() = odom_msg.pose.pose.orientation.y; |
| 85 | + attitude_quater.z() = odom_msg.pose.pose.orientation.z; |
| 86 | + Eigen::Matrix3d rota = attitude_quater.toRotationMatrix(); |
| 87 | + Eigen::Matrix3d nedtoenu; |
| 88 | + nedtoenu << 0, 1, 0, 1, 0, 0, 0, 0, -1; |
| 89 | + rota = nedtoenu * rota; |
| 90 | + Eigen::Quaterniond enuq(rota); |
| 91 | + |
| 92 | + odom_msg.pose.pose.orientation.w = enuq.w(); |
| 93 | + odom_msg.pose.pose.orientation.x = enuq.x(); |
| 94 | + odom_msg.pose.pose.orientation.y = enuq.y(); |
| 95 | + odom_msg.pose.pose.orientation.z = enuq.z(); |
| 96 | + |
| 97 | + std::swap(odom_msg.twist.twist.linear.x, odom_msg.twist.twist.linear.y); |
| 98 | + odom_msg.twist.twist.linear.z = -odom_msg.twist.twist.linear.z; |
| 99 | + std::swap(odom_msg.twist.twist.angular.x, odom_msg.twist.twist.angular.y); |
| 100 | + odom_msg.twist.twist.angular.z = -odom_msg.twist.twist.angular.z; |
| 101 | + } |
| 102 | + |
| 103 | + return odom_msg; |
| 104 | +} |
| 105 | + |
| 106 | +nav_msgs::Odometry GetOdomMsgFromTruthState( |
| 107 | + const msr::airlib::Kinematics::State &truth_state, bool isEnu) { |
| 108 | + nav_msgs::Odometry odom_msg; |
| 109 | + odom_msg.pose.pose.position.x = truth_state.pose.position.x(); |
| 110 | + odom_msg.pose.pose.position.y = truth_state.pose.position.y(); |
| 111 | + odom_msg.pose.pose.position.z = truth_state.pose.position.z(); |
| 112 | + odom_msg.pose.pose.orientation.x = truth_state.pose.orientation.x(); |
| 113 | + odom_msg.pose.pose.orientation.y = truth_state.pose.orientation.y(); |
| 114 | + odom_msg.pose.pose.orientation.z = truth_state.pose.orientation.z(); |
| 115 | + odom_msg.pose.pose.orientation.w = truth_state.pose.orientation.w(); |
| 116 | + odom_msg.twist.twist.linear.x = truth_state.twist.linear.x(); |
| 117 | + odom_msg.twist.twist.linear.y = truth_state.twist.linear.y(); |
| 118 | + odom_msg.twist.twist.linear.z = truth_state.twist.linear.z(); |
| 119 | + odom_msg.twist.twist.angular.x = truth_state.twist.angular.x(); |
| 120 | + odom_msg.twist.twist.angular.y = truth_state.twist.angular.y(); |
| 121 | + odom_msg.twist.twist.angular.z = truth_state.twist.angular.z(); |
| 122 | + |
| 123 | + if (isEnu) { |
| 124 | + std::swap(odom_msg.pose.pose.position.x, odom_msg.pose.pose.position.y); |
| 125 | + odom_msg.pose.pose.position.z = -odom_msg.pose.pose.position.z; |
| 126 | + double roll, pitch, yaw; |
| 127 | + Eigen::Quaterniond attitude_quater; |
| 128 | + attitude_quater.w() = odom_msg.pose.pose.orientation.w; |
| 129 | + attitude_quater.x() = odom_msg.pose.pose.orientation.x; |
| 130 | + attitude_quater.y() = odom_msg.pose.pose.orientation.y; |
| 131 | + attitude_quater.z() = odom_msg.pose.pose.orientation.z; // NED |
| 132 | + Eigen::Matrix3d rota = attitude_quater.toRotationMatrix(); |
| 133 | + Eigen::Matrix3d nedtoenu; |
| 134 | + nedtoenu << 0, 1, 0, 1, 0, 0, 0, 0, -1; // enu |
| 135 | + Eigen::Matrix3d body1tobody2; |
| 136 | + body1tobody2 << 1, 0, 0, 0, -1, 0, 0, 0, -1; |
| 137 | + rota = nedtoenu * rota; |
| 138 | + rota = rota * body1tobody2; |
| 139 | + Eigen::Quaterniond enuq(rota); |
| 140 | + odom_msg.pose.pose.orientation.w = enuq.w(); |
| 141 | + odom_msg.pose.pose.orientation.x = enuq.x(); |
| 142 | + odom_msg.pose.pose.orientation.y = enuq.y(); |
| 143 | + odom_msg.pose.pose.orientation.z = enuq.z(); |
| 144 | + |
| 145 | + std::swap(odom_msg.twist.twist.linear.x, odom_msg.twist.twist.linear.y); |
| 146 | + odom_msg.twist.twist.linear.z = -odom_msg.twist.twist.linear.z; |
| 147 | + odom_msg.twist.twist.angular.y = -odom_msg.twist.twist.angular.y; |
| 148 | + odom_msg.twist.twist.angular.z = -odom_msg.twist.twist.angular.z; |
| 149 | + } |
| 150 | + |
| 151 | + return odom_msg; |
| 152 | +} |
| 153 | + |
| 154 | +sensor_msgs::PointCloud2 GetLidarMsgFromAirsim( |
| 155 | + const msr::airlib::LidarData &lidar_data, const std::string &vehicle_name, |
| 156 | + bool isEnu) { |
| 157 | + sensor_msgs::PointCloud2 lidar_msg; |
| 158 | + lidar_msg.header.stamp = ros::Time::now(); |
| 159 | + lidar_msg.header.frame_id = vehicle_name; |
| 160 | + |
| 161 | + if (lidar_data.point_cloud.size() > 3) { |
| 162 | + lidar_msg.height = 1; |
| 163 | + lidar_msg.width = lidar_data.point_cloud.size() / 3; |
| 164 | + |
| 165 | + lidar_msg.fields.resize(3); |
| 166 | + lidar_msg.fields[0].name = "x"; |
| 167 | + lidar_msg.fields[1].name = "y"; |
| 168 | + lidar_msg.fields[2].name = "z"; |
| 169 | + |
| 170 | + int offset = 0; |
| 171 | + |
| 172 | + for (size_t d = 0; d < lidar_msg.fields.size(); ++d, offset += 4) { |
| 173 | + lidar_msg.fields[d].offset = offset; |
| 174 | + lidar_msg.fields[d].datatype = sensor_msgs::PointField::FLOAT32; |
| 175 | + lidar_msg.fields[d].count = 1; |
| 176 | + } |
| 177 | + |
| 178 | + lidar_msg.is_bigendian = false; |
| 179 | + lidar_msg.point_step = offset; // 4 * num fields |
| 180 | + lidar_msg.row_step = lidar_msg.point_step * lidar_msg.width; |
| 181 | + |
| 182 | + lidar_msg.is_dense = true; // todo |
| 183 | + std::vector<float> data_std = lidar_data.point_cloud; |
| 184 | + |
| 185 | + const unsigned char *bytes = |
| 186 | + reinterpret_cast<const unsigned char *>(data_std.data()); |
| 187 | + std::vector<unsigned char> lidar_msg_data( |
| 188 | + bytes, bytes + sizeof(float) * data_std.size()); |
| 189 | + lidar_msg.data = std::move(lidar_msg_data); |
| 190 | + } else { |
| 191 | + // msg = [] |
| 192 | + } |
| 193 | + |
| 194 | + if (isEnu) { |
| 195 | + // transform lidar_msg to ENU not using ROS library. |
| 196 | + sensor_msgs::PointCloud2 lidar_msg_enu; |
| 197 | + lidar_msg_enu.header.stamp = ros::Time::now(); |
| 198 | + lidar_msg_enu.header.frame_id = vehicle_name; |
| 199 | + lidar_msg_enu.height = lidar_msg.height; |
| 200 | + lidar_msg_enu.width = lidar_msg.width; |
| 201 | + lidar_msg_enu.fields.resize(3); |
| 202 | + lidar_msg_enu.fields[0].name = "x"; |
| 203 | + lidar_msg_enu.fields[1].name = "y"; |
| 204 | + lidar_msg_enu.fields[2].name = "z"; |
| 205 | + int offset = 0; |
| 206 | + for (size_t d = 0; d < lidar_msg_enu.fields.size(); ++d, offset += 4) { |
| 207 | + lidar_msg_enu.fields[d].offset = offset; |
| 208 | + lidar_msg_enu.fields[d].datatype = sensor_msgs::PointField::FLOAT32; |
| 209 | + lidar_msg_enu.fields[d].count = 1; |
| 210 | + } |
| 211 | + lidar_msg_enu.is_bigendian = false; |
| 212 | + lidar_msg_enu.point_step = offset; // 4 * num fields |
| 213 | + lidar_msg_enu.row_step = lidar_msg_enu.point_step * lidar_msg_enu.width; |
| 214 | + lidar_msg_enu.is_dense = true; // todo |
| 215 | + std::vector<float> data_std = lidar_data.point_cloud; |
| 216 | + for (size_t i = 0; i < data_std.size(); i += 3) { |
| 217 | + std::swap(data_std[i], data_std[i + 1]); |
| 218 | + data_std[i + 2] = -data_std[i + 2]; |
| 219 | + } |
| 220 | + const unsigned char *bytes = |
| 221 | + reinterpret_cast<const unsigned char *>(data_std.data()); |
| 222 | + std::vector<unsigned char> lidar_msg_data( |
| 223 | + bytes, bytes + sizeof(float) * data_std.size()); |
| 224 | + lidar_msg_enu.data = std::move(lidar_msg_data); |
| 225 | + lidar_msg = lidar_msg_enu; |
| 226 | + } |
| 227 | + |
| 228 | + return lidar_msg; |
| 229 | +} |
| 230 | + |
| 231 | +airsim_ros_pkgs::Environment GetEnvironmentMsgFromAirsim( |
| 232 | + const msr::airlib::Environment::State &env_data) { |
| 233 | + airsim_ros_pkgs::Environment env_msg; |
| 234 | + env_msg.position.x = env_data.position.x(); |
| 235 | + env_msg.position.y = env_data.position.y(); |
| 236 | + env_msg.position.z = env_data.position.z(); |
| 237 | + env_msg.geo_point.latitude = env_data.geo_point.latitude; |
| 238 | + env_msg.geo_point.longitude = env_data.geo_point.longitude; |
| 239 | + env_msg.geo_point.altitude = env_data.geo_point.altitude; |
| 240 | + env_msg.gravity.x = env_data.gravity.x(); |
| 241 | + env_msg.gravity.y = env_data.gravity.y(); |
| 242 | + env_msg.gravity.z = env_data.gravity.z(); |
| 243 | + env_msg.air_pressure = env_data.air_pressure; |
| 244 | + env_msg.temperature = env_data.temperature; |
| 245 | + env_msg.air_density = env_data.temperature; |
| 246 | + |
| 247 | + return env_msg; |
| 248 | +} |
| 249 | + |
| 250 | +sensor_msgs::MagneticField GetMagMsgFromAirsim( |
| 251 | + const msr::airlib::MagnetometerBase::Output &mag_data) { |
| 252 | + sensor_msgs::MagneticField mag_msg; |
| 253 | + mag_msg.magnetic_field.x = mag_data.magnetic_field_body.x(); |
| 254 | + mag_msg.magnetic_field.y = mag_data.magnetic_field_body.y(); |
| 255 | + mag_msg.magnetic_field.z = mag_data.magnetic_field_body.z(); |
| 256 | + std::copy(std::begin(mag_data.magnetic_field_covariance), |
| 257 | + std::end(mag_data.magnetic_field_covariance), |
| 258 | + std::begin(mag_msg.magnetic_field_covariance)); |
| 259 | + mag_msg.header.stamp = AirsimTimeToRosTime(mag_data.time_stamp); |
| 260 | + |
| 261 | + return mag_msg; |
| 262 | +} |
| 263 | + |
| 264 | +sensor_msgs::NavSatFix GetGpsMsgFromAirsim( |
| 265 | + const msr::airlib::GpsBase::Output &gps_data) { |
| 266 | + sensor_msgs::NavSatFix gps_msg; |
| 267 | + gps_msg.header.stamp = AirsimTimeToRosTime(gps_data.time_stamp); |
| 268 | + gps_msg.latitude = gps_data.gnss.geo_point.latitude; |
| 269 | + gps_msg.longitude = gps_data.gnss.geo_point.longitude; |
| 270 | + gps_msg.altitude = gps_data.gnss.geo_point.altitude; |
| 271 | + gps_msg.status.service = sensor_msgs::NavSatStatus::SERVICE_GLONASS; |
| 272 | + gps_msg.status.status = gps_data.gnss.fix_type; |
| 273 | + |
| 274 | + return gps_msg; |
| 275 | +} |
| 276 | + |
| 277 | +sensor_msgs::Range GetRangeFromAirsim( |
| 278 | + const msr::airlib::DistanceSensorData &dist_data) { |
| 279 | + sensor_msgs::Range dist_msg; |
| 280 | + dist_msg.header.stamp = AirsimTimeToRosTime(dist_data.time_stamp); |
| 281 | + dist_msg.range = dist_data.distance; |
| 282 | + dist_msg.min_range = dist_data.min_distance; |
| 283 | + dist_msg.max_range = dist_data.min_distance; |
| 284 | + |
| 285 | + return dist_msg; |
| 286 | +} |
| 287 | + |
| 288 | +airsim_ros_pkgs::Altimeter GetAltimeterMsgFromAirsim( |
| 289 | + const msr::airlib::BarometerBase::Output &alt_data); |
| 290 | + |
| 291 | +sensor_msgs::Imu GetImuMsgFromAirsim( |
| 292 | + const msr::airlib::ImuBase::Output &imu_data); |
| 293 | + |
21 | 294 | } // namespace helper_functions
|
22 | 295 | } // namespace messages
|
23 | 296 | } // namespace airsim_ros
|
0 commit comments