diff --git a/nav2_amcl/CMakeLists.txt b/nav2_amcl/CMakeLists.txt index 8af943d5cc..916e524088 100644 --- a/nav2_amcl/CMakeLists.txt +++ b/nav2_amcl/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(std_srvs REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2 REQUIRED) find_package(nav2_util REQUIRED) +find_package(nav2_msgs REQUIRED) nav2_package() @@ -50,6 +51,7 @@ set(dependencies tf2_ros tf2 nav2_util + nav2_msgs ) ament_target_dependencies(${executable_name} diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index 763a601ed8..669ea98835 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -34,6 +34,8 @@ #include "nav2_util/lifecycle_node.hpp" #include "nav2_amcl/motion_model/motion_model.hpp" #include "nav2_amcl/sensors/laser/laser.hpp" +#include "nav2_msgs/msg/particle.hpp" +#include "nav2_msgs/msg/particle_cloud.hpp" #include "nav_msgs/srv/set_map.hpp" #include "sensor_msgs/msg/laser_scan.hpp" #include "std_srvs/srv/empty.hpp" @@ -117,6 +119,8 @@ class AmclNode : public nav2_util::LifecycleNode rclcpp_lifecycle::LifecyclePublisher::SharedPtr pose_pub_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr particlecloud_pub_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + particle_cloud_pub_; void initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); void laserReceived(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan); diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index c339e8dbc6..f81dec37ca 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -32,6 +32,7 @@ tf2_ros tf2 nav2_util + nav2_msgs launch_ros launch_testing diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 53de6a7d31..294e0b37af 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -266,6 +266,12 @@ AmclNode::on_activate(const rclcpp_lifecycle::State & /*state*/) // Lifecycle publishers must be explicitly activated pose_pub_->on_activate(); particlecloud_pub_->on_activate(); + particle_cloud_pub_->on_activate(); + + RCLCPP_WARN( + get_logger(), + "Publishing the particle cloud as geometry_msgs/PoseArray msg is deprecated, " + "will be published as nav2_msgs/ParticleCloud in the future"); first_pose_sent_ = false; @@ -301,6 +307,7 @@ AmclNode::on_deactivate(const rclcpp_lifecycle::State & /*state*/) // Lifecycle publishers must be explicitly deactivated pose_pub_->on_deactivate(); particlecloud_pub_->on_deactivate(); + particle_cloud_pub_->on_deactivate(); return nav2_util::CallbackReturn::SUCCESS; } @@ -333,6 +340,7 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/) // PubSub pose_pub_.reset(); particlecloud_pub_.reset(); + particle_cloud_pub_.reset(); // Odometry motion_model_.reset(); @@ -852,17 +860,27 @@ 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;} + + nav2_msgs::msg::ParticleCloud cloud_with_weights_msg; + cloud_with_weights_msg.header.stamp = this->now(); + cloud_with_weights_msg.header.frame_id = global_frame_id_; + cloud_with_weights_msg.particles.resize(set->sample_count); + 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); + 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_with_weights_msg.particles[i].pose = cloud_msg.poses[i]; + cloud_with_weights_msg.particles[i].weight = set->samples[i].weight; } particlecloud_pub_->publish(cloud_msg); + particle_cloud_pub_->publish(cloud_with_weights_msg); } bool @@ -1242,6 +1260,10 @@ AmclNode::initPubSub() "particlecloud", rclcpp::SensorDataQoS()); + particle_cloud_pub_ = create_publisher( + "particle_cloud", + rclcpp::SensorDataQoS()); + pose_pub_ = create_publisher( "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 5f38a9b790..f324715b41 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -18,6 +18,8 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/VoxelGrid.msg" "msg/BehaviorTreeStatusChange.msg" "msg/BehaviorTreeLog.msg" + "msg/Particle.msg" + "msg/ParticleCloud.msg" "srv/GetCostmap.srv" "srv/ClearCostmapExceptRegion.srv" "srv/ClearCostmapAroundRobot.srv" diff --git a/nav2_msgs/msg/Particle.msg b/nav2_msgs/msg/Particle.msg new file mode 100644 index 0000000000..294bbd170d --- /dev/null +++ b/nav2_msgs/msg/Particle.msg @@ -0,0 +1,5 @@ +# This represents an individual particle with weight produced by a particle filter + +geometry_msgs/Pose pose + +float64 weight diff --git a/nav2_msgs/msg/ParticleCloud.msg b/nav2_msgs/msg/ParticleCloud.msg new file mode 100644 index 0000000000..17b07ac707 --- /dev/null +++ b/nav2_msgs/msg/ParticleCloud.msg @@ -0,0 +1,6 @@ +# This represents a particle cloud containing particle poses and weights + +std_msgs/Header header + +# Array of particles in the cloud +Particle[] particles