Caff 80 (#92)

* added support for x-y coordinate waypoints

* lowered camera angle to avoid detecting ground as obstacle

* added global carrot planner

* navigation to first waypoint

* michael tests

* adjust pilones and local costmap issue due to lidar min/max obs height

* tuning planner vels and inflation radius

* basic functionality present. Path planning sucks tho

* need to tune planner

* cleaning for PR

* more cleanup

* create 2 files for waypoints

* newline

* revert camera tilt back to 15

Co-authored-by: Anthony Lem <[email protected]>

description/launch/spawn.launch

@@ -2,7 +2,7 @@
 
     <!-- Spawn at course start -->
     <arg name="world_type" default="grass"/>
-
+    
     <arg name="x" default="0" unless="$(eval arg('world_type') =='pavement')"/>
     <arg name="y" default="-21.5" unless="$(eval arg('world_type') =='pavement')"/>
     <arg name="z" default="0.25" unless="$(eval arg('world_type') =='pavement')"/>

description/urdf/constants.xacro

@@ -20,12 +20,12 @@
 
     <!-- Casters -->
     <xacro:property name="caster_link_mass" value="1.2"/>
-        
+
     <xacro:property name="casters_separation" value="1.12"/>
     <xacro:property name="caster_vertical_offset" value="-0.619167561"/>
-        
+
     <xacro:property name="caster_wheel_mass" value="0.7"/>
-        
+
     <xacro:property name="caster_link_to_wheel_dist_x" value="0.03621719"/>
     <xacro:property name="caster_link_to_wheel_dist_z" value="-0.04361749"/>
 
@@ -108,7 +108,7 @@
     <xacro:property name="ZED_camera_length" value="0.175"/>
     <xacro:property name="ZED_camera_width" value="0.03"/>
     <xacro:property name="ZED_camera_height" value="0.033"/>
-    <xacro:property name="ZED_camera_pitch" value="${15 * pi/180}"/>
+    <xacro:property name="ZED_camera_pitch" value="${15 * pi/180}"/> <!-- 19.66 degrees ensures the camera's field of view overlaps with the end of the lidar readings-->
     <!-- ZED image height/width corresponds to image size for 60Hz refresh-->
     <xacro:property name="ZED_image_refresh" value="60"/>
     <xacro:property name="ZED_image_width" value="3840"/>

nav/load_waypoints/scripts/navigate_waypoints.py

@@ -1,25 +1,28 @@
 #!/usr/bin/env python
 
-from sensor_msgs.msg import NavSatFix
-from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
-from tf.transformations import quaternion_from_euler
-from geometry_msgs.msg import PoseStamped
-from tf import TransformListener
+import json
+import os
+import sys
 
-import rospy, os, json, sys 
+import actionlib
 import rospkg
+import rospy
 import std_msgs.msg
-import actionlib
 import tf
-import utm
 import tf2_ros
+import utm
+from geometry_msgs.msg import PoseStamped
+from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
+from sensor_msgs.msg import NavSatFix
+from tf import TransformListener
+from tf.transformations import quaternion_from_euler
 
 
 class NavigateWaypoints:
     def __init__(self, static_waypoint_file, max_time_for_transform):
         self.waypoints = dict() 
         self.static_waypoint_file = static_waypoint_file
-        self.max_time_for_transform = max_time_for_transform
+        self.max_time_for_transform = max_time_for_transform # Maximum time to wait for the transform. Node shuts down if time limit hit
         self.waited_for_transform = False # Initialize the boolean for whether or waiting has timed out 
 
         self.populate_waypoint_dict() 
@@ -56,8 +59,15 @@ def populate_waypoint_dict(self):
             gps_info = rospy.wait_for_message('gps/fix', NavSatFix)
             # Append the starting gps coordinate to the waypoints dict as the final waypoint
             last_coord_idx = len(self.waypoints) 
+
+            # Append a final waypoint to return to the start (i.e. waypoint to return to start)
             self.waypoints[last_coord_idx] = {
-                'id': last_coord_idx, 'longitude': gps_info.longitude, 'latitude': gps_info.latitude, 'description': 'Initial start location', 'frame_id': 'odom'}
+                'id': last_coord_idx, 
+                'longitude': gps_info.longitude, 
+                'latitude': gps_info.latitude, 
+                'description': 'Initial start location', 
+                'frame_id': 'odom'
+            }
 
             # Show waypoints 
             rospy.loginfo("Successfully loaded waypoints dict")
