Merge pull request #80 from hello-robot/bugfix/navigation_config_noetic

Bugfix/navigation config noetic

stretch_demos/nodes/open_drawer

@@ -36,18 +36,18 @@ class OpenDrawerNode(hm.HelloNode):
         self.letter_height_m = 0.2
         self.wrist_position = None
         self.lift_position = None
-        
+
     def joint_states_callback(self, joint_states):
-        with self.joint_states_lock: 
+        with self.joint_states_lock:
             self.joint_states = joint_states
         wrist_position, wrist_velocity, wrist_effort = hm.get_wrist_state(joint_states)
-        self.wrist_position = wrist_position        
+        self.wrist_position = wrist_position
         lift_position, lift_velocity, lift_effort = hm.get_lift_state(joint_states)
         self.lift_position = lift_position
-        
+
     def align_to_surface(self):
         rospy.loginfo('align_to_surface')
-        trigger_request = TriggerRequest() 
+        trigger_request = TriggerRequest()
         trigger_result = self.trigger_align_with_nearest_cliff_service(trigger_request)
         rospy.loginfo('trigger_result = {0}'.format(trigger_result))
 
@@ -60,7 +60,7 @@ class OpenDrawerNode(hm.HelloNode):
         extension_contact_effort = 18.5 #effort_pct #42.0 #40.0 from funmap
         pose = {'wrist_extension': (extension_m, extension_contact_effort)}
         self.move_to_pose(pose, custom_contact_thresholds=True)
-        
+
     def lower_hook_until_contact(self):
         rospy.loginfo('lower_hook_until_contact')
         max_drop_m = 0.15
@@ -94,7 +94,7 @@ class OpenDrawerNode(hm.HelloNode):
             pose = {'joint_lift': lift_m}
             self.move_to_pose(pose)
             rospy.sleep(0.5) # wait for new lift position
-        
+
     def backoff_from_surface(self):
         rospy.loginfo('backoff_from_surface')
         if self.wrist_position is not None:
@@ -131,7 +131,7 @@ class OpenDrawerNode(hm.HelloNode):
         else:
             rospy.logerr('pull_open: self.wrist_position is None!')
             return False
-        
+
     def move_to_initial_configuration(self):
         initial_pose = {'wrist_extension': 0.01,
                         'joint_wrist_yaw': 1.570796327,
@@ -141,7 +141,7 @@ class OpenDrawerNode(hm.HelloNode):
 
     def trigger_open_drawer_down_callback(self, request):
         return self.open_drawer('down')
-        
+
     def trigger_open_drawer_up_callback(self, request):
         return self.open_drawer('up')
 
@@ -158,11 +158,11 @@ class OpenDrawerNode(hm.HelloNode):
                 message='Failed to backoff from the surface.'
             )
 
-        if direction == 'down': 
+        if direction == 'down':
             self.lower_hook_until_contact()
         elif direction == 'up':
             self.raise_hook_until_contact()
-            
+
         success = self.pull_open()
         if not success:
             return TriggerResponse(
@@ -171,21 +171,21 @@ class OpenDrawerNode(hm.HelloNode):
             )
 
         push_drawer_closed = False
-        if push_drawer_closed: 
+        if push_drawer_closed:
             rospy.sleep(3.0)
             self.push_closed()
-        
+
         return TriggerResponse(
             success=True,
             message='Completed opening the drawer!'
             )
 
-    
+
     def main(self):
         hm.HelloNode.main(self, 'open_drawer', 'open_drawer', wait_for_first_pointcloud=False)
 
         self.joint_states_subscriber = rospy.Subscriber('/stretch/joint_states', JointState, self.joint_states_callback)
-        
+
         self.trigger_open_drawer_service = rospy.Service('/open_drawer/trigger_open_drawer_down',
                                                          Trigger,
                                                          self.trigger_open_drawer_down_callback)
@@ -193,8 +193,8 @@ class OpenDrawerNode(hm.HelloNode):
         self.trigger_open_drawer_service = rospy.Service('/open_drawer/trigger_open_drawer_up',
                                                          Trigger,
                                                          self.trigger_open_drawer_up_callback)
-        
-        
+
+
         rospy.wait_for_service('/funmap/trigger_align_with_nearest_cliff')
         rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_align_with_nearest_cliff.')
         self.trigger_align_with_nearest_cliff_service = rospy.ServiceProxy('/funmap/trigger_align_with_nearest_cliff', Trigger)
@@ -206,12 +206,12 @@ class OpenDrawerNode(hm.HelloNode):
         rospy.wait_for_service('/funmap/trigger_lower_until_contact')
         rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_lower_until_contact.')
         self.trigger_lower_until_contact_service = rospy.ServiceProxy('/funmap/trigger_lower_until_contact', Trigger)
-        
+
         rate = rospy.Rate(self.rate)
         while not rospy.is_shutdown():
             rate.sleep()
 
-        
+
 if __name__ == '__main__':
     try:
         parser = ap.ArgumentParser(description='Open Drawer behavior for stretch.')

stretch_navigation/config/base_local_planner_params.yaml

@@ -1,11 +1,43 @@
+# Base profile
 TrajectoryPlannerROS:
-    max_vel_x: 0.1
-    min_vel_x: 0.01
-    max_vel_theta: 0.1
-    min_in_place_vel_theta: 0.05
+  acc_lim_x: 1.0
+  acc_lim_y: 0.0
+  acc_lim_theta: 1.0
+  max_vel_x: 0.3
+  min_vel_x: 0.0
+  max_vel_y: 0.0
+  min_vel_y: 0.0
+  max_vel_theta: 1.0
+  min_vel_theta: -1.0
+  min_in_place_vel_theta: 0
+  holonomic_robot: false
+  dwa: false
+
+  # Goal Tolerance Parameters
+  yaw_goal_tolerance: 0.09          # (5 degrees) The tolerance in radians for the controller in yaw/rotation when achieving its goal
+  xy_goal_tolerance: 0.10           # (10cm) The tolerance in meters for the controller in the x & y distance when achieving a goal
+  latch_xy_goal_tolerance: false    # If goal tolerance is latched, if the robot ever reaches the goal xy location it will simply rotate in place, even if it ends up outside the goal tolerance while it is doing so.
+
+
+  # forward simulation
+  sim_time: 1.7
+  sim_granularity: 0.025
+  angular_sim_granularity: 0.025
+  vx_samples: 5
+  vtheta_samples: 20
+
+  ## scoring (defaults)
+  meter_scoring: true
+  path_distance_bias: 5.0 # 3 # 0.5
+  goal_distance_bias: 5.0 #1.5 #0.75
+  occdist_scale: 0.02 #0.00625
+
+  pdist_scale: 5.0
+  gdist_scale: 5.0
+  occdist_scale: 0.01
+
+  heading_lookahead: 0.325
+  heading_scoring_timestep: 0.8
+  heading_scoring: true
 
-    acc_lim_theta: 1.0
-    acc_lim_x: 1.0
-    acc_lim_y: 1.0
 
-    holonomic_robot: false

stretch_navigation/config/common_costmap_params.yaml

@@ -1,8 +1,10 @@
 obstacle_range: 2.5
 raytrace_range: 3.0
-robot_radius: 0.2
-inflation_radius: 0.22
+
+footprint: [[0.05, 0.17], [0.05, -0.17], [-0.17, -0.17], [-0.27, -0.06], [-0.27, 0.06], [-0.17, 0.17]]
+footprint_padding: 0.025
+# robot_radius: 0.2
 
 observation_sources: laser_scan_sensor
 
-laser_scan_sensor: {sensor_frame: laser, data_type: LaserScan, topic: scan_filtered, marking: true, clearing: true}
+laser_scan_sensor: {sensor_frame: laser, data_type: LaserScan, topic: scan_filtered, marking: true, clearing: true}

stretch_navigation/config/global_costmap_params_nomap.yaml

@@ -3,3 +3,5 @@ global_costmap:
   robot_base_frame: centered_base_link
   update_frequency: 5.0
   static_map: false
+  inflation_radius: 1.75
+  cost_scaling_factor: 2.58

stretch_navigation/config/global_costmap_params_withmap.yaml

@@ -1,5 +1,7 @@
 global_costmap:
   global_frame: map
-  robot_base_frame: centered_base_link
-  update_frequency: 5.0
+  robot_base_frame: base_link
+  update_frequency:  5.0 
   static_map: true
+  inflation_radius: 1.75
+  cost_scaling_factor: 2.58

stretch_navigation/config/local_costmap_params.yaml

@@ -1,10 +1,12 @@
 local_costmap:
   global_frame: map
-  robot_base_frame: centered_base_link
+  robot_base_frame: base_link
   update_frequency: 5.0
   publish_frequency: 2.0
   static_map: false
   rolling_window: true
-  width: 6.0
-  height: 6.0
+  width: 3.0
+  height: 3.0
   resolution: 0.05
+  inflation_radius: 0.15 # reduce it according to the tight spaces
+  cost_scaling_factor: 1.5

stretch_navigation/launch/navigation.launch

@@ -27,6 +27,7 @@
     <rosparam file="$(find stretch_navigation)/config/local_costmap_params.yaml" command="load" />
     <rosparam file="$(find stretch_navigation)/config/global_costmap_params_withmap.yaml" command="load" />
     <rosparam file="$(find stretch_navigation)/config/base_local_planner_params.yaml" command="load" />
+
     <remap from="/cmd_vel" to="/stretch/cmd_vel" />
   </node>
 

stretch_navigation/rviz/navigation.rviz

@@ -5,6 +5,7 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /Odometry1/Shape1
+        - /Path to Goal1
       Splitter Ratio: 0.6117647290229797
     Tree Height: 539
   - Class: rviz/Selection
@@ -22,7 +23,6 @@ Panels:
     Name: Views
     Splitter Ratio: 0.5
   - Class: rviz/Time
-    Experimental: false
     Name: Time
     SyncMode: 0
     SyncSource: Filtered Laser Scan
@@ -447,14 +447,14 @@ Visualization Manager:
       Radius: 0.009999999776482582
       Shaft Diameter: 0.03999999910593033
       Shaft Length: 0.10000000149011612
-      Topic: /move_base/NavfnROS/plan
+      Topic: /move_base/TrajectoryPlannerROS/global_plan
       Unreliable: false
       Value: true
     - Class: octomap_rviz_plugin/OccupancyGrid
       Enabled: false
-      Max. Height Display: inf
+      Max. Height Display: 3.4028234663852886e+38
       Max. Octree Depth: 16
-      Min. Height Display: -inf
+      Min. Height Display: -3.4028234663852886e+38
       Name: OccupancyGrid
       Octomap Topic: /octomap_full
       Queue Size: 5
@@ -500,7 +500,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 6.752994537353516
+      Distance: 11.901083946228027
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -526,7 +526,7 @@ Window Geometry:
   Height: 836
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000156000002a6fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002a6000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006400000003efc0100000002fb0000000800540069006d0065010000000000000640000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004e4000002a600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002a6fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002a6000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006400000003efc0100000002fb0000000800540069006d0065010000000000000640000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000004e4000002a600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -536,5 +536,5 @@ Window Geometry:
   Views:
     collapsed: false
   Width: 1600
-  X: 0
+  X: 72
   Y: 27