Skip to content

Commit

Permalink
inrow preparation make it optional
Browse files Browse the repository at this point in the history
  • Loading branch information
@GPrathap
GPrathap committed Jun 6, 2024
1 parent db33d37 commit b97db4a
Show file tree
Hide file tree
Showing 2 changed files with 27 additions and 15 deletions.
38 changes: 24 additions & 14 deletions topological_navigation/topological_navigation/edge_action_manager2.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -788,33 +788,43 @@ def execute_row_operation_action(self, action_msg):
# target_goal = NavigateThroughPoses.Goal()
self.get_logger().info("Edge Action Manager: Robot current pose: {},{}"
.format(next_goal.pose.position.x, next_goal.pose.position.y))
self.robot_current_status = self.ACTIONS.ROBOT_STATUS_PREPARATION_STATE
self.publish_robot_current_status_msg(self.ACTIONS.ROW_RECOVERY, self.robot_current_status)
get_proper_alignment = self.execute_row_recovery(intermediate_pose, target_frame_id)
if(get_proper_alignment == False):
self.robot_current_status = self.ACTIONS.ROBOT_STATUS_DISABLE_STATE
self.publish_robot_current_status_msg(self.ACTIONS.ROW_RECOVERY, self.robot_current_status)
return False
self.robot_current_status = self.ACTIONS.ROBOT_STATUS_AUTONOMOUS_NAVIGATION_STATE
self.publish_robot_current_status_msg(self.ACTIONS.ROW_TRAVERSAL, self.robot_current_status)
step_moved = self.execute_row_operation_one_step(next_goal, target_frame_id)
if(step_moved == False):
intermediate_pose, _ = inrow_opt.getNextIntermediateGoal(self.current_robot_pose)
self.robot_current_status = self.ACTIONS.ROBOT_STATUS_AUTONOMOUS_RECOVERY_STATE
if(self.intermediate_dis > 0.0):
self.robot_current_status = self.ACTIONS.ROBOT_STATUS_PREPARATION_STATE
self.publish_robot_current_status_msg(self.ACTIONS.ROW_RECOVERY, self.robot_current_status)
get_proper_alignment = self.execute_row_recovery(intermediate_pose, target_frame_id)
if(get_proper_alignment == False):
self.robot_current_status = self.ACTIONS.ROBOT_STATUS_DISABLE_STATE
self.publish_robot_current_status_msg(self.ACTIONS.ROW_RECOVERY, self.robot_current_status)
return False
self.robot_current_status = self.ACTIONS.ROBOT_STATUS_AUTONOMOUS_NAVIGATION_STATE
self.publish_robot_current_status_msg(self.ACTIONS.ROW_TRAVERSAL, self.robot_current_status)
step_moved = self.execute_row_operation_one_step(next_goal, target_frame_id)
if(step_moved == False):
if(self.intermediate_dis > 0.0):
intermediate_pose, _ = inrow_opt.getNextIntermediateGoal(self.current_robot_pose)
self.robot_current_status = self.ACTIONS.ROBOT_STATUS_AUTONOMOUS_RECOVERY_STATE
self.publish_robot_current_status_msg(self.ACTIONS.ROW_RECOVERY, self.robot_current_status)
get_proper_alignment = self.execute_row_recovery(intermediate_pose, target_frame_id)
if(get_proper_alignment == False):
self.robot_current_status = self.ACTIONS.ROBOT_STATUS_DISABLE_STATE
self.publish_robot_current_status_msg(self.ACTIONS.ROW_RECOVERY, self.robot_current_status)
return False
else:
self.robot_current_status = self.ACTIONS.ROBOT_STATUS_AUTONOMOUS_NAVIGATION_STATE
self.publish_robot_current_status_msg(self.ACTIONS.ROW_TRAVERSAL, self.robot_current_status)
step_moved = self.execute_row_operation_one_step(next_goal, target_frame_id)
if(step_moved == False):
self.robot_current_status = self.ACTIONS.ROBOT_STATUS_DISABLE_STATE
self.publish_robot_current_status_msg(self.ACTIONS.ROW_TRAVERSAL, self.robot_current_status)
return False
else:
self.robot_current_status = self.ACTIONS.ROBOT_STATUS_AUTONOMOUS_NAVIGATION_STATE
self.publish_robot_current_status_msg(self.ACTIONS.ROW_TRAVERSAL, self.robot_current_status)
step_moved = self.execute_row_operation_one_step(next_goal, target_frame_id)
if(step_moved == False):
self.robot_current_status = self.ACTIONS.ROBOT_STATUS_DISABLE_STATE
self.publish_robot_current_status_msg(self.ACTIONS.ROW_TRAVERSAL, self.robot_current_status)
return False
return False
if(get_to_goal):
if step_moved:
self.robot_current_status = self.ACTIONS.ROBOT_STATUS_NATURAL_STATE
Expand Down
4 changes: 3 additions & 1 deletion topological_navigation/topological_navigation/scripts/navigation2.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,6 +91,7 @@ def __init__(self, name, update_params_control_server, edge_action_manager_serve
self.declare_parameter('use_nav2_follow_route', Parameter.Type.BOOL)
self.declare_parameter('use_in_row_operation', Parameter.Type.BOOL)
self.declare_parameter('inrow_step_size', Parameter.Type.DOUBLE)
self.declare_parameter('inrow_step_intermediate_dis', Parameter.Type.DOUBLE)
self.declare_parameter(self.ACTIONS.BT_DEFAULT, Parameter.Type.STRING)
self.declare_parameter(self.ACTIONS.BT_IN_ROW, Parameter.Type.STRING)
self.declare_parameter(self.ACTIONS.BT_GOAL_ALIGN, Parameter.Type.STRING)
Expand All @@ -102,6 +103,7 @@ def __init__(self, name, update_params_control_server, edge_action_manager_serve
self.use_nav2_follow_route = self.get_parameter_or("use_nav2_follow_route", Parameter('bool', Parameter.Type.BOOL, False)).value
self.use_in_row_operation = self.get_parameter_or("use_in_row_operation", Parameter('bool', Parameter.Type.BOOL, False)).value
self.inrow_step_size = self.get_parameter_or("inrow_step_size", Parameter('double', Parameter.Type.DOUBLE, 2.0)).value
self.inrow_step_intermediate_dis = self.get_parameter_or("inrow_step_intermediate_dis", Parameter('double', Parameter.Type.DOUBLE, -1.0)).value

row_traversal_planner = self.get_parameter_or("row_traversal_planner", Parameter('str', Parameter.Type.STRING, "dwb_core::DWBLocalPlanner")).value
default_planner = self.get_parameter_or("default_planner", Parameter('str', Parameter.Type.STRING, "dwb_core::DWBLocalPlanner")).value
Expand Down Expand Up @@ -168,7 +170,7 @@ def __init__(self, name, update_params_control_server, edge_action_manager_serve
break

self.edge_action_manager = edge_action_manager_server
self.edge_action_manager.init(self.ACTIONS, self.rsearch, self.update_params_control_server, self.inrow_step_size)
self.edge_action_manager.init(self.ACTIONS, self.rsearch, self.update_params_control_server, self.inrow_step_size, self.inrow_step_intermediate_dis)

self.edge_reconfigure = self.get_parameter_or("reconfigure_edges", Parameter('bool', Parameter.Type.BOOL, True)).value
self.srv_edge_reconfigure = self.get_parameter_or("reconfigure_edges_srv", Parameter('bool', Parameter.Type.BOOL, False)).value
Expand Down

0 comments on commit b97db4a

Please sign in to comment.