diff --git a/src/simulation/fake_perception/include/fake_perception.hpp b/src/simulation/fake_perception/include/fake_perception.hpp index d7c84c92..6e45b8fd 100644 --- a/src/simulation/fake_perception/include/fake_perception.hpp +++ b/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 ¶meters); @@ -82,8 +85,6 @@ class Perception : public rclcpp::Node { std::string car_frame; std::vector 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::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); diff --git a/src/simulation/fake_perception/src/fake_perception.cpp b/src/simulation/fake_perception/src/fake_perception.cpp index b7c0ac94..3efca5ec 100644 --- a/src/simulation/fake_perception/src/fake_perception.cpp +++ b/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("frames.lidar"); @@ -95,6 +95,8 @@ void Perception::register_pub_subs() { sub_car_info = create_subscription("/sim/car_info", 10, std::bind(&Perception::on_new_car_state, this, std::placeholders::_1)); pub_cones = create_publisher("/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 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(); 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; } diff --git a/src/simulation/simulator/include/sim_viz.hpp b/src/simulation/simulator/include/sim_viz.hpp index 2f58bec9..8d61cb14 100644 --- a/src/simulation/simulator/include/sim_viz.hpp +++ b/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 diff --git a/src/simulation/simulator/src/sim_viz.cpp b/src/simulation/simulator/src/sim_viz.cpp index ad7d0348..abb24bd0 100644 --- a/src/simulation/simulator/src/sim_viz.cpp +++ b/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;