FSF-6559 | Feat/more planning fixes (#364)

* feat: changed to old inspection

* fix: solved WSS readings

* feat: added restart shell script for on-ground testing and state estimation only runs after go_signal

* feat(planning): cone coloring with memory can be (de)activated through launch flag

* fix(planning): better filtering of received cones

* fix(planning): tests reflect flag for using memory

* feat(planning): distance to consider two cones the same definable through launch file

* fix(planning): ordering of the path starts on different places depending if cone coloring uses memory

* fix: changed state estimation to publish various prediciton steps and added positio visualization message

* fix:changed perception frame id

* fix(planning): removes cones which become too close after updating position

* fix: changed outlier robustness

* fix(planning): better smoothing and better selection of initial cones in cone coloring

* fix(planning): car orientation used in smoothing depends on planning using memory or not

* fix(planning): corrected call to smoothing

* feat: added lidar point cloud rotation

* feat(ekf_state_est): vehicle adapter receives xsens imu data

* fix: system ready for new testing on ground

* fix: changed lookahead gain

* fix: changed lidar topic in perception subscription

* fix: first +/- reliable iteration

* feat: increaded FoV

* feat: increaded max ranfe and fov trim

* fix: replaced n_points validation

---------

Co-authored-by: FSFEUP <formulastudentfeup@gmail.com>
Co-authored-by: Lourenço Rodrigues <up202306331@up.pt>

src/control/launch/vehicle.launch.py

@@ -21,7 +21,7 @@ def generate_launch_description():
             DeclareLaunchArgument(
                 "lookahead_gain",
                 description="Variable K -> Lookahead Gain",
-                default_value="2.0",
+                default_value="1.5",
             ),
             DeclareLaunchArgument(
                 "pid_kp",

src/control/src/node_/node_control.cpp

@@ -19,6 +19,9 @@ using namespace common_lib::structures;
 
 using namespace common_lib::communication;
 
+bool received_vehicle_state = false;
+bool received_path_point_array = false;
+
 Control::Control(const ControlParameters& params)
     : Node("control"),
       using_simulated_se_(params.using_simulated_se_),
@@ -32,6 +35,8 @@ Control::Control(const ControlParameters& params)
           [this](const custom_interfaces::msg::PathPointArray& msg) {
             RCLCPP_DEBUG(this->get_logger(), "Received pathpoint array");
             pathpoint_array_ = msg.pathpoint_array;
+            received_path_point_array = true;
+            received_vehicle_state = false;
           })),
       closest_point_pub_(create_publisher<visualization_msgs::msg::Marker>(
           "/control/visualization/closest_point", 10)),
