class RobotControl(object): """ Robot interface class. In charge of getting sensor measurements (ROS subscribers), and transforms them into the 2D plane, and publishes velocity commands. """ def __init__(self, world_map, occupancy_map, pos_init, pos_goal, max_speed, max_omega, x_spacing, y_spacing, t_cam_to_body): """ Initialize the class """ # Handles all the ROS related items self.ros_interface = ROSInterface(t_cam_to_body) def to_degrees(self, rad): angle_degrees = rad * 180 / math.pi return angle_degrees def is_valid_distance(self, d): """ Discard Inf distances """ if (np.isnan(d) or np.isinf(d)): return False else: return True def is_within_range(self, i): """ Only consider -90 to 90 degrees """ #return -math.pi/4 < i < math.pi/4 return math.pi / 2 < abs(i) # LIDAR rotated backwards def process_measurements(self): """ Basic code to run autonomously """ # Check samples from https://github.com/Arkapravo/laser_obstacle_avoidance_pr2/blob/master/src/obstacle_avoidance_pr2.cpp scan = self.ros_interface.get_scan() angular_z = 0 linear_x = 0 k = 0.4 # 0.3 min_linear = 0.09 rotate_fast_value = 1. # 5.8 # rad/s linear_k = 0.15 go_forward = 0.5 # default value action = "None" if (scan == None): return #print "i:%.5f"%angle+"(%.5f"%self.to_degrees(angle)+"degrees) x:%.5f"%real_dist+" x:%.2f"%linear+" z:%.2f"%rotational min_range = scan.ranges[0] min_range_angle = 0 min_angle = 0 for j in range(0, 360): #increment by one degree real_dist = scan.ranges[j] angle = scan.angle_min + j * scan.angle_increment if self.is_within_range(angle) and self.is_valid_distance( real_dist): if (real_dist < min_range): min_range = real_dist min_angle = angle min_range_angle = j / 2 #print "minimum range is [%.3f]"%min_range+" at an angle of [%.3f]"%min_range_angle if ( min_range <= 0.5 ): # min_range<=0.5 gave box pushing like behaviour, min_range<=1.2 gave obstacle avoidance if (min_range_angle < 90): angular_z = rotate_fast_value linear_x = 0 action = "left " else: angular_z = -rotate_fast_value linear_x = 0 action = "right" else: angular_z = 0 linear_x = go_forward action = "straight" print "action:" + action + " min distance[%.2f]" % min_range + " angle [%.2f]" % min_range_angle + " (%.1f)" % angle + " min angle %.1f" % min_angle self.ros_interface.command_velocity(linear_x, angular_z) return
class RobotControl(object): """ Robot interface class. In charge of getting sensor measurements (ROS subscribers), and transforms them into the 2D plane, and publishes velocity commands. """ def __init__(self, pos_init, pos_goal, max_speed, max_omega, x_spacing, y_spacing, t_cam_to_body, min_lin_x, max_lin_x, rotate_fast_value, linear_k, k, min_linear_vel): """ Initialize the class """ self.min_lin_x = min_lin_x self.max_lin_x = max_lin_x self.rotate_fast_value = rotate_fast_value self.linear_k = linear_k self.k = k self.min_linear_vel = min_linear_vel # Handles all the ROS related items self.ros_interface = ROSInterface(t_cam_to_body) def to_degrees(self, rad): angle_degrees = rad * 180 / math.pi return angle_degrees def is_valid_distance(self, d): """ Discard Inf distances """ if (np.isnan(d) or np.isinf(d)): return False else: return True def is_within_range(self, i): """ Only consider -90 to 90 degrees """ #return -math.pi/4 < i < math.pi/4 return math.pi / 2 < abs(i) # LIDAR rotated backwards def process_measurements(self): """ Basic code to run autonomously """ # Check samples from https://github.com/markwsilliman/turtlebot/blob/master/goforward_and_avoid_obstacle.py scan = self.ros_interface.get_scan() linear = 0 rotational = 0 rcount = 0 hack = False msg = "" if (scan == None): return rcount = 0 for i, real_dist in enumerate(scan.ranges): angle = scan.angle_min + i * scan.angle_increment denom = 1. + real_dist * real_dist if self.is_within_range(angle) and self.is_valid_distance( real_dist): linear += math.sin(angle) / denom * 1. rotational += math.cos(angle) / denom * 1. rcount += 1 #print "%.5f"%self.to_degrees(angle)+"degrees x:%.5f"%real_dist+" x:%.2f"%linear+" z:%.2f"%rotational try: linear /= rcount rotational /= rcount except Exception as e: pass """ if (linear > self.k): linear = self.k elif (linear < -self.k): linear = -self.k """ linear_x = linear angular_z = rotational linear_x = linear # check this print "linear_x:%.5f" % linear_x + " angular_z:%.1f" % self.to_degrees( angular_z) + "(%.2f)" % angular_z + " " + msg #self.ros_interface.command_velocity(linear_x, angular_z) return
class RobotControl(object): """ Robot interface class. In charge of getting sensor measurements (ROS subscribers), and transforms them into the 2D plane, and publishes velocity commands. """ def __init__(self, world_map, occupancy_map, pos_init, pos_goal, max_speed, max_omega, x_spacing, y_spacing, t_cam_to_body): """ Initialize the class """ # Handles all the ROS related items self.ros_interface = ROSInterface(t_cam_to_body) def publish_action(self, speed, turn): self.ros_interface.command_velocity(speed, turn) def apply_avoidance_rules(self, distance_from_center, angle): p_angle = util.proportional_rotation(angle) action = "None" if self.inside_too_close_box(distance_from_center): # dangeorous distance, too close to turn, just try to go back action = "Back" rospy.loginfo("{}:{:.2f} {:.2f} dist:{:.2f}".format(action, angle, p_angle, distance_from_center)) self.react_to_inside_too_close_box(p_angle) elif util.is_forward_left(angle): # obstacle to left, go right action = "go forward right" self.publish_action(default_speed, p_angle) elif util.is_forward_right(angle): # obstacle to right, go left action = "go forward left" self.publish_action(default_speed, p_angle) elif util.is_backwards_left(angle): action = "obstacle backwards left" elif util.is_backwards_right(angle): action = "obstacle backwards right" else: action = "unknown" #rospy.loginfo("{}:{:.2f} {:.2f} dist:{:.2f}".format(action, angle, p_angle, distance_from_center)) def react_to_inside_too_close_box(self,angle): # go back and turn 180 degrees self.publish_action(-default_speed*2, 0) rate = rospy.Rate(avoid_check_rate) rate.sleep() if util.is_forward_right(angle): sign = -1 else: sign = 1 z = math.pi*1.6*sign rospy.loginfo('Back angle:'+str(angle)+' new angle:'+str(z)) self.publish_action(0, z) # around 5 rad/s rate.sleep() def inside_too_close_box(self, distance_from_center): forward_box_length, forward_box_width = self.forward_box() return distance_from_center < forward_box_width def get_closest_obstacle(self, scan): # returns closest point to obstacle inside a vehicle's 'forward box' # forward box is a rectangle ahead of the vehicle that if there is an obstacle # inside it it's necessary to do an action in order to avoid it forward_box_length, forward_box_width = self.forward_box() is_inside = False min_distance = max_distance min_i = 0 min_angle = 0 for i, distance in enumerate(scan.ranges): angle = scan.angle_min + i * scan.angle_increment if util.is_within_range(angle) and util.is_valid_distance(distance): #print i, distance, to_degrees(angle), angle if distance < min_distance: if point_is_inside_box(i, distance, scan, forward_box_length, forward_box_width, util.to_degrees(angle)): is_inside = True min_distance = distance min_i = i min_angle = angle #angle = angle_from_scan(min_i, scan) #angle_degrees = angle*180/math.pi #print min_i, angle_degrees, min_distance return is_inside, min_distance, min_angle def forward_box(self): # returns the Vehicle 'forward box' # to do: in future versions forward box should depend on vehicle's speed # more speed -> larger forward box length = Behaviour.body_width*look_ahead_factor width = clearance_safety_factor*Behaviour.body_width/2 return length, width def check_action(): pass def process_measurements(self): """ Basic code to run autonomously """ scan = self.ros_interface.get_scan() if(scan == None): return action = "None" angular_z = 0 linear_x = 0.3 is_inside, distance_from_center, angle = self.get_closest_obstacle(scan) # only do something if obstacle is inside forward box if is_inside: #print 'point inside ', distance_from_center, angle self.apply_avoidance_rules(distance_from_center, angle) else: self.publish_action(linear_x, angular_z) return