77
88from math import sin , cos , atan2 , pi
99
10- from iarc7_safety .iarc_safety_exception import IARCSafetyException
10+ from iarc7_safety .iarc_safety_exception import IARCSafetyException , IARCFatalSafetyException
1111from iarc7_msgs .msg import ObstacleArray
1212from geometry_msgs .msg import PointStamped
1313
1414class ObstacleAvoider (object ):
1515 def __init__ (self , tf_buffer ):
1616 self ._lock = threading .RLock ()
1717
18+ self ._avoid_distance = rospy .get_param ('~obst_avoid_avoid_distance' )
19+ self ._predict_time = rospy .get_param ('~obst_avoid_predict_time' )
20+ self ._response_strength = rospy .get_param ('~obst_avoid_response_strength' )
21+
1822 # Maximum allowed distance from an obstacle to its projection on the nearest flight vector
1923 self ._unsafe_obstacle_threshold = rospy .get_param ("~obst_avoid_norm_limit" )
2024 # Obstacles outside this radius are ignored
@@ -23,6 +27,8 @@ def __init__(self, tf_buffer):
2327 self ._vector_step_size = rospy .get_param ("~obst_avoid_step_size" ) # Divide by 180 to get degrees/step
2428 # Minimum avoid vector
2529 self ._minimum_magnitude = rospy .get_param ("~obst_avoid_min_magnitude" )
30+ # Blending speed
31+ self ._blending_speed = rospy .get_param ('~obst_avoid_blending_speed' )
2632 # Transform timeout
2733 self ._timeout = rospy .Duration (rospy .get_param ("~transform_timeout" ))
2834 self ._obstacle_points = None
@@ -68,40 +74,82 @@ def _update_obstacles(self, obstacles):
6874 tf2_ros .ExtrapolationException ) as ex :
6975 rospy .logwarn ("ObstacleAvoider: Couldn't lookup transform from {} to level_quad" .format (obstacles .header .frame_id ))
7076
71- def get_safe_vector (self , desired_vector ):
77+ def get_safe_vector (self , desired_vector , curr_vel ):
7278 # Find the norm and direction of the velocity in the horizontal plane
73- original_vector_magnitude = np .linalg .norm (desired_vector [:2 ])
74- original_vector_direction = atan2 (desired_vector [1 ], desired_vector [0 ])
79+ #original_vector_magnitude = np.linalg.norm(desired_vector[:2])
80+ #original_vector_direction = atan2(desired_vector[1], desired_vector[0])
81+ curr_vel = np .array (curr_vel )
82+ desired_vector = np .array (desired_vector )
7583 with self ._lock :
76- for angle in np .linspace (0 , pi , num = self ._vector_step_size ):
77- for sign in [- 1 , 1 ]:
78- vector_is_safe = True
79- new_vector = np .array ([cos (original_vector_direction + angle * sign ), sin (original_vector_direction + angle * sign )])
80- for obstacle in self ._obstacle_points :
81- # Project the obstacle vector onto the flight vector
82- obstacle_proj_new_vector = (np .dot (new_vector , obstacle )/ np .dot (new_vector , new_vector ))* new_vector
83-
84- # Don't look at obstacles projected onto the reflection of this vector
85- if np .sign (obstacle_proj_new_vector [0 ]) == np .sign (new_vector [0 ]) and np .sign (obstacle_proj_new_vector [1 ]) == np .sign (new_vector [1 ]):
86- # find distance of the obstacle to its projection on the flight vector
87- obstacle_to_vector_distance = np .linalg .norm (obstacle - obstacle_proj_new_vector )
88- # throw out vectors with obstacles that are too close
89- if obstacle_to_vector_distance <= self ._unsafe_obstacle_threshold :
90- vector_is_safe = False
91- break
92-
93- # If this flight vector is safe, go for it
94- if vector_is_safe :
95- if angle != 0 :
96- rospy .logwarn ("ObstacleAvoider: modified requested velocity by {} radians" .format (angle ))
97- # We need to avoid, so make sure the vector magnitude is non-zero
98- original_vector_magnitude = max (original_vector_magnitude , self ._minimum_magnitude )
99-
100- # Copy Z velocity from commanded vector
101- # New flight vector has same magnitude as the commanded
102- return np .array ([original_vector_magnitude * cos (original_vector_direction + angle ), original_vector_magnitude * sin (original_vector_direction + angle ), desired_vector [2 ]])
103-
104- rospy .logwarn ("ObstacleAvoider: couldn't find safe velocity!" )
105- # Couldn't find a safe vector, just stop
106- return np .array ([0 , 0 , desired_vector [2 ]])
84+ for obstacle in self ._obstacle_points :
85+ if np .linalg .norm (obstacle ) == 0 :
86+ unit_vector = np .array ([1 , 0 , 0 ])
87+ else :
88+ unit_vector = obstacle / np .linalg .norm (obstacle )
89+ away_unit_vector = - unit_vector
90+
91+ turn_dir = np .array ([0 , 0 , np .cross (obstacle , curr_vel )])
92+ if np .linalg .norm (turn_dir ) < 1e-6 :
93+ turn_dir = np .array ([0 , 0 , 1 ])
94+
95+ side_vector = np .cross (turn_dir , obstacle )
96+ unit_side = side_vector / np .linalg .norm (side_vector )
97+
98+ rel_vel = np .dot (unit_vector , curr_vel )
99+ violation = max (0 ,
100+ self ._avoid_distance
101+ - np .linalg .norm (obstacle )
102+ + rel_vel * self ._predict_time )
103+
104+ # Push away
105+ desired_vector [:2 ] += away_unit_vector \
106+ * self ._response_strength * violation ** 2
107+
108+ # Push around
109+ desired_vector += unit_side * violation * 0.5
110+ return desired_vector
111+ #for angle in np.linspace(0, pi, num=self._vector_step_size):
112+ # for sign in [-1, 1]:
113+ # vector_is_safe = True
114+ # new_vector = np.array([cos(original_vector_direction+angle*sign), sin(original_vector_direction+angle*sign)])
115+ # for obstacle in self._obstacle_points:
116+ # # Project the obstacle vector onto the flight vector
117+ # obstacle_proj_new_vector = (np.dot(new_vector, obstacle)/np.dot(new_vector, new_vector))*new_vector
118+
119+ # # Don't look at obstacles projected onto the reflection of this vector
120+ # if np.sign(obstacle_proj_new_vector[0]) == np.sign(new_vector[0]) and np.sign(obstacle_proj_new_vector[1]) == np.sign(new_vector[1]):
121+ # # find distance of the obstacle to its projection on the flight vector
122+ # obstacle_to_vector_distance = np.linalg.norm(obstacle - obstacle_proj_new_vector)
123+ # # throw out vectors with obstacles that are too close
124+ # if obstacle_to_vector_distance <= self._unsafe_obstacle_threshold:
125+ # vector_is_safe = False
126+ # break
127+
128+ # # If this flight vector is safe, go for it
129+ # if vector_is_safe:
130+ # if angle != 0:
131+ # rospy.logwarn("ObstacleAvoider: modified requested velocity by {} radians".format(angle))
132+ # # We need to avoid, so make sure the vector magnitude is non-zero
133+ # response_magnitude = max(original_vector_magnitude, self._minimum_magnitude)
134+
135+ # # Copy Z velocity from commanded vector
136+ # # New flight vector has same magnitude as the commanded
137+ # barely_safe_vel = np.array([
138+ # response_magnitude
139+ # * cos(original_vector_direction+angle*sign),
140+ # response_magnitude
141+ # * sin(original_vector_direction+angle*sign),
142+ # desired_vector[2]])
143+ # reflected_safe_vel = np.array([
144+ # response_magnitude
145+ # * cos(original_vector_direction+2*angle*sign),
146+ # response_magnitude
147+ # * sin(original_vector_direction+2*angle*sign),
148+ # desired_vector[2]])
149+ # blend_constant = np.exp(-original_vector_magnitude / self._blending_speed)
150+ # return barely_safe_vel * blend_constant + reflected_safe_vel * (1-blend_constant)
151+
152+ #rospy.logwarn("ObstacleAvoider: couldn't find safe velocity!")
153+ ## Couldn't find a safe vector, just stop
154+ #return np.array([0, 0, desired_vector[2]])
107155
0 commit comments