Skip to content

Commit

Permalink
Uncrustify
Browse files Browse the repository at this point in the history 
Style changes to conform to the new default setting introduced in ament/ament_lint#210.
Arguments that do not fit on one line must start on a new line.

Signed-off-by: Jacob Perron <jacob@openrobotics.org>
  • Loading branch information
@jacobperron
jacobperron committed Mar 13, 2020
1 parent 0ba55b9 commit 043640c
Show file tree
Hide file tree
Showing 50 changed files with 602 additions and 412 deletions.
37 changes: 24 additions & 13 deletions gazebo_plugins/src/gazebo_ros_ackermann_drive.cpp
View file
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
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
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

0 comments on commit 043640c

Please sign in to comment.