field test

Author: N-rwal

go2_robot_sdk/config/nav2_params_navigation.yaml

@@ -95,19 +95,19 @@ controller_server:
     general_goal_checker:
       stateful: True
       plugin: "nav2_controller::SimpleGoalChecker"
-      xy_goal_tolerance: 0.25
-      yaw_goal_tolerance: 0.25
+      xy_goal_tolerance: 0.4
+      yaw_goal_tolerance: 0.2
     # DWB parameters
     FollowPath:
       plugin: "dwb_core::DWBLocalPlanner"
       debug_trajectory_details: True
       min_vel_x: 0.0
       min_vel_y: 0.0
-      max_vel_x: 3.0 #changed
-      max_vel_y: 0.0
-      max_vel_theta: 3.0 #changed
+      max_vel_x: 2.0 #changed
+      max_vel_y: -1.0
+      max_vel_theta: 1.0
       min_speed_xy: 0.0
-      max_speed_xy: 3.0 #changed
+      max_speed_xy: 3.0
       min_speed_theta: 0.0
       # Add high threshold velocity for turtlebot 3 issue.
       # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
@@ -124,7 +124,7 @@ controller_server:
       linear_granularity: 0.05
       angular_granularity: 0.025
       transform_tolerance: 0.2
-      xy_goal_tolerance: 0.3 #changed
+      xy_goal_tolerance: 0.4
       trans_stopped_velocity: 0.25
       short_circuit_trajectory_evaluation: True
       stateful: True
@@ -151,12 +151,12 @@ local_costmap:
       width: 5
       height: 5
       resolution: 0.05
-      footprint: "[[-0.25, -0.15], [-0.25, 0.15], [0.25, 0.15], [0.25, -0.15]]"
+      footprint: "[[-0.30, -0.20], [-0.30, 0.20], [0.30, 0.20], [0.30, -0.20]]"
       plugins: ["voxel_layer", "inflation_layer"]
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
-        cost_scaling_factor: 1.5
-        inflation_radius: 0.35
+        cost_scaling_factor: 3.5
+        inflation_radius: 0.60
       voxel_layer:
         plugin: "nav2_costmap_2d::VoxelLayer"
         enabled: True
@@ -190,7 +190,7 @@ global_costmap:
       publish_frequency: 2.0
       global_frame: map
       robot_base_frame: base_link
-      footprint: "[[-0.25, -0.15], [-0.25, 0.15], [0.25, 0.15], [0.25, -0.15]]"
+      footprint: "[[-0.30, -0.20], [-0.30, 0.20], [0.30, 0.20], [0.30, -0.20]]"
       width: 100
       height: 100
       resolution: 0.05
@@ -215,8 +215,8 @@ global_costmap:
         map_subscribe_transient_local: True
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
-        cost_scaling_factor: 1.5
-        inflation_radius: 0.35
+        cost_scaling_factor: 3.5
+        inflation_radius: 0.60
       always_send_full_costmap: True
 
 # The yaml_filename does not need to be specified since it going to be set by defaults in launch.
@@ -298,8 +298,8 @@ velocity_smoother:
     smoothing_frequency: 20.0
     scale_velocities: False
     feedback: "OPEN_LOOP"
-    max_velocity: [0.5, 0.0, 1.0]
-    min_velocity: [-0.5, 0.0, -1.0]
+    max_velocity: [0.5, 0.0, 1.5]
+    min_velocity: [-0.5, 0.0, -1.5]
     max_accel: [1.5, 0.0, 2.0]
     max_decel: [-1.5, 0.0, -2.0]
     odom_topic: "odom"

go2_robot_sdk/go2_robot_sdk/initial_pose_pub.py

@@ -11,8 +11,8 @@ def __init__(self):
     def publish_pose(self):
         msg = PoseWithCovarianceStamped()
         msg.header.frame_id = 'map'
-        msg.pose.pose.position.x = -1.0
-        msg.pose.pose.position.y = 0.0
+        msg.pose.pose.position.x = 0.0
+        msg.pose.pose.position.y = -1.0
         msg.pose.pose.orientation.z = 0.0
         msg.pose.pose.orientation.w = 1.0
         msg.pose.covariance = [0.0] * 36

go2_robot_sdk/go2_robot_sdk/sequential_nav_test.py

@@ -56,16 +56,25 @@ def send_goal(self, pose):
         return True
 
     def run_sequence(self):
-        #Forward 2 meters
-        self.send_goal(self.make_pose(1.0, 0.0, 0.0))
-
-        #Rotate 180 degrees in place
-        self.send_goal(self.make_pose(1.0, 0.0, math.pi))
-
-        time.sleep(2.0)
-
-        #return 2 meters
-        self.send_goal(self.make_pose(0.0, 0.0, math.pi))
+        i = 0
+        for i in range (0,2):
+            self.send_goal(self.make_pose(-1.61775, 17.2127, 1.70432)) #main entrance
+            time.sleep(1.0)
+            self.send_goal(self.make_pose(9.57915, 19.4981, -1.21812)) #front main entrance
+            time.sleep(1.0)
+            self.send_goal(self.make_pose(-1.61775, 17.2127, 2.36622)) #main entrance
+            time.sleep(1.0)
+            self.send_goal(self.make_pose(2.27953, -1.53849, -1.41637)) #second hall
+            time.sleep(1.0)
+            self.send_goal(self.make_pose(12.5844, 1.70121, -1.21812)) #front second entrance
+            time.sleep(1.0)
+            self.send_goal(self.make_pose(2.27953, -1.53849, 2.36622)) #second hall
+            time.sleep(1.0)
+            
+        
+        #self.send_goal(self.make_pose(20.0, 20.0, math.pi))
+        #time.sleep(1.0)
+        #self.send_goal(self.make_pose(15.0, 15.0, math.pi))
 
 def main():
     rclpy.init()
@@ -75,3 +84,4 @@ def main():
 if __name__ == '__main__':
     main()
 
+

go2_robot_sdk/launch/2_custom_robot.launch.py

@@ -1,7 +1,7 @@
 image: office.pgm
 mode: trinary
 resolution: 0.050
-origin: [-1.709, -1.498, 0]
+origin: [-6.633, -6.401, 0]
 negate: 0
 occupied_thresh: 0.65
 free_thresh: 0.196