Exemple #1
0
    def plot_graph(self, axis_size=[-0.5, 3, -0.5, 3]):
        for obst in self.obstacles:
            ObstacleMap.plot_polygon(obst)

        for edge in self.edges:
            edge.plot()

        plt.axis('equal')
        plt.axis(axis_size)
        plt.show()
def removePointsInObstacle(exploreArray, map, plot=False):
    newexploreArray = []
    for p in exploreArray:
        if plot:
            plt.plot(p[0], p[1], 'ro')
            for obst in map.obstacles:
                ObstacleMap.plot_polygon(obst)
            plt.show()
        isInObstacle = inObstacle(p, map)
        if not isInObstacle:
            newexploreArray.append(list(p))
    return np.array(newexploreArray)
Exemple #3
0
 def keyPressEvent(self, event:QKeyEvent):
     if event.isAutoRepeat():
         return
     key = event.key()
     if key == Qt.Key_G:
         img = ObstacleMap(self.obstacles, GRAPH_TABLE_RATIO, self.robot.radius)
         print("dumping")
         img.dump_obstacle_grid_to_file("graph.txt")
     elif key == Qt.Key_Ampersand:
         self.ivy.send_action(1)
     elif key == Qt.Key_Eacute:
         self.ivy.send_action(2)
     elif key == Qt.Key_QuoteDbl:
         self.ivy.send_action(3)
     elif key == Qt.Key_Apostrophe:
         self.ivy.send_action(4)
     elif key == Qt.Key_ParenLeft:
         self.ivy.send_action(5)
     elif key == Qt.Key_Minus:
         self.ivy.send_action(6)
     elif key == Qt.Key_Egrave:
         self.ivy.send_action(7)
     elif key == Qt.Key_Underscore:
         self.ivy.send_action(8)
     elif key == Qt.Key_Ccedilla:
         self.ivy.send_action(9)
     elif key == Qt.Key_Agrave:
         self.ivy.send_action(10)
     elif key == Qt.Key_ParenRight:
         self.ivy.send_action(11)
     elif key == Qt.Key_Equal:
         self.ivy.send_action(12)
     elif key == Qt.Key_Z:
         self.robot_speed_command[1] = -1
         self.ivy.send_speed_direction(self.robot_speed_command)
     elif key == Qt.Key_Q:
         self.robot_speed_command[0] = 1
         self.ivy.send_speed_direction(self.robot_speed_command)
     elif key == Qt.Key_S:
         self.robot_speed_command[1] = 1
         self.ivy.send_speed_direction(self.robot_speed_command)
     elif key == Qt.Key_D:
         self.robot_speed_command[0] = -1
         self.ivy.send_speed_direction(self.robot_speed_command)
     elif key == Qt.Key_A:
         self.robot_speed_command[2] = 1
         self.ivy.send_speed_direction(self.robot_speed_command)
     elif key == Qt.Key_E:
         self.robot_speed_command[2] = -1
         self.ivy.send_speed_direction(self.robot_speed_command)
Exemple #4
0
 def __init__(self, level, bounding_box):
     # type: (MCLevel, TransformBox) -> Maps
     self.__width = bounding_box.size.x
     self.__length = bounding_box.size.z
     self.box = bounding_box
     self.obstacle_map = ObstacleMap(self.__width, self.__length, self)
     if level is not None:
         self.height_map = compute_height_map(level, bounding_box)
     else:
         xmin, xmax = bounding_box.minx, bounding_box.maxx
         zmin, zmax = bounding_box.minz, bounding_box.maxz
         self.height_map = array([[0 for _ in range(zmin, zmax)]
                                  for _ in range(xmin, xmax)])
     self.road_network = RoadNetwork(self.__width,
                                     self.__length,
                                     mc_map=self)
     self.fluid_map = FluidMap(self, level)
Exemple #5
0
    def __init__(self, robot):
        #initliase the node
        rospy.init_node('shared_control')

        #set up obstacle map
        self.robot = robot
        self.obstacle_map = ObstacleMap(robot)

        #set up locks
        self.obstacle_map_lock = Lock()
        self.js_lock = Lock()
        self.laser_lock = Lock()
        self.sonar_lock = Lock()
        self.odom_lock = Lock()
        self.polar_range_hist_lock = Lock()

        #set up internal variables
        self.curr_cmd = [0.0, 0.0] #0 linear, 0 angular
        self.curr_vel = [0.0, 0.0] #not moving initially
        self.sonar_readings = []
        self.laser_readings = []
        self.angles = []
        self.cosangles = []
        self.sinangles = []

        #basic safeguarding
        self.max_vel = 0.5
        self.max_turn_vel = 0.5

        #Forward simulation parameters
        self.delay_time = 0.1   #max delay before command is issued
        self.time_applied = 0.2 #how long the command is applied
        self.time_decc = 0.5    #how long to compute deceleration
        self.sim_interval = 0.1 #simulation interval

        #VFH parameters
        self.prh_smooth_window = 2
        self.prh_resolution = 2.0/180.0*np.pi
        self.polar_range_hist = [0]*int(2.0*np.pi/self.prh_resolution)
        self.raw_polar_range_hist = [0]*int(2.0*np.pi/self.prh_resolution)
        self.polar_range_hist_lock = Lock()
        self.max_considered_dist = 2.0

        self.closeness_weight = 0.5
        self.free_zone_weight = 0.5

        self.turning_coeff    = 2.0


        #set up subscribers
        rospy.Subscriber('js_cmd_vel', Twist, self.cmdVelCallback)
        rospy.Subscriber('base_scan', LaserScan, self.laserCallback)
        rospy.Subscriber('sonar_pc', PointCloud, self.sonarCallback)
        rospy.Subscriber('odom', Odometry, self.odomCallback)


        #set up publishers
        self.cmd_pub = rospy.Publisher('cmd_vel', Twist)
        self.obs_pub = rospy.Publisher('obstacle', Marker)
        self.polar_hist_pub = rospy.Publisher('polar_histogram', LaserScan)
        self.zone_score_pub = rospy.Publisher('zone_scores', LaserScan)
        self.projection_pub = rospy.Publisher('projection', PoseArray)
        self.time_taken_pub = rospy.Publisher('sc_time_taken', Float32)

        return
