Merge pull request #2 from N-rwal/arm

Arm

Author: N-rwal

go2_robot_sdk/config/nav2_params_navigation.yaml

@@ -18,8 +18,8 @@ amcl:
     laser_min_range: 0.12
     laser_model_type: "likelihood_field"
     max_beams: 90
-    max_particles: 1500
-    min_particles: 400
+    max_particles: 800
+    min_particles: 200
     odom_frame_id: "odom"
     pf_err: 0.05
     pf_z: 0.99
@@ -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 = 2.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/nav_controller.py

@@ -0,0 +1,43 @@
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+from std_srvs.srv import SetBool
+import subprocess
+
+class NavController(Node):
+    def __init__(self):
+        super().__init__('nav_controller')
+        self.srv = self.create_service(SetBool, 'nav_control', self.callback)
+        self.test_running = False
+        self.get_logger().info('Nav controller ready')
+        self.get_logger().info('Start test: ros2 service call /nav_control std_srvs/srv/SetBool "{data: true}"')
+
+    def callback(self, request, response):
+        if request.data and not self.test_running:
+            self.test_running = True
+            self.get_logger().info('Starting navigation test...')
+
+            result = subprocess.run(
+                ['ros2', 'run', 'go2_robot_sdk', 'sequential_nav_test'],
+                capture_output=True,
+                text=True
+            )
+
+            self.test_running = False
+            response.success = True
+            response.message = f'Test completed. Output: {result.stdout[:100]}...'
+
+        elif not request.data and self.test_running:
+            #could add stop logic here
+            response.success = True
+            response.message = 'Stop requested'
+        else:
+            response.success = False
+            response.message = 'Already in requested state'
+
+        return response
+
+def main():
+    rclpy.init()
+    rclpy.spin(NavController())
+    rclpy.shutdown()

go2_robot_sdk/go2_robot_sdk/sequential_nav_test.py

@@ -0,0 +1,87 @@
+#!/usr/bin/env python3
+
+import math
+import time
+
+import rclpy
+from rclpy.node import Node
+from rclpy.action import ActionClient
+
+from nav2_msgs.action import NavigateToPose
+from geometry_msgs.msg import PoseStamped
+#from tf_transformations import quaternion_from_euler
+
+
+class SequentialNavTest(Node):
+    def __init__(self):
+        super().__init__('sequential_nav_test')
+        
+        self.client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
+
+        self.get_logger().info('Waiting for Nav2 action server...')
+        self.client.wait_for_server()
+        self.get_logger().info('Nav2 action server ready')
+
+        self.run_sequence()
+
+    def make_pose(self, x, y, yaw):
+        pose = PoseStamped()
+        pose.header.frame_id = 'map'
+        pose.header.stamp = self.get_clock().now().to_msg()
+
+        pose.pose.position.x = x
+        pose.pose.position.y = y
+
+        pose.pose.orientation.z = math.sin(yaw / 2.0)
+        pose.pose.orientation.w = math.cos(yaw / 2.0)
+
+        return pose
+
+    def send_goal(self, pose):
+        goal = NavigateToPose.Goal()
+        goal.pose = pose
+
+        future = self.client.send_goal_async(goal)
+        rclpy.spin_until_future_complete(self, future)
+
+        goal_handle = future.result()
+        if not goal_handle.accepted:
+            self.get_logger().error('Goal rejected')
+            return False
+
+        self.get_logger().info('Goal accepted, waiting for result...')
+        result_future = goal_handle.get_result_async()
+        rclpy.spin_until_future_complete(self, result_future)
+
+        return True
+
+    def run_sequence(self):
+        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()
+    node = SequentialNavTest()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()
+
+