Skip to content

Commit ed67cf4

Browse files
velasalevelasale
committed
2nd grade: millrace stable
1 parent 1f82b79 commit ed67cf4

7 files changed

+49
-20
lines changed

‎CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -203,6 +203,7 @@ catkin_install_python(PROGRAMS
203203
# # myfile2
204204
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
205205
# )
206+
install(DIRECTORY config launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
206207

207208
#############
208209
## Testing ##

‎bin/applevision_motion.py

+7-4
Original file line numberDiff line numberDiff line change
@@ -58,11 +58,11 @@ def __init__(self):
5858
self.aCLast = None
5959
self.aDLast = None
6060

61-
self.pixelTolerance = 10
61+
self.pixelTolerance = 75
6262
self.distTolerance = 0.1
6363
self.deadReck = 0.2
6464

65-
self.scale = np.array((0.001,0.001))
65+
self.scale = np.array((0.01,0.01))
6666

6767
self.A = None
6868
self.B = None
@@ -121,7 +121,7 @@ def tryPlan(self):
121121
else:
122122
# Terminate Process
123123
rospy.logwarn("Done!")
124-
# self.wrapUp()
124+
self.wrapUp()
125125
pass
126126

127127
def wrapUp(self):
@@ -153,9 +153,12 @@ def centerCamera(self, c, d):
153153
# v / (np.linalg.norm(v) + 1e-16)
154154

155155
v = (self.B - self.A)
156+
v = v / np.linalg.norm(v)
157+
158+
rospy.logwarn(v)
156159

157160
joint_goal[0] += (v[0] * self.scale[0])
158-
joint_goal[1] -= (v[1] * self.scale[1])
161+
joint_goal[1] += (v[1] * self.scale[1])
159162

160163
self.move_group.go(joint_goal, wait=True)
161164
self.move_group.stop()

‎bin/applevision_vision.py

+2-1
Original file line numberDiff line numberDiff line change
@@ -79,7 +79,7 @@ def __init__(self, net, topic: str, frame: str,
7979
def run_applevision(self, im: Image):
8080
if rospy.Time.now() - im.header.stamp > rospy.Duration.from_sec(0.1):
8181
# this frame is too old
82-
rospy.logdebug('CV: Ignoring old frame')
82+
rospy.logwarn_throttle_identical(3, 'CV: Ignoring old frame')
8383
return None
8484

8585
# convert to cv2 image
@@ -105,6 +105,7 @@ def run_applevision(self, im: Image):
105105
# print(confidences, boxes, "\n")
106106
if len(confidences) > 0:
107107
# Order them for fast indexing later
108+
boxes = [tuple(b) for b in boxes]
108109
confidences, boxes = zip(
109110
*sorted(zip(confidences, boxes), reverse=True))
110111

‎config/camerainfo.yaml

+20
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,20 @@
1+
image_height: 360
2+
image_width: 640
3+
camera_name: palm_camera
4+
distortion_model: 'plumb_bob'
5+
camera_matrix:
6+
rows: 3
7+
cols: 3
8+
data: [552.09305325, 0.0, 333.68780409, 0.0, 555.91052977, 162.88250186, 0, 0, 1]
9+
distortion_coefficients:
10+
rows: 1
11+
cols: 5
12+
data: [0.06133711, -0.00090462, 0.002165, -0.00302198, -0.30672296]
13+
rectification_matrix:
14+
rows: 3
15+
cols: 3
16+
data: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
17+
projection_matrix:
18+
rows: 3
19+
cols: 4
20+
data: [552.09305325, 0.0, 333.68780409, 0.0, 0.0, 555.91052977, 162.88250186, 0, 0.0, 0.0, 1.0, 0.0]

‎launch/config.launch

-13
Original file line numberDiff line numberDiff line change
@@ -2,17 +2,4 @@
22
<include file="$(find applevision_rospkg)/launch/frames.launch" />
33
<node name="tf2_proxy_run" pkg="applevision_rospkg" type="tf2_proxy_run.py" respawn="true" />
44
<node name="cam_image_proc" pkg="image_proc" type="image_proc" ns="palm_camera" />
5-
<group ns="palm_camera">
6-
<node name="camera_info" pkg="rostopic" type="rostopic"
7-
args="pub camera_info sensor_msgs/CameraInfo
8-
'{header: {seq: 0, stamp: {secs: 0, nsecs: 0}, frame_id: 'palm'},
9-
height: 360, width: 640, distortion_model: 'plumb_bob',
10-
D: [0.06133711, -0.00090462, 0.002165, -0.00302198, -0.30672296],
11-
K: [552.09305325, 0.0, 333.68780409, 0.0, 555.91052977, 162.88250186, 0, 0, 1],
12-
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0],
13-
P: [552.09305325, 0.0, 333.68780409, 0.0, 0.0, 555.91052977, 162.88250186, 0, 0.0, 0.0, 1.0, 0.0],
14-
binning_x: 0, binning_y: 0,
15-
roi: {x_offset: 0, y_offset: 0, height: 360, width: 640, do_rectify: false}}' -r 2"
16-
output="screen"/>
17-
</group>
185
</launch>

‎launch/fake_sensor.launch

+13
Original file line numberDiff line numberDiff line change
@@ -4,4 +4,17 @@
44
<node name="applevision_visualizer" pkg="applevision_rospkg" type="applevision_visualizer.py" respawn="true" />
55
<node name="applevision_fake_sensor_data" pkg="applevision_rospkg" type="applevision_fake_sensor_inputs.py" respawn="true" />
66
<node name="applevision_main" pkg="applevision_rospkg" type="applevision_main.py" respawn="true" />
7+
<group ns="palm_camera">
8+
<node name="camera_info" pkg="rostopic" type="rostopic"
9+
args="pub camera_info sensor_msgs/CameraInfo
10+
'{header: {seq: 0, stamp: {secs: 0, nsecs: 0}, frame_id: 'palm'},
11+
height: 360, width: 640, distortion_model: 'plumb_bob',
12+
D: [0.06133711, -0.00090462, 0.002165, -0.00302198, -0.30672296],
13+
K: [552.09305325, 0.0, 333.68780409, 0.0, 555.91052977, 162.88250186, 0, 0, 1],
14+
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0],
15+
P: [552.09305325, 0.0, 333.68780409, 0.0, 0.0, 555.91052977, 162.88250186, 0, 0.0, 0.0, 1.0, 0.0],
16+
binning_x: 0, binning_y: 0,
17+
roi: {x_offset: 0, y_offset: 0, height: 360, width: 640, do_rectify: false}}' -r 2"
18+
output="screen"/>
19+
</group>
720
</launch>

