diff --git a/simulation/models/achilles/model.sdf b/simulation/models/achilles/model.sdf index 812279a5..3fc1548b 100644 --- a/simulation/models/achilles/model.sdf +++ b/simulation/models/achilles/model.sdf @@ -46,79 +46,52 @@ - - 0.15 0.07 0.053 0 0 0.43633 + + 0.15 0.07 0.053 0 -1.5708 0.43633 false 5 - - - - 3 - 1.0 - -.478 - 0.478 - - - - 0.010 - 3 - 0.01 - - - - 0.005 + + 0.01 + 3.0 + 0.5 + + + 0.15 /achilles/sonarLeft - achilles/us_left_link + achilles/us_left_link + 0.5 - - 0.15 -0.07 0.053 0 0 -0.43633 + + 0.15 -0.07 0.053 0 -1.5708 -0.43633 false 5 - - - - 3 - 1.0 - -.478 - 0.478 - - - - 0.010 - 3 - 0.01 - - - - 0.005 + + 0.01 + 3.0 + 0.5 + + + 0.15 /achilles/sonarRight - achilles/us_right_link + achilles/us_right_link + 0.5 - - 0.15 0 0.053 0 0 0 + + 0.15 0 0.053 0 -1.5708 0 false 5 - - - - 3 - 1.0 - -.478 - 0.478 - - - - 0.010 - 3 - 0.01 - - - - 0.005 + + 0.01 + 3.0 + 0.5 + + + 0.15 /achilles/sonarCenter - achilles/us_center_link + achilles/us_center_link + 0.5 diff --git a/simulation/models/aeneas/model.sdf b/simulation/models/aeneas/model.sdf index f9985fc4..e35d754b 100644 --- a/simulation/models/aeneas/model.sdf +++ b/simulation/models/aeneas/model.sdf @@ -46,79 +46,52 @@ - - 0.15 0.07 0.053 0 0 0.43633 + + 0.15 0.07 0.053 0 -1.5708 0.43633 false 5 - - - - 3 - 1.0 - -.478 - 0.478 - - - - 0.010 - 3 - 0.01 - - - - 0.005 + + 0.01 + 3.0 + 0.5 + + + 0.15 /aeneas/sonarLeft - aeneas/us_left_link + aeneas/us_left_link + 0.5 - - 0.15 -0.07 0.053 0 0 -0.43633 + + 0.15 -0.07 0.053 0 -1.5708 -0.43633 false 5 - - - - 3 - 1.0 - -.478 - 0.478 - - - - 0.010 - 3 - 0.01 - - - - 0.005 + + 0.01 + 3.0 + 0.5 + + + 0.15 /aeneas/sonarRight - aeneas/us_right_link + aeneas/us_right_link + 0.5 - - 0.15 0 0.053 0 0 0 + + 0.15 0 0.053 0 -1.5708 0 false 5 - - - - 3 - 1.0 - -.478 - 0.478 - - - - 0.010 - 3 - 0.01 - - - - 0.005 + + 0.01 + 3.0 + 0.5 + + + 0.15 /aeneas/sonarCenter - aeneas/us_center_link + aeneas/us_center_link + 0.5 diff --git a/simulation/models/ajax/model.sdf b/simulation/models/ajax/model.sdf index 38e7cb83..4a2d0538 100644 --- a/simulation/models/ajax/model.sdf +++ b/simulation/models/ajax/model.sdf @@ -46,79 +46,52 @@ - - 0.15 0.07 0.053 0 0 0.43633 + + 0.15 0.07 0.053 0 -1.5708 0.43633 false 5 - - - - 3 - 1.0 - -.478 - 0.478 - - - - 0.010 - 3 - 0.01 - - - - 0.005 + + 0.01 + 3.0 + 0.5 + + + 0.15 /ajax/sonarLeft - ajax/us_left_link + ajax/us_left_link + 0.5 - - 0.15 -0.07 0.053 0 0 -0.43633 + + 0.15 -0.07 0.053 0 -1.5708 -0.43633 false 5 - - - - 3 - 1.0 - -.478 - 0.478 - - - - 0.010 - 3 - 0.01 - - - - 0.005 + + 0.01 + 3.0 + 0.5 + + + 0.15 /ajax/sonarRight - ajax/us_right_link + ajax/us_right_link + 0.5 - - 0.15 0 0.053 0 0 0 + + 0.15 0 0.053 0 -1.5708 0 false 5 - - - - 3 - 1.0 - -.478 - 0.478 - - - - 0.010 - 3 - 0.01 - - - - 0.005 + + 0.01 + 3.0 + 0.5 + + + 0.15 /ajax/sonarCenter - ajax/us_center_link + ajax/us_center_link + 0.5 diff --git a/simulation/models/diomedes/model.sdf b/simulation/models/diomedes/model.sdf index 31fd3e40..f8a151e2 100644 --- a/simulation/models/diomedes/model.sdf +++ b/simulation/models/diomedes/model.sdf @@ -46,79 +46,52 @@ - - 0.15 0.07 0.053 0 0 0.43633 + + 0.15 0.07 0.053 0 -1.5708 0.43633 false 5 - - - - 3 - 1.0 - -.478 - 0.478 - - - - 0.010 - 3 - 0.01 - - - - 0.005 + + 0.01 + 3.0 + 0.5 + + + 0.15 /diomedes/sonarLeft - diomedes/us_left_link + diomedes/us_left_link + 0.5 - - 0.15 -0.07 0.053 0 0 -0.43633 + + 0.15 -0.07 0.053 0 -1.5708 -0.43633 false 5 - - - - 3 - 1.0 - -.478 - 0.478 - - - - 0.010 - 3 - 0.01 - - - - 0.005 + + 0.01 + 3.0 + 0.5 + + + 0.15 /diomedes/sonarRight - diomedes/us_right_link + diomedes/us_right_link + 0.5 - - 0.15 0 0.053 0 0 0 + + 0.15 0 0.053 0 -1.5708 0 false 5 - - - - 3 - 1.0 - -.478 - 0.478 - - - - 0.010 - 3 - 0.01 - - - - 0.005 + + 0.01 + 3.0 + 0.5 + + + 0.15 /diomedes/sonarCenter - diomedes/us_center_link + diomedes/us_center_link + 0.5 diff --git a/simulation/models/hector/model.sdf b/simulation/models/hector/model.sdf index fc4565d5..aa82dcc6 100644 --- a/simulation/models/hector/model.sdf +++ b/simulation/models/hector/model.sdf @@ -46,79 +46,52 @@ - - 0.15 0.07 0.053 0 0 0.43633 + + 0.15 0.07 0.053 0 -1.5708 0.43633 false 5 - - - - 3 - 1.0 - -.478 - 0.478 - - - - 0.010 - 3 - 0.01 - - - - 0.005 + + 0.01 + 3.0 + 0.5 + + + 0.15 /hector/sonarLeft - hector/us_left_link + hector/us_left_link + 0.5 - - 0.15 -0.07 0.053 0 0 -0.43633 + + 0.15 -0.07 0.053 0 -1.5708 -0.43633 false 5 - - - - 3 - 1.0 - -.478 - 0.478 - - - - 0.010 - 3 - 0.01 - - - - 0.005 + + 0.01 + 3.0 + 0.5 + + + 0.15 /hector/sonarRight - hector/us_right_link + hector/us_right_link + 0.5 - - 0.15 0 0.053 0 0 0 + + 0.15 0 0.053 0 -1.5708 0 false 5 - - - - 3 - 1.0 - -.478 - 0.478 - - - - 0.010 - 3 - 0.01 - - - - 0.005 + + 0.01 + 3.0 + 0.5 + + + 0.15 /hector/sonarCenter - hector/us_center_link + hector/us_center_link + 0.5 diff --git a/simulation/models/paris/model.sdf b/simulation/models/paris/model.sdf index ce442113..bea4305a 100644 --- a/simulation/models/paris/model.sdf +++ b/simulation/models/paris/model.sdf @@ -46,79 +46,52 @@ - - 0.15 0.07 0.053 0 0 0.43633 + + 0.15 0.07 0.053 0 -1.5708 0.43633 false 5 - - - - 3 - 1.0 - -.478 - 0.478 - - - - 0.010 - 3 - 0.01 - - - - 0.005 + + 0.01 + 3.0 + 0.5 + + + 0.15 /paris/sonarLeft - paris/us_left_link + paris/us_left_link + 0.5 - - 0.15 -0.07 0.053 0 0 -0.43633 + + 0.15 -0.07 0.053 0 -1.5708 -0.43633 false 5 - - - - 3 - 1.0 - -.478 - 0.478 - - - - 0.010 - 3 - 0.01 - - - - 0.005 + + 0.01 + 3.0 + 0.5 + + + 0.15 /paris/sonarRight - paris/us_right_link + paris/us_right_link + 0.5 - - 0.15 0 0.053 0 0 0 + + 0.15 0 0.053 0 -1.5708 0 false 5 - - - - 3 - 1.0 - -.478 - 0.478 - - - - 0.010 - 3 - 0.01 - - - - 0.005 + + 0.01 + 3.0 + 0.5 + + + 0.15 /paris/sonarCenter - paris/us_center_link + paris/us_center_link + 0.5 diff --git a/simulation/models/swarmie/model.sdf.erb b/simulation/models/swarmie/model.sdf.erb index f4094328..b694ce32 100644 --- a/simulation/models/swarmie/model.sdf.erb +++ b/simulation/models/swarmie/model.sdf.erb @@ -30,19 +30,20 @@ sonar_frames = { us_x = 0.15 us_y = 0.07 us_z = 0.053 +us_pitch = -1.5708 us_yaw = 0.43633 sonar_config = { "left" => { - :x0 => us_x, :y0 => us_y, :z0 => us_z, :yaw => us_yaw, + :x0 => us_x, :y0 => us_y, :z0 => us_z, :pitch => us_pitch, :yaw => us_yaw, :topic => "/" + rovername + "/sonarLeft", :frame => sonar_frames["left"] }, "right" => { - :x0 => us_x, :y0 => -us_y, :z0 => us_z, :yaw => -us_yaw, + :x0 => us_x, :y0 => -us_y, :z0 => us_z, :pitch => us_pitch, :yaw => -us_yaw, :topic => "/" + rovername + "/sonarRight", :frame => sonar_frames["right"] }, "center" => { - :x0 => us_x, :y0 => 0, :z0 => us_z, :yaw => 0, + :x0 => us_x, :y0 => 0, :z0 => us_z, :pitch => us_pitch, :yaw => 0, :topic => "/" + rovername + "/sonarCenter", :frame => sonar_frames["center"] } } @@ -154,33 +155,25 @@ wheel_config = { x0 = sonar_config[key][:x0] y0 = sonar_config[key][:y0] z0 = sonar_config[key][:z0] + pitch = sonar_config[key][:pitch] yaw = sonar_config[key][:yaw] topic = sonar_config[key][:topic] frame = sonar_config[key][:frame] -%> - - <%= x0 %> <%= y0 %> <%= z0 %> 0 0 <%= yaw %> + + <%= x0 %> <%= y0 %> <%= z0 %> 0 <%= pitch %> <%= yaw %> false 5 - - - - 3 - 1.0 - -.478 - 0.478 - - - - 0.010 - 3 - 0.01 - - - - 0.005 + + 0.01 + 3.0 + 0.5 + + + 0.15 <%= topic %> - <%= frame %> + <%= frame %> + 0.5 <%- end -%> diff --git a/simulation/models/thor/model.sdf b/simulation/models/thor/model.sdf index 84d64c28..a1b9b844 100644 --- a/simulation/models/thor/model.sdf +++ b/simulation/models/thor/model.sdf @@ -46,79 +46,52 @@ - - 0.15 0.07 0.053 0 0 0.43633 + + 0.15 0.07 0.053 0 -1.5708 0.43633 false 5 - - - - 3 - 1.0 - -.478 - 0.478 - - - - 0.010 - 3 - 0.01 - - - - 0.005 + + 0.01 + 3.0 + 0.5 + + + 0.15 /thor/sonarLeft - thor/us_left_link + thor/us_left_link + 0.5 - - 0.15 -0.07 0.053 0 0 -0.43633 + + 0.15 -0.07 0.053 0 -1.5708 -0.43633 false 5 - - - - 3 - 1.0 - -.478 - 0.478 - - - - 0.010 - 3 - 0.01 - - - - 0.005 + + 0.01 + 3.0 + 0.5 + + + 0.15 /thor/sonarRight - thor/us_right_link + thor/us_right_link + 0.5 - - 0.15 0 0.053 0 0 0 + + 0.15 0 0.053 0 -1.5708 0 false 5 - - - - 3 - 1.0 - -.478 - 0.478 - - - - 0.010 - 3 - 0.01 - - - - 0.005 + + 0.01 + 3.0 + 0.5 + + + 0.15 /thor/sonarCenter - thor/us_center_link + thor/us_center_link + 0.5 diff --git a/simulation/models/zeus/model.sdf b/simulation/models/zeus/model.sdf index f87b7a62..beba42cc 100644 --- a/simulation/models/zeus/model.sdf +++ b/simulation/models/zeus/model.sdf @@ -46,79 +46,52 @@ - - 0.15 0.07 0.053 0 0 0.43633 + + 0.15 0.07 0.053 0 -1.5708 0.43633 false 5 - - - - 3 - 1.0 - -.478 - 0.478 - - - - 0.010 - 3 - 0.01 - - - - 0.005 + + 0.01 + 3.0 + 0.5 + + + 0.15 /zeus/sonarLeft - zeus/us_left_link + zeus/us_left_link + 0.5 - - 0.15 -0.07 0.053 0 0 -0.43633 + + 0.15 -0.07 0.053 0 -1.5708 -0.43633 false 5 - - - - 3 - 1.0 - -.478 - 0.478 - - - - 0.010 - 3 - 0.01 - - - - 0.005 + + 0.01 + 3.0 + 0.5 + + + 0.15 /zeus/sonarRight - zeus/us_right_link + zeus/us_right_link + 0.5 - - 0.15 0 0.053 0 0 0 + + 0.15 0 0.053 0 -1.5708 0 false 5 - - - - 3 - 1.0 - -.478 - 0.478 - - - - 0.010 - 3 - 0.01 - - - - 0.005 + + 0.01 + 3.0 + 0.5 + + + 0.15 /zeus/sonarCenter - zeus/us_center_link + zeus/us_center_link + 0.5 diff --git a/src/gazebo_plugins/CMakeLists.txt b/src/gazebo_plugins/CMakeLists.txt index 61459fa0..e9e43f1f 100644 --- a/src/gazebo_plugins/CMakeLists.txt +++ b/src/gazebo_plugins/CMakeLists.txt @@ -33,3 +33,6 @@ add_library(${PROJECT_NAME}_score target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) +add_library(${PROJECT_NAME}_sonar + src/SonarPlugin/gazebo_ros_sonar.cpp) +target_link_libraries(${PROJECT_NAME}_sonar ${catkin_LIBRARIES} ${Boost_LIBRARIES} SonarPlugin) diff --git a/src/gazebo_plugins/src/SonarPlugin/gazebo_ros_sonar.cpp b/src/gazebo_plugins/src/SonarPlugin/gazebo_ros_sonar.cpp new file mode 100644 index 00000000..5201b8cf --- /dev/null +++ b/src/gazebo_plugins/src/SonarPlugin/gazebo_ros_sonar.cpp @@ -0,0 +1,256 @@ +/* + * Software License Agreement (Modified BSD License) + * + * Copyright (c) 2013-2015, PAL Robotics, S.L. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PAL Robotics, S.L. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jose Capriles, Bence Magyar, Moussab Bennehar. */ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "gazebo_ros_sonar.h" + +namespace gazebo +{ +// Register this plugin with the simulator +GZ_REGISTER_SENSOR_PLUGIN(GazeboRosSonar) + +//////////////////////////////////////////////////////////////////////////////// +// Constructor +GazeboRosSonar::GazeboRosSonar() +{ + this->seed = 0; +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor +GazeboRosSonar::~GazeboRosSonar() +{ + this->rosnode_->shutdown(); + delete this->rosnode_; +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void GazeboRosSonar::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) +{ + // load plugin + SonarPlugin::Load(_parent, this->sdf); + // Get the world name. + std::string worldName = _parent->WorldName(); + this->world_ = physics::get_world(worldName); + // save pointers + this->sdf = _sdf; + + this->last_update_time_ = common::Time(0); + + GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST; + this->parent_sonar_sensor_ = + dynamic_pointer_cast(_parent); + + if (!this->parent_sonar_sensor_) + gzthrow("GazeboRosSonar controller requires a Sonar Sensor as its parent"); + + this->robot_namespace_ = GetRobotNamespace(_parent, _sdf, "Sonar"); + + if (!this->sdf->HasElement("frameName")) + { + ROS_INFO_NAMED("range", "Range plugin missing , defaults to /world"); + this->frame_name_ = "/world"; + } + else + this->frame_name_ = this->sdf->Get("frameName"); + + ROS_INFO("######### frame name : %s #############", this->frame_name_.c_str()); + + if (!this->sdf->HasElement("topicName")) + { + ROS_INFO_NAMED("sonar", "Sonar plugin missing , defaults to /sonar"); + this->topic_name_ = "/sonar"; + } + else + this->topic_name_ = this->sdf->Get("topicName"); + + if (!this->sdf->HasElement("fov")) + { + ROS_WARN_NAMED("sonar", "Sonar plugin missing , defaults to 0.05"); + this->fov_ = 0.05; + } + else + this->fov_ = _sdf->GetElement("fov")->Get(); + + if (!this->sdf->HasElement("gaussianNoise")) + { + ROS_INFO_NAMED("sonar", "Sonar plugin missing , defaults to 0.0"); + this->gaussian_noise_ = 0; + } + else + this->gaussian_noise_ = this->sdf->Get("gaussianNoise"); + + this->sonar_connect_count_ = 0; + + // Make sure the ROS node for Gazebo has already been initialized + if (!ros::isInitialized()) + { + ROS_FATAL_STREAM_NAMED("sonar", "A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; + } + + ROS_INFO_NAMED("sonar", "Starting Sonar Plugin (ns = %s)", this->robot_namespace_.c_str() ); + // ros callback queue for processing subscription + this->deferred_load_thread_ = boost::thread( + boost::bind(&GazeboRosSonar::LoadThread, this)); +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void GazeboRosSonar::LoadThread() +{ + this->gazebo_node_ = gazebo::transport::NodePtr(new gazebo::transport::Node()); + this->gazebo_node_->Init(this->world_name_); + + this->pmq.startServiceThread(); + + this->rosnode_ = new ros::NodeHandle(this->robot_namespace_); + + + this->tf_prefix_ = tf::getPrefixParam(*this->rosnode_); + if(this->tf_prefix_.empty()) { + this->tf_prefix_ = this->robot_namespace_; + boost::trim_right_if(this->tf_prefix_,boost::is_any_of("/")); + } + ROS_INFO_NAMED("sonar", "Sonar Plugin (ns = %s) , set to \"%s\"", + this->robot_namespace_.c_str(), this->tf_prefix_.c_str()); + + // resolve tf prefix + this->frame_name_ = tf::resolve(this->tf_prefix_, this->frame_name_); + + if (this->topic_name_ != "") + { + ros::AdvertiseOptions ao = + ros::AdvertiseOptions::create( + this->topic_name_, 1, + boost::bind(&GazeboRosSonar::SonarConnect, this), + boost::bind(&GazeboRosSonar::SonarDisconnect, this), + ros::VoidPtr(), NULL); + this->pub_ = this->rosnode_->advertise(ao); + this->pub_queue_ = this->pmq.addPub(); + } + + // Initialize the controller + + // sensor generation off by default + this->parent_sonar_sensor_->SetActive(false); +} + +//////////////////////////////////////////////////////////////////////////////// +// Increment count +void GazeboRosSonar::SonarConnect() +{ + this->sonar_connect_count_++; + // this->parent_sonar_sensor_->SetActive(true); + if (this->sonar_connect_count_ == 1) + this->sonar_sub_ = + this->gazebo_node_->Subscribe(this->parent_sonar_sensor_->Topic(), + &GazeboRosSonar::OnScan, this); +} +//////////////////////////////////////////////////////////////////////////////// +// Decrement count +void GazeboRosSonar::SonarDisconnect() +{ + this->sonar_connect_count_--; + if (this->sonar_connect_count_ == 0) + this->sonar_sub_.reset(); +} + + +//////////////////////////////////////////////////////////////////////////////// +// Update the plugin +void GazeboRosSonar::OnScan(ConstSonarStampedPtr &_msg) +{ + sensor_msgs::Range range_msg_; + range_msg_.header.stamp = ros::Time(_msg->time().sec(), _msg->time().nsec()); + range_msg_.header.frame_id = this->frame_name_; + + range_msg_.radiation_type = sensor_msgs::Range::ULTRASOUND; + + range_msg_.field_of_view = fov_; + range_msg_.max_range = this->parent_sonar_sensor_->RangeMax(); + range_msg_.min_range = this->parent_sonar_sensor_->RangeMin(); + + range_msg_.range = _msg->sonar().range(); + if (range_msg_.range < range_msg_.max_range) + range_msg_.range = std::min(range_msg_.range + this->GaussianKernel(0,gaussian_noise_), parent_sonar_sensor_->RangeMax()); + + this->pub_queue_->push(range_msg_, this->pub_); + +} + +////////////////////////////////////////////////////////////////////////////// +// Utility for adding noise +double GazeboRosSonar::GaussianKernel(double mu, double sigma) +{ + // using Box-Muller transform to generate two independent standard + // normally disbributed normal variables see wikipedia + + // normalized uniform random variable + double U = static_cast(rand_r(&this->seed)) / + static_cast(RAND_MAX); + + // normalized uniform random variable + double V = static_cast(rand_r(&this->seed)) / + static_cast(RAND_MAX); + + double X = sqrt(-2.0 * ::log(U)) * cos(2.0*M_PI * V); + // double Y = sqrt(-2.0 * ::log(U)) * sin(2.0*M_PI * V); + + // there are 2 indep. vars, we'll just use X + // scale to our mu and sigma + X = sigma * X + mu; + return X; +} +} diff --git a/src/gazebo_plugins/src/SonarPlugin/gazebo_ros_sonar.h b/src/gazebo_plugins/src/SonarPlugin/gazebo_ros_sonar.h new file mode 100644 index 00000000..9ccd0fc1 --- /dev/null +++ b/src/gazebo_plugins/src/SonarPlugin/gazebo_ros_sonar.h @@ -0,0 +1,137 @@ +/* + * Software License Agreement (Modified BSD License) + * + * Copyright (c) 2013, PAL Robotics, S.L. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PAL Robotics, S.L. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** \author Jose Capriles, Moussab Bennehar. */ + +#ifndef GAZEBO_ROS_RANGE_H +#define GAZEBO_ROS_RANGE_H + + +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace gazebo +{ + +class GazeboRosSonar : public SonarPlugin +{ + + /// \brief Constructor + public: GazeboRosSonar(); + + /// \brief Destructor + public: ~GazeboRosSonar(); + + /// \brief Load the plugin + /// \param take in SDF root element + public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf); + + /// \brief Keep track of number of connctions + private: int sonar_connect_count_; + private: void SonarConnect(); + private: void SonarDisconnect(); + + // Pointer to the model + GazeboRosPtr gazebo_ros_; + private: std::string world_name_; + private: physics::WorldPtr world_; + /// \brief The parent sensor + private: sensors::SonarSensorPtr parent_sonar_sensor_; + + /// \brief pointer to ros node + private: ros::NodeHandle* rosnode_; + private: ros::Publisher pub_; + private: PubQueue::Ptr pub_queue_; + + /// \brief topic name + private: std::string topic_name_; + + /// \brief frame transform name, should match link name + private: std::string frame_name_; + + /// \brief tf prefix + private: std::string tf_prefix_; + + /// \brief for setting ROS name space + private: std::string robot_namespace_; + + /// \brief sensor field of view + private: double fov_; + /// \brief Gaussian noise + private: double gaussian_noise_; + + /// \brief Gaussian noise generator + private: double GaussianKernel(double mu, double sigma); + + /// update rate of this sensor + private: double update_rate_; + private: double update_period_; + private: common::Time last_update_time_; + + // deferred load in case ros is blocking + private: sdf::ElementPtr sdf; + private: void LoadThread(); + private: boost::thread deferred_load_thread_; + private: unsigned int seed; + + private: gazebo::transport::NodePtr gazebo_node_; + private: gazebo::transport::SubscriberPtr sonar_sub_; + /// \brief Update the controller + private: void OnScan(ConstSonarStampedPtr &_msg); + + /// \brief prevents blocking + private: PubMultiQueue pmq; + +}; +} +#endif // GAZEBO_ROS_RANGE_H