diff --git a/src/arussim/CMakeLists.txt b/src/arussim/CMakeLists.txt index d37acaf..934d6c5 100755 --- a/src/arussim/CMakeLists.txt +++ b/src/arussim/CMakeLists.txt @@ -168,11 +168,25 @@ ament_target_dependencies(supervisor_exec rclcpp tf2_ros tf2) -# Install both executables +# generate executable for ideal_slam.cpp (ground truth SLAM, replaces graph_slam in simulation) +add_executable(ideal_slam_exec src/ideal_slam.cpp) +target_include_directories(ideal_slam_exec PRIVATE include) +ament_target_dependencies(ideal_slam_exec rclcpp + std_msgs + sensor_msgs + geometry_msgs + common_msgs + tf2_ros + tf2 + PCL + pcl_conversions) + +# Install executables install(TARGETS arussim_exec sensors_exec supervisor_exec + ideal_slam_exec DESTINATION lib/${PROJECT_NAME}) # share folders location diff --git a/src/arussim/config/ideal_slam_config.yaml b/src/arussim/config/ideal_slam_config.yaml new file mode 100644 index 0000000..66c45bc --- /dev/null +++ b/src/arussim/config/ideal_slam_config.yaml @@ -0,0 +1,20 @@ +ideal_slam: + ros__parameters: + # TOPICS + map_topic: "/slam/map" # Output: map of cones + lap_count_topic: "/slam/lap_count" # Output: laps completed + track_topic: "/arussim/track" # Input: real cones of the track (ARUSSim) + perception_topic: "/arussim/perception" # Input: cones detected by the simulated lidar + ground_truth_topic: "/arussim/ground_truth" # Input: real state of the car (ARUSSim) + tpl_signal_topic: "/arussim/tpl_signal" # Input: the car crosses the finish line (ARUSSim) + reset_topic: "/arussim/reset" # Input: simulation reset (ARUSSim) + global_frame: "arussim/world" # Frame of the map and parent of the car TF + local_frame: "slam/vehicle" # Frame of the car TF + + # SLAM PARAMETERS + sensor_max_dist: 30.0 # m Detections farther than this are ignored + match_dist: 1.0 # m Max distance between a detection and the real cone it comes from + track_tolerance: 0.001 # m Min change in a cone position to consider that the track has changed + publish_freq: 10.0 # Hz Frequency at which the map and the lap count are published + min_observations: 12 # Times a cone must be detected to be added to the map + min_lap_distance: 30.0 # m Min driven distance to count a new lap diff --git a/src/arussim/src/ideal_slam.cpp b/src/arussim/src/ideal_slam.cpp new file mode 100644 index 0000000..70e1428 --- /dev/null +++ b/src/arussim/src/ideal_slam.cpp @@ -0,0 +1,250 @@ +/** + * @file ideal_slam.cpp + * @brief Ideal SLAM for ARUSSim: replaces graph_slam using the simulator ground truth. + * + * Publishes the same interface as graph_slam: + * - TF arussim/world -> slam/vehicle (perfect pose) + * - /slam/map (cones detected by the simulated lidar, with perfect position) + * - /slam/lap_count (laps completed) + */ +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "arussim/ConeXYZColorScore.h" + +// Cone published in /slam/map. The stack reads the covariances, which are 0 here (ideal SLAM). +struct SlamCone { + PCL_ADD_POINT4D; + int color; + float score; + float cov_xx; + float cov_yy; + float cov_xy; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +POINT_CLOUD_REGISTER_POINT_STRUCT (SlamCone, + (float, x, x) + (float, y, y) + (float, z, z) + (int, color, color) + (float, score, score) + (float, cov_xx, cov_xx) + (float, cov_yy, cov_yy) + (float, cov_xy, cov_xy) +) + + +// Cone colors, same values as in the rest of the stack +constexpr int kBlue = 0; +constexpr int kYellow = 1; +constexpr int kUncolored = -1; + +class IdealSlam : public rclcpp::Node +{ +public: + IdealSlam() : Node("ideal_slam") + { + this->declare_parameter("map_topic", "/slam/map"); + this->declare_parameter("lap_count_topic", "/slam/lap_count"); + this->declare_parameter("track_topic", "/arussim/track"); + this->declare_parameter("perception_topic", "/arussim/perception"); + this->declare_parameter("ground_truth_topic", "/arussim/ground_truth"); + this->declare_parameter("tpl_signal_topic", "/arussim/tpl_signal"); + this->declare_parameter("reset_topic", "/arussim/reset"); + this->declare_parameter("global_frame", "arussim/world"); + this->declare_parameter("local_frame", "slam/vehicle"); + this->declare_parameter("sensor_max_dist", 30.0); + this->declare_parameter("match_dist", 1.0); + this->declare_parameter("track_tolerance", 0.001); + this->declare_parameter("publish_freq", 10.0); + this->declare_parameter("min_observations", 12); + this->declare_parameter("min_lap_distance", 30.0); + global_frame_ = this->get_parameter("global_frame").as_string(); + local_frame_ = this->get_parameter("local_frame").as_string(); + max_dist_ = this->get_parameter("sensor_max_dist").as_double(); + match_dist_ = this->get_parameter("match_dist").as_double(); + track_tolerance_ = this->get_parameter("track_tolerance").as_double(); + min_observations_ = this->get_parameter("min_observations").as_int(); + min_lap_distance_ = this->get_parameter("min_lap_distance").as_double(); + + tf_broadcaster_ = std::make_shared(this); + map_pub_ = this->create_publisher(this->get_parameter("map_topic").as_string(), 10); + lap_count_pub_ = this->create_publisher(this->get_parameter("lap_count_topic").as_string(), 10); + + track_sub_ = this->create_subscription( + this->get_parameter("track_topic").as_string(), 1, std::bind(&IdealSlam::track_callback, this, std::placeholders::_1)); + perception_sub_ = this->create_subscription( + this->get_parameter("perception_topic").as_string(), 1, std::bind(&IdealSlam::perception_callback, this, std::placeholders::_1)); + ground_truth_sub_ = this->create_subscription( + this->get_parameter("ground_truth_topic").as_string(), 1, std::bind(&IdealSlam::ground_truth_callback, this, std::placeholders::_1)); + tpl_sub_ = this->create_subscription( + this->get_parameter("tpl_signal_topic").as_string(), 10, std::bind(&IdealSlam::tpl_callback, this, std::placeholders::_1)); + reset_sub_ = this->create_subscription( + this->get_parameter("reset_topic").as_string(), 1, [this](std_msgs::msg::Bool::SharedPtr) { reset(); }); + + timer_ = this->create_wall_timer(std::chrono::duration(1.0 / this->get_parameter("publish_freq").as_double()), std::bind(&IdealSlam::on_timer, this)); + } + +private: + void reset() + { + observations_.assign(track_.size(), 0); + in_map_.assign(track_.size(), false); + map_.clear(); + passed_.clear(); + lap_count_ = 0; + driven_distance_ = 0.0; + } + + void track_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) + { + pcl::PointCloud new_track; + pcl::fromROSMsg(*msg, new_track); + + // The simulator republishes the track periodically: only reset when it changes + bool changed = new_track.size() != track_.size(); + for (size_t i = 0; !changed && i < new_track.size(); ++i) { + changed = std::abs(new_track[i].x - track_[i].x) > track_tolerance_ + || std::abs(new_track[i].y - track_[i].y) > track_tolerance_; + } + if (changed) { + track_ = new_track; + reset(); + } + } + + void ground_truth_callback(const common_msgs::msg::State::SharedPtr msg) + { + if (has_pose_) driven_distance_ += std::hypot(msg->x - x_, msg->y - y_); + x_ = msg->x; + y_ = msg->y; + yaw_ = msg->yaw; + has_pose_ = true; + + geometry_msgs::msg::TransformStamped tf; + tf.header.stamp = this->now(); + tf.header.frame_id = global_frame_; + tf.child_frame_id = local_frame_; + tf.transform.translation.x = x_; + tf.transform.translation.y = y_; + tf2::Quaternion q; + q.setRPY(0, 0, yaw_); + tf.transform.rotation.x = q.x(); + tf.transform.rotation.y = q.y(); + tf.transform.rotation.z = q.z(); + tf.transform.rotation.w = q.w(); + tf_broadcaster_->sendTransform(tf); + } + + void tpl_callback(const std_msgs::msg::Bool::SharedPtr) + { + // Same criteria as graph_slam: a new lap needs a minimum driven distance + if (driven_distance_ > min_lap_distance_) { + driven_distance_ = 0.0; + lap_count_++; + } + } + + void perception_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) + { + if (!has_pose_) return; + pcl::PointCloud scan; + pcl::fromROSMsg(*msg, scan); + const double c = std::cos(yaw_), s = std::sin(yaw_); + + std::vector counted(track_.size(), false); + for (const auto& p : scan) { + if (std::hypot(p.x, p.y) > max_dist_) continue; // Farther than the sensor range we want to simulate + const double wx = x_ + p.x * c - p.y * s; + const double wy = y_ + p.x * s + p.y * c; + + size_t best = track_.size(); + double best_dist = match_dist_; + for (size_t i = 0; i < track_.size(); ++i) { + const double d = std::hypot(track_[i].x - wx, track_[i].y - wy); + if (d < best_dist) { best_dist = d; best = i; } + } + if (best == track_.size() || counted[best]) continue; + counted[best] = true; + observations_[best]++; + + // Like graph_slam, a cone is published once it has been seen several times + if (in_map_[best] || observations_[best] < min_observations_) continue; + in_map_[best] = true; + + SlamCone cone{}; + cone.x = track_[best].x; + cone.y = track_[best].y; + cone.z = 0; + cone.color = kUncolored; // Uncolored until the car passes it (see on_timer) + cone.score = track_[best].score; + map_.push_back(cone); + passed_.push_back(false); + } + } + + void on_timer() + { + // Same as graph_slam (pass_coloring): a cone is colored when the car has just passed it + const double c = std::cos(yaw_), s = std::sin(yaw_); + for (size_t i = 0; i < map_.size(); ++i) { + if (passed_[i]) continue; + const double dx = map_[i].x - x_, dy = map_[i].y - y_; + const double lx = dx * c + dy * s, ly = -dx * s + dy * c; + if (std::hypot(lx, ly) < 6.0 && lx < 0.0 && lx > -2.0) { + map_[i].color = ly > 0.0 ? kBlue : kYellow; + passed_[i] = true; + } + } + + sensor_msgs::msg::PointCloud2 map_msg; + pcl::toROSMsg(map_, map_msg); + map_msg.header.stamp = this->now(); + map_msg.header.frame_id = global_frame_; + map_pub_->publish(map_msg); + + std_msgs::msg::Int16 lap_msg; + lap_msg.data = lap_count_; + lap_count_pub_->publish(lap_msg); + } + + std::string global_frame_, local_frame_; + int min_observations_; + double max_dist_, match_dist_, track_tolerance_, min_lap_distance_; + pcl::PointCloud track_; + pcl::PointCloud map_; + std::vector passed_; // Cone already colored because the car passed it + std::vector observations_; + std::vector in_map_; + double x_ = 0.0, y_ = 0.0, yaw_ = 0.0, driven_distance_ = 0.0; + bool has_pose_ = false; + int16_t lap_count_ = 0; + + std::shared_ptr tf_broadcaster_; + rclcpp::Publisher::SharedPtr map_pub_; + rclcpp::Publisher::SharedPtr lap_count_pub_; + rclcpp::Subscription::SharedPtr track_sub_; + rclcpp::Subscription::SharedPtr ground_truth_sub_; + rclcpp::Subscription::SharedPtr perception_sub_; + rclcpp::Subscription::SharedPtr tpl_sub_, reset_sub_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +}