diff --git a/stretch_demos/config/reachtoaruco_marker_dict.yaml b/stretch_demos/config/reachtoaruco_marker_dict.yaml
new file mode 100644
index 0000000..5359155
--- /dev/null
+++ b/stretch_demos/config/reachtoaruco_marker_dict.yaml
@@ -0,0 +1,85 @@
+# examples of command line use
+# rosparam load ./stretch_marker_dict.yaml
+# rosparam list
+# rosparam get /aruco_markers/
+
+# Be careful with length_mm values. Stickers and printouts can vary
+# significantly from your expectations. You should measure them
+# directly. For example, in a prototype robot the stickers were closer
+# to 23mm wide than 24mm. This 1mm difference resulted in > 1cm of
+# error in the depths for the marker pose estimates when only using
+# RGB images.
+
+
+'aruco_marker_info':
+ '0':
+ 'length_mm': 47
+ 'use_rgb_only': False
+ 'name': 'test_tag'
+ 'link': 'test_tag'
+ '130':
+ 'length_mm': 47
+ 'use_rgb_only': False
+ 'name': 'base_left'
+ 'link': 'link_aruco_left_base'
+ '131':
+ 'length_mm': 47
+ 'use_rgb_only': False
+ 'name': 'base_right'
+ 'link': 'link_aruco_right_base'
+ '132':
+ 'length_mm': 23.5
+ 'use_rgb_only': False
+ 'name': 'wrist_inside'
+ 'link': 'link_aruco_inner_wrist'
+ '133':
+ 'length_mm': 23.5
+ 'use_rgb_only': False
+ 'name': 'wrist_top'
+ 'link': 'link_aruco_top_wrist'
+ '134':
+ 'length_mm': 31.4
+ 'use_rgb_only': False
+ 'name': 'shoulder_top'
+ 'link': 'link_aruco_shoulder'
+ '245':
+ 'length_mm': 88.0
+ 'use_rgb_only': False
+ 'name': 'docking_station'
+ 'link': None
+ '246':
+ 'length_mm': 179.0
+ 'use_rgb_only': False
+ 'name': 'floor_0'
+ 'link': None
+ '247':
+ 'length_mm': 179.0
+ 'use_rgb_only': False
+ 'name': 'floor_1'
+ 'link': None
+ '248':
+ 'length_mm': 179.0
+ 'use_rgb_only': False
+ 'name': 'floor_2'
+ 'link': None
+ '249':
+ 'length_mm': 179.0
+ 'use_rgb_only': False
+ 'name': 'floor_3'
+ 'link': None
+ '10':
+ 'length_mm': 24
+ 'use_rgb_only': False
+ 'name': 'target_object'
+ 'link': None
+ '21':
+ 'length_mm': 86
+ 'use_rgb_only': False
+ 'name': 'user_pointer'
+ 'link': 'None'
+ 'default':
+ 'length_mm': 24
+ 'use_rgb_only': False
+ 'name': 'unknown'
+ 'link': None
+
\ No newline at end of file
diff --git a/stretch_demos/launch/reach_to_aruco.launch b/stretch_demos/launch/reach_to_aruco.launch
new file mode 100644
index 0000000..98eddd3
--- /dev/null
+++ b/stretch_demos/launch/reach_to_aruco.launch
@@ -0,0 +1,32 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/stretch_demos/nodes/reach_to_aruco b/stretch_demos/nodes/reach_to_aruco
index fd7861d..16c941f 100755
--- a/stretch_demos/nodes/reach_to_aruco
+++ b/stretch_demos/nodes/reach_to_aruco
@@ -1,5 +1,6 @@
#!/usr/bin/env python3
+import time
import rospy
import hello_helpers.hello_misc as hm
from tf2_ros import StaticTransformBroadcaster
@@ -15,6 +16,7 @@ class ReachToMarkerNode(hm.HelloNode):
# 0. 'disengaged' - the node is not active
# 1. 'searching' - the node is searching for the Aruco tag
# 2. 'tracking' - the node is tracking the Aruco tag
+ # 3. 'settling' - the node can't see the Aruco tag due to momentary jitter
self.state = 'searching'
self.target_aruco = 'test_tag'
self.detection_history = [False] * 10
@@ -23,6 +25,7 @@ class ReachToMarkerNode(hm.HelloNode):
self.last_yerr = None
self.pan_target = 0.0
self.tilt_target = 0.0
+ self.settling_starttime = None
def aruco_callback(self, marker_array_msg):
if self.state == 'disengaged':
@@ -36,20 +39,28 @@ class ReachToMarkerNode(hm.HelloNode):
self.detection_history.pop(0)
self.detection_history.append(seen)
detected = max(set(self.detection_history), key=self.detection_history.count)
- self.state = 'tracking' if detected == True else 'searching'
+ if self.state in ['searching', 'settling'] and detected:
+ self.state = 'tracking'
+ if self.state == 'tracking' and not detected:
+ self.state = 'settling'
+ self.settling_starttime = time.time()
+ if self.state == 'settling' and (time.time() > self.settling_starttime + 5.0):
+ self.state = 'searching'
+ self.settling_starttime = None
def searching(self):
if self.state != 'searching':
return
- rospy.loginfo("SEARCHING")
- # reset tracking state variables
- self.stable_history = [False] * 20
- self.last_xerr = None
- self.last_yerr = None
+ rospy.logerr("SEARCHING")
+ # far_right_pan = -3.6
+ # far_left_pan = 1.45
+ # head_tilt = -0.8
+ # num_pan_steps = 7
+ # self.move_to
- # self.pan_target = 0.0
- # self.tilt_target = 0.0
+ self.pan_target = 0.0
+ self.tilt_target = 0.0
self.move_to_pose({'joint_head_pan': self.pan_target, 'joint_head_tilt': self.tilt_target}, return_before_done=True)
def tracking(self):
@@ -64,6 +75,7 @@ class ReachToMarkerNode(hm.HelloNode):
# dead zone since the tag is not moving
self.stable_history.pop(0)
self.stable_history.append(True)
+ rospy.loginfo(' STABLE')
else:
self.stable_history.pop(0)
self.stable_history.append(False)
@@ -81,12 +93,24 @@ class ReachToMarkerNode(hm.HelloNode):
self.pan_target += xcorr
self.tilt_target += ycorr
self.move_to_pose({'joint_head_pan': self.pan_target, 'joint_head_tilt': self.tilt_target}, return_before_done=True)
- rospy.loginfo('pan correction: ' + str(xcorr))
- rospy.loginfo('tilt correction: ' + str(ycorr))
+ rospy.loginfo(' CORRECTION STEP')
+
+ def settling(self):
+ if self.state != 'settling':
+ return
+
+ rospy.logwarn("SETTLING")
+ # reset tracking state variables
+ self.stable_history = [False] * 20
+ self.last_xerr = None
+ self.last_yerr = None
+
+ # wait for jitter to settle
+ self.move_to_pose({'joint_head_pan': self.pan_target, 'joint_head_tilt': self.tilt_target}, return_before_done=True)
def main(self):
- hm.HelloNode.main(self, 'reach_to_marker_node', 'reach_to_marker_node', wait_for_first_pointcloud=False)
+ hm.HelloNode.main(self, 'reach_to_marker_node', 'reach_to_marker_node', wait_for_first_pointcloud=True)
self.br = StaticTransformBroadcaster()
rospy.Subscriber('/aruco/marker_array', MarkerArray, self.aruco_callback)
@@ -95,6 +119,7 @@ class ReachToMarkerNode(hm.HelloNode):
while not rospy.is_shutdown():
self.searching()
self.tracking()
+ self.settling()
rate.sleep()
diff --git a/stretch_demos/rviz/reach_to_aruco.rviz b/stretch_demos/rviz/reach_to_aruco.rviz
new file mode 100644
index 0000000..db9d5fe
--- /dev/null
+++ b/stretch_demos/rviz/reach_to_aruco.rviz
@@ -0,0 +1,539 @@
+Panels:
+ - Class: rviz/Displays
+ Help Height: 78
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /TF1/Frames1
+ Splitter Ratio: 0.5
+ Tree Height: 1079
+ - Class: rviz/Selection
+ Name: Selection
+ - Class: rviz/Tool Properties
+ Expanded:
+ - /2D Pose Estimate1
+ - /2D Nav Goal1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.5886790156364441
+ - Class: rviz/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz/Time
+ Experimental: false
+ Name: Time
+ SyncMode: 0
+ SyncSource: PointCloud2
+Preferences:
+ PromptSaveOnExit: true
+Toolbars:
+ toolButtonStyle: 2
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.029999999329447746
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 10
+ Reference Frame:
+ Value: true
+ - Alpha: 0.800000011920929
+ Class: rviz/RobotModel
+ Collision Enabled: false
+ Enabled: true
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ base_imu:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ base_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ camera_accel_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ camera_accel_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ camera_bottom_screw_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ camera_color_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ camera_color_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ camera_depth_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ camera_depth_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ camera_gyro_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ camera_gyro_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ camera_infra1_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ camera_infra1_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ camera_infra2_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ camera_infra2_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ camera_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ caster_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ laser:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_arm_l0:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_arm_l1:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_arm_l2:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_arm_l3:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_arm_l4:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_aruco_inner_wrist:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_aruco_left_base:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_aruco_right_base:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_aruco_shoulder:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_aruco_top_wrist:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_grasp_center:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ link_gripper:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_gripper_finger_left:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_gripper_finger_right:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_gripper_fingertip_left:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_gripper_fingertip_right:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_head:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_head_pan:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_head_tilt:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_left_wheel:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_lift:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_mast:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_right_wheel:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ link_wrist_yaw:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ respeaker_base:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ Name: RobotModel
+ Robot Description: robot_description
+ TF Prefix: ""
+ Update Interval: 0
+ Value: true
+ Visual Enabled: true
+ - Class: rviz/TF
+ Enabled: true
+ Frame Timeout: 15
+ Frames:
+ All Enabled: false
+ base_imu:
+ Value: false
+ base_link:
+ Value: true
+ camera_accel_frame:
+ Value: false
+ camera_accel_optical_frame:
+ Value: false
+ camera_aligned_depth_to_color_frame:
+ Value: false
+ camera_bottom_screw_frame:
+ Value: false
+ camera_color_frame:
+ Value: true
+ camera_color_optical_frame:
+ Value: false
+ camera_depth_frame:
+ Value: false
+ camera_depth_optical_frame:
+ Value: false
+ camera_gyro_frame:
+ Value: false
+ camera_gyro_optical_frame:
+ Value: false
+ camera_infra1_frame:
+ Value: false
+ camera_infra1_optical_frame:
+ Value: false
+ camera_infra2_frame:
+ Value: false
+ camera_infra2_optical_frame:
+ Value: false
+ camera_link:
+ Value: false
+ caster_link:
+ Value: false
+ laser:
+ Value: false
+ link_arm_l0:
+ Value: false
+ link_arm_l1:
+ Value: false
+ link_arm_l2:
+ Value: false
+ link_arm_l3:
+ Value: false
+ link_arm_l4:
+ Value: false
+ link_aruco_inner_wrist:
+ Value: false
+ link_aruco_left_base:
+ Value: false
+ link_aruco_right_base:
+ Value: false
+ link_aruco_shoulder:
+ Value: false
+ link_aruco_top_wrist:
+ Value: false
+ link_grasp_center:
+ Value: false
+ link_gripper:
+ Value: false
+ link_gripper_finger_left:
+ Value: false
+ link_gripper_finger_right:
+ Value: false
+ link_gripper_fingertip_left:
+ Value: false
+ link_gripper_fingertip_right:
+ Value: false
+ link_head:
+ Value: false
+ link_head_pan:
+ Value: false
+ link_head_tilt:
+ Value: false
+ link_left_wheel:
+ Value: false
+ link_lift:
+ Value: false
+ link_mast:
+ Value: false
+ link_right_wheel:
+ Value: false
+ link_wrist_yaw:
+ Value: false
+ respeaker_base:
+ Value: false
+ Marker Alpha: 1
+ Marker Scale: 1
+ Name: TF
+ Show Arrows: true
+ Show Axes: true
+ Show Names: true
+ Tree:
+ base_link:
+ base_imu:
+ {}
+ caster_link:
+ {}
+ laser:
+ {}
+ link_aruco_left_base:
+ {}
+ link_aruco_right_base:
+ {}
+ link_left_wheel:
+ {}
+ link_mast:
+ link_head:
+ link_head_pan:
+ link_head_tilt:
+ camera_bottom_screw_frame:
+ camera_link:
+ camera_accel_frame:
+ camera_accel_optical_frame:
+ {}
+ camera_aligned_depth_to_color_frame:
+ camera_color_optical_frame:
+ {}
+ camera_color_frame:
+ {}
+ camera_depth_frame:
+ camera_depth_optical_frame:
+ {}
+ camera_gyro_frame:
+ camera_gyro_optical_frame:
+ {}
+ camera_infra1_frame:
+ camera_infra1_optical_frame:
+ {}
+ camera_infra2_frame:
+ camera_infra2_optical_frame:
+ {}
+ link_lift:
+ link_arm_l4:
+ link_arm_l3:
+ link_arm_l2:
+ link_arm_l1:
+ link_arm_l0:
+ link_aruco_inner_wrist:
+ {}
+ link_aruco_top_wrist:
+ {}
+ link_wrist_yaw:
+ link_gripper:
+ link_grasp_center:
+ {}
+ link_gripper_finger_left:
+ link_gripper_fingertip_left:
+ {}
+ link_gripper_finger_right:
+ link_gripper_fingertip_right:
+ {}
+ link_aruco_shoulder:
+ {}
+ respeaker_base:
+ {}
+ link_right_wheel:
+ {}
+ Update Interval: 0
+ Value: true
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 10
+ Min Value: -10
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/PointCloud2
+ Color: 255; 255; 255
+ Color Transformer: RGB8
+ Decay Time: 0
+ Enabled: false
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Min Color: 0; 0; 0
+ Name: PointCloud2
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.009999999776482582
+ Style: Flat Squares
+ Topic: /camera/depth/color/points
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: false
+ - Class: rviz/MarkerArray
+ Enabled: true
+ Marker Topic: /aruco/marker_array
+ Name: MarkerArray
+ Namespaces:
+ {}
+ Queue Size: 100
+ Value: true
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Default Light: true
+ Fixed Frame: base_link
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz/Interact
+ Hide Inactive Objects: true
+ - Class: rviz/MoveCamera
+ - Class: rviz/Select
+ - Class: rviz/FocusCamera
+ - Class: rviz/Measure
+ - Class: rviz/SetInitialPose
+ Theta std deviation: 0.2617993950843811
+ Topic: /initialpose
+ X std deviation: 0.5
+ Y std deviation: 0.5
+ - Class: rviz/SetGoal
+ Topic: /move_base_simple/goal
+ - Class: rviz/PublishPoint
+ Single click: true
+ Topic: /clicked_point
+ Value: true
+ Views:
+ Current:
+ Class: rviz/Orbit
+ Distance: 5.996953964233398
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Field of View: 0.7853981852531433
+ Focal Point:
+ X: 0
+ Y: 0
+ Z: 0
+ Focal Shape Fixed Size: false
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 0.6703981757164001
+ Target Frame:
+ Yaw: 1.7253978252410889
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 1376
+ Hide Left Dock: false
+ Hide Right Dock: false
+ QMainWindow State: 000000ff00000000fd000000040000000000000156000004c2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004c2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004c2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000004c2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a000000003efc0100000002fb0000000800540069006d0065010000000000000a00000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000078f000004c200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: false
+ Width: 2560
+ X: 0
+ Y: 27