Exemple #6
0
class SharedControl():

    def __init__(self, robot):
        #initliase the node
        rospy.init_node('shared_control')

        #set up obstacle map
        self.robot = robot
        self.obstacle_map = ObstacleMap(robot)

        #set up locks
        self.obstacle_map_lock = Lock()
        self.js_lock = Lock()
        self.laser_lock = Lock()
        self.sonar_lock = Lock()
        self.odom_lock = Lock()
        self.polar_range_hist_lock = Lock()

        #set up internal variables
        self.curr_cmd = [0.0, 0.0] #0 linear, 0 angular
        self.curr_vel = [0.0, 0.0] #not moving initially
        self.sonar_readings = []
        self.laser_readings = []
        self.angles = []
        self.cosangles = []
        self.sinangles = []

        #basic safeguarding
        self.max_vel = 0.5
        self.max_turn_vel = 0.5

        #Forward simulation parameters
        self.delay_time = 0.1   #max delay before command is issued
        self.time_applied = 0.2 #how long the command is applied
        self.time_decc = 0.5    #how long to compute deceleration
        self.sim_interval = 0.1 #simulation interval

        #VFH parameters
        self.prh_smooth_window = 2
        self.prh_resolution = 2.0/180.0*np.pi
        self.polar_range_hist = [0]*int(2.0*np.pi/self.prh_resolution)
        self.raw_polar_range_hist = [0]*int(2.0*np.pi/self.prh_resolution)
        self.polar_range_hist_lock = Lock()
        self.max_considered_dist = 2.0

        self.closeness_weight = 0.5
        self.free_zone_weight = 0.5

        self.turning_coeff    = 2.0


        #set up subscribers
        rospy.Subscriber('js_cmd_vel', Twist, self.cmdVelCallback)
        rospy.Subscriber('base_scan', LaserScan, self.laserCallback)
        rospy.Subscriber('sonar_pc', PointCloud, self.sonarCallback)
        rospy.Subscriber('odom', Odometry, self.odomCallback)


        #set up publishers
        self.cmd_pub = rospy.Publisher('cmd_vel', Twist)
        self.obs_pub = rospy.Publisher('obstacle', Marker)
        self.polar_hist_pub = rospy.Publisher('polar_histogram', LaserScan)
        self.zone_score_pub = rospy.Publisher('zone_scores', LaserScan)
        self.projection_pub = rospy.Publisher('projection', PoseArray)
        self.time_taken_pub = rospy.Publisher('sc_time_taken', Float32)

        return

    #=============================================================================================
    # ROS Callbacks
    #=============================================================================================

    def cmdVelCallback(self, data):
        self.js_lock.acquire()

        #set the current requested command velocity
        self.curr_cmd = [data.linear.x, data.angular.z]

        self.js_lock.release()
        return

    def laserCallback(self, data):
        self.laser_lock.acquire()

        #initialise angles if not done yet
        if len(self.angles) != len(data.ranges):
            self.angles = []
            self.angles = np.arange(data.angle_min, data.angle_max+data.angle_increment, data.angle_increment)

        #calculate the cos and sin angles (caching)
        if len(self.cosangles) != len(self.angles):
            self.cosangles = [cos(angle) for angle in self.angles]
            self.sinangles = [sin(angle) for angle in self.angles]


        #set the points
        self.laser_header = data.header

        #warning: this transformation assumes that the laser is NOT rotated in any way
        #technically, we should use a tf transform for this (future work)
        self.laser_readings = [ [ data.ranges[i]*self.cosangles[i] + self.robot.laser_pos[0],
                                  data.ranges[i]*self.sinangles[i] + self.robot.laser_pos[1] ]
                                    for i in range(len(data.ranges)) ]

        self.laser_lock.release()
        return

    def sonarCallback(self, data):
        self.sonar_lock.acquire()
        #set the sonar readings
        self.sonar_readings = [ [pt.x, pt.y] for pt in data.points]

        self.sonar_lock.release()
        return


    def odomCallback(self, data):
        self.odom_lock.acquire()

        self.curr_vel = [data.twist.twist.linear.x, data.twist.twist.angular.z]

        self.odom_lock.release()


    #=============================================================================================
    # Obstacle Map
    #=============================================================================================

    def updateObstacleMap(self):
        """
        Updates the obstacle map
        """

        all_sensor_readings = self.laser_readings + self.sonar_readings

        #we remove all the sensor readings that occur inside the robot frame
        restricted_sensor_readings = []
        for pt in all_sensor_readings:
            if not self.obstacle_map.inRobot(pt):
                restricted_sensor_readings.append(pt)

        #add the obstacles to the obstacle map
        self.obstacle_map_lock.acquire()
        self.obstacle_map.addObstacles(restricted_sensor_readings)
        self.obstacle_map_lock.release()

        return

    #=============================================================================================
    # DWA Functions
    #=============================================================================================

    def checkForCollision(self, cmd, rough=False, publish=False):
        """
        Checks for collisions for a given command velocity
        @param: cmd the desired command
        @param: rough perform "rough" or point-in-polygon check?
        @param: publish publish the projection?
        @returns: True if this command results in a collision, False otherwise
        """
        #we first compute the expected trajectory
        trajectory = self.computeTrajectoryWithDecceleration(cmd, self.curr_vel, self.delay_time,
            self.time_applied, self.time_decc)

        if publish:
            self.publishProjection(trajectory)

        #then we check against the obstacle map for possible collisions
        first_pos = trajectory[0]
        if rough:
            for pos in trajectory:
                if np.linalg.norm(np.array(pos) - np.array(first_pos)) > 0.05:
                    if rough:
                        if self.obstacle_map.checkForCollisionAt_Rough(pos):
                            return True
                    else:
                        if self.obstacle_map.checkForCollisionAt(pos):
                            return True

        return False


    def computeTrajectoryWithDecceleration(self, cmd_vel, current_speed, delay_time, time_applied, time_decc):
        """
        computes the trajectory with the  cmd_vel applied for time_applied and then back to 0,0 for time_decc
        @param cmd_vel: the intended command velocities
        @param current_speed: the current speed
        @param time_applied: the time for which the current speed is applied
        @param time_decc: the time for which (0,0) is applied
        @return:
        """
        accs = [self.robot.acc_x, self.robot.acc_th, self.robot.dacc_x,self.robot.dacc_th ]

        #project forwards in time
        sim_interval = self.sim_interval
        sim_interval_sq = sim_interval*sim_interval
        cmd_vel = list(cmd_vel)
        current_speed = list(current_speed)

        max_acc_x = self.robot.max_acc_x
        max_acc_th = self.robot.max_acc_th
        max_decc_x = self.robot.max_dacc_x
        max_decc_th = self.robot.max_dacc_th


        code = """
            double x, y, th;
            x = y = th = 0;

            double vx, vth;
            vx = current_speed[0];
            vth = current_speed[1];

            double cmd_x = cmd_vel[0];
            double cmd_th = cmd_vel[1];

            //are we accelerating or decelerating?
            double acc_x;
            double acc_th;

            bool b_ax = false;
            if (cmd_x > vx) {
                acc_x = accs[0];
                b_ax = true;
            } else {
                acc_x = accs[2];
                acc_x = -1*acc_x;
            }

            bool b_ath = false;
            if (cmd_th > vth) {
                acc_th = accs[1];
                b_ath = true;
            } else {
                acc_th = -1* (double) accs[3];

            }

            py::list traj;

            //delay at the beginning
            for (double i=0; i<=delay_time; i+=sim_interval) {
                //check if we have already attained our velocity
                if (fabs(vx - (double) cmd_x) < 1e-6) vx = cmd_x;

                //check if we have exceeded our target
                if (b_ax && (vx > cmd_x )) vx = cmd_x;
                if (!b_ax && (vx < cmd_x )) vx = cmd_x;

                if (fabs(vth - (double) cmd_th) < 1e-6) vth = cmd_th;
                if (b_ath && (vth > cmd_th )) vth = cmd_th;
                if (!b_ath && (vth < cmd_th )) vth = cmd_th;

                x += vx*cos(th)*sim_interval;
                y += vx*sin(th)*sim_interval;
                th += vth*sim_interval;

                py::list new_pt;
                new_pt.append(x);
                new_pt.append(y);
                new_pt.append(th);
                traj.append(new_pt);
            }

            //application of the command
            for (double i=0; i<=time_applied; i+=sim_interval) {
                vx += acc_x*sim_interval;

                //check if we have already attained our velocity
                if (fabs(vx - (double) cmd_x) < 1e-6) vx = cmd_x;

                //check if we have exceeded our target
                if (b_ax && (vx > cmd_x )) vx = cmd_x;
                if (!b_ax && (vx < cmd_x )) vx = cmd_x;

                vth += acc_th*sim_interval;

                if (fabs(vth - (double) cmd_th) < 1e-6) vth = cmd_th;
                if (b_ath && (vth > cmd_th )) vth = cmd_th;
                if (!b_ath && (vth < cmd_th )) vth = cmd_th;

                x += vx*cos(th)*sim_interval;
                y += vx*sin(th)*sim_interval;
                th += vth*sim_interval;

                py::list new_pt;
                new_pt.append(x);
                new_pt.append(y);
                new_pt.append(th);
                traj.append(new_pt);
            }

            //delay before decellerating
            for (double i=0; i<=delay_time; i+=sim_interval) {
                //check if we have already attained our velocity
                if (fabs(vx - (double) cmd_x) < 1e-6) vx = cmd_x;

                //check if we have exceeded our target
                if (b_ax && (vx > cmd_x )) vx = cmd_x;
                if (!b_ax && (vx < cmd_x )) vx = cmd_x;

                if (fabs(vth - (double) cmd_th) < 1e-6) vth = cmd_th;
                if (b_ath && (vth > cmd_th )) vth = cmd_th;
                if (!b_ath && (vth < cmd_th )) vth = cmd_th;

                x += vx*cos(th)*sim_interval;
                y += vx*sin(th)*sim_interval;
                th += vth*sim_interval;

                py::list new_pt;
                new_pt.append(x);
                new_pt.append(y);
                new_pt.append(th);
                traj.append(new_pt);
            }

            //now we apply a stop
            b_ax = false;
            if (0 > vx) {
                acc_x = accs[0];
                b_ax = true;
            } else {
                acc_x = -1 * (double) accs[2];
            }

            b_ath = false;
            if (0 > vth) {
                acc_th = accs[1];
                b_ath = true;
            } else {
                acc_th = -1 * (double) accs[3];
            }

            for (double i=0; i<=time_decc; i+=sim_interval) {
                vx += acc_x*sim_interval;

                //check if we have already attained our velocity
                if (fabs(vx) < 1e-6) vx = 0;

                //check if we have exceeded our target
                if (b_ax && (vx > 0 )) vx = 0;
                if (!b_ax && (vx < 0 )) vx = 0;

                vth += acc_th*sim_interval;

                if (fabs(vth) < 1e-6) vth = 0;
                if (b_ath && (vth > 0 )) vth = 0;
                if (!b_ath && (vth < 0 )) vth = 0;

                x += vx*cos(th)*sim_interval;
                y += vx*sin(th)*sim_interval;
                th += vth*sim_interval;

                py::list new_pt;
                new_pt.append(x);
                new_pt.append(y);
                new_pt.append(th);
                traj.append(new_pt);


                bool stopped = (fabs(vth) < 1e-2) && (fabs(vx) < 1e-2);
                if (stopped) break;

            }

            return_val = traj;
        """
        traj = weave.inline(code, ['cmd_vel', 'current_speed', 'accs','sim_interval', 'delay_time',  'sim_interval_sq',
                                   'time_applied', 'max_acc_x', 'max_acc_th', 'max_decc_x', 'max_decc_th',
                                   'time_decc'], type_converters=converters.blitz)

        return traj

    def findLimitedDWACmd(self, cmd):
        """
        Performs obstacle avoidance with a DWA variant
        @param: cmd the desired command
        @returns: cmd the augmented command
        """
        #if no collision, we just return
        if self.checkForCollision(cmd, rough=True, publish=True) == False:
            return cmd

        #create a set of commands to test
        best_cmd = [0.0,0.0]
        cmds = [ [scale*cmd[0], scale*cmd[1]] for scale in np.arange(1.0, -0.2, -0.2) ]
        #print cmds
        for cmd_to_test in cmds:
            if not self.checkForCollision(cmd_to_test, rough=True):
                best_cmd = cmd_to_test
                break

        return best_cmd

    #=============================================================================================
    # VFH Functions
    #=============================================================================================

    def computePolarRangeHist(self):
        '''
        Generates a polar histogram given the obstacle map
        '''
        prh_resolution = self.prh_resolution
        max_considered_dist = self.max_considered_dist


        polar_range_hist_len = int((2.0*np.pi/prh_resolution))
        obstacles = list(self.obstacle_map.obstacles_in_memory)

        n_obstacles = len(obstacles)
        prh_smooth_window = self.prh_smooth_window
        pi = np.pi
        code = """
                int i;
                double *polar_range_hist = new double[polar_range_hist_len];
                double *temp_polar_range_hist = new double[polar_range_hist_len];

                for (int i=0; i<polar_range_hist_len; i++) {
                    polar_range_hist[i] = max_considered_dist;

                }

                for (i=0; i<n_obstacles; i++) {
                    double direction = atan2(obstacles[i][1], obstacles[i][0]) + pi;
                    double r = sqrt( pow(obstacles[i][1],2.0) + pow(obstacles[i][0],2.0));
                    if (r > max_considered_dist) r = max_considered_dist;

                    //int key = ((int) round(direction/prh_resolution))%polar_range_hist_len;
                    int key = ((int) round(direction/prh_resolution));

                    while (key < 0) {
                        key = polar_range_hist_len + key;
                    }
                    key = key%polar_range_hist_len;

                    if (polar_range_hist[key] > r) {
                        polar_range_hist[key] = r;
                    }
                }

                for (int i=0; i<polar_range_hist_len; i++) {
                    temp_polar_range_hist[i] = polar_range_hist[i];

                }

                for (i=0; i<polar_range_hist_len; i++) {
                    int lb = i-prh_smooth_window;
                    int up = i+prh_smooth_window+1;
                    double sum = 0.0;
                    for (int j=lb; j < up; j++) {

                        int key = j;
                        while (key < 0) key = polar_range_hist_len + key;

                        key = key%polar_range_hist_len;

                        sum += (1.0 - fabs(j-i)/(polar_range_hist_len + 1.0))*polar_range_hist[key];
                    }
                    temp_polar_range_hist[i] = sum/(double) (prh_smooth_window*2.0);
                }


                py::list result;
                for (i =0; i<polar_range_hist_len; i++) {
                    result.append(temp_polar_range_hist[i]);
                }

                delete [] polar_range_hist;
                delete [] temp_polar_range_hist;

                return_val = result;
            """
        polar_range_hist = weave.inline(code, ['prh_resolution', 'polar_range_hist_len',
                                               'obstacles', 'pi',
                                               'n_obstacles', 'prh_smooth_window', 'max_considered_dist'],
                                                type_converters=converters.blitz)

        self.polar_range_hist_lock.acquire()
        #self.polar_range_hist = 1.0 - np.exp(-1.0*np.array(polar_range_hist)/1.25)
        self.polar_range_hist = np.array(polar_range_hist)
        self.polar_range_hist_lock.release()

    def getAngleDiff_deg(self,a1, a2):
        dif = np.mod(np.abs(a1 - a2), 360)

        if dif > 180:
            dif = 360 - dif

        return dif


    def getClosenessMeasure(self, x, y, sd):
        return np.exp(-(np.linalg.norm(np.array(x) - np.array(y)))/sd)

    def findVFHCmd(self, cmd):
        """
        Performs obstacle avoidance with a VFH variant
        @param: cmd the desired command
        @returns: cmd the augmented command
        """
        self.computePolarRangeHist()
        self.publishPolarHistogram()

        if cmd == [0, 0]:
            return cmd

        #modify based on polar histogram
        #get new turning velocity
        prh_threshold = 0.01

        #setup variables we'll use
        local_prh = np.array(self.polar_range_hist)
        prh_resolution = self.prh_resolution
        prh_resolution_deg = self.prh_resolution*180/np.pi
        polar_range_hist_len = int((2.0*np.pi/prh_resolution))
        degree_angles = np.arange(-180, 180, prh_resolution_deg )

        assert(len(degree_angles) == polar_range_hist_len)
        closeness_scores = np.array([0.0]*polar_range_hist_len)

        #compute the closeness of the degree to the user's command
        for i in range(len(closeness_scores)):
            closeness_scores[i] = self.getAngleDiff_deg(cmd[1]*180/np.pi, degree_angles[i])
            closeness_scores[i] = np.exp(-np.fabs(closeness_scores[i])*np.pi/180) #convert to radians

        #compute the zone scores
        zone_scores = closeness_scores*self.closeness_weight + local_prh*self.free_zone_weight

        #publish the zone scores
        self.publishZoneScores(zone_scores, prh_resolution)

        #threshold the zone values
        for i in range(len(local_prh)):
            if local_prh[i] < prh_threshold:
                zone_scores[i] = -100

        #search for the best value
        range_to_search = len(zone_scores)/4
        midpoint = round(len(local_prh)/2)

        max_item = np.argmax(zone_scores[
                             (midpoint - range_to_search):(midpoint + range_to_search)])

        new_turning_vel = degree_angles[max_item + (midpoint-range_to_search)]

        #perform conversion
        new_turning_vel = self.turning_coeff*(new_turning_vel/180)


        #limit to a maximum
        new_turning_vel = np.sign(new_turning_vel)*max( abs(self.max_turn_vel), abs(new_turning_vel))


        #get new forward velocity - this is identify to basic 
        obstacles = list(self.obstacle_map.obstacles_in_memory)

        curr_xspeed = abs(self.curr_vel[0])
        if curr_xspeed > 0.25:
            #the faster you are going, the more modification is performed
            front_modifier = 0.5 + 0.5*(self.max_vel - curr_xspeed)
            side_modifier = 0.5 + 0.5*(self.max_vel - curr_xspeed)
        else:
            front_modifier = 1.0
            side_modifier = 1.0

        for obs in obstacles:
            new_modifier = 1.0
            if (np.sign(cmd[0])*obs[0] > 0) and (abs(obs[1]) < self.robot.footprint[1][1]):
                dist = abs(obs[0])

                if dist < 0.5:
                    new_modifier = 0.0
                elif dist > 2.0:
                    new_modifier = 1.0
                else:
                    new_modifier = (dist/2.0)

            front_modifier = min(new_modifier, front_modifier)
        new_forward_vel = front_modifier*cmd[0]

        #change the turning velocity if necessary (when going backwards)
        if new_forward_vel < 0:
            new_turning_vel *= -1

        #set the best command
        best_cmd = [new_forward_vel, new_turning_vel]

        return best_cmd


    #=============================================================================================
    # Basic Collision Prevention
    #=============================================================================================
    def findBasicSafeguardedCmd(self, cmd):
        """
        Performs basic "ad-hoc" safeguarding
        @param: cmd the desired command
        @returns: cmd the augmented command
        """
        obstacles = list(self.obstacle_map.obstacles_in_memory)

        curr_xspeed = abs(self.curr_vel[0])
        if curr_xspeed > 0.25:
            #the faster you are going, the more modification is performed
            front_modifier = 0.5 + 0.5*(self.max_vel - curr_xspeed)
            side_modifier = 0.5 + 0.5*(self.max_vel - curr_xspeed)
        else:
            front_modifier = 1.0
            side_modifier = 1.0

        for obs in obstacles:
            new_modifier = 1.0
            if (np.sign(cmd[0])*obs[0] > 0) and (abs(obs[1]) < self.robot.footprint[1][1]):
                dist = abs(obs[0])

                if dist < 0.7:
                    new_modifier = 0.0
                elif dist > 2.0:
                    new_modifier = 1.0
                else:
                    new_modifier = (dist/2.0)

            front_modifier = min(new_modifier, front_modifier)

        for obs in obstacles:
            new_modifier = 1.0
            if (np.sign(cmd[1])*obs[1] > 0) and (abs(obs[0]) < self.robot.footprint[2][0]):
                dist = abs(obs[1])

                if dist < 0.50:
                    new_modifier = 0.0
                elif dist > 2.0:
                    new_modifier = 1.0
                else:
                    new_modifier = (dist/2.0)

            side_modifier = min(new_modifier, side_modifier)
        rospy.loginfo('Basic Modifiers: ' + str(front_modifier) + ', ' + str(side_modifier))

        best_cmd = [front_modifier*cmd[0], side_modifier*cmd[1]]
        return best_cmd


    #=============================================================================================
    # ROS Publishing Functions
    #=============================================================================================

    def publishCmd(self, cmd):
        """
        Publishes the velocity command
        """
        cmd_to_publish = Twist()
        cmd_to_publish.linear.x = cmd[0]
        cmd_to_publish.angular.z = cmd[1]
        self.cmd_pub.publish(cmd_to_publish)


    def publishObstacles(self):
        """
        Publishes the obstacles as markers
        """
        mk = Marker()
        mk.header.stamp = rospy.get_rostime()
        mk.header.frame_id = '/base_link'

        mk.ns='basic_shapes'
        mk.id = 0
        mk.type = Marker.POINTS
        mk.scale.x = 0.3
        mk.scale.y = 0.3
        mk.scale.z = 0.3
        mk.color.r = 1.0
        mk.color.a = 1.0

        for value in self.obstacle_map.obstacles_in_memory:
            p = Point()
            p.x = value[0]
            p.y = value[1]
            mk.points.append(p)


        self.obs_pub.publish(mk)


    def publishPolarHistogram(self):
        """
        Publishes the polar histogram
        """
        pc = LaserScan()
        pc.header.frame_id = "/base_link"
        pc.header.stamp = rospy.get_rostime()

        pc.angle_min = -np.pi
        pc.angle_max = np.pi
        pc.angle_increment = self.prh_resolution
        pc.range_min = 0.00
        pc.range_max = 5.0

        self.polar_range_hist_lock.acquire()
        for r in self.polar_range_hist:
            pc.ranges.append(r)

        self.polar_range_hist_lock.release()
        self.polar_hist_pub.publish(pc)


    def publishZoneScores(self,data, prh_resolution):
        """
        Publishes the zone scores
        """
        pc = LaserScan()
        pc.header.frame_id = "/base_link"
        pc.header.stamp = rospy.get_rostime()

        pc.angle_min = -np.pi
        pc.angle_max = np.pi
        pc.angle_increment = prh_resolution
        pc.range_min = 0.00
        pc.range_max = 5.0

        for r in data:
            pc.ranges.append(r)

        self.zone_score_pub.publish(pc)

    def publishProjection(self, data):
        """
        Publishes the forward projection/simulation
        """
        proj = PoseArray()
        proj.header.stamp = rospy.get_rostime()
        proj.header.frame_id = "/base_link"
        for pt in data:
            pos = Pose()
            pos.position.x = pt[0]
            pos.position.y = pt[1]
            orient = quaternion_from_euler(0,0,pt[2])
            pos.orientation.x = orient[0]
            pos.orientation.y = orient[1]
            pos.orientation.z = orient[2]
            pos.orientation.w = orient[3]
            proj.poses.append(pos)

        self.projection_pub.publish(proj)

    def publishTimeTaken(self, data):
        """
        Publishes the time taken
        """
        time_taken = Float32()
        time_taken.data = data
        self.time_taken_pub.publish(data)

    #=============================================================================================
    # Main Functions
    #=============================================================================================

    def updateAndPublish(self):

        #first we update the obstacle map
        start_time = time.time()

        rospy.loginfo("Updating Obstacle map")
        self.updateObstacleMap()

        rospy.loginfo("Publishing Obstacles")
        self.publishObstacles()

        rospy.loginfo("Finding Shared-Control Command")
        start_time = rospy.get_time()
        curr_cmd = self.curr_cmd

        #uncomment to choose the algorithm you want to test
        best_cmd = self.findBasicSafeguardedCmd(curr_cmd)
        #best_cmd = self.findLimitedDWACmd(curr_cmd)
        #best_cmd = self.findVFHCmd(curr_cmd)

        elapsed_time = rospy.get_time() - start_time
        self.publishTimeTaken(elapsed_time)

        rospy.loginfo("Publishing Shared-Control Command")
        self.publishCmd(best_cmd)

        return

    def startLoop(self, rate=10):
        rospy.loginfo("Starting shared control")
        r = rospy.Rate(rate)
        try:
            while not rospy.is_shutdown():
                self.updateAndPublish()
                r.sleep()
        except rospy.ROSInterruptException:
            pass

        return
