Skip to content

Commit e273650

Browse files
beneisnerbeneisner
committed
kinda works, calibrates cam 0 but not cam 1 for some reason smh
1 parent 0ce81ba commit e273650

11 files changed

+142
-48
lines changed

‎config/table1.scene

+1-1
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
(noname)++
22
* cam0_beam
3-
0.3564 0.254 0.4317
3+
0.53335 0.254 0.4317
44
0 0 0 1
55
1
66
box

‎launch/calibrate.launch

+2
Original file line numberDiff line numberDiff line change
@@ -40,6 +40,8 @@
4040

4141
<!-- Sensor properties -->
4242
<arg name="color_resolution" value="2160P"/>
43+
<!-- <arg name="color_resolution" value="1080P"/> -->
44+
4345

4446
<!-- ROS node properties. -->
4547
<arg name="required" value="true"/>

‎launch/calibrate_intrinsics.launch

+35
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
<launch>
2+
<arg name="camera_id" default="0"/>
3+
<arg name="sensor_sn" value="$(eval '000003493812,000180921812,000059793712,000263392612'.split(',')[arg('camera_id')])"/>
4+
<arg name="cam_type" default="rgb"/>
5+
<arg name="color_resolution" default="2160P"/>
6+
7+
<arg name="size" default="8x6" />
8+
<arg name="square" default="0.034" />
9+
<arg name="ir_mono8_scaling_factor" value="0.05" />
10+
11+
12+
<group ns="k4a_$(arg camera_id)">
13+
<include file="$(find azure_kinect_ros_driver)/launch/kinect_rgbd.launch">
14+
<arg name="sensor_sn" value="$(arg sensor_sn)"/>
15+
<arg name="tf_prefix" value="k4a_$(arg camera_id)/"/>
16+
<arg name="color_resolution" value="$(arg color_resolution)"/>
17+
<arg name="required" value="true"/>
18+
<arg name="overwrite_robot_description" value="false"/>
19+
<arg name="point_cloud" value="false"/>
20+
<arg name="rgb_point_cloud" value="false"/>
21+
<arg name="ir_mono8_scaling_factor" value="$(arg ir_mono8_scaling_factor)" />
22+
</include>
23+
</group>
24+
25+
<arg name="camera_name" value="/k4a_$(arg camera_id)/$(arg cam_type)" />
26+
<arg name="image_topic" value="$(arg camera_name)/image_raw" />
27+
28+
<node pkg="camera_calibration" name="cameracalibrator" type="cameracalibrator.py" output="screen"
29+
args="--size $(arg size) --square $(arg square)">
30+
<remap from="image" to="$(arg image_topic)" />
31+
<remap from="camera" to="$(arg camera_name)" />
32+
</node>
33+
34+
35+
</launch>

‎launch/launch_cam.launch

+5-4
Original file line numberDiff line numberDiff line change
@@ -10,12 +10,13 @@
1010
<arg name="tf_prefix" value="k4a_$(arg camera_id)/"/>
1111

1212
<!-- Sensor properties -->
13+
<!-- <arg name="color_resolution" value="1080P"/> -->
1314
<arg name="color_resolution" value="1080P"/>
1415
<arg name="wired_sync_mode" value="$(arg wired_sync_mode)"/>
15-
16-
<arg name="point_cloud" value="true"/>
17-
<arg name="rgb_point_cloud" value="true"/>
18-
16+
<arg name="point_cloud" value="false"/>
17+
<arg name="rgb_point_cloud" value="false"/>
18+
<arg name="fps" value="15" />
19+
<arg name="ir_mono8_scaling_factor" value="0.05" />
1920

2021
<!-- ROS node properties. -->
2122
<arg name="required" value="true"/>

‎launch/scrap.launch

+5
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
<node pkg="camera_calibration" name="cameracalibrator" type="cameracalibrator.py" output="screen"
2+
args="--size $(arg size) --square $(arg square)">
3+
<param name="image" value="$(arg image_topic)" />
4+
<param name="camera" value="$(arg camera_name)" />
5+
</node>

