Fujun's commit, all corrected files

Flowerst-0416 · Nov 30, 2022 · a332871 · a332871
1 parent eb1d52d
commit a332871
Show file tree

Hide file tree

Showing 18 changed files with 225 additions and 179 deletions.
diff --git a/README.md b/README.md
@@ -221,13 +221,12 @@ Note: The tag size should not be measured from the outside of the tag. The tag s
 
 ![Tag size example](images/tag_size_example.png)
 
-#### 2.2.2 Calibration Process
 
+#### 2.2.2 Tag Measurement
 - Getting the *camera to Tag* transform: 
 
     - Start the ximea camera node
 
-
     ```shell
     cd ~/calibration_ws
     source devel/setup.bash
@@ -245,8 +244,6 @@ Note: The tag size should not be measured from the outside of the tag. The tag s
 Now the **camera to Tag** transform can be viewed by echoing the **tf** rostopic.
 
 - Getting the *end-effector to Tag* transform: Use the CAD of your April tag holder and Camera holder to obtain the transform.
-- As a sanity check for the calculation, the tag position derived from your **end-effector to Tag** transform and from **end-effector-camera-tag** transform chains should be exactly the same.
-
 #### 2.2.3 Calibration Process
 After properly setting up the Apriltag pose estimation pipeline(that includes image undistortion and publishing updated rectified camera matrix), the **camera to Tag** transform should be available to you.
 

diff --git a/apriltag_ros/.gitignore b/apriltag_ros/.gitignore
@@ -0,0 +1,3 @@
+**/matlab_rosbag-0.5.0-linux64/
+**/*.bag
+.vscode/
diff --git a/apriltag_ros/.travis.yml b/apriltag_ros/.travis.yml
@@ -0,0 +1,21 @@
+# This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git).
+# For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst)
+
+language: generic # optional, just removes the language badge
+services:
+  - docker
+cache:
+  directories:
+    - $HOME/.ccache
+git:
+  quiet: true # optional, silences the cloning of the target repository
+env:
+  global:
+    - CCACHE_DIR=$HOME/.ccache # enables C/C++ caching in industrial_ci
+  matrix: # each line is a job
+    - ROS_DISTRO="melodic"
+    - ROS_DISTRO="noetic"
+install:
+  - git clone --quiet --depth 1 https://github.com/ros-industrial/industrial_ci.git .industrial_ci
+script:
+  - .industrial_ci/travis.sh
diff --git a/apriltag_ros/apriltag_ros/scripts/analyze_image b/apriltag_ros/apriltag_ros/scripts/analyze_image
diff --git a/apriltag_ros/apriltag_ros/src/common_functions.cpp b/apriltag_ros/apriltag_ros/src/common_functions.cpp
@@ -394,7 +394,7 @@ AprilTagDetectionArray TagDetector::detectTags (
       tf::poseStampedMsgToTF(pose, tag_transform);
       tf_pub_.sendTransform(tf::StampedTransform(tag_transform,
                                                  tag_transform.stamp_,
-                                                 image->header.frame_id,
+                                                 "/camera",
                                                  detection_names[i]));
     }
   }

diff --git a/blaser_ros/cfg/LaserDetector.cfg b/blaser_ros/cfg/LaserDetector.cfg
diff --git a/blaser_ros/scripts/im_hsv_shower.py b/blaser_ros/scripts/im_hsv_shower.py
diff --git a/blaser_ros/scripts/im_saver.py b/blaser_ros/scripts/im_saver.py
diff --git a/blaser_ros/scripts/imu_driver.py b/blaser_ros/scripts/imu_driver.py
diff --git a/handeye.ipynb b/handeye.ipynb
diff --git a/multi_calibration/CMakeLists.txt b/multi_calibration/CMakeLists.txt
@@ -89,10 +89,10 @@ find_package(PCL REQUIRED)
 # )
 
 ## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   std_msgs
-# )
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+)
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##
@@ -154,7 +154,6 @@ add_executable(trajectory_planner_node
   src/trajectory_planner_node.cpp
   src/trajectory_planner.cpp
   src/plannerFactory.cpp
-  src/scanning_planner.cpp
 )
 
 add_executable(image_saving_node

diff --git a/multi_calibration/cfg/calib_params.yaml b/multi_calibration/cfg/calib_params.yaml
@@ -1,43 +1,56 @@
 # Primary blaser (with Magsafe) params
-# rectCameraMatrix:
-#   - 6.6760052591980673e+02
-#   - 6.6702754763359098e+02
-#   - 1.2238794285085744e+03
-#   - 9.1994665573314273e+02
-# handeyeTransform:
-#   - -0.04196641  
-#   - 0.0012018   
-#   - 0.05935809
-#   - -0.14216268 
-#   - 0.1234418   
-#   - -0.69520724 
-#   - 0.69371377
-# laserPlane:
-#   - -3.10622
-#   - -102.576
-#   - -100
-#   - 11.1776
-
-# Backup blaser (without Magsafe) with toolchanger params
 rectCameraMatrix:
-  - 763.353
-  - 762.501
-  - 1223.29
-  - 919.153
+  - 6.6760052591980673e+02
+  - 6.6702754763359098e+02
+  - 1.2238794285085744e+03
+  - 9.1994665573314273e+02
 handeyeTransform:
-  - -0.04196641
-  - 0.0012018
-  - 0.05935809
-  - -0.14216268 
-  - 0.1234418  
-  - -0.69520724
-  - 0.69371377
+ # - -0.04196641  
+ # - 0.0012018   
+ # - 0.05935809
+ # - -0.14216268 
+ # - 0.1234418   
+ # - -0.69520724 
+ # - 0.69371377
+   - -0.04229084  
+   - 0.00013644  
+   - 0.08916438
+   - -0.14717839 
+   - 0.12003515  
+   - -0.6935283  
+   - 0.69494502
 laserPlane:
-  - -5.03368
-  - -103.61
+  - -3.10622
+  - -102.576
   - -100
-  - 11.1094
+  - 11.1776
+# Backup blaser (without Magsafe) params
+# rectCameraMatrix:
+#   - 763.353
+#   - 762.501
+#   - 1223.29
+#   - 919.153
+# handeyeTransform:
+#   - -0.06511489
+#   - -0.01076986
+#   - 0.07839755
+#   - 0.13290656 
+#   - -0.13393638  
+#   - 0.69268388
+#   - -0.69612206
+# laserPlane:
+#   - -5.12874
+#   - -103.609
+#   - -100
+#   - 11.2323
 
+  # - -0.04111489
+  # - -0.00076986
+  # - 0.07739755
+  # - 0.13290656 
+  # - -0.13393638  
+  # - 0.69268388
+  # - -0.69612206
 
 # laser Detector params
 
@@ -57,4 +70,5 @@ f_vis: false
 cameraTopic: "/ximea_cam/image_raw/compressed"  # Got rid of the visual republish
 statusTopic: "/execution_status"
 pcTopic: "/laser_point_cloud"
+laserTopic: "/laser_pts"
 
diff --git a/multi_calibration/cfg/trajectory_planner.yaml b/multi_calibration/cfg/trajectory_planner.yaml
@@ -1,15 +1,15 @@
 homePositions:
-  - 1.570796        # shoulder_pan_joint
-  - -2.35619449019  # shoulder_lift_joint
-  - 2.35619449019   # elbow_joint
-  - -3.14159        # wrist_1_joint
-  - -1.570796       # wrist_2_joint
+  - -1.570796        # shoulder_pan_joint
+  - -1.570796  # shoulder_lift_joint
+  - -1.570796   # elbow_joint
+  - 0        # wrist_1_joint
+  - 1.570796       # wrist_2_joint
   - 0.0             # wrist_3_joint
 
 bedOrigin:
-  - -0.113007152    # x
-  - 0.494493063     # y
-  - 0.554776374     # z
+  - 0.081    # x
+  - 0.61     # y
+  - 0.430     # z
 
 cartVelocityScaling: 0.07
 cartAccelerationScaling: 0.02

diff --git a/multi_calibration/cfg/ximea_80_calib.yaml b/multi_calibration/cfg/ximea_80_calib.yaml
@@ -1,41 +1,41 @@
 %YAML:1.0
 ---
 # Primary blaser camera (with Magsafe)
-# model_type: MEI
-# camera_name: camera
-# image_width: 2448
-# image_height: 1840
-# mirror_parameters:
-#    xi: 1.8516756874116116e+00
-# distortion_parameters:
-#    k1: -8.1988658236129097e-01
-#    k2: 6.3879257711668663e-02
-#    p1: 8.5244705461647498e-03
-#    p2: -8.1949134295897057e-03
-# projection_parameters:
-#    gamma1: 3.3459441827828582e+03
-#    gamma2: 3.3406052649685403e+03
-#    u0: 1.2074996866814947e+03
-#    v0: 8.7172321297820872e+02
-# width: 2448
-# height: 1840
-
-# Backup blaser camera (without Magsafe) with toolchanger
 model_type: MEI
 camera_name: camera
 image_width: 2448
 image_height: 1840
 mirror_parameters:
-   xi: 2.2233477162
+   xi: 1.8516756874116116e+00
 distortion_parameters:
-   k1: -0.8088886525
-   k2: -0.8067357586
-   p1: 0.0079433344
-   p2: -0.0037897510
+   k1: -8.1988658236129097e-01
+   k2: 6.3879257711668663e-02
+   p1: 8.5244705461647498e-03
+   p2: -8.1949134295897057e-03
 projection_parameters:
-   gamma1: 3811.1549321366
-   gamma2: 3805.6811535143
-   u0: 1221.7500158745
-   v0: 892.4705799546
+   gamma1: 3.3459441827828582e+03
+   gamma2: 3.3406052649685403e+03
+   u0: 1.2074996866814947e+03
+   v0: 8.7172321297820872e+02
 width: 2448
-height: 1840
+height: 1840
+
+# Backup blaser camera (without Magsafe)
+# model_type: MEI
+# camera_name: camera
+# image_width: 2448
+# image_height: 1840
+# mirror_parameters:
+#    xi: 2.2233477162
+# distortion_parameters:
+#    k1: -0.8088886525
+#    k2: -0.8067357586
+#    p1: 0.0079433344
+#    p2: -0.0037897510
+# projection_parameters:
+#    gamma1: 3811.1549321366
+#    gamma2: 3805.6811535143
+#    u0: 1221.7500158745
+#    v0: 892.4705799546
+# width: 2448
+# height: 1840
diff --git a/multi_calibration/include/trajectory_planner.hpp b/multi_calibration/include/trajectory_planner.hpp
@@ -10,7 +10,6 @@
 #include <tf/transform_listener.h>
 #include <iostream>
 #include "std_msgs/String.h"
-#include <eigen_conversions/eigen_msg.h>
 
 
 #include <moveit/move_group_interface/move_group_interface.h>

diff --git a/multi_calibration/launch/auto_calibration.launch b/multi_calibration/launch/auto_calibration.launch
@@ -31,7 +31,7 @@
     <group if="$(eval arg('hand_eye') == 1)">
         <node name="image_rect_node" pkg="multi_calibration" type="image_rect_node" output="screen" args="$(find multi_calibration)/cfg/ximea_80_calib.yaml" />
         <include file="$(find apriltag_ros)/launch/continuous_detection.launch"/>
-        <rosparam file="$(dirname)/../cfg/calib_params.yaml"/> 
+        <rosparam file="$(dirname)/../cfg/calib_params.yaml"/>
     </group>
 
     <group if="$(eval arg('laser_cam') == 1)">