From 6db6da650d0d58c6678a7e8945d55f04544733a3 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Wed, 12 Feb 2020 15:57:08 +0000 Subject: [PATCH 1/5] gazebo_mavlink_interface: add the possbility to subscribe to multiple lidar sensors connect to the model --- include/gazebo_mavlink_interface.h | 15 ++-- src/gazebo_mavlink_interface.cpp | 119 ++++++++++++++++++++++++----- 2 files changed, 109 insertions(+), 25 deletions(-) diff --git a/include/gazebo_mavlink_interface.h b/include/gazebo_mavlink_interface.h index 5c65023098..e7d4eb0939 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 @@ -162,7 +163,7 @@ class GazeboMavlinkInterface : public ModelPlugin { groundtruth_lat_rad(0.0), groundtruth_lon_rad(0.0), groundtruth_altitude(0.0), - lidar_orientation_ {}, + lidar_orientations_ {}, sonar_orientation_ {}, mavlink_udp_port_(kDefaultMavlinkUdpPort), mavlink_tcp_port_(kDefaultMavlinkTcpPort), @@ -248,7 +249,7 @@ 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 LidarCallback(LidarPtr& lidar_msg, uint8_t id); void SonarCallback(SonarPtr& sonar_msg); void OpticalFlowCallback(OpticalFlowPtr& opticalFlow_msg); void IRLockCallback(IRLockPtr& irlock_msg); @@ -298,7 +299,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_; @@ -308,6 +308,9 @@ class GazeboMavlinkInterface : public ModelPlugin { transport::SubscriberPtr mag_sub_; transport::SubscriberPtr baro_sub_; + // array of SubscriberPtrs to multiple lidar subscriptions + std::vector lidar_subs_; + std::string imu_sub_topic_; std::string lidar_sub_topic_; std::string opticalFlow_sub_topic_; @@ -332,8 +335,10 @@ 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 + std::vector lidar_orientations_; ///< Lidars link orientations with respect to the base_link + ignition::math::Quaterniond sonar_orientation_; ///< Sonar link orientation with respect to the base_link + + std::vector lidar_ids_; ignition::math::Vector3d gravity_W_; ignition::math::Vector3d velocity_prev_W_; diff --git a/src/gazebo_mavlink_interface.cpp b/src/gazebo_mavlink_interface.cpp index e5ce8cbd06..f37f94117e 100644 --- a/src/gazebo_mavlink_interface.cpp +++ b/src/gazebo_mavlink_interface.cpp @@ -30,6 +30,27 @@ 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&, uint8_t); + + /// \brief The sensor ID. + uint8_t 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); + } +}; + void GazeboMavlinkInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) { model_ = _model; @@ -268,9 +289,65 @@ void GazeboMavlinkInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf sigIntConnection_ = event::Events::ConnectSigInt( boost::bind(&GazeboMavlinkInterface::onSigInt, this)); + // Get model joints in order to check for sensors + auto joints = model_->GetJoints(); + std::vector joint_names; + for (size_t y = 0; y < joints.size(); y ++) { + joint_names.push_back(joints[y]->GetName()); + } + + const std::regex lidar_model("(lidar|sf10a)(.*_joint)"); + const std::regex sonar_model("(sonar|mb1240-xl-ez4)(.*_joint)"); + + // Verify if the Lidar (or model specific) sensor joint exists + for (std::vector::iterator it = joint_names.begin(); it != joint_names.end(); ++it) { + if (std::regex_match (*it, lidar_model)) { + // Get sensor link name + const std::string link_name = (*it).substr(0, (*it).size() - 6); + + // Get sensor ID from link name + uint8_t lidar_id = 0; + try { + lidar_id = std::stoi(link_name.substr(link_name.find_last_not_of("0123456789") + 1)); + } catch(...) { + gzwarn << "No identifier on Lidar link. Using 0 as default sensor ID" << std::endl; + } + + // Get the lidar link orientation with respect to the base_link + const auto lidar_link = model_->GetLink(link_name + "::link"); + if (lidar_link != NULL) { +#if GAZEBO_MAJOR_VERSION >= 9 + lidar_orientations_.push_back(lidar_link->RelativePose().Rot()); +#else + lidar_orientations_.push_back(ignitionFromGazeboMath(lidar_link->GetRelativePose()).Rot()); +#endif + lidar_ids_.push_back(lidar_id); + } + + // 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{this, &GazeboMavlinkInterface::LidarCallback, lidar_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 lidar sensors + auto subscriberPtr = node_handle_->Subscribe("~/" + model_->GetName() + "/link/" + link_name, + &SensorHelperStorage::callback, + &callback_entry.first->second); + + lidar_subs_.push_back(subscriberPtr); + } + } + // 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); @@ -552,16 +629,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) { @@ -950,14 +1017,14 @@ void GazeboMavlinkInterface::GroundtruthCallback(GtPtr& groundtruth_msg) { // the FCU } -void GazeboMavlinkInterface::LidarCallback(LidarPtr& lidar_message) { +void GazeboMavlinkInterface::LidarCallback(LidarPtr& lidar_message, uint8_t 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 +1035,12 @@ 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(std::vector::size_type i = 0; i != lidar_ids_.size(); i++) { + if (lidar_ids_[i] == id) { + q_bs = (lidar_orientations_[i] * q_ls).Inverse(); + } + } sensor_msg.quaternion[0] = q_bs.W(); sensor_msg.quaternion[1] = q_bs.X(); @@ -980,8 +1052,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); @@ -1042,6 +1116,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); From 16c078442974a37099d9dc95a9d0595bfb10473a Mon Sep 17 00:00:00 2001 From: TSC21 Date: Wed, 12 Feb 2020 18:11:22 +0000 Subject: [PATCH 2/5] gazebo_mavlink_interface: generalize multi-sensor subscription --- include/gazebo_mavlink_interface.h | 37 ++++--- src/gazebo_mavlink_interface.cpp | 165 ++++++++++++++++------------- 2 files changed, 110 insertions(+), 92 deletions(-) diff --git a/include/gazebo_mavlink_interface.h b/include/gazebo_mavlink_interface.h index e7d4eb0939..9b3cd3f9e0 100644 --- a/include/gazebo_mavlink_interface.h +++ b/include/gazebo_mavlink_interface.h @@ -84,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 kDefaultLidarModelJointNaming("(lidar|sf10a)(.*_joint)"); +static const std::regex kDefaultSonarModelJointNaming("(sonar|mb1240-xl-ez4)(.*_joint)"); + namespace gazebo { typedef const boost::shared_ptr CommandMotorSpeedPtr; @@ -106,9 +110,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"; @@ -139,8 +141,6 @@ 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), @@ -163,8 +163,7 @@ class GazeboMavlinkInterface : public ModelPlugin { groundtruth_lat_rad(0.0), groundtruth_lon_rad(0.0), groundtruth_altitude(0.0), - lidar_orientations_ {}, - sonar_orientation_ {}, + sensor_orientations_ {}, mavlink_udp_port_(kDefaultMavlinkUdpPort), mavlink_tcp_port_(kDefaultMavlinkTcpPort), simulator_socket_fd_(0), @@ -250,7 +249,7 @@ class GazeboMavlinkInterface : public ModelPlugin { void GpsCallback(GpsPtr& gps_msg); void GroundtruthCallback(GtPtr& groundtruth_msg); void LidarCallback(LidarPtr& lidar_msg, uint8_t id); - void SonarCallback(SonarPtr& sonar_msg); + void SonarCallback(SonarPtr& sonar_msg, uint8_t id); void OpticalFlowCallback(OpticalFlowPtr& opticalFlow_msg); void IRLockCallback(IRLockPtr& irlock_msg); void VisionCallback(OdomPtr& odom_msg); @@ -277,6 +276,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&, uint8_t), + GazeboMavlinkInterface* ptr); + // Serial interface void open(); void close(); @@ -299,7 +310,6 @@ class GazeboMavlinkInterface : public ModelPlugin { transport::PublisherPtr joint_control_pub_[n_out_max]; transport::SubscriberPtr imu_sub_; - transport::SubscriberPtr sonar_sub_; transport::SubscriberPtr opticalFlow_sub_; transport::SubscriberPtr irlock_sub_; transport::SubscriberPtr gps_sub_; @@ -308,13 +318,10 @@ class GazeboMavlinkInterface : public ModelPlugin { transport::SubscriberPtr mag_sub_; transport::SubscriberPtr baro_sub_; - // array of SubscriberPtrs to multiple lidar subscriptions - std::vector lidar_subs_; + std::vector sensor_subs_; ///< array of SubscriberPtrs to multiple sensor subscriptions 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_; @@ -335,10 +342,8 @@ class GazeboMavlinkInterface : public ModelPlugin { double imu_update_interval_ = 0.004; ///< Used for non-lockstep - std::vector lidar_orientations_; ///< Lidars link orientations with respect to the base_link - ignition::math::Quaterniond sonar_orientation_; ///< Sonar link orientation with respect to the base_link - - std::vector lidar_ids_; + std::vector sensor_orientations_; ///< Sensor link orientations with respect to the base_link + std::vector sensor_ids_; ///< Sensor IDs ignition::math::Vector3d gravity_W_; ignition::math::Vector3d velocity_prev_W_; diff --git a/src/gazebo_mavlink_interface.cpp b/src/gazebo_mavlink_interface.cpp index f37f94117e..e8ecd60598 100644 --- a/src/gazebo_mavlink_interface.cpp +++ b/src/gazebo_mavlink_interface.cpp @@ -51,6 +51,80 @@ struct SensorHelperStorage { } }; +template +void GazeboMavlinkInterface::CreateSensorSubscription( + void (GazeboMavlinkInterface::*fp)(const boost::shared_ptr&, uint8_t), + GazeboMavlinkInterface* ptr) { + + // Adjust regex according to the function pointer + std::regex model; + if (fp == &GazeboMavlinkInterface::LidarCallback) { + model = kDefaultLidarModelJointNaming; + + } else if (fp == &GazeboMavlinkInterface::SonarCallback) { + model = kDefaultSonarModelJointNaming; + + } else { + gzerr << "Unsupported sensor type callback. Add support by creating a regex to a specific sensor joint first. " + << "Then extend the check for the function pointer to the specific topic callback." + << std::endl; + } + + // Get model joints in order to check for sensors + auto joints = model_->GetJoints(); + std::vector joint_names; + for (size_t y = 0; y < joints.size(); y ++) { + joint_names.push_back(joints[y]->GetName()); + } + + // Verify if the Sensor (or model specific) sensor joint exists + for (std::vector::iterator it = joint_names.begin(); it != joint_names.end(); ++it) { + if (std::regex_match (*it, model)) { + // Get sensor link name + const std::string link_name = (*it).substr(0, (*it).size() - 6); + + // Get sensor ID from link name + uint8_t sensor_id = 0; + try { + 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 + const auto sensor_link = model_->GetLink(link_name + "::link"); + if (sensor_link != NULL) { +#if GAZEBO_MAJOR_VERSION >= 9 + sensor_orientations_.push_back(sensor_link->RelativePose().Rot()); +#else + sensor_orientations_.push_back(ignitionFromGazeboMath(sensor_link->GetRelativePose()).Rot()); +#endif + sensor_ids_.push_back(sensor_id); + } + + // 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); + + sensor_subs_.push_back(subscriberPtr); + } + } +} + void GazeboMavlinkInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) { model_ = _model; @@ -75,10 +149,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_); @@ -289,67 +361,9 @@ void GazeboMavlinkInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf sigIntConnection_ = event::Events::ConnectSigInt( boost::bind(&GazeboMavlinkInterface::onSigInt, this)); - // Get model joints in order to check for sensors - auto joints = model_->GetJoints(); - std::vector joint_names; - for (size_t y = 0; y < joints.size(); y ++) { - joint_names.push_back(joints[y]->GetName()); - } - - const std::regex lidar_model("(lidar|sf10a)(.*_joint)"); - const std::regex sonar_model("(sonar|mb1240-xl-ez4)(.*_joint)"); - - // Verify if the Lidar (or model specific) sensor joint exists - for (std::vector::iterator it = joint_names.begin(); it != joint_names.end(); ++it) { - if (std::regex_match (*it, lidar_model)) { - // Get sensor link name - const std::string link_name = (*it).substr(0, (*it).size() - 6); - - // Get sensor ID from link name - uint8_t lidar_id = 0; - try { - lidar_id = std::stoi(link_name.substr(link_name.find_last_not_of("0123456789") + 1)); - } catch(...) { - gzwarn << "No identifier on Lidar link. Using 0 as default sensor ID" << std::endl; - } - - // Get the lidar link orientation with respect to the base_link - const auto lidar_link = model_->GetLink(link_name + "::link"); - if (lidar_link != NULL) { -#if GAZEBO_MAJOR_VERSION >= 9 - lidar_orientations_.push_back(lidar_link->RelativePose().Rot()); -#else - lidar_orientations_.push_back(ignitionFromGazeboMath(lidar_link->GetRelativePose()).Rot()); -#endif - lidar_ids_.push_back(lidar_id); - } - - // 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{this, &GazeboMavlinkInterface::LidarCallback, lidar_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 lidar sensors - auto subscriberPtr = node_handle_->Subscribe("~/" + model_->GetName() + "/link/" + link_name, - &SensorHelperStorage::callback, - &callback_entry.first->second); - - lidar_subs_.push_back(subscriberPtr); - } - } - // Subscribe to messages of other plugins. imu_sub_ = node_handle_->Subscribe("~/" + model_->GetName() + imu_sub_topic_, &GazeboMavlinkInterface::ImuCallback, 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); @@ -357,6 +371,10 @@ 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); + // Create subscriptions to the distance sensors + CreateSensorSubscription(&GazeboMavlinkInterface::LidarCallback, this); + CreateSensorSubscription(&GazeboMavlinkInterface::SonarCallback, this); + // Publish gazebo's motor_speed message motor_velocity_reference_pub_ = node_handle_->Advertise("~/" + model_->GetName() + motor_velocity_reference_pub_topic_, 1); @@ -628,16 +646,6 @@ void GazeboMavlinkInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf } standard_normal_distribution_ = std::normal_distribution(0.0f, 1.0f); - - // 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. @@ -1036,9 +1044,9 @@ void GazeboMavlinkInterface::LidarCallback(LidarPtr& lidar_message, uint8_t id) lidar_message->orientation().z()); ignition::math::Quaterniond q_bs; - for(std::vector::size_type i = 0; i != lidar_ids_.size(); i++) { - if (lidar_ids_[i] == id) { - q_bs = (lidar_orientations_[i] * q_ls).Inverse(); + for(std::vector::size_type i = 0; i != sensor_ids_.size(); i++) { + if (sensor_ids_[i] == id) { + q_bs = (sensor_orientations_[i] * q_ls).Inverse(); } } @@ -1092,14 +1100,19 @@ void GazeboMavlinkInterface::OpticalFlowCallback(OpticalFlowPtr& opticalFlow_mes send_mavlink_message(&msg); } -void GazeboMavlinkInterface::SonarCallback(SonarPtr& sonar_message) { +void GazeboMavlinkInterface::SonarCallback(SonarPtr& sonar_message, uint8_t 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(std::vector::size_type i = 0; i != sensor_ids_.size(); i++) { + if (sensor_ids_[i] == id) { + q_ls = (sensor_orientations_[i]).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 @@ -1107,7 +1120,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(); From 9def734d2eae705542279c9b488735263ed5096c Mon Sep 17 00:00:00 2001 From: TSC21 Date: Wed, 12 Feb 2020 19:04:29 +0000 Subject: [PATCH 3/5] gazebo_mavlink_interface: make id const and pass by reference --- include/gazebo_mavlink_interface.h | 6 +++--- src/gazebo_mavlink_interface.cpp | 8 ++++---- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/include/gazebo_mavlink_interface.h b/include/gazebo_mavlink_interface.h index 9b3cd3f9e0..8fbc32971f 100644 --- a/include/gazebo_mavlink_interface.h +++ b/include/gazebo_mavlink_interface.h @@ -248,8 +248,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, uint8_t id); - void SonarCallback(SonarPtr& sonar_msg, uint8_t id); + void LidarCallback(LidarPtr& lidar_msg, const uint8_t& id); + void SonarCallback(SonarPtr& sonar_msg, const uint8_t& id); void OpticalFlowCallback(OpticalFlowPtr& opticalFlow_msg); void IRLockCallback(IRLockPtr& irlock_msg); void VisionCallback(OdomPtr& odom_msg); @@ -285,7 +285,7 @@ class GazeboMavlinkInterface : public ModelPlugin { */ template void CreateSensorSubscription( - void (GazeboMavlinkInterface::*fp)(const boost::shared_ptr&, uint8_t), + void (GazeboMavlinkInterface::*fp)(const boost::shared_ptr&, const uint8_t&), GazeboMavlinkInterface* ptr); // Serial interface diff --git a/src/gazebo_mavlink_interface.cpp b/src/gazebo_mavlink_interface.cpp index e8ecd60598..d508563693 100644 --- a/src/gazebo_mavlink_interface.cpp +++ b/src/gazebo_mavlink_interface.cpp @@ -38,7 +38,7 @@ struct SensorHelperStorage { GazeboMavlinkInterface* ptr; /// \brief Function pointer to the subscriber callback with additional parameters. - void (GazeboMavlinkInterface::*fp)(const boost::shared_ptr&, uint8_t); + void (GazeboMavlinkInterface::*fp)(const boost::shared_ptr&, const uint8_t&); /// \brief The sensor ID. uint8_t sensor_id; @@ -53,7 +53,7 @@ struct SensorHelperStorage { template void GazeboMavlinkInterface::CreateSensorSubscription( - void (GazeboMavlinkInterface::*fp)(const boost::shared_ptr&, uint8_t), + void (GazeboMavlinkInterface::*fp)(const boost::shared_ptr&, const uint8_t&), GazeboMavlinkInterface* ptr) { // Adjust regex according to the function pointer @@ -1025,7 +1025,7 @@ void GazeboMavlinkInterface::GroundtruthCallback(GtPtr& groundtruth_msg) { // the FCU } -void GazeboMavlinkInterface::LidarCallback(LidarPtr& lidar_message, uint8_t id) { +void GazeboMavlinkInterface::LidarCallback(LidarPtr& lidar_message, const uint8_t& id) { mavlink_distance_sensor_t sensor_msg; sensor_msg.time_boot_ms = lidar_message->time_usec() / 1e3; // [ms] sensor_msg.min_distance = lidar_message->min_distance() * 100.0; // [cm] @@ -1100,7 +1100,7 @@ void GazeboMavlinkInterface::OpticalFlowCallback(OpticalFlowPtr& opticalFlow_mes send_mavlink_message(&msg); } -void GazeboMavlinkInterface::SonarCallback(SonarPtr& sonar_message, uint8_t id) { +void GazeboMavlinkInterface::SonarCallback(SonarPtr& sonar_message, const uint8_t& 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; From 6bf35a4f0579dce5c936edd6017fdf98e25efedd Mon Sep 17 00:00:00 2001 From: TSC21 Date: Thu, 13 Feb 2020 12:06:30 +0000 Subject: [PATCH 4/5] gazebo_mavlink_interface: CreateSensorSubscription: use links instead of joints; use std::map to store SubsPtr, ID and orientation of the sensors --- include/gazebo_mavlink_interface.h | 22 ++++---- src/gazebo_mavlink_interface.cpp | 81 +++++++++++++++--------------- 2 files changed, 52 insertions(+), 51 deletions(-) diff --git a/include/gazebo_mavlink_interface.h b/include/gazebo_mavlink_interface.h index 8fbc32971f..1a97c55f97 100644 --- a/include/gazebo_mavlink_interface.h +++ b/include/gazebo_mavlink_interface.h @@ -85,8 +85,8 @@ 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 kDefaultLidarModelJointNaming("(lidar|sf10a)(.*_joint)"); -static const std::regex kDefaultSonarModelJointNaming("(sonar|mb1240-xl-ez4)(.*_joint)"); +static const std::regex kDefaultLidarModelLinkNaming("(lidar|sf10a)(.*::link)"); +static const std::regex kDefaultSonarModelLinkNaming("(sonar|mb1240-xl-ez4)(.*::link)"); namespace gazebo { @@ -102,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 = ""; @@ -146,6 +149,7 @@ class GazeboMavlinkInterface : public ModelPlugin { vision_sub_topic_(kDefaultVisionTopic), mag_sub_topic_(kDefaultMagTopic), baro_sub_topic_(kDefaultBarometerTopic), + sensor_map_ {}, model_ {}, world_(nullptr), left_elevon_joint_(nullptr), @@ -163,7 +167,6 @@ class GazeboMavlinkInterface : public ModelPlugin { groundtruth_lat_rad(0.0), groundtruth_lon_rad(0.0), groundtruth_altitude(0.0), - sensor_orientations_ {}, 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, const uint8_t& id); - void SonarCallback(SonarPtr& sonar_msg, const uint8_t& id); + 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); @@ -285,8 +288,8 @@ class GazeboMavlinkInterface : public ModelPlugin { */ template void CreateSensorSubscription( - void (GazeboMavlinkInterface::*fp)(const boost::shared_ptr&, const uint8_t&), - GazeboMavlinkInterface* ptr); + void (GazeboMavlinkInterface::*fp)(const boost::shared_ptr&, const int&), + GazeboMavlinkInterface* ptr, const physics::Link_V& links); // Serial interface void open(); @@ -318,7 +321,7 @@ class GazeboMavlinkInterface : public ModelPlugin { transport::SubscriberPtr mag_sub_; transport::SubscriberPtr baro_sub_; - std::vector sensor_subs_; ///< array of SubscriberPtrs to multiple sensor subscriptions + Sensor_M sensor_map_; // Map of sensor SubscriberPtr, IDs and orientations std::string imu_sub_topic_; std::string opticalFlow_sub_topic_; @@ -342,9 +345,6 @@ class GazeboMavlinkInterface : public ModelPlugin { double imu_update_interval_ = 0.004; ///< Used for non-lockstep - std::vector sensor_orientations_; ///< Sensor link orientations with respect to the base_link - std::vector sensor_ids_; ///< Sensor IDs - ignition::math::Vector3d gravity_W_; ignition::math::Vector3d velocity_prev_W_; ignition::math::Vector3d mag_n_; diff --git a/src/gazebo_mavlink_interface.cpp b/src/gazebo_mavlink_interface.cpp index d508563693..240cefa8ad 100644 --- a/src/gazebo_mavlink_interface.cpp +++ b/src/gazebo_mavlink_interface.cpp @@ -38,10 +38,10 @@ struct SensorHelperStorage { GazeboMavlinkInterface* ptr; /// \brief Function pointer to the subscriber callback with additional parameters. - void (GazeboMavlinkInterface::*fp)(const boost::shared_ptr&, const uint8_t&); + void (GazeboMavlinkInterface::*fp)(const boost::shared_ptr&, const int&); /// \brief The sensor ID. - uint8_t 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 @@ -53,54 +53,47 @@ struct SensorHelperStorage { template void GazeboMavlinkInterface::CreateSensorSubscription( - void (GazeboMavlinkInterface::*fp)(const boost::shared_ptr&, const uint8_t&), - GazeboMavlinkInterface* ptr) { + 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) { - model = kDefaultLidarModelJointNaming; + // should look for lidar links + model = kDefaultLidarModelLinkNaming; } else if (fp == &GazeboMavlinkInterface::SonarCallback) { - model = kDefaultSonarModelJointNaming; + // should look for sonar links + model = kDefaultSonarModelLinkNaming; } else { - gzerr << "Unsupported sensor type callback. Add support by creating a regex to a specific sensor joint first. " - << "Then extend the check for the function pointer to the specific topic callback." + 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; } - // Get model joints in order to check for sensors - auto joints = model_->GetJoints(); - std::vector joint_names; - for (size_t y = 0; y < joints.size(); y ++) { - joint_names.push_back(joints[y]->GetName()); - } + // 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)) { - // Verify if the Sensor (or model specific) sensor joint exists - for (std::vector::iterator it = joint_names.begin(); it != joint_names.end(); ++it) { - if (std::regex_match (*it, model)) { - // Get sensor link name - const std::string link_name = (*it).substr(0, (*it).size() - 6); + // 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 - uint8_t sensor_id = 0; + 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 - const auto sensor_link = model_->GetLink(link_name + "::link"); - if (sensor_link != NULL) { -#if GAZEBO_MAJOR_VERSION >= 9 - sensor_orientations_.push_back(sensor_link->RelativePose().Rot()); -#else - sensor_orientations_.push_back(ignitionFromGazeboMath(sensor_link->GetRelativePose()).Rot()); -#endif - sensor_ids_.push_back(sensor_id); - } + #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; @@ -120,7 +113,10 @@ void GazeboMavlinkInterface::CreateSensorSubscription( &SensorHelperStorage::callback, &callback_entry.first->second); - sensor_subs_.push_back(subscriberPtr); + // Store the SubscriberPtr, sensor ID and sensor orientation + sensor_map_.insert(std::pair(subscriberPtr, + SensorIdRot_P(sensor_id, sensor_orientation)) + ); } } } @@ -371,9 +367,12 @@ 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); - CreateSensorSubscription(&GazeboMavlinkInterface::SonarCallback, this); + 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); @@ -1025,7 +1024,7 @@ void GazeboMavlinkInterface::GroundtruthCallback(GtPtr& groundtruth_msg) { // the FCU } -void GazeboMavlinkInterface::LidarCallback(LidarPtr& lidar_message, const uint8_t& id) { +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; // [ms] sensor_msg.min_distance = lidar_message->min_distance() * 100.0; // [cm] @@ -1044,9 +1043,10 @@ void GazeboMavlinkInterface::LidarCallback(LidarPtr& lidar_message, const uint8_ lidar_message->orientation().z()); ignition::math::Quaterniond q_bs; - for(std::vector::size_type i = 0; i != sensor_ids_.size(); i++) { - if (sensor_ids_[i] == id) { - q_bs = (sensor_orientations_[i] * q_ls).Inverse(); + 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(); } } @@ -1100,7 +1100,7 @@ void GazeboMavlinkInterface::OpticalFlowCallback(OpticalFlowPtr& opticalFlow_mes send_mavlink_message(&msg); } -void GazeboMavlinkInterface::SonarCallback(SonarPtr& sonar_message, const uint8_t& id) { +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; @@ -1108,9 +1108,10 @@ void GazeboMavlinkInterface::SonarCallback(SonarPtr& sonar_message, const uint8_ sensor_msg.current_distance = sonar_message->current_distance() * 100.0; ignition::math::Quaterniond q_ls; - for(std::vector::size_type i = 0; i != sensor_ids_.size(); i++) { - if (sensor_ids_[i] == id) { - q_ls = (sensor_orientations_[i]).Inverse(); + 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(); } } From e2fe7e9803b4e2a98f54b93c1bd0876660a9016f Mon Sep 17 00:00:00 2001 From: TSC21 Date: Thu, 13 Feb 2020 12:53:13 +0000 Subject: [PATCH 5/5] iris_obs_avoid: extend with 4 more distance sensors (downward, forward, left and right) --- models/iris_obs_avoid/iris_obs_avoid.sdf | 77 +++++++++++++++++++++++- 1 file changed, 76 insertions(+), 1 deletion(-) 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 + + + + + + + + + + +