From dae0fdeb754392f138563b69a84488f8e1668755 Mon Sep 17 00:00:00 2001 From: TSC21 Date: Sat, 17 Aug 2019 18:09:58 +0100 Subject: [PATCH] add support for both sonar and lidar rotation determination by transform checks --- include/common.h | 91 ++++++++++++------ include/gazebo_lidar_plugin.h | 6 +- include/gazebo_mavlink_interface.h | 2 +- include/gazebo_sonar_plugin.h | 26 +++-- models/iris_irlock/iris_irlock.sdf | 4 +- models/iris_opt_flow/iris_opt_flow.sdf | 6 +- models/lidar/model.sdf | 3 +- models/mb1240-xl-ez4/mb1240-xl-ez4.sdf.jinja | 2 + models/px4flow/px4flow.sdf.jinja | 3 +- models/sonar/model.sdf | 29 +++--- src/gazebo_lidar_plugin.cpp | 46 +++++---- src/gazebo_sonar_plugin.cpp | 99 +++++++++++--------- 12 files changed, 191 insertions(+), 126 deletions(-) diff --git a/include/common.h b/include/common.h index 7e81acc5f9b5..ed6370783b68 100644 --- a/include/common.h +++ b/include/common.h @@ -22,6 +22,7 @@ #include +#include #include #include #include @@ -220,27 +221,10 @@ const ignition::math::Quaterniond getSensorOrientation(const gazebo::physics::Mo const std::string& parentSensorModelName, T& parentSensor) { - // Get the `base_link` rotation WRT world - gazebo::physics::LinkPtr baseLink = nullptr; - std::vector linkList = rootModel->GetLinks(); // Get list of all links in the root model - for(auto link : linkList) { - std::string linkName = link->GetName(); - if(linkName.find("::base_link") != std::string::npos) { - baseLink = rootModel->GetLink(linkName); // Get the pointer to the `base_link` - break; - } - } - if (!baseLink) - gzthrow("Sensor plugin requires the `base_link` element to be defined"); - - // This is the rotation of the 'base_link` WRT world -#if GAZEBO_MAJOR_VERSION >= 9 - ignition::math::Quaterniond q_wb = baseLink->WorldPose().Rot(); -#else - ignition::math::Quaterniond q_wb = ignitionFromGazeboMath(baseLink->GetWorldPose()).Rot(); -#endif + // Get list of all links in the root model + std::vector linkList = rootModel->GetLinks(); - // Get the parent sensor link rotation WRT world + // Get the parent sensor link rotation WRT parent entity gazebo::physics::LinkPtr parentSensorLink = nullptr; for(auto link : linkList) { std::string linkName = link->GetName(); @@ -251,19 +235,70 @@ const ignition::math::Quaterniond getSensorOrientation(const gazebo::physics::Mo } if (!parentSensorLink) gzthrow("Sensor plugin requires a `link` element for its parent sensor to be defined"); - - // This is the rotation of the parentSensorLink WRT world #if GAZEBO_MAJOR_VERSION >= 9 - ignition::math::Quaterniond q_wl = parentSensorLink->WorldPose().Rot(); + ignition::math::Quaterniond q_pl = parentSensorLink->RelativePose().Rot(); #else - ignition::math::Quaterniond q_wl = ignitionFromGazeboMath(parentSensorLink->GetWorldPose()).Rot(); + ignition::math::Quaterniond q_pl = ignitionFromGazeboMath(parentSensorLink->RelativePose()).Rot(); #endif // Calculate parent sensor rotation WRT `base_link` - ignition::math::Quaterniond q_bl = q_wb.Inverse() * q_wl; // This is the rotation of the parent sensor link WRT `base_link` - ignition::math::Quaterniond q_ls = parentSensor->Pose().Rot(); // This is the rotation of the parent sensor WRT parent sensor link - - return (q_bl * q_ls).Inverse(); // Return the rotation of the parent sensor WRT `base_link` + ignition::math::Quaterniond q_ls = parentSensor->Pose().Rot(); + + /** + * @note Given a bug on the sonar sensor which doesn't allow to obtain the + * correct sensor to link rotation one must use the rotation of base_link WRT + * to the sensor link so to get the proper rotation. + * + * Also, it's assumed on the SDF of the sonar sensor a -90 degrees rotation in + * pitch so the sensor ray matches the X-axis of the link: + * + * 0 0 0 0 -1.57 0 + * + * ... + * + * + * + * + * + * + * - Example of a forward facing sonar link: + * + * model://sonar + * 0 0 -0.05 0 0 0 + * + * + * - For a lidar sensor link however, the direction would depend on the sensor + * pose orientation, since all the transforms are taken into account. + * + * - Ex lidar sensor: + * + * 0 0 0 0 1.57 0 + * + * + * + * + * + * + * + * + * - For the above sensor to be poiting forward, then the link should have the + * following pose: + * + * model://lidar + * 0 0 -0.05 0 -1.57 0 + * + */ + + // Check if the sensor is of type SonarSensor + std::string sensorType = typeid(parentSensor).name(); + if (sensorType.find("SonarSensor") != std::string::npos) { + return q_pl.Inverse(); + + } else { + + ignition::math::Quaterniond q_bs = (q_pl * q_ls).Inverse(); + return q_bs; // Return the rotation of the parent sensor WRT `base_link` + } } /** diff --git a/include/gazebo_lidar_plugin.h b/include/gazebo_lidar_plugin.h index 313f94d21799..ebc6eccab1f1 100644 --- a/include/gazebo_lidar_plugin.h +++ b/include/gazebo_lidar_plugin.h @@ -63,7 +63,7 @@ namespace gazebo public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf); /// \brief Pointer to parent - protected: physics::WorldPtr world; + protected: physics::WorldPtr world_; /// \brief The parent sensor private: @@ -79,8 +79,8 @@ namespace gazebo /// \brief The connection tied to RayPlugin::OnNewLaserScans() private: - event::ConnectionPtr newLaserScansConnection; - sensor_msgs::msgs::Range lidar_message; + event::ConnectionPtr newLaserScansConnection_; + sensor_msgs::msgs::Range lidar_message_; }; } #endif diff --git a/include/gazebo_mavlink_interface.h b/include/gazebo_mavlink_interface.h index c0688feb16ce..1818e83ab861 100644 --- a/include/gazebo_mavlink_interface.h +++ b/include/gazebo_mavlink_interface.h @@ -107,7 +107,7 @@ static const std::string kDefaultMotorVelocityReferencePubTopic = "/gazebo/comma static const std::string kDefaultImuTopic = "/imu"; static const std::string kDefaultLidarTopic = "/link/lidar"; static const std::string kDefaultOpticalFlowTopic = "/px4flow/link/opticalFlow"; -static const std::string kDefaultSonarTopic = "/sonar_model/link/sonar"; +static const std::string kDefaultSonarTopic = "/link/sonar"; static const std::string kDefaultIRLockTopic = "/camera/link/irlock"; static const std::string kDefaultGPSTopic = "/gps"; static const std::string kDefaultVisionTopic = "/vision_odom"; diff --git a/include/gazebo_sonar_plugin.h b/include/gazebo_sonar_plugin.h index 099bcb7be5a1..4f0e3533ccaf 100644 --- a/include/gazebo_sonar_plugin.h +++ b/include/gazebo_sonar_plugin.h @@ -22,16 +22,24 @@ #ifndef _GAZEBO_SONAR_PLUGIN_HH_ #define _GAZEBO_SONAR_PLUGIN_HH_ +#include +#include #include +#include +#include "gazebo/physics/physics.hh" +#include "gazebo/transport/transport.hh" + +#include "gazebo/msgs/msgs.hh" #include #include -#include + +#include #include namespace gazebo { - static constexpr uint8_t kDefaultRotation = 0; // current types are described as https://github.com/PX4/Firmware/blob/master/msg/distance_sensor.msg + static constexpr auto kDefaultSonarTopic = "sonar"; /// \brief A Ray Sensor Plugin class GAZEBO_VISIBLE SonarPlugin : public SensorPlugin @@ -50,20 +58,22 @@ namespace gazebo public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf); /// \brief Pointer to parent - protected: physics::WorldPtr world; + protected: physics::WorldPtr world_; /// \brief The parent sensor private: - sensors::SonarSensorPtr parentSensor; + sensors::SonarSensorPtr parentSensor_; + std::string sonar_topic_; transport::NodePtr node_handle_; transport::PublisherPtr sonar_pub_; std::string namespace_; - int rotation_; - /// \brief The connection tied to RayPlugin::OnNewLaserScans() + gazebo::msgs::Quaternion orientation_; + + /// \brief The connection tied to SonarPlugin::OnNewScans() private: - event::ConnectionPtr newScansConnection; - sensor_msgs::msgs::Range sonar_message; + event::ConnectionPtr newScansConnection_; + sensor_msgs::msgs::Range sonar_message_; }; } #endif diff --git a/models/iris_irlock/iris_irlock.sdf b/models/iris_irlock/iris_irlock.sdf index 446e3abd503b..f67b75609603 100644 --- a/models/iris_irlock/iris_irlock.sdf +++ b/models/iris_irlock/iris_irlock.sdf @@ -7,7 +7,7 @@ model://lidar - -0.12 0 0 0 3.1415 0 + -0.12 0 0 0 0 0 lidar::link @@ -38,4 +38,4 @@ - \ No newline at end of file + diff --git a/models/iris_opt_flow/iris_opt_flow.sdf b/models/iris_opt_flow/iris_opt_flow.sdf index a4619d751640..3ffd1bee08c9 100644 --- a/models/iris_opt_flow/iris_opt_flow.sdf +++ b/models/iris_opt_flow/iris_opt_flow.sdf @@ -24,13 +24,13 @@ - model://sf10a + model://lidar 0 0 -0.05 0 0 0 - + iris::base_link - sf10a::link + lidar::link diff --git a/models/lidar/model.sdf b/models/lidar/model.sdf index 5064b1da8f84..6ac4d15a2266 100644 --- a/models/lidar/model.sdf +++ b/models/lidar/model.sdf @@ -2,6 +2,7 @@ + 0 0 0 0 0 0 0 0 0 0 0 0 0.01 @@ -25,7 +26,7 @@ - 0 0 0 0 1.570896 0 + 0 0 0 0 1.57079633 0 diff --git a/models/mb1240-xl-ez4/mb1240-xl-ez4.sdf.jinja b/models/mb1240-xl-ez4/mb1240-xl-ez4.sdf.jinja index 0316eae767f2..d0e2160abb57 100644 --- a/models/mb1240-xl-ez4/mb1240-xl-ez4.sdf.jinja +++ b/models/mb1240-xl-ez4/mb1240-xl-ez4.sdf.jinja @@ -6,6 +6,7 @@ {%- set ixx = m/12*(w**2 + h**2) -%} {%- set iyy = m/12*(l**2 + h**2) -%} {%- set izz = m/12*(l**2 + w**2) -%} +{%- set deg2rad = 0.01745329251 -%} {%- macro box(l, w, h) -%} @@ -46,6 +47,7 @@ + 0 0 0 0 {{ -90*deg2rad }} 0 0 2.00 diff --git a/models/px4flow/px4flow.sdf.jinja b/models/px4flow/px4flow.sdf.jinja index 966ddb4ca803..937a409d9bf5 100644 --- a/models/px4flow/px4flow.sdf.jinja +++ b/models/px4flow/px4flow.sdf.jinja @@ -43,13 +43,14 @@ - 0 0 0 0 -1.5707963259 3.141592652 + 0 0 0 0 -{{ 90*deg2rad }} {{ 180*deg2rad }} 1 200 + diff --git a/models/sonar/model.sdf b/models/sonar/model.sdf index 65495d238cc9..3abdb34cf600 100644 --- a/models/sonar/model.sdf +++ b/models/sonar/model.sdf @@ -1,5 +1,6 @@ + - + 0 0 0 0 0 0 @@ -16,22 +17,26 @@ - 0.005 0.015 0.005 + + 0.005 0.015 0.005 + - + - 0 0 0 0 1.57 0 - - 0.02 - 5.0 - 1.33974388395 - - + 0 0 0 0 -1.57 0 + + 0.02 + 5.0 + 1.33974388395 + + + + 1 20 true diff --git a/src/gazebo_lidar_plugin.cpp b/src/gazebo_lidar_plugin.cpp index 70e710f0e20b..93b38f08658e 100644 --- a/src/gazebo_lidar_plugin.cpp +++ b/src/gazebo_lidar_plugin.cpp @@ -43,26 +43,24 @@ RayPlugin::RayPlugin() ///////////////////////////////////////////////// RayPlugin::~RayPlugin() { - this->newLaserScansConnection->~Connection(); - - this->newLaserScansConnection.reset(); - - this->parentSensor_.reset(); - this->world.reset(); + newLaserScansConnection_->~Connection(); + newLaserScansConnection_.reset(); + parentSensor_.reset(); + world_.reset(); } ///////////////////////////////////////////////// void RayPlugin::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) { // Get then name of the parent sensor - this->parentSensor_ = std::dynamic_pointer_cast(_parent); + parentSensor_ = std::dynamic_pointer_cast(_parent); - if (!this->parentSensor_) + if (!parentSensor_) gzthrow("RayPlugin requires a Ray Sensor as its parent"); - this->world = physics::get_world(this->parentSensor_->WorldName()); + world_ = physics::get_world(parentSensor_->WorldName()); - this->newLaserScansConnection = this->parentSensor_->LaserShape()->ConnectNewLaserScans( + newLaserScansConnection_ = parentSensor_->LaserShape()->ConnectNewLaserScans( boost::bind(&RayPlugin::OnNewLaserScans, this)); if (_sdf->HasElement("robotNamespace")) @@ -93,8 +91,8 @@ void RayPlugin::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) } // get lidar topic name - if(_sdf->HasElement("lidarTopic")) { - lidar_topic_ = _sdf->GetElement("lidarTopic")->Get(); + if(_sdf->HasElement("topic")) { + lidar_topic_ = parentSensor_->Topic(); } else { lidar_topic_ = kDefaultLidarTopic; gzwarn << "[gazebo_lidar_plugin] Using default lidar topic " << lidar_topic_ << "\n"; @@ -114,9 +112,9 @@ void RayPlugin::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) // Get the pointer to the root model #if GAZEBO_MAJOR_VERSION >= 9 - const physics::ModelPtr rootModel = this->world->ModelByName(rootModelName); + const physics::ModelPtr rootModel = world_->ModelByName(rootModelName); #else - const physics::ModelPtr rootModel = this->world->GetModel(rootModelName); + const physics::ModelPtr rootModel = world_->GetModel(rootModelName); #endif // the second to the last name is the model name @@ -140,14 +138,14 @@ void RayPlugin::OnNewLaserScans() { // Get the current simulation time. #if GAZEBO_MAJOR_VERSION >= 9 - common::Time now = world->SimTime(); + common::Time now = world_->SimTime(); #else - common::Time now = world->GetSimTime(); + common::Time now = world_->GetSimTime(); #endif - lidar_message.set_time_usec(now.Double() * 1e6); - lidar_message.set_min_distance(min_distance_); - lidar_message.set_max_distance(max_distance_); + lidar_message_.set_time_usec(now.Double() * 1e6); + lidar_message_.set_min_distance(min_distance_); + lidar_message_.set_max_distance(max_distance_); double current_distance = parentSensor_->Range(0); @@ -158,10 +156,10 @@ void RayPlugin::OnNewLaserScans() current_distance = max_distance_; } - lidar_message.set_current_distance(current_distance); - lidar_message.set_h_fov(kDefaultFOV); - lidar_message.set_v_fov(kDefaultFOV); - lidar_message.set_allocated_orientation(new gazebo::msgs::Quaternion(orientation_)); + lidar_message_.set_current_distance(current_distance); + lidar_message_.set_h_fov(kDefaultFOV); + lidar_message_.set_v_fov(kDefaultFOV); + lidar_message_.set_allocated_orientation(new gazebo::msgs::Quaternion(orientation_)); - lidar_pub_->Publish(lidar_message); + lidar_pub_->Publish(lidar_message_); } diff --git a/src/gazebo_sonar_plugin.cpp b/src/gazebo_sonar_plugin.cpp index 7b4a921f804f..333ad7cd2bbb 100644 --- a/src/gazebo_sonar_plugin.cpp +++ b/src/gazebo_sonar_plugin.cpp @@ -19,16 +19,8 @@ * Author: Nate Koenig mod by John Hsu */ -#include "gazebo/physics/physics.hh" #include "gazebo_sonar_plugin.h" -#include -#include -#include -#include -#include "gazebo/transport/transport.hh" -#include "gazebo/msgs/msgs.hh" - #include #include #include @@ -50,71 +42,92 @@ SonarPlugin::SonarPlugin() ///////////////////////////////////////////////// SonarPlugin::~SonarPlugin() { - this->parentSensor.reset(); - this->world.reset(); + newScansConnection_->~Connection(); + newScansConnection_.reset(); + parentSensor_.reset(); + world_.reset(); } ///////////////////////////////////////////////// void SonarPlugin::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) { -// Get then name of the parent sensor - this->parentSensor = std::dynamic_pointer_cast(_parent); + // Get then name of the parent sensor + parentSensor_ = std::dynamic_pointer_cast(_parent); - if (!this->parentSensor) + if (!parentSensor_) gzthrow("SonarPlugin requires a Sonar Sensor as its parent"); - this->world = physics::get_world(this->parentSensor->WorldName()); + world_ = physics::get_world(parentSensor_->WorldName()); - this->parentSensor->SetActive(false); - this->newScansConnection = this->parentSensor->ConnectUpdated(boost::bind(&SonarPlugin::OnNewScans, this)); - this->parentSensor->SetActive(true); + parentSensor_->SetActive(false); + newScansConnection_ = parentSensor_->ConnectUpdated(boost::bind(&SonarPlugin::OnNewScans, this)); + parentSensor_->SetActive(true); if (_sdf->HasElement("robotNamespace")) namespace_ = _sdf->GetElement("robotNamespace")->Get(); else gzwarn << "[gazebo_sonar_plugin] Please specify a robotNamespace.\n"; - // get rotation of the sensor according to https://github.com/PX4/Firmware/blob/master/msg/distance_sensor.msg - if (_sdf->HasElement("rotation")) { - rotation_ = _sdf->GetElement("rotation")->Get(); + // get sonar topic name + if(_sdf->HasElement("topic")) { + sonar_topic_ = parentSensor_->Topic(); } else { - gzwarn << "[gazebo_sonar_plugin] Using default rotation: " << kDefaultRotation << "\n"; - rotation_ = kDefaultRotation; + sonar_topic_ = kDefaultSonarTopic; + gzwarn << "[gazebo_sonar_plugin] Using default sonar topic " << sonar_topic_ << "\n"; } node_handle_ = transport::NodePtr(new transport::Node()); node_handle_->Init(namespace_); + // Get the root model name const string scopedName = _parent->ParentName(); - string topicName = "~/" + scopedName + "/sonar"; - boost::replace_all(topicName, "::", "/"); + vector names_splitted; + boost::split(names_splitted, scopedName, boost::is_any_of("::")); + names_splitted.erase(std::remove_if(begin(names_splitted), end(names_splitted), + [](const string& name) + { return name.size() == 0; }), end(names_splitted)); + std::string rootModelName = names_splitted.front(); // The first element is the name of the root model + + // Get the pointer to the root model +#if GAZEBO_MAJOR_VERSION >= 9 + const physics::ModelPtr rootModel = world_->ModelByName(rootModelName); +#else + const physics::ModelPtr rootModel = world_->GetModel(rootModelName); +#endif + + // the second to the last name is the model name + const std::string parentSensorModelName = names_splitted.rbegin()[1]; - sonar_pub_ = node_handle_->Advertise(topicName, 10); + // Get the sensor orientation + const ignition::math::Quaterniond q_bs = getSensorOrientation(rootModel, parentSensorModelName, parentSensor_); + + // set the orientation + orientation_.set_x(q_bs.X()); + orientation_.set_y(q_bs.Y()); + orientation_.set_z(q_bs.Z()); + orientation_.set_w(q_bs.W()); + + // start sonar topic publishing + sonar_pub_ = node_handle_->Advertise("~/" + names_splitted[0] + "/link/" + sonar_topic_, 10); } void SonarPlugin::OnNewScans() { // Get the current simulation time. #if GAZEBO_MAJOR_VERSION >= 9 - common::Time now = world->SimTime(); + common::Time now = world_->SimTime(); #else - common::Time now = world->GetSimTime(); + common::Time now = world_->GetSimTime(); #endif - sonar_message.set_time_usec(now.Double() * 1e6); - sonar_message.set_min_distance(parentSensor->RangeMin()); - sonar_message.set_max_distance(parentSensor->RangeMax()); - sonar_message.set_current_distance(parentSensor->Range()); - - sonar_message.set_h_fov(2.0f * atan(parentSensor->Radius() / parentSensor->RangeMax())); - sonar_message.set_v_fov(2.0f * atan(parentSensor->Radius() / parentSensor->RangeMax())); - ignition::math::Quaterniond pose_model_quaternion = parentSensor->Pose().Rot(); - gazebo::msgs::Quaternion* orientation = new gazebo::msgs::Quaternion(); - orientation->set_x(pose_model_quaternion.X()); - orientation->set_y(pose_model_quaternion.Y()); - orientation->set_z(pose_model_quaternion.Z()); - orientation->set_w(pose_model_quaternion.W()); - sonar_message.set_allocated_orientation(orientation); - - sonar_pub_->Publish(sonar_message); + sonar_message_.set_time_usec(now.Double() * 1e6); + sonar_message_.set_min_distance(parentSensor_->RangeMin()); + sonar_message_.set_max_distance(parentSensor_->RangeMax()); + sonar_message_.set_current_distance(parentSensor_->Range()); + + sonar_message_.set_h_fov(2.0f * atan(parentSensor_->Radius() / parentSensor_->RangeMax())); + sonar_message_.set_v_fov(2.0f * atan(parentSensor_->Radius() / parentSensor_->RangeMax())); + sonar_message_.set_allocated_orientation(new gazebo::msgs::Quaternion(orientation_)); + + sonar_pub_->Publish(sonar_message_); }