Merge with main

dependencies_install.sh

@@ -26,20 +26,20 @@ rospack find sensor_msgs -y
 sudo apt-get install libpcap-dev -y
 
 # ground truth generation
-sudo pip install pandas
+pip install pandas
 sudo apt-get install python3-matplotlib -y
 
 # evaluator
-sudo pip3 install transforms3d
+pip install transforms3d
 
 #cloud storage
-sudo pip install google-cloud-storage
+pip install google-cloud-storage
 
 #local dashboard
-sudo pip install dash
+pip install dash
 
 # needed to not broke evaluator
-sudo pip install numpy==1.21.5
+pip install numpy==1.21.5
 
-sudo pip install transforms3d==0.4.1
-sudo pip install scipy==1.10.0
+pip install transforms3d==0.4.1
+pip install scipy==1.10.0

src/cloud_storage/send_bucket.py

@@ -11,6 +11,7 @@
     "evaluator": "performance/evaluator_metrics",
     "power_log": "performance/power_log_metrics",
     "planning_cone_coloring": "performance/other_metrics/planning",
+    "test": "test",
 }
 
 

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/dashboard/dashboard.py

@@ -31,6 +31,7 @@
     "Planning_exec_time",
     "Planning_cone_coloring",
     "Power_log",
+    "Test",
 ]
 
 # Define the main layout of the app
@@ -64,6 +65,7 @@ def get_dashboard_layout(dashboard):
         "Planning_exec_time": "planning_exec_time",
         "Planning_cone_coloring": "planning_cone_coloring",
         "Power_log": "power_log",
+        "Test": "test",
     }
 
     condition = dashboard_conditions.get(dashboard, "")
@@ -162,6 +164,60 @@ def get_dashboard_layout(dashboard):
                 ],
                 style={"display": "flex"},
             ),
+            html.Div(
+                [
+                    dcc.Graph(
+                        id=f"graph4-{dashboard}",
+                        style={"width": "70%", "height": "500px"},
+                    ),
+                    html.Div(
+                        [
+                            html.Label("Y-Axis"),
+                            dcc.Dropdown(
+                                id=f"graph4-{dashboard}-metrics-dropdown",
+                                multi=True,
+                                style={"minWidth": "200px"},
+                            ),
+                            html.Br(),
+                            html.Label("X-Axis"),
+                            dcc.Dropdown(
+                                id=f"x-axis-dropdown-{dashboard}-3",
+                                style={"minWidth": "200px"},
+                            ),
+                        ],
+                        id=f"graph4-metrics-{dashboard}",
+                        style={"minWidth": "200px", "width": "25%"},
+                    ),
+                ],
+                style={"display": "flex"},
+            ),
+            html.Div(
+                [
+                    dcc.Graph(
+                        id=f"graph5-{dashboard}",
+                        style={"width": "70%", "height": "500px"},
+                    ),
+                    html.Div(
+                        [
+                            html.Label("Y-Axis"),
+                            dcc.Dropdown(
+                                id=f"graph5-{dashboard}-metrics-dropdown",
+                                multi=True,
+                                style={"minWidth": "200px"},
+                            ),
+                            html.Br(),
+                            html.Label("X-Axis"),
+                            dcc.Dropdown(
+                                id=f"x-axis-dropdown-{dashboard}-3",
+                                style={"minWidth": "200px"},
+                            ),
+                        ],
+                        id=f"graph5-metrics-{dashboard}",
+                        style={"minWidth": "200px", "width": "25%"},
+                    ),
+                ],
+                style={"display": "flex"},
+            ),
         ]
     )
 
@@ -294,7 +350,22 @@ def update_graph(
                 labels={"value": "Metrics", x_axis: x_axis},
                 title="Scatter Plot",
             )
-
+        elif graph_type == "bar":
+            fig = px.bar(
+                df_melted,
+                x=x_axis,
+                y="value",
+                color="Source_Metric",
+                barmode="group",
+                labels={"value": "Metrics", x_axis: x_axis},
+                title="Bar Chart",
+            )
+        elif graph_type == "heatmap":
+            fig = px.imshow(
+            df_melted.pivot_table(index=x_axis, columns="variable", values="value"),
+            labels={"value": "Metric Value", x_axis: x_axis},
+            title="Heatmap",
+            )
         return fig
 
 
@@ -303,9 +374,13 @@ def update_graph(
     create_update_metric_dropdowns_callback(dashboard, 1)
     create_update_metric_dropdowns_callback(dashboard, 2)
     create_update_metric_dropdowns_callback(dashboard, 3)
+    create_update_metric_dropdowns_callback(dashboard, 4)
+    create_update_metric_dropdowns_callback(dashboard, 5)
     create_update_graph_callback(f"graph1-{dashboard}", dashboard, 1, "line")
     create_update_graph_callback(f"graph2-{dashboard}", dashboard, 2, "line")
     create_update_graph_callback(f"graph3-{dashboard}", dashboard, 3, "scatter")
+    create_update_graph_callback(f"graph4-{dashboard}", dashboard, 4, "bar")
+    create_update_graph_callback(f"graph5-{dashboard}", dashboard, 5, "heatmap")
 
 
 # Cleanup function to remove the temp folder

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;