Beispiel #1
0
 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()