Skip to content

Commit 943ae18

Browse files
TSC21TSC21
committed
lidar: adjust sensor orientation computation pipeline
1 parent 06eb458 commit 943ae18

8 files changed

+75
-62
lines changed

‎include/common.h

+8
Original file line numberDiff line numberDiff line change
@@ -230,4 +230,12 @@ static const auto q_ng = ignition::math::Quaterniond(0, 0.70711, 0.70711, 0);
230230
*/
231231
static const auto q_br = ignition::math::Quaterniond(0, 1, 0, 0);
232232

233+
// sensor X-axis unit vector in `base_link` frame
234+
static const ignition::math::Vector3d kDownwardRotation = ignition::math::Vector3d(0, 0, -1);
235+
static const ignition::math::Vector3d kUpwardRotation = ignition::math::Vector3d(0, 0, 1);
236+
static const ignition::math::Vector3d kBackwardRotation = ignition::math::Vector3d(-1, 0, 0);
237+
static const ignition::math::Vector3d kForwardRotation = ignition::math::Vector3d(1, 0, 0);
238+
static const ignition::math::Vector3d kLeftRotation = ignition::math::Vector3d(0, 1, 0);
239+
static const ignition::math::Vector3d kRightRotation = ignition::math::Vector3d(0, -1, 0);
240+
233241
#endif // SITL_GAZEBO_COMMON_H_

‎include/gazebo_lidar_plugin.h

+2-9
Original file line numberDiff line numberDiff line change
@@ -36,14 +36,6 @@ namespace gazebo
3636
static constexpr double kDefaultMinDistance = 0.2;
3737
static constexpr double kDefaultMaxDistance = 15.0;
3838

39-
// sensor X-axis unit vector in `base_link` frame
40-
static const ignition::math::Vector3d kDownwardRotation = ignition::math::Vector3d(0, 0, -1);
41-
static const ignition::math::Vector3d kUpwardRotation = ignition::math::Vector3d(0, 0, 1);
42-
static const ignition::math::Vector3d kBackwardRotation = ignition::math::Vector3d(-1, 0, 0);
43-
static const ignition::math::Vector3d kForwardRotation = ignition::math::Vector3d(1, 0, 0);
44-
static const ignition::math::Vector3d kLeftRotation = ignition::math::Vector3d(0, 1, 0);
45-
static const ignition::math::Vector3d kRightRotation = ignition::math::Vector3d(0, -1, 0);
46-
4739
/// \brief A Ray Sensor Plugin
4840
class GAZEBO_VISIBLE RayPlugin : public SensorPlugin
4941
{
@@ -71,7 +63,8 @@ namespace gazebo
7163
std::string namespace_;
7264
double min_distance_;
7365
double max_distance_;
74-
int rotation_;
66+
67+
gazebo::msgs::Quaternion orientation_;
7568

7669
/// \brief The connection tied to RayPlugin::OnNewLaserScans()
7770
private:

‎include/gazebo_mavlink_interface.h

+3
Original file line numberDiff line numberDiff line change
@@ -263,6 +263,9 @@ class GazeboMavlinkInterface : public ModelPlugin {
263263
bool IsRunning();
264264
void onSigInt();
265265

266+
template <class T>
267+
void setSensorOrientation(ignition::math::Vector3d u_Xs, T sensor_msg);
268+
266269
// Serial interface
267270
void open();
268271
void close();

‎models/lidar/model.sdf

+1-1
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,7 @@
2525
</material>
2626
</visual>
2727
<sensor name="laser" type="ray">
28-
<pose>0 0 0 0 -1.570896 0</pose>
28+
<pose>0 0 0 0 1.570896 0</pose>
2929
<ray>
3030
<scan>
3131
<horizontal>

‎msgs/Range.proto

+3-4
Original file line numberDiff line numberDiff line change
@@ -8,8 +8,7 @@ message Range
88
required float min_distance = 2;
99
required float max_distance = 3;
1010
required float current_distance = 4;
11-
required int32 rotation = 5;
12-
optional float h_fov = 6;
13-
optional float v_fov = 7;
14-
optional gazebo.msgs.Quaternion orientation = 8;
11+
optional float h_fov = 5;
12+
optional float v_fov = 6;
13+
optional gazebo.msgs.Quaternion orientation = 7;
1514
}

‎src/gazebo_lidar_plugin.cpp

+8-18
Original file line numberDiff line numberDiff line change
@@ -170,23 +170,11 @@ void RayPlugin::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
170170
ignition::math::Quaterniond q_ls = parentSensor_->Pose().Rot(); // This is the rotation of the parent sensor WRT parent sensor link
171171
ignition::math::Quaterniond q_bs = (q_bl * q_ls).Inverse(); // This is the rotation of the parent sensor WRT `base_link`
172172

173-
const ignition::math::Vector3d u_Xb = kForwardRotation; // This is unit vector of X-axis `base_link`
174-
const ignition::math::Vector3d u_Xs = q_bs.RotateVectorReverse(u_Xb); // This is unit vector of X-axis sensor in `base_link` frame
175-
176-
// Current rotation types are described as https://github.com/PX4/Firmware/blob/master/msg/distance_sensor.msg
177-
rotation_ = -1; // Invalid rotation
178-
if(u_Xs.Dot(kDownwardRotation) > 0.99)
179-
rotation_ = 25;
180-
else if(u_Xs.Dot(kUpwardRotation) > 0.99)
181-
rotation_ = 24;
182-
else if(u_Xs.Dot(kBackwardRotation) > 0.99)
183-
rotation_ = 12;
184-
else if(u_Xs.Dot(kForwardRotation) > 0.99)
185-
rotation_ = 0;
186-
else if(u_Xs.Dot(kLeftRotation) > 0.99)
187-
rotation_ = 6;
188-
else if(u_Xs.Dot(kRightRotation) > 0.99)
189-
rotation_ = 2;
173+
// set the orientation
174+
orientation_.set_x(q_bs.X());
175+
orientation_.set_y(q_bs.Y());
176+
orientation_.set_z(q_bs.Z());
177+
orientation_.set_w(q_bs.W());
190178
}
191179