@@ -51,13 +56,28 @@ Control::Control(const ControlParameters& params)
 
 // This function is called when a new pose is received
 void Control::publish_control(const custom_interfaces::msg::VehicleState& vehicle_state_msg) {
+
   if (!go_signal_) {
     RCLCPP_INFO(rclcpp::get_logger("control"),
                  "Go Signal Not received");
 
     return;
   }
 
+  if (received_path_point_array && !received_vehicle_state) {
+      RCLCPP_DEBUG(rclcpp::get_logger("control"),
+                  "First Vehicle State Received");
+
+      received_vehicle_state = true;
+      custom_interfaces::msg::PathPoint initial;
+      initial.x = vehicle_state_msg.position.x;
+      initial.y = vehicle_state_msg.position.y;
+      initial.v = vehicle_state_msg.linear_velocity;
+
+      // Insert at the beginning of the pathpoint_array_ (true "push_front")
+      pathpoint_array_.insert(pathpoint_array_.begin(), initial);
+  }
+
   rclcpp::Time start = this->now();
 
   // update vehicle pose
@@ -77,6 +97,7 @@ void Control::publish_control(const custom_interfaces::msg::VehicleState& vehicl
   // update the Lookahead point
   auto [lookahead_point, lookahead_velocity, lookahead_error] =
       this->point_solver_.update_lookahead_point(pathpoint_array_, closest_point_id);
+
   if (lookahead_error) {
     RCLCPP_DEBUG(rclcpp::get_logger("control"), "PurePursuit: Failed to update lookahed point");
     return;

src/ekf_state_est/include/adapter_ekf_state_est/vehicle.hpp

@@ -10,6 +10,7 @@
 #include "custom_interfaces/msg/operational_status.hpp"
 #include "custom_interfaces/msg/steering_angle.hpp"
 #include "custom_interfaces/msg/wheel_rpm.hpp"
+#include "geometry_msgs/msg/vector3_stamped.hpp"
 #include "std_srvs/srv/empty.hpp"
 #include "std_srvs/srv/trigger.hpp"
 
@@ -25,12 +26,18 @@ class VehicleAdapter : public Adapter {
   rclcpp::Subscription<custom_interfaces::msg::OperationalStatus>::SharedPtr
       _operational_status_subscription_;  ///< Subscriber for operational status
 
+  message_filters::Subscriber<geometry_msgs::msg::Vector3Stamped> _free_acceleration_subscription_;
+  message_filters::Subscriber<geometry_msgs::msg::Vector3Stamped> _angular_velocity_subscription_;
+
   message_filters::Subscriber<custom_interfaces::msg::ImuData> _roll_accx_imu_subscription_;
   message_filters::Subscriber<custom_interfaces::msg::ImuData> _yaw_accy_imu_subscription_;
 
   using ImuPolicy = message_filters::sync_policies::ApproximateTime<
       custom_interfaces::msg::ImuData,
       custom_interfaces::msg::ImuData>;  ///< Policy for synchronizing IMU data
+  using XsensImuPolicy = message_filters::sync_policies::ApproximateTime<
+      geometry_msgs::msg::Vector3Stamped,
+      geometry_msgs::msg::Vector3Stamped>;  ///< Policy for synchronizing Xsens IMU data
   using WheelSteerPolicy = message_filters::sync_policies::ApproximateTime<
       custom_interfaces::msg::WheelRPM, custom_interfaces::msg::WheelRPM,
       custom_interfaces::msg::SteeringAngle>;  ///< Policy for synchronizing wheel speeds and
@@ -40,6 +47,8 @@ class VehicleAdapter : public Adapter {
 
   std::shared_ptr<message_filters::Synchronizer<ImuPolicy>> _imu_sync_;
 
+  std::shared_ptr<message_filters::Synchronizer<XsensImuPolicy>> _xsens_imu_sync_;
+
   rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr
       _finished_client_;  ///< Client for finished signal
 
@@ -62,6 +71,18 @@ class VehicleAdapter : public Adapter {
   void imu_subscription_callback(const custom_interfaces::msg::ImuData& roll_accx_data,
                                  const custom_interfaces::msg::ImuData& yaw_accy_data);
 
+  /**
+   * @brief Xsens IMU subscription callback, which receives both free acceleration and angular
+   * velocity data separately and synchronizes them to then call State Estimation node's IMU
+   * callback
+   *
+   * @param free_acceleration_msg acceleration without gravity in x, y and z axis
+   * @param angular_velocity_msg angular velocity around x, y and z axis
+   */
+  void xsens_imu_subscription_callback(
+      const geometry_msgs::msg::Vector3Stamped::SharedPtr& free_acceleration_msg,
+      const geometry_msgs::msg::Vector3Stamped::SharedPtr& angular_velocity_msg);
+
 public:
   explicit VehicleAdapter(std::shared_ptr<SENode> se_node);
 

src/ekf_state_est/launch/vehicle.launch.py

@@ -13,7 +13,7 @@ def generate_launch_description():
             DeclareLaunchArgument(
                 "data_association_limit_distance",
                 description="Maximum distance to admit landmarks",
-                default_value="20.00",
+                default_value="17.50",
             ),  # meters
             DeclareLaunchArgument(
                 "wss_noise",
@@ -28,7 +28,7 @@ def generate_launch_description():
             DeclareLaunchArgument(
                 "observation_noise",
                 description="Noise value for observations (sigma)",
-                default_value="0.04",
+                default_value="0.03",
             ),
             DeclareLaunchArgument(
                 "motion_model",
@@ -48,7 +48,7 @@ def generate_launch_description():
             DeclareLaunchArgument(
                 "use_odometry",
                 description="Either use odometry or IMU (TODO: remove for complete velocity estimation)",
-                default_value="False",
+                default_value="True",
             ),
             DeclareLaunchArgument(
                 "use_simulated_perception",

src/ekf_state_est/src/adapter_ekf_state_est/vehicle.cpp

@@ -21,6 +21,12 @@ VehicleAdapter::VehicleAdapter(std::shared_ptr<SENode> se_node) : Adapter(se_nod
       imu_policy, _yaw_accy_imu_subscription_, _roll_accx_imu_subscription_);
   this->_imu_sync_->registerCallback(&VehicleAdapter::imu_subscription_callback, this);
 
+  this->_free_acceleration_subscription_.subscribe(this->node_, "/filter/free_acceleration");
+  this->_angular_velocity_subscription_.subscribe(this->node_, "/imu/angular_velocity");
+  const XsensImuPolicy xsens_imu_policy(10);
+  this->_xsens_imu_sync_ = std::make_shared<message_filters::Synchronizer<XsensImuPolicy>>(
+      xsens_imu_policy, _free_acceleration_subscription_, _angular_velocity_subscription_);
+  this->_xsens_imu_sync_->registerCallback(&VehicleAdapter::xsens_imu_subscription_callback, this);
 
   this->_rl_wheel_rpm_subscription_.subscribe(this->node_, "/vehicle/rl_rpm");
   this->_rr_wheel_rpm_subscription_.subscribe(this->node_, "/vehicle/rr_rpm");
@@ -35,29 +41,37 @@ VehicleAdapter::VehicleAdapter(std::shared_ptr<SENode> se_node) : Adapter(se_nod
       this->node_->create_client<std_srvs::srv::Trigger>("/as_srv/mission_finished");
 }
 
-void VehicleAdapter::wheel_speeds_subscription_callback (
+void VehicleAdapter::wheel_speeds_subscription_callback(
     const custom_interfaces::msg::WheelRPM& rl_wheel_rpm_msg,
     const custom_interfaces::msg::WheelRPM& rr_wheel_rpm_msg,
     const custom_interfaces::msg::SteeringAngle& steering_angle_msg) {
-
   RCLCPP_INFO(this->node_->get_logger(), "Received WSS!");
-  
+
   this->node_->_wheel_speeds_subscription_callback(rl_wheel_rpm_msg.rl_rpm, rr_wheel_rpm_msg.rr_rpm,
                                                    0.0, 0.0, steering_angle_msg.steering_angle,
                                                    steering_angle_msg.header.stamp);
 }
 
-
 void VehicleAdapter::imu_subscription_callback(
-  const custom_interfaces::msg::ImuData& roll_accx_data,
-  const custom_interfaces::msg::ImuData& yaw_accy_data) {
+    const custom_interfaces::msg::ImuData& roll_accx_data,
+    const custom_interfaces::msg::ImuData& yaw_accy_data) {
   auto imu_msg = sensor_msgs::msg::Imu();
   imu_msg.angular_velocity.z = yaw_accy_data.gyro;
   imu_msg.linear_acceleration.x = roll_accx_data.acc;
   imu_msg.linear_acceleration.y = yaw_accy_data.acc;
   this->node_->_imu_subscription_callback(imu_msg);
 }
 
+void VehicleAdapter::xsens_imu_subscription_callback(
+    const geometry_msgs::msg::Vector3Stamped::SharedPtr& free_acceleration_msg,
+    const geometry_msgs::msg::Vector3Stamped::SharedPtr& angular_velocity_msg) {
+  auto imu_msg = sensor_msgs::msg::Imu();
+  imu_msg.angular_velocity.z = angular_velocity_msg->vector.z;
+  imu_msg.linear_acceleration.x = free_acceleration_msg->vector.x;
+  imu_msg.linear_acceleration.y = free_acceleration_msg->vector.y;
+  this->node_->_imu_subscription_callback(imu_msg);
+}
+
 // TODO: implement a more complex logic, like the one in inspection node
 void VehicleAdapter::finish() {
   this->_finished_client_->async_send_request(

src/ekf_state_est/src/ros_node/se_node.cpp

@@ -92,10 +92,9 @@ void SENode::_perception_subscription_callback(const custom_interfaces::msg::Con
     return;
   }
 
-/*
   if (!this->_go_){
     return;
-  }*/
+  }
 
   rclcpp::Time start_time = this->get_clock()->now();
 
@@ -129,11 +128,11 @@ void SENode::_imu_subscription_callback(const sensor_msgs::msg::Imu &imu_msg) {
     return;
   }
 
-  /*
+
   if (!this->_go_){
     return;
   }
-  */
+
 
   rclcpp::Time start_time = this->get_clock()->now();
 
@@ -179,11 +178,11 @@ double difference = 10;
 void SENode::_wheel_speeds_subscription_callback(double rl_speed, double rr_speed, double fl_speed,
                                                  double fr_speed, double steering_angle,
                                                  const rclcpp::Time &timestamp) {
-  /*
+
   if (!this->_go_){
     return;
   }
-  */
+
 
   bool change = false;
 

src/perception/include/perception/parameters_factory.hpp

@@ -30,7 +30,7 @@ PerceptionParameters load_adapter_parameters() {
       adapter_node->declare_parameter("euclidean_fitness_epsilon", 1e-6);
   double fov_trim = adapter_node->declare_parameter("fov_trim", 90);
   params.adapter_ = adapter_node->declare_parameter("adapter", "eufs");
-  params.vehicle_frame_id_ = params.adapter_ == "eufs" ? "velodyne" : "rslidar";
+  params.vehicle_frame_id_ = params.adapter_ == "eufs" ? "velodyne" : "hesai_lidar";
   params.pc_max_range_ = adapter_node->declare_parameter("pc_max_range", 15.0);
 
   // Create shared pointers for components
@@ -62,7 +62,7 @@ PerceptionParameters load_adapter_parameters() {
   params.clustering_ = std::make_shared<DBSCAN>(clustering_n_neighbours, clustering_epsilon);
   params.cone_differentiator_ = std::make_shared<LeastSquaresDifferentiation>();
 
-    params.cone_validators_ = {std::make_shared<CylinderValidator>(0.228, 0.325),
+    params.cone_validators_ = {std::make_shared<CylinderValidator>(0.200, 0.325),
                                std::make_shared<HeightValidator>(min_height, max_height),
                                std::make_shared<DeviationValidator>(min_xoy, max_xoy, min_z, max_z),
                                std::make_shared<ZScoreValidator>(min_z_score_x, max_z_score_x,

src/perception/launch/vehicle.launch.py

@@ -20,12 +20,12 @@ def generate_launch_description():
             DeclareLaunchArgument(
                 "fov_trim",
                 description="Trim the points received to a max angle",
-                default_value="30",  # degrees
+                default_value="45",  # degrees
             ),
             DeclareLaunchArgument(
                 "pc_max_range",
                 description="Point cloud filtering based on distance (m)",
-                default_value="20.0",
+                default_value="30.0",
             ),
 
             DeclareLaunchArgument(
@@ -36,7 +36,7 @@ def generate_launch_description():
             DeclareLaunchArgument(
                 "clustering_epsilon",
                 description="Epsilon for Clustering algorithm",
-                default_value="0.2",
+                default_value="0.7",
             ),
             DeclareLaunchArgument(
                 "horizontal_resolution",
@@ -96,7 +96,7 @@ def generate_launch_description():
             DeclareLaunchArgument(
                 "min_height",
                 description="Minimum height of a cluster to be considered a cone",
-                default_value="0.1"
+                default_value="0.13"
             ),
             DeclareLaunchArgument(
                 "max_height",

src/perception/src/perception/perception_node.cpp

@@ -64,7 +64,7 @@ Perception::Perception(const PerceptionParameters& params)
   this->_adapter_ = params.adapter_;
   if (params.adapter_ == "vehicle") {
     this->_point_cloud_subscription = this->create_subscription<sensor_msgs::msg::PointCloud2>(
-        "/rslidar_points", 10, [this](const sensor_msgs::msg::PointCloud2::SharedPtr msg) {
+        "/lidar_points", 10, [this](const sensor_msgs::msg::PointCloud2::SharedPtr msg) {
           this->point_cloud_callback(msg);
         });
   } else if (params.adapter_ == "eufs") {
@@ -133,6 +133,7 @@ void Perception::point_cloud_callback(const sensor_msgs::msg::PointCloud2::Share
 
   for (auto cluster : clusters) {
     bool is_valid = true;
+    if (cluster.get_point_cloud()->points.size() <= 4) is_valid = false;
     for (auto validator : _cone_validators_) {
       is_valid = is_valid && validator->coneValidator(&cluster, _ground_plane_);
 
@@ -192,23 +193,31 @@ void Perception::fov_trimming(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud, double
 
   double center_x = -x_discount;  // assuming (0, 0) as the center
 
-  for (const auto& point : cloud->points) {
+  for (auto& point : cloud->points) {
+
+    double x = point.x;
+    double y = point.y;
+
+    // This rotates 90º:
+    point.x = -y;
+    point.y = x;
+
     // Calculate distance from the origin (assuming the sensor is at the origin)
     double distance = std::sqrt((point.x - center_x) * (point.x - center_x) + point.y * point.y);
 
     // Calculate the angle in the XY plane
     double angle = std::atan2(point.y, point.x - center_x) * 180 /
                    M_PI;  // get angle and convert in to degrees
 
-    if (distance <= 1.0) {  // Ignore points from the vehicle
+    if (distance <= 1.2) {  // Ignore points from the vehicle
       continue;
     }
 
     if (point.z >= -0.1 && point.z <= 0.1) {  // Ignore points on the ground
       continue;
     }
 
-    if (point.z >= -0.2){
+    if (point.z >= -0.22){
       continue;
     }
 

src/planning/include/planning/cone_coloring.hpp

@@ -106,6 +106,13 @@ class ConeColoring {
                         const TwoDVector& previous_to_last_vector,
                         const double& colored_to_input_cones_ratio) const;
 
+  /**
+   * @brief Useful when using memory to delete cones which after some
+   * times being detected become too close.
+   *
+   */
+  void remove_too_close_cones();
+
   /**
    * @brief select the next cone (which minimizes the cost) to be colored if its cost is less than
    * the maximum established in the configuration

src/planning/include/planning/smoothing.hpp

@@ -8,7 +8,7 @@
 #include "config/smoothing_config.hpp"
 #include "utils/splines.hpp"
 
-constexpr double MAX_DISTANCE_BETWEEN_POINTS = 8.5;
+constexpr double MAX_DISTANCE_BETWEEN_POINTS = 4.5;
 
 using PathPoint = common_lib::structures::PathPoint;
 using Pose = common_lib::structures::Pose;
@@ -29,8 +29,10 @@ class PathSmoothing {
    *
    * @param unord_path unoredred path points
    * @param car_pose pose of the car to start ordering according to the closest point
+   * @param initial_car_orientation initial orientation of the car (usually 0 but not on some tests)
    */
-  void order_path(std::vector<PathPoint>& unord_path, const Pose& car_pose) const;
+  void order_path(std::vector<PathPoint>& unord_path, const Pose& car_pose,
+                  const double initial_car_orientation) const;
 
 public:
   /**
@@ -48,11 +50,12 @@ class PathSmoothing {
    *
    * @param unordered_path input vector of unordered path points
    * @param car_pose pose of the car to start ordering according to the closest point
+   * @param initial_car_orientation initial orientation of the car (usually 0 but not on some tests)
    * @return std::vector<PathPoint> smoothed path points ordered from the closest to the car to
    * farthest
    */
-  std::vector<PathPoint> smooth_path(std::vector<PathPoint>& unordered_path,
-                                     const Pose& car_pose) const;
+  std::vector<PathPoint> smooth_path(std::vector<PathPoint>& unordered_path, const Pose& car_pose,
+                                     const double initial_car_orientation) const;
 };
 
 #endif  // SRC_PLANNING_INCLUDE_PLANNING_SMOOTHING2_HPP_