Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Feature: add multi sensor subscription #411

Merged
merged 5 commits into from
Feb 13, 2020
Merged

Feature: add multi sensor subscription #411

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
6db6da6
gazebo_mavlink_interface: add the possbility to subscribe to multiple…
TSC21 Feb 12, 2020
16c0784
gazebo_mavlink_interface: generalize multi-sensor subscription
TSC21 Feb 12, 2020
9def734
gazebo_mavlink_interface: make id const and pass by reference
TSC21 Feb 12, 2020
6bf35a4
gazebo_mavlink_interface: CreateSensorSubscription: use links instead…
TSC21 Feb 13, 2020
e2fe7e9
iris_obs_avoid: extend with 4 more distance sensors (downward, forwar…
TSC21 Feb 13, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
40 changes: 25 additions & 15 deletions include/gazebo_mavlink_interface.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
* limitations under the License.
*/
#include <vector>
#include <regex>
#include <thread>
#include <mutex>
#include <condition_variable>
Expand Down Expand Up @@ -83,6 +84,10 @@ static constexpr auto kDefaultBaudRate = 921600;
static constexpr ssize_t MAX_SIZE = MAVLINK_MAX_PACKET_LEN + 16;
static constexpr size_t MAX_TXQ_SIZE = 1000;

//! Default distance sensor model joint naming
static const std::regex kDefaultLidarModelLinkNaming("(lidar|sf10a)(.*::link)");
static const std::regex kDefaultSonarModelLinkNaming("(sonar|mb1240-xl-ez4)(.*::link)");

namespace gazebo {

typedef const boost::shared_ptr<const mav_msgs::msgs::CommandMotorSpeed> CommandMotorSpeedPtr;
Expand All @@ -97,6 +102,9 @@ typedef const boost::shared_ptr<const sensor_msgs::msgs::SITLGps> GpsPtr;
typedef const boost::shared_ptr<const sensor_msgs::msgs::MagneticField> MagnetometerPtr;
typedef const boost::shared_ptr<const sensor_msgs::msgs::Pressure> BarometerPtr;

typedef std::pair<const int, const ignition::math::Quaterniond> SensorIdRot_P;
typedef std::map<transport::SubscriberPtr, SensorIdRot_P > Sensor_M;

// Default values
static const std::string kDefaultNamespace = "";

Expand All @@ -105,9 +113,7 @@ static const std::string kDefaultNamespace = "";
static const std::string kDefaultMotorVelocityReferencePubTopic = "/gazebo/command/motor_speed";

static const std::string kDefaultImuTopic = "/imu";
static const std::string kDefaultLidarTopic = "lidar";
static const std::string kDefaultOpticalFlowTopic = "/px4flow/link/opticalFlow";
static const std::string kDefaultSonarTopic = "sonar";
static const std::string kDefaultIRLockTopic = "/camera/link/irlock";
static const std::string kDefaultGPSTopic = "/gps";
static const std::string kDefaultVisionTopic = "/vision_odom";
Expand Down Expand Up @@ -138,13 +144,12 @@ class GazeboMavlinkInterface : public ModelPlugin {
send_odometry_(false),
imu_sub_topic_(kDefaultImuTopic),
opticalFlow_sub_topic_(kDefaultOpticalFlowTopic),
lidar_sub_topic_(kDefaultLidarTopic),
sonar_sub_topic_(kDefaultSonarTopic),
irlock_sub_topic_(kDefaultIRLockTopic),
gps_sub_topic_(kDefaultGPSTopic),
vision_sub_topic_(kDefaultVisionTopic),
mag_sub_topic_(kDefaultMagTopic),
baro_sub_topic_(kDefaultBarometerTopic),
sensor_map_ {},
model_ {},
world_(nullptr),
left_elevon_joint_(nullptr),
Expand All @@ -162,8 +167,6 @@ class GazeboMavlinkInterface : public ModelPlugin {
groundtruth_lat_rad(0.0),
groundtruth_lon_rad(0.0),
groundtruth_altitude(0.0),
lidar_orientation_ {},
sonar_orientation_ {},
mavlink_udp_port_(kDefaultMavlinkUdpPort),
mavlink_tcp_port_(kDefaultMavlinkTcpPort),
simulator_socket_fd_(0),
Expand Down Expand Up @@ -248,8 +251,8 @@ class GazeboMavlinkInterface : public ModelPlugin {
void ImuCallback(ImuPtr& imu_msg);
void GpsCallback(GpsPtr& gps_msg);
void GroundtruthCallback(GtPtr& groundtruth_msg);
void LidarCallback(LidarPtr& lidar_msg);
void SonarCallback(SonarPtr& sonar_msg);
void LidarCallback(LidarPtr& lidar_msg, const int& id);
void SonarCallback(SonarPtr& sonar_msg, const int& id);
TSC21 marked this conversation as resolved.
Show resolved Hide resolved
void OpticalFlowCallback(OpticalFlowPtr& opticalFlow_msg);
void IRLockCallback(IRLockPtr& irlock_msg);
void VisionCallback(OdomPtr& odom_msg);
Expand All @@ -276,6 +279,18 @@ class GazeboMavlinkInterface : public ModelPlugin {
template <class T>
void setMavlinkSensorOrientation(const ignition::math::Vector3d& u_Xs, T& sensor_msg);

/**
* @brief A helper class that allows the creation of multiple subscriptions to sensors.
* It gets the sensor link/joint and creates the subscriptions based on those.
* It also allows to set the initial rotation of the sensor, to allow computing
* the sensor orientation quaternion.
* @details GazeboMsgT The type of the message that will be subscribed to the Gazebo framework.
*/
template <typename GazeboMsgT>
void CreateSensorSubscription(
void (GazeboMavlinkInterface::*fp)(const boost::shared_ptr<GazeboMsgT const>&, const int&),
GazeboMavlinkInterface* ptr, const physics::Link_V& links);

// Serial interface
void open();
void close();
Expand All @@ -298,8 +313,6 @@ class GazeboMavlinkInterface : public ModelPlugin {
transport::PublisherPtr joint_control_pub_[n_out_max];

transport::SubscriberPtr imu_sub_;
transport::SubscriberPtr lidar_sub_;
transport::SubscriberPtr sonar_sub_;
transport::SubscriberPtr opticalFlow_sub_;
transport::SubscriberPtr irlock_sub_;
transport::SubscriberPtr gps_sub_;
Expand All @@ -308,10 +321,10 @@ class GazeboMavlinkInterface : public ModelPlugin {
transport::SubscriberPtr mag_sub_;
transport::SubscriberPtr baro_sub_;

Sensor_M sensor_map_; // Map of sensor SubscriberPtr, IDs and orientations

std::string imu_sub_topic_;
std::string lidar_sub_topic_;
std::string opticalFlow_sub_topic_;
std::string sonar_sub_topic_;
std::string irlock_sub_topic_;
std::string gps_sub_topic_;
std::string groundtruth_sub_topic_;
Expand All @@ -332,9 +345,6 @@ class GazeboMavlinkInterface : public ModelPlugin {

double imu_update_interval_ = 0.004; ///< Used for non-lockstep

ignition::math::Quaterniond lidar_orientation_; ///< Lidar link orientation with respect to the base_link
ignition::math::Quaterniond sonar_orientation_; ///< Sonar link orientation with respect to the base_link

ignition::math::Vector3d gravity_W_;
ignition::math::Vector3d velocity_prev_W_;
ignition::math::Vector3d mag_n_;
Expand Down
77 changes: 76 additions & 1 deletion models/iris_obs_avoid/iris_obs_avoid.sdf
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,5 +21,80 @@
</axis>
</joint>

</model>
<!--downward-facing lidar-->
<include>
<uri>model://lidar</uri>
<pose>0 0 -0.05 0 0 0</pose>
<name>lidar0</name>
</include>

<joint name="lidar0_joint" type="fixed">
<parent>iris::base_link</parent>
<child>lidar0::link</child>
</joint>

<!--forward-facing lidar-->
<include>
<uri>model://lidar</uri>
<pose>0 0 -0.05 0 -1.57079633 0</pose>
<name>lidar1</name>
</include>

<joint name="lidar1_joint" type="fixed">
<parent>iris::base_link</parent>
<child>lidar1::link</child>
</joint>

<!--right-facing lidar-->
<include>
<uri>model://lidar</uri>
<pose>0 0 -0.05 -1.57079633 0 0</pose>
<name>lidar2</name>
</include>

<joint name="lidar2_joint" type="fixed">
<parent>iris::base_link</parent>
<child>lidar2::link</child>
</joint>

<!--left-facing lidar-->
<include>
<uri>model://lidar</uri>
<pose>0 0 -0.05 1.57079633 0 0</pose>
<name>lidar3</name>
</include>

<joint name="lidar3_joint" type="fixed">
<parent>iris::base_link</parent>
<child>lidar3::link</child>
</joint>

<!-- NOTE: PX4 uORB is currently limited to a max of 4 instances per topic
which doesn't allow to add more sensor data stream -->

<!--backward-facing lidar-->
<!-- <include>
<uri>model://lidar</uri>
<pose>0 0 -0.05 0 1.57079633 0</pose>
<name>lidar4</name>
</include>

<joint name="lidar4_joint" type="fixed">
<parent>iris::base_link</parent>
<child>lidar4::link</child>
</joint> -->

<!--upward-facing lidar-->
<!-- <include>
<uri>model://lidar</uri>
<pose>0 0 -0.05 3.14159265 0 0</pose>
<name>lidar5</name>
</include>

<joint name="lidar5_joint" type="fixed">
<parent>iris::base_link</parent>
<child>lidar5::link</child>
</joint> -->

</model>
</sdf>
Loading