@@ -87,6 +97,7 @@ def wait_for_utm_transform(self):
             else:
                 try:
                     now = rospy.Time.now()
+
                     # Wait for transform from /map to /utm
                     listener.waitForTransform("map", "/utm", now, rospy.Duration(5.0))
                     rospy.loginfo("Transform found. Time waited for transform: %s s"%(rospy.get_time() - start_time))
@@ -100,29 +111,24 @@ def wait_for_utm_transform(self):
 
     def get_next_waypoint(self):
         waypoint = self.waypoints[self.curr_waypoint_idx]
-        print self.curr_waypoint_idx
+        print(self.curr_waypoint_idx)
         self.curr_waypoint_idx += 1
         return waypoint
-
-            #converts gps coordinated to frame (odom,map,etc)
+
     def get_pose_from_gps(self, longitude, latitude, frame, pose_test_var = None):
-        utm_coords = utm.from_latlon(latitude, longitude)
+        '''converts gps coordinated to frame (odom,map,etc)'''
 
-        # create PoseStamped message to set up for do_transform_pose 
+        # create PoseStamped message to set up for do_transform_pose
+        utm_coords = utm.from_latlon(latitude, longitude)
         utm_pose = PoseStamped()
         utm_pose.header.frame_id = 'utm'
         utm_pose.pose.position.x = utm_coords[0]
         utm_pose.pose.position.y = utm_coords[1]
-        utm_pose.pose.orientation.w = 1.0 #make sure its right side up
+        utm_pose.pose.orientation.w = 1.0 # to make sure its right side up
 
-        #t = self.tf.getLatestCommonTime("/"+frame, "utm")
         p_in_frame = self.tf.transformPose("/"+frame, utm_pose)
-        #print p_in_frame
 
         return p_in_frame
-
-
-
 
     def send_and_wait_goal_to_move_base(self, curr_waypoint):
         # Create an action client called "move_base" with action definition file "MoveBaseAction"
@@ -131,24 +137,20 @@ def send_and_wait_goal_to_move_base(self, curr_waypoint):
         # Waits until the action server has started up and started listening for goals.
         action_client.wait_for_server()
 
-        pose = self.get_pose_from_gps(curr_waypoint["longitude"], curr_waypoint["latitude"], curr_waypoint["frame_id"])
-
-        # ==> TESTING CODE
-        #pose = self.get_pose_from_gps(None, None, None, pose_test_var = pose)
-
         # Creates a new goal with the MoveBaseGoal constructor
         goal = MoveBaseGoal()
         goal.target_pose.header.frame_id = curr_waypoint["frame_id"]
         goal.target_pose.header.stamp = rospy.Time.now()
 
         # Set goal position and orientation
+        pose = self.get_pose_from_gps(curr_waypoint["longitude"], curr_waypoint["latitude"], curr_waypoint["frame_id"])
         goal.target_pose.pose = pose.pose
 
         # Sends goal and waits until the action is completed (or aborted if it is impossible)
         action_client.send_goal(goal)
 
         # Waits for the server to finish performing the action.
-        finished_within_time = action_client.wait_for_result(rospy.Duration(60))
+        finished_within_time = action_client.wait_for_result(rospy.Duration(6000))
 
         if not finished_within_time:  
             action_client.cancel_goal()  