‎launch/verify_calibration_multi.launch

+41
Original file line numberDiff line numberDiff line change
@@ -5,10 +5,51 @@
55

66
<!-- Launch static publishers which describe the frame of the tag w.r.t. the robot. -->
77
<include file="$(find rpad_ros)/launch/calibration_frame_publishers.launch" />
8+
<arg name="sensor_sn_0" default="$(eval '000003493812,000180921812,000059793712,000263392612'.split(',')[0])"/>
9+
<arg name="sensor_sn_1" default="$(eval '000003493812,000180921812,000059793712,000263392612'.split(',')[1])"/>
810

911
<!-- Launch the robot planning scene! -->
1012
<include file="$(find rpad_ros)/launch/table1_planning_scene.launch" />
1113

14+
<group ns="k4a_0">
15+
<include file="$(find azure_kinect_ros_driver)/launch/kinect_rgbd.launch">
16+
<arg name="sensor_sn" value="$(arg sensor_sn_0)"/>
17+
<arg name="tf_prefix" value="k4a_0/"/>
18+
<arg name="color_resolution" value="1080P"/>
19+
20+
<arg name="required" value="true"/>
21+
<arg name="overwrite_robot_description" value="false"/>
22+
<arg name="point_cloud" value="false"/>
23+
<arg name="rgb_point_cloud" value="false"/>
24+
<arg name="ir_mono8_scaling_factor" value="0.05" />
25+
</include>
26+
</group>
27+
28+
<include file="$(find rpad_ros)/launch/publish_calibration.launch">
29+
<arg name="namespace_prefix" value="k4a_0"/>
30+
<arg name="tracking_base_frame" value="k4a_0/camera_base"/>
31+
</include>
32+
33+
<group ns="k4a_1">
34+
<include file="$(find azure_kinect_ros_driver)/launch/kinect_rgbd.launch">
35+
<arg name="sensor_sn" value="$(arg sensor_sn_1)"/>
36+
<arg name="tf_prefix" value="k4a_1/"/>
37+
<arg name="color_resolution" value="1080P"/>
38+
39+
<arg name="required" value="true"/>
40+
<arg name="overwrite_robot_description" value="false"/>
41+
<arg name="point_cloud" value="false"/>
42+
<arg name="rgb_point_cloud" value="false"/>
43+
<arg name="ir_mono8_scaling_factor" value="0.05" />
44+
<arg name="node_start_delay" value="2.0"/>
45+
</include>
46+
</group>
47+
48+
<include file="$(find rpad_ros)/launch/publish_calibration.launch">
49+
<arg name="namespace_prefix" value="k4a_1"/>
50+
<arg name="tracking_base_frame" value="k4a_1/camera_base"/>
51+
</include>
52+
1253

1354
<node type="rviz" name="rviz" pkg="rviz" args="-d $(find rpad_ros)/launch/verify_calibration_multi.rviz" />
1455

‎launch/verify_calibration_multi.rviz

+4-4
Original file line numberDiff line numberDiff line change
@@ -119,7 +119,7 @@ Visualization Manager:
119119
Color: 255; 255; 255
120120
Color Transformer: RGB8
121121
Decay Time: 0
122-
Enabled: false
122+
Enabled: true
123123
Invert Rainbow: false
124124
Max Color: 255; 255; 255
125125
Min Color: 0; 0; 0
@@ -134,7 +134,7 @@ Visualization Manager:
134134
Unreliable: false
135135
Use Fixed Frame: true
136136
Use rainbow: true
137-
Value: false
137+
Value: true
138138
- Alpha: 1
139139
Autocompute Intensity Bounds: true
140140
Autocompute Value Bounds:
@@ -147,7 +147,7 @@ Visualization Manager:
147147
Color: 255; 255; 255
148148
Color Transformer: RGB8
149149
Decay Time: 0
150-
Enabled: false
150+
Enabled: true
151151
Invert Rainbow: false
152152
Max Color: 255; 255; 255
153153
Min Color: 0; 0; 0
@@ -162,7 +162,7 @@ Visualization Manager:
162162
Unreliable: false
163163
Use Fixed Frame: true
164164
Use rainbow: true
165-
Value: false
165+
Value: true
166166
Enabled: true
167167
Global Options:
168168
Background Color: 48; 48; 48

