diff --git a/beluga/include/beluga/motion/differential_drive_model.hpp b/beluga/include/beluga/motion/differential_drive_model.hpp index a7bdc58b2..356810d42 100644 --- a/beluga/include/beluga/motion/differential_drive_model.hpp +++ b/beluga/include/beluga/motion/differential_drive_model.hpp @@ -52,6 +52,8 @@ class DifferentialDriveModel : public Mixin { return state * Sophus::SE2d{first_rotation, Eigen::Vector2d{0.0, 0.0}} * Sophus::SE2d{second_rotation, translation}; } + Sophus::SE2d last_pose() const { return last_pose_ ? last_pose_.value() : Sophus::SE2d{}; } + void update_motion(const Sophus::SE2d& pose) { if (last_pose_) { const auto translation = pose.translation() - last_pose_.value().translation(); diff --git a/beluga_amcl/include/beluga_amcl/amcl_node.hpp b/beluga_amcl/include/beluga_amcl/amcl_node.hpp index 92101fead..7362e6ea5 100644 --- a/beluga_amcl/include/beluga_amcl/amcl_node.hpp +++ b/beluga_amcl/include/beluga_amcl/amcl_node.hpp @@ -23,9 +23,9 @@ #include -#include -#include #include +#include +#include #include #include #include @@ -42,12 +42,10 @@ class AmclNode : public rclcpp_lifecycle::LifecycleNode public: template using SensorModel = typename beluga::LikelihoodFieldModel; + using ParticleFilter = beluga::AMCL; using rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - // TODO(nahuel): Use a real motion model. - using ParticleFilter = beluga::AMCL; - explicit AmclNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); virtual ~AmclNode(); diff --git a/beluga_amcl/include/beluga_amcl/convert.hpp b/beluga_amcl/include/beluga_amcl/convert.hpp index 734daedd4..001f3eb65 100644 --- a/beluga_amcl/include/beluga_amcl/convert.hpp +++ b/beluga_amcl/include/beluga_amcl/convert.hpp @@ -15,7 +15,10 @@ #ifndef BELUGA_AMCL__CONVERT_HPP_ #define BELUGA_AMCL__CONVERT_HPP_ +#include + #include +#include #include namespace Sophus @@ -36,6 +39,13 @@ geometry_msgs::msg::Pose toMsg(const Sophus::SE2 & pose) return message; } +template +void fromMsg(const geometry_msgs::msg::Transform & message, Sophus::SE2 & transform) +{ + transform.translation() = Eigen::Vector2d{message.translation.x, message.translation.y}; + transform.so2() = Sophus::SO2{tf2::getYaw(message.rotation)}; +} + } // namespace Sophus #endif // BELUGA_AMCL__CONVERT_HPP_ diff --git a/beluga_amcl/src/amcl_node.cpp b/beluga_amcl/src/amcl_node.cpp index 0b901670c..7a5f44b49 100644 --- a/beluga_amcl/src/amcl_node.cpp +++ b/beluga_amcl/src/amcl_node.cpp @@ -159,6 +159,50 @@ AmclNode::AmclNode(const rclcpp::NodeOptions & options) declare_parameter("resample_interval", rclcpp::ParameterValue(1), descriptor); } + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = + "Rotation noise from rotation for the differential drive model."; + descriptor.floating_point_range.resize(1); + descriptor.floating_point_range[0].from_value = 0; + descriptor.floating_point_range[0].to_value = std::numeric_limits::max(); + descriptor.floating_point_range[0].step = 0; + declare_parameter("alpha1", rclcpp::ParameterValue(0.2), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = + "Rotation noise from translation for the differential drive model."; + descriptor.floating_point_range.resize(1); + descriptor.floating_point_range[0].from_value = 0; + descriptor.floating_point_range[0].to_value = std::numeric_limits::max(); + descriptor.floating_point_range[0].step = 0; + declare_parameter("alpha2", rclcpp::ParameterValue(0.2), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = + "Translation noise from translation for the differential drive model."; + descriptor.floating_point_range.resize(1); + descriptor.floating_point_range[0].from_value = 0; + descriptor.floating_point_range[0].to_value = std::numeric_limits::max(); + descriptor.floating_point_range[0].step = 0; + declare_parameter("alpha3", rclcpp::ParameterValue(0.2), descriptor); + } + + { + auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); + descriptor.description = + "Translation noise from rotation for the differential drive model."; + descriptor.floating_point_range.resize(1); + descriptor.floating_point_range[0].from_value = 0; + descriptor.floating_point_range[0].to_value = std::numeric_limits::max(); + descriptor.floating_point_range[0].step = 0; + declare_parameter("alpha4", rclcpp::ParameterValue(0.2), descriptor); + } + { auto descriptor = rcl_interfaces::msg::ParameterDescriptor(); descriptor.description = "Rotational movement required before performing a filter update."; @@ -308,8 +352,10 @@ AmclNode::CallbackReturn AmclNode::on_activate(const rclcpp_lifecycle::State &) rclcpp_lifecycle::LifecycleNode>>( shared_from_this(), get_parameter("scan_topic").as_string(), rmw_qos_profile_sensor_data); + // Message filter that caches laser scan readings until it is possible to transform + // from laser frame to odom frame and update the particle filter. laser_scan_filter_ = std::make_unique>( - *laser_scan_sub_, *tf_buffer_, get_parameter("base_frame_id").as_string(), 50, + *laser_scan_sub_, *tf_buffer_, get_parameter("odom_frame_id").as_string(), 50, get_node_logging_interface(), get_node_clock_interface(), tf2::durationFromSec(1.0)); @@ -373,9 +419,16 @@ void AmclNode::map_callback(nav_msgs::msg::OccupancyGrid::SharedPtr map) get_parameter("z_rand").as_double(), get_parameter("sigma_hit").as_double()}; + const auto differential_drive_model_params = beluga::DifferentialDriveModelParam{ + get_parameter("alpha1").as_double(), + get_parameter("alpha2").as_double(), + get_parameter("alpha3").as_double(), + get_parameter("alpha4").as_double()}; + particle_filter_ = std::make_unique( generation_params, kld_resampling_params, + differential_drive_model_params, likelihood_field_model_params, OccupancyGrid{map}); @@ -433,6 +486,19 @@ void AmclNode::timer_callback() message.pose.covariance[35] = covariance.coeff(2, 2); pose_pub_->publish(message); } + + { + // TODO(nahuel): Publish estimated map to odom transform. + auto message = geometry_msgs::msg::TransformStamped{}; + message.header.stamp = now() + tf2::durationFromSec(1.0); + message.header.frame_id = get_parameter("global_frame_id").as_string(); + message.child_frame_id = get_parameter("odom_frame_id").as_string(); + message.transform.rotation.x = 0; + message.transform.rotation.y = 0; + message.transform.rotation.z = 0; + message.transform.rotation.w = 1; + tf_broadcaster_->sendTransform(message); + } } void AmclNode::laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan) @@ -442,6 +508,31 @@ void AmclNode::laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_ } try { + { + 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 = + 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) { + // To avoid loss of diversity in the particle population, don't + // resample when the state is known to be static. + // See 'Probabilistics Robotics, Chapter 4.2.4'. + return; + } + particle_filter_->update_motion(odom_to_base_transform); + } + { auto base_to_laser_transform = tf2::Transform{}; tf2::convert( @@ -449,7 +540,8 @@ void AmclNode::laser_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_ 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); + std::chrono::seconds(1)).transform, + base_to_laser_transform); const float range_min = std::max( laser_scan->range_min, diff --git a/beluga_example/config/params.yaml b/beluga_example/config/params.yaml index 2c440362b..2df8dfeda 100644 --- a/beluga_example/config/params.yaml +++ b/beluga_example/config/params.yaml @@ -1,9 +1,14 @@ amcl: ros__parameters: + alpha1: 0.1 + alpha2: 0.05 + alpha3: 0.1 + alpha4: 0.05 global_frame_id: map odom_frame_id: odom base_frame_id: base map_topic: map + scan_topic: scan max_particles: 2000 min_particles: 500 pf_err: 0.05 diff --git a/beluga_example/models/turtlebot.model.yaml b/beluga_example/models/turtlebot.model.yaml index cd9e51d91..387e7dcd4 100644 --- a/beluga_example/models/turtlebot.model.yaml +++ b/beluga_example/models/turtlebot.model.yaml @@ -20,7 +20,7 @@ plugins: - type: ModelTfPublisher name: tf_publisher - publish_tf_world: true + publish_tf_world: false - type: Laser name: laser_front diff --git a/beluga_example/rviz/rviz.rviz b/beluga_example/rviz/rviz.rviz index b38ebace3..39d85e789 100644 --- a/beluga_example/rviz/rviz.rviz +++ b/beluga_example/rviz/rviz.rviz @@ -132,9 +132,10 @@ Visualization Manager: Show Names: true Tree: map: - base: - laser_front: - {} + odom: + base: + laser_front: + {} Update Interval: 0 Value: true - Alpha: 1