Skip to content

Commit

Permalink
Refactor laser_callback method (#89)
Browse files Browse the repository at this point in the history 
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 <[email protected]>
  • Loading branch information
@nahueespinosa
nahueespinosa authored Jan 27, 2023
1 parent 4cbcda4 commit 6a7c166
Show file tree
Hide file tree
Showing 4 changed files with 76 additions and 73 deletions.
3 changes: 0 additions & 3 deletions beluga/include/beluga/motion/differential_drive_model.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down
2 changes: 2 additions & 0 deletions beluga_amcl/include/beluga_amcl/amcl_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -82,6 +82,8 @@ class AmclNode : public rclcpp_lifecycle::LifecycleNode
std::unique_ptr<message_filters::Subscriber<sensor_msgs::msg::LaserScan,
rclcpp_lifecycle::LifecycleNode>> laser_scan_sub_;
message_filters::Connection laser_scan_connection_;

Sophus::SE2d last_odom_to_base_transform_;
};

} // namespace beluga_amcl
Expand Down
142 changes: 73 additions & 69 deletions beluga_amcl/src/amcl_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<nav2_msgs::msg::ParticleCloud>(
"particle_cloud",
Expand Down Expand Up @@ -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();
Expand All @@ -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)
Expand All @@ -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<float>(get_parameter("laser_min_range").as_double()),
static_cast<float>(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<float>(get_parameter("laser_min_range").as_double()),
static_cast<float>(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<double, std::milli>(time2 - time1).count(),
std::chrono::duration<double, std::milli>(time3 - time2).count(),
std::chrono::duration<double, std::milli>(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);
}
}

Expand Down
2 changes: 1 addition & 1 deletion beluga_example/rviz/rviz.rviz
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down

0 comments on commit 6a7c166

Please sign in to comment.