‎launch/verify_calibration_single.launch

+17-11
Original file line numberDiff line numberDiff line change
@@ -20,29 +20,35 @@
2020
<!-- Launch the camera driver -->
2121
<group ns="k4a_$(arg camera_id)">
2222

23-
<!-- <include file="$(find azure_kinect_ros_driver)/launch/kinect_rgbd.launch">
23+
<include file="$(find azure_kinect_ros_driver)/launch/kinect_rgbd.launch">
2424
<arg name="sensor_sn" value="$(arg sensor_sn)"/>
2525
<arg name="tf_prefix" value="k4a_$(arg camera_id)/"/>
26-
<arg name="color_resolution" value="2160P"/>
26+
<!-- <arg name="color_resolution" value="2160P"/> -->
27+
<arg name="color_resolution" value="1080P"/>
28+
<arg name="depth_registration" value="false"/>
29+
2730
<arg name="required" value="true"/>
2831
<arg name="overwrite_robot_description" value="false"/>
29-
<arg name="point_cloud" value="true"/>
30-
<arg name="rgb_point_cloud" value="true"/>
32+
<arg name="point_cloud" value="false"/>
33+
<arg name="rgb_point_cloud" value="false"/>
34+
<arg name="ir_mono8_scaling_factor" value="0.05" />
3135
</include>
3236

33-
<include file="$(find rpad_ros)/launch/depth_rect.launch" /> -->
37+
<!-- <include file="$(find rpad_ros)/launch/depth_rect.launch" />
3438
<include file="$(find rpad_ros)/launch/launch_cam.launch">
3539
<arg name="sensor_sn" value="$(arg sensor_sn)"/>
3640
<arg name="camera_id" value="$(arg camera_id)"/>
37-
</include>
41+
</include> -->
3842

39-
<!-- Publish transforms -->
40-
<include file="$(find rpad_ros)/launch/publish_calibration.launch">
41-
<arg name="namespace_prefix" value="k4a_$(arg camera_id)"/>
42-
<arg name="tracking_base_frame" value="k4a_$(arg camera_id)/camera_base"/>
43-
</include>
43+
4444
</group>
4545

46+
<!-- Publish transforms -->
47+
<include file="$(find rpad_ros)/launch/publish_calibration.launch">
48+
<arg name="namespace_prefix" value="k4a_$(arg camera_id)"/>
49+
<arg name="tracking_base_frame" value="k4a_$(arg camera_id)/camera_base"/>
50+
</include>
51+
4652
<!-- Launch the tracker. -->
4753
<include file="$(find rpad_ros)/launch/aruco_single.launch">
4854
<arg name="markerId" value="$(arg aruco_marker_id)"/>

‎launch/verify_calibration_single.rviz

