//////////////////////////////////////////////////////////////////////////////// // Load the controller void GazeboRosRange::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) { // load plugin RayPlugin::Load(_parent, this->sdf); // Get then name of the parent sensor this->parent_sensor_ = _parent; // Get the world name. # if GAZEBO_MAJOR_VERSION >= 7 std::string worldName = _parent->WorldName(); # else std::string worldName = _parent->GetWorldName(); # endif 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_ray_sensor_ = dynamic_pointer_cast<sensors::RaySensor>(this->parent_sensor_); if (!this->parent_ray_sensor_) gzthrow("GazeboRosRange controller requires a Ray Sensor as its parent"); this->robot_namespace_ = ""; if (this->sdf->HasElement("robotNamespace")) this->robot_namespace_ = this->sdf->Get<std::string>("robotNamespace") + "/"; if (!this->sdf->HasElement("frameName")) { ROS_INFO("Range plugin missing <frameName>, defaults to /world"); this->frame_name_ = "/world"; } else this->frame_name_ = this->sdf->Get<std::string>("frameName"); if (!this->sdf->HasElement("topicName")) { ROS_INFO("Range plugin missing <topicName>, defaults to /range"); this->topic_name_ = "/range"; } else this->topic_name_ = this->sdf->Get<std::string>("topicName"); if (!this->sdf->HasElement("radiation")) { ROS_WARN("Range plugin missing <radiation>, defaults to ultrasound"); this->radiation_ = "ultrasound"; } else this->radiation_ = _sdf->GetElement("radiation")->Get<std::string>(); if (!this->sdf->HasElement("fov")) { ROS_WARN("Range plugin missing <fov>, defaults to 0.05"); this->fov_ = 0.05; } else this->fov_ = _sdf->GetElement("fov")->Get<double>(); if (!this->sdf->HasElement("gaussianNoise")) { ROS_INFO("Range plugin missing <gaussianNoise>, defaults to 0.0"); this->gaussian_noise_ = 0; } else this->gaussian_noise_ = this->sdf->Get<double>("gaussianNoise"); if (!this->sdf->HasElement("updateRate")) { ROS_INFO("Range plugin missing <updateRate>, defaults to 0"); this->update_rate_ = 0; } else this->update_rate_ = this->sdf->Get<double>("updateRate"); // prepare to throttle this plugin at the same rate // ideally, we should invoke a plugin update when the sensor updates, // have to think about how to do that properly later if (this->update_rate_ > 0.0) this->update_period_ = 1.0/this->update_rate_; else this->update_period_ = 0.0; this->range_connect_count_ = 0; this->range_msg_.header.frame_id = this->frame_name_; if (this->radiation_==std::string("ultrasound")) this->range_msg_.radiation_type = sensor_msgs::Range::ULTRASOUND; else this->range_msg_.radiation_type = sensor_msgs::Range::INFRARED; this->range_msg_.field_of_view = fov_; # if GAZEBO_MAJOR_VERSION >= 7 this->range_msg_.max_range = this->parent_ray_sensor_->RangeMax(); this->range_msg_.min_range = this->parent_ray_sensor_->RangeMin(); # else this->range_msg_.max_range = this->parent_ray_sensor_->GetRangeMax(); this->range_msg_.min_range = this->parent_ray_sensor_->GetRangeMin(); # endif // Init ROS if (ros::isInitialized()) { // ros callback queue for processing subscription this->deferred_load_thread_ = boost::thread( boost::bind(&GazeboRosRange::LoadThread, this)); } else { gzerr << "Not loading plugin since ROS hasn't been " << "properly initialized. Try starting gazebo with ros plugin:\n" << " gazebo -s libgazebo_ros_api_plugin.so\n"; } }
//////////////////////////////////////////////////////////////////////////////// // Load the controller void GazeboRosVelodyneLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) { // load plugin RayPlugin::Load(_parent, _sdf); // Get then name of the parent sensor parent_sensor_ = _parent; // Get the world name. #if GAZEBO_MAJOR_VERSION >= 7 std::string worldName = _parent->WorldName(); #else std::string worldName = _parent->GetWorldName(); #endif world_ = physics::get_world(worldName); last_update_time_ = world_->GetSimTime(); node_ = transport::NodePtr(new transport::Node()); node_->Init(worldName); #if GAZEBO_MAJOR_VERSION >= 7 parent_ray_sensor_ = std::dynamic_pointer_cast<sensors::RaySensor>(parent_sensor_); #else parent_ray_sensor_ = boost::dynamic_pointer_cast<sensors::RaySensor>(parent_sensor_); #endif if (!parent_ray_sensor_) { gzthrow("GazeboRosVelodyneLaser controller requires a Ray Sensor as its parent"); } robot_namespace_ = ""; if (_sdf->HasElement("robotNamespace")) { robot_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/"; } if (!_sdf->HasElement("frameName")) { ROS_INFO("Velodyne laser plugin missing <frameName>, defaults to /world"); frame_name_ = "/world"; } else { frame_name_ = _sdf->GetElement("frameName")->Get<std::string>(); } if (!_sdf->HasElement("min_range")) { ROS_INFO("Velodyne laser plugin missing <min_range>, defaults to 0"); min_range_ = 0; } else { min_range_ = _sdf->GetElement("min_range")->Get<double>(); } if (!_sdf->HasElement("max_range")) { ROS_INFO("Velodyne laser plugin missing <max_range>, defaults to infinity"); max_range_ = INFINITY; } else { max_range_ = _sdf->GetElement("max_range")->Get<double>(); } if (!_sdf->HasElement("topicName")) { ROS_INFO("Velodyne laser plugin missing <topicName>, defaults to /world"); topic_name_ = "/world"; } else { topic_name_ = _sdf->GetElement("topicName")->Get<std::string>(); } if (!_sdf->HasElement("gaussianNoise")) { ROS_INFO("Velodyne laser plugin missing <gaussianNoise>, defaults to 0.0"); gaussian_noise_ = 0; } else { gaussian_noise_ = _sdf->GetElement("gaussianNoise")->Get<double>(); } // Make sure the ROS node for Gazebo has already been initialized if (!ros::isInitialized()) { ROS_FATAL_STREAM("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; } rosnode_ = new ros::NodeHandle(robot_namespace_); // resolve tf prefix std::string prefix; rosnode_->getParam(std::string("tf_prefix"), prefix); frame_name_ = tf::resolve(prefix, frame_name_); if (topic_name_ != "") { // Custom Callback Queue ros::AdvertiseOptions ao = ros::AdvertiseOptions::create<sensor_msgs::PointCloud2>( topic_name_,1, boost::bind( &GazeboRosVelodyneLaser::laserConnect,this), boost::bind( &GazeboRosVelodyneLaser::laserDisconnect,this), ros::VoidPtr(), &laser_queue_); pub_ = rosnode_->advertise(ao); } // sensor generation off by default parent_ray_sensor_->SetActive(false); // start custom queue for laser callback_laser_queue_thread_ = boost::thread( boost::bind( &GazeboRosVelodyneLaser::laserQueueThread,this ) ); #if GAZEBO_MAJOR_VERSION >= 7 ROS_INFO("Velodyne laser plugin ready, %i lasers", parent_ray_sensor_->VerticalRangeCount()); #else ROS_INFO("Velodyne laser plugin ready, %i lasers", parent_ray_sensor_->GetVerticalRangeCount()); #endif }