Please enter the commit message for your changes. Lines starting

with '#' will be ignored, and an empty message aborts the commit. On branch master Initial commit Changes to be committed: new file: bashes/README.md new file: bashes/radar-driver-switch.sh new file: dependencies/dependencies.txt new file: src/LSLIDAR_X_ROS2-20240228/src/README.md new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/CMakeLists.txt new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/include/lslidar_driver/input.h new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/include/lslidar_driver/lsiosr.h new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/include/lslidar_driver/lslidar_driver.h new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lslidar_double_launch.py new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10_net_launch.py new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10_uart_launch.py new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10p_net_launch.py new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsm10p_uart_launch.py new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10_launch.py new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10_net_launch.py new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10p_launch.py new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/lsn10p_net_launch.py new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/launch/viewer_scan_launch.py new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/package.xml new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsm10_net.yaml new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsm10p_net.yaml new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsn10_net.yaml new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_net_ros2/lsn10p_net.yaml new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsm10.yaml new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsm10_p.yaml new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsn10.yaml new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsn10p.yaml new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/rviz/lslidar.rviz new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/input.cc new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lsiosr.cpp new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver.cc new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/src/lslidar_driver_node.cc new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/CMakeLists.txt new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarDifop.msg new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarPacket.msg new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarPoint.msg new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarScan.msg new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/msg/LslidarSweep.msg new file: src/LSLIDAR_X_ROS2-20240228/src/lslidar_msgs/package.xml new file: src/LSLIDAR_X_ROS2-20240228/src/version.txt new file: src/LSLIDAR_X_ROS2-20240228/src/wheeltec_udev.sh new file: "src/LSLIDAR_X_ROS2-20240228/src/\351\225\255\347\245\236Lsx\351\233\267\350\276\276\346\227\213\350\275\254\350\247\222\345\272\246.png" new file: src/LSLIDAR_X_ROS2-20240228/wheeltec_lidar.launch.py new file: "src/LSLIDAR_X_ROS2-20240228/wheeltec_lidar.launch.py\344\273\205\345\234\250WHEELTEC\351\225\234\345\203\217\344\270\255\344\275\277\347\224\250" new file: src/cyy_navigation2/CMakeLists.txt new file: src/cyy_navigation2/bt/follow_point.xml new file: src/cyy_navigation2/bt/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml new file: src/cyy_navigation2/bt/navigate_through_poses_w_replanning_and_recovery.xml new file: src/cyy_navigation2/bt/navigate_to_pose_w_replanning_and_recovery.xml new file: src/cyy_navigation2/bt/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml new file: src/cyy_navigation2/bt/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml new file: src/cyy_navigation2/bt/navigate_w_replanning_distance.xml new file: src/cyy_navigation2/bt/navigate_w_replanning_only_if_goal_is_updated.xml new file: src/cyy_navigation2/bt/navigate_w_replanning_only_if_path_becomes_invalid.xml new file: src/cyy_navigation2/bt/navigate_w_replanning_speed.xml new file: src/cyy_navigation2/bt/navigate_w_replanning_time.xml new file: src/cyy_navigation2/bt/odometry_calibration.xml new file: src/cyy_navigation2/config/nav2_params.yaml new file: src/cyy_navigation2/launch/car_bringup.launch.py new file: src/cyy_navigation2/launch/cyy_nav.launch.py new file: src/cyy_navigation2/launch/cyy_nav_box.launch.py new file: src/cyy_navigation2/maps/cyy_map.data new file: src/cyy_navigation2/maps/cyy_map.pgm new file: src/cyy_navigation2/maps/cyy_map.yaml new file: src/cyy_navigation2/maps/cyy_map1.pgm new file: src/cyy_navigation2/maps/cyy_map1.yaml new file: src/cyy_navigation2/package.xml new file: src/cyy_navigation2/param/nav2_params.yaml new file: src/cyy_navigation2/param/slam_toolbox_localization.yaml new file: src/cyy_slamtoolbox/CMakeLists.txt new file: src/cyy_slamtoolbox/config/angular_filter_example.yaml new file: src/cyy_slamtoolbox/config/box_filter_example.yaml new file: src/cyy_slamtoolbox/config/footprint_filter_example.yaml new file: src/cyy_slamtoolbox/config/intensity_filter_example.yaml new file: src/cyy_slamtoolbox/config/laser_filter_config.yaml new file: src/cyy_slamtoolbox/config/mapper_params_lifelong.yaml new file: src/cyy_slamtoolbox/config/mapper_params_localization.yaml new file: src/cyy_slamtoolbox/config/mapper_params_offline.yaml new file: src/cyy_slamtoolbox/config/mapper_params_online_async.yaml new file: src/cyy_slamtoolbox/config/mapper_params_online_sync.yaml new file: src/cyy_slamtoolbox/config/mask_filter_example.yaml new file: src/cyy_slamtoolbox/config/median_filter_example.yaml new file: src/cyy_slamtoolbox/config/median_spatial_filter_example.yaml new file: src/cyy_slamtoolbox/config/multiple_filters_example.yaml new file: src/cyy_slamtoolbox/config/pass_through_example.yaml new file: src/cyy_slamtoolbox/config/polygon_filter_example.yaml new file: src/cyy_slamtoolbox/config/range_filter_example.yaml new file: src/cyy_slamtoolbox/config/scan_blob_filter_example.yaml new file: src/cyy_slamtoolbox/config/sector_filter_example.yaml new file: src/cyy_slamtoolbox/config/shadow_filter_example.yaml new file: src/cyy_slamtoolbox/config/slam_toolbox_default.rviz new file: src/cyy_slamtoolbox/config/speckle_filter_example.yaml new file: src/cyy_slamtoolbox/launch/cyy_slam_toolbox_launch.launch.py new file: src/cyy_slamtoolbox/launch/cyy_slam_toolbox_location.launch.py new file: src/cyy_slamtoolbox/launch/filter.launch.py new file: src/cyy_slamtoolbox/package.xml new file: src/gc_navigation_fish/CMakeLists.txt new file: src/gc_navigation_fish/launch/gc_navigation.launch.py new file: src/gc_navigation_fish/maps/test_map.pgm new file: src/gc_navigation_fish/maps/test_map.yaml new file: src/gc_navigation_fish/package.xml new file: src/gc_navigation_fish/param/gc_navigation.yaml new file: src/gc_navigation_fish/param/navigation_test.yaml new file: src/gc_slam_toolbox_fish/CMakeLists.txt new file: src/gc_slam_toolbox_fish/config/gc_2d.lua new file: src/gc_slam_toolbox_fish/launch/catograph.launch.py new file: src/gc_slam_toolbox_fish/package.xml new file: src/origincar_base/CMakeLists.txt new file: src/origincar_base/config/ekf.yaml new file: src/origincar_base/config/imu.yaml new file: src/origincar_base/include/origincar_base/Quaternion_Solution.h new file: src/origincar_base/include/origincar_base/origincar_base.h new file: src/origincar_base/launch/__pycache__/testtwo.launch.cpython-38.pyc new file: src/origincar_base/launch/base_serial.launch.py new file: src/origincar_base/launch/ekf.launch.py new file: src/origincar_base/launch/origincar_bringup.launch.py new file: src/origincar_base/launch/robot_mode_description.launch.py new file: src/origincar_base/msg/Position.msg new file: src/origincar_base/package.xml new file: src/origincar_base/scripts/cmd_vel_to_ackermann_drive.py new file: src/origincar_base/src/Quaternion_Solution.cpp new file: src/origincar_base/src/origincar_base.cpp new file: src/origincar_description/CMakeLists.txt new file: src/origincar_description/CMakeLists.txt.save new file: src/origincar_description/config/joint_names_origincar_description.yaml new file: "src/origincar_description/config/joint_names_origincar_description.yaml\357\200\272Zone.Identifier" new file: src/origincar_description/launch/display.launch new file: src/origincar_description/launch/display.launch.py new file: src/origincar_description/launch/gazebo.launch new file: src/origincar_description/launch/gazebo.launch.py new file: src/origincar_description/meshes/base_link.STL new file: "src/origincar_description/meshes/base_link.STL\357\200\272Zone.Identifier" new file: src/origincar_description/meshes/down_left_Link.STL new file: "src/origincar_description/meshes/down_left_Link.STL\357\200\272Zone.Identifier" new file: src/origincar_description/meshes/down_right_Link.STL new file: "src/origincar_description/meshes/down_right_Link.STL\357\200\272Zone.Identifier" new file: src/origincar_description/meshes/up_left_Link.STL new file: "src/origincar_description/meshes/up_left_Link.STL\357\200\272Zone.Identifier" new file: src/origincar_description/meshes/up_right_Link.STL new file: "src/origincar_description/meshes/up_right_Link.STL\357\200\272Zone.Identifier" new file: src/origincar_description/package.xml new file: src/origincar_description/rviz/README new file: src/origincar_description/rviz/display.rviz new file: src/origincar_description/urdf/origincar.urdf new file: src/origincar_description/urdf/origincar.xacro new file: src/origincar_description/world/fishbot.world new file: src/origincar_description/world/gc_world.world new file: src/origincar_description/world/test.world new file: src/origincar_msg/CMakeLists.txt new file: src/origincar_msg/msg/Data.msg new file: src/origincar_msg/msg/Sign.msg new file: src/origincar_msg/package.xml new file: src/zbw_slamtoolbox/CMakeLists.txt new file: src/zbw_slamtoolbox/config/mapper_params_online_async.yaml new file: src/zbw_slamtoolbox/config/mapper_params_online_sync copy.yaml new file: src/zbw_slamtoolbox/config/navigation.yaml new file: src/zbw_slamtoolbox/launch/navigation.launch.py new file: src/zbw_slamtoolbox/launch/slamtoolbox.launch.py new file: src/zbw_slamtoolbox/package.xml new file: zbw_slamtoolbox/CMakeLists.txt new file: zbw_slamtoolbox/config/mapper_params_online_async.yaml new file: zbw_slamtoolbox/config/mapper_params_online_sync copy.yaml new file: zbw_slamtoolbox/config/navigation.yaml new file: zbw_slamtoolbox/launch/navigation.launch.py new file: zbw_slamtoolbox/launch/slamtoolbox.launch.py new file: zbw_slamtoolbox/package.xml Changes not staged for commit: deleted: zbw_slamtoolbox/CMakeLists.txt deleted: zbw_slamtoolbox/config/mapper_params_online_async.yaml deleted: zbw_slamtoolbox/config/mapper_params_online_sync copy.yaml deleted: zbw_slamtoolbox/config/navigation.yaml deleted: zbw_slamtoolbox/launch/navigation.launch.py deleted: zbw_slamtoolbox/launch/slamtoolbox.launch.py deleted: zbw_slamtoolbox/package.xml Untracked files: .gitignore
2026-03-05 10:27:00 +08:00
commit 7fbddb15c2
166 changed files with 12323 additions and 0 deletions
--- a/src/origincar_base/CMakeLists.txt
+++ b/src/origincar_base/CMakeLists.txt
@@ -0,0 +1,89 @@
+cmake_minimum_required(VERSION 3.5)
+project(origincar_base)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+#find_package(actionlib REQUIRED)
+#find_package(actionlib_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+
+find_package(nav2_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(std_srvs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(ackermann_msgs REQUIRED)                         # CHANGE
+find_package(origincar_msg REQUIRED)                         # CHANGE
+find_package(serial REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(origincar_description REQUIRED)
+find_package(robot_localization REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/Position.msg"
+ )
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+include_directories(
+  include
+  ${robot_localization_INCLUDE_DIRS}
+)
+set(origincar_base_node_SRCS
+  src/origincar_base.cpp
+  src/Quaternion_Solution.cpp
+)
+
+add_executable(origincar_base_node src/origincar_base.cpp src/Quaternion_Solution.cpp)
+ament_target_dependencies(origincar_base_node tf2_ros tf2 tf2_geometry_msgs rclcpp std_msgs geometry_msgs robot_localization nav_msgs std_srvs sensor_msgs ackermann_msgs serial origincar_msg origincar_description)
+
+#add_executable(testNode src/test.cpp src/Quaternion_Solution.cpp)
+#ament_target_dependencies(testNode rclcpp std_msgs nav_msgs std_srvs sensor_msgs ackermann_msgs serial origincar_msg)
+
+install(PROGRAMS scripts/cmd_vel_to_ackermann_drive.py DESTINATION lib/${PROJECT_NAME})
+
+install(TARGETS
+origincar_base_node
+
+DESTINATION lib/${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(
+  DIRECTORY launch  config 
+  DESTINATION share/${PROJECT_NAME}
+)
+ament_package()
--- a/src/origincar_base/config/ekf.yaml
+++ b/src/origincar_base/config/ekf.yaml
@@ -0,0 +1,222 @@
+### ekf config file ###
+ekf_filter_node:
+    ros__parameters:
+# 启用通过话题设置初始位姿
+        use_pose_with_covariance_stamped: true  # 允许通过话题重置
+        pose0: /set_pose  # 指定话题名称
+        pose0_config: [true, true, false,  # 使用 x, y
+                      false, false, true]  # 使用 yaw
+        pose0_differential: false
+# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
+# computation until it receives at least one message from one of the inputs. It will then run continuously at the
+# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
+        frequency: 30.0
+
+# The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict
+# cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the
+# filter will generate new output. Defaults to 1 / frequency if not specified.
+        sensor_timeout: 2.0
+
+# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
+# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
+# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
+# by, for example, an IMU. Defaults to false if unspecified.
+        two_d_mode: true
+
+# Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for
+# future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if
+# unspecified.
+        transform_time_offset: 0.0
+
+# Use this parameter to provide specify how long the tf listener should wait for a transform to become available. 
+# Defaults to 0.0 if unspecified.
+        transform_timeout: 0.2
+
+# If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is
+# unhappy with any settings or data.
+        print_diagnostics: false
+
+# Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by
+# debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious
+# effects on the performance of the node. Defaults to false if unspecified.
+        debug: false
+
+# Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path.
+        debug_out_file: /path/to/debug/file.txt
+
+# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
+        publish_tf: true
+
+# Whether to publish the acceleration state. Defaults to false if unspecified.
+        publish_acceleration: false
+
+# REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and
+# earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames.
+# The robot's position in the odom frame will drift over time, but is accurate in the short term and should be
+# continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom
+# frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your
+# robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based
+# localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame.
+# ekf_localization_node and ukf_localization_node are not concerned with the earth frame.
+# Here is how to use the following settings:
+# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
+#     1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of
+#         odom_frame.
+# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set
+#   "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.
+# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates
+# from landmark observations) then:
+#     3a. Set your "world_frame" to your map_frame value
+#     3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state
+#         estimation node from robot_localization! However, that instance should *not* fuse the global data.
+        map_frame: map              # Defaults to "map" if unspecified
+        odom_frame: odom_combined            # Defaults to "odom" if unspecified
+        base_link_frame: base_footprint  # Defaults to "base_link" if unspecified
+        world_frame: odom_combined           # Defaults to the value of odom_frame if unspecified
+
+# The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry,
+# geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped,
+# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0,
+# odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no
+# default values, and must be specified.
+        odom0: odom
+# Each sensor reading updates some or all of the filter's state. These options give you greater control over which
+# values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only
+# want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the
+# values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types
+# do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message
+# has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false
+# if unspecified, effectively making this parameter required for each sensor.
+        odom0_config: [true, false, false,
+                       false, false, false,
+                       true,  true,  false,
+                       false, false, true,
+                       false, false, false]
+
+# If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase
+# the size of the subscription queue so that more measurements are fused.
+        odom0_queue_size: 10
+
+# [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result
+# of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's
+# algorithm.
+        odom0_nodelay: false
+
+# [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under-
+# report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they
+# arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also
+# measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't
+# always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose
+# data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then
+# integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true
+# for twist measurements has no effect.
+        odom0_differential: true
+
+# [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point"
+# for all future measurements. While you can achieve the same effect with the differential paremeter, the key
+# difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before
+# integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true.
+        odom0_relative: false
+
+# [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to
+# control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to
+# numeric_limits<double>::max() if unspecified. It is strongly recommended that these parameters be removed if not
+# required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation.
+# For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying
+# the thresholds.
+#         odom0_pose_rejection_threshold: 5.0
+#         odom0_twist_rejection_threshold: 1.0
+
+        imu0: /imu/data_raw
+        imu0_config: [false, false, false,
+                      false, false, true,
+                      false, false, false,
+                      false, false, true,
+                      false,  false, false]
+
+        imu0_nodelay: false
+        imu0_differential: false
+        imu0_relative: true
+        imu0_queue_size: 10
+        imu0_pose_rejection_threshold: 20.0                 # Note the difference in parameter names
+        imu0_twist_rejection_threshold: 1.542                #
+        imu0_linear_acceleration_rejection_threshold: 10.0  #
+
+# [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set
+# this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame.
+        imu0_remove_gravitational_acceleration: true
+
+# [ADVANCED]  The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no
+# acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During
+# correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be
+# problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When
+# this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially
+# noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance
+# for the velocity variable in question, or decrease the  variance of the variable in question in the measurement
+# itself. In addition, users can also take advantage of the control command being issued to the robot at the time we
+# make the prediction. If control is used, it will get converted into an acceleration term, which will be used during
+# predicition. Note that if an acceleration measurement for the variable in question is available from one of the
+# inputs, the control term will be ignored.
+# Whether or not we use the control input during predicition. Defaults to false.
+        use_control: false
+# Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to
+# false.
+        stamped_control: false
+# The last issued control command will be used in prediction for this period. Defaults to 0.2.
+        control_timeout: 0.2
+# Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw.
+        control_config: [true, false, false, false, false, true]
+# Places limits on how large the acceleration term will be. Should match your robot's kinematics.
+        acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
+# Acceleration and deceleration limits are not always the same for robots.
+        deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
+# If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these
+# gains
+        acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
+# If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these
+# gains
+        deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]
+# [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is
+# exposed as a configuration parameter. This matrix represents the noise we add to the total error after each
+# prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be.
+# However, if users find that a given variable is slow to converge, one approach is to increase the
+# process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error
+# to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are
+# ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if
+# unspecified.
+        process_noise_covariance: [0.05, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+                                   0.0,    0.05, 0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+                                   0.0,    0.0,    0.06, 0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+                                   0.0,    0.0,    0.0,    0.03, 0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+                                   0.0,    0.0,    0.0,    0.0,    0.03, 0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.06, 0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.025, 0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.025, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.04, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
+                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.01, 0.0,    0.0,    0.0,    0.0,    0.0,
+                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.01, 0.0,    0.0,    0.0,    0.0,
+                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.02, 0.0,    0.0,    0.0,
+                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.01, 0.0,    0.0,
+                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.01, 0.0,
+                                   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.015]
+# [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal
+# value (variance) to a large value will result in rapid convergence for initial measurements of the variable in
+# question. Users should take care not to use large values for variables that will not be measured directly. The values
+# are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below
+#if unspecified.
+        initial_estimate_covariance: [1e-9, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
+                                      0.0,    1e-9, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
+                                      0.0,    0.0,    1e-9, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
+                                      0.0,    0.0,    0.0,    1e-9, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
+                                      0.0,    0.0,    0.0,    0.0,    1e-9, 0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
+                                      0.0,    0.0,    0.0,    0.0,    0.0,    1e-9, 0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
+                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1e-9, 0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
+                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1e-9, 0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
+                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1e-9, 0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
+                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1e-9,  0.0,     0.0,     0.0,    0.0,    0.0,
+                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     1e-9,  0.0,     0.0,    0.0,    0.0,
+                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     1e-9,  0.0,    0.0,    0.0,
+                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     1e-9, 0.0,    0.0,
+                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    1e-9, 0.0,
+                                      0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    1e-9]
+
--- a/src/origincar_base/config/imu.yaml
+++ b/src/origincar_base/config/imu.yaml
@@ -0,0 +1,7 @@
+imu_filter_madgwick:
+    ros__parameters:
+        fixed_frame: "base_footprint"
+        use_mag: false
+        publish_tf: false
+        world_frame: "enu"
+        orientation_stddev: 0.05
--- a/src/origincar_base/include/origincar_base/Quaternion_Solution.h
+++ b/src/origincar_base/include/origincar_base/Quaternion_Solution.h
@@ -0,0 +1,10 @@
+
+#ifndef __QUATERNION_SOLUTION_H_
+#define __QUATERNION_SOLUTION_H_
+#include "origincar_base.h"
+float InvSqrt(float number);
+void Quaternion_Solution(float gx, float gy, float gz, float ax, float ay, float az);
+extern volatile float q0, q1, q2, q3;    
+#endif
+
+
--- a/src/origincar_base/include/origincar_base/origincar_base.h
+++ b/src/origincar_base/include/origincar_base/origincar_base.h
@@ -0,0 +1,228 @@
+
+#ifndef _ORIGINCAR_BASE_H_
+#define _ORIGINCAR_BASE_H_
+
+#include <memory>
+#include <inttypes.h>
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
+#include <csignal>
+#include <thread>
+
+#include <iostream>
+#include <string.h>
+#include <string>
+#include <iostream>
+#include <math.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <rcl/types.h>
+#include <sys/stat.h>
+
+#include <serial/serial.h>
+#include <fcntl.h>
+#include <stdbool.h>
+#include <std_msgs/msg/string.hpp>
+#include <std_msgs/msg/float32.hpp>
+#include <std_msgs/msg/int32.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+#include <geometry_msgs/msg/vector3.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "tf2/LinearMath/Transform.h"
+#include "tf2/LinearMath/Quaternion.h"
+#include <tf2_ros/transform_broadcaster.h>
+#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp"
+#include "origincar_msg/msg/data.hpp"
+#include "origincar_msg/msg/sign.hpp"  // 匹配信号发送
+#include <sensor_msgs/msg/imu.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+using namespace std;
+
+
+#define SEND_DATA_CHECK   1          //Send data check flag bits //发送数据校验标志位
+#define READ_DATA_CHECK   0          //Receive data to check flag bits //接收数据校验标志位
+#define FRAME_HEADER      0X7B       //Frame head //帧头
+#define FRAME_TAIL        0X7D       //Frame tail //帧尾
+#define RECEIVE_DATA_SIZE 24         //The length of the data sent by the lower computer //下位机发送过来的数据的长度
+#define SEND_DATA_SIZE    11         //The length of data sent by ROS to the lower machine //ROS向下位机发送的数据的长度
+#define PI 				  3.1415926f //PI //圆周率
+
+#define GYROSCOPE_RATIO   0.00026644f
+
+#define ACCEl_RATIO 	  1671.84f
+
+extern sensor_msgs::msg::Imu Mpu6050;
+
+const double odom_pose_covariance[36]   = {1e-3,    0,    0,   0,   0,    0,
+										      0, 1e-3,    0,   0,   0,    0,
+										      0,    0,  1e6,   0,   0,    0,
+										      0,    0,    0, 1e6,   0,    0,
+										      0,    0,    0,   0, 1e6,    0,
+										      0,    0,    0,   0,   0,  1e3 };
+
+const double odom_pose_covariance2[36]  = {1e-9,    0,    0,   0,   0,    0,
+										      0, 1e-3, 1e-9,   0,   0,    0,
+										      0,    0,  1e6,   0,   0,    0,
+										      0,    0,    0, 1e6,   0,    0,
+										      0,    0,    0,   0, 1e6,    0,
+										      0,    0,    0,   0,   0, 1e-9 };
+
+const double odom_twist_covariance[36]  = {1e-3,    0,    0,   0,   0,    0,
+										      0, 1e-3,    0,   0,   0,    0,
+										      0,    0,  1e6,   0,   0,    0,
+										      0,    0,    0, 1e6,   0,    0,
+										      0,    0,    0,   0, 1e6,    0,
+										      0,    0,    0,   0,   0,  1e3 };
+
+const double odom_twist_covariance2[36] = {1e-9,    0,    0,   0,   0,    0,
+										      0, 1e-3, 1e-9,   0,   0,    0,
+										      0,    0,  1e6,   0,   0,    0,
+										      0,    0,    0, 1e6,   0,    0,
+										      0,    0,    0,   0, 1e6,    0,
+										      0,    0,    0,   0,   0, 1e-9} ;
+
+typedef struct __Vel_Pos_Data_
+{
+	float X;
+	float Y;
+	float Z;
+
+} Vel_Pos_Data;
+
+typedef struct __MPU6050_DATA_
+{
+	short accele_x_data;
+	short accele_y_data;
+	short accele_z_data;
+    short gyros_x_data;
+	short gyros_y_data;
+	short gyros_z_data;
+
+} MPU6050_DATA;
+
+typedef struct _SEND_DATA_
+{
+	uint8_t tx[SEND_DATA_SIZE];
+	float X_speed;
+	float Y_speed;
+	float Z_speed;
+	unsigned char Frame_Tail;
+} SEND_DATA;
+
+typedef struct _RECEIVE_DATA_
+{
+	uint8_t rx[RECEIVE_DATA_SIZE];
+	uint8_t Flag_Stop;
+	unsigned char Frame_Header;
+	float X_speed;
+	float Y_speed;
+	float Z_speed;
+	float Power_Voltage;
+	unsigned char Frame_Tail;
+} RECEIVE_DATA;
+
+class origincar_base : public rclcpp::Node
+
+{
+public:
+	origincar_base();
+	~origincar_base();
+	void Control();
+	void Publish_Odom();
+
+public : 
+	serial::Serial Stm32_Serial;
+
+private:
+	void declare_parameters();
+	void get_parameters();
+
+
+	void Cmd_Vel_Callback(const geometry_msgs::msg::Twist::SharedPtr twist_aux);
+	void Akm_Cmd_Vel_Callback(const ackermann_msgs::msg::AckermannDriveStamped::SharedPtr akm_ctl);
+
+	void Publish_ImuSensor();
+	void Publish_Voltage();
+	auto createQuaternionMsgFromYaw(double yaw);
+
+	bool Get_Sensor_Data();
+	unsigned char Check_Sum(unsigned char Count_Number,unsigned char mode);
+	short IMU_Trans(uint8_t Data_High,uint8_t Data_Low);
+	float Odom_Trans(uint8_t Data_High,uint8_t Data_Low);
+
+	void Sign_Switch_Callback(const std_msgs::msg::Int32::SharedPtr sign_switch);
+
+private:
+  void reset_filter() 
+  {
+    auto msg = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>();
+    
+    // 设置头信息
+    msg->header.stamp = this->now();
+    msg->header.frame_id = "odom_combined";
+
+    // 重置位姿
+    msg->pose.pose.position.x = 0.54;
+    msg->pose.pose.position.y = 0.2;
+    msg->pose.pose.orientation.w = 1.0;
+
+    // 设置关键协方差
+    std::array<double, 36> cov = {0};
+    cov[0] = 0.5;  // x
+    cov[7] = 0.5;  // y
+    cov[35] = 0.5; // yaw
+    msg->pose.covariance = cov;
+
+    // 发布重置指令
+    pose_pub_->publish(*msg);
+    RCLCPP_WARN(this->get_logger(), "FILTER RESET TRIGGERED!");
+  }
+
+  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_pub_;
+	rclcpp::Time _Now, _Last_Time;
+	float Sampling_Time;
+	rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr Cmd_Vel_Sub;
+	rclcpp::Subscription<ackermann_msgs::msg::AckermannDriveStamped>::SharedPtr Akm_Cmd_Vel_Sub;
+
+	rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_publisher;
+	rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr voltage_publisher;
+	rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_publisher; 
+
+	rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr test_publisher;
+
+	rclcpp::Publisher<origincar_msg::msg::Data>::SharedPtr robotpose_publisher;
+	rclcpp::Publisher<origincar_msg::msg::Data>::SharedPtr robotvel_publisher;
+	// rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr tf_pub_;
+
+	std::shared_ptr<tf2_ros::TransformBroadcaster> tf_bro;
+	rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr tf_pub_;
+	rclcpp::TimerBase::SharedPtr test_timer;
+
+	rclcpp::TimerBase::SharedPtr odom_timer;
+	rclcpp::TimerBase::SharedPtr imu_timer;
+	rclcpp::TimerBase::SharedPtr voltage_timer;
+
+	rclcpp::TimerBase::SharedPtr robotpose_timer;
+	rclcpp::TimerBase::SharedPtr robotvel_timer;
+
+	std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+
+	rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr Sign_Switch_Sub;
+
+	string usart_port_name, robot_frame_id, gyro_frame_id, odom_frame_id, akm_cmd_vel, test;
+	std::string cmd_vel;
+	int serial_baud_rate;
+	RECEIVE_DATA Receive_Data;
+	SEND_DATA Send_Data;
+
+	Vel_Pos_Data Robot_Pos;
+	Vel_Pos_Data Robot_Vel;
+	MPU6050_DATA Mpu6050_Data;
+	float Power_voltage;
+    size_t count_;
+};
+
+
+#endif //_ORIGINCAR_BASE_H_
--- a/src/origincar_base/launch/pycache/testtwo.launch.cpython-38.pyc
+++ b/src/origincar_base/launch/pycache/testtwo.launch.cpython-38.pyc
--- a/src/origincar_base/launch/base_serial.launch.py
+++ b/src/origincar_base/launch/base_serial.launch.py
@@ -0,0 +1,47 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch.conditions import IfCondition, UnlessCondition
+import launch_ros.actions
+
+def generate_launch_description():
+    akmcar = LaunchConfiguration('akmcar', default='false')
+
+    robot_parameters = [
+        {'usart_port_name': '/dev/ttyACM0',
+         'serial_baud_rate': 115200,
+         'robot_frame_id': 'base_link',
+         'odom_frame_id': 'odom',
+         'cmd_vel': 'cmd_vel',
+         'product_number': 0}
+    ]
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'akmcar',
+            default_value='false',
+            description='Use simulation (Gazebo) clock if true'
+        ),
+
+        launch_ros.actions.Node(
+            condition=IfCondition(akmcar),
+            package='origincar_base',
+            executable='origincar_base_node',
+            parameters=robot_parameters + [{'akm_cmd_vel': 'ackermann_cmd'}],
+            remappings=[('/cmd_vel', 'cmd_vel')],
+        ),
+
+        launch_ros.actions.Node(
+            condition=IfCondition(akmcar),
+            package='origincar_base',
+            executable='cmd_vel_to_ackermann_drive.py',
+            name='cmd_vel_to_ackermann_drive',
+        ),
+
+        launch_ros.actions.Node(
+            condition=UnlessCondition(akmcar),
+            package='origincar_base',
+            executable='origincar_base_node',
+            parameters=robot_parameters + [{'akm_cmd_vel': 'none'}],
+        )
+    ])
--- a/src/origincar_base/launch/ekf.launch.py
+++ b/src/origincar_base/launch/ekf.launch.py
@@ -0,0 +1,33 @@
+#!/usr/bin/python3
+
+import os
+import launch
+import launch_ros
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    
+    package_name = 'origincar_base'
+    ld =  launch.LaunchDescription()
+    pkg_share = launch_ros.substitutions.FindPackageShare(package=package_name).find(package_name) 
+    robot_localization_file_path = os.path.join(pkg_share, 'config/ekf.yaml')
+    use_sim_time = LaunchConfiguration('use_sim_time')
+
+      # Start robot localization using an Extended Kalman filter
+    robot_localization_node = Node(
+      package='robot_localization',
+      executable='ekf_node',
+      name='ekf_filter_node',
+      output='screen',
+      remappings=[("odometry/filtered", "odom_combined")],
+      parameters=[
+          robot_localization_file_path,
+          {'use_sim_time': use_sim_time}
+      ])
+
+    ld.add_action(launch.actions.DeclareLaunchArgument(name='use_sim_time', default_value='false',
+                                            description='Flag to disable use_sim_time'))
+    ld.add_action(robot_localization_node)
+    return ld
--- a/src/origincar_base/launch/origincar_bringup.launch.py
+++ b/src/origincar_base/launch/origincar_bringup.launch.py
@@ -0,0 +1,90 @@
+import os
+from pathlib import Path
+import launch
+from launch.actions import SetEnvironmentVariable
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (DeclareLaunchArgument, GroupAction,
+                            IncludeLaunchDescription, SetEnvironmentVariable)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PythonExpression
+from launch_ros.actions import PushRosNamespace
+import launch_ros.actions
+from launch.conditions import UnlessCondition
+
+def generate_launch_description():
+    # Get the launch directory
+    bringup_dir = get_package_share_directory('origincar_base')
+    launch_dir = os.path.join(bringup_dir, 'launch')
+    ekf_config = Path(get_package_share_directory('origincar_base'), 'config', 'ekf.yaml')
+    imu_config = Path(get_package_share_directory('origincar_base'), 'config', 'imu.yaml')
+
+    
+    carto_slam = LaunchConfiguration('carto_slam', default='false')
+    carto_slam_dec = DeclareLaunchArgument('carto_slam',default_value='false')
+            
+    origincar_base = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(launch_dir, 'base_serial.launch.py')),
+            launch_arguments={'akmcar': 'false'}.items(),
+    )
+
+    choose_car = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(launch_dir, 'robot_mode_description.launch.py')),
+    )
+
+    
+    base_to_link = launch_ros.actions.Node(
+            package='tf2_ros', 
+            executable='static_transform_publisher', 
+            name='base_to_link',
+            arguments=['0', '0', '0','0', '0','0','base_footprint','base_link'],#arguments=['0.41', '0.12', '0','0', '0','0','base_footprint','base_link'],
+    )
+    
+
+    base_to_gyro = launch_ros.actions.Node(
+            package='tf2_ros', 
+            executable='static_transform_publisher', 
+            name='base_to_gyro',
+            arguments=['0', '0', '0','0', '0','0','base_footprint','gyro_link'],
+    )
+    
+    link_to_laser = launch_ros.actions.Node(
+            package='tf2_ros', 
+            executable='static_transform_publisher', 
+            name='link_to_laser',
+            arguments=['0', '0', '0','0', '0','0','base_link','laser'],
+    )
+
+    imu_filter_node =  launch_ros.actions.Node(
+        package='imu_filter_madgwick',
+        executable='imu_filter_madgwick_node',
+        parameters=[imu_config]
+    )
+    
+    robot_ekf = launch_ros.actions.Node(
+            condition=UnlessCondition(carto_slam),
+            package='robot_localization', 
+            executable='ekf_node', 
+            parameters=[ekf_config],
+            remappings=[("odometry/filtered", "odom_combined")]
+            )
+                              
+    joint_state_publisher_node = launch_ros.actions.Node(
+            package='joint_state_publisher', 
+            executable='joint_state_publisher', 
+            name='joint_state_publisher',
+    )
+
+    ld = LaunchDescription()
+
+    ld.add_action(carto_slam_dec)
+    ld.add_action(origincar_base)
+    ld.add_action(base_to_link)
+    ld.add_action(base_to_gyro)
+    ld.add_action(joint_state_publisher_node)
+    ld.add_action(choose_car)
+    ld.add_action(imu_filter_node)
+    ld.add_action(robot_ekf)
+
+    return ld
+
--- a/src/origincar_base/launch/robot_mode_description.launch.py
+++ b/src/origincar_base/launch/robot_mode_description.launch.py
@@ -0,0 +1,24 @@
+import os
+from pathlib import Path
+import launch_ros.actions
+import launch
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (DeclareLaunchArgument, GroupAction,LogInfo,
+                            IncludeLaunchDescription, SetEnvironmentVariable)
+from launch.substitutions import LaunchConfiguration
+
+def generate_launch_description():
+    origincar_description = GroupAction([
+        launch_ros.actions.Node(
+            package='robot_state_publisher', 
+            executable='robot_state_publisher', 
+            name='robot_state_publisher',
+            arguments=[os.path.join(get_package_share_directory('origincar_description'),'urdf','origincar.urdf')]
+        )
+    ])
+ 
+    ld = LaunchDescription()
+
+    ld.add_action(origincar_description)
+    return ld
--- a/src/origincar_base/msg/Position.msg
+++ b/src/origincar_base/msg/Position.msg
@@ -0,0 +1,3 @@
+float32 angle_x
+float32 angle_y
+float32 distance
--- a/src/origincar_base/package.xml
+++ b/src/origincar_base/package.xml
@@ -0,0 +1,44 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>origincar_base</name>
+  <version>0.0.0</version>
+  <description>ROS2 origincar_base for origincar_base</description>
+  <maintainer email="ps-micro@todo.todo">ps-micro</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <build_export_depend>tf2_geometry_msgs</build_export_depend>
+
+
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+  <build_depend>tf2_geometry_msgs</build_depend>
+
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <depend>ament_index_cpp</depend>
+  <depend>geometry_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>rclpy</depend>
+  <depend>serial</depend>
+  <depend>nav2_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>ackermann_msgs</depend>
+  <depend>origincar_msg</depend>
+  <depend>origincar_description</depend>
+  <depend>robot_localization</depend>
+  
+  <test_depend>boost</test_depend>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
--- a/src/origincar_base/scripts/cmd_vel_to_ackermann_drive.py
+++ b/src/origincar_base/scripts/cmd_vel_to_ackermann_drive.py
@@ -0,0 +1,47 @@
+#!/usr/bin/env python3
+
+import math
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Twist
+from ackermann_msgs.msg import AckermannDriveStamped
+from rclpy.qos import QoSProfile
+
+class CmdVel2AckermannDriveNode(Node):
+    def __init__(self):
+        super().__init__('cmd_vel_to_ackermann_drive')
+        self.publisher = self.create_publisher(AckermannDriveStamped, '/ackermann_cmd', QoSProfile(depth=10))
+        self.subscription = self.create_subscription(Twist, 'cmd_vel', self.cmd_callback, QoSProfile(depth=10))
+        self.wheelbase = 0.143  #轮距
+        self.frame_id = 'odom_combined'
+        self.cmd_angle_instead_rotvel = False
+
+    def convert_trans_rot_vel_to_steering_angle(self, vel, omega):
+        if omega == 0 or vel == 0:
+            return 0
+        radius = vel / omega
+        return math.atan(self.wheelbase / radius)
+
+    def cmd_callback(self, data):
+        vel = data.linear.x
+        if self.cmd_angle_instead_rotvel:
+            steering = data.angular.z
+        else:
+            steering = 1.0 * self.convert_trans_rot_vel_to_steering_angle(vel, data.angular.z)
+
+        msg = AckermannDriveStamped()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.header.frame_id = self.frame_id
+        msg.drive.steering_angle = steering
+        msg.drive.speed = vel
+        self.publisher.publish(msg)
+
+def main():
+    rclpy.init()
+    node = CmdVel2AckermannDriveNode()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()
--- a/src/origincar_base/src/Quaternion_Solution.cpp
+++ b/src/origincar_base/src/Quaternion_Solution.cpp
@@ -0,0 +1,91 @@
+
+#include "origincar_base/Quaternion_Solution.h"
+#define SAMPLING_FREQ 20.0f // 采样频率
+/**************************************
+Date: May 31, 2020
+Function: 平方根倒数 求四元数用到
+***************************************/
+float InvSqrt(float number)
+{
+  volatile long i;
+    volatile float x, y;
+    volatile const float f = 1.5F;
+    x = number * 0.5F;
+    y = number;
+    i = * (( long * ) &y);
+    i = 0x5f375a86 - ( i >> 1 );
+    y = * (( float * ) &i);
+    y = y * ( f - ( x * y * y ) );
+
+  return y;
+}
+/**************************************
+Date: May 31, 2020
+Function: 四元数解算
+***************************************/
+volatile float twoKp = 1.0f;     // 2 * proportional gain (Kp)
+volatile float twoKi = 0.0f;     // 2 * integral gain (Ki)
+volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;          // quaternion of sensor frame relative to auxiliary frame
+volatile float integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki
+void Quaternion_Solution(float gx, float gy, float gz, float ax, float ay, float az)
+{
+  float recipNorm;
+  float halfvx, halfvy, halfvz;
+  float halfex, halfey, halfez;
+  float qa, qb, qc;
+  // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
+  if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+    // 首先把加速度计采集到的值(三维向量)转化为单位向量，即向量除以模
+    recipNorm = InvSqrt(ax * ax + ay * ay + az * az);
+    ax *= recipNorm;
+    ay *= recipNorm;
+    az *= recipNorm;      
+    // 把四元数换算成方向余弦中的第三行的三个元素
+    halfvx = q1 * q3 - q0 * q2;
+    halfvy = q0 * q1 + q2 * q3;
+    halfvz = q0 * q0 - 0.5f + q3 * q3;
+    //误差是估计的重力方向和测量的重力方向的交叉乘积之和
+    halfex = (ay * halfvz - az * halfvy);
+    halfey = (az * halfvx - ax * halfvz);
+    halfez = (ax * halfvy - ay * halfvx);
+    // 计算并应用积分反馈（如果启用）
+    if(twoKi > 0.0f) {
+      integralFBx += twoKi * halfex * (1.0f / SAMPLING_FREQ);  // integral error scaled by Ki
+      integralFBy += twoKi * halfey * (1.0f / SAMPLING_FREQ);
+      integralFBz += twoKi * halfez * (1.0f / SAMPLING_FREQ);
+      gx += integralFBx;        // apply integral feedback
+      gy += integralFBy;
+      gz += integralFBz;
+    }
+    else {
+      integralFBx = 0.0f;       // prevent integral windup
+      integralFBy = 0.0f;
+      integralFBz = 0.0f;
+    }
+    // Apply proportional feedback
+    gx += twoKp * halfex;
+    gy += twoKp * halfey;
+    gz += twoKp * halfez;
+  }
+  // Integrate rate of change of quaternion
+  gx *= (0.5f * (1.0f / SAMPLING_FREQ));   // pre-multiply common factors
+  gy *= (0.5f * (1.0f / SAMPLING_FREQ));
+  gz *= (0.5f * (1.0f / SAMPLING_FREQ));
+  qa = q0;
+  qb = q1;
+  qc = q2;
+  q0 += (-qb * gx - qc * gy - q3 * gz);
+  q1 += (qa * gx + qc * gz - q3 * gy);
+  q2 += (qa * gy - qb * gz + q3 * gx);
+  q3 += (qa * gz + qb * gy - qc * gx); 
+  // Normalise quaternion
+  recipNorm = InvSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+  q0 *= recipNorm;
+  q1 *= recipNorm;
+  q2 *= recipNorm;
+  q3 *= recipNorm;
+  Mpu6050.orientation.w = q0;
+  Mpu6050.orientation.x = q1;
+  Mpu6050.orientation.y = q2;
+  Mpu6050.orientation.z = q3;
+}
--- a/src/origincar_base/src/origincar_base.cpp
+++ b/src/origincar_base/src/origincar_base.cpp
@@ -0,0 +1,469 @@
+#include "origincar_base/origincar_base.h"
+#include "rclcpp/rclcpp.hpp"
+#include "origincar_base/Quaternion_Solution.h"
+#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" 
+#include "origincar_msg/msg/data.hpp"
+#include "robot_localization/srv/set_pose.hpp"
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+
+using std::placeholders::_1;
+using namespace std;
+void sigintHandler(int sig);
+sensor_msgs::msg::Imu Mpu6050;
+rclcpp::Node::SharedPtr node_handle = nullptr;
+int Init_imu_num = 0;
+float gyro_z_sum=0;
+
+int main(int argc, char *argv[])
+{
+    rclcpp::init(argc, argv);
+    signal(SIGINT, sigintHandler);
+    origincar_base Robot_Control;
+    Robot_Control.Control();
+    rclcpp::shutdown();
+    return 0;
+}
+
+
+
+
+short origincar_base::IMU_Trans(uint8_t Data_High,uint8_t Data_Low)
+{
+    short transition_16;
+    transition_16 = 0;
+    transition_16 |=  Data_High<<8;
+    transition_16 |=  Data_Low;
+    return transition_16;
+}
+
+float origincar_base::Odom_Trans(uint8_t Data_High,uint8_t Data_Low)
+{
+    float data_return;
+    short transition_16;
+    transition_16 = 0;
+    transition_16 |=  Data_High<<8;
+    transition_16 |=  Data_Low;
+    data_return   =  (transition_16 / 1000)+(transition_16 % 1000)*0.001;
+    return data_return;
+  }
+
+void origincar_base::Akm_Cmd_Vel_Callback(const ackermann_msgs::msg::AckermannDriveStamped::SharedPtr akm_ctl)
+{
+    short  transition;
+    std::cout<<"linerx"<<akm_ctl->drive.speed<<std::endl;
+    std::cout<<"angular"<<akm_ctl->drive.steering_angle<<std::endl;
+  
+    Send_Data.tx[0]=FRAME_HEADER;
+    Send_Data.tx[1] = 0;
+    Send_Data.tx[2] = 0; 
+
+    transition=0;
+    transition = akm_ctl->drive.speed*1000;
+    Send_Data.tx[4] = transition;
+    Send_Data.tx[3] = transition>>8;
+
+    transition=0;
+    transition = akm_ctl->drive.steering_angle*1000/2;
+    Send_Data.tx[8] = transition;
+    Send_Data.tx[7] = transition>>8;
+
+    Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK); 
+    Send_Data.tx[10]=FRAME_TAIL;
+
+    try {
+      Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx));
+    } catch (serial::IOException& e) {
+        RCLCPP_ERROR(this->get_logger(),("Unable to send data through serial port"));
+    }
+}
+
+void origincar_base::Cmd_Vel_Callback(const geometry_msgs::msg::Twist::SharedPtr twist_aux)
+{
+  RCLCPP_INFO(this->get_logger(),"linarx: %.2f, angularz: %.2f ", twist_aux->linear.x, twist_aux->angular.z);
+  std::cout<<"linerx"<<twist_aux->linear.x<<std::endl;
+  std::cout<<"angular"<<twist_aux->angular.z<<std::endl;
+    short  transition;
+    Send_Data.tx[0]=FRAME_HEADER;
+    Send_Data.tx[1] = 0;
+    Send_Data.tx[2] = 0; 
+
+    transition=0;
+    transition = twist_aux->linear.x*1000;
+    Send_Data.tx[4] = transition;
+    Send_Data.tx[3] = transition>>8;
+
+    transition=0;
+    transition = twist_aux->linear.y*1000;
+    Send_Data.tx[6] = transition;
+    Send_Data.tx[5] = transition>>8;
+
+    transition=0;
+    transition = (twist_aux->angular.z)*1000;
+    Send_Data.tx[8] = transition;
+    Send_Data.tx[7] = transition>>8;
+
+    Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK);
+    Send_Data.tx[10]=FRAME_TAIL;
+
+    try 
+    {
+      Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx));
+    } catch (serial::IOException& e) {
+        RCLCPP_ERROR(this->get_logger(),("Unable to send data through serial port"));
+    }
+}
+
+void origincar_base::Sign_Switch_Callback(const std_msgs::msg::Int32::SharedPtr sign_switch)
+{
+  (void)sign_switch;
+    if (sign_switch->data == -1) {
+         memset(&Robot_Pos, 0, sizeof(Robot_Pos));
+         memset(&Mpu6050, 0, sizeof(Mpu6050));
+         q0 = 0;
+         q1 = 0;
+         q2 = 0;
+         q3 = 0;
+         Robot_Pos.X = 0.54;
+         Robot_Pos.Y = 0.2;
+         Robot_Pos.Z = 0;
+         Robot_Vel.X = 0;
+         Robot_Vel.Y = 0;
+         Robot_Vel.Z = 0;
+         memset(&Robot_Vel, 0, sizeof(Robot_Vel));
+         reset_filter();
+     }
+    else if(sign_switch->data == -2) {
+         memset(&Robot_Pos, 0, sizeof(Robot_Pos));
+         Robot_Pos.X = 2.54;
+         Robot_Pos.Y = 2.5;
+         Robot_Pos.Z = -1.5708;
+         Robot_Vel.X = 0;
+         Robot_Vel.Y = 0;
+         Robot_Vel.Z = 0;
+         memset(&Robot_Vel, 0, sizeof(Robot_Vel));
+         reset_filter();
+     }
+     /* 
+     else if (sign_switch->data == 6) {
+         memset(&Robot_Pos, 0, sizeof(Robot_Pos));
+         Robot_Pos.X = 2;
+         Robot_Pos.Y = 2;
+         memset(&Robot_Vel, 0, sizeof(Robot_Vel));
+     }*/
+     
+}
+
+void origincar_base::Publish_ImuSensor()
+{
+    sensor_msgs::msg::Imu Imu_Data_Pub;
+    Imu_Data_Pub.header.stamp = rclcpp::Node::now();
+    Imu_Data_Pub.header.frame_id = gyro_frame_id; 
+                                                  
+    Imu_Data_Pub.orientation.x = Mpu6050.orientation.x;
+    Imu_Data_Pub.orientation.y = Mpu6050.orientation.y;
+    Imu_Data_Pub.orientation.z = Mpu6050.orientation.z;
+    Imu_Data_Pub.orientation.w = Mpu6050.orientation.w;
+    Imu_Data_Pub.orientation_covariance[0] = 1e6; 
+    Imu_Data_Pub.orientation_covariance[4] = 1e6;
+    Imu_Data_Pub.orientation_covariance[8] = 1e-6;
+    Imu_Data_Pub.angular_velocity.x = Mpu6050.angular_velocity.x;
+    Imu_Data_Pub.angular_velocity.y = Mpu6050.angular_velocity.y;
+    Imu_Data_Pub.angular_velocity.z = Mpu6050.angular_velocity.z;
+    Imu_Data_Pub.angular_velocity_covariance[0] = 1e6;
+    Imu_Data_Pub.angular_velocity_covariance[4] = 1e6;
+    Imu_Data_Pub.angular_velocity_covariance[8] = 1e-6;
+    Imu_Data_Pub.linear_acceleration.x = Mpu6050.linear_acceleration.x;
+    Imu_Data_Pub.linear_acceleration.y = Mpu6050.linear_acceleration.y;
+    Imu_Data_Pub.linear_acceleration.z = Mpu6050.linear_acceleration.z;
+
+    imu_publisher->publish(Imu_Data_Pub);
+
+}
+
+void origincar_base::Publish_Odom()
+{
+    tf2::Quaternion q;
+    q.setRPY(0,0,Robot_Pos.Z);
+    geometry_msgs::msg::Quaternion odom_quat=tf2::toMsg(q);
+    
+    origincar_msg::msg::Data robotpose;
+    origincar_msg::msg::Data robotvel;
+    nav_msgs::msg::Odometry odom;
+
+    odom.header.stamp = rclcpp::Node::now();
+    odom.header.frame_id = odom_frame_id;
+    odom.child_frame_id = robot_frame_id;
+
+    odom.pose.pose.position.x = Robot_Pos.X;
+    odom.pose.pose.position.y = Robot_Pos.Y;
+
+    odom.pose.pose.position.z = 0.0;
+    odom.pose.pose.orientation = odom_quat;
+    //odom_quat;
+
+
+    odom.twist.twist.linear.x =  Robot_Vel.X;
+    odom.twist.twist.linear.y =  Robot_Vel.Y;
+    odom.twist.twist.angular.z = Robot_Vel.Z; 
+
+    robotpose.x = Robot_Pos.X;
+    robotpose.y = Robot_Pos.Y;
+    robotpose.z = Robot_Pos.Z;
+
+    robotvel.x = Robot_Vel.X;
+    robotvel.y = Robot_Vel.Y;
+    robotvel.z = Robot_Vel.Z;
+
+    geometry_msgs::msg::TransformStamped t;
+
+    t.header.stamp = rclcpp::Node::now();
+    t.header.frame_id = odom_frame_id;
+    t.child_frame_id  = robot_frame_id;
+
+    t.transform.translation.x = Robot_Pos.X;
+    t.transform.translation.y = Robot_Pos.Y;
+    t.transform.translation.z = 0.0;
+
+    t.transform.rotation = odom_quat;
+    tf_broadcaster_->sendTransform(t);
+    odom_publisher->publish(odom);
+    robotpose_publisher->publish(robotpose);
+    robotvel_publisher->publish(robotvel); 
+}
+
+void origincar_base::Publish_Voltage()
+{
+    std_msgs::msg::Float32 voltage_msgs;
+    static float Count_Voltage_Pub = 0;
+
+    if (Count_Voltage_Pub++ > 10) {
+        Count_Voltage_Pub = 0;
+        voltage_msgs.data = Power_voltage;
+        voltage_publisher->publish(voltage_msgs);
+    }
+}
+
+unsigned char origincar_base::Check_Sum(unsigned char Count_Number,unsigned char mode)
+{
+    unsigned char check_sum = 0, k;
+
+    if (mode == 0) {
+      for(k=0; k < Count_Number; k++) {
+        check_sum = check_sum^Receive_Data.rx[k];
+      }
+    } else if (mode == 1) {
+      for (k=0; k < Count_Number; k++) {
+        check_sum = check_sum^Send_Data.tx[k];
+      }
+    }
+
+    return check_sum;
+}
+
+bool origincar_base::Get_Sensor_Data()
+{
+    short transition_16 = 0, j = 0, Header_Pos = 0, Tail_Pos = 0;
+    uint8_t Receive_Data_Pr[RECEIVE_DATA_SIZE] = {0}; 
+    Stm32_Serial.read(Receive_Data_Pr,sizeof (Receive_Data_Pr)); 
+    for (j = 0; j < 24; j++) {
+      if (Receive_Data_Pr[j] == FRAME_HEADER)
+      Header_Pos=j;
+      else if (Receive_Data_Pr[j] == FRAME_TAIL)
+      Tail_Pos = j;
+    }
+
+    if (Tail_Pos == (Header_Pos + 23)) {
+      memcpy(Receive_Data.rx, Receive_Data_Pr, sizeof(Receive_Data_Pr));
+    }  else if (Header_Pos == (1 + Tail_Pos)) {
+      for (j = 0;j < 24; j++)
+      Receive_Data.rx[j] = Receive_Data_Pr[(j+Header_Pos) % 24];
+    }  else {
+    return false;
+    }
+
+    Receive_Data.Frame_Header = Receive_Data.rx[0];
+    Receive_Data.Frame_Tail = Receive_Data.rx[23];
+    if (Receive_Data.Frame_Header == FRAME_HEADER) {
+      if (Receive_Data.Frame_Tail == FRAME_TAIL) {
+        if (Receive_Data.rx[22] == Check_Sum(22,READ_DATA_CHECK)||(Header_Pos == (1 + Tail_Pos))) {
+          Receive_Data.Flag_Stop=Receive_Data.rx[1];
+          Robot_Vel.X = Odom_Trans(Receive_Data.rx[2],Receive_Data.rx[3]);
+        
+          Robot_Vel.Y = Odom_Trans(Receive_Data.rx[4],Receive_Data.rx[5]);
+                                                                          
+          //Robot_Vel.Z = Odom_Trans(Receive_Data.rx[6],Receive_Data.rx[7]); 
+
+          Mpu6050_Data.accele_x_data = IMU_Trans(Receive_Data.rx[8],Receive_Data.rx[9]);
+          Mpu6050_Data.accele_y_data = IMU_Trans(Receive_Data.rx[10],Receive_Data.rx[11]);
+          Mpu6050_Data.accele_z_data = IMU_Trans(Receive_Data.rx[12],Receive_Data.rx[13]);
+          Mpu6050_Data.gyros_x_data = IMU_Trans(Receive_Data.rx[14],Receive_Data.rx[15]);
+          Mpu6050_Data.gyros_y_data = IMU_Trans(Receive_Data.rx[16],Receive_Data.rx[17]);
+          Mpu6050_Data.gyros_z_data = IMU_Trans(Receive_Data.rx[18],Receive_Data.rx[19]);
+
+          Mpu6050.linear_acceleration.x = Mpu6050_Data.accele_x_data / ACCEl_RATIO;
+          Mpu6050.linear_acceleration.y = Mpu6050_Data.accele_y_data / ACCEl_RATIO;
+          Mpu6050.linear_acceleration.z = Mpu6050_Data.accele_z_data / ACCEl_RATIO;
+
+          Mpu6050.angular_velocity.x =  Mpu6050_Data.gyros_x_data * GYROSCOPE_RATIO;
+          Mpu6050.angular_velocity.y =  Mpu6050_Data.gyros_y_data * GYROSCOPE_RATIO;
+          
+          if(Init_imu_num<100)
+          {
+            Init_imu_num += 1;
+            gyro_z_sum +=  Mpu6050_Data.gyros_z_data ;
+            Mpu6050.angular_velocity.z = 0;
+          }
+          else
+          {
+            Mpu6050.angular_velocity.z = (Mpu6050_Data.gyros_z_data - (gyro_z_sum / (float)Init_imu_num))*GYROSCOPE_RATIO;
+            //RCLCPP_INFO(this->get_logger(),"gyro_z_sum: %.2f, err: %.2f, gyroz: %.2f ", gyro_z_sum ,(gyro_z_sum / (float)Init_imu_num), Mpu6050.angular_velocity.z);
+          }
+          Robot_Vel.Z = Mpu6050.angular_velocity.z;
+          transition_16 = 0;
+          transition_16 |=  Receive_Data.rx[20]<<8;
+          transition_16 |=  Receive_Data.rx[21];
+          Power_voltage = transition_16/1000+(transition_16 % 1000)*0.001;
+
+          return true;
+        }
+      }
+    }
+
+    return false;
+}
+
+void origincar_base::Control()
+{
+    rclcpp::Time current_time, last_time;
+    current_time = rclcpp::Node::now();
+    last_time = rclcpp::Node::now();
+    while(rclcpp::ok()) {
+      current_time = rclcpp::Node::now();
+      Sampling_Time = (current_time - last_time).seconds();
+      if (true == Get_Sensor_Data()) {
+        Robot_Pos.X+=1.03*(Robot_Vel.X * cos(Robot_Pos.Z) - Robot_Vel.Y * sin(Robot_Pos.Z)) * Sampling_Time;
+        Robot_Pos.Y+=1.01*(Robot_Vel.X * sin(Robot_Pos.Z) + Robot_Vel.Y * cos(Robot_Pos.Z)) * Sampling_Time;//1.125
+        Robot_Pos.Z+= Mpu6050.angular_velocity.z * Sampling_Time;
+
+        Quaternion_Solution(Mpu6050.angular_velocity.x, Mpu6050.angular_velocity.y, Mpu6050.angular_velocity.z,\
+                  Mpu6050.linear_acceleration.x, Mpu6050.linear_acceleration.y, Mpu6050.linear_acceleration.z);
+        Publish_ImuSensor();
+        Publish_Voltage();
+        Publish_Odom();
+        rclcpp::spin_some(this->get_node_base_interface());
+      }
+      last_time = current_time;  
+    }
+}
+
+origincar_base::origincar_base()
+: rclcpp::Node ("origincar_base")
+{
+  memset(&Robot_Pos, 0, sizeof(Robot_Pos));
+  memset(&Robot_Vel, 0, sizeof(Robot_Vel));
+  memset(&Receive_Data, 0, sizeof(Receive_Data));
+  memset(&Send_Data, 0, sizeof(Send_Data));
+  memset(&Mpu6050_Data, 0, sizeof(Mpu6050_Data));
+  Robot_Pos.X = 0.54;
+  Robot_Pos.Y = 0.2;
+
+  int serial_baud_rate = 115200;
+
+  this->declare_parameter<std::string>("usart_port_name", "/dev/ttyCH343USB0");
+  this->declare_parameter<std::string>("cmd_vel", "cmd_vel");
+  this->declare_parameter<std::string>("akm_cmd_vel", "ackermann_cmd");
+  this->declare_parameter<std::string>("odom_frame_id", "odom");
+  this->declare_parameter<std::string>("robot_frame_id", "base_link");
+  this->declare_parameter<std::string>("gyro_frame_id", "gyro_link");
+
+  this->get_parameter("serial_baud_rate", serial_baud_rate);
+  this->get_parameter("usart_port_name", usart_port_name);
+  this->get_parameter("cmd_vel", cmd_vel);
+  this->get_parameter("akm_cmd_vel", akm_cmd_vel);
+  this->get_parameter("odom_frame_id", odom_frame_id);
+  this->get_parameter("robot_frame_id", robot_frame_id);
+  this->get_parameter("gyro_frame_id", gyro_frame_id);
+
+  odom_publisher = create_publisher<nav_msgs::msg::Odometry>("odom", 10);
+
+  imu_publisher = create_publisher<sensor_msgs::msg::Imu>("imu/data_raw", 10);
+
+  voltage_publisher = create_publisher<std_msgs::msg::Float32>("PowerVoltage", 1);
+
+  robotpose_publisher = create_publisher<origincar_msg::msg::Data>("robotpose", 10);
+
+  robotvel_publisher = create_publisher<origincar_msg::msg::Data>("robotvel", 10);
+  
+  pose_pub_ = create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
+            "/set_pose", 
+            rclcpp::SystemDefaultsQoS().reliable());
+  
+  tf_bro = std::make_shared<tf2_ros::TransformBroadcaster>(this);
+  tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
+  Cmd_Vel_Sub = create_subscription<geometry_msgs::msg::Twist>(
+      cmd_vel, 1, std::bind(&origincar_base::Cmd_Vel_Callback, this, _1));
+  Akm_Cmd_Vel_Sub = create_subscription<ackermann_msgs::msg::AckermannDriveStamped>(
+      akm_cmd_vel, 1, std::bind(&origincar_base::Akm_Cmd_Vel_Callback, this, _1));
+
+  Sign_Switch_Sub = create_subscription<std_msgs::msg::Int32>(
+       "/sign4return", 1, std::bind(&origincar_base::Sign_Switch_Callback, this, _1));
+  try  {
+    Stm32_Serial.setPort("/dev/ttyACM0");
+    Stm32_Serial.setBaudrate(serial_baud_rate);
+    serial::Timeout _time = serial::Timeout::simpleTimeout(2000);
+    Stm32_Serial.setTimeout(_time);
+    Stm32_Serial.open();
+  } catch (serial::IOException& e) {
+    RCLCPP_ERROR(this->get_logger(),"origincar_base can not open serial port,Please check the serial port cable! ");
+  }
+  if(Stm32_Serial.isOpen()) {
+    RCLCPP_INFO(this->get_logger(),"origincar_base serial port opened");
+  }
+}
+
+
+void sigintHandler(int sig)
+{
+    sig = sig;
+      printf("OriginBot shutdown...\n");
+    serial::Serial Stm32_Serial;
+    Stm32_Serial.setPort("/dev/ttyACM0");
+    Stm32_Serial.setBaudrate(115200);
+    serial::Timeout _time = serial::Timeout::simpleTimeout(2000);
+    Stm32_Serial.setTimeout(_time);
+    Stm32_Serial.open();                                       
+    SEND_DATA Send_Data;
+    if (Stm32_Serial.isOpen()) {
+    Send_Data.tx[0]=FRAME_HEADER;
+    Send_Data.tx[1] = 0;
+    Send_Data.tx[2] = 0;
+
+    Send_Data.tx[4] = 0;
+    Send_Data.tx[3] = 0;
+
+    Send_Data.tx[6] = 0;
+    Send_Data.tx[5] = 0;
+
+    Send_Data.tx[7] = 0;
+    Send_Data.tx[8] = 0;
+    int check_sum = 0;
+    for (int k = 0; k < 9; k++) {
+        check_sum = check_sum^Send_Data.tx[k];
+      }
+    Send_Data.tx[9]=check_sum;
+    Send_Data.tx[10]=FRAME_TAIL;
+
+    try {
+        Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx));
+    } catch (serial::IOException& e) {
+    }
+
+  }
+    // 关闭ROS2接口，清除资源
+    rclcpp::shutdown();
+}
+
+origincar_base::~origincar_base()
+{
+  RCLCPP_INFO(this->get_logger(),"Shutting down");
+}