@@ -159,18 +161,17 @@ def send_and_wait_goal_to_move_base(self, curr_waypoint):
     def navigate_waypoints(self):
         while True:
             curr_waypoint = self.get_next_waypoint()
-
             self.send_and_wait_goal_to_move_base(curr_waypoint)
-   
+
             if (self.curr_waypoint_idx >= len(self.waypoints)):
                 break
 
 
-
 if __name__ == "__main__":
-    static_waypoint_file = 'static_waypoints.json' # File name for static waypoints (provided at competition-time) 
-    max_time_for_transform = 60.0 # Maximum time to wait for the transform. The node times out and shut down if this limit is exceeded.
-
+    # CHANGE THIS TO GET MAP SPECIFIC GPS WAYPOINTS
+    static_waypoint_file = 'static_waypoints_pavement.json'
+    # static_waypoint_file = 'static_waypoints_grass.json'
+
     rospy.init_node('navigate_waypoints')
-    waypoints = NavigateWaypoints(static_waypoint_file, max_time_for_transform)
-    waypoints.navigate_waypoints() 
+    waypoints = NavigateWaypoints(static_waypoint_file, max_time_for_transform=60.0)
+    waypoints.navigate_waypoints()

nav/load_waypoints/scripts/static_waypoints.json → nav/load_waypoints/scripts/static_waypoints_grass.json

@@ -1,4 +1,4 @@
-{ 
+{
     "waypoints" : [
         {
             "id" : 0,
@@ -40,5 +40,5 @@
             "pose_test" : [14.56,-14.39,0,0,0,-1.99],
             "pose_test_norm" : [15.115, -14.65, 0, 0, 0, -1.9989176939225397]
         }
-    ]   
+    ]
 }

nav/load_waypoints/scripts/static_waypoints_pavement.json

@@ -0,0 +1,44 @@
+{
+    "waypoints" : [
+        {
+            "id" : 0,
+            "longitude" : -79.3905355,
+            "latitude" : 43.6571768,
+            "description"  : "Entrance of No-Man's Land Part 1",
+            "test_loc_description": "start no-mans land",
+            "frame_id" : "odom",
+            "pose_test" : [11.2,0,0,0,0,0],
+            "pose_test_norm" : [10,0,0,0,0,0]
+        },
+        {
+            "id" : 1,
+            "longitude" : -79.3903788,
+            "latitude" : 43.6571667,
+            "description"  : "End of No-Man's Land Part 1",
+            "test_loc_description": "Ramp Start",
+            "frame_id" : "odom",
+            "pose_test" : [11.2,0,0,0,0,0],
+            "pose_test_norm" : [10,0,0,0,0,0]
+        },
+        {
+            "id" : 2,
+            "longitude" : -79.3902588,
+            "latitude" : 43.6571678,
+            "description"  : "Start of No-Man's Land Part 2",
+            "test_loc_description": "Ramp End",
+            "frame_id" : "odom",
+            "pose_test" : [11.2,0,0,0,0,0],
+            "pose_test_norm" : [10,0,0,0,0,0]
+        },
+        {
+            "id" : 3,
+            "longitude" : -79.3901090,
+            "latitude" : 43.6571607,
+            "description"  : "Start of No-Man's Land Part 2",
+            "test_loc_description": "Ramp End",
+            "frame_id" : "odom",
+            "pose_test" : [11.2,0,0,0,0,0],
+            "pose_test_norm" : [10,0,0,0,0,0]
+        }
+    ]
+}

nav/nav_stack/config/base_local_planner_params.yaml

@@ -4,13 +4,15 @@ NavfnROS:
   allow_unknown: true
   # A tolerance on the goal point for the planner.
   default_tolerance: 0.1
+  planner_frequency: 100
 
 # Plans robot movement
 TrajectoryPlannerROS:
   max_vel_x: 2.0
   min_vel_x: 0.2
-  max_vel_theta: 0.5
-  min_in_place_vel_theta: 0.3
+  max_vel_theta: 0.8
+  min_vel_theta: -0.8
+  min_in_place_vel_theta: -0.5
 
   acc_lim_theta: 2.0
   acc_lim_x: 2.5
