From c51abf4df36fad5abf879b67da39c2e365120199 Mon Sep 17 00:00:00 2001 From: evanyan13 Date: Wed, 17 Apr 2024 12:53:37 +0800 Subject: [PATCH 1/4] Able to avoid obstacle while following path but path not updated --- .../global_planner_package/global_map.py | 2 +- .../navi_main/global_planner_package/mover.py | 19 ++++++++++++++++--- .../navi_main/global_planner_package/utils.py | 2 +- 3 files changed, 18 insertions(+), 5 deletions(-) diff --git a/src/navi_main/navi_main/global_planner_package/global_map.py b/src/navi_main/navi_main/global_planner_package/global_map.py index 54e9d8d..67a839c 100644 --- a/src/navi_main/navi_main/global_planner_package/global_map.py +++ b/src/navi_main/navi_main/global_planner_package/global_map.py @@ -32,7 +32,7 @@ def categorise_data(self, data): return cat_data.astype(np.int8) def add_obstacle_buffer(self): - buffer_size = int(ROBOT_RADIUS / self.resolution / 2) + buffer_size = int(ROBOT_RADIUS / self.resolution * 2 / 3) new_data = np.copy(self.data) for y in range(self.height): diff --git a/src/navi_main/navi_main/global_planner_package/mover.py b/src/navi_main/navi_main/global_planner_package/mover.py index 1cf9bcb..57c2b18 100644 --- a/src/navi_main/navi_main/global_planner_package/mover.py +++ b/src/navi_main/navi_main/global_planner_package/mover.py @@ -32,6 +32,7 @@ def __init__(self, planner): self.velocity_publisher = self.create_publisher(Twist, 'cmd_vel', 10) self.navi_timer = self.create_timer(0.01, self.manage_mover) + # self.path_refresh_timer = self.create_timer(0.1, self.update_scan) def path_callback(self, path_msg: Path): """ @@ -42,7 +43,7 @@ def path_callback(self, path_msg: Path): self.current_path = new_path self.current_goal_index = 0 self.new_path = True - self.planner.switch_to_global() + self.manage_mover() # self.safe_start_timer('global_mover_timer', MOVER_REFRESH, self.follow_global_path) def scan_callback(self, scan_msg: LaserScan): @@ -52,6 +53,13 @@ def scan_callback(self, scan_msg: LaserScan): self.laser_range = np.array(scan_msg.ranges) self.laser_range[self.laser_range == 0] = np.nan + self.update_scan() + + def update_scan(self): + logger.info("Updating scan...") + if self.laser_range.size == 0: + return + num_ranges = len(self.laser_range) degrees_per_index = 270 / num_ranges indices_per_side = int((FRONT_ANGLE / 2) / degrees_per_index) @@ -71,6 +79,8 @@ def scan_callback(self, scan_msg: LaserScan): self.obstacle_detected = True else: self.obstacle_detected = False + + logger.info(f"Scan updated {min_distance}") def manage_mover(self): """ @@ -86,6 +96,7 @@ def manage_mover(self): def spin_local_mover(self): if self.robot_pos: # if not self.global_mover_ready: + self.update_scan() if self.obstacle_detected: logger.info(f"local_mover: Obstacle detected, adjusting position") self.adjust_obstacle() @@ -113,6 +124,7 @@ def follow_global_path(self): logger.info(f"No current path received") return + self.update_scan() if self.obstacle_detected: logger.info(f"global_mover: Obstacle detected, adjusting position") self.adjust_obstacle() @@ -148,6 +160,7 @@ def move_to_point(self, goal): return while distance_to_goal >= MOVE_TOL or abs(heading_error) > np.radians(1.5): + self.update_scan() if self.obstacle_detected: self.adjust_obstacle() @@ -186,7 +199,6 @@ def adjust_obstacle(self): self.rotatebot(float(lr2i)) logger.info("Complete rotation") self.send_velocity(LINEAR_VEL, 0.0) - time.sleep(1) self.check_obstacle_clear() def rotatebot(self, rot_angle): @@ -240,14 +252,15 @@ def rotatebot(self, rot_angle): twist.angular.z = 0.0 # stop the rotation self.velocity_publisher.publish(twist) - self.stop_moving() def check_obstacle_clear(self): # Check if the obstacle is still detected + self.update_scan() if not self.obstacle_detected: logger.info("Obstacle cleared, resuming path") self.reset_path() self.planner.switch_to_global() + self.manage_mover() else: logger.info("Obstacle still detected, checking further") self.adjust_obstacle() diff --git a/src/navi_main/navi_main/global_planner_package/utils.py b/src/navi_main/navi_main/global_planner_package/utils.py index a07f84d..de08958 100644 --- a/src/navi_main/navi_main/global_planner_package/utils.py +++ b/src/navi_main/navi_main/global_planner_package/utils.py @@ -42,7 +42,7 @@ # Gazebo MOVE_TOL = 0.10 LINEAR_VEL = 0.10 -ANGULAR_VEL = 0.10 +ANGULAR_VEL = 0.15 STOP_DISTANCE = 0.20 # metres LOOKAHEAD_DIST = 1.0 FRONT_ANGLE = 45 From 304a4c2248531b96943ecf2f42e47cd9f37e2f53 Mon Sep 17 00:00:00 2001 From: evanyan13 Date: Wed, 17 Apr 2024 13:20:28 +0800 Subject: [PATCH 2/4] Manage to detect and react to obstacle while following path --- .../navi_main/global_planner_package/mover.py | 18 +++++++++--------- .../navi_main/global_planner_package/utils.py | 2 +- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/navi_main/navi_main/global_planner_package/mover.py b/src/navi_main/navi_main/global_planner_package/mover.py index 57c2b18..a7efb12 100644 --- a/src/navi_main/navi_main/global_planner_package/mover.py +++ b/src/navi_main/navi_main/global_planner_package/mover.py @@ -54,9 +54,8 @@ def scan_callback(self, scan_msg: LaserScan): self.laser_range[self.laser_range == 0] = np.nan self.update_scan() - + def update_scan(self): - logger.info("Updating scan...") if self.laser_range.size == 0: return @@ -79,9 +78,7 @@ def update_scan(self): self.obstacle_detected = True else: self.obstacle_detected = False - - logger.info(f"Scan updated {min_distance}") - + def manage_mover(self): """ Determine the navigation mode based on planner's state and execute appropriate methods @@ -119,6 +116,8 @@ def follow_global_path(self): Follows the given path by moving to each point sequentially. """ self.new_path = False + if self.new_path: + self.manage_mover() if not self.current_path: logger.info(f"No current path received") @@ -126,7 +125,7 @@ def follow_global_path(self): self.update_scan() if self.obstacle_detected: - logger.info(f"global_mover: Obstacle detected, adjusting position") + logger.info(f"global_mover: Obstacle detected, ROTATING") self.adjust_obstacle() return @@ -156,19 +155,20 @@ def move_to_point(self, goal): if distance_to_goal < MOVE_TOL: self.current_goal_index += 1 - logger.info(f"Waypoint reached. Moving to waypoint index {self.current_goal_index}") + logger.info(f"move_to_point: Waypoint reached. Moving to waypoint index {self.current_goal_index}") return while distance_to_goal >= MOVE_TOL or abs(heading_error) > np.radians(1.5): self.update_scan() if self.obstacle_detected: self.adjust_obstacle() + break if self.new_path: break if abs(heading_error) > np.radians(1): - logger.info(f"Robot not aligned, rotating... {abs(heading_error)}") + logger.info(f"move_to_point: Robot not aligned, rotating...") self.rotatebot(heading_error) # Update heading_error after each rotation dy = goal[1] - self.robot_pos.y @@ -176,7 +176,7 @@ def move_to_point(self, goal): target_heading = atan2(dy, dx) heading_error = self.normalise_angle(target_heading - self.robot_pos.theta) else: - logger.info("Robot aligned. Moving forward.") + logger.info("move_to_point: Robot aligned. Moving forward.") linear = LINEAR_VEL * (1 - 2 * abs(heading_error) / pi) angular = 0.0 # No rotation needed, robot is aligned self.send_velocity(linear, angular) diff --git a/src/navi_main/navi_main/global_planner_package/utils.py b/src/navi_main/navi_main/global_planner_package/utils.py index de08958..e75edbf 100644 --- a/src/navi_main/navi_main/global_planner_package/utils.py +++ b/src/navi_main/navi_main/global_planner_package/utils.py @@ -43,7 +43,7 @@ MOVE_TOL = 0.10 LINEAR_VEL = 0.10 ANGULAR_VEL = 0.15 -STOP_DISTANCE = 0.20 # metres +STOP_DISTANCE = 0.30 # metres LOOKAHEAD_DIST = 1.0 FRONT_ANGLE = 45 FRONT_ANGLES = range(-FRONT_ANGLE, FRONT_ANGLE + 1, 1) From 7072fb8eaeddaa80175f907b7e4e67d53c87088b Mon Sep 17 00:00:00 2001 From: evanyan13 Date: Wed, 17 Apr 2024 13:45:08 +0800 Subject: [PATCH 3/4] Update astar to path map that are further away from the walls --- main.rviz | 497 ++++++++++++++++++ .../astar_path_finder.py | 12 +- 2 files changed, 508 insertions(+), 1 deletion(-) create mode 100644 main.rviz diff --git a/main.rviz b/main.rviz new file mode 100644 index 0000000..a662c7d --- /dev/null +++ b/main.rviz @@ -0,0 +1,497 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1/Frames1 + - /Map1 + - /Path1 + Splitter Ratio: 0.5794117450714111 + Tree Height: 363 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /Publish Point1 + - /2D Pose Estimate1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/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: + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + base_footprint: + Value: false + base_link: + Value: true + base_scan: + Value: true + caster_back_link: + Value: false + imu_link: + Value: false + map: + Value: true + odom: + Value: false + wheel_left_link: + Value: false + wheel_right_link: + Value: false + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + map: + odom: + base_footprint: + base_link: + base_scan: + {} + caster_back_link: + {} + imu_link: + {} + wheel_left_link: + {} + wheel_right_link: + {} + 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_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.019999999552965164 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Angle Tolerance: 0.10000000149011612 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: false + Enabled: false + Keep: 100 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /odom + Value: false + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false + Value: true + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: true + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_plan + Value: true + Enabled: true + Name: Global Map + - Class: rviz_common/Group + Displays: + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/footprint + Value: true + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_plan + Value: true + Enabled: true + Name: Local Map + - Alpha: 1 + Arrow Length: 0.05000000074505806 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/PoseArray + Color: 0; 192; 0 + Enabled: true + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: PoseArray + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Arrow (Flat) + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /particlecloud + Value: true + Enabled: false + Name: Navigation + - Class: rviz_common/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 0.18203988671302795 + Min Value: 0.18195410072803497 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 0; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: scan_matched_points2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /scan_matched_points2 + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Trajectories + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /trajectory_node_list + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Constraints + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /constraint_list + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Landmark Poses + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /landmark_poses_list + Value: false + Enabled: true + Name: Cartographer + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: path + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: initialpose + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 6.985499382019043 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.27696776390075684 + Y: 0.9297961592674255 + Z: 0.08237607777118683 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.484796404838562 + Target Frame: + Value: Orbit (rviz) + Yaw: 3.144380569458008 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 744 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd0000000400000000000001560000028efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000028e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000002d00000028e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 720 + X: 72 + Y: 27 diff --git a/src/navi_main/navi_main/global_planner_package/astar_path_finder.py b/src/navi_main/navi_main/global_planner_package/astar_path_finder.py index bf044eb..350b789 100644 --- a/src/navi_main/navi_main/global_planner_package/astar_path_finder.py +++ b/src/navi_main/navi_main/global_planner_package/astar_path_finder.py @@ -26,6 +26,7 @@ def find_astar_path(map: GlobalMap, start: GlobalPlannerNode, goal: GlobalPlanne cells = {(start_cell.i, start_cell.j): start_cell} turning_cost = 2 + wall_penalty = 2 while open_set: current_f, current = heapq.heappop(open_set) @@ -57,7 +58,16 @@ def find_astar_path(map: GlobalMap, start: GlobalPlannerNode, goal: GlobalPlanne else: turn_cost = 0 - tentative_g = current.g + 1 + turn_cost + # Determine if there is a wall nearby + wall_cost = 0 + for di in range(-1, 2): + for dj in range(-1, 2): + ni, nj = neighbour.i + di, neighbour.j + dj + if not map.is_indice_avail(ni, nj): + wall_cost += wall_penalty / (abs(di) + abs(dj) + 1) + + tentative_g = current.g + 1 + turn_cost + wall_cost + # print(f"tentative_g {tentative_g} turning cost: {turn_cost}") if (neighbour.i, neighbour.j) not in cells or tentative_g < neighbour.g: neighbour.parent = current From 222e1ff0d81519f6a66e27614c9e633a2a83a765 Mon Sep 17 00:00:00 2001 From: evanyan13 Date: Wed, 17 Apr 2024 13:59:07 +0800 Subject: [PATCH 4/4] Generally sufficient for path following --- .../navi_main/global_planner_package/mover.py | 20 +++++++++---------- .../navi_main/global_planner_package/utils.py | 4 +--- 2 files changed, 11 insertions(+), 13 deletions(-) diff --git a/src/navi_main/navi_main/global_planner_package/mover.py b/src/navi_main/navi_main/global_planner_package/mover.py index a7efb12..057a4cb 100644 --- a/src/navi_main/navi_main/global_planner_package/mover.py +++ b/src/navi_main/navi_main/global_planner_package/mover.py @@ -158,7 +158,7 @@ def move_to_point(self, goal): logger.info(f"move_to_point: Waypoint reached. Moving to waypoint index {self.current_goal_index}") return - while distance_to_goal >= MOVE_TOL or abs(heading_error) > np.radians(1.5): + while distance_to_goal >= MOVE_TOL or abs(heading_error) > np.radians(2): self.update_scan() if self.obstacle_detected: self.adjust_obstacle() @@ -167,7 +167,7 @@ def move_to_point(self, goal): if self.new_path: break - if abs(heading_error) > np.radians(1): + if abs(heading_error) > np.radians(2): logger.info(f"move_to_point: Robot not aligned, rotating...") self.rotatebot(heading_error) # Update heading_error after each rotation @@ -176,7 +176,7 @@ def move_to_point(self, goal): target_heading = atan2(dy, dx) heading_error = self.normalise_angle(target_heading - self.robot_pos.theta) else: - logger.info("move_to_point: Robot aligned. Moving forward.") + logger.info("move_to_point: MOVING FORWARD") linear = LINEAR_VEL * (1 - 2 * abs(heading_error) / pi) angular = 0.0 # No rotation needed, robot is aligned self.send_velocity(linear, angular) @@ -186,18 +186,18 @@ def move_to_point(self, goal): distance_to_goal = hypot(dx, dy) def adjust_obstacle(self): - self.get_logger().info('Adjusting to obstacle') + self.get_logger().info('adjust_obstacle: Adjusting to obstacle...') if self.laser_range.size != 0: # use nanargmax as there are nan's in laser_range added to replace 0's lr2i = np.nanargmax(self.laser_range) - self.get_logger().info('Picked direction: %d %f m' % (lr2i, self.laser_range[lr2i])) + # self.get_logger().info('Picked direction: %d %f m' % (lr2i, self.laser_range[lr2i])) else: lr2i = 0 self.get_logger().info('No data!') # rotate to that direction self.rotatebot(float(lr2i)) - logger.info("Complete rotation") + logger.info("adjust_obstacle: COMPLETE ADJUSTING") self.send_velocity(LINEAR_VEL, 0.0) self.check_obstacle_clear() @@ -217,7 +217,7 @@ def rotatebot(self, rot_angle): target_yaw = current_yaw + math.radians(rot_angle) # convert to complex notation c_target_yaw = complex(math.cos(target_yaw),math.sin(target_yaw)) - logger.info('Rotation start: %f' % math.degrees(cmath.phase(c_target_yaw))) + # logger.info('Rotation start: %f' % math.degrees(cmath.phase(c_target_yaw))) # divide the two complex numbers to get the change in direction c_change = c_target_yaw / c_yaw # get the sign of the imaginary component to figure out which way we have to turn @@ -247,7 +247,7 @@ def rotatebot(self, rot_angle): c_dir_diff = np.sign(c_change.imag) # self.get_logger().info('c_change_dir: %f c_dir_diff: %f' % (c_change_dir, c_dir_diff)) - logger.info('Rotation ended: %f' % math.degrees(current_yaw)) + # logger.info('Rotation ended: %f' % math.degrees(current_yaw)) # set the rotation speed to 0 twist.angular.z = 0.0 # stop the rotation @@ -257,12 +257,12 @@ def check_obstacle_clear(self): # Check if the obstacle is still detected self.update_scan() if not self.obstacle_detected: - logger.info("Obstacle cleared, resuming path") + logger.info("check_obstacle_clear: Obstacle cleared, RESUMING...") self.reset_path() self.planner.switch_to_global() self.manage_mover() else: - logger.info("Obstacle still detected, checking further") + logger.info("check_obstacle_clear: Obstacle still detected, CHECKING") self.adjust_obstacle() def send_velocity(self, linear, angular): diff --git a/src/navi_main/navi_main/global_planner_package/utils.py b/src/navi_main/navi_main/global_planner_package/utils.py index e75edbf..4506ac0 100644 --- a/src/navi_main/navi_main/global_planner_package/utils.py +++ b/src/navi_main/navi_main/global_planner_package/utils.py @@ -25,9 +25,7 @@ # planner PATH_REFRESH = 5.0 # seconds -MOVER_PATH_REFRESH = 60 -SIMP_STEPS = 10 -MOVER_REFRESH = 1.0 +MOVER_PATH_REFRESH = 180.0 # Actual bot # global_mover