Merge pull request RoboSense-LiDAR#39 from DUT-Racing/fix/fake_percep…

…tion

Fix/fake perception

src/simulation/fake_perception/include/fake_perception.hpp

@@ -65,9 +65,12 @@ class Perception : public rclcpp::Node {
    public:
     Perception();
 
-    void generate_lidar_cone_data(tf2_ros::Buffer &buffer);
+    void generate_lidar_cone_data();
 
    private:
+    tf2_ros::Buffer buffer;
+    tf2_ros::TransformListener transform_listener;
+
     OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle;
     rcl_interfaces::msg::SetParametersResult parameters_callback(const std::vector<rclcpp::Parameter> &parameters);
 
@@ -82,8 +85,6 @@ class Perception : public rclcpp::Node {
     std::string car_frame;
     std::vector<std::string> color_order;
 
-    geometry_msgs::msg::TransformStamped map_to_lidar_transform;
-
     // Lidar Parameter variables
     double lidar_FOV;
     DOF_params lidar_DOF;
@@ -101,6 +102,8 @@ class Perception : public rclcpp::Node {
 
     rclcpp::Publisher<cv_msgs::msg::ConeList>::SharedPtr pub_cones;
 
+    rclcpp::TimerBase::SharedPtr timer_perception;
+
     void on_new_track(const sim_msgs::msg::Track &message);
     void on_new_car_state(const sim_msgs::msg::CarInfo &message);
 

src/simulation/fake_perception/src/fake_perception.cpp

@@ -4,7 +4,7 @@ using namespace std::chrono_literals;
 
 std::default_random_engine random_engine;
 
-Perception::Perception() : Node("fake_perception") {
+Perception::Perception() : Node("fake_perception"), buffer(get_clock()), transform_listener(buffer) {
     parameter_callback_handle = add_on_set_parameters_callback(std::bind(&Perception::parameters_callback, this, std::placeholders::_1));
 
     declare_parameter<std::string>("frames.lidar");
@@ -95,6 +95,8 @@ void Perception::register_pub_subs() {
     sub_car_info = create_subscription<sim_msgs::msg::CarInfo>("/sim/car_info", 10, std::bind(&Perception::on_new_car_state, this, std::placeholders::_1));
 
     pub_cones = create_publisher<cv_msgs::msg::ConeList>("/cv/cones", 10);
+
+    timer_perception = rclcpp::create_timer(this, get_clock(), 1000ms / 10, std::bind(&Perception::generate_lidar_cone_data, this));
 }
 
 void Perception::on_new_track(const sim_msgs::msg::Track &track) {
@@ -286,34 +288,8 @@ std::list<ConeDetection> Perception::get_visible_cones(const geometry_msgs::msg:
     return visible_cones;
 }
 
-void Perception::generate_lidar_cone_data(tf2_ros::Buffer &buffer) {
-    try {
-        /*
-         * This maps coordinates relative to the map to coordinates relative to the velodyne.
-         *
-         * Calculate this by concatenating velodyne->car and car->map
-         * then we get the actual distance from the velodyne (on the real car)
-         * to the map instead of on the distorted car (since lidar readings are
-         * not affected by SE drifts)
-         *
-         * TODO: this can be greatly simplified
-         */
-        geometry_msgs::msg::TransformStamped baselink_to_lidar_transform = buffer.lookupTransform(lidar_frame, car_frame, rclcpp::Time(0), 1ns);
-        tf2::Transform baselink_to_lidar_transform_tf2;
-        tf2::fromMsg(baselink_to_lidar_transform.transform, baselink_to_lidar_transform_tf2);
-
-        geometry_msgs::msg::TransformStamped map_to_carcg_transform = buffer.lookupTransform(car_frame, "map", rclcpp::Time(0), 1ns);
-        tf2::Transform map_to_carcg_transform_tf2;
-        tf2::fromMsg(map_to_carcg_transform.transform, map_to_carcg_transform_tf2);
-
-        geometry_msgs::msg::Transform map_to_lidar_transform_unstamped = tf2::toMsg(baselink_to_lidar_transform_tf2 * map_to_carcg_transform_tf2);
-
-        map_to_lidar_transform.header.stamp = now();
-        map_to_lidar_transform.transform = map_to_lidar_transform_unstamped;
-    } catch (tf2::TransformException &ex) {
-        RCLCPP_WARN(get_logger(), "%s", ex.what());
-        rclcpp::sleep_for(100ms);
-    }
+void Perception::generate_lidar_cone_data() {
+    const auto map_to_lidar_transform = buffer.lookupTransform(lidar_frame, "map", rclcpp::Time(0), 1ns);
 
     // Start time of cone detection
     rclcpp::Time time_exec_start = now();
@@ -374,20 +350,7 @@ int main(int argc, char **argv) {
     auto perception_node = std::make_shared<Perception>();
     RCLCPP_INFO(perception_node->get_logger(), "Fake Perception Node Main Setup");
 
-    tf2_ros::Buffer tfBuffer(perception_node->get_clock());
-    tf2_ros::TransformListener tfListener(tfBuffer);
-
-    rclcpp::Rate lidar_rate(10.0);
-
-    while (rclcpp::ok()) {
-        // Listen to the car tf and publish the updated world state
-        if (track_received_) {
-            perception_node->generate_lidar_cone_data(tfBuffer);
-        }
-
-        lidar_rate.sleep();
-        rclcpp::spin_some(perception_node);
-    }
+    rclcpp::spin(perception_node);
 
     return 0;
 }

src/simulation/simulator/include/sim_viz.hpp

@@ -114,7 +114,7 @@ class SimViz : public rclcpp::Node {
     void add_cone_marker(visualization_msgs::msg::MarkerArray &array, const sim_msgs::msg::TrackObject &track_object, int32_t marker_id);
 
     // Create a single detected cone marker and add it the marker array
-    void add_detected_cone_marker(visualization_msgs::msg::MarkerArray &array, const cv_msgs::msg::Cone &cone, int32_t marker_id);
+    void add_detected_cone_marker(visualization_msgs::msg::MarkerArray &array, const cv_msgs::msg::ConeList &cones, int32_t cone_index);
 };
 
 #endif

src/simulation/simulator/src/sim_viz.cpp

@@ -72,7 +72,7 @@ void SimViz::detected_cones_callback(const cv_msgs::msg::ConeList &cone_list) {
     visualization_msgs::msg::MarkerArray marker_array;
 
     for (uint32_t i = 0; i < cone_list.cones.size(); i++) {
-        add_detected_cone_marker(marker_array, cone_list.cones[i], i);
+        add_detected_cone_marker(marker_array, cone_list, i);
     }
 
     pub_detected_cones->publish(marker_array);
@@ -292,13 +292,14 @@ void SimViz::add_cone_marker(visualization_msgs::msg::MarkerArray &marker_array,
     marker_array.markers.push_back(marker);
 }
 
-void SimViz::add_detected_cone_marker(visualization_msgs::msg::MarkerArray &marker_array, const cv_msgs::msg::Cone &cone, int32_t id) {
+void SimViz::add_detected_cone_marker(visualization_msgs::msg::MarkerArray &marker_array, const cv_msgs::msg::ConeList &cones, int32_t cone_index) {
+    const auto cone = cones.cones[cone_index];
+
     visualization_msgs::msg::Marker marker;
 
-    marker.header.frame_id = car_frame;
-    marker.header.stamp = now();
+    marker.header = cones.header;
     marker.lifetime = rclcpp::Duration(0ns);
-    marker.id = id;
+    marker.id = cone_index;
     marker.type = visualization_msgs::msg::Marker::SPHERE;
     marker.action = visualization_msgs::msg::Marker::ADD;