diff --git a/src/common/common_meta/launch/acceleration_launch.py b/src/common/common_meta/launch/acceleration_launch.py index 5dcc1187..b6039723 100644 --- a/src/common/common_meta/launch/acceleration_launch.py +++ b/src/common/common_meta/launch/acceleration_launch.py @@ -27,7 +27,8 @@ def generate_launch_description(): ) return LaunchDescription([ - create_node(pkg='pc_supervisor', params=[{'mission_name': 'acceleration', 'sim_mode': False}]), + rosbag_record, + # create_node(pkg='pc_supervisor', params=[{'mission_name': 'acceleration', 'sim_mode': False}]), IncludeLaunchDescription( PythonLaunchDescriptionSource(rslidar_launch)), # IncludeLaunchDescription( @@ -35,7 +36,7 @@ def generate_launch_description(): create_node(pkg='can_interface'), create_node(pkg='epos_interface'), create_node(pkg='perception', - params=[{'slow_straight_cropping': True, + params=[{'slow_straight_cropping': False, 'max_x_fov': 50.0, 'max_y_fov': 7.0, 'max_x_clip': 150.0, @@ -63,7 +64,6 @@ def generate_launch_description(): # '/launch/foxglove_bridge_launch.xml' # ]) # ), - # rosbag_record, Node(package='common_meta', executable='param_dumper_exec', name='param_dumper'), ]) diff --git a/src/common/common_meta/launch/trackdrive_launch.py b/src/common/common_meta/launch/trackdrive_launch.py index 7e065e03..841cc908 100644 --- a/src/common/common_meta/launch/trackdrive_launch.py +++ b/src/common/common_meta/launch/trackdrive_launch.py @@ -31,22 +31,34 @@ def generate_launch_description(): create_node(pkg='perception'), create_node(pkg='path_planning', params=[{'v_max': 7.0, - 'ax_max': 5.0, - 'ay_max': 5.0}]), + 'ax_max': 3.5, + 'ay_max': 3.5}]), create_node(pkg='controller', params=[{'look_ahead_distance': 5.0, 'optimized_look_ahead_distance': 7.0, - 'min_cmd': -5.0, - 'max_cmd': 6.0, + 'min_cmd': -4.0, + 'max_cmd': 4.0, 'use_optimized_trajectory': True, 'first_lap_steer_control': "PP", 'optimized_steer_control': "MPC", + 'use_variable_look_ahead_distance' : True, + 'min_look_ahead_distance' : 4.0, + 'max_look_ahead_distance ' : 8.0, + 'lambda_vx' : 0.9, + 'lambda_k' : 1.0, + 'cost_lateral_error': 15.0, + 'cost_angular_error':5.0, + 'cost_steering_delta' : 2000.0, 'auto_start': False}]), create_node(pkg='graph_slam', params=[{'finish_line_offset': 10.0, 'add_big_cones': False, 'x_big_cone': 7.85, - 'y_big_cone': 1.5}]), + 'y_big_cone': 1.5, + 'store_map' : True, + 'store_opt_traj': True, + 'load_map' : False, + 'load_opt_traj': False}]), create_node(pkg='car_state', params=[{'simulation': False, 'mission': 'trackdrive', @@ -54,9 +66,10 @@ def generate_launch_description(): create_node(pkg='trajectory_optimization', params=[{'v_max': 10.0, 'd_min': 1.3, - 'mu_y': 0.6, - 'mu_throttle': 0.5, - 'mu_brake': 0.3}]), + 'mu_y': 0.4, + 'mu_throttle': 0.4, + 'mu_brake': 0.4, + 'first_lap_optimization_method': "none"}]), create_node(pkg='visualization'), # IncludeLaunchDescription( # AnyLaunchDescriptionSource([ diff --git a/src/interfaces/epos_interface/include/epos_interface/epos_interface_node.hpp b/src/interfaces/epos_interface/include/epos_interface/epos_interface_node.hpp index bce4a6b7..ff7ffab2 100644 --- a/src/interfaces/epos_interface/include/epos_interface/epos_interface_node.hpp +++ b/src/interfaces/epos_interface/include/epos_interface/epos_interface_node.hpp @@ -87,4 +87,4 @@ class EPOS_interface : public rclcpp::Node * @brief Function to disable and close the EPOS4 controller. */ void clean_and_close(); -}; +}; \ No newline at end of file diff --git a/src/interfaces/epos_interface/src/epos_interface_node.cpp b/src/interfaces/epos_interface/src/epos_interface_node.cpp index ebe18835..f7256b67 100644 --- a/src/interfaces/epos_interface/src/epos_interface_node.cpp +++ b/src/interfaces/epos_interface/src/epos_interface_node.cpp @@ -146,6 +146,4 @@ int main(int argc, char *argv[]) rclcpp::spin(EPOS_interface_node); rclcpp::shutdown(); return 0; -} - - +} \ No newline at end of file diff --git a/src/perception/config/perception_config.yaml b/src/perception/config/perception_config.yaml index a9700455..255b4cac 100644 --- a/src/perception/config/perception_config.yaml +++ b/src/perception/config/perception_config.yaml @@ -15,7 +15,7 @@ perception: clipping_box_values_topic: "/perception/clipping_box_values" wait_driving: false # Wait for AS_DRIVING status before processing - use_artificial_timestamp: false # Use the node's current time as timestamp instead of the one from the LiDAR messages + use_artificial_timestamp: true # Use the node's current time as timestamp instead of the one from the LiDAR messages # Deskewing deskewing: true # Motion compensation within each LiDAR scan using point timestamps @@ -30,10 +30,10 @@ perception: # Clipping parameters clipping: true # Enable clipping of the point cloud - max_x_clip: 50.0 + max_x_clip: 40.0 max_y_clip: 5.0 - min_x_clip: -30.0 - min_y_clip: -80.0 + min_x_clip: -25.0 + min_y_clip: -72.0 max_z_clip: 3.0 min_z_clip: -10.0 diff --git a/src/slam/graph_slam/src/graph_slam_node.cpp b/src/slam/graph_slam/src/graph_slam_node.cpp index 4fa19b1d..b6acc555 100644 --- a/src/slam/graph_slam/src/graph_slam_node.cpp +++ b/src/slam/graph_slam/src/graph_slam_node.cpp @@ -983,7 +983,8 @@ g2o::VertexSE2* GraphSlam::get_last_valid_pose() { return pose_vertices_[i]; } } - return nullptr; + auto v = new g2o::VertexSE2(); + return v; }