‎launch/real_sensor_robot.launch

+6-2
Original file line numberDiff line numberDiff line change
@@ -1,15 +1,19 @@
11
<launch>
22
<node name="dist_rosserial" pkg="rosserial_python" type="serial_node.py">
3-
<param name="port" value="/dev/ttyACM0" />
3+
<param name="port" value="/dev/ttyACM1" />
44
<!-- baud can be any value, serial port is virtual -->
55
<param name="baud" value="115200" />
66
</node>
7-
<node name="cam_cv_camera" pkg="cv_camera" type="cv_camera_node" ns="palm_camera">
7+
<node name="palm_camera" pkg="cv_camera" type="cv_camera_node">
8+
<param name="rate" value="30.0" />
89
<param name="device_id" value="0" />
910
<param name="image_width" value="640" />
1011
<param name="image_height" value="360" />
12+
<param name="frame_id" value="palm" />
13+
<param name="camera_info_url" value="file://$(find applevision_rospkg)/config/camerainfo.yaml" />
1114
</node>
1215
<include file="$(find applevision_rospkg)/launch/config.launch" />
16+
<node name="applevision_vision" pkg="applevision_rospkg" type="applevision_vision.py" respawn="true" />
1317
<node name="applevision_visualizer" pkg="applevision_rospkg" type="applevision_visualizer.py" respawn="true" />
1418
<!-- <node name="applevision_main" pkg="applevision_rospkg" type="applevision_main.py" respawn="true" /> -->
1519
</launch>

0 commit comments

Comments
 (0)