diff --git a/include/gazebo_mavlink_interface.h b/include/gazebo_mavlink_interface.h index 5c65023098..1a97c55f97 100644 --- a/include/gazebo_mavlink_interface.h +++ b/include/gazebo_mavlink_interface.h @@ -19,6 +19,7 @@ * limitations under the License. */ #include +#include #include #include #include @@ -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 CommandMotorSpeedPtr; @@ -97,6 +102,9 @@ typedef const boost::shared_ptr GpsPtr; typedef const boost::shared_ptr MagnetometerPtr; typedef const boost::shared_ptr BarometerPtr; +typedef std::pair SensorIdRot_P; +typedef std::map Sensor_M; + // Default values static const std::string kDefaultNamespace = ""; @@ -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"; @@ -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), @@ -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), @@ -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); void OpticalFlowCallback(OpticalFlowPtr& opticalFlow_msg); void IRLockCallback(IRLockPtr& irlock_msg); void VisionCallback(OdomPtr& odom_msg); @@ -276,6 +279,18 @@ class GazeboMavlinkInterface : public ModelPlugin { template 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 + void CreateSensorSubscription( + void (GazeboMavlinkInterface::*fp)(const boost::shared_ptr&, const int&), + GazeboMavlinkInterface* ptr, const physics::Link_V& links); + // Serial interface void open(); void close(); @@ -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_; @@ -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_; @@ -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_; diff --git a/models/iris_obs_avoid/iris_obs_avoid.sdf b/models/iris_obs_avoid/iris_obs_avoid.sdf index e6dc0b02b7..9f4332b052 100644 --- a/models/iris_obs_avoid/iris_obs_avoid.sdf +++ b/models/iris_obs_avoid/iris_obs_avoid.sdf @@ -21,5 +21,80 @@ - + + + model://lidar + 0 0 -0.05 0 0 0 + lidar0 + + + + iris::base_link + lidar0::link + + + + + model://lidar + 0 0 -0.05 0 -1.57079633 0 + lidar1 + + + + iris::base_link + lidar1::link + + + + + model://lidar + 0 0 -0.05 -1.57079633 0 0 + lidar2 + + + + iris::base_link + lidar2::link + + + + + model://lidar + 0 0 -0.05 1.57079633 0 0 + lidar3 + + + + iris::base_link + lidar3::link + + + + + + + + + + + diff --git a/src/gazebo_mavlink_interface.cpp b/src/gazebo_mavlink_interface.cpp index e5ce8cbd06..240cefa8ad 100644 --- a/src/gazebo_mavlink_interface.cpp +++ b/src/gazebo_mavlink_interface.cpp @@ -30,6 +30,97 @@ GazeboMavlinkInterface::~GazeboMavlinkInterface() { updateConnection_->~Connection(); } +/// \brief A helper class that provides storage for additional parameters that are inserted into the callback. +/// \details GazeboMsgT The type of the message that will be subscribed to the Gazebo framework. +template +struct SensorHelperStorage { + /// \brief Pointer to the ROS interface plugin class. + GazeboMavlinkInterface* ptr; + + /// \brief Function pointer to the subscriber callback with additional parameters. + void (GazeboMavlinkInterface::*fp)(const boost::shared_ptr&, const int&); + + /// \brief The sensor ID. + int sensor_id; + + /// \brief This is what gets passed into the Gazebo Subscribe method as a callback, + /// and hence can onlyhave one parameter (note boost::bind() does not work with the + /// current Gazebo Subscribe() definitions). + void callback(const boost::shared_ptr& msg_ptr) { + (ptr->*fp)(msg_ptr, sensor_id); + } +}; + +template +void GazeboMavlinkInterface::CreateSensorSubscription( + void (GazeboMavlinkInterface::*fp)(const boost::shared_ptr&, const int&), + GazeboMavlinkInterface* ptr, const physics::Link_V& links) { + + // Adjust regex according to the function pointer + std::regex model; + if (fp == &GazeboMavlinkInterface::LidarCallback) { + // should look for lidar links + model = kDefaultLidarModelLinkNaming; + + } else if (fp == &GazeboMavlinkInterface::SonarCallback) { + // should look for sonar links + model = kDefaultSonarModelLinkNaming; + + } else { + gzerr << "Unsupported sensor type callback. Add support by creating a regex to a specific sensor link first. " + << "Then extend the check for the function pointer in CreateSensorSubscription() to the specific topic callback." + << std::endl; + } + + // Verify if the sensor link exists + for (physics::Link_V::const_iterator it = links.begin(); it != links.end(); ++it) { + if (std::regex_match ((*it)->GetName(), model)) { + + // Get sensor link name (without the ''::link' suffix) + const std::string link_name = (*it)->GetName().substr(0, (*it)->GetName().size() - 6); + + // Get sensor ID from link name + int sensor_id = 0; + try { + // get the sensor id by getting the (last) numbers on the link name (ex. lidar10::link, gets id 10) + sensor_id = std::stoi(link_name.substr(link_name.find_last_not_of("0123456789") + 1)); + } catch(...) { + gzwarn << "No identifier on link. Using 0 as default sensor ID" << std::endl; + } + + // Get the sensor link orientation with respect to the base_link + #if GAZEBO_MAJOR_VERSION >= 9 + const auto sensor_orientation = (*it)->RelativePose().Rot(); + #else + const auto sensor_orientation = ignitionFromGazeboMath((*it)->GetRelativePose()).Rot(); + #endif + + // One map will be created for each Gazebo message type + static std::map > callback_map; + + // Store the callback entries + auto callback_entry = callback_map.emplace( + "~/" + model_->GetName() + "/link/" + link_name, + SensorHelperStorage{ptr, fp, sensor_id}); + + // Check if element was already present + if (!callback_entry.second) + gzerr << "Tried to add element to map but the gazebo topic name was already present in map." + << std::endl; + + // Create the subscriber for the sensors + auto subscriberPtr = node_handle_->Subscribe("~/" + model_->GetName() + "/link/" + link_name, + &SensorHelperStorage::callback, + &callback_entry.first->second); + + // Store the SubscriberPtr, sensor ID and sensor orientation + sensor_map_.insert(std::pair(subscriberPtr, + SensorIdRot_P(sensor_id, sensor_orientation)) + ); + } + } +} + void GazeboMavlinkInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) { model_ = _model; @@ -54,10 +145,8 @@ void GazeboMavlinkInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf getSdfParam(_sdf, "imuSubTopic", imu_sub_topic_, imu_sub_topic_); getSdfParam(_sdf, "gpsSubTopic", gps_sub_topic_, gps_sub_topic_); getSdfParam(_sdf, "visionSubTopic", vision_sub_topic_, vision_sub_topic_); - getSdfParam(_sdf, "lidarSubTopic", lidar_sub_topic_, lidar_sub_topic_); getSdfParam(_sdf, "opticalFlowSubTopic", opticalFlow_sub_topic_, opticalFlow_sub_topic_); - getSdfParam(_sdf, "sonarSubTopic", sonar_sub_topic_, sonar_sub_topic_); getSdfParam(_sdf, "irlockSubTopic", irlock_sub_topic_, irlock_sub_topic_); getSdfParam(_sdf, "magSubTopic", mag_sub_topic_, mag_sub_topic_); getSdfParam(_sdf, "baroSubTopic", baro_sub_topic_, baro_sub_topic_); @@ -270,9 +359,7 @@ void GazeboMavlinkInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf // Subscribe to messages of other plugins. imu_sub_ = node_handle_->Subscribe("~/" + model_->GetName() + imu_sub_topic_, &GazeboMavlinkInterface::ImuCallback, this); - lidar_sub_ = node_handle_->Subscribe("~/" + model_->GetName() + "/link/" + lidar_sub_topic_, &GazeboMavlinkInterface::LidarCallback, this); opticalFlow_sub_ = node_handle_->Subscribe("~/" + model_->GetName() + opticalFlow_sub_topic_, &GazeboMavlinkInterface::OpticalFlowCallback, this); - sonar_sub_ = node_handle_->Subscribe("~/" + model_->GetName() + "/link/" + sonar_sub_topic_, &GazeboMavlinkInterface::SonarCallback, this); irlock_sub_ = node_handle_->Subscribe("~/" + model_->GetName() + irlock_sub_topic_, &GazeboMavlinkInterface::IRLockCallback, this); gps_sub_ = node_handle_->Subscribe("~/" + model_->GetName() + gps_sub_topic_, &GazeboMavlinkInterface::GpsCallback, this); groundtruth_sub_ = node_handle_->Subscribe("~/" + model_->GetName() + groundtruth_sub_topic_, &GazeboMavlinkInterface::GroundtruthCallback, this); @@ -280,6 +367,13 @@ void GazeboMavlinkInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf mag_sub_ = node_handle_->Subscribe("~/" + model_->GetName() + mag_sub_topic_, &GazeboMavlinkInterface::MagnetometerCallback, this); baro_sub_ = node_handle_->Subscribe("~/" + model_->GetName() + baro_sub_topic_, &GazeboMavlinkInterface::BarometerCallback, this); + // Get the model links + auto links = model_->GetLinks(); + + // Create subscriptions to the distance sensors + CreateSensorSubscription(&GazeboMavlinkInterface::LidarCallback, this, links); + CreateSensorSubscription(&GazeboMavlinkInterface::SonarCallback, this, links); + // Publish gazebo's motor_speed message motor_velocity_reference_pub_ = node_handle_->Advertise("~/" + model_->GetName() + motor_velocity_reference_pub_topic_, 1); @@ -551,26 +645,6 @@ void GazeboMavlinkInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf } standard_normal_distribution_ = std::normal_distribution(0.0f, 1.0f); - - // Get the lidar link orientation with respect to the base_link - const auto lidar_link = model_->GetLink(lidar_sub_topic_ + "::link"); - if (lidar_link != NULL) { -#if GAZEBO_MAJOR_VERSION >= 9 - lidar_orientation_ = lidar_link->RelativePose().Rot(); -#else - lidar_orientation_ = ignitionFromGazeboMath(lidar_link->GetRelativePose()).Rot(); -#endif - } - - // Get the sonar link orientation with respect to the base_link - const auto sonar_link = model_->GetLink(sonar_sub_topic_ + "::link"); - if (sonar_link != NULL) { -#if GAZEBO_MAJOR_VERSION >= 9 - sonar_orientation_ = sonar_link->RelativePose().Rot(); -#else - sonar_orientation_ = ignitionFromGazeboMath(sonar_link->GetRelativePose()).Rot(); -#endif - } } // This gets called by the world update start event. @@ -950,14 +1024,14 @@ void GazeboMavlinkInterface::GroundtruthCallback(GtPtr& groundtruth_msg) { // the FCU } -void GazeboMavlinkInterface::LidarCallback(LidarPtr& lidar_message) { +void GazeboMavlinkInterface::LidarCallback(LidarPtr& lidar_message, const int& id) { mavlink_distance_sensor_t sensor_msg; - sensor_msg.time_boot_ms = lidar_message->time_usec() / 1e3; - sensor_msg.min_distance = lidar_message->min_distance() * 100.0; - sensor_msg.max_distance = lidar_message->max_distance() * 100.0; - sensor_msg.current_distance = lidar_message->current_distance() * 100.0; + sensor_msg.time_boot_ms = lidar_message->time_usec() / 1e3; // [ms] + sensor_msg.min_distance = lidar_message->min_distance() * 100.0; // [cm] + sensor_msg.max_distance = lidar_message->max_distance() * 100.0; // [cm] + sensor_msg.current_distance = lidar_message->current_distance() * 100.0; // [cm] sensor_msg.type = 0; - sensor_msg.id = 0; + sensor_msg.id = id; sensor_msg.covariance = 0; sensor_msg.horizontal_fov = lidar_message->h_fov(); sensor_msg.vertical_fov = lidar_message->v_fov(); @@ -968,7 +1042,13 @@ void GazeboMavlinkInterface::LidarCallback(LidarPtr& lidar_message) { lidar_message->orientation().y(), lidar_message->orientation().z()); - ignition::math::Quaterniond q_bs = (lidar_orientation_ * q_ls).Inverse(); + ignition::math::Quaterniond q_bs; + for (Sensor_M::iterator it = sensor_map_.begin(); it != sensor_map_.end(); ++it) { + // check the ID of the sensor on the sensor map and apply the respective rotation + if (it->second.first == id) { + q_bs = (it->second.second * q_ls).Inverse(); + } + } sensor_msg.quaternion[0] = q_bs.W(); sensor_msg.quaternion[1] = q_bs.X(); @@ -980,8 +1060,10 @@ void GazeboMavlinkInterface::LidarCallback(LidarPtr& lidar_message) { setMavlinkSensorOrientation(u_Xs, sensor_msg); - //distance needed for optical flow message - optflow_distance = lidar_message->current_distance(); //[m] + // distance needed for optical flow message + if (sensor_msg.orientation == MAV_SENSOR_ORIENTATION::MAV_SENSOR_ROTATION_PITCH_270) { + optflow_distance = lidar_message->current_distance(); // [m] + } mavlink_message_t msg; mavlink_msg_distance_sensor_encode_chan(1, 200, MAVLINK_COMM_0, &msg, &sensor_msg); @@ -1018,14 +1100,20 @@ void GazeboMavlinkInterface::OpticalFlowCallback(OpticalFlowPtr& opticalFlow_mes send_mavlink_message(&msg); } -void GazeboMavlinkInterface::SonarCallback(SonarPtr& sonar_message) { +void GazeboMavlinkInterface::SonarCallback(SonarPtr& sonar_message, const int& id) { mavlink_distance_sensor_t sensor_msg = {}; sensor_msg.time_boot_ms = sonar_message->time_usec() / 1e3; sensor_msg.min_distance = sonar_message->min_distance() * 100.0; sensor_msg.max_distance = sonar_message->max_distance() * 100.0; sensor_msg.current_distance = sonar_message->current_distance() * 100.0; - ignition::math::Quaterniond q_ls = sonar_orientation_.Inverse(); + ignition::math::Quaterniond q_ls; + for (Sensor_M::iterator it = sensor_map_.begin(); it != sensor_map_.end(); ++it) { + // check the ID of the sensor on the sensor map and apply the respective rotation + if (it->second.first == id) { + q_ls = (it->second.second).Inverse(); + } + } const ignition::math::Vector3d u_Xb = kForwardRotation; // This is unit vector of X-axis `base_link` const ignition::math::Vector3d u_Xs = q_ls.RotateVectorReverse(u_Xb); // This is unit vector of X-axis sensor in `base_link` frame @@ -1033,7 +1121,7 @@ void GazeboMavlinkInterface::SonarCallback(SonarPtr& sonar_message) { setMavlinkSensorOrientation(u_Xs, sensor_msg); sensor_msg.type = 1; - sensor_msg.id = 1; + sensor_msg.id = 100 + id; // to differentiate from Lidars sensor_msg.covariance = 0; sensor_msg.horizontal_fov = sonar_message->h_fov(); sensor_msg.vertical_fov = sonar_message->v_fov(); @@ -1042,6 +1130,11 @@ void GazeboMavlinkInterface::SonarCallback(SonarPtr& sonar_message) { sensor_msg.quaternion[2] = q_ls.Y(); sensor_msg.quaternion[3] = q_ls.Z(); + // distance needed for optical flow message + if (sensor_msg.orientation == MAV_SENSOR_ORIENTATION::MAV_SENSOR_ROTATION_PITCH_270) { + optflow_distance = sonar_message->current_distance(); // [m] + } + mavlink_message_t msg; mavlink_msg_distance_sensor_encode_chan(1, 200, MAVLINK_COMM_0, &msg, &sensor_msg); send_mavlink_message(&msg);