192180
/////////////////////////////////////////////////
@@ -213,7 +201,9 @@ void RayPlugin::OnNewLaserScans()
213201
}
214202

215203
lidar_message.set_current_distance(current_distance);
216-
lidar_message.set_rotation(rotation_);
204+
lidar_message.set_h_fov(0.0523598776); // 3 degrees standard
205+
lidar_message.set_v_fov(0.0523598776); // 3 degrees standard
206+
lidar_message.set_allocated_orientation(new gazebo::msgs::Quaternion(orientation_));
217207

218208
lidar_pub_->Publish(lidar_message);
219209
}

‎src/gazebo_mavlink_interface.cpp

+50-29
Original file line numberDiff line numberDiff line change
@@ -609,6 +609,29 @@ void GazeboMavlinkInterface::send_mavlink_message(const mavlink_message_t *messa
609609
}
610610
}
611611

612+
template <class T>
613+
void GazeboMavlinkInterface::setSensorOrientation(ignition::math::Vector3d u_Xs, T sensor_msg) {
614+
const ignition::math::Vector3d u_Xb = kForwardRotation; // This is unit vector of X-axis `base_link`
615+
616+
// Current rotation types are described as https://mavlink.io/en/messages/common.html#MAV_SENSOR_ORIENTATION
617+
if(u_Xs.Dot(kDownwardRotation) > 0.99)
618+
sensor_msg.orientation = MAV_SENSOR_ORIENTATION::MAV_SENSOR_ROTATION_PITCH_270;
619+
else if(u_Xs.Dot(kUpwardRotation) > 0.99)
620+
sensor_msg.orientation = MAV_SENSOR_ORIENTATION::MAV_SENSOR_ROTATION_PITCH_90;
621+
else if(u_Xs.Dot(kBackwardRotation) > 0.99)
622+
sensor_msg.orientation = MAV_SENSOR_ORIENTATION::MAV_SENSOR_ROTATION_PITCH_180;
623+
else if(u_Xs.Dot(kForwardRotation) > 0.99)
624+
sensor_msg.orientation = MAV_SENSOR_ORIENTATION::MAV_SENSOR_ROTATION_NONE;
625+
else if(u_Xs.Dot(kLeftRotation) > 0.99)
626+
sensor_msg.orientation = MAV_SENSOR_ORIENTATION::MAV_SENSOR_ROTATION_YAW_270;
627+
else if(u_Xs.Dot(kRightRotation) > 0.99)
628+
sensor_msg.orientation = MAV_SENSOR_ORIENTATION::MAV_SENSOR_ROTATION_YAW_90;
629+
else {
630+
sensor_msg.orientation = MAV_SENSOR_ORIENTATION::MAV_SENSOR_ROTATION_CUSTOM;
631+
}
632+
633+
}
634+
612635
void GazeboMavlinkInterface::forward_mavlink_message(const mavlink_message_t *message)
613636
{
614637
if (gotSigInt_) {
@@ -849,8 +872,24 @@ void GazeboMavlinkInterface::LidarCallback(LidarPtr& lidar_message) {
849872
sensor_msg.current_distance = lidar_message->current_distance() * 100.0;
850873
sensor_msg.type = 0;
851874
sensor_msg.id = 0;
852-
sensor_msg.orientation = lidar_message->rotation();
853875
sensor_msg.covariance = 0;
876+
sensor_msg.horizontal_fov = lidar_message->h_fov();
877+
sensor_msg.vertical_fov = lidar_message->v_fov();
878+
sensor_msg.quaternion[0] = lidar_message->orientation().w();
879+
sensor_msg.quaternion[1] = lidar_message->orientation().x();
880+
sensor_msg.quaternion[2] = lidar_message->orientation().y();
881+
sensor_msg.quaternion[3] = lidar_message->orientation().z();
882+
883+
ignition::math::Quaterniond q_bs = ignition::math::Quaterniond(
884+
lidar_message->orientation().w(),
885+
lidar_message->orientation().x(),
886+
lidar_message->orientation().y(),
887+
lidar_message->orientation().z());
888+
889+
const ignition::math::Vector3d u_Xb = kForwardRotation; // This is unit vector of X-axis `base_link`
890+
const ignition::math::Vector3d u_Xs = q_bs.RotateVectorReverse(u_Xb); // This is unit vector of X-axis sensor in `base_link` frame
891+
892+
setSensorOrientation(u_Xs, sensor_msg);
854893

855894
//distance needed for optical flow message
856895
optflow_distance = lidar_message->current_distance(); //[m]
@@ -896,45 +935,27 @@ void GazeboMavlinkInterface::SonarCallback(SonarPtr& sonar_message) {
896935
sensor_msg.min_distance = sonar_message->min_distance() * 100.0;
897936
sensor_msg.max_distance = sonar_message->max_distance() * 100.0;
898937
sensor_msg.current_distance = sonar_message->current_distance() * 100.0;
899-
ignition::math::Quaterniond q_gr = ignition::math::Quaterniond(
938+
939+
ignition::math::Quaterniond q_bs = ignition::math::Quaterniond(
900940
sonar_message->orientation().w(),
901941
sonar_message->orientation().x(),
902942
sonar_message->orientation().y(),
903943
sonar_message->orientation().z());
904944

905-
// rotation of the sensor with respect to the vehicle
906-
ignition::math::Vector3d euler = q_gr.Euler();
907-
int roll = static_cast<int>(round(GetDegrees360(euler.X())));
908-
int pitch = static_cast<int>(round(GetDegrees360(euler.Y())));
909-
int yaw = static_cast<int>(round(GetDegrees360(euler.Z())));
910-
911-
912-
if (roll == 0 && pitch == 0 && yaw == 0) {
913-
sensor_msg.orientation = 25; // downward facing
914-
} else if (roll == 0 && pitch == 180 && yaw == 0) {
915-
sensor_msg.orientation = 24; // upward facing
916-
} else if (roll == 0 && pitch == 90 && yaw == 180) {
917-
sensor_msg.orientation = 12; // backward facing
918-
} else if (roll == 0 && pitch == 90 && yaw == 0) {
919-
sensor_msg.orientation = 0; // forward facing
920-
} else if (roll == 0 && pitch == 90 && yaw == 90) {
921-
sensor_msg.orientation = 6; // left facing
922-
} else if (roll == 0 && pitch == 90 && yaw == 270) {
923-
sensor_msg.orientation = 2; // right facing
924-
} else {
925-
sensor_msg.orientation = 100; // custom orientation
926-
sensor_msg.quaternion[0] = sonar_message->orientation().w();
927-
sensor_msg.quaternion[1] = sonar_message->orientation().x();
928-
sensor_msg.quaternion[2] = sonar_message->orientation().y();
929-
sensor_msg.quaternion[3] = sonar_message->orientation().z();
930-
}
945+
const ignition::math::Vector3d u_Xb = kForwardRotation; // This is unit vector of X-axis `base_link`
946+
const ignition::math::Vector3d u_Xs = q_bs.RotateVectorReverse(u_Xb); // This is unit vector of X-axis sensor in `base_link` frame
947+
948+
setSensorOrientation(u_Xs, sensor_msg);
931949

932950
sensor_msg.type = 1;
933951
sensor_msg.id = 1;
934-
sensor_msg.orientation = sonar_message->rotation();
935952
sensor_msg.covariance = 0;
936953
sensor_msg.horizontal_fov = sonar_message->h_fov();
937954
sensor_msg.vertical_fov = sonar_message->v_fov();
955+
sensor_msg.quaternion[0] = sonar_message->orientation().w();
956+
sensor_msg.quaternion[1] = sonar_message->orientation().x();
957+
sensor_msg.quaternion[2] = sonar_message->orientation().y();
958+
sensor_msg.quaternion[3] = sonar_message->orientation().z();
938959

939960
mavlink_message_t msg;
940961
mavlink_msg_distance_sensor_encode_chan(1, 200, MAVLINK_COMM_0, &msg, &sensor_msg);

‎src/gazebo_sonar_plugin.cpp

-1
Original file line numberDiff line numberDiff line change
@@ -105,7 +105,6 @@ void SonarPlugin::OnNewScans()
105105
sonar_message.set_min_distance(parentSensor->RangeMin());
106106
sonar_message.set_max_distance(parentSensor->RangeMax());
107107
sonar_message.set_current_distance(parentSensor->Range());
108-
sonar_message.set_rotation(rotation_);
109108

110109
sonar_message.set_h_fov(2.0f * atan(parentSensor->Radius() / parentSensor->RangeMax()));
111110
sonar_message.set_v_fov(2.0f * atan(parentSensor->Radius() / parentSensor->RangeMax()));

0 commit comments

Comments
 (0)