Working on Calibrating AtlasCar2 with VLP #32

lardemua · Apr 21, 2022 · b4410f7 · b4410f7
1 parent 36a96bb
commit b4410f7
Show file tree

Hide file tree

Showing 8 changed files with 289 additions and 428 deletions.
diff --git a/atlascar2_bringup/launch/record_sensor_data_sim.launch b/atlascar2_bringup/launch/record_sensor_data_sim.launch
@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<launch>
+    <!--arguments-->
+    <arg name="only_raw_data" default="true"/>
+    <!--Whether or not to record only raw data-->
+    <arg name="bag" default="atlascar2"/>
+
+    <param name="/use_sim_time" value="false"/>
+
+    <!--To record only raw data-->
+    <group if="$(arg only_raw_data)">
+        <node name="rosbag_record" pkg="rosbag" type="record" output="screen" args="
+            /atlascar2/frontal_laser/cloud
+            /atlascar2/left_laser/laserscan
+            /atlascar2/right_laser/laserscan
+            /atlascar2/top_left_camera/camera_info
+            /atlascar2/top_left_camera/image_raw/compressed
+            /atlascar2/top_right_camera/camera_info
+            /atlascar2/top_right_camera/image_raw/compressed
+            /lidar_1/velodyne_points
+            /tf
+            /tf_static
+            /atlascar2/joint_states
+            /atlascar2/ackermann_steering_controller/cmd_vel
+            /atlascar2/ackermann_steering_controller/odom
+            -o /tmp/bag_file.bag">
+        </node>
+
+<!--        http://wiki.ros.org/openni_launch/Tutorials/BagRecordingPlayback-->
+    </group>
+</launch>
diff --git a/atlascar2_calibration/calibration/config.yml b/atlascar2_calibration/calibration/config.yml
@@ -33,7 +33,8 @@
 description_file: "package://atlascar2_description/urdf/atlascar2_ackermann_VLP.urdf.xacro"
 
 # The calibration framework requires a bagfile to extract the necessary data for the calibration.
-bag_file: "$ROS_BAGS/atlascar2/gazebo_fixed.bag"
+# bag_file: "$ROS_BAGS/atlascar2/gazebo_fixed.bag"
+bag_file: "$ROS_BAGS/atlascar2/test_miguel.bag"
 
 # You must define a frame of reference for the optimization process.
 # It must exist in the transformation chains of all the sensors which are being calibrated.
@@ -72,22 +73,22 @@ sensors:
   #   /world_camera/rgb/image_raw
   #
 
-  top_left_camera: 
+  top_left_camera:
     link: "atlascar2/top_left_camera_optical"
     parent_link: "atlascar2/base_link"
     child_link: "atlascar2/top_left_camera"
     topic_name: "/atlascar2/top_left_camera/image_raw"
     throttle: 10
     modality: rgb
-  
-  top_right_camera: 
+
+  top_right_camera:
     link: "atlascar2/top_right_camera_optical"
     parent_link: "atlascar2/base_link"
     child_link: "atlascar2/top_right_camera"
     topic_name: "/atlascar2/top_right_camera/image_raw"
     throttle: 10
     modality: rgb
-  
+
   lidar_1:
     link: "atlascar2/VLP_lidar"
     parent_link: "atlascar2/base_link"
@@ -98,7 +99,6 @@ sensors:
 # The calibration requires a detectable pattern.
 # This section describes the properties of the calibration pattern used in th calibration.
 calibration_pattern:
-
   # The frame id (or link) of the pattern.
   # This link/transformation will be optimized.
   link: "pattern_link"
@@ -130,22 +130,20 @@ calibration_pattern:
   # The border width from the edge corner to the pattern physical edge.
   # Used for 3D sensors and lidars.
   # It can be a scalar (same border in x and y directions), or it can be {'x': ..., 'y': ,,,}
-  border_size: {'x': 0.04, 'y': 0.03}
-
+  border_size: { "x": 0.04, "y": 0.03 }
 
   # The number of corners the pattern has in the X and Y dimensions.
   # Note: The charuco detector uses the number of squares per dimension in its detector.
   # Internally we add a +1 to Y and X dimensions to account for that.
   # Therefore, the number of corners should be used even for the charuco pattern.
-  dimension: {"x": 11, "y": 8}
+  dimension: { "x": 11, "y": 8 }
 
   # The length of the square edge.
   size: 0.06
 
   # The length of the charuco inner marker.
   inner_size: 0.045
 
-
 # Miscellaneous configuration
 
 # If your calibration problem is not fully constrained you should anchored one of the sensors.
@@ -155,4 +153,3 @@ anchored_sensor: ""
 
 # Max time delta (in milliseconds) between sensor data messages when creating a collection.
 max_duration_between_msgs: 1000
-
diff --git a/atlascar2_calibration/calibration/summary.pdf b/atlascar2_calibration/calibration/summary.pdf
diff --git a/atlascar2_calibration/launch/collect_data.launch b/atlascar2_calibration/launch/collect_data.launch
@@ -25,7 +25,7 @@
 @arg marker_size   The size of the interaction marker that is used to trigger a data save.
 
 @arg bag_file  Absolute path to the playing bag.
