From 6a7c1660b96faf8086daf92ac2872dcea703b911 Mon Sep 17 00:00:00 2001 From: Nahuel Espinosa Date: Fri, 27 Jan 2023 13:30:44 -0300 Subject: [PATCH] Refactor laser_callback method (#89) This is directly related to #57, but it should make #40 much easier to implement. It is a small refactor of the `laser_callback` method in `amcl_node.cpp`. The functional changes are: - We now get different errors depending on which transform lookup failed. - We update the sensor and motion models with every laser scan message, if the robot has not moved we only skip the resampling step. - We publish a new estimate after every update of the filter. - Removes `last_odom()` method from the differential drive model interface. Signed-off-by: Nahuel Espinosa --- .../motion/differential_drive_model.hpp | 3 - beluga_amcl/include/beluga_amcl/amcl_node.hpp | 2 + beluga_amcl/src/amcl_node.cpp | 142 +++++++++--------- beluga_example/rviz/rviz.rviz | 2 +- 4 files changed, 76 insertions(+), 73 deletions(-) diff --git a/beluga/include/beluga/motion/differential_drive_model.hpp b/beluga/include/beluga/motion/differential_drive_model.hpp index 9b6c9b4f2..92c222770 100644 --- a/beluga/include/beluga/motion/differential_drive_model.hpp +++ b/beluga/include/beluga/motion/differential_drive_model.hpp @@ -104,9 +104,6 @@ class DifferentialDriveModel : public Mixin { return state * Sophus::SE2d{first_rotation, Eigen::Vector2d{0.0, 0.0}} * Sophus::SE2d{second_rotation, translation}; } - /// Gets the last update. - Sophus::SE2d last_pose() const { return last_pose_ ? last_pose_.value() : Sophus::SE2d{}; } - /// Updates the motion model. /** * This will not update particles. diff --git a/beluga_amcl/include/beluga_amcl/amcl_node.hpp b/beluga_amcl/include/beluga_amcl/amcl_node.hpp index 829331638..d0cbd8007 100644 --- a/beluga_amcl/include/beluga_amcl/amcl_node.hpp +++ b/beluga_amcl/include/beluga_amcl/amcl_node.hpp @@ -82,6 +82,8 @@ class AmclNode : public rclcpp_lifecycle::LifecycleNode std::unique_ptr> laser_scan_sub_; message_filters::Connection laser_scan_connection_; + + Sophus::SE2d last_odom_to_base_transform_; }; } // namespace beluga_amcl diff --git a/beluga_amcl/src/amcl_node.cpp b/beluga_amcl/src/amcl_node.cpp index 3e4c0e372..5042007c1 100644 --- a/beluga_amcl/src/amcl_node.cpp +++ b/beluga_amcl/src/amcl_node.cpp @@ -366,8 +366,9 @@ AmclNode::CallbackReturn AmclNode::on_configure(const rclcpp_lifecycle::State &) { RCLCPP_INFO(get_logger(), "Configuring"); + // TODO(nahuel): Add a parameter for the timer period. using namespace std::chrono_literals; - timer_ = create_wall_timer(500ms, std::bind(&AmclNode::timer_callback, this)); + timer_ = create_wall_timer(200ms, std::bind(&AmclNode::timer_callback, this)); particle_cloud_pub_ = create_publisher( "particle_cloud", @@ -536,6 +537,8 @@ void AmclNode::timer_callback() return; } + // TODO(nahuel): Throttle the particle cloud publishing in the + // laser_callback method instead of using a ROS timer. { auto message = nav2_msgs::msg::ParticleCloud{}; message.header.stamp = now(); @@ -550,28 +553,6 @@ void AmclNode::timer_callback() }); particle_cloud_pub_->publish(message); } - - { - const auto [pose, covariance] = particle_filter_->estimated_pose(); - auto message = geometry_msgs::msg::PoseWithCovarianceStamped{}; - message.header.stamp = now(); - message.header.frame_id = get_parameter("global_frame_id").as_string(); - tf2::toMsg(pose, message.pose.pose); - message.pose.covariance = tf2::covarianceEigenToRowMajor(covariance); - pose_pub_->publish(message); - } - - { - // TODO(nahuel): Publish estimated map to odom transform. - auto message = geometry_msgs::msg::TransformStamped{}; - // Sending a transform that is valid into the future so that odom can be used. - message.header.stamp = now() + - tf2::durationFromSec(get_parameter("transform_tolerance").as_double()); - message.header.frame_id = get_parameter("global_frame_id").as_string(); - message.child_frame_id = get_parameter("odom_frame_id").as_string(); - message.transform = tf2::toMsg(Sophus::SE2d{}); - tf_broadcaster_->sendTransform(message); - } } void AmclNode::laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan) @@ -580,71 +561,94 @@ void AmclNode::laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_ return; } + auto odom_to_base_transform = Sophus::SE2d{}; try { + tf2::convert( + tf_buffer_->lookupTransform( + get_parameter("odom_frame_id").as_string(), + get_parameter("base_frame_id").as_string(), + laser_scan->header.stamp, + std::chrono::seconds(1)).transform, + odom_to_base_transform); + } catch (const tf2::TransformException & error) { + RCLCPP_ERROR(get_logger(), "Could not transform from odom to base: %s", error.what()); + return; + } + + auto base_to_laser_transform = Sophus::SE3d{}; + try { + tf2::convert( + tf_buffer_->lookupTransform( + get_parameter("base_frame_id").as_string(), + laser_scan->header.frame_id, + laser_scan->header.stamp, + std::chrono::seconds(1)).transform, + base_to_laser_transform); + } catch (const tf2::TransformException & error) { + RCLCPP_ERROR(get_logger(), "Could not transform from base to laser: %s", error.what()); + return; + } + + { + const auto time1 = std::chrono::high_resolution_clock::now(); + particle_filter_->update_motion(odom_to_base_transform); + particle_filter_->sample(); + const auto time2 = std::chrono::high_resolution_clock::now(); + particle_filter_->update_sensor( + pre_process_points( + *laser_scan, + base_to_laser_transform, + static_cast(get_parameter("laser_min_range").as_double()), + static_cast(get_parameter("laser_max_range").as_double()))); + particle_filter_->importance_sample(); + const auto time3 = std::chrono::high_resolution_clock::now(); { - auto odom_to_base_transform = Sophus::SE2d{}; - tf2::convert( - tf_buffer_->lookupTransform( - get_parameter("odom_frame_id").as_string(), - get_parameter("base_frame_id").as_string(), - laser_scan->header.stamp, - std::chrono::seconds(1)).transform, - odom_to_base_transform); - - const auto delta = odom_to_base_transform * particle_filter_->last_pose().inverse(); - const bool has_moved = + const auto delta = odom_to_base_transform * last_odom_to_base_transform_.inverse(); + const bool has_moved_since_last_resample = std::abs(delta.translation().x()) > get_parameter("update_min_d").as_double() || std::abs(delta.translation().y()) > get_parameter("update_min_d").as_double() || std::abs(delta.so2().log()) > get_parameter("update_min_a").as_double(); - - if (!has_moved) { + if (has_moved_since_last_resample) { // To avoid loss of diversity in the particle population, don't // resample when the state is known to be static. // See 'Probabilistic Robotics, Chapter 4.2.4'. - RCLCPP_INFO_THROTTLE( - get_logger(), *get_clock(), 2000, - "Skipping the particle filter update as the robot is not moving"); - return; + particle_filter_->resample(); + last_odom_to_base_transform_ = odom_to_base_transform; } - particle_filter_->update_motion(odom_to_base_transform); - } - - { - auto base_to_laser_transform = Sophus::SE3d{}; - tf2::convert( - tf_buffer_->lookupTransform( - get_parameter("base_frame_id").as_string(), - laser_scan->header.frame_id, - laser_scan->header.stamp, - std::chrono::seconds(1)).transform, - base_to_laser_transform); - - particle_filter_->update_sensor( - pre_process_points( - *laser_scan, - base_to_laser_transform, - static_cast(get_parameter("laser_min_range").as_double()), - static_cast(get_parameter("laser_max_range").as_double()))); } - - const auto time1 = std::chrono::high_resolution_clock::now(); - particle_filter_->sample(); - const auto time2 = std::chrono::high_resolution_clock::now(); - particle_filter_->importance_sample(); - const auto time3 = std::chrono::high_resolution_clock::now(); - particle_filter_->resample(); const auto time4 = std::chrono::high_resolution_clock::now(); RCLCPP_INFO_THROTTLE( get_logger(), *get_clock(), 2000, - "Filter update statistics: %ld particles %ld points - %.3fms %.3fms %.3fms", + "Particle fitler update stats: %ld particles %ld points - %.3fms %.3fms %.3fms", particle_filter_->particles().size(), laser_scan->ranges.size(), std::chrono::duration(time2 - time1).count(), std::chrono::duration(time3 - time2).count(), std::chrono::duration(time4 - time3).count()); - } catch (const tf2::TransformException & error) { - RCLCPP_ERROR(get_logger(), "Could not transform laser: %s", error.what()); + } + + const auto [pose, covariance] = particle_filter_->estimated_pose(); + + { + auto message = geometry_msgs::msg::PoseWithCovarianceStamped{}; + message.header.stamp = now(); + message.header.frame_id = get_parameter("global_frame_id").as_string(); + tf2::toMsg(pose, message.pose.pose); + message.pose.covariance = tf2::covarianceEigenToRowMajor(covariance); + pose_pub_->publish(message); + } + + { + // TODO(nahuel): Publish estimated map to odom transform. + auto message = geometry_msgs::msg::TransformStamped{}; + // Sending a transform that is valid into the future so that odom can be used. + message.header.stamp = now() + + tf2::durationFromSec(get_parameter("transform_tolerance").as_double()); + message.header.frame_id = get_parameter("global_frame_id").as_string(); + message.child_frame_id = get_parameter("odom_frame_id").as_string(); + message.transform = tf2::toMsg(Sophus::SE2d{}); + tf_broadcaster_->sendTransform(message); } } diff --git a/beluga_example/rviz/rviz.rviz b/beluga_example/rviz/rviz.rviz index c9d5cbeb4..2f2eb8a37 100644 --- a/beluga_example/rviz/rviz.rviz +++ b/beluga_example/rviz/rviz.rviz @@ -58,7 +58,7 @@ Visualization Manager: Class: nav2_rviz_plugins/ParticleCloud Color: 255; 25; 0 Enabled: true - Max Arrow Length: 0.10000000149011612 + Max Arrow Length: 0.05000000074505806 Min Arrow Length: 0.05000000074505806 Name: ParticleCloud Shape: Arrow (3D)