From: Erik Andresen Date: Fri, 20 Mar 2026 12:30:20 +0000 (+0100) Subject: Added robot_localization X-Git-Url: https://defiant.homedns.org/gitweb/?a=commitdiff_plain;h=24464dd280a2414a5697c926f5d3b1c003395ea5;p=a4wd3.git Added robot_localization --- diff --git a/.screen-startup b/.screen-startup index 5f9573f..67f5862 100644 --- a/.screen-startup +++ b/.screen-startup @@ -3,4 +3,5 @@ source $HOME/.screenrc -screen 0 zsh -is eval 'ros2 run a4wd3 hw_node --ros-args --log-level info' +#screen 0 zsh -is eval 'ros2 run a4wd3 hw_node --ros-args --log-level info' +screen 0 zsh -is eval 'ros2 launch a4wd3 a4wd3_launch.py' diff --git a/avr/motor_ctrl/main.c b/avr/motor_ctrl/main.c index 30cadeb..6867e78 100644 --- a/avr/motor_ctrl/main.c +++ b/avr/motor_ctrl/main.c @@ -106,6 +106,13 @@ */ + /** + * 1 = aft right + * 2 = front right + * 3 = aft left + * 4 = front left + */ + #define KP 0.010499758766982191 #define KI 1.049975876698219 #define KD 0.0 @@ -906,8 +913,8 @@ static void update_pos(void) { new_speed3 = pos3_diff/PID_T; new_speed4 = pos4_diff/PID_T; - diff_left_m = (pos1_diff + pos2_diff)/(2*STEP_PER_M_LEFT); - diff_right_m = (pos3_diff + pos4_diff)/(2*STEP_PER_M_RIGHT); + diff_left_m = (pos3_diff + pos4_diff)/(2*STEP_PER_M_LEFT); + diff_right_m = (pos1_diff + pos2_diff)/(2*STEP_PER_M_RIGHT); angle_diff = (diff_right_m - diff_left_m) / WHEEL_DIST; angle_new = angle.f + angle_diff; @@ -1199,18 +1206,18 @@ int main(void) { speed_wish_right*=STEP_PER_M_RIGHT; if (aft_handicap > 0) { - speed1_wish = speed_wish_left * (100-aft_handicap)/100.0; - speed3_wish = speed_wish_right * (100-aft_handicap)/100.0; + speed3_wish = speed_wish_left * (100-aft_handicap)/100.0; + speed1_wish = speed_wish_right * (100-aft_handicap)/100.0; } else { - speed1_wish = speed_wish_left; - speed3_wish = speed_wish_right; + speed3_wish = speed_wish_left; + speed1_wish = speed_wish_right; } if (front_handicap > 0) { - speed2_wish = speed_wish_left * (100-front_handicap)/100.0; - speed4_wish = speed_wish_right * (100-front_handicap)/100.0; + speed4_wish = speed_wish_left * (100-front_handicap)/100.0; + speed2_wish = speed_wish_right * (100-front_handicap)/100.0; } else { - speed2_wish = speed_wish_left; - speed4_wish = speed_wish_right; + speed4_wish = speed_wish_left; + speed2_wish = speed_wish_right; } motor1_mode = MOTOR_PID; motor2_mode = MOTOR_PID; diff --git a/avr/motor_ctrl/main.hex b/avr/motor_ctrl/main.hex index f5fbb5a..a139dff 100644 --- a/avr/motor_ctrl/main.hex +++ b/avr/motor_ctrl/main.hex @@ -240,40 +240,40 @@ :100EF000A4009101261B3109B901072E000C880BB3 :100F0000990B0E9479119601A7010E946D1220E0B1 :100F100030E048EC52E40E94CF100E9441117093DF -:100F2000D5006093D4006091A400C101861B910993 +:100F2000D1006093D0006091A400C101861B91099B :100F3000BC01072E000C880B990B0E9479119801B7 :100F40004D8159810E946D1220E030E048EC52E45E -:100F50000E94CF100E9441117093D1006093D00085 +:100F50000E94CF100E9441117093D5006093D4007D :100F60008091A500811509F42AC36091A500D101E3 :100F7000A61BB109BD01072E000C880B990B0E941E :100F800079119601A7010E946D1220E030E048EC33 :100F900052E40E94CF100E944111462F572F5093C8 -:100FA000D3004093D2006091A5009101261B310926 +:100FA000CF004093CE006091A5009101261B31092E :100FB000B901072E000C880B990B0E94791198013A :100FC0004D8159810E946D1220E030E048EC52E4DE -:100FD0000E94CF100E944111462F572F5093CF00EF -:100FE0004093CE0031E03093DB003093DA00309351 +:100FD0000E94CF100E944111462F572F5093D300EB +:100FE0004093D20031E03093DB003093DA0030934D :100FF000D9003093D8008091C5008E3408F47AC6A9 :101000001092C500F8948091E2009091E3009A8BD1 :10101000898BA091E000B091E100BC8BAB8B20915B :10102000DE003091DF003E8B2D8B8091DC009091B3 :10103000DD00988F8F8B789480918E0090918F0037 -:10104000C988DA88C81AD90A80918C0090918D00DD -:101050007D01E81AF90A80918A0090918B00281B83 -:10106000390B3A8F298F80918800909189000F89E0 -:10107000188D081B190BB6010D2C000C880B990B51 -:101080000E9479112AE037ED43E25CE30E94CF1021 -:101090000E94411169837A838B839C83B7010F2C53 -:1010A000000C880B990B0E9479112AE037ED43E27E -:1010B0005CE30E94CF100E9441116D837E838F8379 -:1010C0009887A98DBA8DBD01BB0F880B990B0E9423 +:10104000A989BA89A81BB90BBA8FA98F80918C0086 +:1010500090918D00EB88FC88E81AF90A80918A004B +:1010600090918B008901081B190B80918800909149 +:101070008900CF88D88CC81AD90ABD01BB0F880B4C +:10108000990B0E9479112AE037ED43E25CE30E945C +:10109000CF100E94411169837A838B839C83B701AF +:1010A0000F2C000C880B990B0E9479112AE037ED68 +:1010B00043E25CE30E94CF100E9441116D837E8366 +:1010C0008F839887B801012E000C880B990B0E9422 :1010D00079112AE037ED43E25CE30E94CF100E94D1 -:1010E000411169877A878B879C87B801012E000C94 +:1010E000411169877A878B879C87B6010D2C000C8C :1010F000880B990B0E9479112AE037ED43E25CE3FB :101100000E94CF100E9441116D877E878F87988B38 -:10111000B6016E0D7F1D072E000C880B990B0E94E7 +:10111000B8016C0D7D1D072E000C880B990B0E94E9 :1011200079112AE939E44BE156E40E94CF104B01D2 -:101130005C01698D7A8D600F711F072E000C880B82 +:101130005C01698D7A8D6E0D7F1D072E000C880B6A :10114000990B0E9479112AE939E44BE156E40E9497 :10115000CF102B013C019401A5010E9458102AECEC :1011600037E34BEF5EE30E94CF106D8F7E8F182F19 @@ -345,9 +345,9 @@ :1015800009F065CCF8946BE780E090E00E941916B2 :10159000E199FECFA8E1B8E00FB6F894A895A1BDF7 :1015A0000FBEB1BD54CCB601C7010E94411170936A -:1015B000D5006093D400B8018D819981CBCCB60160 -:1015C000C7010E944111462F572F5093D3004093DB -:1015D000D200B8018D819981FDCC2BED3FE049EC23 +:1015B000D1006093D000B8018D819981CBCCB60168 +:1015C000C7010E944111462F572F5093CF004093DF +:1015D000CE00B8018D819981FDCC2BED3FE049EC27 :1015E00050ECC701B6010E94C51087FFE3CD2BED7B :1015F0003FE049EC50E4C701B6010E945910D8CD34 :1016000081E02091D4003091D500232B19F0811571 diff --git a/launch/a4wd3_launch.py b/launch/a4wd3_launch.py new file mode 100644 index 0000000..de16d6e --- /dev/null +++ b/launch/a4wd3_launch.py @@ -0,0 +1,39 @@ +#!/usr/bin/env python +# -*- coding: iso-8859-15 -*- + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='a4wd3', + executable='hw_node', + name='hw_node', + parameters=[{"enable_odom_tf": False}], + output="screen", + ), + Node( + package='bno085_uart', + executable='bno085', + name='bno085', + parameters=[{"port": "/dev/ttyS2", "hz": 20.0}], + output="screen", + ), + Node( + package='tf2_ros', + executable='static_transform_publisher', + name='tf_base_imu', + arguments = ['--x', '0', '--y', '0.00', '--yaw', '0.0', '--frame-id', 'base_link', '--child-frame-id', 'imu'], + output="screen" + ), + Node( + package='robot_localization', + executable='ekf_node', + name='ekf_filter_node', + output='screen', + parameters = [os.path.join(get_package_share_directory('a4wd3'), 'params', 'robot_localization.yaml')], + ), + ]) diff --git a/params/robot_localization.yaml b/params/robot_localization.yaml new file mode 100644 index 0000000..3dc4200 --- /dev/null +++ b/params/robot_localization.yaml @@ -0,0 +1,241 @@ +### ekf config file ### +ekf_filter_node: + ros__parameters: + # 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: 20.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: 0.1 + + # 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.0 + + # 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: true + + # 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 we'll allow old measurements to cause a re-publication of the updated state + permit_corrected_publication: false + + # Whether to publish the acceleration state. Defaults to false if unspecified. + publish_acceleration: false + + # Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified. + publish_tf: true + + # 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 # Defaults to "odom" if unspecified + base_link_frame: base_link # Defaults to "base_link" if unspecified + world_frame: odom # 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: [false, false, false, + false, false, false, + true, true, true, + false, false, false, + 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: 2 + + # [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: false + + # [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: true + + # [ADVANCED] Whether to use the starting pose of child_frame_id as the origin of odometry. + # Note: this is different from setting odom0_relative to true, as when child_frame is different from + # base_link_frame, the rotation of base_link will be coupled into the translation of child_frame. + # Set to true for fusing secondary odometry sources that are rigidly connected to base_link but has a non-zero + # offset from base_link. + odom0_pose_use_child_frame: 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::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 + imu0_config: [false, false, false, + true, true, true, + false, false, false, + true, true, true, + false, false, false] + imu0_nodelay: false + imu0_differential: false + imu0_relative: false + imu0_pose_use_child_frame: false + imu0_queue_size: 5 + #imu0_pose_rejection_threshold: 0.8 # Note the difference in parameter names + #imu0_twist_rejection_threshold: 0.8 # + #imu0_linear_acceleration_rejection_threshold: 0.8 # + + # [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: false + + odom1: /odometry/gps + odom1_config: [true, true, false, + false, false, false, + false, false, false, + false, false, false, + false, false, false] + odom1_queue_size: 2 + odom1_nodelay: false + odom1_differential: false + odom1_relative: false + + ## [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: true + ## 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.10, 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.10, 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.10, 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.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.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.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.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.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.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.1, 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.1, 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.1] + # [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] + + # If true, will dynamically scale the process_noise_covariance based on the robot's velocity. This is useful, e.g., when you want your + # robots estimate error covariance to stop growing when the robot is stationary. Defaults to false. + dynamic_process_noise_covariance: true diff --git a/src/hw_node.cpp b/src/hw_node.cpp index 18625c2..fa13cce 100644 --- a/src/hw_node.cpp +++ b/src/hw_node.cpp @@ -18,6 +18,8 @@ #include #include #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2_ros/transform_broadcaster.h" +#include "geometry_msgs/msg/transform_stamped.hpp" extern "C" { #include @@ -95,9 +97,13 @@ class A4wd3 : public rclcpp::Node { pub_odom = this->create_publisher("odom", 10); pub_bat = this->create_publisher("battery", 10); sub_cmd_vel = this->create_subscription("cmd_vel", 10, std::bind(&A4wd3::cmdvel_callback, this, std::placeholders::_1)); + tf_broadcaster = std::make_unique(*this); + this->declare_parameter("enable_odom_tf", true); this->declare_parameter("odom_covar_xy", 0.01); this->declare_parameter("odom_covar_angle", 0.52); + enable_odom_tf = this->get_parameter("enable_odom_tf").as_bool(); + set_speed(0, 0); init_pwr(); timer = this->create_wall_timer(100ms, std::bind(&A4wd3::timer_callback, this)); @@ -110,11 +116,12 @@ class A4wd3 : public rclcpp::Node { rclcpp::Publisher::SharedPtr pub_odom; rclcpp::Publisher::SharedPtr pub_bat; rclcpp::Subscription::SharedPtr sub_cmd_vel; + std::unique_ptr tf_broadcaster; std::vector cmd_vel; double pos_x=0, pos_y=0, angle=0; + bool enable_odom_tf; void set_speed(float trans, float rot) { - rot*=-1; std::vector buf; buf.push_back((*(uint32_t*)&trans)>>24); buf.push_back((*(uint32_t*)&trans)>>16); @@ -159,21 +166,35 @@ class A4wd3 : public rclcpp::Node { float pos_x = values[2].f; float pos_y = values[3].f; float angle = values[4].f; - speed_rot*=-1; - angle*=-1; RCLCPP_DEBUG(this->get_logger(), "Odom from I2C: [%.2f, %.2f], Angle: %d, Speed: [%.2f, %.2f]", pos_x, pos_y, (int)(angle*180/M_PI+0.5), speed_trans, speed_rot); double odom_covar_xy = this->get_parameter("odom_covar_xy").as_double(); double odom_covar_angle = this->get_parameter("odom_covar_angle").as_double(); + tf2::Quaternion odom_quat; + odom_quat.setRPY(0, 0, angle); + + if (enable_odom_tf) { + // first, we'll publish the transform over tf + geometry_msgs::msg::TransformStamped odom_trans; + odom_trans.header.stamp = this->get_clock()->now(); + odom_trans.header.frame_id = "odom"; + odom_trans.child_frame_id = "base_link"; + odom_trans.transform.translation.x = pos_x; + odom_trans.transform.translation.y = pos_y; + odom_trans.transform.translation.z = 0.0; + odom_trans.transform.rotation.x = odom_quat.x(); + odom_trans.transform.rotation.y = odom_quat.y(); + odom_trans.transform.rotation.z = odom_quat.z(); + odom_trans.transform.rotation.w = odom_quat.w(); + tf_broadcaster->sendTransform(odom_trans); + } + auto odom = nav_msgs::msg::Odometry(); odom.header.stamp = this->get_clock()->now(); odom.header.frame_id = "odom"; - tf2::Quaternion odom_quat; - odom_quat.setRPY(0, 0, angle); - // set the position odom.pose.pose.position.x = pos_x; odom.pose.pose.position.y = pos_y;