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.

Sign up for GitHub

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

Uncrustify #1060

Merged
merged 1 commit into from
Mar 14, 2020
Merged

Uncrustify #1060

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
37 changes: 24 additions & 13 deletions gazebo_plugins/src/gazebo_ros_ackermann_drive.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -216,15 +216,17 @@ void GazeboRosAckermannDrive::Load(gazebo::physics::ModelPtr _model, sdf::Elemen
impl_->joints_[GazeboRosAckermannDrivePrivate::STEER_WHEEL] =
_model->GetJoint(steering_wheel_joint);
if (!impl_->joints_[GazeboRosAckermannDrivePrivate::STEER_WHEEL]) {
RCLCPP_WARN(impl_->ros_node_->get_logger(),
RCLCPP_WARN(
impl_->ros_node_->get_logger(),
"Steering wheel joint [%s] not found.", steering_wheel_joint.c_str());
impl_->joints_.resize(6);
}

auto front_right_joint = _sdf->Get<std::string>("front_right_joint", "front_right_joint").first;
impl_->joints_[GazeboRosAckermannDrivePrivate::FRONT_RIGHT] = _model->GetJoint(front_right_joint);
if (!impl_->joints_[GazeboRosAckermannDrivePrivate::FRONT_RIGHT]) {
RCLCPP_ERROR(impl_->ros_node_->get_logger(),
RCLCPP_ERROR(
impl_->ros_node_->get_logger(),
"Front right wheel joint [%s] not found, plugin will not work.", front_right_joint.c_str());
impl_->ros_node_.reset();
return;
Expand All @@ -233,7 +235,8 @@ void GazeboRosAckermannDrive::Load(gazebo::physics::ModelPtr _model, sdf::Elemen
auto front_left_joint = _sdf->Get<std::string>("front_left_joint", "front_left_joint").first;
impl_->joints_[GazeboRosAckermannDrivePrivate::FRONT_LEFT] = _model->GetJoint(front_left_joint);
if (!impl_->joints_[GazeboRosAckermannDrivePrivate::FRONT_LEFT]) {
RCLCPP_ERROR(impl_->ros_node_->get_logger(),
RCLCPP_ERROR(
impl_->ros_node_->get_logger(),
"Front left wheel joint [%s] not found, plugin will not work.", front_left_joint.c_str());
impl_->ros_node_.reset();
return;
Expand All @@ -242,7 +245,8 @@ void GazeboRosAckermannDrive::Load(gazebo::physics::ModelPtr _model, sdf::Elemen
auto rear_right_joint = _sdf->Get<std::string>("rear_right_joint", "rear_right_joint").first;
impl_->joints_[GazeboRosAckermannDrivePrivate::REAR_RIGHT] = _model->GetJoint(rear_right_joint);
if (!impl_->joints_[GazeboRosAckermannDrivePrivate::REAR_RIGHT]) {
RCLCPP_ERROR(impl_->ros_node_->get_logger(),
RCLCPP_ERROR(
impl_->ros_node_->get_logger(),
"Rear right wheel joint [%s] not found, plugin will not work.", rear_right_joint.c_str());
impl_->ros_node_.reset();
return;
Expand All @@ -251,7 +255,8 @@ void GazeboRosAckermannDrive::Load(gazebo::physics::ModelPtr _model, sdf::Elemen
auto rear_left_joint = _sdf->Get<std::string>("rear_left_joint", "rear_left_joint").first;
impl_->joints_[GazeboRosAckermannDrivePrivate::REAR_LEFT] = _model->GetJoint(rear_left_joint);
if (!impl_->joints_[GazeboRosAckermannDrivePrivate::REAR_LEFT]) {
RCLCPP_ERROR(impl_->ros_node_->get_logger(),
RCLCPP_ERROR(
impl_->ros_node_->get_logger(),
"Rear left wheel joint [%s] not found, plugin will not work.", rear_left_joint.c_str());
impl_->ros_node_.reset();
return;
Expand All @@ -262,7 +267,8 @@ void GazeboRosAckermannDrive::Load(gazebo::physics::ModelPtr _model, sdf::Elemen
impl_->joints_[GazeboRosAckermannDrivePrivate::STEER_RIGHT] =
_model->GetJoint(right_steering_joint);
if (!impl_->joints_[GazeboRosAckermannDrivePrivate::STEER_RIGHT]) {
RCLCPP_ERROR(impl_->ros_node_->get_logger(),
RCLCPP_ERROR(
impl_->ros_node_->get_logger(),
"Right wheel steering joint [%s] not found, plugin will not work.",
right_steering_joint.c_str());
impl_->ros_node_.reset();
Expand All @@ -274,7 +280,8 @@ void GazeboRosAckermannDrive::Load(gazebo::physics::ModelPtr _model, sdf::Elemen
impl_->joints_[GazeboRosAckermannDrivePrivate::STEER_LEFT] =
_model->GetJoint(left_steering_joint);
if (!impl_->joints_[GazeboRosAckermannDrivePrivate::STEER_LEFT]) {
RCLCPP_ERROR(impl_->ros_node_->get_logger(),
RCLCPP_ERROR(
impl_->ros_node_->get_logger(),
"Left wheel steering joint [%s] not found, plugin will not work.",
left_steering_joint.c_str());
impl_->ros_node_.reset();
Expand Down Expand Up @@ -351,8 +358,8 @@ void GazeboRosAckermannDrive::Load(gazebo::physics::ModelPtr _model, sdf::Elemen
"cmd_vel", rclcpp::QoS(rclcpp::KeepLast(1)),
std::bind(&GazeboRosAckermannDrivePrivate::OnCmdVel, impl_.get(), std::placeholders::_1));

RCLCPP_INFO(impl_->ros_node_->get_logger(),
"Subscribed to [%s]", impl_->cmd_vel_sub_->get_topic_name());
RCLCPP_INFO(
impl_->ros_node_->get_logger(), "Subscribed to [%s]", impl_->cmd_vel_sub_->get_topic_name());

// Odometry
impl_->odometry_frame_ = _sdf->Get<std::string>("odometry_frame", "odom").first;
Expand All @@ -364,7 +371,8 @@ void GazeboRosAckermannDrive::Load(gazebo::physics::ModelPtr _model, sdf::Elemen
impl_->odometry_pub_ = impl_->ros_node_->create_publisher<nav_msgs::msg::Odometry>(
"odom", rclcpp::QoS(rclcpp::KeepLast(1)));

RCLCPP_INFO(impl_->ros_node_->get_logger(), "Advertise odometry on [%s]",
RCLCPP_INFO(
impl_->ros_node_->get_logger(), "Advertise odometry on [%s]",
impl_->odometry_pub_->get_topic_name());
}

Expand All @@ -374,7 +382,8 @@ void GazeboRosAckermannDrive::Load(gazebo::physics::ModelPtr _model, sdf::Elemen
impl_->distance_pub_ = impl_->ros_node_->create_publisher<std_msgs::msg::Float32>(
"distance", rclcpp::QoS(rclcpp::KeepLast(1)));

RCLCPP_INFO(impl_->ros_node_->get_logger(), "Advertise distance on [%s]",
RCLCPP_INFO(
impl_->ros_node_->get_logger(), "Advertise distance on [%s]",
impl_->distance_pub_->get_topic_name());
}

Expand All @@ -386,14 +395,16 @@ void GazeboRosAckermannDrive::Load(gazebo::physics::ModelPtr _model, sdf::Elemen
std::make_shared<tf2_ros::TransformBroadcaster>(impl_->ros_node_);

if (impl_->publish_odom_tf_) {
RCLCPP_INFO(impl_->ros_node_->get_logger(),
RCLCPP_INFO(
impl_->ros_node_->get_logger(),
"Publishing odom transforms between [%s] and [%s]", impl_->odometry_frame_.c_str(),
impl_->robot_base_frame_.c_str());
}

if (impl_->publish_wheel_tf_) {
for (auto & joint : impl_->joints_) {
RCLCPP_INFO(impl_->ros_node_->get_logger(),
RCLCPP_INFO(
impl_->ros_node_->get_logger(),
"Publishing wheel transforms between [%s], [%s] and [%s]",
impl_->robot_base_frame_.c_str(), joint->GetName().c_str(), joint->GetName().c_str());
}
Expand Down
6 changes: 4 additions & 2 deletions gazebo_plugins/src/gazebo_ros_bumper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -73,7 +73,8 @@ void GazeboRosBumper::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _

impl_->parent_sensor_ = std::dynamic_pointer_cast<gazebo::sensors::ContactSensor>(_sensor);
if (!impl_->parent_sensor_) {
RCLCPP_ERROR(impl_->ros_node_->get_logger(),
RCLCPP_ERROR(
impl_->ros_node_->get_logger(),
"Contact sensor parent is not of type ContactSensor. Aborting");
impl_->ros_node_.reset();
return;
Expand All @@ -83,7 +84,8 @@ void GazeboRosBumper::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _
impl_->pub_ = impl_->ros_node_->create_publisher<gazebo_msgs::msg::ContactsState>(
"bumper_states", rclcpp::SensorDataQoS());

RCLCPP_INFO(impl_->ros_node_->get_logger(), "Publishing contact states to [%s]",
RCLCPP_INFO(
impl_->ros_node_->get_logger(), "Publishing contact states to [%s]",
impl_->pub_->get_topic_name());

// Get tf frame for output
Expand Down
79 changes: 51 additions & 28 deletions gazebo_plugins/src/gazebo_ros_camera.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -158,7 +158,8 @@ void GazeboRosCamera::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _
impl_->sensor_type_ = GazeboRosCameraPrivate::CAMERA;
gazebo::CameraPlugin::Load(_sensor, _sdf);
} else {
RCLCPP_ERROR(impl_->ros_node_->get_logger(),
RCLCPP_ERROR(
impl_->ros_node_->get_logger(),
"Plugin must be attached to sensor of type `camera`, `depth` or `multicamera`");
impl_->ros_node_.reset();
return;
Expand All @@ -173,13 +174,14 @@ void GazeboRosCamera::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _
if (impl_->sensor_type_ != GazeboRosCameraPrivate::MULTICAMERA) {
// Image publisher
// TODO(louise) Migrate image_connect logic once SubscriberStatusCallback is ported to ROS2
impl_->image_pub_.push_back(image_transport::create_publisher(impl_->ros_node_.get(),
impl_->camera_name_ + "/image_raw"));
impl_->image_pub_.push_back(
image_transport::create_publisher(
impl_->ros_node_.get(), impl_->camera_name_ + "/image_raw"));

// TODO(louise) Uncomment this once image_transport::Publisher has a function to return the
// full topic.
// RCLCPP_INFO(impl_->ros_node_->get_logger(), "Publishing images to [%s]",
// impl_->image_pub_.getTopic());
// RCLCPP_INFO(
// impl_->ros_node_->get_logger(), "Publishing images to [%s]", impl_->image_pub_.getTopic());

// Camera info publisher
// TODO(louise) Migrate ImageConnect logic once SubscriberStatusCallback is ported to ROS2
Expand All @@ -188,16 +190,20 @@ void GazeboRosCamera::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _
impl_->camera_name_ + "/camera_info",
rclcpp::SensorDataQoS()));

RCLCPP_INFO(impl_->ros_node_->get_logger(), "Publishing camera info to [%s]",
RCLCPP_INFO(
impl_->ros_node_->get_logger(), "Publishing camera info to [%s]",
impl_->camera_info_pub_.back()->get_topic_name());

} else {
for (uint64_t i = 0; i < impl_->num_cameras_; ++i) {
// Image publisher
impl_->image_pub_.push_back(image_transport::create_publisher(impl_->ros_node_.get(),
impl_->camera_name_ + "/" + MultiCameraPlugin::camera_[i]->Name() + "/image_raw"));
impl_->image_pub_.push_back(
image_transport::create_publisher(
impl_->ros_node_.get(),
impl_->camera_name_ + "/" + MultiCameraPlugin::camera_[i]->Name() + "/image_raw"));

// RCLCPP_INFO(impl_->ros_node_->get_logger(), "Publishing %s camera images to [%s]",
// RCLCPP_INFO(
// impl_->ros_node_->get_logger(), "Publishing %s camera images to [%s]",
// MultiCameraPlugin::camera_[i]->Name().c_str(),
// impl_->image_pub_.back().getTopic());

Expand All @@ -207,16 +213,17 @@ void GazeboRosCamera::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _
impl_->camera_name_ + "/" + MultiCameraPlugin::camera_[i]->Name() + "/camera_info",
rclcpp::SensorDataQoS()));

RCLCPP_INFO(impl_->ros_node_->get_logger(), "Publishing %s camera info to [%s]",
RCLCPP_INFO(
impl_->ros_node_->get_logger(), "Publishing %s camera info to [%s]",
MultiCameraPlugin::camera_[i]->Name().c_str(),
impl_->camera_info_pub_[i]->get_topic_name());
}
}

if (impl_->sensor_type_ == GazeboRosCameraPrivate::DEPTH) {
// Depth image publisher
impl_->depth_image_pub_ = image_transport::create_publisher(impl_->ros_node_.get(),
impl_->camera_name_ + "/depth/image_raw");
impl_->depth_image_pub_ = image_transport::create_publisher(
impl_->ros_node_.get(), impl_->camera_name_ + "/depth/image_raw");

// RCLCPP_INFO(impl_->ros_node_->get_logger(), "Publishing depth images to [%s]",
// impl_->depth_image_pub_.getTopic().c_str());
Expand All @@ -226,14 +233,16 @@ void GazeboRosCamera::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _
impl_->ros_node_->create_publisher<sensor_msgs::msg::CameraInfo>(
impl_->camera_name_ + "/depth/camera_info", rclcpp::QoS(rclcpp::KeepLast(1)));

RCLCPP_INFO(impl_->ros_node_->get_logger(), "Publishing depth camera info to [%s]",
RCLCPP_INFO(
impl_->ros_node_->get_logger(), "Publishing depth camera info to [%s]",
impl_->depth_camera_info_pub_->get_topic_name());

// Point cloud publisher
impl_->point_cloud_pub_ = impl_->ros_node_->create_publisher<sensor_msgs::msg::PointCloud2>(
impl_->camera_name_ + "/points", rclcpp::QoS(rclcpp::KeepLast(1)));

RCLCPP_INFO(impl_->ros_node_->get_logger(), "Publishing pointcloud to [%s]",
RCLCPP_INFO(
impl_->ros_node_->get_logger(), "Publishing pointcloud to [%s]",
impl_->point_cloud_pub_->get_topic_name());
}

Expand All @@ -244,7 +253,8 @@ void GazeboRosCamera::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _
rclcpp::QoS(rclcpp::KeepLast(1)),
std::bind(&GazeboRosCamera::OnTrigger, this, std::placeholders::_1));

RCLCPP_INFO(impl_->ros_node_->get_logger(), "Subscribed to [%s]",
RCLCPP_INFO(
impl_->ros_node_->get_logger(), "Subscribed to [%s]",
impl_->trigger_sub_->get_topic_name());

SetCameraEnabled(false);
Expand Down Expand Up @@ -279,22 +289,26 @@ void GazeboRosCamera::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _
impl_->img_encoding_.emplace_back(sensor_msgs::image_encodings::RGB16);
impl_->img_step_.push_back(6);
} else if (format == "BAYER_RGGB8") {
RCLCPP_INFO(impl_->ros_node_->get_logger(),
RCLCPP_INFO(
impl_->ros_node_->get_logger(),
"bayer simulation may be computationally expensive.");
impl_->img_encoding_.emplace_back(sensor_msgs::image_encodings::BAYER_RGGB8);
impl_->img_step_.push_back(1);
} else if (format == "BAYER_BGGR8") {
RCLCPP_INFO(impl_->ros_node_->get_logger(),
RCLCPP_INFO(
impl_->ros_node_->get_logger(),
"bayer simulation may be computationally expensive.");
impl_->img_encoding_.emplace_back(sensor_msgs::image_encodings::BAYER_BGGR8);
impl_->img_step_.push_back(1);
} else if (format == "BAYER_GBRG8") {
RCLCPP_INFO(impl_->ros_node_->get_logger(),
RCLCPP_INFO(
impl_->ros_node_->get_logger(),
"bayer simulation may be computationally expensive.");
impl_->img_encoding_.emplace_back(sensor_msgs::image_encodings::BAYER_GBRG8);
impl_->img_step_.push_back(1);
} else if (format == "BAYER_GRBG8") {
RCLCPP_INFO(impl_->ros_node_->get_logger(),
RCLCPP_INFO(
impl_->ros_node_->get_logger(),
"bayer simulation may be computationally expensive.");
impl_->img_encoding_.emplace_back(sensor_msgs::image_encodings::BAYER_GRBG8);
impl_->img_step_.push_back(1);
Expand Down Expand Up @@ -338,7 +352,8 @@ void GazeboRosCamera::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _
if (focal_length == 0) {
focal_length = computed_focal_length;
} else if (!ignition::math::equal(focal_length, computed_focal_length)) {
RCLCPP_WARN(impl_->ros_node_->get_logger(),
RCLCPP_WARN(
impl_->ros_node_->get_logger(),
"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"
Expand Down Expand Up @@ -425,7 +440,8 @@ void GazeboRosCamera::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _
camera_info_msg.p[11] = 0.0;

// Initialize camera_info_manager
impl_->camera_info_manager_.push_back(std::make_shared<camera_info_manager::CameraInfoManager>(
impl_->camera_info_manager_.push_back(
std::make_shared<camera_info_manager::CameraInfoManager>(
impl_->ros_node_.get(), impl_->camera_name_));
impl_->camera_info_manager_.back()->setCameraInfo(camera_info_msg);
}
Expand Down Expand Up @@ -485,7 +501,8 @@ void GazeboRosCamera::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _
std::string param_name = parameter.get_name();
if (param_name == "update_rate") {
if (nullptr != impl_->trigger_sub_) {
RCLCPP_WARN(impl_->ros_node_->get_logger(),
RCLCPP_WARN(
impl_->ros_node_->get_logger(),
"Cannot set update rate for triggered camera");
result.successful = false;
} else {
Expand All @@ -502,14 +519,17 @@ void GazeboRosCamera::Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _
}

if (rate >= 0.0) {
RCLCPP_INFO(impl_->ros_node_->get_logger(),
RCLCPP_INFO(
impl_->ros_node_->get_logger(),
"Camera update rate changed to [%.2f Hz]", rate);
} else {
RCLCPP_WARN(impl_->ros_node_->get_logger(),
RCLCPP_WARN(
impl_->ros_node_->get_logger(),
"Camera update rate should be positive. Setting to maximum");
}
} else {
RCLCPP_WARN(impl_->ros_node_->get_logger(),
RCLCPP_WARN(
impl_->ros_node_->get_logger(),
"Value for param [update_rate] has to be of double type.");
result.successful = false;
}
Expand Down Expand Up @@ -555,7 +575,8 @@ void GazeboRosCamera::NewFrame(
sensor_update_time);

// Copy from src to image_msg
sensor_msgs::fillImage(impl_->image_msg_, impl_->img_encoding_[_camera_num], _height, _width,
sensor_msgs::fillImage(
impl_->image_msg_, impl_->img_encoding_[_camera_num], _height, _width,
impl_->img_step_[_camera_num] * _width, reinterpret_cast<const void *>(_image));

impl_->image_pub_[_camera_num].publish(impl_->image_msg_);
Expand Down Expand Up @@ -708,10 +729,12 @@ void GazeboRosCamera::OnNewDepthFrame(

if (impl_->image_msg_.data.size() == _width * _height * 3) {
// color
std::memcpy(&impl_->cloud_msg_.data[cloud_index + 16],
std::memcpy(
&impl_->cloud_msg_.data[cloud_index + 16],
&impl_->image_msg_.data[(i + j * _width) * 3], 3 * sizeof(uint8_t));
} else if (impl_->image_msg_.data.size() == _height * _width) {
std::memcpy(&impl_->cloud_msg_.data[cloud_index + 16],
std::memcpy(
&impl_->cloud_msg_.data[cloud_index + 16],
&impl_->image_msg_.data[i + j * _width], 3 * sizeof(uint8_t));
} else {
// no image
Expand Down
Loading