From 12156dd7746db76e9dc754b4fddc8074187980de Mon Sep 17 00:00:00 2001 From: Darren Churchill Date: Tue, 19 Feb 2019 16:42:29 -0800 Subject: [PATCH 1/2] Add sonar plugin from outstanding PR #697 on ros-simulation/gazebo_ros_pkgs Thanks to @mbennehar https://github.com/ros-simulation/gazebo_ros_pkgs/pull/697 This is modeled after the range sensor plugin, but by using the sonar sensor, beams hitting the ground don't get reflected back to the sensor. This makes the sensor much more realistic, allowing its field of view to be larger without causing false ground readings. With the increased field of view, objects on the ground will sometimes reflect the beam. Co-authored-by: Moussab Bennehar --- src/gazebo_plugins/CMakeLists.txt | 3 + .../src/SonarPlugin/gazebo_ros_sonar.cpp | 256 ++++++++++++++++++ .../src/SonarPlugin/gazebo_ros_sonar.h | 137 ++++++++++ 3 files changed, 396 insertions(+) create mode 100644 src/gazebo_plugins/src/SonarPlugin/gazebo_ros_sonar.cpp create mode 100644 src/gazebo_plugins/src/SonarPlugin/gazebo_ros_sonar.h 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 From df365ed6e50e04e3adf31bc4e775695e9199e380 Mon Sep 17 00:00:00 2001 From: Darren Churchill Date: Tue, 19 Feb 2019 16:43:05 -0800 Subject: [PATCH 2/2] Use sonar sensor plugin in rover models. --- simulation/models/achilles/model.sdf | 93 +++++++++---------------- simulation/models/aeneas/model.sdf | 93 +++++++++---------------- simulation/models/ajax/model.sdf | 93 +++++++++---------------- simulation/models/diomedes/model.sdf | 93 +++++++++---------------- simulation/models/hector/model.sdf | 93 +++++++++---------------- simulation/models/paris/model.sdf | 93 +++++++++---------------- simulation/models/swarmie/model.sdf.erb | 39 +++++------ simulation/models/thor/model.sdf | 93 +++++++++---------------- simulation/models/zeus/model.sdf | 93 +++++++++---------------- 9 files changed, 280 insertions(+), 503 deletions(-) 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