From 5c175587a76608932d274444b716fa3e8161faa6 Mon Sep 17 00:00:00 2001 From: Sarathkrishnan Ramesh Date: Sat, 2 May 2020 20:59:08 +0530 Subject: [PATCH 1/6] Change publishers to publish unique ptrs --- nav2_amcl/src/amcl_node.cpp | 45 ++++++++++--------- nav2_bt_navigator/src/ros_topic_logger.cpp | 9 ++-- nav2_controller/src/nav2_controller.cpp | 5 ++- nav2_costmap_2d/plugins/voxel_layer.cpp | 38 ++++++++-------- nav2_costmap_2d/src/costmap_2d_cloud.cpp | 44 +++++++++--------- nav2_costmap_2d/src/costmap_2d_markers.cpp | 36 +++++++-------- nav2_costmap_2d/src/costmap_2d_publisher.cpp | 27 ++++++----- nav2_costmap_2d/src/costmap_2d_ros.cpp | 10 ++--- .../dwb_core/src/publisher.cpp | 44 +++++++++--------- .../src/lifecycle_manager_client.cpp | 27 +++++------ .../include/nav2_recoveries/recovery.hpp | 11 ++--- nav2_recoveries/plugins/back_up.cpp | 13 +++--- nav2_recoveries/plugins/spin.cpp | 9 ++-- nav2_rviz_plugins/src/nav2_panel.cpp | 15 ++++--- .../src/dummy_controller/dummy_controller.cpp | 17 +++---- .../localization/test_localization_node.cpp | 39 ++++++++-------- 16 files changed, 204 insertions(+), 185 deletions(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 53de6a7d31..1306356faf 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -852,17 +852,18 @@ AmclNode::publishParticleCloud(const pf_sample_set_t * set) { // If initial pose is not known, AMCL does not know the current pose if (!initial_pose_is_known_) {return;} - geometry_msgs::msg::PoseArray cloud_msg; - cloud_msg.header.stamp = this->now(); - cloud_msg.header.frame_id = global_frame_id_; - cloud_msg.poses.resize(set->sample_count); + auto cloud_msg = std::make_unique(); + cloud_msg->header.stamp = this->now(); + cloud_msg->header.frame_id = global_frame_id_; + cloud_msg->poses.resize(set->sample_count); for (int i = 0; i < set->sample_count; i++) { - cloud_msg.poses[i].position.x = set->samples[i].pose.v[0]; - cloud_msg.poses[i].position.y = set->samples[i].pose.v[1]; - cloud_msg.poses[i].position.z = 0; - cloud_msg.poses[i].orientation = orientationAroundZAxis(set->samples[i].pose.v[2]); + cloud_msg->poses[i].position.x = set->samples[i].pose.v[0]; + cloud_msg->poses[i].position.y = set->samples[i].pose.v[1]; + cloud_msg->poses[i].position.z = 0; + cloud_msg->poses[i].orientation = orientationAroundZAxis(set->samples[i].pose.v[2]); } - particlecloud_pub_->publish(cloud_msg); + + particlecloud_pub_->publish(std::move(cloud_msg)); } bool @@ -923,35 +924,35 @@ AmclNode::publishAmclPose( return; } - geometry_msgs::msg::PoseWithCovarianceStamped p; + auto p = std::make_unique(); // Fill in the header - p.header.frame_id = global_frame_id_; - p.header.stamp = laser_scan->header.stamp; + p->header.frame_id = global_frame_id_; + p->header.stamp = laser_scan->header.stamp; // Copy in the pose - p.pose.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0]; - p.pose.pose.position.y = hyps[max_weight_hyp].pf_pose_mean.v[1]; - p.pose.pose.orientation = orientationAroundZAxis(hyps[max_weight_hyp].pf_pose_mean.v[2]); + p->pose.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0]; + p->pose.pose.position.y = hyps[max_weight_hyp].pf_pose_mean.v[1]; + p->pose.pose.orientation = orientationAroundZAxis(hyps[max_weight_hyp].pf_pose_mean.v[2]); // Copy in the covariance, converting from 3-D to 6-D pf_sample_set_t * set = pf_->sets + pf_->current_set; for (int i = 0; i < 2; i++) { for (int j = 0; j < 2; j++) { // Report the overall filter covariance, rather than the // covariance for the highest-weight cluster - // p.covariance[6*i+j] = hyps[max_weight_hyp].pf_pose_cov.m[i][j]; - p.pose.covariance[6 * i + j] = set->cov.m[i][j]; + // p->covariance[6*i+j] = hyps[max_weight_hyp].pf_pose_cov.m[i][j]; + p->pose.covariance[6 * i + j] = set->cov.m[i][j]; } } - p.pose.covariance[6 * 5 + 5] = set->cov.m[2][2]; + p->pose.covariance[6 * 5 + 5] = set->cov.m[2][2]; float temp = 0.0; - for (auto covariance_value : p.pose.covariance) { + for (auto covariance_value : p->pose.covariance) { temp += covariance_value; } - temp += p.pose.pose.position.x + p.pose.pose.position.y; + temp += p->pose.pose.position.x + p->pose.pose.position.y; if (!std::isnan(temp)) { RCLCPP_DEBUG(get_logger(), "Publishing pose"); - pose_pub_->publish(p); + last_published_pose_ = *p; first_pose_sent_ = true; - last_published_pose_ = p; + pose_pub_->publish(std::move(p)); } else { RCLCPP_WARN( get_logger(), "AMCL covariance or pose is NaN, likely due to an invalid " diff --git a/nav2_bt_navigator/src/ros_topic_logger.cpp b/nav2_bt_navigator/src/ros_topic_logger.cpp index e5c0c39e0b..3c1348f38d 100644 --- a/nav2_bt_navigator/src/ros_topic_logger.cpp +++ b/nav2_bt_navigator/src/ros_topic_logger.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include "nav2_bt_navigator/ros_topic_logger.hpp" #include "tf2_ros/buffer_interface.h" @@ -56,10 +57,10 @@ void RosTopicLogger::callback( void RosTopicLogger::flush() { if (event_log_.size() > 0) { - nav2_msgs::msg::BehaviorTreeLog log_msg; - log_msg.timestamp = ros_node_->now(); - log_msg.event_log = event_log_; - log_pub_->publish(log_msg); + auto log_msg = std::make_unique(); + log_msg->timestamp = ros_node_->now(); + log_msg->event_log = event_log_; + log_pub_->publish(std::move(log_msg)); event_log_.clear(); } } diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp index bf9e5c9a24..ef7798137e 100644 --- a/nav2_controller/src/nav2_controller.cpp +++ b/nav2_controller/src/nav2_controller.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include "nav2_core/exceptions.hpp" #include "nav_2d_utils/conversions.hpp" @@ -346,8 +347,8 @@ void ControllerServer::updateGlobalPath() void ControllerServer::publishVelocity(const geometry_msgs::msg::TwistStamped & velocity) { - auto cmd_vel = velocity.twist; - vel_publisher_->publish(cmd_vel); + auto cmd_vel = std::make_unique(velocity.twist); + vel_publisher_->publish(std::move(cmd_vel)); } void ControllerServer::publishZeroVelocity() diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp index b9f3f224b0..31eb421a30 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/nav2_costmap_2d/plugins/voxel_layer.cpp @@ -207,24 +207,25 @@ void VoxelLayer::updateBounds( } if (publish_voxel_) { - nav2_msgs::msg::VoxelGrid grid_msg; + auto grid_msg = std::make_unique(); unsigned int size = voxel_grid_.sizeX() * voxel_grid_.sizeY(); - grid_msg.size_x = voxel_grid_.sizeX(); - grid_msg.size_y = voxel_grid_.sizeY(); - grid_msg.size_z = voxel_grid_.sizeZ(); - grid_msg.data.resize(size); - memcpy(&grid_msg.data[0], voxel_grid_.getData(), size * sizeof(unsigned int)); - - grid_msg.origin.x = origin_x_; - grid_msg.origin.y = origin_y_; - grid_msg.origin.z = origin_z_; - - grid_msg.resolutions.x = resolution_; - grid_msg.resolutions.y = resolution_; - grid_msg.resolutions.z = z_resolution_; - grid_msg.header.frame_id = global_frame_; - grid_msg.header.stamp = node_->now(); - voxel_pub_->publish(grid_msg); + grid_msg->size_x = voxel_grid_.sizeX(); + grid_msg->size_y = voxel_grid_.sizeY(); + grid_msg->size_z = voxel_grid_.sizeZ(); + grid_msg->data.resize(size); + memcpy(&grid_msg->data[0], voxel_grid_.getData(), size * sizeof(unsigned int)); + + grid_msg->origin.x = origin_x_; + grid_msg->origin.y = origin_y_; + grid_msg->origin.z = origin_z_; + + grid_msg->resolutions.x = resolution_; + grid_msg->resolutions.y = resolution_; + grid_msg->resolutions.z = z_resolution_; + grid_msg->header.frame_id = global_frame_; + grid_msg->header.stamp = node_->now(); + + voxel_pub_->publish(std::move(grid_msg)); } updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y); @@ -400,7 +401,8 @@ void VoxelLayer::raytraceFreespace( clearing_endpoints_.header.frame_id = global_frame_; clearing_endpoints_.header.stamp = clearing_observation.cloud_->header.stamp; - clearing_endpoints_pub_->publish(clearing_endpoints_); + auto cloud_msg = std::make_unique(std::move(clearing_endpoints_)); + clearing_endpoints_pub_->publish(std::move(cloud_msg)); } } diff --git a/nav2_costmap_2d/src/costmap_2d_cloud.cpp b/nav2_costmap_2d/src/costmap_2d_cloud.cpp index ac4f091c76..2ab4a28de7 100644 --- a/nav2_costmap_2d/src/costmap_2d_cloud.cpp +++ b/nav2_costmap_2d/src/costmap_2d_cloud.cpp @@ -132,17 +132,17 @@ void voxelCallback(const nav2_msgs::msg::VoxelGrid::ConstSharedPtr grid) } { - sensor_msgs::msg::PointCloud cloud; - cloud.points.resize(num_marked); - cloud.channels.resize(1); - cloud.channels[0].values.resize(num_marked); - cloud.channels[0].name = "rgb"; - cloud.header.frame_id = frame_id; - cloud.header.stamp = stamp; - - sensor_msgs::msg::ChannelFloat32 & chan = cloud.channels[0]; + auto cloud = std::make_unique(); + cloud->points.resize(num_marked); + cloud->channels.resize(1); + cloud->channels[0].values.resize(num_marked); + cloud->channels[0].name = "rgb"; + cloud->header.frame_id = frame_id; + cloud->header.stamp = stamp; + + sensor_msgs::msg::ChannelFloat32 & chan = cloud->channels[0]; for (uint32_t i = 0; i < num_marked; ++i) { - geometry_msgs::msg::Point32 & p = cloud.points[i]; + geometry_msgs::msg::Point32 & p = cloud->points[i]; float & cval = chan.values[i]; Cell & c = g_marked[i]; @@ -159,21 +159,21 @@ void voxelCallback(const nav2_msgs::msg::VoxelGrid::ConstSharedPtr grid) memcpy(&cval, &col, sizeof col); } - pub_marked->publish(cloud); + pub_marked->publish(std::move(cloud)); } { - sensor_msgs::msg::PointCloud cloud; - cloud.points.resize(num_unknown); - cloud.channels.resize(1); - cloud.channels[0].values.resize(num_unknown); - cloud.channels[0].name = "rgb"; - cloud.header.frame_id = frame_id; - cloud.header.stamp = stamp; - - sensor_msgs::msg::ChannelFloat32 & chan = cloud.channels[0]; + auto cloud = std::make_unique(); + cloud->points.resize(num_unknown); + cloud->channels.resize(1); + cloud->channels[0].values.resize(num_unknown); + cloud->channels[0].name = "rgb"; + cloud->header.frame_id = frame_id; + cloud->header.stamp = stamp; + + sensor_msgs::msg::ChannelFloat32 & chan = cloud->channels[0]; for (uint32_t i = 0; i < num_unknown; ++i) { - geometry_msgs::msg::Point32 & p = cloud.points[i]; + geometry_msgs::msg::Point32 & p = cloud->points[i]; float & cval = chan.values[i]; Cell & c = g_unknown[i]; @@ -190,7 +190,7 @@ void voxelCallback(const nav2_msgs::msg::VoxelGrid::ConstSharedPtr grid) memcpy(&cval, &col, sizeof col); } - pub_unknown->publish(cloud); + pub_unknown->publish(std::move(cloud)); } timer.end(); diff --git a/nav2_costmap_2d/src/costmap_2d_markers.cpp b/nav2_costmap_2d/src/costmap_2d_markers.cpp index 60e8ab1b3d..0e7b5b5586 100644 --- a/nav2_costmap_2d/src/costmap_2d_markers.cpp +++ b/nav2_costmap_2d/src/costmap_2d_markers.cpp @@ -111,31 +111,31 @@ void voxelCallback(const nav2_msgs::msg::VoxelGrid::ConstSharedPtr grid) } } - visualization_msgs::msg::Marker m; - m.header.frame_id = frame_id; - m.header.stamp = stamp; - m.ns = g_node->get_namespace(); - m.id = 0; - m.type = visualization_msgs::msg::Marker::CUBE_LIST; - m.action = visualization_msgs::msg::Marker::ADD; - m.pose.orientation.w = 1.0; - m.scale.x = x_res; - m.scale.y = y_res; - m.scale.z = z_res; - m.color.r = g_colors_r[nav2_voxel_grid::MARKED]; - m.color.g = g_colors_g[nav2_voxel_grid::MARKED]; - m.color.b = g_colors_b[nav2_voxel_grid::MARKED]; - m.color.a = g_colors_a[nav2_voxel_grid::MARKED]; - m.points.resize(num_markers); + auto m = std::make_unique(); + m->header.frame_id = frame_id; + m->header.stamp = stamp; + m->ns = g_node->get_namespace(); + m->id = 0; + m->type = visualization_msgs::msg::Marker::CUBE_LIST; + m->action = visualization_msgs::msg::Marker::ADD; + m->pose.orientation.w = 1.0; + m->scale.x = x_res; + m->scale.y = y_res; + m->scale.z = z_res; + m->color.r = g_colors_r[nav2_voxel_grid::MARKED]; + m->color.g = g_colors_g[nav2_voxel_grid::MARKED]; + m->color.b = g_colors_b[nav2_voxel_grid::MARKED]; + m->color.a = g_colors_a[nav2_voxel_grid::MARKED]; + m->points.resize(num_markers); for (uint32_t i = 0; i < num_markers; ++i) { Cell & c = g_cells[i]; - geometry_msgs::msg::Point & p = m.points[i]; + geometry_msgs::msg::Point & p = m->points[i]; p.x = c.x; p.y = c.y; p.z = c.z; } - pub->publish(m); + pub->publish(std::move(m)); timer.end(); RCLCPP_INFO( diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp index e971cc77f6..dbe03bf8aa 100644 --- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp @@ -170,7 +170,9 @@ void Costmap2DPublisher::publishCostmap() { if (node_->count_subscribers(costmap_raw_pub_->get_topic_name()) > 0) { prepareCostmap(); - costmap_raw_pub_->publish(costmap_raw_); + + auto costmap_raw_ptr = std::make_unique(std::move(costmap_raw_)); + costmap_raw_pub_->publish(std::move(costmap_raw_ptr)); } float resolution = costmap_->getResolution(); @@ -182,28 +184,29 @@ void Costmap2DPublisher::publishCostmap() { if (node_->count_subscribers(costmap_pub_->get_topic_name()) > 0) { prepareGrid(); - costmap_pub_->publish(grid_); + auto grid_ptr = std::make_unique(std::move(grid_)); + costmap_pub_->publish(std::move(grid_ptr)); } } else if (x0_ < xn_) { if (node_->count_subscribers(costmap_update_pub_->get_topic_name()) > 0) { std::unique_lock lock(*(costmap_->getMutex())); // Publish Just an Update - map_msgs::msg::OccupancyGridUpdate update; - update.header.stamp = rclcpp::Time(); - update.header.frame_id = global_frame_; - update.x = x0_; - update.y = y0_; - update.width = xn_ - x0_; - update.height = yn_ - y0_; - update.data.resize(update.width * update.height); + auto update = std::make_unique(); + update->header.stamp = rclcpp::Time(); + update->header.frame_id = global_frame_; + update->x = x0_; + update->y = y0_; + update->width = xn_ - x0_; + update->height = yn_ - y0_; + update->data.resize(update->width * update->height); unsigned int i = 0; for (unsigned int y = y0_; y < yn_; y++) { for (unsigned int x = x0_; x < xn_; x++) { unsigned char cost = costmap_->getCost(x, y); - update.data[i++] = cost_translation_table_[cost]; + update->data[i++] = cost_translation_table_[cost]; } } - costmap_update_pub_->publish(update); + costmap_update_pub_->publish(std::move(update)); } } diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 4a56cd178d..376bf1d9c7 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -441,13 +441,13 @@ Costmap2DROS::updateMap() const double yaw = tf2::getYaw(pose.pose.orientation); layered_costmap_->updateMap(x, y, yaw); - geometry_msgs::msg::PolygonStamped footprint; - footprint.header.frame_id = global_frame_; - footprint.header.stamp = now(); - transformFootprint(x, y, yaw, padded_footprint_, footprint); + auto footprint = std::make_unique(); + footprint->header.frame_id = global_frame_; + footprint->header.stamp = now(); + transformFootprint(x, y, yaw, padded_footprint_, *footprint); RCLCPP_DEBUG(get_logger(), "Publishing footprint"); - footprint_pub_->publish(footprint); + footprint_pub_->publish(std::move(footprint)); initialized_ = true; } } diff --git a/nav2_dwb_controller/dwb_core/src/publisher.cpp b/nav2_dwb_controller/dwb_core/src/publisher.cpp index 0a3c847912..c5f25d0989 100644 --- a/nav2_dwb_controller/dwb_core/src/publisher.cpp +++ b/nav2_dwb_controller/dwb_core/src/publisher.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include "nav_2d_utils/conversions.hpp" #include "nav2_util/node_utils.hpp" @@ -163,7 +164,7 @@ DWBPublisher::publishTrajectories(const dwb_msgs::msg::LocalPlanEvaluation & res if (node_->count_subscribers(marker_pub_->get_topic_name()) < 1) {return;} if (!publish_trajectories_) {return;} - visualization_msgs::msg::MarkerArray ma; + auto ma = std::make_unique(); visualization_msgs::msg::Marker m; if (results.twists.size() == 0) {return;} @@ -216,9 +217,9 @@ DWBPublisher::publishTrajectories(const dwb_msgs::msg::LocalPlanEvaluation & res pt.z = 0; m.points.push_back(pt); } - ma.markers.push_back(m); + ma->markers.push_back(m); } - marker_pub_->publish(ma); + marker_pub_->publish(std::move(ma)); } void @@ -228,10 +229,13 @@ DWBPublisher::publishLocalPlan( { if (!publish_local_plan_) {return;} - nav_msgs::msg::Path path = - nav_2d_utils::poses2DToPath(traj.poses, header.frame_id, header.stamp); + auto path = + std::make_unique( + nav_2d_utils::poses2DToPath( + traj.poses, header.frame_id, + header.stamp)); if (node_->count_subscribers(local_pub_->get_topic_name()) < 1) { - local_pub_->publish(path); + local_pub_->publish(std::move(path)); } } @@ -244,21 +248,21 @@ DWBPublisher::publishCostGrid( if (!publish_cost_grid_pc_) {return;} - sensor_msgs::msg::PointCloud cost_grid_pc; - cost_grid_pc.header.frame_id = costmap_ros->getGlobalFrameID(); - cost_grid_pc.header.stamp = node_->now(); + auto cost_grid_pc = std::make_unique(); + cost_grid_pc->header.frame_id = costmap_ros->getGlobalFrameID(); + cost_grid_pc->header.stamp = node_->now(); nav2_costmap_2d::Costmap2D * costmap = costmap_ros->getCostmap(); double x_coord, y_coord; unsigned int size_x = costmap->getSizeInCellsX(); unsigned int size_y = costmap->getSizeInCellsY(); - cost_grid_pc.points.resize(size_x * size_y); + cost_grid_pc->points.resize(size_x * size_y); unsigned int i = 0; for (unsigned int cy = 0; cy < size_y; cy++) { for (unsigned int cx = 0; cx < size_x; cx++) { costmap->mapToWorld(cx, cy, x_coord, y_coord); - cost_grid_pc.points[i].x = x_coord; - cost_grid_pc.points[i].y = y_coord; + cost_grid_pc->points[i].x = x_coord; + cost_grid_pc->points[i].y = y_coord; i++; } } @@ -268,23 +272,23 @@ DWBPublisher::publishCostGrid( totals.values.resize(size_x * size_y, 0.0); for (TrajectoryCritic::Ptr critic : critics) { - unsigned int channel_index = cost_grid_pc.channels.size(); - critic->addCriticVisualization(cost_grid_pc); - if (channel_index == cost_grid_pc.channels.size()) { + unsigned int channel_index = cost_grid_pc->channels.size(); + critic->addCriticVisualization(*cost_grid_pc); + if (channel_index == cost_grid_pc->channels.size()) { // No channels were added, so skip to next critic continue; } double scale = critic->getScale(); for (i = 0; i < size_x * size_y; i++) { - totals.values[i] += cost_grid_pc.channels[channel_index].values[i] * scale; + totals.values[i] += cost_grid_pc->channels[channel_index].values[i] * scale; } } - cost_grid_pc.channels.push_back(totals); + cost_grid_pc->channels.push_back(totals); // TODO(crdelsey): convert pc to pc2 // sensor_msgs::msg::PointCloud2 cost_grid_pc2; // convertPointCloudToPointCloud2(cost_grid_pc, cost_grid_pc2); - cost_grid_pc_pub_->publish(cost_grid_pc); + cost_grid_pc_pub_->publish(std::move(cost_grid_pc)); } void @@ -312,8 +316,8 @@ DWBPublisher::publishGenericPlan( { if (node_->count_subscribers(pub.get_topic_name()) < 1) {return;} if (!flag) {return;} - nav_msgs::msg::Path path = nav_2d_utils::pathToPath(plan); - pub.publish(path); + auto path = std::make_unique(nav_2d_utils::pathToPath(plan)); + pub.publish(std::move(path)); } } // namespace dwb_core diff --git a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp index a6e300e77d..4a65c05b8a 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include "tf2_geometry_msgs/tf2_geometry_msgs.h" #include "nav2_util/geometry_utils.hpp" @@ -110,19 +111,19 @@ void LifecycleManagerClient::set_initial_pose(double x, double y, double theta) { const double PI = 3.141592653589793238463; - geometry_msgs::msg::PoseWithCovarianceStamped pose; - - pose.header.frame_id = "map"; - pose.header.stamp = node_->now(); - pose.pose.pose.position.x = x; - pose.pose.pose.position.y = y; - pose.pose.pose.position.z = 0.0; - pose.pose.pose.orientation = orientationAroundZAxis(theta); - pose.pose.covariance[6 * 0 + 0] = 0.5 * 0.5; - pose.pose.covariance[6 * 1 + 1] = 0.5 * 0.5; - pose.pose.covariance[6 * 5 + 5] = PI / 12.0 * PI / 12.0; - - initial_pose_publisher_->publish(pose); + auto pose = std::make_unique(); + + pose->header.frame_id = "map"; + pose->header.stamp = node_->now(); + pose->pose.pose.position.x = x; + pose->pose.pose.position.y = y; + pose->pose.pose.position.z = 0.0; + pose->pose.pose.orientation = orientationAroundZAxis(theta); + pose->pose.covariance[6 * 0 + 0] = 0.5 * 0.5; + pose->pose.covariance[6 * 1 + 1] = 0.5 * 0.5; + pose->pose.covariance[6 * 5 + 5] = PI / 12.0 * PI / 12.0; + + initial_pose_publisher_->publish(std::move(pose)); } bool diff --git a/nav2_recoveries/include/nav2_recoveries/recovery.hpp b/nav2_recoveries/include/nav2_recoveries/recovery.hpp index 731188cecb..80606e90ff 100644 --- a/nav2_recoveries/include/nav2_recoveries/recovery.hpp +++ b/nav2_recoveries/include/nav2_recoveries/recovery.hpp @@ -21,6 +21,7 @@ #include #include #include +#include #include "rclcpp/rclcpp.hpp" #include "tf2_ros/transform_listener.h" @@ -203,12 +204,12 @@ class Recovery : public nav2_core::Recovery void stopRobot() { - geometry_msgs::msg::Twist cmd_vel; - cmd_vel.linear.x = 0.0; - cmd_vel.linear.y = 0.0; - cmd_vel.angular.z = 0.0; + auto cmd_vel = std::make_unique(); + cmd_vel->linear.x = 0.0; + cmd_vel->linear.y = 0.0; + cmd_vel->angular.z = 0.0; - vel_pub_->publish(cmd_vel); + vel_pub_->publish(std::move(cmd_vel)); } }; diff --git a/nav2_recoveries/plugins/back_up.cpp b/nav2_recoveries/plugins/back_up.cpp index fc785d6964..4fd53696ed 100644 --- a/nav2_recoveries/plugins/back_up.cpp +++ b/nav2_recoveries/plugins/back_up.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include "back_up.hpp" #include "nav2_util/node_utils.hpp" @@ -77,23 +78,23 @@ Status BackUp::onCycleUpdate() return Status::SUCCEEDED; } // TODO(mhpanah): cmd_vel value should be passed as a parameter - geometry_msgs::msg::Twist cmd_vel; - cmd_vel.linear.y = 0.0; - cmd_vel.angular.z = 0.0; - command_x_ < 0 ? cmd_vel.linear.x = -command_speed_ : cmd_vel.linear.x = command_speed_; + auto cmd_vel = std::make_unique(); + cmd_vel->linear.y = 0.0; + cmd_vel->angular.z = 0.0; + command_x_ < 0 ? cmd_vel->linear.x = -command_speed_ : cmd_vel->linear.x = command_speed_; geometry_msgs::msg::Pose2D pose2d; pose2d.x = current_pose.pose.position.x; pose2d.y = current_pose.pose.position.y; pose2d.theta = tf2::getYaw(current_pose.pose.orientation); - if (!isCollisionFree(distance, cmd_vel, pose2d)) { + if (!isCollisionFree(distance, *cmd_vel, pose2d)) { stopRobot(); RCLCPP_WARN(node_->get_logger(), "Collision Ahead - Exiting BackUp"); return Status::SUCCEEDED; } - vel_pub_->publish(cmd_vel); + vel_pub_->publish(std::move(cmd_vel)); return Status::RUNNING; } diff --git a/nav2_recoveries/plugins/spin.cpp b/nav2_recoveries/plugins/spin.cpp index 547d56c169..501f814b7e 100644 --- a/nav2_recoveries/plugins/spin.cpp +++ b/nav2_recoveries/plugins/spin.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "spin.hpp" #pragma GCC diagnostic push @@ -106,21 +107,21 @@ Status Spin::onCycleUpdate() double vel = sqrt(2 * rotational_acc_lim_ * relative_yaw); vel = std::min(std::max(vel, min_rotational_vel_), max_rotational_vel_); - geometry_msgs::msg::Twist cmd_vel; - cmd_yaw_ < 0 ? cmd_vel.angular.z = -vel : cmd_vel.angular.z = vel; + auto cmd_vel = std::make_unique(); + cmd_yaw_ < 0 ? cmd_vel->angular.z = -vel : cmd_vel->angular.z = vel; geometry_msgs::msg::Pose2D pose2d; pose2d.x = current_pose.pose.position.x; pose2d.y = current_pose.pose.position.y; pose2d.theta = tf2::getYaw(current_pose.pose.orientation); - if (!isCollisionFree(relative_yaw, cmd_vel, pose2d)) { + if (!isCollisionFree(relative_yaw, *cmd_vel, pose2d)) { stopRobot(); RCLCPP_WARN(node_->get_logger(), "Collision Ahead - Exiting Spin"); return Status::SUCCEEDED; } - vel_pub_->publish(cmd_vel); + vel_pub_->publish(std::move(cmd_vel)); return Status::RUNNING; } diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp index 776f11ca70..9d1b315683 100644 --- a/nav2_rviz_plugins/src/nav2_panel.cpp +++ b/nav2_rviz_plugins/src/nav2_panel.cpp @@ -19,6 +19,7 @@ #include #include +#include #include "nav2_rviz_plugins/goal_common.hpp" #include "rviz_common/display_context.hpp" @@ -609,7 +610,7 @@ Nav2Panel::updateWpNavigationMarkers() { resetUniqueId(); - visualization_msgs::msg::MarkerArray marker_array; + auto marker_array = std::make_unique(); for (size_t i = 0; i < acummulated_poses_.size(); i++) { // Draw a green arrow at the waypoint pose @@ -628,7 +629,7 @@ Nav2Panel::updateWpNavigationMarkers() arrow_marker.color.a = 1.0f; arrow_marker.lifetime = rclcpp::Duration(0); arrow_marker.frame_locked = false; - marker_array.markers.push_back(arrow_marker); + marker_array->markers.push_back(arrow_marker); // Draw a red circle at the waypoint pose visualization_msgs::msg::Marker circle_marker; @@ -646,7 +647,7 @@ Nav2Panel::updateWpNavigationMarkers() circle_marker.color.a = 1.0f; circle_marker.lifetime = rclcpp::Duration(0); circle_marker.frame_locked = false; - marker_array.markers.push_back(circle_marker); + marker_array->markers.push_back(circle_marker); // Draw the waypoint number visualization_msgs::msg::Marker marker_text; @@ -666,16 +667,16 @@ Nav2Panel::updateWpNavigationMarkers() marker_text.lifetime = rclcpp::Duration(0); marker_text.frame_locked = false; marker_text.text = "wp_" + std::to_string(i + 1); - marker_array.markers.push_back(marker_text); + marker_array->markers.push_back(marker_text); } - if (marker_array.markers.empty()) { + if (marker_array->markers.empty()) { visualization_msgs::msg::Marker clear_all_marker; clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL; - marker_array.markers.push_back(clear_all_marker); + marker_array->markers.push_back(clear_all_marker); } - wp_navigation_markers_pub_->publish(marker_array); + wp_navigation_markers_pub_->publish(std::move(marker_array)); } } // namespace nav2_rviz_plugins diff --git a/nav2_system_tests/src/dummy_controller/dummy_controller.cpp b/nav2_system_tests/src/dummy_controller/dummy_controller.cpp index d87b87f857..16643ed4a6 100644 --- a/nav2_system_tests/src/dummy_controller/dummy_controller.cpp +++ b/nav2_system_tests/src/dummy_controller/dummy_controller.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include "dummy_controller.hpp" @@ -76,9 +77,9 @@ DummyController::followPath(const nav2_behavior_tree::FollowPathCommand::SharedP } // Output control command - geometry_msgs::msg::Twist cmd_vel; - cmd_vel.linear.x = 0.1; - vel_pub_->publish(cmd_vel); + auto cmd_vel = std::make_unique(); + cmd_vel->linear.x = 0.1; + vel_pub_->publish(std::move(cmd_vel)); if (current_time - start_time >= 30s) { RCLCPP_INFO(get_logger(), "Reached end point"); @@ -93,11 +94,11 @@ DummyController::followPath(const nav2_behavior_tree::FollowPathCommand::SharedP void DummyController::setZeroVelocity() { - geometry_msgs::msg::Twist cmd_vel; - cmd_vel.linear.x = 0.0; - cmd_vel.linear.y = 0.0; - cmd_vel.angular.z = 0.0; - vel_pub_->publish(cmd_vel); + auto cmd_vel = std::make_unique(); + cmd_vel->linear.x = 0.0; + cmd_vel->linear.y = 0.0; + cmd_vel->angular.z = 0.0; + vel_pub_->publish(std::move(cmd_vel)); } } // namespace nav2_system_tests diff --git a/nav2_system_tests/src/localization/test_localization_node.cpp b/nav2_system_tests/src/localization/test_localization_node.cpp index 32ecf21451..c0faa7e5e0 100644 --- a/nav2_system_tests/src/localization/test_localization_node.cpp +++ b/nav2_system_tests/src/localization/test_localization_node.cpp @@ -36,6 +36,7 @@ class TestAmclPose : public ::testing::Test { public: TestAmclPose() + : testPose_(std::make_unique()) { pose_callback_ = false; initTestPose(); @@ -53,7 +54,7 @@ class TestAmclPose : public ::testing::Test subscription_ = node->create_subscription( "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), std::bind(&TestAmclPose::amcl_pose_callback, this, _1)); - initial_pose_pub_->publish(testPose_); + initial_pose_pub_->publish(*testPose_); } bool defaultAmclTest(); @@ -72,7 +73,7 @@ class TestAmclPose : public ::testing::Test } rclcpp::Publisher::SharedPtr initial_pose_pub_; rclcpp::Subscription::SharedPtr subscription_; - geometry_msgs::msg::PoseWithCovarianceStamped testPose_; + std::unique_ptr testPose_; double amcl_pose_x; double amcl_pose_y; bool pose_callback_; @@ -81,15 +82,15 @@ class TestAmclPose : public ::testing::Test bool TestAmclPose::defaultAmclTest() { - initial_pose_pub_->publish(testPose_); + initial_pose_pub_->publish(*testPose_); while (!pose_callback_) { // TODO(mhpanah): Initial pose should only be published once. - initial_pose_pub_->publish(testPose_); + initial_pose_pub_->publish(*testPose_); std::this_thread::sleep_for(1s); rclcpp::spin_some(node); } - if (std::abs(amcl_pose_x - testPose_.pose.pose.position.x) < tol_ && - std::abs(amcl_pose_y - testPose_.pose.pose.position.y) < tol_) + if (std::abs(amcl_pose_x - testPose_->pose.pose.position.x) < tol_ && + std::abs(amcl_pose_y - testPose_->pose.pose.position.y) < tol_) { return true; } else { @@ -99,21 +100,21 @@ bool TestAmclPose::defaultAmclTest() void TestAmclPose::initTestPose() { - testPose_.header.frame_id = "map"; - testPose_.header.stamp = rclcpp::Time(); - testPose_.pose.pose.position.x = -2.0; - testPose_.pose.pose.position.y = -0.5; - testPose_.pose.pose.position.z = 0.0; - testPose_.pose.pose.orientation.x = 0.0; - testPose_.pose.pose.orientation.y = 0.0; - testPose_.pose.pose.orientation.z = 0.0; - testPose_.pose.pose.orientation.w = 1.0; + testPose_->header.frame_id = "map"; + testPose_->header.stamp = rclcpp::Time(); + testPose_->pose.pose.position.x = -2.0; + testPose_->pose.pose.position.y = -0.5; + testPose_->pose.pose.position.z = 0.0; + testPose_->pose.pose.orientation.x = 0.0; + testPose_->pose.pose.orientation.y = 0.0; + testPose_->pose.pose.orientation.z = 0.0; + testPose_->pose.pose.orientation.w = 1.0; for (int i = 0; i < 35; i++) { - testPose_.pose.covariance[i] = 0.0; + testPose_->pose.covariance[i] = 0.0; } - testPose_.pose.covariance[0] = 0.08; - testPose_.pose.covariance[7] = 0.08; - testPose_.pose.covariance[35] = 0.05; + testPose_->pose.covariance[0] = 0.08; + testPose_->pose.covariance[7] = 0.08; + testPose_->pose.covariance[35] = 0.05; } TEST_F(TestAmclPose, SimpleAmclTest) From cb2705e1230ab26ef1bcaca9ade084060a5313f6 Mon Sep 17 00:00:00 2001 From: Sarathkrishnan Ramesh Date: Mon, 4 May 2020 17:28:16 +0530 Subject: [PATCH 2/6] Revert test case modification --- .../localization/test_localization_node.cpp | 39 +++++++++---------- 1 file changed, 19 insertions(+), 20 deletions(-) diff --git a/nav2_system_tests/src/localization/test_localization_node.cpp b/nav2_system_tests/src/localization/test_localization_node.cpp index c0faa7e5e0..32ecf21451 100644 --- a/nav2_system_tests/src/localization/test_localization_node.cpp +++ b/nav2_system_tests/src/localization/test_localization_node.cpp @@ -36,7 +36,6 @@ class TestAmclPose : public ::testing::Test { public: TestAmclPose() - : testPose_(std::make_unique()) { pose_callback_ = false; initTestPose(); @@ -54,7 +53,7 @@ class TestAmclPose : public ::testing::Test subscription_ = node->create_subscription( "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), std::bind(&TestAmclPose::amcl_pose_callback, this, _1)); - initial_pose_pub_->publish(*testPose_); + initial_pose_pub_->publish(testPose_); } bool defaultAmclTest(); @@ -73,7 +72,7 @@ class TestAmclPose : public ::testing::Test } rclcpp::Publisher::SharedPtr initial_pose_pub_; rclcpp::Subscription::SharedPtr subscription_; - std::unique_ptr testPose_; + geometry_msgs::msg::PoseWithCovarianceStamped testPose_; double amcl_pose_x; double amcl_pose_y; bool pose_callback_; @@ -82,15 +81,15 @@ class TestAmclPose : public ::testing::Test bool TestAmclPose::defaultAmclTest() { - initial_pose_pub_->publish(*testPose_); + initial_pose_pub_->publish(testPose_); while (!pose_callback_) { // TODO(mhpanah): Initial pose should only be published once. - initial_pose_pub_->publish(*testPose_); + initial_pose_pub_->publish(testPose_); std::this_thread::sleep_for(1s); rclcpp::spin_some(node); } - if (std::abs(amcl_pose_x - testPose_->pose.pose.position.x) < tol_ && - std::abs(amcl_pose_y - testPose_->pose.pose.position.y) < tol_) + if (std::abs(amcl_pose_x - testPose_.pose.pose.position.x) < tol_ && + std::abs(amcl_pose_y - testPose_.pose.pose.position.y) < tol_) { return true; } else { @@ -100,21 +99,21 @@ bool TestAmclPose::defaultAmclTest() void TestAmclPose::initTestPose() { - testPose_->header.frame_id = "map"; - testPose_->header.stamp = rclcpp::Time(); - testPose_->pose.pose.position.x = -2.0; - testPose_->pose.pose.position.y = -0.5; - testPose_->pose.pose.position.z = 0.0; - testPose_->pose.pose.orientation.x = 0.0; - testPose_->pose.pose.orientation.y = 0.0; - testPose_->pose.pose.orientation.z = 0.0; - testPose_->pose.pose.orientation.w = 1.0; + testPose_.header.frame_id = "map"; + testPose_.header.stamp = rclcpp::Time(); + testPose_.pose.pose.position.x = -2.0; + testPose_.pose.pose.position.y = -0.5; + testPose_.pose.pose.position.z = 0.0; + testPose_.pose.pose.orientation.x = 0.0; + testPose_.pose.pose.orientation.y = 0.0; + testPose_.pose.pose.orientation.z = 0.0; + testPose_.pose.pose.orientation.w = 1.0; for (int i = 0; i < 35; i++) { - testPose_->pose.covariance[i] = 0.0; + testPose_.pose.covariance[i] = 0.0; } - testPose_->pose.covariance[0] = 0.08; - testPose_->pose.covariance[7] = 0.08; - testPose_->pose.covariance[35] = 0.05; + testPose_.pose.covariance[0] = 0.08; + testPose_.pose.covariance[7] = 0.08; + testPose_.pose.covariance[35] = 0.05; } TEST_F(TestAmclPose, SimpleAmclTest) From 63507ef0b600ce84238eae8da123b0d6c1ceeb3d Mon Sep 17 00:00:00 2001 From: Sarathkrishnan Ramesh Date: Tue, 5 May 2020 13:14:31 +0530 Subject: [PATCH 3/6] Update function signature to receive unique_ptrs --- nav2_recoveries/plugins/back_up.cpp | 6 +++--- nav2_recoveries/plugins/back_up.hpp | 2 +- nav2_recoveries/plugins/spin.cpp | 6 +++--- nav2_recoveries/plugins/spin.hpp | 2 +- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/nav2_recoveries/plugins/back_up.cpp b/nav2_recoveries/plugins/back_up.cpp index 4fd53696ed..bf25666dec 100644 --- a/nav2_recoveries/plugins/back_up.cpp +++ b/nav2_recoveries/plugins/back_up.cpp @@ -88,7 +88,7 @@ Status BackUp::onCycleUpdate() pose2d.y = current_pose.pose.position.y; pose2d.theta = tf2::getYaw(current_pose.pose.orientation); - if (!isCollisionFree(distance, *cmd_vel, pose2d)) { + if (!isCollisionFree(distance, cmd_vel, pose2d)) { stopRobot(); RCLCPP_WARN(node_->get_logger(), "Collision Ahead - Exiting BackUp"); return Status::SUCCEEDED; @@ -101,7 +101,7 @@ Status BackUp::onCycleUpdate() bool BackUp::isCollisionFree( const double & distance, - const geometry_msgs::msg::Twist & cmd_vel, + std::unique_ptr & cmd_vel, geometry_msgs::msg::Pose2D & pose2d) { // Simulate ahead by simulate_ahead_time_ in cycle_frequency_ increments @@ -111,7 +111,7 @@ bool BackUp::isCollisionFree( const int max_cycle_count = static_cast(cycle_frequency_ * simulate_ahead_time_); while (cycle_count < max_cycle_count) { - sim_position_change = cmd_vel.linear.x * (cycle_count / cycle_frequency_); + sim_position_change = cmd_vel->linear.x * (cycle_count / cycle_frequency_); pose2d.x += sim_position_change; cycle_count++; diff --git a/nav2_recoveries/plugins/back_up.hpp b/nav2_recoveries/plugins/back_up.hpp index 3ce3623e5a..286410a7dc 100644 --- a/nav2_recoveries/plugins/back_up.hpp +++ b/nav2_recoveries/plugins/back_up.hpp @@ -38,7 +38,7 @@ class BackUp : public Recovery protected: bool isCollisionFree( const double & distance, - const geometry_msgs::msg::Twist & cmd_vel, + std::unique_ptr & cmd_vel, geometry_msgs::msg::Pose2D & pose2d); void onConfigure() override; diff --git a/nav2_recoveries/plugins/spin.cpp b/nav2_recoveries/plugins/spin.cpp index 501f814b7e..6c8a4093c6 100644 --- a/nav2_recoveries/plugins/spin.cpp +++ b/nav2_recoveries/plugins/spin.cpp @@ -115,7 +115,7 @@ Status Spin::onCycleUpdate() pose2d.y = current_pose.pose.position.y; pose2d.theta = tf2::getYaw(current_pose.pose.orientation); - if (!isCollisionFree(relative_yaw, *cmd_vel, pose2d)) { + if (!isCollisionFree(relative_yaw, cmd_vel, pose2d)) { stopRobot(); RCLCPP_WARN(node_->get_logger(), "Collision Ahead - Exiting Spin"); return Status::SUCCEEDED; @@ -128,7 +128,7 @@ Status Spin::onCycleUpdate() bool Spin::isCollisionFree( const double & relative_yaw, - const geometry_msgs::msg::Twist & cmd_vel, + std::unique_ptr & cmd_vel, geometry_msgs::msg::Pose2D & pose2d) { // Simulate ahead by simulate_ahead_time_ in cycle_frequency_ increments @@ -137,7 +137,7 @@ bool Spin::isCollisionFree( const int max_cycle_count = static_cast(cycle_frequency_ * simulate_ahead_time_); while (cycle_count < max_cycle_count) { - sim_position_change = cmd_vel.angular.z * (cycle_count / cycle_frequency_); + sim_position_change = cmd_vel->angular.z * (cycle_count / cycle_frequency_); pose2d.theta += sim_position_change; cycle_count++; diff --git a/nav2_recoveries/plugins/spin.hpp b/nav2_recoveries/plugins/spin.hpp index 358fdec1c9..342d8e1dfd 100644 --- a/nav2_recoveries/plugins/spin.hpp +++ b/nav2_recoveries/plugins/spin.hpp @@ -40,7 +40,7 @@ class Spin : public Recovery protected: bool isCollisionFree( const double & distance, - const geometry_msgs::msg::Twist & cmd_vel, + std::unique_ptr & cmd_vel, geometry_msgs::msg::Pose2D & pose2d); double min_rotational_vel_; From ed6ff7d18fa75d101c4e7e29c77e713dc11164eb Mon Sep 17 00:00:00 2001 From: Sarathkrishnan Ramesh Date: Tue, 5 May 2020 19:48:31 +0530 Subject: [PATCH 4/6] Update publishers in nav2_costmap_2d to publish unique ptrs --- .../nav2_costmap_2d/costmap_2d_publisher.hpp | 8 +- .../include/nav2_costmap_2d/voxel_layer.hpp | 1 - nav2_costmap_2d/plugins/voxel_layer.cpp | 15 ++-- nav2_costmap_2d/src/costmap_2d_publisher.cpp | 82 ++++++++++--------- 4 files changed, 57 insertions(+), 49 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp index bbd8e6988d..9d893dbada 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp @@ -118,8 +118,8 @@ class Costmap2DPublisher private: /** @brief Prepare grid_ message for publication. */ - void prepareGrid(); - void prepareCostmap(); + std::unique_ptr prepareGrid(); + std::unique_ptr prepareCostmap(); /** @brief Publish the latest full costmap to the new subscriber. */ // void onNewSubscription(const ros::SingleSubscriberPublisher& pub); @@ -151,8 +151,8 @@ class Costmap2DPublisher // Service for getting the costmaps rclcpp::Service::SharedPtr costmap_service_; - nav_msgs::msg::OccupancyGrid grid_; - nav2_msgs::msg::Costmap costmap_raw_; + float grid_resolution; + unsigned int grid_width, grid_height; // Translate from 0-255 values in costmap to -1 to 100 values in message. static char * cost_translation_table_; }; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp index 764496cc68..92445cd71f 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp @@ -99,7 +99,6 @@ class VoxelLayer : public ObstacleLayer double z_resolution_, origin_z_; int unknown_threshold_, mark_threshold_, size_z_; rclcpp::Publisher::SharedPtr clearing_endpoints_pub_; - sensor_msgs::msg::PointCloud clearing_endpoints_; inline bool worldToMap3DFloat( double wx, double wy, double wz, double & mx, double & my, diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp index 31eb421a30..819075a3e7 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/nav2_costmap_2d/plugins/voxel_layer.cpp @@ -290,6 +290,8 @@ void VoxelLayer::raytraceFreespace( double * max_x, double * max_y) { + auto clearing_endpoints_ = std::make_unique(); + size_t clearing_observation_cloud_size = clearing_observation.cloud_->height * clearing_observation.cloud_->width; if (clearing_observation_cloud_size == 0) { @@ -311,8 +313,8 @@ void VoxelLayer::raytraceFreespace( bool publish_clearing_points = (node_->count_subscribers("clearing_endpoints") > 0); if (publish_clearing_points) { - clearing_endpoints_.points.clear(); - clearing_endpoints_.points.reserve(clearing_observation_cloud_size); + clearing_endpoints_->points.clear(); + clearing_endpoints_->points.reserve(clearing_observation_cloud_size); } // we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later @@ -392,17 +394,16 @@ void VoxelLayer::raytraceFreespace( point.x = wpx; point.y = wpy; point.z = wpz; - clearing_endpoints_.points.push_back(point); + clearing_endpoints_->points.push_back(point); } } } if (publish_clearing_points) { - clearing_endpoints_.header.frame_id = global_frame_; - clearing_endpoints_.header.stamp = clearing_observation.cloud_->header.stamp; + clearing_endpoints_->header.frame_id = global_frame_; + clearing_endpoints_->header.stamp = clearing_observation.cloud_->header.stamp; - auto cloud_msg = std::make_unique(std::move(clearing_endpoints_)); - clearing_endpoints_pub_->publish(std::move(cloud_msg)); + clearing_endpoints_pub_->publish(std::move(clearing_endpoints_)); } } diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp index dbe03bf8aa..c1172102e4 100644 --- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp @@ -107,85 +107,93 @@ void Costmap2DPublisher::onNewSubscription(const ros::SingleSubscriberPublisher& } */ // prepare grid_ message for publication. -void Costmap2DPublisher::prepareGrid() +std::unique_ptr Costmap2DPublisher::prepareGrid() { std::unique_lock lock(*(costmap_->getMutex())); - double resolution = costmap_->getResolution(); + grid_resolution = costmap_->getResolution(); + grid_width = costmap_->getSizeInCellsX(); + grid_height = costmap_->getSizeInCellsY(); + + auto grid_ = std::make_unique(); - grid_.header.frame_id = global_frame_; - grid_.header.stamp = rclcpp::Time(); + grid_->header.frame_id = global_frame_; + grid_->header.stamp = rclcpp::Time(); - grid_.info.resolution = resolution; + grid_->info.resolution = grid_resolution; - grid_.info.width = costmap_->getSizeInCellsX(); - grid_.info.height = costmap_->getSizeInCellsY(); + grid_->info.width = grid_width; + grid_->info.height = grid_height; double wx, wy; costmap_->mapToWorld(0, 0, wx, wy); - grid_.info.origin.position.x = wx - resolution / 2; - grid_.info.origin.position.y = wy - resolution / 2; - grid_.info.origin.position.z = 0.0; - grid_.info.origin.orientation.w = 1.0; + grid_->info.origin.position.x = wx - grid_resolution / 2; + grid_->info.origin.position.y = wy - grid_resolution / 2; + grid_->info.origin.position.z = 0.0; + grid_->info.origin.orientation.w = 1.0; saved_origin_x_ = costmap_->getOriginX(); saved_origin_y_ = costmap_->getOriginY(); - grid_.data.resize(grid_.info.width * grid_.info.height); + grid_->data.resize(grid_->info.width * grid_->info.height); unsigned char * data = costmap_->getCharMap(); - for (unsigned int i = 0; i < grid_.data.size(); i++) { - grid_.data[i] = cost_translation_table_[data[i]]; + for (unsigned int i = 0; i < grid_->data.size(); i++) { + grid_->data[i] = cost_translation_table_[data[i]]; } + + return std::move(grid_); } -void Costmap2DPublisher::prepareCostmap() +std::unique_ptr Costmap2DPublisher::prepareCostmap() { std::unique_lock lock(*(costmap_->getMutex())); double resolution = costmap_->getResolution(); - costmap_raw_.header.frame_id = global_frame_; - costmap_raw_.header.stamp = node_->now(); + auto costmap_raw_ = std::make_unique(); - costmap_raw_.metadata.layer = "master"; - costmap_raw_.metadata.resolution = resolution; + costmap_raw_->header.frame_id = global_frame_; + costmap_raw_->header.stamp = node_->now(); - costmap_raw_.metadata.size_x = costmap_->getSizeInCellsX(); - costmap_raw_.metadata.size_y = costmap_->getSizeInCellsY(); + costmap_raw_->metadata.layer = "master"; + costmap_raw_->metadata.resolution = resolution; + + costmap_raw_->metadata.size_x = costmap_->getSizeInCellsX(); + costmap_raw_->metadata.size_y = costmap_->getSizeInCellsY(); double wx, wy; costmap_->mapToWorld(0, 0, wx, wy); - costmap_raw_.metadata.origin.position.x = wx - resolution / 2; - costmap_raw_.metadata.origin.position.y = wy - resolution / 2; - costmap_raw_.metadata.origin.position.z = 0.0; - costmap_raw_.metadata.origin.orientation.w = 1.0; + costmap_raw_->metadata.origin.position.x = wx - resolution / 2; + costmap_raw_->metadata.origin.position.y = wy - resolution / 2; + costmap_raw_->metadata.origin.position.z = 0.0; + costmap_raw_->metadata.origin.orientation.w = 1.0; - costmap_raw_.data.resize(costmap_raw_.metadata.size_x * costmap_raw_.metadata.size_y); + costmap_raw_->data.resize(costmap_raw_->metadata.size_x * costmap_raw_->metadata.size_y); unsigned char * data = costmap_->getCharMap(); - for (unsigned int i = 0; i < costmap_raw_.data.size(); i++) { - costmap_raw_.data[i] = data[i]; + for (unsigned int i = 0; i < costmap_raw_->data.size(); i++) { + costmap_raw_->data[i] = data[i]; } + + return std::move(costmap_raw_); } void Costmap2DPublisher::publishCostmap() { if (node_->count_subscribers(costmap_raw_pub_->get_topic_name()) > 0) { - prepareCostmap(); + auto costmap_raw_ = prepareCostmap(); - auto costmap_raw_ptr = std::make_unique(std::move(costmap_raw_)); - costmap_raw_pub_->publish(std::move(costmap_raw_ptr)); + costmap_raw_pub_->publish(std::move(costmap_raw_)); } float resolution = costmap_->getResolution(); - if (always_send_full_costmap_ || grid_.info.resolution != resolution || - grid_.info.width != costmap_->getSizeInCellsX() || - grid_.info.height != costmap_->getSizeInCellsY() || + if (always_send_full_costmap_ || grid_resolution != resolution || + grid_width != costmap_->getSizeInCellsX() || + grid_height != costmap_->getSizeInCellsY() || saved_origin_x_ != costmap_->getOriginX() || saved_origin_y_ != costmap_->getOriginY()) { if (node_->count_subscribers(costmap_pub_->get_topic_name()) > 0) { - prepareGrid(); - auto grid_ptr = std::make_unique(std::move(grid_)); - costmap_pub_->publish(std::move(grid_ptr)); + auto grid_ = prepareGrid(); + costmap_pub_->publish(std::move(grid_)); } } else if (x0_ < xn_) { if (node_->count_subscribers(costmap_update_pub_->get_topic_name()) > 0) { From 0c25c4651e55403b12f79f26b4b96f05904b6334 Mon Sep 17 00:00:00 2001 From: Sarathkrishnan Ramesh Date: Thu, 7 May 2020 17:20:25 +0530 Subject: [PATCH 5/6] Update publishers in nav2_planner and nav2_map_server * Change nav2_map_server publisher to publish occupancy grid unique ptr * Change publisher in nav2_planner to publish path unique ptr * Remove smart pointer return from functions in nav2_costmap_2d * Run cpp_lint manually in nav2_costmap_2d --- .../nav2_costmap_2d/costmap_2d_publisher.hpp | 6 +++-- nav2_costmap_2d/plugins/voxel_layer.cpp | 2 ++ nav2_costmap_2d/src/costmap_2d_cloud.cpp | 2 ++ nav2_costmap_2d/src/costmap_2d_markers.cpp | 2 ++ nav2_costmap_2d/src/costmap_2d_publisher.cpp | 18 ++++++--------- nav2_costmap_2d/src/costmap_2d_ros.cpp | 1 + .../include/nav2_map_server/map_server.hpp | 2 +- nav2_map_server/src/map_server/map_server.cpp | 22 +++++++++---------- nav2_planner/src/planner_server.cpp | 4 +++- 9 files changed, 33 insertions(+), 26 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp index 9d893dbada..83349d4470 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp @@ -118,8 +118,8 @@ class Costmap2DPublisher private: /** @brief Prepare grid_ message for publication. */ - std::unique_ptr prepareGrid(); - std::unique_ptr prepareCostmap(); + void prepareGrid(); + void prepareCostmap(); /** @brief Publish the latest full costmap to the new subscriber. */ // void onNewSubscription(const ros::SingleSubscriberPublisher& pub); @@ -153,6 +153,8 @@ class Costmap2DPublisher float grid_resolution; unsigned int grid_width, grid_height; + std::unique_ptr grid_; + std::unique_ptr costmap_raw_; // Translate from 0-255 values in costmap to -1 to 100 values in message. static char * cost_translation_table_; }; diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp index 819075a3e7..5235e23675 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/nav2_costmap_2d/plugins/voxel_layer.cpp @@ -41,6 +41,8 @@ #include #include #include +#include +#include #include "pluginlib/class_list_macros.hpp" #include "sensor_msgs/point_cloud2_iterator.hpp" diff --git a/nav2_costmap_2d/src/costmap_2d_cloud.cpp b/nav2_costmap_2d/src/costmap_2d_cloud.cpp index 2ab4a28de7..52157b3b7a 100644 --- a/nav2_costmap_2d/src/costmap_2d_cloud.cpp +++ b/nav2_costmap_2d/src/costmap_2d_cloud.cpp @@ -27,6 +27,8 @@ #include #include +#include +#include #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/point_cloud.hpp" diff --git a/nav2_costmap_2d/src/costmap_2d_markers.cpp b/nav2_costmap_2d/src/costmap_2d_markers.cpp index 0e7b5b5586..281150bbf2 100644 --- a/nav2_costmap_2d/src/costmap_2d_markers.cpp +++ b/nav2_costmap_2d/src/costmap_2d_markers.cpp @@ -38,6 +38,8 @@ *********************************************************************/ #include #include +#include +#include #include "rclcpp/rclcpp.hpp" #include "visualization_msgs/msg/marker.hpp" diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp index c1172102e4..56797e566b 100644 --- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp @@ -40,6 +40,7 @@ #include #include +#include #include "nav2_costmap_2d/cost_values.hpp" @@ -107,14 +108,14 @@ void Costmap2DPublisher::onNewSubscription(const ros::SingleSubscriberPublisher& } */ // prepare grid_ message for publication. -std::unique_ptr Costmap2DPublisher::prepareGrid() +void Costmap2DPublisher::prepareGrid() { std::unique_lock lock(*(costmap_->getMutex())); grid_resolution = costmap_->getResolution(); grid_width = costmap_->getSizeInCellsX(); grid_height = costmap_->getSizeInCellsY(); - auto grid_ = std::make_unique(); + grid_ = std::make_unique(); grid_->header.frame_id = global_frame_; grid_->header.stamp = rclcpp::Time(); @@ -139,16 +140,14 @@ std::unique_ptr Costmap2DPublisher::prepareGrid() for (unsigned int i = 0; i < grid_->data.size(); i++) { grid_->data[i] = cost_translation_table_[data[i]]; } - - return std::move(grid_); } -std::unique_ptr Costmap2DPublisher::prepareCostmap() +void Costmap2DPublisher::prepareCostmap() { std::unique_lock lock(*(costmap_->getMutex())); double resolution = costmap_->getResolution(); - auto costmap_raw_ = std::make_unique(); + costmap_raw_ = std::make_unique(); costmap_raw_->header.frame_id = global_frame_; costmap_raw_->header.stamp = node_->now(); @@ -172,15 +171,12 @@ std::unique_ptr Costmap2DPublisher::prepareCostmap() for (unsigned int i = 0; i < costmap_raw_->data.size(); i++) { costmap_raw_->data[i] = data[i]; } - - return std::move(costmap_raw_); } void Costmap2DPublisher::publishCostmap() { if (node_->count_subscribers(costmap_raw_pub_->get_topic_name()) > 0) { - auto costmap_raw_ = prepareCostmap(); - + prepareCostmap(); costmap_raw_pub_->publish(std::move(costmap_raw_)); } float resolution = costmap_->getResolution(); @@ -192,7 +188,7 @@ void Costmap2DPublisher::publishCostmap() saved_origin_y_ != costmap_->getOriginY()) { if (node_->count_subscribers(costmap_pub_->get_topic_name()) > 0) { - auto grid_ = prepareGrid(); + prepareGrid(); costmap_pub_->publish(std::move(grid_)); } } else if (x0_ < xn_) { diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 376bf1d9c7..b9aaf8f4f7 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -41,6 +41,7 @@ #include #include #include +#include #include "nav2_costmap_2d/layered_costmap.hpp" #include "nav2_util/execution_timer.hpp" diff --git a/nav2_map_server/include/nav2_map_server/map_server.hpp b/nav2_map_server/include/nav2_map_server/map_server.hpp index a184a2dc39..d3ace49e90 100644 --- a/nav2_map_server/include/nav2_map_server/map_server.hpp +++ b/nav2_map_server/include/nav2_map_server/map_server.hpp @@ -142,7 +142,7 @@ class MapServer : public nav2_util::LifecycleNode std::string frame_id_; // The message to publish on the occupancy grid topic - std::unique_ptr msg_; + nav_msgs::msg::OccupancyGrid msg_; }; } // namespace nav2_map_server diff --git a/nav2_map_server/src/map_server/map_server.cpp b/nav2_map_server/src/map_server/map_server.cpp index 660a2e604e..2c2e174f9c 100644 --- a/nav2_map_server/src/map_server/map_server.cpp +++ b/nav2_map_server/src/map_server/map_server.cpp @@ -50,6 +50,7 @@ #include #include #include +#include #include "yaml-cpp/yaml.h" #include "lifecycle_msgs/msg/state.hpp" @@ -88,8 +89,6 @@ MapServer::on_configure(const rclcpp_lifecycle::State & /*state*/) std::string topic_name = get_parameter("topic_name").as_string(); frame_id_ = get_parameter("frame_id").as_string(); - // initialize Occupancy Grid msg - needed by loadMapResponseFromYaml() - msg_ = std::make_unique(); // Shared pointer to LoadMap::Response is also should be initialized // in order to avoid null-pointer dereference std::shared_ptr rsp = @@ -127,7 +126,8 @@ MapServer::on_activate(const rclcpp_lifecycle::State & /*state*/) // Publish the map using the latched topic occ_pub_->on_activate(); - occ_pub_->publish(*msg_); + auto occ_grid = std::make_unique(msg_); + occ_pub_->publish(std::move(occ_grid)); return nav2_util::CallbackReturn::SUCCESS; } @@ -150,7 +150,6 @@ MapServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) occ_pub_.reset(); occ_service_.reset(); load_map_service_.reset(); - msg_.reset(); return nav2_util::CallbackReturn::SUCCESS; } @@ -182,7 +181,7 @@ void MapServer::getMapCallback( return; } RCLCPP_INFO(get_logger(), "Handling GetMap request"); - response->map = *msg_; + response->map = msg_; } void MapServer::loadMapCallback( @@ -200,7 +199,8 @@ void MapServer::loadMapCallback( RCLCPP_INFO(get_logger(), "Handling LoadMap request"); // Load from file if (loadMapResponseFromYaml(request->map_url, response)) { - occ_pub_->publish(*msg_); // publish new map + auto occ_grid = std::make_unique(msg_); + occ_pub_->publish(std::move(occ_grid)); // publish new map } } @@ -208,7 +208,7 @@ bool MapServer::loadMapResponseFromYaml( const std::string & yaml_file, std::shared_ptr response) { - switch (loadMapFromYaml(yaml_file, *msg_)) { + switch (loadMapFromYaml(yaml_file, msg_)) { case MAP_DOES_NOT_EXIST: response->result = nav2_msgs::srv::LoadMap::Response::RESULT_MAP_DOES_NOT_EXIST; return false; @@ -222,7 +222,7 @@ bool MapServer::loadMapResponseFromYaml( // Correcting msg_ header when it belongs to spiecific node updateMsgHeader(); - response->map = *msg_; + response->map = msg_; response->result = nav2_msgs::srv::LoadMap::Response::RESULT_SUCCESS; } @@ -231,9 +231,9 @@ bool MapServer::loadMapResponseFromYaml( void MapServer::updateMsgHeader() { - msg_->info.map_load_time = now(); - msg_->header.frame_id = frame_id_; - msg_->header.stamp = now(); + msg_.info.map_load_time = now(); + msg_.header.frame_id = frame_id_; + msg_.header.stamp = now(); } } // namespace nav2_map_server diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index b24e34f1c1..97d33b33fe 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include "builtin_interfaces/msg/duration.hpp" #include "nav2_util/costmap.hpp" @@ -294,7 +295,8 @@ PlannerServer::computePlan() void PlannerServer::publishPlan(const nav_msgs::msg::Path & path) { - plan_publisher_->publish(path); + auto msg = std::make_unique(path); + plan_publisher_->publish(std::move(msg)); } } // namespace nav2_planner From 628bf78e75e733433c6500aa8ccea2574d65eb16 Mon Sep 17 00:00:00 2001 From: Sarathkrishnan Ramesh Date: Thu, 7 May 2020 18:59:48 +0530 Subject: [PATCH 6/6] Minor fixes * Adhere to conventions of smart pointers passing to function * Change publisher in dwb_core to publish unique pointer --- nav2_dwb_controller/dwb_core/src/publisher.cpp | 3 ++- nav2_recoveries/plugins/back_up.cpp | 4 ++-- nav2_recoveries/plugins/back_up.hpp | 2 +- nav2_recoveries/plugins/spin.cpp | 4 ++-- nav2_recoveries/plugins/spin.hpp | 2 +- 5 files changed, 8 insertions(+), 7 deletions(-) diff --git a/nav2_dwb_controller/dwb_core/src/publisher.cpp b/nav2_dwb_controller/dwb_core/src/publisher.cpp index bc6c5fea0c..7f7ceb62c3 100644 --- a/nav2_dwb_controller/dwb_core/src/publisher.cpp +++ b/nav2_dwb_controller/dwb_core/src/publisher.cpp @@ -152,7 +152,8 @@ DWBPublisher::publishEvaluation(std::shared_ptrpublish(*results); + auto msg = std::make_unique(*results); + eval_pub_->publish(std::move(msg)); } publishTrajectories(*results); diff --git a/nav2_recoveries/plugins/back_up.cpp b/nav2_recoveries/plugins/back_up.cpp index bf25666dec..9706e6e82b 100644 --- a/nav2_recoveries/plugins/back_up.cpp +++ b/nav2_recoveries/plugins/back_up.cpp @@ -88,7 +88,7 @@ Status BackUp::onCycleUpdate() pose2d.y = current_pose.pose.position.y; pose2d.theta = tf2::getYaw(current_pose.pose.orientation); - if (!isCollisionFree(distance, cmd_vel, pose2d)) { + if (!isCollisionFree(distance, cmd_vel.get(), pose2d)) { stopRobot(); RCLCPP_WARN(node_->get_logger(), "Collision Ahead - Exiting BackUp"); return Status::SUCCEEDED; @@ -101,7 +101,7 @@ Status BackUp::onCycleUpdate() bool BackUp::isCollisionFree( const double & distance, - std::unique_ptr & cmd_vel, + geometry_msgs::msg::Twist * cmd_vel, geometry_msgs::msg::Pose2D & pose2d) { // Simulate ahead by simulate_ahead_time_ in cycle_frequency_ increments diff --git a/nav2_recoveries/plugins/back_up.hpp b/nav2_recoveries/plugins/back_up.hpp index 286410a7dc..4f04bd0352 100644 --- a/nav2_recoveries/plugins/back_up.hpp +++ b/nav2_recoveries/plugins/back_up.hpp @@ -38,7 +38,7 @@ class BackUp : public Recovery protected: bool isCollisionFree( const double & distance, - std::unique_ptr & cmd_vel, + geometry_msgs::msg::Twist * cmd_vel, geometry_msgs::msg::Pose2D & pose2d); void onConfigure() override; diff --git a/nav2_recoveries/plugins/spin.cpp b/nav2_recoveries/plugins/spin.cpp index 6c8a4093c6..dcbf045a13 100644 --- a/nav2_recoveries/plugins/spin.cpp +++ b/nav2_recoveries/plugins/spin.cpp @@ -115,7 +115,7 @@ Status Spin::onCycleUpdate() pose2d.y = current_pose.pose.position.y; pose2d.theta = tf2::getYaw(current_pose.pose.orientation); - if (!isCollisionFree(relative_yaw, cmd_vel, pose2d)) { + if (!isCollisionFree(relative_yaw, cmd_vel.get(), pose2d)) { stopRobot(); RCLCPP_WARN(node_->get_logger(), "Collision Ahead - Exiting Spin"); return Status::SUCCEEDED; @@ -128,7 +128,7 @@ Status Spin::onCycleUpdate() bool Spin::isCollisionFree( const double & relative_yaw, - std::unique_ptr & cmd_vel, + geometry_msgs::msg::Twist * cmd_vel, geometry_msgs::msg::Pose2D & pose2d) { // Simulate ahead by simulate_ahead_time_ in cycle_frequency_ increments diff --git a/nav2_recoveries/plugins/spin.hpp b/nav2_recoveries/plugins/spin.hpp index 342d8e1dfd..0a74c72134 100644 --- a/nav2_recoveries/plugins/spin.hpp +++ b/nav2_recoveries/plugins/spin.hpp @@ -40,7 +40,7 @@ class Spin : public Recovery protected: bool isCollisionFree( const double & distance, - std::unique_ptr & cmd_vel, + geometry_msgs::msg::Twist * cmd_vel, geometry_msgs::msg::Pose2D & pose2d); double min_rotational_vel_;