Exemple #7
0
    def __init__(self, robot):
        #initliase the node
        rospy.init_node('shared_control')

        #set up obstacle map
        self.robot = robot
        self.obstacle_map = ObstacleMap(robot)

        #set up locks
        self.obstacle_map_lock = Lock()
        self.js_lock = Lock()
        self.laser_lock = Lock()
        self.sonar_lock = Lock()
        self.odom_lock = Lock()
        self.polar_range_hist_lock = Lock()

        #set up internal variables
        self.curr_cmd = [0.0, 0.0]  #0 linear, 0 angular
        self.curr_vel = [0.0, 0.0]  #not moving initially
        self.sonar_readings = []
        self.laser_readings = []
        self.angles = []
        self.cosangles = []
        self.sinangles = []

        #basic safeguarding
        self.max_vel = 0.5
        self.max_turn_vel = 0.5

        #Forward simulation parameters
        self.delay_time = 0.1  #max delay before command is issued
        self.time_applied = 0.2  #how long the command is applied
        self.time_decc = 0.5  #how long to compute deceleration
        self.sim_interval = 0.1  #simulation interval

        #VFH parameters
        self.prh_smooth_window = 2
        self.prh_resolution = 2.0 / 180.0 * np.pi
        self.polar_range_hist = [0] * int(2.0 * np.pi / self.prh_resolution)
        self.raw_polar_range_hist = [0] * int(
            2.0 * np.pi / self.prh_resolution)
        self.polar_range_hist_lock = Lock()
        self.max_considered_dist = 2.0

        self.closeness_weight = 0.5
        self.free_zone_weight = 0.5

        self.turning_coeff = 2.0

        #set up subscribers
        rospy.Subscriber('js_cmd_vel', Twist, self.cmdVelCallback)
        rospy.Subscriber('base_scan', LaserScan, self.laserCallback)
        rospy.Subscriber('sonar_pc', PointCloud, self.sonarCallback)
        rospy.Subscriber('odom', Odometry, self.odomCallback)

        #set up publishers
        self.cmd_pub = rospy.Publisher('cmd_vel', Twist)
        self.obs_pub = rospy.Publisher('obstacle', Marker)
        self.polar_hist_pub = rospy.Publisher('polar_histogram', LaserScan)
        self.zone_score_pub = rospy.Publisher('zone_scores', LaserScan)
        self.projection_pub = rospy.Publisher('projection', PoseArray)
        self.time_taken_pub = rospy.Publisher('sc_time_taken', Float32)

        return