@@ -25,3 +27,13 @@ TrajectoryPlannerROS:
   # Tolerances
   xy_goal_tolerance:  0.5
   yaw_goal_tolerance: 3.14
+
+  # Recovery
+  escape_vel: -0.3
+
+  # Cost function
+  pdist_scale: 0.6 # Default: 0.6
+  gdist_scale: 0.6 # Default: 0.6
+  occdist_scale: 0.01 # Default: 0.01
+  heading_scoring: false # Default: false
+  heading_lookahead: 0.35 # Default: 0.35

nav/nav_stack/config/costmap_common_params.yaml

@@ -2,3 +2,4 @@ max_obstacle_height: 3.3 # Can be a large number, currently matched to the obsta
 footprint: [[-0.734, -0.582], [-0.734, 0.582], [0.734, 0.582], [0.734, -0.582]]
 
 # NOTE: Caffeine is generally parallel to the ground with 3-wheel drive + battery HACK
+observation_sources: laser_scan_sensor

nav/nav_stack/config/global_costmap_params.yaml

@@ -1,46 +1,54 @@
 global_costmap:
   global_frame: odom   # Because we don't have a map to load in, we use the odom frame as our global frame (rather than /map)
   robot_base_frame: base_link
-  update_frequency: 2.0 # Set update rate lower for large maps
+  update_frequency: 5.0 # Set update rate lower for large maps; 20
+  publish_frequency: 2.0
   rolling_window: true  # Set this to true bc static map == false (this makes setting 2D nav goals in rviz work)
   static_map: false     # Not supplying a static map
+  always_send_full_costmap: false
+  resolution: 0.1
 
   # Full width and height of IGVC map
   # TODO: Update size if needed
-  width: 50
-  height: 50
+  width: 100
+  height: 100
+  resolution: 0.1
+
 
   # Parameters are the same as local cost map, but is kept separate for future adjustments of the global cost map
   obstacle_layer:
-    observation_sources: laser_scan_sensor point_cloud_sensor
+    observation_sources: laser_scan_sensor
     enabled: true
     track_unknown_space: true
 
     laser_scan_sensor: {
-      min_obstacle_height: -0.125, # half wheel diameter
-      sensor_frame: base_laser, 
-      data_type: LaserScan, 
-      topic: scan, 
-      marking: true, 
+      min_obstacle_height: -10.0, # In case odometry z-estimate goes too high/low,
+      max_obstacle_height: 10.0,  # we still add lidar scans to costmap
+      sensor_frame: base_laser,
+      data_type: LaserScan,
+      topic: scan,
+      marking: true,
       clearing: true,
       obstacle_range: 3.7,   # Decide how far we should see obstacles, should be less than raytrace range
-      raytrace_range: 4      # max lidar is rated for is 4m
+      raytrace_range: 4,     # max lidar is rated for is 4m
+      inf_is_valid: true,
+      expected_update_rate: 5
     }
 
     point_cloud_sensor: {
-      min_obstacle_height: -0.05, 
-      sensor_frame: zed_camera_link, 
-      data_type: PointCloud2, 
-      topic: zed/zed_node/point_cloud/cloud_registered, 
-      marking: true, 
-      clearing: true, 
+      min_obstacle_height: -0.1,
+      sensor_frame: zed_camera_link,
+      data_type: PointCloud2,
+      topic: zed/zed_node/point_cloud/cloud_registered,
+      marking: true,
+      clearing: true, # true
       obstacle_range: 15.0,
       raytrace_range: 16.0
     }
 
   inflation_layer:
     enabled:              true
-    inflation_radius:     0.4
+    inflation_radius:     0.65  # Tunable parameter (affect navigation plan)
 
   plugins:
     - {name: obstacle_layer,      type: "costmap_2d::ObstacleLayer"}