Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: Add Lidar Sensor for Airsim

Browse Source 
Adds variable length array of vector3f parsing
This commit is contained in:
Rajat Singhal 2019-07-17 21:57:14 +05:30 committed by Andrew Tridgell
parent 124d18228b
commit 2d25971801
2 changed files with 58 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
libraries/SITL/SIM_AirSim.cpp
View File

@ -116,6 +116,54 @@ bool AirSim::parse_sensors(const char *json)
}
break;
}
case DATA_VECTOR3F_ARRAY: {
// - array of floats that represent [x,y,z] coordinate for each point hit within the range
// x0, y0, z0, x1, y1, z1, ..., xn, yn, zn
// example: [23.1,0.677024,1.4784,-8.97607135772705,-8.976069450378418,-8.642673492431641e-07,]
if (*p++ != '[') {
return false;
}
uint16_t n = 0;
struct vector3f_array *v = (struct vector3f_array *)key.ptr;
while (true) {
if (n >= v->length) {
Vector3f *d = (Vector3f *)realloc(v->data, sizeof(Vector3f)*(n+1));
if (d == nullptr) {
return false;
}
v->data = d;
v->length = n+1;
}
if (sscanf(p, "%f,%f,%f,", &v->data[n].x, &v->data[n].y, &v->data[n].z) != 3) {
printf("Failed to parse Vector3f for %s/%s[%u]\n", key.section, key.key, n);
return false;
}
n++;
// Goto 3rd occurence of ,
p = strchr(p,',');
if (!p) {
return false;
}
p++;
p = strchr(p,',');
if (!p) {
return false;
}
p++;
p = strchr(p,',');
if (!p) {
return false;
}
p++;
// Reached end of point cloud
if (p[0] == ']') {
break;
}
}
v->length = n;
break;
}
}
}
return true;
@ -149,15 +197,11 @@ void AirSim::recv_fdm()
return;
}
bool parse_ok = parse_sensors((const char *)(p1+1));
parse_sensors((const char *)(p1+1));
memmove(sensor_buffer, p2, sensor_buffer_len - (p2 - sensor_buffer));
sensor_buffer_len = sensor_buffer_len - (p2 - sensor_buffer);
if (!parse_ok) {
return;
}
accel_body = Vector3f(state.imu.linear_acceleration[0],
state.imu.linear_acceleration[1],
state.imu.linear_acceleration[2]);
@ -188,6 +232,8 @@ void AirSim::recv_fdm()
}
}
scanner.points = state.lidar.points;
#if 0
AP::logger().Write("ASM1", "TimeUS,TUS,R,P,Y,GX,GY,GZ",
"QQffffff",

9
libraries/SITL/SIM_AirSim.h
View File

@ -60,7 +60,7 @@ private:
bool parse_sensors(const char *json);
// buffer for parsing pose data in JSON format
uint8_t sensor_buffer[2048];
uint8_t sensor_buffer[65000];
uint32_t sensor_buffer_len;
enum data_type {
@ -68,6 +68,7 @@ private:
DATA_FLOAT,
DATA_DOUBLE,
DATA_VECTOR3F,
DATA_VECTOR3F_ARRAY,
};
struct {
@ -85,6 +86,9 @@ private:
struct {
Vector3f world_linear_velocity;
} velocity;
struct {
struct vector3f_array points;
} lidar;
} state, last_state;
// table to aid parsing of JSON sensor data
@ -93,7 +97,7 @@ private:
const char *key;
void *ptr;
enum data_type type;
} keytable[10] = {
} keytable[11] = {
{ "", "timestamp", &state.timestamp, DATA_UINT64 },
{ "imu", "angular_velocity", &state.imu.angular_velocity, DATA_VECTOR3F },
{ "imu", "linear_acceleration", &state.imu.linear_acceleration, DATA_VECTOR3F },
@ -104,6 +108,7 @@ private:
{ "pose", "pitch", &state.pose.pitch, DATA_FLOAT },
{ "pose", "yaw", &state.pose.yaw, DATA_FLOAT },
{ "velocity", "world_linear_velocity", &state.velocity.world_linear_velocity, DATA_VECTOR3F },
{ "lidar", "point_cloud", &state.lidar.points, DATA_VECTOR3F_ARRAY },
};
};