From 13d3a299702a3017c066cb23667af4adca9bdf8c Mon Sep 17 00:00:00 2001 From: manuelgitgomes Date: Wed, 27 Jul 2022 10:44:10 +0100 Subject: [PATCH] #41 Bugfixing in visual odometry --- atlascar2_bringup/launch/bringup.launch | 3 +- atlascar2_bringup/launch/model.launch | 2 +- .../urdf/initial_estimate.urdf.xacro | 736 +++++++++++++++--- .../urdf/optimized.urdf.xacro | 54 +- ...ptimized_2022_05_06-04_45_32_PM.urdf.xacro | 639 --------------- ...ptimized_2022_05_06-04_47_28_PM.urdf.xacro | 639 --------------- ...ptimized_2022_05_06-04_49_49_PM.urdf.xacro | 639 --------------- ...ptimized_2022_05_06-04_51_12_PM.urdf.xacro | 639 --------------- ...ptimized_2022_05_06-04_54_01_PM.urdf.xacro | 639 --------------- atlascar2_odom/launch/odom_bringup.launch | 9 + atlascar2_odom/nodes/ackermann_to_odom.py | 131 ++-- .../nodes/can_ros_msgs_to_ackermann.py | 15 +- .../{ => nodes}/icp_visual_odometry.py | 44 +- 13 files changed, 779 insertions(+), 3410 deletions(-) delete mode 100644 atlascar2_calibration/urdf/optimized/optimized_2022_05_06-04_45_32_PM.urdf.xacro delete mode 100644 atlascar2_calibration/urdf/optimized/optimized_2022_05_06-04_47_28_PM.urdf.xacro delete mode 100644 atlascar2_calibration/urdf/optimized/optimized_2022_05_06-04_49_49_PM.urdf.xacro delete mode 100644 atlascar2_calibration/urdf/optimized/optimized_2022_05_06-04_51_12_PM.urdf.xacro delete mode 100644 atlascar2_calibration/urdf/optimized/optimized_2022_05_06-04_54_01_PM.urdf.xacro create mode 100755 atlascar2_odom/launch/odom_bringup.launch rename atlascar2_odom/{ => nodes}/icp_visual_odometry.py (87%) diff --git a/atlascar2_bringup/launch/bringup.launch b/atlascar2_bringup/launch/bringup.launch index 227802a..afc1c11 100755 --- a/atlascar2_bringup/launch/bringup.launch +++ b/atlascar2_bringup/launch/bringup.launch @@ -31,8 +31,7 @@ - - + diff --git a/atlascar2_bringup/launch/model.launch b/atlascar2_bringup/launch/model.launch index 43d8eab..c86bbc1 100755 --- a/atlascar2_bringup/launch/model.launch +++ b/atlascar2_bringup/launch/model.launch @@ -1,7 +1,7 @@ - + \ No newline at end of file diff --git a/atlascar2_calibration/urdf/initial_estimate.urdf.xacro b/atlascar2_calibration/urdf/initial_estimate.urdf.xacro index 3d2d08d..998fe9f 100644 --- a/atlascar2_calibration/urdf/initial_estimate.urdf.xacro +++ b/atlascar2_calibration/urdf/initial_estimate.urdf.xacro @@ -1,99 +1,639 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - steer_bot_hardware_gazebo/SteerBotHardwareGazebo - - - - - - Gazebo/Purple - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Grey + + 120 + + 0 0 0 0 -1.5707 1.5707 + 1.0471975512 + + 1272 + 1016 + R8G8B8 + + + 0.5 + 300 + + + gaussian + 0.0 + 0.007 + + + + true + 120 + top_right_camera + image_raw + camera_info + top_right_camera_optical + 0.07 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Grey + + 120 + + 0 0 0 0 -1.5707 1.5707 + 1.0471975512 + + 1272 + 1016 + R8G8B8 + + + 0.5 + 300 + + + gaussian + 0.0 + 0.007 + + + + true + 120 + top_left_camera + image_raw + camera_info + top_left_camera_optical + 0.07 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0 0 0 0 0 0 + false + 10 + + + + 1875 + 1 + -3.141592653589793 + 3.141592653589793 + + + 16 + 1 + -0.2617993877991494 + 0.2617993877991494 + + + + 0.3 + 131.0 + 0.001 + + + gaussian + 0.0 + 0.0 + + + + top_laser/cloud + top_laser + False + 0.9 + 130.0 + 0.008 + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + + hardware_interface/PositionJointInterface + + + + Gazebo/Yellow + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + + hardware_interface/VelocityJointInterface + + + + + + + + + + + + + + + + + + + + + + + true + + 0 0 0 0 0 0 + false + 50 + + + + 720 + 1 + -2.35619 + 2.35619 + + + + 0.1 + 30.0 + 0.01 + + + gaussian + 0.0 + 0.001 + + + + /left_laser/scan + left_laser + / + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + 0 0 0 0 0 0 + false + 50 + + + + 720 + 1 + -2.35619 + 2.35619 + + + + 0.1 + 30.0 + 0.01 + + + gaussian + 0.0 + 0.001 + + + + /right_laser/scan + right_laser + / + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + + hardware_interface/EffortJointInterface + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + + hardware_interface/EffortJointInterface + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + + hardware_interface/EffortJointInterface + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + + hardware_interface/EffortJointInterface + + + + + steer_bot_hardware_gazebo/SteerBotHardwareGazebo + + + + Gazebo/Purple + \ No newline at end of file diff --git a/atlascar2_calibration/urdf/optimized.urdf.xacro b/atlascar2_calibration/urdf/optimized.urdf.xacro index ced11a6..134e22c 100644 --- a/atlascar2_calibration/urdf/optimized.urdf.xacro +++ b/atlascar2_calibration/urdf/optimized.urdf.xacro @@ -13,7 +13,7 @@ - + @@ -21,7 +21,7 @@ - + @@ -34,7 +34,7 @@ - + @@ -109,7 +109,7 @@ - + @@ -177,18 +177,18 @@ - + - + - + @@ -216,12 +216,12 @@ - + - + @@ -271,8 +271,8 @@ - - + + @@ -302,8 +302,8 @@ - - + + @@ -328,7 +328,7 @@ - + @@ -385,7 +385,7 @@ - + @@ -443,8 +443,8 @@ - - + + @@ -464,8 +464,8 @@ - - + + @@ -504,8 +504,8 @@ - - + + @@ -525,8 +525,8 @@ - - + + @@ -565,8 +565,8 @@ - - + + @@ -598,8 +598,8 @@ - - + + diff --git a/atlascar2_calibration/urdf/optimized/optimized_2022_05_06-04_45_32_PM.urdf.xacro b/atlascar2_calibration/urdf/optimized/optimized_2022_05_06-04_45_32_PM.urdf.xacro deleted file mode 100644 index bdb16db..0000000 --- a/atlascar2_calibration/urdf/optimized/optimized_2022_05_06-04_45_32_PM.urdf.xacro +++ /dev/null @@ -1,639 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Grey - - 120 - - 0 0 0 0 -1.5707 1.5707 - 1.0471975512 - - 1272 - 1016 - R8G8B8 - - - 0.5 - 300 - - - gaussian - 0.0 - 0.007 - - - - true - 120 - top_right_camera - image_raw - camera_info - top_right_camera_optical - 0.07 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Grey - - 120 - - 0 0 0 0 -1.5707 1.5707 - 1.0471975512 - - 1272 - 1016 - R8G8B8 - - - 0.5 - 300 - - - gaussian - 0.0 - 0.007 - - - - true - 120 - top_left_camera - image_raw - camera_info - top_left_camera_optical - 0.07 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 0 0 0 0 0 0 - false - 10 - - - - 1875 - 1 - -3.141592653589793 - 3.141592653589793 - - - 16 - 1 - -0.2617993877991494 - 0.2617993877991494 - - - - 0.3 - 131.0 - 0.001 - - - gaussian - 0.0 - 0.0 - - - - top_laser/cloud - top_laser - False - 0.9 - 130.0 - 0.008 - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - - hardware_interface/PositionJointInterface - - - - Gazebo/Yellow - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - - hardware_interface/VelocityJointInterface - - - - - - - - - - - - - - - - - - - - - - - true - - 0 0 0 0 0 0 - false - 50 - - - - 720 - 1 - -2.35619 - 2.35619 - - - - 0.1 - 30.0 - 0.01 - - - gaussian - 0.0 - 0.001 - - - - /left_laser/scan - left_laser - / - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - - 0 0 0 0 0 0 - false - 50 - - - - 720 - 1 - -2.35619 - 2.35619 - - - - 0.1 - 30.0 - 0.01 - - - gaussian - 0.0 - 0.001 - - - - /right_laser/scan - right_laser - / - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - - hardware_interface/EffortJointInterface - - - - transmission_interface/SimpleTransmission - - - hardware_interface/EffortJointInterface - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - - hardware_interface/EffortJointInterface - - - - transmission_interface/SimpleTransmission - - - hardware_interface/EffortJointInterface - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - - hardware_interface/EffortJointInterface - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - - hardware_interface/EffortJointInterface - - - - - steer_bot_hardware_gazebo/SteerBotHardwareGazebo - - - - Gazebo/Purple - - \ No newline at end of file diff --git a/atlascar2_calibration/urdf/optimized/optimized_2022_05_06-04_47_28_PM.urdf.xacro b/atlascar2_calibration/urdf/optimized/optimized_2022_05_06-04_47_28_PM.urdf.xacro deleted file mode 100644 index 62b389f..0000000 --- a/atlascar2_calibration/urdf/optimized/optimized_2022_05_06-04_47_28_PM.urdf.xacro +++ /dev/null @@ -1,639 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Grey - - 120 - - 0 0 0 0 -1.5707 1.5707 - 1.0471975512 - - 1272 - 1016 - R8G8B8 - - - 0.5 - 300 - - - gaussian - 0.0 - 0.007 - - - - true - 120 - top_right_camera - image_raw - camera_info - top_right_camera_optical - 0.07 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Grey - - 120 - - 0 0 0 0 -1.5707 1.5707 - 1.0471975512 - - 1272 - 1016 - R8G8B8 - - - 0.5 - 300 - - - gaussian - 0.0 - 0.007 - - - - true - 120 - top_left_camera - image_raw - camera_info - top_left_camera_optical - 0.07 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 0 0 0 0 0 0 - false - 10 - - - - 1875 - 1 - -3.141592653589793 - 3.141592653589793 - - - 16 - 1 - -0.2617993877991494 - 0.2617993877991494 - - - - 0.3 - 131.0 - 0.001 - - - gaussian - 0.0 - 0.0 - - - - top_laser/cloud - top_laser - False - 0.9 - 130.0 - 0.008 - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - - hardware_interface/PositionJointInterface - - - - Gazebo/Yellow - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - - hardware_interface/VelocityJointInterface - - - - - - - - - - - - - - - - - - - - - - - true - - 0 0 0 0 0 0 - false - 50 - - - - 720 - 1 - -2.35619 - 2.35619 - - - - 0.1 - 30.0 - 0.01 - - - gaussian - 0.0 - 0.001 - - - - /left_laser/scan - left_laser - / - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - - 0 0 0 0 0 0 - false - 50 - - - - 720 - 1 - -2.35619 - 2.35619 - - - - 0.1 - 30.0 - 0.01 - - - gaussian - 0.0 - 0.001 - - - - /right_laser/scan - right_laser - / - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - - hardware_interface/EffortJointInterface - - - - transmission_interface/SimpleTransmission - - - hardware_interface/EffortJointInterface - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - - hardware_interface/EffortJointInterface - - - - transmission_interface/SimpleTransmission - - - hardware_interface/EffortJointInterface - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - - hardware_interface/EffortJointInterface - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - - hardware_interface/EffortJointInterface - - - - - steer_bot_hardware_gazebo/SteerBotHardwareGazebo - - - - Gazebo/Purple - - \ No newline at end of file diff --git a/atlascar2_calibration/urdf/optimized/optimized_2022_05_06-04_49_49_PM.urdf.xacro b/atlascar2_calibration/urdf/optimized/optimized_2022_05_06-04_49_49_PM.urdf.xacro deleted file mode 100644 index 28f21a1..0000000 --- a/atlascar2_calibration/urdf/optimized/optimized_2022_05_06-04_49_49_PM.urdf.xacro +++ /dev/null @@ -1,639 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Grey - - 120 - - 0 0 0 0 -1.5707 1.5707 - 1.0471975512 - - 1272 - 1016 - R8G8B8 - - - 0.5 - 300 - - - gaussian - 0.0 - 0.007 - - - - true - 120 - top_right_camera - image_raw - camera_info - top_right_camera_optical - 0.07 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Grey - - 120 - - 0 0 0 0 -1.5707 1.5707 - 1.0471975512 - - 1272 - 1016 - R8G8B8 - - - 0.5 - 300 - - - gaussian - 0.0 - 0.007 - - - - true - 120 - top_left_camera - image_raw - camera_info - top_left_camera_optical - 0.07 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 0 0 0 0 0 0 - false - 10 - - - - 1875 - 1 - -3.141592653589793 - 3.141592653589793 - - - 16 - 1 - -0.2617993877991494 - 0.2617993877991494 - - - - 0.3 - 131.0 - 0.001 - - - gaussian - 0.0 - 0.0 - - - - top_laser/cloud - top_laser - False - 0.9 - 130.0 - 0.008 - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - - hardware_interface/PositionJointInterface - - - - Gazebo/Yellow - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - - hardware_interface/VelocityJointInterface - - - - - - - - - - - - - - - - - - - - - - - true - - 0 0 0 0 0 0 - false - 50 - - - - 720 - 1 - -2.35619 - 2.35619 - - - - 0.1 - 30.0 - 0.01 - - - gaussian - 0.0 - 0.001 - - - - /left_laser/scan - left_laser - / - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - - 0 0 0 0 0 0 - false - 50 - - - - 720 - 1 - -2.35619 - 2.35619 - - - - 0.1 - 30.0 - 0.01 - - - gaussian - 0.0 - 0.001 - - - - /right_laser/scan - right_laser - / - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - - hardware_interface/EffortJointInterface - - - - transmission_interface/SimpleTransmission - - - hardware_interface/EffortJointInterface - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - - hardware_interface/EffortJointInterface - - - - transmission_interface/SimpleTransmission - - - hardware_interface/EffortJointInterface - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - - hardware_interface/EffortJointInterface - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - - hardware_interface/EffortJointInterface - - - - - steer_bot_hardware_gazebo/SteerBotHardwareGazebo - - - - Gazebo/Purple - - \ No newline at end of file diff --git a/atlascar2_calibration/urdf/optimized/optimized_2022_05_06-04_51_12_PM.urdf.xacro b/atlascar2_calibration/urdf/optimized/optimized_2022_05_06-04_51_12_PM.urdf.xacro deleted file mode 100644 index d488648..0000000 --- a/atlascar2_calibration/urdf/optimized/optimized_2022_05_06-04_51_12_PM.urdf.xacro +++ /dev/null @@ -1,639 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Grey - - 120 - - 0 0 0 0 -1.5707 1.5707 - 1.0471975512 - - 1272 - 1016 - R8G8B8 - - - 0.5 - 300 - - - gaussian - 0.0 - 0.007 - - - - true - 120 - top_right_camera - image_raw - camera_info - top_right_camera_optical - 0.07 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Grey - - 120 - - 0 0 0 0 -1.5707 1.5707 - 1.0471975512 - - 1272 - 1016 - R8G8B8 - - - 0.5 - 300 - - - gaussian - 0.0 - 0.007 - - - - true - 120 - top_left_camera - image_raw - camera_info - top_left_camera_optical - 0.07 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 0 0 0 0 0 0 - false - 10 - - - - 1875 - 1 - -3.141592653589793 - 3.141592653589793 - - - 16 - 1 - -0.2617993877991494 - 0.2617993877991494 - - - - 0.3 - 131.0 - 0.001 - - - gaussian - 0.0 - 0.0 - - - - top_laser/cloud - top_laser - False - 0.9 - 130.0 - 0.008 - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - - hardware_interface/PositionJointInterface - - - - Gazebo/Yellow - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - - hardware_interface/VelocityJointInterface - - - - - - - - - - - - - - - - - - - - - - - true - - 0 0 0 0 0 0 - false - 50 - - - - 720 - 1 - -2.35619 - 2.35619 - - - - 0.1 - 30.0 - 0.01 - - - gaussian - 0.0 - 0.001 - - - - /left_laser/scan - left_laser - / - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - - 0 0 0 0 0 0 - false - 50 - - - - 720 - 1 - -2.35619 - 2.35619 - - - - 0.1 - 30.0 - 0.01 - - - gaussian - 0.0 - 0.001 - - - - /right_laser/scan - right_laser - / - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - - hardware_interface/EffortJointInterface - - - - transmission_interface/SimpleTransmission - - - hardware_interface/EffortJointInterface - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - - hardware_interface/EffortJointInterface - - - - transmission_interface/SimpleTransmission - - - hardware_interface/EffortJointInterface - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - - hardware_interface/EffortJointInterface - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - - hardware_interface/EffortJointInterface - - - - - steer_bot_hardware_gazebo/SteerBotHardwareGazebo - - - - Gazebo/Purple - - \ No newline at end of file diff --git a/atlascar2_calibration/urdf/optimized/optimized_2022_05_06-04_54_01_PM.urdf.xacro b/atlascar2_calibration/urdf/optimized/optimized_2022_05_06-04_54_01_PM.urdf.xacro deleted file mode 100644 index ced11a6..0000000 --- a/atlascar2_calibration/urdf/optimized/optimized_2022_05_06-04_54_01_PM.urdf.xacro +++ /dev/null @@ -1,639 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Grey - - 120 - - 0 0 0 0 -1.5707 1.5707 - 1.0471975512 - - 1272 - 1016 - R8G8B8 - - - 0.5 - 300 - - - gaussian - 0.0 - 0.007 - - - - true - 120 - top_right_camera - image_raw - camera_info - top_right_camera_optical - 0.07 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Grey - - 120 - - 0 0 0 0 -1.5707 1.5707 - 1.0471975512 - - 1272 - 1016 - R8G8B8 - - - 0.5 - 300 - - - gaussian - 0.0 - 0.007 - - - - true - 120 - top_left_camera - image_raw - camera_info - top_left_camera_optical - 0.07 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 0 0 0 0 0 0 - false - 10 - - - - 1875 - 1 - -3.141592653589793 - 3.141592653589793 - - - 16 - 1 - -0.2617993877991494 - 0.2617993877991494 - - - - 0.3 - 131.0 - 0.001 - - - gaussian - 0.0 - 0.0 - - - - top_laser/cloud - top_laser - False - 0.9 - 130.0 - 0.008 - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - - hardware_interface/PositionJointInterface - - - - Gazebo/Yellow - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - - hardware_interface/VelocityJointInterface - - - - - - - - - - - - - - - - - - - - - - - true - - 0 0 0 0 0 0 - false - 50 - - - - 720 - 1 - -2.35619 - 2.35619 - - - - 0.1 - 30.0 - 0.01 - - - gaussian - 0.0 - 0.001 - - - - /left_laser/scan - left_laser - / - - - - - - - - - - - - - - - - - - - - - - - - - - - - - true - - 0 0 0 0 0 0 - false - 50 - - - - 720 - 1 - -2.35619 - 2.35619 - - - - 0.1 - 30.0 - 0.01 - - - gaussian - 0.0 - 0.001 - - - - /right_laser/scan - right_laser - / - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - - hardware_interface/EffortJointInterface - - - - transmission_interface/SimpleTransmission - - - hardware_interface/EffortJointInterface - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - - hardware_interface/EffortJointInterface - - - - transmission_interface/SimpleTransmission - - - hardware_interface/EffortJointInterface - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - - hardware_interface/EffortJointInterface - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - - hardware_interface/EffortJointInterface - - - - - steer_bot_hardware_gazebo/SteerBotHardwareGazebo - - - - Gazebo/Purple - - \ No newline at end of file diff --git a/atlascar2_odom/launch/odom_bringup.launch b/atlascar2_odom/launch/odom_bringup.launch new file mode 100755 index 0000000..1687655 --- /dev/null +++ b/atlascar2_odom/launch/odom_bringup.launch @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/atlascar2_odom/nodes/ackermann_to_odom.py b/atlascar2_odom/nodes/ackermann_to_odom.py index 8bd6802..8d59bf2 100755 --- a/atlascar2_odom/nodes/ackermann_to_odom.py +++ b/atlascar2_odom/nodes/ackermann_to_odom.py @@ -1,74 +1,36 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 + +from functools import partial +from math import sin, cos -import math -from math import sin, cos, pi import rospy import tf from nav_msgs.msg import Odometry -from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3 +from geometry_msgs.msg import Point, Pose, Quaternion from ackermann_msgs.msg import AckermannDriveStamped -def odom_callback(data): - global last_time, current_time, x, y, th, vx, vy, vth, wheelbase, covariance_twist, covariance_pose - global odom_pub, odom_broadcaster - current_time = rospy.Time.now() - - # compute odometry in a typical way given the velocities of the robot - dt = (current_time - last_time).to_sec() - delta_x = vx * cos(th) * dt - delta_y = vx * sin(th) * dt - delta_th = vth * dt - x += delta_x - y += delta_y - th += delta_th - - - # vx of the vehicle is the speed and vth is the angular velocity - vx = data.drive.speed - vy = 0.0 - vth = data.drive.steering_angle_velocity - - odomMsg = Odometry() - odomMsg.header.stamp = rospy.Time.now() - odomMsg.twist.twist.linear.x = vx - odomMsg.twist.twist.linear.y = vy - odomMsg.twist.twist.angular.z = vth - odomMsg.twist.covariance = covariance_twist - - odomMsg.header.frame_id = 'atlascar2/odom' - odomMsg.child_frame_id = 'atlascar2/base_footprint' - - # since all odometry is 6DOF we'll need a quaternion created from yaw - odom_quat = tf.transformations.quaternion_from_euler(0, 0, th) - - # first, we'll publish the transform over tf - odom_broadcaster.sendTransform( - (x, y, 0.), - odom_quat, - current_time, - "atlascar2/base_footprint", - "atlascar2/odom" - ) +def odomCallback(msg, odom_velocity): + """Subscribe to the ackermann drive message and retrieve its velocity value#s - # set the position - odomMsg.pose.pose = Pose(Point(x, y, 0.), Quaternion(*odom_quat)) - odomMsg.pose.covariance = covariance_pose + Args: + msg (AckermannDriveStamped): Retrieved from /ackermann_steering_controller/ackermann_drive + odom_velocity (dictionary): iterable variable to store the values + """ - # publish the message - odom_pub.publish(odomMsg) - last_time = current_time + odom_velocity['vx'] = msg.drive.speed + odom_velocity['vth'] = msg.drive.steering_angle_velocity def main(): - global last_time, current_time, x, y, th, vx, vy, vth, wheelbase, covariance_twist, covariance_pose - global odom_pub, odom_broadcaster, twist_pub - rospy.init_node('odometry_publisher') - odom_pub = rospy.Publisher("atlascar2/odom", Odometry, queue_size=10) + + # Defining initial variables + odom_velocity = {'vx': 0, 'vy': 0, 'vth': 0} + odomMsg = Odometry() + odom_pub = rospy.Publisher("odom", Odometry, queue_size=10) odom_broadcaster = tf.TransformBroadcaster() - wheelbase = rospy.get_param('~wheelbase', 2.55) current_time = rospy.Time.now() last_time = rospy.Time.now() @@ -78,18 +40,61 @@ def main(): covariance_pose = [0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000.0] - - # subscribe to the ackermann messages - rospy.Subscriber('ackermann_steering_controller/ackermann_drive', AckermannDriveStamped, odom_callback, queue_size=10) + x = 0.0 y = 0.0 th = 0.0 - vx = 0.0 - vy = 0.0 - vth = 0.0 + # Defining callback partial + odomCallback_partial = partial(odomCallback, odom_velocity=odom_velocity) - rospy.spin() + # subscribe to the ackermann messages + rospy.Subscriber('ackermann_steering_controller/ackermann_drive', AckermannDriveStamped, odomCallback_partial, queue_size=10) + + + rate = rospy.Rate(10) + + while not rospy.is_shutdown(): + # compute odometry in a typical way given the velocities of the robot + dt = (current_time - last_time).to_sec() + delta_x = odom_velocity['vx'] * cos(th) * dt + delta_y = odom_velocity['vx'] * sin(th) * dt + delta_th = odom_velocity['vth'] * dt + x += delta_x + y += delta_y + th += delta_th + + odomMsg.header.stamp = rospy.Time.now() + odomMsg.twist.twist.linear.x = odom_velocity['vx'] + odomMsg.twist.twist.linear.y = odom_velocity['vy'] + odomMsg.twist.twist.angular.z = odom_velocity['vth'] + odomMsg.twist.covariance = covariance_twist + + odomMsg.header.frame_id = 'odom' + odomMsg.child_frame_id = 'base_footprint' + + # since all odometry is 6DOF we'll need a quaternion created from yaw + odom_quat = tf.transformations.quaternion_from_euler(0, 0, th) + + # first, we'll publish the transform over tf + odom_broadcaster.sendTransform( + (x, y, 0.), + odom_quat, + current_time, + "base_footprint", + "odom" + ) + + # set the position + odomMsg.pose.pose = Pose(Point(x, y, 0.), Quaternion(*odom_quat)) + odomMsg.pose.covariance = covariance_pose + + # publish the message + odom_pub.publish(odomMsg) + last_time = current_time + current_time = rospy.Time.now() + + rate.sleep() if __name__ == '__main__': diff --git a/atlascar2_odom/nodes/can_ros_msgs_to_ackermann.py b/atlascar2_odom/nodes/can_ros_msgs_to_ackermann.py index 062dd3b..1f1ba04 100755 --- a/atlascar2_odom/nodes/can_ros_msgs_to_ackermann.py +++ b/atlascar2_odom/nodes/can_ros_msgs_to_ackermann.py @@ -7,7 +7,7 @@ import math -def can_msgs_callback(msg, can_msgs): +def canMsgsCallback(msg, can_msgs): if msg.id == 566: can_msgs['previous_angle'] = can_msgs['angle'] can_msgs['angle'] = msg.data @@ -17,24 +17,24 @@ def can_msgs_callback(msg, can_msgs): can_msgs['pulse_time'] = rospy.Time.now() can_msgs['new_pulse'] = True -def receive_all(ack_pub): +def processCANMessages(ack_pub): """Receives the steering angle and encoder tick messages""" steering_angle = 0 steer_velocity = 0 speed = 0 ackMsg = AckermannDriveStamped() maxPPR = 1000 - wheelbase = rospy.get_param('~wheelbase', 2.55) + wheelbase = rospy.get_param('~wheelbase', 2.55/1.115) wheel_radius = 0.285 can_msgs = {'previous_angle': None, 'previous_angle_time': rospy.Time.now(), 'angle': None, 'angle_time': rospy.Time.now(), 'new_angle': False, 'previous_pulse': None, 'previous_pulse_time': rospy.Time.now(), 'pulse': None, 'pulse_time': rospy.Time.now(), 'new_pulse': False} - can_msgs_callback_partial = partial(can_msgs_callback, can_msgs=can_msgs) - rospy.Subscriber('/received_messages', Frame, can_msgs_callback_partial) + can_msgs_callback_partial = partial(canMsgsCallback, can_msgs=can_msgs) + rospy.Subscriber('/can_messages', Frame, can_msgs_callback_partial) while not rospy.is_shutdown(): # print(can_msgs) if can_msgs['new_pulse']: - if can_msgs['pulse_time'].to_sec() - can_msgs['previous_pulse_time'].to_sec() < 1: + if can_msgs['pulse_time'].to_sec() - can_msgs['previous_pulse_time'].to_sec() < 0.01: continue # calculates the speed in m/s if can_msgs['previous_pulse'] != None: @@ -74,8 +74,7 @@ def receive_all(ack_pub): def main(): rospy.init_node('ackermann_publisher') ack_pub = rospy.Publisher('ackermann_steering_controller/ackermann_drive', AckermannDriveStamped, queue_size=10) - receive_all(ack_pub) - # rospy.spin() + processCANMessages(ack_pub) if __name__ == '__main__': diff --git a/atlascar2_odom/icp_visual_odometry.py b/atlascar2_odom/nodes/icp_visual_odometry.py similarity index 87% rename from atlascar2_odom/icp_visual_odometry.py rename to atlascar2_odom/nodes/icp_visual_odometry.py index b69c228..503dfa5 100755 --- a/atlascar2_odom/icp_visual_odometry.py +++ b/atlascar2_odom/nodes/icp_visual_odometry.py @@ -70,8 +70,6 @@ def ICPTransformations(source_point_cloud, target_point_cloud, threshold, T_init o3d.pipelines.registration.TransformationEstimationPointToPoint(), o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=200000)) print(reg_p2p) - print("Transformation is:") - print(reg_p2p.transformation) if show_images: drawRegistrationResults(source_point_cloud, target_point_cloud, reg_p2p.transformation, T_init) @@ -103,6 +101,7 @@ def main(): required=True) ap.add_argument("-s", "--sensor", help="Sensor used.", type=str, required=True) ap.add_argument("-si", "--show_images", help="If true the script shows images.", action='store_true', default=False) + ap.add_argument("-2d", "--2_dimensions", help="If true the script assumes only variation in 2D, that is, in x, y and yaw.", action='store_true', default=False) # Save args args = vars(ap.parse_args()) @@ -114,7 +113,7 @@ def main(): dataset = json.load(f) # Define variables - threshold = 0.1 + threshold = 0.2 od = OrderedDict(sorted(dataset['collections'].items(), key=lambda t: int(t[0]))) pointclouds = {} transforms = {} @@ -143,20 +142,19 @@ def main(): print('The sensor ' + args['sensor'] + ' does not have a pointcloud to retrieve, so ICP would not work.\nShutting down...') exit(0) - print('Source collection is ' + list(od.keys())[0]) - - od = {'001': None, '002': None, '003': None} + print('\nSource collection is ' + list(od.keys())[0]) # Defining source points source_point_cloud = pointclouds[list(od.keys())[0]] while True: - source_picked_points = pickPoints(source_point_cloud) - # Conditions of alignment - if not (len(source_picked_points) >= 3): - print('\nYou have chosen less than 3 points in the pointcloud, please redo it.') - continue - else: - break + source_picked_points = pickPoints(source_point_cloud) + # Conditions of alignment + if not (len(source_picked_points) >= 3): + print('\nYou have chosen less than 3 points in the pointcloud, please redo it.') + continue + else: + break + for collection_key, collection in od.items(): target_point_cloud = pointclouds[collection_key] @@ -185,12 +183,26 @@ def main(): p2p = o3d.pipelines.registration.TransformationEstimationPointToPoint() T_target_to_source = p2p.compute_transformation(source_point_cloud, target_point_cloud, o3d.utility.Vector2iVector(corr)) - + + + if args['2_dimensions']: + T_target_to_source[2, 0:3] = np.array([0, 0, 1]) + T_target_to_source[0:3, 2] = np.array([0, 0, 1]) + T_target_to_source[2, 3] = 0 + + # Aligning using ICP print('T_target_to_source = \n' + str(T_target_to_source)) reg_p2p = ICPTransformations(source_point_cloud, target_point_cloud, threshold, T_target_to_source, show_images) - transforms[collection_key] = reg_p2p.transformation - print(reg_p2p.transformation) + transform = np.linalg.inv(reg_p2p.transformation) + if args['2_dimensions']: + transform[2, 0:3] = np.array([0, 0, 1]) + transform[0:3, 2] = np.array([0, 0, 1]) + transform[2, 3] = 0 + print("Transformation is:") + print(transform) + transforms[collection_key] = transform + # Saving the dataset print(Fore.YELLOW + '\nSaving odometry transformations' + Style.RESET_ALL)