def obstacleCallback(self, obstacle_poses):
amount_obstacles = len(obstacle_poses.poses)
# rospy.loginfo('Number of obstacles: %d', len(obstacle_poses.poses))
amount_obstacles_on_track = 0
obstacle_poses_on_track = PoseArray()
obstacle_poses_on_track.header = obstacle_poses.header
avoidance_active = BoolStamped()
avoidance_active.data = False
target = LanePose()
# target.header.frame_id = self.robot_name
target.v_ref = 10 # max speed high, current top 0.38
for x in range(amount_obstacles):
# rospy.loginfo(obstacle_poses.poses[x].position.z)
if obstacle_poses.poses[x].position.z > 0:
# Bounding window
# get relative coordinates
x_obstacle = obstacle_poses.poses[x].position.x * 1000 # mm
y_obstacle = obstacle_poses.poses[x].position.y * 1000 # mm
# get global coordinates
global_pos_vec = self.avoider.coordinatetransform(
x_obstacle, y_obstacle, self.theta_current, self.d_current)
# rospy.loginfo('x_obstacle = %f', x_obstacle)
# rospy.loginfo('y_obstacle = %f', y_obstacle)
# rospy.loginfo('theta_current = %f', self.theta_current)
# rospy.loginfo('d_current = %f', self.d_current)
x_global = global_pos_vec[0] # mm
y_global = global_pos_vec[1] # mm
# rospy.loginfo(global_pos_vec)
# check if obstacle is within boundaries
if x_global < self.x_bounding_width and abs(
y_global) < self.y_bounding_width:
# rospy.loginfo('Obstacle in range - Beware')
obstacle_poses_on_track.poses.append(
obstacle_poses.poses[x])
amount_obstacles_on_track += 1
if amount_obstacles_on_track == 0: # rospy.loginfo('0 obstacles on track')
v = 0
elif amount_obstacles_on_track == 1:
#ToDo: check if self.d_current can be accessed through forwarding of self
# targets = self.avoider.avoid(obstacle_poses_on_track, self.d_current, self.theta_current)
# target.d_ref = targets[0]
target.v_ref = 0 # due to inaccuracies in theta, stop in any case
# if targets[1]: # emergency stop
# target.v_ref = 0
# self.theta_target_pub.publish(targets[2]) # theta not calculated in current version
# rospy.loginfo('1 obstacles on track')
# rospy.loginfo('d_target= %f', targets[0])
# rospy.loginfo('emergency_stop = %f', targets[1])
avoidance_active.data = True
else:
target.v_ref = 0
# rospy.loginfo('%d obstacles on track', amount_obstacles_on_track)
avoidance_active.data = True
target.v_ref = 0
# rospy.loginfo('emergency_stop = 1')
self.obstavoidpose_topic.publish(target)
self.avoid_pub.publish(avoidance_active)
# rospy.loginfo('Avoidance flag set: %s', avoidance_active.data)
return
def MainLoop(self):
if not self.active:
return
rate = rospy.Rate(self.rate)
while not rospy.is_shutdown():
# run state machine
if self.state == self.state_dict['IDLE']:
# waiting for FSMState to tell us that Duckiebot is at an intersection (see FSMCallback)
pass
elif self.state == self.state_dict['INITIALIZING_LOCALIZATION']:
if self.InitializeLocalization():
self.state = self.state_dict['INITIALIZING_PATH']
rospy.loginfo(
"[%s] Initialized intersection localization, initializing path."
% (self.node_name))
elif self.state == self.state_dict['INITIALIZING_PATH']:
if self.InitializePath():
self.state = self.state_dict['TRAVERSING']
self.s = 0.0
self.traversing_start = rospy.Time.now()
rospy.loginfo(
"[%s] Initialized path, waiting for go signal." %
(self.node_name))
else:
rospy.loginfo(
"[%s] Could not initialize path. Trying again." %
(self.node_name))
elif self.state == self.state_dict['TRAVERSING']:
'''open-loop'''
if self.open_loop:
msg_cmds = Twist2DStamped()
msg_cmds.header.stamp = rospy.Time.now()
if 1.0 < (rospy.Time.now() -
self.traversing_start).to_sec() and self.go:
pos, vel = self.pathPlanner.EvaluatePath(self.s)
dt = 0.01
_, vel2 = self.pathPlanner.EvaluatePath(self.s + dt)
self.alpha = self.v / np.linalg.norm(vel)
dir = vel / np.linalg.norm(vel)
dir2 = vel2 / np.linalg.norm(vel2)
theta = np.arctan2(dir[1], dir[0])
theta2 = np.arctan2(dir2[1], dir2[0])
omega = (theta2 - theta) / dt
msg_cmds.v = self.v
msg_cmds.omega = self.alpha * omega
if (msg_cmds.v -
0.5 * math.fabs(msg_cmds.omega) * 0.1) < 0.065:
msg_cmds.v = 0.065 + 0.5 * math.fabs(
msg_cmds.omega) * 0.1
self.alpha = self.alpha * msg_cmds.v / self.v
msg_cmds.v = msg_cmds.v * self.v_scale
msg_cmds.omega = msg_cmds.omega * self.omega_scale
self.s = self.s + self.alpha * (rospy.Time.now(
) - self.traversing_last_time).to_sec()
if (self.s > 0.99):
msg_cmds.v = self.v
msg_cmds.omega = 0.0
self.state = self.state_dict['DONE']
self.done_time = rospy.Time.now()
else:
msg_cmds.v = 0.0
msg_cmds.omega = 0.0
self.traversing_last_time = rospy.Time.now()
self.pub_cmds.publish(msg_cmds)
else:
'''closed-loop'''
msg_lane_pose = LanePose()
msg_lane_pose.header.stamp = rospy.Time.now()
if 1.0 < (rospy.Time.now() -
self.traversing_start).to_sec() and self.go:
pose, _ = self.poseEstimator.PredictState(
msg_lane_pose.header.stamp)
dist, theta, curvature, self.s = self.pathPlanner.ComputeLaneError(
pose, self.s)
#rospy.loginfo("the s is: "+str(self.s))
if (self.s > self.s_max):
msg_lane_pose.v_ref = self.v
msg_lane_pose.d = 0.0
msg_lane_pose.d_ref = 0.0
msg_lane_pose.phi = 0.0
msg_lane_pose.curvature_ref = 0.0
self.state = self.state_dict['DONE']
self.done_time = rospy.Time.now()
else:
msg_lane_pose.v_ref = self.v
msg_lane_pose.d = dist
msg_lane_pose.d_ref = 0.0
msg_lane_pose.phi = theta
msg_lane_pose.curvature_ref = curvature
else:
msg_lane_pose.v_ref = 0.0
msg_lane_pose.d = 0.0
msg_lane_pose.d_ref = 0.0
msg_lane_pose.phi = 0.0
msg_lane_pose.curvature_ref = 0.0
self.pub_lane_pose.publish(msg_lane_pose)
elif self.state == self.state_dict['DONE']:
# switch back to lane following if in lane
if self.in_lane and (rospy.Time.now() - self.in_lane_time
).to_sec() < self.in_lane_timeout:
msg_done = BoolStamped()
msg_done.header.stamp = rospy.Time.now()
msg_done.data = True
self.pub_done.publish(msg_done)
self.state = self.state_dict['IDLE']
# go straight for 2.0 secs if not in lane yet. After 2 secs stop.
else:
'''open-loop'''
if self.open_loop:
if (rospy.Time.now() - self.done_time
).to_sec() < self.in_lane_wait_time:
msg_cmds = Twist2DStamped()
msg_cmds.header.stamp = rospy.Time.now()
msg_cmds.v = self.v * self.v_scale
msg_cmds.omega = 0.0 * self.omega_scale
self.pub_cmds.publish(msg_cmds)
else:
msg_cmds = Twist2DStamped()
msg_cmds.header.stamp = rospy.Time.now()
msg_cmds.v = 0.0 * self.v_scale
msg_cmds.omega = 0.0 * self.omega_scale
self.pub_cmds.publish(msg_cmds)
rospy.loginfo(
"[%s] Could not find lane. Stopping now." %
(self.node_name))
self.state = self.state_dict['ERROR']
else:
'''closed-loop'''
if (rospy.Time.now() - self.done_time
).to_sec() < self.in_lane_wait_time:
msg_lane_pose = LanePose()
msg_lane_pose.header.stamp = rospy.Time.now()
msg_lane_pose.v_ref = self.v
msg_lane_pose.d = 0.0
msg_lane_pose.d_ref = 0.0
msg_lane_pose.phi = 0.0
msg_lane_pose.curvature_ref = 0.0
self.pub_lane_pose.publish(msg_lane_pose)
else:
msg_lane_pose = LanePose()
msg_lane_pose.header.stamp = rospy.Time.now()
msg_lane_pose.v_ref = 0.0
msg_lane_pose.d = 0.0
msg_lane_pose.d_ref = 0.0
msg_lane_pose.phi = 0.0
msg_lane_pose.curvature_ref = 0.0
self.pub_lane_pose.publish(msg_lane_pose)
rospy.loginfo(
"[%s] Could not find lane. Stopping now." %
(self.node_name))
self.state = self.state_dict['ERROR']
else:
rospy.loginfo("[%s] Something went wrong." % (self.node_name))
rate.sleep()