Add settling time to handle pan jitter

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
+    

stretch_demos/launch/reach_to_aruco.launch

@@ -0,0 +1,32 @@
+<launch>
+
+  <arg name="rviz" default="true" doc="whether to show Rviz" />
+
+  <!-- REALSENSE D435i -->
+  <include file="$(find stretch_core)/launch/d435i_high_resolution.launch"></include>
+  <node name="d435i_configure" pkg="stretch_core" type="d435i_configure" output="screen">
+    <!--<param name="initial_mode" type="string" value="Default"/>-->
+    <param name="initial_mode" type="string" value="High Accuracy"/>
+  </node>
+  <!-- -->
+
+  <!-- ARUCO MARKER DETECTOR -->
+  <rosparam command="load" file="$(find stretch_demos)/config/reachtoaruco_marker_dict.yaml" />
+  <node name="detect_aruco_markers" pkg="stretch_core" type="detect_aruco_markers" output="screen"/>
+  <!-- -->
+
+  <!-- STRETCH DRIVER -->
+  <param name="/stretch_driver/broadcast_odom_tf" type="bool" value="false"/>
+  <param name="/stretch_driver/fail_out_of_range_goal" type="bool" value="false"/>
+  <include file="$(find stretch_core)/launch/stretch_driver.launch" pass_all_args="true"/>
+  <!-- -->
+
+  <!-- KEYBOARD TELEOP -->
+  <node name="keyboard_teleop" pkg="stretch_core" type="keyboard_teleop" output="screen" />
+  <!-- -->
+
+  <!-- VISUALIZE -->
+  <node name="rviz" pkg="rviz" type="rviz" output="screen" args="-d $(find stretch_demos)/rviz/reach_to_aruco.rviz" if="$(arg rviz)" />
+  <!-- -->
+
+</launch>

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()
 
 

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: <Fixed 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: <Fixed 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