class PointCloudAccumulatorNode(object): def __init__(self): rospy.init_node('pcl_accumulator') self.obstacles_accumulator = NumpyAccumulator(3) self.ground_accumulator = NumpyAccumulator(3) self.tf_listener = TransformListener() rospy.Subscriber("point_cloud_ground", PointCloud2, lambda point_cloud: self.ground_cloud_handler(point_cloud)) rospy.Subscriber("point_cloud_obstacles", PointCloud2, lambda point_cloud: self.obstacle_cloud_handler(point_cloud)) rospy.Subscriber("map_static", OccupancyGrid, lambda grid: self.static_map_handler(grid)) self.ground_point_cloud_publisher = rospy.Publisher('acc_point_cloud_ground', PointCloud2, queue_size=1) self.obstacles_static_point_cloud_publisher = rospy.Publisher('acc_point_cloud_obstacles_static', PointCloud2, queue_size=1) self.obstacles_dynamic_point_cloud_publisher = rospy.Publisher('acc_point_cloud_obstacles_dynamic', PointCloud2, queue_size=1) self.static_map = None rospy.Subscriber("request_acc", Empty, lambda request: self.request_handler(request)) rospy.spin() def point_cloud_to_np_array(self, point_cloud): # assuming 4 float32-s for a point a = np.fromstring(point_cloud.data, dtype=np.float32).reshape((-1, 4)) a[:3] = 1.0 # use homogeneous coords for matrix transformation return a def transform_cloud(self, point_cloud, msg_header, target_frame): self.tf_listener.waitForTransform(msg_header.frame_id, target_frame, msg_header.stamp, rospy.Duration(4.0)) mat44 = self.tf_listener.asMatrix(target_frame, msg_header) # assuming point_cloud is a Nx4 matrix return np.dot(point_cloud, mat44.T) def obstacle_cloud_handler(self, point_cloud_msg): point_cloud = self.point_cloud_to_np_array(point_cloud_msg) point_cloud = self.transform_cloud(point_cloud, point_cloud_msg.header, MAP_FRAME) self.obstacles_accumulator.append(point_cloud[:, 0:3]) print("Obstacle cloud received, accumulated size: {0}".format(self.obstacles_accumulator.get_rows())) def ground_cloud_handler(self, point_cloud_msg): point_cloud = self.point_cloud_to_np_array(point_cloud_msg) point_cloud = self.transform_cloud(point_cloud, point_cloud_msg.header, MAP_FRAME) self.ground_accumulator.append(point_cloud[:, 0:3]) print("Ground cloud received, accumulated size: {0}".format(self.ground_accumulator.get_rows())) def static_map_handler(self, map): self.static_map = map print("Static map received") def request_handler(self, request): if self.static_map is None: print("No map received") return start_time = time.time() grid = LocalOccupancyGrid(self.static_map, LocalOccupancyGridParams(rospy, '~grid_')) obstacle_cloud = self.obstacles_accumulator.get() ground_cloud = self.ground_accumulator.get() cols = grid.get_col_i(obstacle_cloud) rows = grid.get_row_i(obstacle_cloud) valid_pos = np.logical_and( np.logical_and(cols >= 0, cols < grid.cols()), np.logical_and(rows >= 0, rows < grid.rows()), ) obstacle_cloud = obstacle_cloud[valid_pos] ground_cols = grid.get_col_i(ground_cloud) ground_rows = grid.get_row_i(ground_cloud) ground_valid_pos = np.logical_and( np.logical_and(ground_cols >= 0, ground_cols < grid.cols()), np.logical_and(ground_rows >= 0, ground_rows < grid.rows()), ) ground_cloud = ground_cloud[ground_valid_pos] cols = cols[valid_pos] rows = rows[valid_pos] threshold = STATIC_THRESHOLD obstacle_static_cloud = obstacle_cloud[grid.get_grid()[rows, cols] > threshold, :] obstacle_dynamic_cloud = obstacle_cloud[grid.get_grid()[rows, cols] < threshold, :] now = rospy.Time.now() publish_point_cloud(self.ground_point_cloud_publisher, ground_cloud, MAP_FRAME, now) publish_point_cloud(self.obstacles_static_point_cloud_publisher, obstacle_static_cloud, MAP_FRAME, now) publish_point_cloud(self.obstacles_dynamic_point_cloud_publisher, obstacle_dynamic_cloud, MAP_FRAME, now) print("Point clouds published, generation took {0}s".format(time.time() - start_time))
class Laser2PCWorld(): #translate laser to pc and publish point cloud to /cloud_in def __init__(self): self.laserSub = rospy.Subscriber('/scan', LaserScan, self.laserCallBack) self.laser = [float('inf')] self.pc_laser = None self.pc_world = None self._tf = TransformListener() self.occupied_cells = [] self.free_cells = [] self.mat_laser2world = None def laserCallBack(self, data): self.laser = data def update(self): # Get the transformation matrix for laser to world target_frame = "world" src_frame = "laser0_frame" if self._tf.frameExists(target_frame) and self._tf.frameExists( src_frame): try: self.mat_laser2world = self._tf.asMatrix( target_frame, self.laser.header) except Exception as e: print(e) else: print('Unable to find:', self._tf.frameExists(target_frame), self._tf.frameExists(src_frame)) # Transform laser data to world coordinate if not isinstance(self.laser, LaserScan) or not isinstance( self.mat_laser2world, np.ndarray): return try: l = copy.deepcopy(self.laser) mat = copy.deepcopy(self.mat_laser2world) free = [] #in world coord occuiped = [] #in world coord pc_laser = [] pc_world = [] for r in range(len(l.ranges)): radius = 0 encountered_obs = True if l.ranges[r] == float('inf'): #append free cells till max range radius = l.range_max encountered_obs = False theta = 180 - (l.angle_min + r * l.angle_increment ) #due to differences in ros corrdinate frame x = l.ranges[radius] * np.cos(theta) y = l.ranges[radius] * np.sin(theta) z = 0 pc_laser.append([x, y, z]) col = np.array([[x, y, z, 1]]).T tran = np.matmul(mat, col).reshape((4)) pc_world.append([tran[X], tran[Y], tran[X]]) discretized_pt = self.append_free_space( [tran[X], tran[Y], tran[X]]) # print("discretized_pt", len(discretized_pt)) if encountered_obs: # print("before", len(occuiped)) occuiped.extend(discretized_pt[-10:]) # print("pt", discretized_pt[-10:]) free.extend(discretized_pt[:-10]) # print("after", len(occuiped)) else: free.extend(discretized_pt) # print("encoutered obs", encountered_obs) # print("occuipied", len(occuiped)) # print("free", len(free)) self.pc_laser = pc_laser self.pc_world = pc_world self.occupied_cells = occuiped self.free_cells = free except Exception as e: print(e) def append_free_space(self, pt): distance = 0.05 a = abs(pt[X]) / distance b = abs(pt[Y]) / distance c = abs(pt[Z]) / distance num_of_sample = max(a, b, c) x = np.linspace(0, pt[X], num_of_sample) y = np.linspace(0, pt[Y], num_of_sample) z = np.linspace(0, pt[Z], num_of_sample) coords = zip(x, y, z) return coords @property def ready(self): laser_ready = isinstance(self.laser, LaserScan) mat_ready = isinstance(self.mat_laser2world, np.ndarray) return laser_ready and mat_ready @property def measurements(self): return self._measurements
class pcScan_to_pcWorld(): def __init__(self): self.pcSub = rospy.Subscriber('/slam_cloud', pc, self.callback) self._tf = TransformListener() self.laser_pc = None self.mat_laser2world = None self.world_pc = None self.laser_pc_sync = None def callback(self, data): self.laser_pc = data def update(self): target_frame = "world" src_frame = "laser0_frame" if self._tf.frameExists(target_frame) and self._tf.frameExists( src_frame): try: self.mat_laser2world = self._tf.asMatrix( target_frame, self.laser_pc.header) except Exception as e: print(e) else: print('Unable to find:', self._tf.frameExists(target_frame), self._tf.frameExists(src_frame)) if not isinstance(self.laser_pc, pc) or not isinstance( self.mat_laser2world, np.ndarray): return try: lpc = copy.deepcopy(self.laser_pc) mat = copy.deepcopy(self.mat_laser2world) wpc = copy.deepcopy(self.laser_pc) for idx, p in enumerate(np.array(lpc.points)): col = np.array([[float(p.x), float(p.y), float(p.z), 1.0]]).T tran_p = np.matmul(mat, col).reshape((4)) wpc.points[idx].x = tran_p[X] wpc.points[idx].y = tran_p[Y] wpc.points[idx].z = tran_p[Z] self.world_pc = wpc self.laser_pc_sync = lpc except Exception as e: print(e) def get_Cell(self): thres = 0.05 #threshold to be considered as occupied pt = copy.deepcopy(self.world_pc.points) val = copy.deepcopy(self.world_pc.channels[0].values) #'intensities' mat = copy.deepcopy(self.mat_laser2world) pt = list(pt) val = np.array(val) trans_pt = [[p.x, p.y, p.z] for p in pt] trans_pt = np.array(trans_pt) occupied_world = trans_pt[val < thres] laser_pt = copy.deepcopy(self.laser_pc_sync.points) laser_pt = list(laser_pt) trans_laser_pt = [[p.x, p.y, p.z] for p in laser_pt] trans_laser_pt = np.array(trans_laser_pt) occupied_laser = trans_laser_pt[val < thres] # free = trans_pt[val >=thres] #Higher intensity laser means obstacle doesnt exist #Calculate free space based on distance from obstacles free_world = [] distance = 0.5 for pt in occupied_laser: a = abs(pt[X]) / distance b = abs(pt[Y]) / distance c = abs(pt[Z]) / distance num_of_sample = max(a, b, c) x = np.linspace(0, pt[X], num_of_sample) y = np.linspace(0, pt[Y], num_of_sample) z = np.linspace(0, pt[Z], num_of_sample) coords = zip(x, y, z) for c in coords: #Translate back to World Coordinate col = np.array([[float(c[X]), float(c[Y]), float(c[Z]), 1.0]]).T tran_p = np.matmul(mat, col).reshape((4)) wx = tran_p[X] wy = tran_p[Y] wz = tran_p[Z] free_world.append((wx, wy, wz)) print("Occupied", len(occupied_world), "Free", len(free_world)) return occupied_world, free_world def ready(self): return isinstance(self.world_pc, pc)
class AdaptiveClbfNode(object): def __init__(self): self.tf = TransformListener() self.odom_frame = rospy.get_namespace() + "odom" self.base_link_frame = rospy.get_namespace() + "base_link" self.adaptive_clbf = AdaptiveClbf(odim=6) self.prev_odom_timestamp = rospy.Time(0) self.prev_goal_timestamp = rospy.Time(0) self.joy_cmd_time = rospy.Time(0) self.e_stop_time = rospy.Time(0) self.joy_cmd = AckermannDriveStamped() self.heartbeat_time = rospy.Time(0) self.pointcloud_time = rospy.Time(0) self.odom = Odometry() self.encoder_odom = Odometry() self.use_pose_goal = True self.x = np.zeros((4,1)) self.u_prev = np.zeros((2,1)) self.z_ref = np.zeros((5,1)) self.z_ref_dot = np.zeros((5,1)) self.current_vel_body_x = 0.0 self.current_vel_body_y = 0.0 self.enable = True self.sent_train_goal = False self.params={} self.params["vehicle_length"] = rospy.get_param('~vehicle_length',0.5) self.params["steering_limit"] = rospy.get_param('~steering_limit',1.0) self.params["scale_acceleration"] = rospy.get_param('~scale_acceleration', 1.0) self.params["acceleration_gain"] = rospy.get_param('~acceleration_gain', 1.0) self.params["acceleration_deadband"] = rospy.get_param('~acceleration_deadband', 0.0) self.params["max_accel"] = rospy.get_param('~max_accel',1.0) self.params["min_accel"] = rospy.get_param('~min_accel',-1.0) self.params["kp_z"] = rospy.get_param('~kp_z',1.0) self.params["kd_z"] = rospy.get_param('~kd_z',1.0) self.params["clf_epsilon"] = rospy.get_param('~clf_epsilon',1.0) self.params["reverse_velocity_goal"] = rospy.get_param('~reverse_velocity_goal',True) self.params["learning_verbose"] = rospy.get_param('~learning_verbose',False) self.params["use_model"] = rospy.get_param('~use_model',True) self.params["add_data"] = rospy.get_param('~add_data',True) self.params["model_train"] = rospy.get_param('~model_train',True) self.params["check_model"] = rospy.get_param('~check_model',True) self.params["measurement_noise"] = rospy.get_param('~measurement_noise', 1.0) self.params["N_data"] = rospy.get_param('~N_data',200) self.params["max_error"] = rospy.get_param('~max_error',1.0) self.params["use_qp"] = rospy.get_param('~use_qp',True) self.params["qp_u_cost"] = rospy.get_param('~qp_u_cost',0.01) self.params["qp_u_prev_cost"] = rospy.get_param('~qp_u_prev_cost',1.0) self.params["qp_p1_cost"] = rospy.get_param('~qp_p1_cost',1.0e6) self.params["qp_p2_cost"] = rospy.get_param('~qp_p2_cost',1.0e6) self.params["qp_ksig"] = rospy.get_param('~qp_ksig',1.0) self.params["qp_max_var"] = rospy.get_param('~qp_max_var',1.0) self.params["qp_verbose"] = rospy.get_param('~qp_verbose',False) self.params["max_velocity"] = rospy.get_param('~max_velocity',5.0) self.params["min_velocity"] = rospy.get_param('~min_velocity',0.001) self.params["barrier_vel_gamma"] = rospy.get_param('~barrier_vel_gamma',1.0) self.params["use_barrier_vel"] = rospy.get_param('~use_barrier_vel',True) self.params["use_barrier_pointcloud"] = rospy.get_param('~use_barrier_pointcloud',True) self.params["barrier_radius"] = rospy.get_param('~barrier_radius',1.0) self.params["barrier_radius_velocity_scale"] = rospy.get_param('~barrier_radius_velocity_scale',1.0) self.params["barrier_pc_gamma_p"] = rospy.get_param('~barrier_pc_gamma_p',1.0) self.params["barrier_pc_gamma"] = rospy.get_param('~barrier_pc_gamma',1.0) self.params["barrier_max_distance"] = rospy.get_param('~barrier_max_distance',1.0) self.params["barrier_resolution"] = rospy.get_param('~barrier_resolution',1.0) self.params["verbose"] = rospy.get_param('~verbose',False) self.kp_goal = rospy.get_param('~kp_goal',1.0) self.desired_vel = rospy.get_param('~desired_vel',0.5) # Create a dynamic reconfigure client self.dyn_reconfig_client = DynamicReconfigureClient('controller_adaptiveclbf_reconfig', timeout=30, config_callback=self.reconfigure_cb) # Create publishers self.pub_debug = rospy.Publisher('~debug', DebugData, queue_size=10) self.pub_control = rospy.Publisher('output', AckermannDriveStamped, queue_size = 1) self.pub_ref = rospy.Publisher('reference_vis', Odometry, queue_size = 1) self.pub_odom = rospy.Publisher('odom_vis', Odometry, queue_size = 1) # Create subscribers rospy.Subscriber('pose_target', PoseStamped, self.pose_goal_cb, queue_size=1) rospy.Subscriber('odometry_target', Odometry, self.odometry_goal_cb, queue_size=1) rospy.Subscriber('odometry', Odometry, self.odometry_cb, queue_size=1) rospy.Subscriber('encoder/odom', Odometry, self.encoders_cb, queue_size=1) rospy.Subscriber('scan', LaserScan, self.pointcloud_cb, queue_size=1) rospy.Subscriber('joy_cmd', AckermannDriveStamped, self.joy_cmd_cb, queue_size = 1) rospy.Subscriber('heartbeat_base', Empty, self.heartbeat_cb, queue_size = 1) rospy.Subscriber('e_stop', Bool, self.e_stop_cb, queue_size = 1) # training timer rate = rospy.get_param('~rate', 30.0) self.dt = 1.0 / rate self.params["dt"] = self.dt self.timer = rospy.Timer(rospy.Duration(self.dt), self.timer_cb) self.odom_cb_called = False self.reset_ref = True self.cbf_pointcloud_list=[] # Initialize message variables. self.enable = rospy.get_param('~enable', True) if self.enable: self.start() else: self.stop() def start(self): """Turn on publisher.""" self.pub_debug = rospy.Publisher('~debug', DebugData, queue_size=10) def stop(self): """Turn off publisher.""" self.pub_debug.unregister() def pose_goal_cb(self,goal): x_des = 0 y_des = 0 desired_vel = 0 desired_heading = 0 if "base_link" in goal.header.frame_id: q = (goal.pose.orientation.x, goal.pose.orientation.y, goal.pose.orientation.z, goal.pose.orientation.w) euler = tr.euler_from_quaternion(q) rel_yaw = euler[2] q2 = (self.odom.pose.pose.orientation.x, self.odom.pose.pose.orientation.y, self.odom.pose.pose.orientation.z, self.odom.pose.pose.orientation.w) euler2 = tr.euler_from_quaternion(q2) curr_yaw = euler2[2] desired_heading = curr_yaw + rel_yaw dx = np.cos(curr_yaw) * goal.pose.position.x - np.sin(curr_yaw) * goal.pose.position.y dy = np.sin(curr_yaw) * goal.pose.position.x + np.cos(curr_yaw) * goal.pose.position.y x_des = self.odom.pose.pose.position.x + dx y_des = self.odom.pose.pose.position.y + dy else: # get current desired state q = (goal.pose.orientation.x, goal.pose.orientation.y, goal.pose.orientation.z, goal.pose.orientation.w) euler = tr.euler_from_quaternion(q) desired_heading = euler[2] x_des = goal.pose.position.x y_des = goal.pose.position.y # scale desired velocity if goal is a fixed position if self.kp_goal > 0: distance_from_goal = np.sqrt((goal.pose.position.x-self.odom.pose.pose.position.x)**2 + (goal.pose.position.y-self.odom.pose.pose.position.y)**2 ) desired_vel = np.minimum(self.desired_vel,distance_from_goal * self.kp_goal) else: desired_vel = self.desired_vel if self.params["reverse_velocity_goal"] and self.current_vel_body_x < 0: desired_vel = - np.abs(desired_vel) x_ref = np.array([[x_des,y_des,desired_heading,desired_vel]]).T self.z_ref = self.adaptive_clbf.dyn.convert_x_to_z(x_ref) self.z_ref_dot = np.zeros((5,1)) self.prev_goal_timestamp = goal.header.stamp def odometry_goal_cb(self,goal): dt = (goal.header.stamp - self.prev_goal_timestamp).to_sec() if dt < 0: rospy.logwarn("detected jump back in time! resetting prev_goal_timestamp.") self.prev_goal_timestamp = goal.header.stamp return if dt < self.dt / 4.0: rospy.logwarn("dt is too small! (%f) skipping this goal callback!", dt) return # get current desired state q = (goal.pose.pose.orientation.x, goal.pose.pose.orientation.y, goal.pose.pose.orientation.z, goal.pose.pose.orientation.w) euler = tr.euler_from_quaternion(q) desired_heading = euler[2] desired_vel = goal.twist.twist.linear.x x_ref = np.array([[goal.pose.pose.position.x,goal.pose.pose.position.y,desired_heading,desired_vel]]).T z_ref_new = self.adaptive_clbf.dyn.convert_x_to_z(x_ref) self.z_ref_dot = (z_ref_new - self.z_ref) / dt self.z_ref = z_ref_new self.prev_goal_timestamp = goal.header.stamp def timer_cb(self,_event): if self.params["model_train"]: if not self.sent_train_goal: self.train_start_time = rospy.get_rostime() print("sending training goal") self.adaptive_clbf.train_model_action_client.send_goal(self.adaptive_clbf.train_model_goal) self.sent_train_goal = True return # states 0 = PENDING, 1 = ACTIVE, 2 = PREEMPTED, 3 = SUCCEEDED state = self.adaptive_clbf.train_model_action_client.get_state() # print("State:",state) if self.sent_train_goal and (state == GoalStatus.PENDING or state == GoalStatus.ACTIVE): return elif state == GoalStatus.SUCCEEDED: result = self.adaptive_clbf.train_model_action_client.get_result() if hasattr(result, 'model_trained'): self.adaptive_clbf.model_trained = self.adaptive_clbf.train_model_action_client.get_result().model_trained else: self.adaptive_clbf.model_trained = False self.sent_train_goal = False end_time = rospy.get_rostime() # if self.params["verbose"]: rospy.logwarn(["training latency (ms): ", (end_time-self.train_start_time).to_sec() * 1000.0]) def joy_cmd_cb(self, ackermann_drive_msg): self.joy_cmd = ackermann_drive_msg if self.joy_cmd.drive.speed == 0: self.joy_cmd.drive.jerk = -100 self.joy_cmd_time = rospy.get_rostime() rospy.logwarn("Detected joystick override!") def heartbeat_cb(self, msg): self.heartbeat_time = rospy.get_rostime() def e_stop_cb(self, msg): if msg.data: self.e_stop_time = rospy.get_rostime() def encoders_cb(self,encoder_odom): self.encoder_odom = encoder_odom def pointcloud_cb(self,pointcloud): # get current state self.pointcloud_time = rospy.get_rostime() if not self.params["use_barrier_pointcloud"]: return q = (self.odom.pose.pose.orientation.x, self.odom.pose.pose.orientation.y, self.odom.pose.pose.orientation.z, self.odom.pose.pose.orientation.w) euler = tr.euler_from_quaternion(q) current_heading = euler[2] x_curr = self.odom.pose.pose.position.x y_curr = self.odom.pose.pose.position.y angles = np.arange(pointcloud.angle_min,pointcloud.angle_max-pointcloud.angle_increment,pointcloud.angle_increment) + current_heading ranges = np.array(pointcloud.ranges) angles = angles[np.isfinite(ranges)] ranges = ranges[np.isfinite(ranges)] x = np.cos(angles) * np.array(ranges) + x_curr y = np.sin(angles) * np.array(ranges) + y_curr # Downsample pointcloud barrier_max_distance = self.params["barrier_max_distance"] barrier_resolution = self.params["barrier_resolution"] if (x.size > 0): x_min = -barrier_max_distance x_max = barrier_max_distance y_min = -barrier_max_distance y_max = barrier_max_distance x_filtered = [x[0]] y_filtered = [y[0]] for i in range(1, len(x)): # Remove points that are too close to each other if np.sqrt((x[i] - x_filtered[-1])**2 + (y[i] - y_filtered[-1])**2) > barrier_resolution: # Remove points that are far away from the robot if (x[i] - x_curr < x_max) and (x[i] - x_curr > x_min) and (y[i] - y_curr < y_max) and (y[i] - y_curr > y_min) : x_filtered.append(x[i]) y_filtered.append(y[i]) x = np.array(x_filtered) y = np.array(y_filtered) # update barriers curr_vel = np.sqrt(self.odom.twist.twist.linear.x ** 2 + self.odom.twist.twist.linear.y ** 2) radius = curr_vel * self.params["barrier_radius_velocity_scale"] radius = np.maximum(radius,self.params["barrier_radius"]) self.adaptive_clbf.update_barrier_locations(x=x,y=y,radius=radius) def odometry_cb(self, odom): start_time = rospy.get_rostime() # TODO: theory - maybe dt of rostime and header does not match dt of actual odometry data. this might cause big problems. dt = (odom.header.stamp - self.prev_odom_timestamp).to_sec() if dt < 0: rospy.logwarn("detected jump back in time! resetting prev_odom_timestamp.") self.prev_odom_timestamp = self.odom.header.stamp self.timer = rospy.Timer(rospy.Duration(self.dt), self.timer_cb) self.sent_train_goal = False return if dt < self.dt: # rospy.logwarn("dt is too small! (%f) skipping this odometry callback!", dt) return # get current odom in base_link frame try: t = self.tf.getLatestCommonTime(self.odom_frame, self.base_link_frame) position, quaternion = self.tf.lookupTransform(self.odom_frame, self.base_link_frame, t) hdr = copy.copy(odom.header) hdr.frame_id = odom.child_frame_id camera_to_bl = self.tf.asMatrix(self.base_link_frame,hdr) except: raise return self.odom.header = odom.header self.odom.child_frame_id = self.base_link_frame self.odom.pose.pose.position.x = position[0] self.odom.pose.pose.position.y = position[1] self.odom.pose.pose.position.z = position[2] self.odom.pose.pose.orientation.x = quaternion[0] self.odom.pose.pose.orientation.y = quaternion[1] self.odom.pose.pose.orientation.z = quaternion[2] self.odom.pose.pose.orientation.w = quaternion[3] twist_rot = np.array([odom.twist.twist.angular.x,odom.twist.twist.angular.y,odom.twist.twist.angular.z]) twist_vel = np.array([odom.twist.twist.linear.x,odom.twist.twist.linear.y,odom.twist.twist.linear.z]) out_rot = np.matmul(camera_to_bl[0:3,0:3], twist_rot) out_vel = np.matmul(camera_to_bl[0:3,0:3], twist_vel) + np.cross(camera_to_bl[0:3,-1],out_rot) self.odom.twist.twist.linear.x = out_vel[0] self.odom.twist.twist.linear.y = out_vel[1] self.odom.twist.twist.linear.z = out_vel[2] self.odom.twist.twist.angular.x = out_rot[0] self.odom.twist.twist.angular.y = out_rot[1] self.odom.twist.twist.angular.z = out_rot[2] self.prev_odom_timestamp = odom.header.stamp if not self.enable: return # get current state and observations q = (self.odom.pose.pose.orientation.x, self.odom.pose.pose.orientation.y, self.odom.pose.pose.orientation.z, self.odom.pose.pose.orientation.w) euler = tr.euler_from_quaternion(q) current_roll = euler[0] current_pitch = euler[1] current_heading = euler[2] self.current_vel_body_x = np.cos(current_heading) * self.odom.twist.twist.linear.x + np.sin(current_heading) * self.odom.twist.twist.linear.y self.current_vel_body_y = -np.sin(current_heading) * self.odom.twist.twist.linear.x + np.cos(current_heading) * self.odom.twist.twist.linear.y # self.current_vel_body_x = self.odom.twist.twist.linear.x # self.current_vel_body_y = self.odom.twist.twist.linear.y self.x=np.array([[self.odom.pose.pose.position.x,self.odom.pose.pose.position.y,current_heading,self.current_vel_body_x]]).T self.obs = np.array([[current_heading, self.current_vel_body_y, self.odom.twist.twist.angular.z, self.u_prev[0], self.u_prev[1], current_roll, current_pitch # self.encoder_odom.twist.twist.linear.x, # self.encoder_odom.twist.twist.angular.z ]],dtype=np.float32).T self.z = self.adaptive_clbf.dyn.convert_x_to_z(self.x) add_data = self.params["add_data"] # don't add data if no motion if (self.current_vel_body_x**2 + self.current_vel_body_y**2) < 0.01: add_data = False # don't add data if moving backwards if self.current_vel_body_x < -0.1: add_data = False # dont' add first timestep of data if not self.odom_cb_called: add_data = False self.odom_cb_called = True # clear barriers if pointcloud callback is too long ago. if (rospy.get_rostime() - self.pointcloud_time).to_sec() > 1.0: rospy.logwarn("Clearing barriers because no points recieved!") self.adaptive_clbf.update_barrier_locations(x=np.array([]),y=np.array([]),radius=0) # get control! u = self.adaptive_clbf.get_control(self.z,self.z_ref,self.z_ref_dot,dt=dt,obs=self.obs,use_model=self.params["use_model"],add_data=add_data,check_model=self.params["check_model"],use_qp=self.params["use_qp"]) self.x_ref = self.adaptive_clbf.dyn.convert_z_to_x(self.z_ref) # nan check if np.isnan(u).any(): u = np.array([0.0,0.0]) self.u_prev = copy.copy(u) # make message u_msg = AckermannDriveStamped() u_msg.drive.steering_angle = u[0] if self.params["scale_acceleration"] == 0.0: # directly apply acclerations if u[1] > 0: u_msg.drive.acceleration = u[1] * self.params["acceleration_gain"] + self.params["acceleration_deadband"] else: u_msg.drive.acceleration = u[1] * self.params["acceleration_gain"] - self.params["acceleration_deadband"] else: # or, assume underlying velocity controller u_msg.drive.speed = self.current_vel_body_x + self.params["scale_acceleration"] * u[1] u_msg.header.stamp = self.odom.header.stamp if (rospy.get_rostime() - self.heartbeat_time).to_sec() > 2.0: # or (rospy.get_rostime() - self.e_stop_time).to_sec() < 1.0: rospy.logwarn("Publishing zero command, heartbeat lost or e_stopped.") zero_msg = AckermannDriveStamped() zero_msg.drive.jerk = -100.0 zero_msg.header.stamp = self.odom.header.stamp self.pub_control.publish(zero_msg) elif (rospy.get_rostime() - self.joy_cmd_time).to_sec() < 0.5: self.joy_cmd.header.stamp = self.odom.header.stamp self.pub_control.publish(self.joy_cmd) rospy.logwarn("Publishing joystick override command.") else: self.pub_control.publish(u_msg) end_time = rospy.get_rostime() # publish reference pose for visualization x_ref_msg = Odometry() x_ref_msg.header.frame_id = self.odom_frame x_ref_msg.header.stamp = self.odom.header.stamp x_ref_msg.pose.pose.position.x = self.x_ref[0] x_ref_msg.pose.pose.position.y = self.x_ref[1] x_ref_msg.pose.pose.position.z = 0.0 x_ref_msg.pose.pose.orientation.x = 0.0 x_ref_msg.pose.pose.orientation.y = 0.0 x_ref_msg.pose.pose.orientation.z = np.sin(self.x_ref[2]/2.0) x_ref_msg.pose.pose.orientation.w = np.cos(self.x_ref[2]/2.0) # cov = self.adaptive_clbf.sigma # x_ref_msg.pose.covariance[0] = cov[0] # x_ref_msg.pose.covariance[7] = cov[1] x_ref_msg.pose.covariance[0] = self.adaptive_clbf.predict_error x_ref_msg.pose.covariance[7] = self.adaptive_clbf.predict_error self.pub_ref.publish(x_ref_msg) self.pub_odom.publish(self.odom) cb_latency = (end_time-start_time).to_sec() * 1000.0 if self.params["verbose"]: rospy.logwarn(["callback latency (ms): ", cb_latency]) # publish debug data debug_msg = DebugData() debug_msg.header.stamp = self.odom.header.stamp debug_msg.latency = cb_latency debug_msg.dt = dt debug_msg.heading = current_heading debug_msg.vel_x_body = self.current_vel_body_x debug_msg.vel_y_body = self.current_vel_body_y debug_msg.odom = self.odom debug_msg.z = self.adaptive_clbf.debug["z"] debug_msg.z_ref = self.adaptive_clbf.debug["z_ref"] debug_msg.z_dot = self.adaptive_clbf.debug["z_dot"] debug_msg.y_out = self.adaptive_clbf.debug["y_out"] debug_msg.mu_rm = self.adaptive_clbf.debug["mu_rm"] debug_msg.mu_pd = self.adaptive_clbf.debug["mu_pd"] debug_msg.mu_ad = self.adaptive_clbf.debug["mu_ad"] debug_msg.rho = self.adaptive_clbf.debug["rho"] debug_msg.mu_qp = self.adaptive_clbf.debug["mu_qp"] debug_msg.mu_model = self.adaptive_clbf.debug["mu_model"] debug_msg.mu = self.adaptive_clbf.debug["mu"] debug_msg.u_new = self.adaptive_clbf.debug["u_new"] debug_msg.u_unsat = self.adaptive_clbf.debug["u_unsat"] debug_msg.trueDelta = self.adaptive_clbf.debug["trueDelta"] debug_msg.true_predict_error = self.adaptive_clbf.debug["true_predict_error"] debug_msg.mDelta = self.adaptive_clbf.debug["mDelta"] debug_msg.sigDelta = self.adaptive_clbf.debug["sigDelta"] self.pub_debug.publish(debug_msg) def reconfigure_cb(self, config): """Create a callback function for the dynamic reconfigure client.""" # Fill in local variables with values received from dynamic reconfigure # clients (typically the GUI). self.params["vehicle_length"] = config["vehicle_length"] self.params["steering_limit"] = config["steering_limit"] self.params["scale_acceleration"] = config["scale_acceleration"] self.params["acceleration_gain"] = config["acceleration_gain"] self.params["acceleration_deadband"] = config["acceleration_deadband"] self.params["max_accel"] = config["max_accel"] self.params["min_accel"] = config["min_accel"] self.params["kp_z"] = config["kp_z"] self.params["kd_z"] = config["kd_z"] self.params["clf_epsilon"] = config["clf_epsilon"] self.params["learning_verbose"] = config["learning_verbose"] self.params["measurement_noise"] = config["measurement_noise"] self.params["N_data"] = config["N_data"] self.params["max_error"] = config["max_error"] self.params["use_qp"] = config["use_qp"] self.params["qp_u_cost"] = config["qp_u_cost"] self.params["qp_u_prev_cost"] = config["qp_u_prev_cost"] self.params["qp_p1_cost"] = config["qp_p1_cost"] self.params["qp_p2_cost"] = config["qp_p2_cost"] self.params["qp_ksig"] = config["qp_ksig"] self.params["qp_max_var"] = config["qp_max_var"] self.params["qp_verbose"] = config["qp_verbose"] self.params["max_velocity"] = config["max_velocity"] self.params["min_velocity"] = config["min_velocity"] self.params["barrier_vel_gamma"] = config["barrier_vel_gamma"] self.params["use_barrier_vel"] = config["use_barrier_vel"] self.params["use_barrier_pointcloud"] = config["use_barrier_pointcloud"] self.params["barrier_radius"] = config["barrier_radius"] self.params["barrier_radius_velocity_scale"] = config["barrier_radius_velocity_scale"] self.params["barrier_pc_gamma_p"] = config["barrier_pc_gamma_p"] self.params["barrier_pc_gamma"] = config["barrier_pc_gamma"] self.params["barrier_max_distance"] = config["barrier_max_distance"] self.params["barrier_resolution"] = config["barrier_resolution"] self.params["verbose"] = config["verbose"] self.params["use_model"] = config["use_model"] self.params["model_train"] = config["model_train"] self.params["add_data"] = config["add_data"] self.params["check_model"] = config["check_model"] self.params["reverse_velocity_goal"] = config["reverse_velocity_goal"] self.kp_goal = config["kp_goal"] self.desired_vel = config["desired_vel"] rate = config["rate"] self.dt = 1.0 / rate self.params["dt"] = self.dt self.adaptive_clbf.update_params(self.params) # Check to see if node should be started or stopped. if self.enable != config["enable"]: if config["enable"]: self.start() else: self.stop() self.enable = config["enable"]
class PointCloudFilterNode(object): def __init__(self): rospy.init_node('pcl_filter') self.tf_listener = TransformListener() rospy.Subscriber( "point_cloud_obstacles", PointCloud2, lambda point_cloud: self.obstacle_cloud_handler(point_cloud)) rospy.Subscriber("static", OccupancyGrid, lambda grid: self.map_handler(grid)) self.static_point_cloud_publisher = rospy.Publisher( 'point_cloud_static', PointCloud2, queue_size=1) self.dynamic_point_cloud_publisher = rospy.Publisher( 'point_cloud_dynamic', PointCloud2, queue_size=1) self.map = None rospy.spin() def point_cloud_to_np_array(self, point_cloud): # assuming 4 float32-s for a point a = np.fromstring(point_cloud.data, dtype=np.float32).reshape((-1, 4)) a[:3] = 1.0 # use homogeneous coords for matrix transformation return a def transform_cloud(self, point_cloud, msg_header, target_frame): self.tf_listener.waitForTransform(msg_header.frame_id, target_frame, msg_header.stamp, rospy.Duration(4.0)) mat44 = self.tf_listener.asMatrix(target_frame, msg_header) # assuming point_cloud is a Nx4 matrix return np.dot(point_cloud, mat44.T) def obstacle_cloud_handler(self, point_cloud_msg): if self.map is None: print("Cannot filter point cloud, no map received yet", file=sys.stderr) return point_cloud = self.point_cloud_to_np_array(point_cloud_msg) point_cloud = self.transform_cloud(point_cloud, point_cloud_msg.header, self.map.header.frame_id) grid = LocalOccupancyGrid(self.map, LocalOccupancyGridParams(rospy, '~grid_')) cols = grid.get_col_i(point_cloud[:, 0:3]) rows = grid.get_row_i(point_cloud[:, 0:3]) valid_pos = np.logical_and( np.logical_and(cols >= 0, cols < grid.cols()), np.logical_and(rows >= 0, rows < grid.rows()), ) obstacle_cloud = point_cloud[valid_pos] cols = grid.get_col_i(obstacle_cloud[:, 0:3]) rows = grid.get_row_i(obstacle_cloud[:, 0:3]) threshold = STATIC_THRESHOLD obstacle_static_cloud = obstacle_cloud[( 1 - grid.get_grid())[rows, cols] > threshold, :] obstacle_dynamic_cloud = obstacle_cloud[( 1 - grid.get_grid())[rows, cols] < threshold, :] now = rospy.Time.now() print("Publishing filtered point clouds, ({},{})".format( obstacle_static_cloud.shape[0], obstacle_dynamic_cloud.shape[0])) publish_point_cloud(self.static_point_cloud_publisher, obstacle_static_cloud, self.map.header.frame_id, now) publish_point_cloud(self.dynamic_point_cloud_publisher, obstacle_dynamic_cloud, self.map.header.frame_id, now) def map_handler(self, map): self.map = map print("Static map received")
class CameraController: def __init__(self, horizontal_fov=110, vertical_fov=88, camera_frame="camera_link", world_frame="map"): self.iwt_pub = rospy.Publisher("iwt_camera_stream", ImageWithTransform, queue_size=1) self.image_pub = rospy.Publisher("image_color", Image, queue_size=1) self.info_pub = rospy.Publisher("camera_info", CameraInfo, queue_size=10) self.tfl = TransformListener() self.bridge = CvBridge() self.resolution = (640, 480) self.power_on = False self.camera_info = None self.camera_frame = camera_frame self.world_frame = world_frame self.set_camera_info(horizontal_fov, vertical_fov) self.buffer_img = None self.anchor_time = rospy.Time(0) self.matrix4x4 = np.eye(4) self.has_sent_stable = False self.skip_count = 5 def set_camera_info(self, horizontal_fov, vertical_fov): rect_matrix = [0.0 for i in range(9)] f_x = 1 / tan(radians(horizontal_fov) * 0.5) f_y = 1 / tan(radians(vertical_fov) * 0.5) c_x, c_y = 0.0, 0.0 intr_matrix = [f_x, 0.0, c_x, 0.0, f_y, c_y, 0.0, 0.0, 1.0] proj_matrix = [ f_x, 0.0, c_x, 0.0, 0.0, f_y, c_y, 0.0, 0.0, 0.0, 1.0, 0.0 ] self.camera_info = CameraInfo(height=self.resolution[1], width=self.resolution[1], K=intr_matrix, R=rect_matrix, P=proj_matrix) self.camera_info.header.frame_id = self.camera_frame # uses endless iterator to generate stream and process, which allows # us to use picamera continuous and sequence modes yet still publish # each frame def _stream_to_ros(self, camera, size): with piCamArray.PiRGBArray(camera, size=size) as stream: while self.power_on and not rospy.is_shutdown(): yield stream snap_time = rospy.Time.now( ) # - rospy.Duration(0.025) # delay to handle sync error image = self.undistort( stream.array) if UNDISTORT else stream.array self.camera_info.header.stamp = snap_time self.publish_standard_camera(image, snap_time) # image with camera world transform for localization if self.skip_count < 1: self.publish_image_with_stable_transform(image, snap_time) self.skip_count = 5 self.skip_count -= 1 stream.truncate(0) # saves 100 images to file, used for calibration routine def _stream_raw_to_file(self, camera, size): with piCamArray.PiRGBArray(camera, size=size) as stream: frame_num = 0 while frame_num < 100: yield stream print("Frame", frame_num) snap_time = rospy.Time.now( ) # - rospy.Duration(0.025) # delay to handle sync error image = stream.array self.camera_info.header.stamp = snap_time self.publish_standard_camera(image, snap_time) frame_num += 1 self.output = io.open( 'calibration/calibrate%02d.jpg' % frame_num, 'wb') self.output.write(cv2.imencode(".jpg", image)[1]) self.output.close() stream.truncate(0) self.turn_off() # publishes on ROS the standard image as per any camera controller def publish_standard_camera(self, image, snap_time): self.info_pub.publish(self.camera_info) # standard camera image ros broadcast rosmsg = self.bridge.cv2_to_imgmsg(image, "bgr8") rosmsg.header.stamp = snap_time rosmsg.header.frame_id = self.camera_frame self.image_pub.publish(rosmsg) # publishes on ROS the transform stamped image, noting if stable or not def publish_image_with_stable_transform(self, image, snap_time): try: self.camera_info.header.stamp = rospy.Time(0) matrix4x4 = self.tfl.asMatrix(self.world_frame, self.camera_info.header) is_similar = is_similar_transform(matrix4x4, self.matrix4x4) is_stable_for_time = is_similar and ( snap_time - self.anchor_time).to_sec() > SLAM_LAG if not is_similar: self.matrix4x4 = matrix4x4 self.anchor_time = snap_time self.has_sent_stable = False self.camera_info.header.stamp = snap_time cmp_img = CompressedImage( data=np.array(cv2.imencode(".jpg", image)[1]).tostring()) cmp_img.header.stamp = self.anchor_time msg = ImageWithTransform(compressedImage=cmp_img, cameraInfo=self.camera_info, flat_transform=np.reshape(matrix4x4, 16)) if not self.has_sent_stable and is_stable_for_time: msg.is_stable = True self.has_sent_stable = True else: msg.is_stable = False self.iwt_pub.publish(msg) except tfException: print("Camera can not find transform for ", self.world_frame, " or ", self.camera_info.header.frame_id) @staticmethod def make_camera_obj(framerate=30): camera = PiCamera(sensor_mode=4, resolution='VGA', framerate=framerate) camera.rotation = ROTATE camera.start_preview() sleep(2) # allow camera to warm up camera.stop_preview() return camera def turn_on(self): self.power_on = True with self.make_camera_obj(framerate=30) as camera: while self.power_on and not rospy.is_shutdown(): camera.capture_sequence(self._stream_to_ros( camera, self.resolution), format='bgr', use_video_port=True) # tells camera to run the calibration routine def calibrate(self): self.power_on = True with self.make_camera_obj(framerate=5) as camera: for count in range(5): print("starting in " + str(5 - count) + "...") sleep(1) while self.power_on and not rospy.is_shutdown(): camera.capture_sequence(self._stream_raw_to_file( camera, self.resolution), format='bgr', use_video_port=True) def turn_off(self): self.power_on = False #applies the distortion matrix through opencv to rectify fisheye images def undistort(self, img): h, w = img.shape[:2] map1, map2 = cv2.fisheye.initUndistortRectifyMap( K, D, np.eye(3), K, DIM, cv2.CV_16SC2) return cv2.remap(img, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT)