Skip to content

Commit

Permalink
remove unrelated changes
Browse files Browse the repository at this point in the history
Signed-off-by: Kenta Konagaya <[email protected]>
  • Loading branch information
kenconnor committed Nov 19, 2024
1 parent 3702db9 commit e549516
Show file tree
Hide file tree
Showing 2 changed files with 8 additions and 103 deletions.
26 changes: 7 additions & 19 deletions cabot_ui/cabot_ui/geojson.py
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
# Copyright (c) 2020, 2024 Carnegie Mellon University and Miraikan
# Copyright (c) 2020, 2022 Carnegie Mellon University
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
Expand Down Expand Up @@ -145,8 +145,7 @@ def marshal(cls, dic):
"hulop_tags": None,
"hulop_poi_external_category": None,
"hulop_show_labels_zoomlevel": None,
"hulop_road_width": 1.5,
"levDif_max": None
"hulop_road_width": 1.5
}

def __getattr__(self, name):
Expand Down Expand Up @@ -413,15 +412,9 @@ class NavigationMode(enum.Enum):
Standard = 0
Narrow = 1
Tight = 2
ClimbUpStep = 3
ClimbDownStep = 4

@classmethod
def get_mode(cls, width, step_height, is_facing_upward):
if step_height is not None:
if step_height < 5.0:
res_mode = NavigationMode.ClimbUpStep if is_facing_upward else NavigationMode.ClimbDownStep
return res_mode
def get_mode(cls, width):
if width < 1.0:
return NavigationMode.Tight
if width < 1.2:
Expand Down Expand Up @@ -490,6 +483,10 @@ def is_leaf(self):
return False
return self.start_node.is_leaf or self.end_node.is_leaf

@property
def navigation_mode(self):
return NavigationMode.get_mode(self.properties.hulop_road_width)

@property
def length(self):
"""distance from start to end"""
Expand Down Expand Up @@ -550,15 +547,6 @@ def is_temp(self):
def pose(self):
return geoutil.Pose.pose_from_points(self.source_node.local_geometry, self.target_node.local_geometry)

@property
def navigation_mode(self):
is_facing_upward = self.source_node == self.start_node
return NavigationMode.get_mode(
width=self.properties.hulop_road_width,
step_height=self.properties.levDif_max,
is_facing_upward=is_facing_upward
)


class Node(Object):
"""Node class"""
Expand Down
85 changes: 1 addition & 84 deletions cabot_ui/cabot_ui/navgoal.py
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
# Copyright (c) 2020, 2024 Carnegie Mellon University and Miraikan
# Copyright (c) 2022 Carnegie Mellon University
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
Expand Down Expand Up @@ -639,12 +639,6 @@ def get_parameters_for(cls, mode):
/cabot/people_speed_control_node:
social_distance_x: 2.0
social_distance_y: 0.5
/cabot/odrive_can_node_right:
vel_gain: 1.0
vel_integrator_gain: 4.0
/cabot/odrive_can_node_left:
vel_gain: 1.0
vel_integrator_gain: 4.0
"""
if mode == geojson.NavigationMode.Narrow:
params = """
Expand Down Expand Up @@ -675,12 +669,6 @@ def get_parameters_for(cls, mode):
complete_stop: [false,false,true,false,true,false,true]
/cabot/speed_control_node_touch_false:
complete_stop: [false,false,true,false,true,false,true]
/cabot/odrive_can_node_right:
vel_gain: 1.0
vel_integrator_gain: 4.0
/cabot/odrive_can_node_left:
vel_gain: 1.0
vel_integrator_gain: 4.0
"""
if mode == geojson.NavigationMode.Tight:
params = """
Expand Down Expand Up @@ -711,76 +699,6 @@ def get_parameters_for(cls, mode):
complete_stop: [false,false,true,false,true,false,true]
/cabot/speed_control_node_touch_false:
complete_stop: [false,false,true,false,true,false,true]
/cabot/odrive_can_node_right:
vel_gain: 1.0
vel_integrator_gain: 4.0
/cabot/odrive_can_node_left:
vel_gain: 1.0
vel_integrator_gain: 4.0
"""
if mode == geojson.NavigationMode.ClimbUpStep:
params = """
/planner_server:
CaBot.path_adjusted_center: 0.0
CaBot.path_adjusted_minimum_path_width: 0.0
CaBot.path_width: 0.0
CaBot.min_iteration_count: 5
CaBot.max_iteration_count: 10
CaBot.ignore_people: True
/footprint_publisher:
footprint_mode: 0
/controller_server:
FollowPath.max_vel_x: 0.3
FollowPath.sim_time: 1.7
cabot_goal_checker.xy_goal_tolerance: 0.5
/global_costmap/global_costmap:
people_obstacle_layer.people_enabled: False
inflation_layer.inflation_radius: 0.75
/local_costmap/local_costmap:
inflation_layer.inflation_radius: 0.75
/cabot/lidar_speed_control_node:
min_distance: 1.0
/cabot/people_speed_control_node:
social_distance_x: 1.0
social_distance_y: 0.50
/cabot/odrive_can_node_right:
vel_gain: 1.0
vel_integrator_gain: 4.0
/cabot/odrive_can_node_left:
vel_gain: 1.0
vel_integrator_gain: 4.0
"""
if mode == geojson.NavigationMode.ClimbDownStep:
params = """
/planner_server:
CaBot.path_adjusted_center: 0.0
CaBot.path_adjusted_minimum_path_width: 0.0
CaBot.path_width: 0.0
CaBot.min_iteration_count: 5
CaBot.max_iteration_count: 10
CaBot.ignore_people: True
/footprint_publisher:
footprint_mode: 0
/controller_server:
FollowPath.max_vel_x: 0.3
FollowPath.sim_time: 1.7
cabot_goal_checker.xy_goal_tolerance: 0.5
/global_costmap/global_costmap:
people_obstacle_layer.people_enabled: False
inflation_layer.inflation_radius: 0.75
/local_costmap/local_costmap:
inflation_layer.inflation_radius: 0.75
/cabot/lidar_speed_control_node:
min_distance: 1.0
/cabot/people_speed_control_node:
social_distance_x: 1.0
social_distance_y: 0.50
/cabot/odrive_can_node_right:
vel_gain: 1.0
vel_integrator_gain: 4.0
/cabot/odrive_can_node_left:
vel_gain: 1.0
vel_integrator_gain: 4.0
"""
data = yaml.safe_load(params)
return data
Expand Down Expand Up @@ -901,7 +819,6 @@ def nav_params_keys(self):
def nav_params(self):
new_mode = self.navcog_routes[self.route_index][2]
self.mode = new_mode
self.delegate.activity_log("cabot/navigation", "change_mode", f"{self.mode}")
return Nav2Params.get_parameters_for(new_mode)

def enter(self):
Expand Down

0 comments on commit e549516

Please sign in to comment.