+29-27
Original file line numberDiff line numberDiff line change
@@ -5,10 +5,9 @@ Panels:
55
Property Tree Widget:
66
Expanded:
77
- /Global Options1
8-
- /TF1
98
- /TF1/Frames1
109
- /MotionPlanning1
11-
- /MotionPlanning1/Planning Request1
10+
- /PointCloud21
1211
Splitter Ratio: 0.6176470518112183
1312
Tree Height: 284
1413
- Class: rviz/Selection
@@ -61,18 +60,20 @@ Visualization Manager:
6160
Frame Timeout: 15
6261
Frames:
6362
All Enabled: false
64-
k4a_0/camera_base:
63+
aruco_581:
64+
Value: true
65+
k4a_1/camera_base:
66+
Value: true
67+
k4a_1/camera_body:
68+
Value: true
69+
k4a_1/camera_visor:
70+
Value: true
71+
k4a_1/depth_camera_link:
72+
Value: true
73+
k4a_1/imu_link:
74+
Value: true
75+
k4a_1/rgb_camera_link:
6576
Value: true
66-
k4a_0/camera_body:
67-
Value: false
68-
k4a_0/camera_visor:
69-
Value: false
70-
k4a_0/depth_camera_link:
71-
Value: false
72-
k4a_0/imu_link:
73-
Value: false
74-
k4a_0/rgb_camera_link:
75-
Value: false
7677
panda_EE:
7778
Value: false
7879
panda_K:
@@ -140,16 +141,17 @@ Visualization Manager:
140141
Tree:
141142
world:
142143
panda_link0:
143-
k4a_0/camera_base:
144-
k4a_0/camera_body:
144+
k4a_1/camera_base:
145+
k4a_1/camera_body:
145146
{}
146-
k4a_0/camera_visor:
147+
k4a_1/camera_visor:
147148
{}
148-
k4a_0/depth_camera_link:
149-
k4a_0/imu_link:
150-
{}
151-
k4a_0/rgb_camera_link:
149+
k4a_1/depth_camera_link:
150+
k4a_1/imu_link:
152151
{}
152+
k4a_1/rgb_camera_link:
153+
aruco_581:
154+
{}
153155
panda_link0_sc:
154156
{}
155157
panda_link1:
@@ -526,7 +528,7 @@ Visualization Manager:
526528
Size (Pixels): 3
527529
Size (m): 0.003000000026077032
528530
Style: Flat Squares
529-
Topic: /k4a_0/depth_registered/points
531+
Topic: /k4a_1/depth_to_rgb/points
530532
Unreliable: false
531533
Use Fixed Frame: true
532534
Use rainbow: true
@@ -559,25 +561,25 @@ Visualization Manager:
559561
Views:
560562
Current:
561563
Class: rviz/Orbit
562-
Distance: 3.039633274078369
564+
Distance: 1.3559300899505615
563565
Enable Stereo Rendering:
564566
Stereo Eye Separation: 0.05999999865889549
565567
Stereo Focal Distance: 1
566568
Swap Stereo Eyes: false
567569
Value: false
568570
Field of View: 0.7853981852531433
569571
Focal Point:
570-
X: 0
571-
Y: 0
572-
Z: 0
572+
X: 0.5573740005493164
573+
Y: -0.09734819084405899
574+
Z: 0.10925242304801941
573575
Focal Shape Fixed Size: true
574576
Focal Shape Size: 0.05000000074505806
575577
Invert Z Axis: false
576578
Name: Current View
577579
Near Clip Distance: 0.009999999776482582
578-
Pitch: 0.6153978109359741
580+
Pitch: 0.480397492647171
579581
Target Frame: <Fixed Frame>
580-
Yaw: 1.3735837936401367
582+
Yaw: 0.33857399225234985
581583
Saved: ~
582584
Window Geometry:
583585
Aruco Detection Image:

‎package.xml

+2
Original file line numberDiff line numberDiff line change
@@ -51,6 +51,8 @@
5151
<buildtool_depend>catkin</buildtool_depend>
5252
<depend>rospy</depend>
5353
<depend>aruco_ros</depend>
54+
<depend>tf</depend>
55+
<depend>camera_calibration</depend>
5456

5557
<!-- The export tag contains other, unspecified, tags -->
5658
<export>

‎scripts/table1_planning_scene.py

+1-1
Original file line numberDiff line numberDiff line change
@@ -50,7 +50,7 @@ def init_scene(scene: moveit_commander.PlanningSceneInterface):
5050
cam0_beam_pose.header.frame_id = "/world"
5151
cam0_beam_pose.header.stamp = timestamp
5252
cam0_beam_pose.pose.orientation.w = 1
53-
cam0_beam_pose.pose.position.x = 0.3564
53+
cam0_beam_pose.pose.position.x = table_edge_xn + 0.70485
5454
cam0_beam_pose.pose.position.y = table_edge_yp
5555
cam0_beam_pose.pose.position.z = cam_beam_dz / 2.0 - 0.1778
5656

0 commit comments

Comments
 (0)