Exemple #8
0
class SharedControl():
    def __init__(self, robot):
        #initliase the node
        rospy.init_node('shared_control')

        #set up obstacle map
        self.robot = robot
        self.obstacle_map = ObstacleMap(robot)

        #set up locks
        self.obstacle_map_lock = Lock()
        self.js_lock = Lock()
        self.laser_lock = Lock()
        self.sonar_lock = Lock()
        self.odom_lock = Lock()
        self.polar_range_hist_lock = Lock()

        #set up internal variables
        self.curr_cmd = [0.0, 0.0]  #0 linear, 0 angular
        self.curr_vel = [0.0, 0.0]  #not moving initially
        self.sonar_readings = []
        self.laser_readings = []
        self.angles = []
        self.cosangles = []
        self.sinangles = []

        #basic safeguarding
        self.max_vel = 0.5
        self.max_turn_vel = 0.5

        #Forward simulation parameters
        self.delay_time = 0.1  #max delay before command is issued
        self.time_applied = 0.2  #how long the command is applied
        self.time_decc = 0.5  #how long to compute deceleration
        self.sim_interval = 0.1  #simulation interval

        #VFH parameters
        self.prh_smooth_window = 2
        self.prh_resolution = 2.0 / 180.0 * np.pi
        self.polar_range_hist = [0] * int(2.0 * np.pi / self.prh_resolution)
        self.raw_polar_range_hist = [0] * int(
            2.0 * np.pi / self.prh_resolution)
        self.polar_range_hist_lock = Lock()
        self.max_considered_dist = 2.0

        self.closeness_weight = 0.5
        self.free_zone_weight = 0.5

        self.turning_coeff = 2.0

        #set up subscribers
        rospy.Subscriber('js_cmd_vel', Twist, self.cmdVelCallback)
        rospy.Subscriber('base_scan', LaserScan, self.laserCallback)
        rospy.Subscriber('sonar_pc', PointCloud, self.sonarCallback)
        rospy.Subscriber('odom', Odometry, self.odomCallback)

        #set up publishers
        self.cmd_pub = rospy.Publisher('cmd_vel', Twist)
        self.obs_pub = rospy.Publisher('obstacle', Marker)
        self.polar_hist_pub = rospy.Publisher('polar_histogram', LaserScan)
        self.zone_score_pub = rospy.Publisher('zone_scores', LaserScan)
        self.projection_pub = rospy.Publisher('projection', PoseArray)
        self.time_taken_pub = rospy.Publisher('sc_time_taken', Float32)

        return

    #=============================================================================================
    # ROS Callbacks
    #=============================================================================================

    def cmdVelCallback(self, data):
        self.js_lock.acquire()

        #set the current requested command velocity
        self.curr_cmd = [data.linear.x, data.angular.z]

        self.js_lock.release()
        return

    def laserCallback(self, data):
        self.laser_lock.acquire()

        #initialise angles if not done yet
        if len(self.angles) != len(data.ranges):
            self.angles = []
            self.angles = np.arange(data.angle_min,
                                    data.angle_max + data.angle_increment,
                                    data.angle_increment)

        #calculate the cos and sin angles (caching)
        if len(self.cosangles) != len(self.angles):
            self.cosangles = [cos(angle) for angle in self.angles]
            self.sinangles = [sin(angle) for angle in self.angles]

        #set the points
        self.laser_header = data.header

        #warning: this transformation assumes that the laser is NOT rotated in any way
        #technically, we should use a tf transform for this (future work)
        self.laser_readings = [[
            data.ranges[i] * self.cosangles[i] + self.robot.laser_pos[0],
            data.ranges[i] * self.sinangles[i] + self.robot.laser_pos[1]
        ] for i in range(len(data.ranges))]

        self.laser_lock.release()
        return

    def sonarCallback(self, data):
        self.sonar_lock.acquire()
        #set the sonar readings
        self.sonar_readings = [[pt.x, pt.y] for pt in data.points]

        self.sonar_lock.release()
        return

    def odomCallback(self, data):
        self.odom_lock.acquire()

        self.curr_vel = [data.twist.twist.linear.x, data.twist.twist.angular.z]

        self.odom_lock.release()

    #=============================================================================================
    # Obstacle Map
    #=============================================================================================

    def updateObstacleMap(self):
        """
        Updates the obstacle map
        """

        all_sensor_readings = self.laser_readings + self.sonar_readings

        #we remove all the sensor readings that occur inside the robot frame
        restricted_sensor_readings = []
        for pt in all_sensor_readings:
            if not self.obstacle_map.inRobot(pt):
                restricted_sensor_readings.append(pt)

        #add the obstacles to the obstacle map
        self.obstacle_map_lock.acquire()
        self.obstacle_map.addObstacles(restricted_sensor_readings)
        self.obstacle_map_lock.release()

        return

    #=============================================================================================
    # DWA Functions
    #=============================================================================================

    def checkForCollision(self, cmd, rough=False, publish=False):
        """
        Checks for collisions for a given command velocity
        @param: cmd the desired command
        @param: rough perform "rough" or point-in-polygon check?
        @param: publish publish the projection?
        @returns: True if this command results in a collision, False otherwise
        """
        #we first compute the expected trajectory
        trajectory = self.computeTrajectoryWithDecceleration(
            cmd, self.curr_vel, self.delay_time, self.time_applied,
            self.time_decc)

        if publish:
            self.publishProjection(trajectory)

        #then we check against the obstacle map for possible collisions
        first_pos = trajectory[0]
        if rough:
            for pos in trajectory:
                if np.linalg.norm(np.array(pos) - np.array(first_pos)) > 0.05:
                    if rough:
                        if self.obstacle_map.checkForCollisionAt_Rough(pos):
                            return True
                    else:
                        if self.obstacle_map.checkForCollisionAt(pos):
                            return True

        return False

    def computeTrajectoryWithDecceleration(self, cmd_vel, current_speed,
                                           delay_time, time_applied,
                                           time_decc):
        """
        computes the trajectory with the  cmd_vel applied for time_applied and then back to 0,0 for time_decc
        @param cmd_vel: the intended command velocities
        @param current_speed: the current speed
        @param time_applied: the time for which the current speed is applied
        @param time_decc: the time for which (0,0) is applied
        @return:
        """
        accs = [
            self.robot.acc_x, self.robot.acc_th, self.robot.dacc_x,
            self.robot.dacc_th
        ]

        #project forwards in time
        sim_interval = self.sim_interval
        sim_interval_sq = sim_interval * sim_interval
        cmd_vel = list(cmd_vel)
        current_speed = list(current_speed)

        max_acc_x = self.robot.max_acc_x
        max_acc_th = self.robot.max_acc_th
        max_decc_x = self.robot.max_dacc_x
        max_decc_th = self.robot.max_dacc_th

        code = """
            double x, y, th;
            x = y = th = 0;

            double vx, vth;
            vx = current_speed[0];
            vth = current_speed[1];

            double cmd_x = cmd_vel[0];
            double cmd_th = cmd_vel[1];

            //are we accelerating or decelerating?
            double acc_x;
            double acc_th;

            bool b_ax = false;
            if (cmd_x > vx) {
                acc_x = accs[0];
                b_ax = true;
            } else {
                acc_x = accs[2];
                acc_x = -1*acc_x;
            }

            bool b_ath = false;
            if (cmd_th > vth) {
                acc_th = accs[1];
                b_ath = true;
            } else {
                acc_th = -1* (double) accs[3];

            }

            py::list traj;

            //delay at the beginning
            for (double i=0; i<=delay_time; i+=sim_interval) {
                //check if we have already attained our velocity
                if (fabs(vx - (double) cmd_x) < 1e-6) vx = cmd_x;

                //check if we have exceeded our target
                if (b_ax && (vx > cmd_x )) vx = cmd_x;
                if (!b_ax && (vx < cmd_x )) vx = cmd_x;

                if (fabs(vth - (double) cmd_th) < 1e-6) vth = cmd_th;
                if (b_ath && (vth > cmd_th )) vth = cmd_th;
                if (!b_ath && (vth < cmd_th )) vth = cmd_th;

                x += vx*cos(th)*sim_interval;
                y += vx*sin(th)*sim_interval;
                th += vth*sim_interval;

                py::list new_pt;
                new_pt.append(x);
                new_pt.append(y);
                new_pt.append(th);
                traj.append(new_pt);
            }

            //application of the command
            for (double i=0; i<=time_applied; i+=sim_interval) {
                vx += acc_x*sim_interval;

                //check if we have already attained our velocity
                if (fabs(vx - (double) cmd_x) < 1e-6) vx = cmd_x;

                //check if we have exceeded our target
                if (b_ax && (vx > cmd_x )) vx = cmd_x;
                if (!b_ax && (vx < cmd_x )) vx = cmd_x;

                vth += acc_th*sim_interval;

                if (fabs(vth - (double) cmd_th) < 1e-6) vth = cmd_th;
                if (b_ath && (vth > cmd_th )) vth = cmd_th;
                if (!b_ath && (vth < cmd_th )) vth = cmd_th;

                x += vx*cos(th)*sim_interval;
                y += vx*sin(th)*sim_interval;
                th += vth*sim_interval;

                py::list new_pt;
                new_pt.append(x);
                new_pt.append(y);
                new_pt.append(th);
                traj.append(new_pt);
            }

            //delay before decellerating
            for (double i=0; i<=delay_time; i+=sim_interval) {
                //check if we have already attained our velocity
                if (fabs(vx - (double) cmd_x) < 1e-6) vx = cmd_x;

                //check if we have exceeded our target
                if (b_ax && (vx > cmd_x )) vx = cmd_x;
                if (!b_ax && (vx < cmd_x )) vx = cmd_x;

                if (fabs(vth - (double) cmd_th) < 1e-6) vth = cmd_th;
                if (b_ath && (vth > cmd_th )) vth = cmd_th;
                if (!b_ath && (vth < cmd_th )) vth = cmd_th;

                x += vx*cos(th)*sim_interval;
                y += vx*sin(th)*sim_interval;
                th += vth*sim_interval;

                py::list new_pt;
                new_pt.append(x);
                new_pt.append(y);
                new_pt.append(th);
                traj.append(new_pt);
            }

            //now we apply a stop
            b_ax = false;
            if (0 > vx) {
                acc_x = accs[0];
                b_ax = true;
            } else {
                acc_x = -1 * (double) accs[2];
            }

            b_ath = false;
            if (0 > vth) {
                acc_th = accs[1];
                b_ath = true;
            } else {
                acc_th = -1 * (double) accs[3];
            }

            for (double i=0; i<=time_decc; i+=sim_interval) {
                vx += acc_x*sim_interval;

                //check if we have already attained our velocity
                if (fabs(vx) < 1e-6) vx = 0;

                //check if we have exceeded our target
                if (b_ax && (vx > 0 )) vx = 0;
                if (!b_ax && (vx < 0 )) vx = 0;

                vth += acc_th*sim_interval;

                if (fabs(vth) < 1e-6) vth = 0;
                if (b_ath && (vth > 0 )) vth = 0;
                if (!b_ath && (vth < 0 )) vth = 0;

                x += vx*cos(th)*sim_interval;
                y += vx*sin(th)*sim_interval;
                th += vth*sim_interval;

                py::list new_pt;
                new_pt.append(x);
                new_pt.append(y);
                new_pt.append(th);
                traj.append(new_pt);


                bool stopped = (fabs(vth) < 1e-2) && (fabs(vx) < 1e-2);
                if (stopped) break;

            }

            return_val = traj;
        """
        traj = weave.inline(code, [
            'cmd_vel', 'current_speed', 'accs', 'sim_interval', 'delay_time',
            'sim_interval_sq', 'time_applied', 'max_acc_x', 'max_acc_th',
            'max_decc_x', 'max_decc_th', 'time_decc'
        ],
                            type_converters=converters.blitz)

        return traj

    def findLimitedDWACmd(self, cmd):
        """
        Performs obstacle avoidance with a DWA variant
        @param: cmd the desired command
        @returns: cmd the augmented command
        """
        #if no collision, we just return
        if self.checkForCollision(cmd, rough=True, publish=True) == False:
            return cmd

        #create a set of commands to test
        best_cmd = [0.0, 0.0]
        cmds = [[scale * cmd[0], scale * cmd[1]]
                for scale in np.arange(1.0, -0.2, -0.2)]
        #print cmds
        for cmd_to_test in cmds:
            if not self.checkForCollision(cmd_to_test, rough=True):
                best_cmd = cmd_to_test
                break

        return best_cmd

    #=============================================================================================
    # VFH Functions
    #=============================================================================================

    def computePolarRangeHist(self):
        '''
        Generates a polar histogram given the obstacle map
        '''
        prh_resolution = self.prh_resolution
        max_considered_dist = self.max_considered_dist

        polar_range_hist_len = int((2.0 * np.pi / prh_resolution))
        obstacles = list(self.obstacle_map.obstacles_in_memory)

        n_obstacles = len(obstacles)
        prh_smooth_window = self.prh_smooth_window
        pi = np.pi
        code = """
                int i;
                double *polar_range_hist = new double[polar_range_hist_len];
                double *temp_polar_range_hist = new double[polar_range_hist_len];

                for (int i=0; i<polar_range_hist_len; i++) {
                    polar_range_hist[i] = max_considered_dist;

                }

                for (i=0; i<n_obstacles; i++) {
                    double direction = atan2(obstacles[i][1], obstacles[i][0]) + pi;
                    double r = sqrt( pow(obstacles[i][1],2.0) + pow(obstacles[i][0],2.0));
                    if (r > max_considered_dist) r = max_considered_dist;

                    //int key = ((int) round(direction/prh_resolution))%polar_range_hist_len;
                    int key = ((int) round(direction/prh_resolution));

                    while (key < 0) {
                        key = polar_range_hist_len + key;
                    }
                    key = key%polar_range_hist_len;

                    if (polar_range_hist[key] > r) {
                        polar_range_hist[key] = r;
                    }
                }

                for (int i=0; i<polar_range_hist_len; i++) {
                    temp_polar_range_hist[i] = polar_range_hist[i];

                }

                for (i=0; i<polar_range_hist_len; i++) {
                    int lb = i-prh_smooth_window;
                    int up = i+prh_smooth_window+1;
                    double sum = 0.0;
                    for (int j=lb; j < up; j++) {

                        int key = j;
                        while (key < 0) key = polar_range_hist_len + key;

                        key = key%polar_range_hist_len;

                        sum += (1.0 - fabs(j-i)/(polar_range_hist_len + 1.0))*polar_range_hist[key];
                    }
                    temp_polar_range_hist[i] = sum/(double) (prh_smooth_window*2.0);
                }


                py::list result;
                for (i =0; i<polar_range_hist_len; i++) {
                    result.append(temp_polar_range_hist[i]);
                }

                delete [] polar_range_hist;
                delete [] temp_polar_range_hist;

                return_val = result;
            """
        polar_range_hist = weave.inline(code, [
            'prh_resolution', 'polar_range_hist_len', 'obstacles', 'pi',
            'n_obstacles', 'prh_smooth_window', 'max_considered_dist'
        ],
                                        type_converters=converters.blitz)

        self.polar_range_hist_lock.acquire()
        #self.polar_range_hist = 1.0 - np.exp(-1.0*np.array(polar_range_hist)/1.25)
        self.polar_range_hist = np.array(polar_range_hist)
        self.polar_range_hist_lock.release()

    def getAngleDiff_deg(self, a1, a2):
        dif = np.mod(np.abs(a1 - a2), 360)

        if dif > 180:
            dif = 360 - dif

        return dif

    def getClosenessMeasure(self, x, y, sd):
        return np.exp(-(np.linalg.norm(np.array(x) - np.array(y))) / sd)

    def findVFHCmd(self, cmd):
        """
        Performs obstacle avoidance with a VFH variant
        @param: cmd the desired command
        @returns: cmd the augmented command
        """
        self.computePolarRangeHist()
        self.publishPolarHistogram()

        if cmd == [0, 0]:
            return cmd

        #modify based on polar histogram
        #get new turning velocity
        prh_threshold = 0.01

        #setup variables we'll use
        local_prh = np.array(self.polar_range_hist)
        prh_resolution = self.prh_resolution
        prh_resolution_deg = self.prh_resolution * 180 / np.pi
        polar_range_hist_len = int((2.0 * np.pi / prh_resolution))
        degree_angles = np.arange(-180, 180, prh_resolution_deg)

        assert (len(degree_angles) == polar_range_hist_len)
        closeness_scores = np.array([0.0] * polar_range_hist_len)

        #compute the closeness of the degree to the user's command
        for i in range(len(closeness_scores)):
            closeness_scores[i] = self.getAngleDiff_deg(
                cmd[1] * 180 / np.pi, degree_angles[i])
            closeness_scores[i] = np.exp(-np.fabs(closeness_scores[i]) *
                                         np.pi / 180)  #convert to radians

        #compute the zone scores
        zone_scores = closeness_scores * self.closeness_weight + local_prh * self.free_zone_weight

        #publish the zone scores
        self.publishZoneScores(zone_scores, prh_resolution)

        #threshold the zone values
        for i in range(len(local_prh)):
            if local_prh[i] < prh_threshold:
                zone_scores[i] = -100

        #search for the best value
        range_to_search = len(zone_scores) / 4
        midpoint = round(len(local_prh) / 2)

        max_item = np.argmax(zone_scores[(midpoint -
                                          range_to_search):(midpoint +
                                                            range_to_search)])

        new_turning_vel = degree_angles[max_item +
                                        (midpoint - range_to_search)]

        #perform conversion
        new_turning_vel = self.turning_coeff * (new_turning_vel / 180)

        #limit to a maximum
        new_turning_vel = np.sign(new_turning_vel) * max(
            abs(self.max_turn_vel), abs(new_turning_vel))

        #get new forward velocity - this is identify to basic
        obstacles = list(self.obstacle_map.obstacles_in_memory)

        curr_xspeed = abs(self.curr_vel[0])
        if curr_xspeed > 0.25:
            #the faster you are going, the more modification is performed
            front_modifier = 0.5 + 0.5 * (self.max_vel - curr_xspeed)
            side_modifier = 0.5 + 0.5 * (self.max_vel - curr_xspeed)
        else:
            front_modifier = 1.0
            side_modifier = 1.0

        for obs in obstacles:
            new_modifier = 1.0
            if (np.sign(cmd[0]) * obs[0] > 0) and (abs(obs[1]) <
                                                   self.robot.footprint[1][1]):
                dist = abs(obs[0])

                if dist < 0.5:
                    new_modifier = 0.0
                elif dist > 2.0:
                    new_modifier = 1.0
                else:
                    new_modifier = (dist / 2.0)

            front_modifier = min(new_modifier, front_modifier)
        new_forward_vel = front_modifier * cmd[0]

        #change the turning velocity if necessary (when going backwards)
        if new_forward_vel < 0:
            new_turning_vel *= -1

        #set the best command
        best_cmd = [new_forward_vel, new_turning_vel]

        return best_cmd

    #=============================================================================================
    # Basic Collision Prevention
    #=============================================================================================
    def findBasicSafeguardedCmd(self, cmd):
        """
        Performs basic "ad-hoc" safeguarding
        @param: cmd the desired command
        @returns: cmd the augmented command
        """
        obstacles = list(self.obstacle_map.obstacles_in_memory)

        curr_xspeed = abs(self.curr_vel[0])
        if curr_xspeed > 0.25:
            #the faster you are going, the more modification is performed
            front_modifier = 0.5 + 0.5 * (self.max_vel - curr_xspeed)
            side_modifier = 0.5 + 0.5 * (self.max_vel - curr_xspeed)
        else:
            front_modifier = 1.0
            side_modifier = 1.0

        for obs in obstacles:
            new_modifier = 1.0
            if (np.sign(cmd[0]) * obs[0] > 0) and (abs(obs[1]) <
                                                   self.robot.footprint[1][1]):
                dist = abs(obs[0])

                if dist < 0.7:
                    new_modifier = 0.0
                elif dist > 2.0:
                    new_modifier = 1.0
                else:
                    new_modifier = (dist / 2.0)

            front_modifier = min(new_modifier, front_modifier)

        for obs in obstacles:
            new_modifier = 1.0
            if (np.sign(cmd[1]) * obs[1] > 0) and (abs(obs[0]) <
                                                   self.robot.footprint[2][0]):
                dist = abs(obs[1])

                if dist < 0.50:
                    new_modifier = 0.0
                elif dist > 2.0:
                    new_modifier = 1.0
                else:
                    new_modifier = (dist / 2.0)

            side_modifier = min(new_modifier, side_modifier)
        rospy.loginfo('Basic Modifiers: ' + str(front_modifier) + ', ' +
                      str(side_modifier))

        best_cmd = [front_modifier * cmd[0], side_modifier * cmd[1]]
        return best_cmd

    #=============================================================================================
    # ROS Publishing Functions
    #=============================================================================================

    def publishCmd(self, cmd):
        """
        Publishes the velocity command
        """
        cmd_to_publish = Twist()
        cmd_to_publish.linear.x = cmd[0]
        cmd_to_publish.angular.z = cmd[1]
        self.cmd_pub.publish(cmd_to_publish)

    def publishObstacles(self):
        """
        Publishes the obstacles as markers
        """
        mk = Marker()
        mk.header.stamp = rospy.get_rostime()
        mk.header.frame_id = '/base_link'

        mk.ns = 'basic_shapes'
        mk.id = 0
        mk.type = Marker.POINTS
        mk.scale.x = 0.3
        mk.scale.y = 0.3
        mk.scale.z = 0.3
        mk.color.r = 1.0
        mk.color.a = 1.0

        for value in self.obstacle_map.obstacles_in_memory:
            p = Point()
            p.x = value[0]
            p.y = value[1]
            mk.points.append(p)

        self.obs_pub.publish(mk)

    def publishPolarHistogram(self):
        """
        Publishes the polar histogram
        """
        pc = LaserScan()
        pc.header.frame_id = "/base_link"
        pc.header.stamp = rospy.get_rostime()

        pc.angle_min = -np.pi
        pc.angle_max = np.pi
        pc.angle_increment = self.prh_resolution
        pc.range_min = 0.00
        pc.range_max = 5.0

        self.polar_range_hist_lock.acquire()
        for r in self.polar_range_hist:
            pc.ranges.append(r)

        self.polar_range_hist_lock.release()
        self.polar_hist_pub.publish(pc)

    def publishZoneScores(self, data, prh_resolution):
        """
        Publishes the zone scores
        """
        pc = LaserScan()
        pc.header.frame_id = "/base_link"
        pc.header.stamp = rospy.get_rostime()

        pc.angle_min = -np.pi
        pc.angle_max = np.pi
        pc.angle_increment = prh_resolution
        pc.range_min = 0.00
        pc.range_max = 5.0

        for r in data:
            pc.ranges.append(r)

        self.zone_score_pub.publish(pc)

    def publishProjection(self, data):
        """
        Publishes the forward projection/simulation
        """
        proj = PoseArray()
        proj.header.stamp = rospy.get_rostime()
        proj.header.frame_id = "/base_link"
        for pt in data:
            pos = Pose()
            pos.position.x = pt[0]
            pos.position.y = pt[1]
            orient = quaternion_from_euler(0, 0, pt[2])
            pos.orientation.x = orient[0]
            pos.orientation.y = orient[1]
            pos.orientation.z = orient[2]
            pos.orientation.w = orient[3]
            proj.poses.append(pos)

        self.projection_pub.publish(proj)

    def publishTimeTaken(self, data):
        """
        Publishes the time taken
        """
        time_taken = Float32()
        time_taken.data = data
        self.time_taken_pub.publish(data)

    #=============================================================================================
    # Main Functions
    #=============================================================================================

    def updateAndPublish(self):

        #first we update the obstacle map
        start_time = time.time()

        rospy.loginfo("Updating Obstacle map")
        self.updateObstacleMap()

        rospy.loginfo("Publishing Obstacles")
        self.publishObstacles()

        rospy.loginfo("Finding Shared-Control Command")
        start_time = rospy.get_time()
        curr_cmd = self.curr_cmd

        #uncomment to choose the algorithm you want to test
        best_cmd = self.findBasicSafeguardedCmd(curr_cmd)
        #best_cmd = self.findLimitedDWACmd(curr_cmd)
        #best_cmd = self.findVFHCmd(curr_cmd)

        elapsed_time = rospy.get_time() - start_time
        self.publishTimeTaken(elapsed_time)

        rospy.loginfo("Publishing Shared-Control Command")
        self.publishCmd(best_cmd)

        return

    def startLoop(self, rate=10):
        rospy.loginfo("Starting shared control")
        r = rospy.Rate(rate)
        try:
            while not rospy.is_shutdown():
                self.updateAndPublish()
                r.sleep()
        except rospy.ROSInterruptException:
            pass

        return
def build_graph(path_to_map, robot_radius):
    obst_map = ObstacleMap(robot_radius)
    obst_map.construct_obstacle_map(path_to_map)
    graph = Graph(obst_map.obstacles, obst_map.map_dimensions, robot_radius)
    graph.build_visibility_graph()
    return graph