Skip to content

Commit

Permalink
v.0.4.1 beta
Browse files Browse the repository at this point in the history
  • Loading branch information
@szepilot
szepilot committed Dec 11, 2024
1 parent 310840f commit 7e8d708
Show file tree
Hide file tree
Showing 5 changed files with 212 additions and 42 deletions.
41 changes: 38 additions & 3 deletions launch/kalman_pos_duro1lexus.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,18 +12,21 @@ def generate_launch_description():
{"gnss_pose_topic" : "/lexus3/gps/duro/current_pose"},
{"slam_pose_topic" : "/lexus3/gps/duro/current_pose"},
{"vehicle_status_topic" : "/lexus3/vehicle_status"},
{"nav_sat_fix_topic" : "gps/duro/fix"},
{"gnss_covariance_topic" : "gps/duro/fix"},
{"slam_covariance_topic" : "gps/duro/fix"},
{"imu_topic" : "/lexus3/gps/duro/imu"},
{"est_cog_topic" : "estimated_pose_cog"},
{"est_baselink_topic" : "estimated_pose_baselink"},
{"est_accuracy_topic" : "estimation_accuracy"},
{"est_trav_distance_odom_topic" : "distance"},
{"est_trav_distance_est_pos_topic" : "estimated_trav_dist_est_pos"},
{"loop_rate_hz" : 60},
{"gnss_available", False}
{"slam_available", False}
{"gnss_available", False},
{"slam_available", False},
{"gnss_accuracy_limit" : 10.0},
{"slam_accuracy_limit" : 10.0},
{"gnss_default_covariance" : 15.0},
{"slam_default_covariance" : 15.0},
{"dynamic_time_calc" : True},
{"do_not_wait_for_gnss_msgs" : True},
{"kinematic_model_max_speed" : 0.3},
Expand All @@ -39,6 +42,38 @@ def generate_launch_description():
{"vehicle_param_swr" : 1.0}
]
),
Node(
package='kalman_pos',
executable='vehicle_status_convert',
output='screen',
parameters=[
{"speed_topic": "/nissan/vehicle_speed"},
{"steer_topic": "/nissan/vehicle_steering"},
{"status_topic": "/nissan/vehicle_status"},
]
),
Node(
package='tf2_ros',
executable='static_transform_publisher',
name='tf_imu_ned_enu',
output='screen',
arguments=['1.5385', '0.0', '-0.325', '0.0', '0', '0.0', 'imu_link_ned', 'duro'] # https://github.com/szenergy/szenergy-public-resources/wiki/H-sensorset2020.A
),
Node(
package='imu_transformer',
executable='imu_transformer_node',
name='imu_data_transformer',
output='screen',
remappings=[
('imu_in', '/nissan/gps/duro/imu'),
('imu_out', '/nissan/gps/duro/imu_cog'),
('mag_in', '/nissan/gps/duro/mag'),
('mag_out', '/nissan/gps/duro/mag_cog')
],
parameters=[
{'target_frame': 'duro'}
]
),
])


