Skip to content

Commit ca1a54c

Browse files
harderthanharderthan
committed
brings message converting functions
1 parent 95ea0fb commit ca1a54c

File tree

3 files changed

+322
-7
lines changed

3 files changed

+322
-7
lines changed

‎airsim_ros_pkgs/CMakeLists.txt

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -58,7 +58,8 @@ target_link_libraries(airsim_settings_parser
5858

5959
set(ARISIM_SOURCES
6060
src/airsim_ros_wrapper.cpp
61-
src/command_subscriber.cpp)
61+
src/command_subscriber.cpp
62+
src/messages.cpp)
6263
add_library(airsim_ros ${ARISIM_SOURCES})
6364
add_dependencies(airsim_ros
6465
${${PROJECT_NAME}_EXPORTED_TARGETS}

‎airsim_ros_pkgs/include/airsim_ros_pkgs/messages.hpp

Lines changed: 44 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,12 @@
44
#include <string>
55

66
#include "mavros_msgs/AttitudeTarget.h"
7+
#include "nav_msgs/Odometry.h"
8+
#include "sensor_msgs/Imu.h"
9+
#include "sensor_msgs/MagneticField.h"
10+
#include "sensor_msgs/NavSatFix.h"
11+
#include "sensor_msgs/PointCloud2.h"
12+
#include "sensor_msgs/Range.h"
713
#include "tf2/LinearMath/Quaternion.h"
814

915
// ROS custom msgs.
@@ -26,6 +32,14 @@
2632

2733
#include "common/AirSimSettings.hpp"
2834
#include "common/CommonStructs.hpp"
35+
#include "sensors/barometer/BarometerBase.hpp"
36+
#include "sensors/distance/DistanceSimple.hpp"
37+
#include "sensors/gps/GpsBase.hpp"
38+
#include "sensors/imu/ImuBase.hpp"
39+
#include "sensors/lidar/LidarSimple.hpp"
40+
#include "sensors/magnetometer/MagnetometerBase.hpp"
41+
#include "vehicles/car/api/CarApiBase.hpp"
42+
#include "vehicles/multirotor/api/MultirotorApiBase.hpp"
2943
#include "vehicles/multirotor/api/MultirotorCommon.hpp"
3044

3145
namespace airsim_ros {
@@ -34,10 +48,37 @@ namespace messages {
3448
namespace helper_functions {
3549

3650
airsim_ros_pkgs::GPSYaw GetGpsMsgFromAirsimGeoPoint(
37-
const msr::airlib::GeoPoint& geo_point);
51+
const msr::airlib::GeoPoint &geo_point);
3852

39-
msr::airlib::Quaternionr GetAirlibQuatFromRos(
40-
const tf2::Quaternion& tf2_quat);
53+
msr::airlib::Quaternionr GetAirlibQuatFromRos(const tf2::Quaternion &tf2_quat);
54+
55+
nav_msgs::Odometry GetOdomMsgFromMultirotorState(
56+
const msr::airlib::MultirotorState &drone_state, bool isEnu);
57+
58+
nav_msgs::Odometry GetOdomMsgFromTruthState(
59+
const msr::airlib::Kinematics::State &truth_state, bool isEnu);
60+
61+
sensor_msgs::PointCloud2 GetLidarMsgFromAirsim(
62+
const msr::airlib::LidarData &lidar_data, const std::string &vehicle_name,
63+
bool isEnu);
64+
65+
airsim_ros_pkgs::Environment GetEnvironmentMsgFromAirsim(
66+
const msr::airlib::Environment::State &env_data);
67+
68+
sensor_msgs::MagneticField GetMagMsgFromAirsim(
69+
const msr::airlib::MagnetometerBase::Output &mag_data);
70+
71+
sensor_msgs::NavSatFix GetGpsMsgFromAirsim(
72+
const msr::airlib::GpsBase::Output &gps_data);
73+
74+
sensor_msgs::Range GetRangeFromAirsim(
75+
const msr::airlib::DistanceSensorData &dist_data);
76+
77+
airsim_ros_pkgs::Altimeter GetAltimeterMsgFromAirsim(
78+
const msr::airlib::BarometerBase::Output &alt_data);
79+
80+
sensor_msgs::Imu GetImuMsgFromAirsim(
81+
const msr::airlib::ImuBase::Output &imu_data);
4182

4283
} // namespace helper_functions
4384

‎airsim_ros_pkgs/src/messages.cpp

Lines changed: 276 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,23 +1,296 @@
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
229

330
namespace airsim_ros {
431
namespace messages {
532
namespace helper_functions {
633

734
airsim_ros_pkgs::GPSYaw GetGpsMsgFromAirsimGeoPoint(
8-
const msr::airlib::GeoPoint& geo_point) {
35+
const msr::airlib::GeoPoint &geo_point) {
936
airsim_ros_pkgs::GPSYaw gps_msg;
1037
gps_msg.latitude = geo_point.latitude;
1138
gps_msg.longitude = geo_point.longitude;
1239
gps_msg.altitude = geo_point.altitude;
1340
return gps_msg;
1441
}
1542

16-
msr::airlib::Quaternionr GetAirlibQuatFromRos(const tf2::Quaternion& tf2_quat) {
43+
msr::airlib::Quaternionr GetAirlibQuatFromRos(const tf2::Quaternion &tf2_quat) {
1744
return msr::airlib::Quaternionr(tf2_quat.w(), tf2_quat.x(), tf2_quat.y(),
1845
tf2_quat.z());
1946
}
2047

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+
21294
} // namespace helper_functions
22295
} // namespace messages
23296
} // namespace airsim_ros

0 commit comments

Comments
 (0)