Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix gazebo7 deprecation warnings (jade-devel) #466

Merged
merged 2 commits into from Jun 17, 2016
Merged
Changes from all commits
Commits
File filter...
Filter file types
Jump to…
Jump to file or symbol
Failed to load files and symbols.

Always

Just for now

@@ -62,7 +62,11 @@ inline std::string GetModelName ( const sensors::SensorPtr &parent )
{
std::string modelName;
std::vector<std::string> values;
# if GAZEBO_MAJOR_VERSION >= 7
std::string scopedName = parent->ScopedName();
# else
std::string scopedName = parent->GetScopedName();
#endif
boost::replace_all ( scopedName, "::", "," );
boost::split ( values, scopedName, boost::is_any_of ( "," ) );
if ( values.size() < 2 ) {
@@ -60,17 +60,36 @@ void MultiCameraPlugin::Load(sensors::SensorPtr _sensor,
return;
}

# if GAZEBO_MAJOR_VERSION >= 7
for (unsigned int i = 0; i < this->parentSensor->CameraCount(); ++i)
# else
for (unsigned int i = 0; i < this->parentSensor->GetCameraCount(); ++i)
# endif
{
# if GAZEBO_MAJOR_VERSION >= 7
this->camera.push_back(this->parentSensor->Camera(i));
# else
this->camera.push_back(this->parentSensor->GetCamera(i));
# endif

// save camera attributes
# if GAZEBO_MAJOR_VERSION >= 7
this->width.push_back(this->camera[i]->ImageWidth());
this->height.push_back(this->camera[i]->ImageHeight());
this->depth.push_back(this->camera[i]->ImageDepth());
this->format.push_back(this->camera[i]->ImageFormat());
# else
this->width.push_back(this->camera[i]->GetImageWidth());
this->height.push_back(this->camera[i]->GetImageHeight());
this->depth.push_back(this->camera[i]->GetImageDepth());
this->format.push_back(this->camera[i]->GetImageFormat());
# endif

# if GAZEBO_MAJOR_VERSION >= 7
std::string cameraName = this->parentSensor->Camera(i)->Name();
# else
std::string cameraName = this->parentSensor->GetCamera(i)->GetName();
# endif
// gzdbg << "camera(" << i << ") name [" << cameraName << "]\n";

// FIXME: hardcoded 2 camera support only
@@ -79,7 +79,12 @@ void GazeboRosBlockLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
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);

last_update_time_ = this->world_->GetSimTime();
@@ -206,7 +211,11 @@ void GazeboRosBlockLaser::OnNewLaserScans()
{
if (this->topic_name_ != "")
{
# if GAZEBO_MAJOR_VERSION >= 7
common::Time sensor_update_time = this->parent_sensor_->LastUpdateTime();
# else
common::Time sensor_update_time = this->parent_sensor_->GetLastUpdateTime();
# endif
if (last_update_time_ < sensor_update_time)
{
this->PutLaserData(sensor_update_time);
@@ -240,13 +249,26 @@ void GazeboRosBlockLaser::PutLaserData(common::Time &_updateTime)
math::Angle minAngle = this->parent_ray_sensor_->GetAngleMin();
#endif

# if GAZEBO_MAJOR_VERSION >= 7
double maxRange = this->parent_ray_sensor_->RangeMax();
double minRange = this->parent_ray_sensor_->RangeMin();
int rayCount = this->parent_ray_sensor_->RayCount();
int rangeCount = this->parent_ray_sensor_->RangeCount();
# else
double maxRange = this->parent_ray_sensor_->GetRangeMax();
double minRange = this->parent_ray_sensor_->GetRangeMin();
int rayCount = this->parent_ray_sensor_->GetRayCount();
int rangeCount = this->parent_ray_sensor_->GetRangeCount();
# endif

#if GAZEBO_MAJOR_VERSION >= 7
int verticalRayCount = this->parent_ray_sensor_->VerticalRayCount();
int verticalRangeCount = this->parent_ray_sensor_->VerticalRangeCount();
# else
int verticalRayCount = this->parent_ray_sensor_->GetVerticalRayCount();
int verticalRangeCount = this->parent_ray_sensor_->GetVerticalRangeCount();
# endif

#if GAZEBO_MAJOR_VERSION >= 6
auto verticalMaxAngle = this->parent_ray_sensor_->VerticalAngleMax();
auto verticalMinAngle = this->parent_ray_sensor_->VerticalAngleMin();
@@ -304,22 +326,37 @@ void GazeboRosBlockLaser::PutLaserData(common::Time &_updateTime)
j3 = hja + vjb * rayCount;
j4 = hjb + vjb * rayCount;
// range readings of 4 corners
# if GAZEBO_MAJOR_VERSION >= 7
r1 = std::min(this->parent_ray_sensor_->LaserShape()->GetRange(j1) , maxRange-minRange);
r2 = std::min(this->parent_ray_sensor_->LaserShape()->GetRange(j2) , maxRange-minRange);
r3 = std::min(this->parent_ray_sensor_->LaserShape()->GetRange(j3) , maxRange-minRange);
r4 = std::min(this->parent_ray_sensor_->LaserShape()->GetRange(j4) , maxRange-minRange);
# else
r1 = std::min(this->parent_ray_sensor_->GetLaserShape()->GetRange(j1) , maxRange-minRange);
r2 = std::min(this->parent_ray_sensor_->GetLaserShape()->GetRange(j2) , maxRange-minRange);
r3 = std::min(this->parent_ray_sensor_->GetLaserShape()->GetRange(j3) , maxRange-minRange);
r4 = std::min(this->parent_ray_sensor_->GetLaserShape()->GetRange(j4) , maxRange-minRange);
# endif

// Range is linear interpolation if values are close,
// and min if they are very different
r = (1-vb)*((1 - hb) * r1 + hb * r2)
+ vb *((1 - hb) * r3 + hb * r4);

// Intensity is averaged
#if GAZEBO_MAJOR_VERSION >= 7
intensity = 0.25*(this->parent_ray_sensor_->LaserShape()->GetRetro(j1) +
this->parent_ray_sensor_->LaserShape()->GetRetro(j2) +
this->parent_ray_sensor_->LaserShape()->GetRetro(j3) +
this->parent_ray_sensor_->LaserShape()->GetRetro(j4));
# else
intensity = 0.25*(this->parent_ray_sensor_->GetLaserShape()->GetRetro(j1) +
this->parent_ray_sensor_->GetLaserShape()->GetRetro(j2) +
this->parent_ray_sensor_->GetLaserShape()->GetRetro(j3) +
this->parent_ray_sensor_->GetLaserShape()->GetRetro(j4));

# endif

// std::cout << " block debug "
// << " ij("<<i<<","<<j<<")"
// << " j1234("<<j1<<","<<j2<<","<<j3<<","<<j4<<")"
@@ -347,17 +384,17 @@ void GazeboRosBlockLaser::PutLaserData(common::Time &_updateTime)
point.y = r * cos(pAngle) * sin(yAngle);
point.z = -r * sin(pAngle);

this->cloud_msg_.points.push_back(point);
}
else
{
this->cloud_msg_.points.push_back(point);
}
else
{
geometry_msgs::Point32 point;
//pAngle is rotated by yAngle:
point.x = r * cos(pAngle) * cos(yAngle) + this->GaussianKernel(0,this->gaussian_noise_);
point.y = r * cos(pAngle) * sin(yAngle) + this->GaussianKernel(0,this->gaussian_noise_);
point.z = -r * sin(pAngle) + this->GaussianKernel(0,this->gaussian_noise_);
this->cloud_msg_.points.push_back(point);
} // only 1 channel
this->cloud_msg_.points.push_back(point);
} // only 1 channel

this->cloud_msg_.channels[0].values.push_back(intensity + this->GaussianKernel(0,this->gaussian_noise_)) ;
}
@@ -136,7 +136,11 @@ void GazeboRosBumper::OnContact()
return;

msgs::Contacts contacts;
# if GAZEBO_MAJOR_VERSION >= 7
contacts = this->parentSensor->Contacts();
# else
contacts = this->parentSensor->GetContacts();
# endif
/// \TODO: need a time for each Contact in i-loop, they may differ
this->contact_state_msg_.header.frame_id = this->frame_name_;
this->contact_state_msg_.header.stamp = ros::Time(contacts.time().sec(),
@@ -210,7 +214,7 @@ void GazeboRosBumper::OnContact()
// For each collision contact
// Create a ContactState
gazebo_msgs::ContactState state;
/// \TODO:
/// \TODO:
gazebo::msgs::Contact contact = contacts.contact(i);

state.collision1_name = contact.collision1();
@@ -76,7 +76,11 @@ void GazeboRosCamera::OnNewFrame(const unsigned char *_image,
unsigned int _width, unsigned int _height, unsigned int _depth,
const std::string &_format)
{
# if GAZEBO_MAJOR_VERSION >= 7
this->sensor_update_time_ = this->parentSensor_->LastUpdateTime();
# else
this->sensor_update_time_ = this->parentSensor_->GetLastUpdateTime();
# endif

if (!this->parentSensor->IsActive())
{
@@ -105,7 +105,11 @@ void GazeboRosCameraUtils::Load(sensors::SensorPtr _parent,
const std::string &_camera_name_suffix)
{
// Get the world name.
# if GAZEBO_MAJOR_VERSION >= 7
std::string world_name = _parent->WorldName();
# else
std::string world_name = _parent->GetWorldName();
# endif

// Get the world_
this->world_ = physics::get_world(world_name);
@@ -119,7 +123,7 @@ void GazeboRosCameraUtils::Load(sensors::SensorPtr _parent,

std::stringstream ss;
this->robot_namespace_ = GetRobotNamespace(_parent, _sdf, "Camera");

this->image_topic_name_ = "image_raw";
if (this->sdf->HasElement("imageTopicName"))
this->image_topic_name_ = this->sdf->Get<std::string>("imageTopicName");
@@ -278,7 +282,7 @@ void GazeboRosCameraUtils::LoadThread()
*this->rosnode_, this->camera_name_));

this->itnode_ = new image_transport::ImageTransport(*this->rosnode_);

// resolve tf prefix
this->tf_prefix_ = tf::getPrefixParam(*this->rosnode_);
if(this->tf_prefix_.empty()) {
@@ -289,8 +293,8 @@ void GazeboRosCameraUtils::LoadThread()

ROS_INFO("Camera Plugin (ns = %s) <tf_prefix_>, set to \"%s\"",
this->robot_namespace_.c_str(), this->tf_prefix_.c_str());


if (!this->camera_name_.empty())
{
dyn_srv_ =
@@ -464,9 +468,15 @@ void GazeboRosCameraUtils::Init()
this->cy_ = (static_cast<double>(this->height_) + 1.0) /2.0;


# if GAZEBO_MAJOR_VERSION >= 7
double computed_focal_length =
(static_cast<double>(this->width_)) /
(2.0 * tan(this->camera_->HFOV().Radian() / 2.0));
# else
double computed_focal_length =
(static_cast<double>(this->width_)) /
(2.0 * tan(this->camera_->GetHFOV().Radian() / 2.0));
# endif

if (this->focal_length_ == 0)
{
@@ -477,6 +487,17 @@ void GazeboRosCameraUtils::Init()
// check against float precision
if (!gazebo::math::equal(this->focal_length_, computed_focal_length))
{
# if GAZEBO_MAJOR_VERSION >= 7
ROS_WARN("The <focal_length>[%f] you have provided for camera_ [%s]"
" is inconsistent with specified image_width [%d] and"
" HFOV [%f]. Please double check to see that"
" focal_length = width_ / (2.0 * tan(HFOV/2.0)),"
" the explected focal_lengtth value is [%f],"
" please update your camera_ model description accordingly.",
this->focal_length_, this->parentSensor_->Name().c_str(),
this->width_, this->camera_->HFOV().Radian(),
computed_focal_length);
# else
ROS_WARN("The <focal_length>[%f] you have provided for camera_ [%s]"
" is inconsistent with specified image_width [%d] and"
" HFOV [%f]. Please double check to see that"
@@ -486,6 +507,8 @@ void GazeboRosCameraUtils::Init()
this->focal_length_, this->parentSensor_->GetName().c_str(),
this->width_, this->camera_->GetHFOV().Radian(),
computed_focal_length);

# endif
}
}

@@ -602,7 +625,11 @@ void GazeboRosCameraUtils::PublishCameraInfo()

if (this->camera_info_pub_.getNumSubscribers() > 0)
{
# if GAZEBO_MAJOR_VERSION >= 7
this->sensor_update_time_ = this->parentSensor_->LastUpdateTime();
# else
this->sensor_update_time_ = this->parentSensor_->GetLastUpdateTime();
# endif
common::Time cur_time = this->world_->GetSimTime();
if (cur_time - this->last_info_update_time_ >= this->update_period_)
{
@@ -191,7 +191,11 @@ void GazeboRosDepthCamera::OnNewDepthFrame(const float *_image,
if (!this->initialized_ || this->height_ <=0 || this->width_ <=0)
return;

# if GAZEBO_MAJOR_VERSION >= 7
this->depth_sensor_update_time_ = this->parentSensor->LastUpdateTime();
# else
this->depth_sensor_update_time_ = this->parentSensor->GetLastUpdateTime();
# endif
if (this->parentSensor->IsActive())
{
if (this->point_cloud_connect_count_ <= 0 &&
@@ -227,7 +231,11 @@ void GazeboRosDepthCamera::OnNewRGBPointCloud(const float *_pcd,
if (!this->initialized_ || this->height_ <=0 || this->width_ <=0)
return;

# if GAZEBO_MAJOR_VERSION >= 7
this->depth_sensor_update_time_ = this->parentSensor->LastUpdateTime();
# else
this->depth_sensor_update_time_ = this->parentSensor->GetLastUpdateTime();
# endif
if (!this->parentSensor->IsActive())
{
if (this->point_cloud_connect_count_ > 0)
@@ -293,7 +301,11 @@ void GazeboRosDepthCamera::OnNewImageFrame(const unsigned char *_image,
return;

//ROS_ERROR("camera_ new frame %s %s",this->parentSensor_->GetName().c_str(),this->frame_name_.c_str());
# if GAZEBO_MAJOR_VERSION >= 7
this->sensor_update_time_ = this->parentSensor->LastUpdateTime();
# else
this->sensor_update_time_ = this->parentSensor->GetLastUpdateTime();
# endif

if (!this->parentSensor->IsActive())
{
@@ -376,7 +388,11 @@ bool GazeboRosDepthCamera::FillPointCloudHelper(
float* toCopyFrom = (float*)data_arg;
int index = 0;

# if GAZEBO_MAJOR_VERSION >= 7
double hfov = this->parentSensor->DepthCamera()->HFOV().Radian();
# else
double hfov = this->parentSensor->GetDepthCamera()->GetHFOV().Radian();
# endif
double fl = ((double)this->width) / (2.0 *tan(hfov/2.0));

// convert depth to point cloud
@@ -485,7 +501,11 @@ void GazeboRosDepthCamera::PublishCameraInfo()

if (this->depth_info_connect_count_ > 0)
{
# if GAZEBO_MAJOR_VERSION >= 7
this->sensor_update_time_ = this->parentSensor_->LastUpdateTime();
# else
this->sensor_update_time_ = this->parentSensor_->GetLastUpdateTime();
# endif
common::Time cur_time = this->world_->GetSimTime();
if (cur_time - this->last_depth_image_camera_info_update_time_ >= this->update_period_)
{
@@ -34,6 +34,7 @@
#include <gazebo/sensors/GpuRaySensor.hh>
#include <gazebo/sensors/SensorTypes.hh>
#include <gazebo/transport/transport.hh>
#include <gazebo/gazebo_config.h>

#include <tf/tf.h>
#include <tf/transform_listener.h>
@@ -69,8 +70,14 @@ void GazeboRosLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
{
// load plugin
GpuRayPlugin::Load(_parent, this->sdf);

// 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;
@@ -167,7 +174,11 @@ void GazeboRosLaser::LaserConnect()
this->laser_connect_count_++;
if (this->laser_connect_count_ == 1)
this->laser_scan_sub_ =
# if GAZEBO_MAJOR_VERSION >= 7
this->gazebo_node_->Subscribe(this->parent_ray_sensor_->Topic(),
# else
this->gazebo_node_->Subscribe(this->parent_ray_sensor_->GetTopic(),
# endif
&GazeboRosLaser::OnScan, this);
}

ProTip! Use n and p to navigate between commits in a pull request.
You can’t perform that action at this time.