update for fix filter data drop

CentroEPiaggio · Oct 10, 2024 · a52f0bf · a52f0bf
1 parent ab185a7
commit a52f0bf
Show file tree

Hide file tree

Showing 6 changed files with 42 additions and 33 deletions.
diff --git a/pronto_core/src/rbis_update_interface.cpp b/pronto_core/src/rbis_update_interface.cpp
@@ -1,12 +1,12 @@
 #include "pronto_core/rbis_update_interface.hpp"
 #include "pronto_core/rotations.hpp"
 #include <iostream>
-
+#define DEBUG_MODE_IM 0
 namespace pronto {
 
 const char * RBISUpdateInterface::sensor_enum_chars = "igvlfsorxdukpabmtwy";
 const char * RBISUpdateInterface::sensor_enum_strings[] =
-    { "ins", "gps", "vicon", "laser", "laser_gpf", "scan_matcher", "optic_flow", "reset", "invalid", "rgbd", "fovis", "legodo", "pose_meas", "altimeter", "airspeed", "sideslip", "init_message", "viewer", "yawlock" };
+    { "ins", "gps", "vicon", "laser", "laser_gpf", "scan_matcher", "optic_flow", "reset", "invalid", "rgbd", "fovis", "legodo", "pose_meas", "altimeter", "airspeed", "sideslip", "init_message", "viewer", "yawlock" , "wheel_odom" };
 
 RBISUpdateInterface::sensor_enum RBISUpdateInterface::sensor_enum_from_char(char sensor_char)
 {
@@ -84,8 +84,9 @@ void RBISIndexedMeasurement::updateFilter(const RBIS & prior_state, const RBIM &
   RBIS dstate;
   RBIM dcov;
 
-#if DEBUG_MODE
+#if DEBUG_MODE_IM
     std::cout << "mfallon a\n";
+    std::cout<< "DIOCANEEEEEE\n";
     std::cout << " meas: " << measurement.transpose() << "\n";
     std::cout << " mcov: " << measurement_cov << "\n";
     std::cout << "index: " << index.transpose() << "\n";
@@ -96,7 +97,7 @@ void RBISIndexedMeasurement::updateFilter(const RBIS & prior_state, const RBIM &
   double current_loglikelihood = indexedMeasurement(measurement, measurement_cov, index, prior_state, prior_cov, dstate,
       dcov);
 
-#if DEBUG_MODE
+#if DEBUG_MODE_IM
   std::cout << "dstat: " << dstate <<"\n";
   std::cout << " dcov: " << dcov <<"\n";
   std::cout << dstate.position() << " pos\n";
@@ -146,7 +147,7 @@ void RBISIndexedPlusOrientationMeasurement::updateFilter(const RBIS & prior_stat
                                                                 prior_cov,
                                                                 dstate,
                                                                 dcov);
-#if true
+#if false
   std::cerr << "======================================" << std::endl;
   std::cerr << sensorIdToString(sensor_id)  << std::endl;
   std::cerr << "======================================" << std::endl;
@@ -157,7 +158,7 @@ void RBISIndexedPlusOrientationMeasurement::updateFilter(const RBIS & prior_stat
 #endif
   rbisApplyDelta(prior_state, prior_cov, dstate, dcov, posterior_state, posterior_covariance);
 
-#if true
+#if false
   std::cerr << "    Prior velocity: " << prior_state.velocity().transpose().format(CleanFmt) << std::endl;
   std::cerr << "Posterior velocity: " << posterior_state.velocity().transpose().format(CleanFmt) << std::endl;
   std::cerr << "    Prior position: " << prior_state.position().transpose().format(CleanFmt) << std::endl;

diff --git a/pronto_ros/include/pronto_ros/ros_frontend.hpp b/pronto_ros/include/pronto_ros/ros_frontend.hpp
@@ -20,6 +20,7 @@
 #include <cxxabi.h>
 #include <nav_msgs/msg/path.hpp>
 #include <eigen3/Eigen/Eigen>
+#include "rclcpp/qos.hpp"
 
 template<typename T>
 std::string type_name()
@@ -84,10 +85,12 @@ class ROSFrontEnd {
         if (!subscribe) {
             return;
         }
+        rclcpp::QoS qos(10);
+        qos.reliability(rclcpp::ReliabilityPolicy::BestEffort);
         RCLCPP_INFO_STREAM(nh_->get_logger(), sensor_id << " subscribing to " << topic
                                                       << " with SecondaryMsgT = " << type_name<SecondaryMsgT>());
         secondary_subscribers_[sensor_id] = nh_->create_subscription<SecondaryMsgT>(
-            topic, 10000,
+            topic, qos,
             [this, sensor_id](typename SecondaryMsgT::UniquePtr msg) {
                 this->secondaryCallback<MsgT, SecondaryMsgT>(std::move(msg), sensor_id);
             });
@@ -188,7 +191,8 @@ void ROSFrontEnd::addInitModule(SensingModule<MsgT>& module,
     }
     RCLCPP_INFO_STREAM(nh_->get_logger(), "Sensor init id: " << sensor_id);
     RCLCPP_INFO_STREAM(nh_->get_logger(), "Topic: " << topic);
-
+    rclcpp::QoS qos(10);
+    qos.reliability(rclcpp::ReliabilityPolicy::BestEffort);
     // add the sensor to the list of sensor that require initialization
     std::pair<SensorId, bool> init_id_pair(sensor_id, false);
     initialized_list_.insert(init_id_pair);
@@ -201,7 +205,7 @@ void ROSFrontEnd::addInitModule(SensingModule<MsgT>& module,
         RCLCPP_ERROR_STREAM(nh_->get_logger(), sensor_id << " subscribing to " << topic);
         RCLCPP_ERROR_STREAM(nh_->get_logger(), " with MsgT = " << type_name<MsgT>());
         init_subscribers_[sensor_id] = nh_->create_subscription<MsgT>(
-            topic, 10000,
+            topic, qos,
             [this, sensor_id](typename MsgT::UniquePtr msg) {
                 this->initCallback<MsgT>(std::move(msg), sensor_id);
             });
@@ -226,7 +230,8 @@ void ROSFrontEnd::addSensingModule(SensingModule<MsgT>& module,
     RCLCPP_INFO_STREAM(nh_->get_logger(), "Roll forward: " << (roll_forward ? "yes" : "no"));
     RCLCPP_INFO_STREAM(nh_->get_logger(), "Publish head: " << (publish_head ? "yes" : "no"));
     RCLCPP_INFO_STREAM(nh_->get_logger(), "Topic: " << topic);
-
+    rclcpp::QoS qos(10);
+    qos.reliability(rclcpp::ReliabilityPolicy::BestEffort);
     // store the will to roll forward when the message is received
     std::pair<SensorId, bool> roll_pair(sensor_id, roll_forward);
     roll_forward_.insert(roll_pair);
@@ -245,7 +250,7 @@ void ROSFrontEnd::addSensingModule(SensingModule<MsgT>& module,
         RCLCPP_ERROR_STREAM(nh_->get_logger(), sensor_id << " subscribing to " << topic
                                                          << " with MsgT = " << type_name<MsgT>());
         sensors_subscribers_[sensor_id] = nh_->create_subscription<MsgT>(
-            topic, 10000,
+            topic, qos,
             [this, sensor_id](typename MsgT::UniquePtr msg) {
                 this->callback<MsgT>(std::move(msg), sensor_id);
             });

diff --git a/pronto_ros/src/pronto_ros_conversions.cpp b/pronto_ros/src/pronto_ros_conversions.cpp
@@ -130,7 +130,7 @@ void poseMeasurementFromROS(const nav_msgs::msg::Odometry &ros_msg,
 void jointStateFromROS(const sensor_msgs::msg::JointState &ros_msg, JointState &msg)
 {
   // it is caller's responsibility to check that both joint states have the same size
-   msg.utime = ros_msg.header.stamp.nanosec / 1000 + ros_msg.header.stamp.sec * std::pow(10,6);
+   msg.utime = int(ros_msg.header.stamp.nanosec / 1000 )+ ros_msg.header.stamp.sec * std::pow(10,6);
   msg.joint_position = std::move(ros_msg.position);
   msg.joint_velocity = std::move(ros_msg.velocity);
   msg.joint_effort = std::move(ros_msg.effort);

diff --git a/pronto_ros2_node/launch/bench_pronto.launch.py b/pronto_ros2_node/launch/bench_pronto.launch.py
@@ -11,7 +11,7 @@
 def generate_launch_description():
 
 
-    pronto_instance = ['1','2','3']
+    pronto_instance = ['2']
 
     bag_name = "Exp_2023_11_22_01_19_54"
     exp_name = os.path.join("bags",bag_name,bag_name+"_0.mcap")

diff --git a/pronto_ros2_node/src/wheeled_odometry.cpp b/pronto_ros2_node/src/wheeled_odometry.cpp
@@ -10,23 +10,23 @@ namespace pronto{
     {
         int mode_param;
         //declare parameters
-        node_ptr_->declare_parameter<double>(sensor_name + "r_linear",0.0);
+        node_ptr_->declare_parameter<double>(sensor_name + "r_xyz",0.0);
         node_ptr_->declare_parameter<double>(sensor_name + "r_chi",0.0);
         node_ptr_->declare_parameter<int>(sensor_name + "mode",0);
         //get parameters 
         if(! node_ptr_->get_parameter(sensor_name + "r_xyz",r_lin_))
         {
-                RCLCPP_ERROR(node_ptr_->get_logger(),"error in parsing base_link_name parameter, the default name [%f] will be use",DEFAULT_R_LIN);
+                RCLCPP_ERROR(node_ptr_->get_logger(),"error in parsing r_xyz parameter, the default name [%f] will be use",DEFAULT_R_LIN);
                 r_lin_ = DEFAULT_R_LIN;
         }
         if(! node_ptr_->get_parameter(sensor_name + "r_chi",r_ang_))
         {
-                RCLCPP_ERROR(node_ptr_->get_logger(),"error in parsing base_link_name parameter, the default name [%f] will be use",DEFAULT_R_ANG);
+                RCLCPP_ERROR(node_ptr_->get_logger(),"error in parsing r_chi parameter, the default name [%f] will be use",DEFAULT_R_ANG);
                 r_ang_ = DEFAULT_R_ANG;
         }
         if(! node_ptr_->get_parameter(sensor_name + "mode",mode_param))
         {
-                RCLCPP_ERROR(node_ptr_->get_logger(),"error in parsing base_link_name parameter, the default name [%d] will be use",(int)DEFAULT_MODE);
+                RCLCPP_ERROR(node_ptr_->get_logger(),"error in parsing mode parameter, the default name [%d] will be use",(int)DEFAULT_MODE);
                 mode_ = OdomMode::MODE_BOTH;
         }
         else
@@ -61,12 +61,14 @@ namespace pronto{
             break;
         default:
             break;
+
         }
+        RCLCPP_ERROR_STREAM(node_ptr_->get_logger(),"the z_indexes are "<<z_indices_);
     };
 
     RBISUpdateInterface* WheeledOdometry::processMessage(const OdomMsg* msg,StateEstimator *est )
     {
-        uint64_t utime = msg->header.stamp.sec * std::pow(10,6) + msg->header.stamp.nanosec /1000;
+        uint64_t utime = msg->header.stamp.sec * std::pow(10,6) + int(msg->header.stamp.nanosec /1000);
 
         switch (mode_)
         {

diff --git a/pronto_tuning/launch/omnicar_exp.launch.py b/pronto_tuning/launch/omnicar_exp.launch.py
@@ -20,7 +20,7 @@ def generate_launch_description():
 
     exp_name_value = LaunchConfiguration("exp_name")
     package_path = FindPackageShare("pronto_tuning")
-    exp_name_arg = DeclareLaunchArgument("exp_name",default_value="Exp_1_cmd.yaml")
+    exp_name_arg = DeclareLaunchArgument("exp_name",default_value="Exp_o_cmd.yaml")
 
 
     #start qualisys
@@ -40,13 +40,14 @@ def generate_launch_description():
 
     bag_base_path = "/home/ros/docker_pronto_ws/bags/"
     #starts controller 
-    omni_controller_spawner = Node(
-    package="controller_manager",
-    executable="spawner",
-    arguments=["omni_controller", "--controller-manager", "/controller_manager"],
-    )   
+    IMU_Broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["IMU_Broadcaster", "--controller-manager", "/controller_manager"],
+    )
+
 
-    ordered_omni_controller_spawner = TimerAction(actions=[omni_controller_spawner],period=1.0)
+    ordered_IMU_Broadcaster_spawner = TimerAction(actions=[IMU_Broadcaster_spawner],period=1.0)
 
     state_broadcaster_spawner = Node(
     package="controller_manager",
@@ -56,18 +57,18 @@ def generate_launch_description():
 
     ordered_state_broadcaster_spawner = RegisterEventHandler(
 
-        OnProcessExit(target_action=omni_controller_spawner, on_exit=[state_broadcaster_spawner])
+        OnProcessExit(target_action=IMU_Broadcaster_spawner, on_exit=[state_broadcaster_spawner])
     )
 
-    IMU_Broadcaster_spawner = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["IMU_Broadcaster", "--controller-manager", "/controller_manager"],
-    )
 
-    ordered_IMU_Broadcaster_spawner = RegisterEventHandler(
+    omni_controller_spawner = Node(
+    package="controller_manager",
+    executable="spawner",
+    arguments=["omni_controller", "--controller-manager", "/controller_manager"],
+    )  
+    ordered_omni_controller_spawner = RegisterEventHandler(
 
-        OnProcessExit(target_action=state_broadcaster_spawner, on_exit=[IMU_Broadcaster_spawner])
+        OnProcessExit(target_action=state_broadcaster_spawner, on_exit=[omni_controller_spawner])
     )
 
     rf2o_odom = IncludeLaunchDescription(