-    default: $(env ROS_BAGS)/atlascar2/gazebo_fixed.bag
+    default: $(env ROS_BAGS)/atlascar2/test_miguel.bag
 @arg bag_start Playback starting time (in seconds). default: 0.0
 @arg bag_rate  Playback rate. default: 1.0
 -->
@@ -43,7 +43,7 @@
     <arg name="rviz_file" default="$(find atlascar2_calibration)/rviz/collect_data.rviz"/>
     <arg name="description_file" default="$(find atlascar2_calibration)/urdf/initial_estimate.urdf.xacro"/>
     <!-- arguments to be passed onto playbag.launch  -->
-    <arg name="bag_file" default="$(env ROS_BAGS)/atlascar2/gazebo_fixed.bag"/>
+    <arg name="bag_file" default="$(env ROS_BAGS)/atlascar2/test_miguel.bag"/>
     <arg name="bag_start" default="0"/>
     <arg name="bag_rate" default="1"/>
     <arg name="ssl" default="lambda sensor_name: False"/>

diff --git a/atlascar2_calibration/launch/dataset_playback.launch b/atlascar2_calibration/launch/dataset_playback.launch
@@ -22,7 +22,7 @@
     Sets up image decompressors if needed, reads the urdf robot description.
 
 @arg bag_file  Absolute path to the playing bag.
-    default: $(env ROS_BAGS)/atlascar2/gazebo_fixed.bag
+    default: $(env ROS_BAGS)/atlascar2/test_miguel.bag
 @arg bag_start Playback starting time (in seconds). default: 0.0
 @arg bag_rate  Playback rate. default: 1.0
 @arg use_tfs  Use tfs in the bag file or generate new ones using the urdf, joint state messages and the robot state publisher. default: False

diff --git a/atlascar2_calibration/launch/playbag.launch b/atlascar2_calibration/launch/playbag.launch
@@ -22,15 +22,15 @@
     Sets up image decompressors if needed, reads the urdf robot description.
 
 @arg bag_file  Absolute path to the playing bag.
-    default: $(env ROS_BAGS)/atlascar2/gazebo_fixed.bag
+    default: $(env ROS_BAGS)/atlascar2/test_miguel.bag
 @arg bag_start Playback starting time (in seconds). default: 0.0
 @arg bag_rate  Playback rate. default: 1.0
 @arg use_tfs  Use tfs in the bag file or generate new ones using the urdf, joint state messages and the robot state publisher. default: False
 -->
 
 <launch>
 
-    <arg name="bag_file" default="$(env ROS_BAGS)/atlascar2/gazebo_fixed.bag"/>
+    <arg name="bag_file" default="$(env ROS_BAGS)/atlascar2/test_miguel.bag"/>
     <arg name="bag_start" default="0"/>
     <arg name="bag_rate" default="1"/>
     <arg name="optimized" default="false"/>
@@ -87,6 +87,15 @@
     <!-- %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -->
 
 
+    <!-- %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -->
+    <!-- Image Topic Decompression -->
+    <node pkg="image_transport" type="republish" name="decompressor_top_left_camera" output="screen"
+          args="compressed in:=/atlascar2/top_left_camera/image_raw raw out:=/atlascar2/top_left_camera/image_raw"/>
+    <node pkg="image_transport" type="republish" name="decompressor_top_right_camera" output="screen"
+          args="compressed in:=/atlascar2/top_right_camera/image_raw raw out:=/atlascar2/top_right_camera/image_raw"/>
+
+    <!-- %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -->
+
 
 
     <!-- %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -->

diff --git a/atlascar2_calibration/launch/set_initial_estimate.launch b/atlascar2_calibration/launch/set_initial_estimate.launch
@@ -22,7 +22,7 @@
     Rviz interactive markers are used to set the pose of the sensors.
 
 @arg bag_file  Absolute path to the playing bag.
-    default: $(env ROS_BAGS)/atlascar2/gazebo_fixed.bag
+    default: $(env ROS_BAGS)/atlascar2/test_miguel.bag
 @arg bag_start Playback starting time (in seconds). default: 0.0
 @arg bag_rate  Playback rate. default: 1.0
 @arg marker_size  Size of the markers. default: 0.5
@@ -36,7 +36,7 @@
     <arg name="output_file" default="$(find atlascar2_calibration)/urdf/initial_estimate.urdf.xacro"/>
     <arg name="rviz_file" default="$(find atlascar2_calibration)/rviz/set_initial_estimate.rviz"/>
     <!-- arguments to be passed onto playbag.launch  -->
-    <arg name="bag_file" default="$(env ROS_BAGS)/atlascar2/gazebo_fixed.bag"/>
+    <arg name="bag_file" default="$(env ROS_BAGS)/atlascar2/test_miguel.bag"/>
     <arg name="bag_start" default="0"/>
     <arg name="bag_rate" default="1"/>
     <arg name="marker_size" default="0.5"/>