80 changes: 80 additions & 0 deletions launch/kalman_pos_nova1lexus.launch.py
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,80 @@
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
return LaunchDescription([
Node(
package="kalman_pos",
executable='kalman_pos_node',
output='screen',
parameters=[
{"gnss_pose_topic": "/lexus3/gps/nova/current_pose"},
{"slam_pose_topic": "/lexus3/gps/duro/current_pose"},
{"vehicle_status_topic": "/lexus3/vehicle_status"},
{"gnss_covariance_topic": "/lexus3/gps/nova/fix"},
{"slam_covariance_topic": "gps/duro/fix"},
{"imu_topic": "/lexus3/gps/nova/imu"}, # cog
{"est_cog_topic": "estimated_pose_cog"},
{"est_baselink_topic": "estimated_pose_baselink"},
{"est_accuracy_topic": "estimation_accuracy"},
{"est_trav_distance_odom_topic": "distance"},
{"est_trav_distance_est_pos_topic": "estimated_trav_dist_est_pos"},
{"loop_rate_hz": 60},
{"gnss_available": False},
{"slam_available": False},
{"gnss_accuracy_limit": 10.0},
{"slam_accuracy_limit": 10.0},
{"gnss_default_covariance": 15.0},
{"slam_default_covariance": 15.0},
{"dynamic_time_calc": True},
{"do_not_wait_for_gnss_msgs": True},
{"kinematic_model_max_speed": 10.1},
{"use_raw_model": False},
{"orientation_est_enabled": False},
{"msg_timeout": 2000.0},
{"vehicle_param_c1": 30000.0},
{"vehicle_param_c2": 30000.0},
{"vehicle_param_m": 1800.0},
{"vehicle_param_jz": 2700.0},
{"vehicle_param_l1": 1.2},
{"vehicle_param_l2": 1.589},
{"vehicle_param_swr": 1.0}
]
),
# Node(
# package='kalman_pos',
# executable='vehicle_status_convert',
# output='screen',
# parameters=[
# {"speed_topic": "/lexus3/vehicle_speed"},
# {"steer_topic": "/lexus3/vehicle_steering"},
# {"status_topic": "/lexus3/vehicle_status"},
# ]
# ),
# Node(
# package='tf2_ros',
# executable='static_transform_publisher',
# name='tf_imu_ned_enu',
# output='screen',
# # https://github.com/szenergy/szenergy-public-resources/wiki/H-sensorset2020.A
# # https://github.com/szenergy/szenergy-public-resources/wiki/H-sensorset2022.L
# arguments=['1.589', '0.0', '-0.325', '0.0',
# '0', '0.0', 'imu_link_ned', 'nova'], # ???
# ),
# TODO: debug, Erno
# Node(
# package='imu_transformer',
# executable='imu_transformer_node',
# name='imu_data_transformer',
# output='screen',
# remappings=[
# ('imu_in', '/lexus3/gps/nova/imu'),
# ('imu_out', '/lexus3/gps/nova/imu_cog'),
# # ('mag_in', '/lexus3/gps/nova/mag'),
# # ('mag_out', '/lexus3/gps/nova/mag_cog')
# ],
# parameters=[
# {'target_frame': 'nova'}, # ???
# ]
# ),
])
4 changes: 2 additions & 2 deletions src/CombinedVehicleModel.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,10 +43,10 @@ void cCombinedVehicleModel::initEKFMatrices(void) {

iPDyn_wEKFwExtPos1Pos2_m = matrix<double>(5, 5);
iQDyn_wEKFwExtPos1Pos2_m = matrix<double>(5, 5);
iRDyn_wEKFwExtPos1Pos2_m = matrix<double>(5, 5);
iRDyn_wEKFwExtPos1Pos2_m = matrix<double>(7, 7);
iPKin_wEKFwExtPos1Pos2_m = matrix<double>(5, 5);
iQKin_wEKFwExtPos1Pos2_m = matrix<double>(5, 5);
iRKin_wEKFwExtPos1Pos2_m = matrix<double>(5, 5);
iRKin_wEKFwExtPos1Pos2_m = matrix<double>(7, 7);

iPDyn_wEKFwoExtPos_m = matrix<double>(5, 5);
iQDyn_wEKFwoExtPos_m = matrix<double>(5, 5);
Expand Down
16 changes: 14 additions & 2 deletions src/PositionEstimation.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
#include "PositionEstimation.h"

#include <sys/time.h>
#include <algorithm>
#include <boost/date_time/posix_time/ptime.hpp>
#include "tf2/transform_datatypes.h"

Expand All @@ -17,6 +18,7 @@ cPositionEstimation::~cPositionEstimation() {
}

void cPositionEstimation::initEstimation(bool pDynamicTimeCalcEnabled_b, int pLoopRateHz_i32, sVehicleParameters &pVehicleParameters_s, float pKinematicModelMaxSpeed_f, bool pOriEstimationEnabled_b ) {
std::cout << "initEstimation\n";
iFirstIteration_b = true;
iLoopRateHz_i32 = pLoopRateHz_i32;
iTs_d = 1 / iLoopRateHz_i32;
Expand Down Expand Up @@ -53,7 +55,7 @@ void cPositionEstimation::initEstimation(bool pDynamicTimeCalcEnabled_b, int pLo
iKinSpeedLimit_d = pKinematicModelMaxSpeed_f;
iDefaultKinSpeedLimit_d = pKinematicModelMaxSpeed_f;

iAccuracyScaleFactor_d = 10;
iAccuracyScaleFactor_d = 20;
}

void cPositionEstimation::setMeasuredValuesVehicleState(double pSteeringAngle_d, double pVehicleSpeed_d) {
Expand Down Expand Up @@ -214,7 +216,17 @@ void cPositionEstimation::iterateEstimation(bool pUseRawModel_b, bool pGNSSAvail
iPrevSLAMMeasPosX_d = iCombinedVehicleModel_cl.iMeasuredValues_s.iPosition2X_d;
iPrevSLAMMeasPosY_d = iCombinedVehicleModel_cl.iMeasuredValues_s.iPosition2Y_d;
iPrevEstPosX_d = lCurrentModelStates_st.iPositionX_d;
iPrevEstPosY_d = lCurrentModelStates_st.iPositionY_d;
iPrevEstPosY_d = lCurrentModelStates_st.iPositionY_d;

iAccuracyScaleFactor_d = 20;
if (pGNSSAvailable_b) {
iAccuracyScaleFactor_d = std::max((double)iAccuracyScaleFactor_d, pGNSSCovariance_da[0]);
iAccuracyScaleFactor_d = std::max((double)iAccuracyScaleFactor_d, pGNSSCovariance_da[1]);
}
if (pGNSSAvailable_b) {
iAccuracyScaleFactor_d = std::max((double)iAccuracyScaleFactor_d, pSLAMCovariance_da[0]);
iAccuracyScaleFactor_d = std::max((double)iAccuracyScaleFactor_d, pSLAMCovariance_da[1]);
}
}

void cPositionEstimation::getModelStates(sModelStates* pOutModelStates_s) {
Expand Down
Loading

0 comments on commit 7e8d708

Please sign in to comment.