diff --git a/crazyflie/scripts/simple_mapper_multiranger.py b/crazyflie/scripts/simple_mapper_multiranger.py index afd4ef851..096b6126a 100755 --- a/crazyflie/scripts/simple_mapper_multiranger.py +++ b/crazyflie/scripts/simple_mapper_multiranger.py @@ -27,16 +27,19 @@ GLOBAL_SIZE_Y = 20.0 MAP_RES = 0.1 + class SimpleMapperMultiranger(Node): def __init__(self): super().__init__('simple_mapper_multiranger') self.declare_parameter('robot_prefix', '/cf231') - robot_prefix = self.get_parameter('robot_prefix').value - - self.odom_subscriber = self.create_subscription(Odometry, robot_prefix + '/odom', self.odom_subscribe_callback, 10) - self.ranges_subscriber = self.create_subscription(LaserScan, robot_prefix + '/scan', self.scan_subscribe_callback, 10) - self.position = [0.0, 0.0, 0.0] - self.angles = [0.0, 0.0, 0.0] + robot_prefix = self.get_parameter('robot_prefix').value + + self.odom_subscriber = self.create_subscription( + Odometry, robot_prefix + '/odom', self.odom_subscribe_callback, 10) + self.ranges_subscriber = self.create_subscription( + LaserScan, robot_prefix + '/scan', self.scan_subscribe_callback, 10) + self.position = [0.0, 0.0, 0.0] + self.angles = [0.0, 0.0, 0.0] self.ranges = [0.0, 0.0, 0.0, 0.0] self.range_max = 3.5 @@ -52,13 +55,14 @@ def __init__(self): self.position_update = False - self.map = [-1] * int(GLOBAL_SIZE_X / MAP_RES) * int(GLOBAL_SIZE_Y / MAP_RES) + self.map = [-1] * int(GLOBAL_SIZE_X / MAP_RES) * \ + int(GLOBAL_SIZE_Y / MAP_RES) self.map_publisher = self.create_publisher(OccupancyGrid, robot_prefix + '/map', - qos_profile=QoSProfile( depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL, history=HistoryPolicy.KEEP_LAST,)) + qos_profile=QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL, history=HistoryPolicy.KEEP_LAST,)) - self.get_logger().info(f"Simple mapper set for crazyflie "+ robot_prefix + + self.get_logger().info(f"Simple mapper set for crazyflie " + robot_prefix + f" using the odom and scan topic") - + def odom_subscribe_callback(self, msg): self.position[0] = msg.pose.pose.position.x self.position[1] = msg.pose.pose.position.y @@ -81,15 +85,17 @@ def scan_subscribe_callback(self, msg): if self.position_update is False: return for i in range(len(data)): - point_x = int((data[i][0] - GLOBAL_SIZE_X / 2.0)/ MAP_RES ) - point_y = int((data[i][1] - GLOBAL_SIZE_Y / 2.0)/ MAP_RES ) + point_x = int((data[i][0] - GLOBAL_SIZE_X / 2.0) / MAP_RES) + point_y = int((data[i][1] - GLOBAL_SIZE_Y / 2.0) / MAP_RES) points_x.append(point_x) points_y.append(point_y) - position_x_map = int((self.position[0] - GLOBAL_SIZE_X / 2.0)/ MAP_RES ) - position_y_map = int((self.position[1] - GLOBAL_SIZE_Y / 2.0)/ MAP_RES ) + position_x_map = int( + (self.position[0] - GLOBAL_SIZE_X / 2.0) / MAP_RES) + position_y_map = int( + (self.position[1] - GLOBAL_SIZE_Y / 2.0) / MAP_RES) for line_x, line_y in bresenham(position_x_map, position_y_map, point_x, point_y): - self.map[line_y * int(GLOBAL_SIZE_X / MAP_RES) + line_x] = 0 - self.map[point_y * int(GLOBAL_SIZE_X / MAP_RES) + point_x] = 100 + self.map[line_y * int(GLOBAL_SIZE_X / MAP_RES) + line_x] = 0 + self.map[point_y * int(GLOBAL_SIZE_X / MAP_RES) + point_x] = 100 msg = OccupancyGrid() msg.header.stamp = self.get_clock().now().to_msg() @@ -132,33 +138,34 @@ def rotate_and_create_points(self): return data def rot(self, roll, pitch, yaw, origin, point): - cosr = math.cos((roll)) - cosp = math.cos((pitch)) - cosy = math.cos((yaw)) + cosr = math.cos((roll)) + cosp = math.cos((pitch)) + cosy = math.cos((yaw)) + + sinr = math.sin((roll)) + sinp = math.sin((pitch)) + siny = math.sin((yaw)) - sinr = math.sin((roll)) - sinp = math.sin((pitch)) - siny = math.sin((yaw)) + roty = np.array([[cosy, -siny, 0], + [siny, cosy, 0], + [0, 0, 1]]) - roty = np.array([[cosy, -siny, 0], - [siny, cosy, 0], - [0, 0, 1]]) + rotp = np.array([[cosp, 0, sinp], + [0, 1, 0], + [-sinp, 0, cosp]]) - rotp = np.array([[cosp, 0, sinp], - [0, 1, 0], - [-sinp, 0, cosp]]) + rotr = np.array([[1, 0, 0], + [0, cosr, -sinr], + [0, sinr, cosr]]) - rotr = np.array([[1, 0, 0], - [0, cosr, -sinr], - [0, sinr, cosr]]) + rotFirst = np.dot(rotr, rotp) - rotFirst = np.dot(rotr, rotp) + rot = np.array(np.dot(rotFirst, roty)) - rot = np.array(np.dot(rotFirst, roty)) + tmp = np.subtract(point, origin) + tmp2 = np.dot(rot, tmp) + return np.add(tmp2, origin) - tmp = np.subtract(point, origin) - tmp2 = np.dot(rot, tmp) - return np.add(tmp2, origin) def main(args=None): @@ -170,4 +177,4 @@ def main(args=None): if __name__ == '__main__': - main() \ No newline at end of file + main()