コード例 #1
0
    def __init__(self):
        rospy.init_node("uw_teleop")

        self.vel_pub1 = rospy.Publisher('/dataNavigator1',
                                        Odometry,
                                        queue_size=10)
        self.vel_pub2 = rospy.Publisher('/dataNavigator2',
                                        Odometry,
                                        queue_size=10)

        self.lidar_pub = rospy.Publisher('/dataNavigatorlidar',
                                         Odometry,
                                         queue_size=10)
        self.rate = rospy.Rate(20)
        self.joy_sub = rospy.Subscriber("/joy", Joy, self.joy_msg_callback)

        self.bf1_odom_sub = rospy.Subscriber("/uwsim/BlueFox1Odom", Odometry,
                                             self.pose1_callback)
        self.bf2_odom_sub = rospy.Subscriber("/uwsim/BlueFox2Odom", Odometry,
                                             self.pose2_callback)

        self.bf1_laser_sub = rospy.Subscriber("/BlueFox1/multibeam11",
                                              LaserScan, self.laser1_callback)
        self.bf1_lidar_sub = rospy.Subscriber("/lidar/multibeam", LaserScan,
                                              self.lidar_callback)
        self.bf2_laser_sub = rospy.Subscriber("/BlueFox2/multibeam", LaserScan,
                                              self.laser2_callback)

        # messages required by /ndt_mapping
        self.lidar_pc2_pub = rospy.Publisher('points_raw',
                                             PointCloud2,
                                             queue_size=10)
        self.imu_pub = rospy.Publisher("/imu_raw", Imu, queue_size=10)
        self.vehicle_odom_pub = rospy.Publisher('/odom_pose',
                                                Odometry,
                                                queue_size=10)

        self.joy_data = Joy()
        self.vel_cmd1 = Odometry()
        self.vel_cmd2 = Odometry()

        self.bf1_pose = None
        self.bf2_pose = None
        self.odom_hz = 10

        self.bf1_vel = None
        self.bf2_vel = None
        self.bf1_laser = LaserScan()
        self.bf2_laser = LaserScan()

        self.joy_button = None

        self.lidar_counter = 0  # this is a hack to make lidar spin

        self.laser_projection = LaserProjection()

        self.velodyne_mapped_laserscan = LaserScan()

        self.tf_buffer = tf2_ros.Buffer()
        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
コード例 #2
0
    def __init__(self, **kwargs):
        self.robot_frame = rospy.get_param('~robot_frame', 'load/base_link')
        self.laser_frame = rospy.get_param('~laser_frame', 'load/laser')
        self.footprint = rospy.get_param(
            '~footprint',
            [[-0.33, 0.33], [0.33, 0.33], [0.33, -0.33], [-0.33, -0.33]])

        # class variables
        self.tf_listener = tf.TransformListener()
        self.laser_proj = LaserProjection()
        self.only_use_half = kwargs.get('only_use_half', False)
        self.use_front_half = True
        footprint_padding = kwargs.get('footprint_padding', 0.1)
        self.set_footprint_padding(footprint_padding)
        self.cloud = None
        self.debug = kwargs.get('debug', False)

        # publisher
        self.pc_pub = rospy.Publisher('~pc_debug', PointCloud2, queue_size=1)
        self.footprint_pub = rospy.Publisher('~footprint_debug',
                                             PolygonStamped,
                                             queue_size=1)

        # subscribers
        laser_sub = rospy.Subscriber('~laser', LaserScan, self.laser_cb)

        rospy.sleep(0.5)
        if self.debug:
            self.pub_debug_footprint()
            rospy.sleep(0.2)
コード例 #3
0
    def __init__(self):
        rospy.init_node('map_to_pointcloud')
        
        self.br = tf.TransformBroadcaster()
        self.transform = TransformStamped()
        self.laser_footprint_transform = TransformStamped()
        self.tf_listener = tf.TransformListener()
        self.ts_old = rospy.Time.now()
        self.index_old = 0
        

        self.tf_listener.waitForTransform("/pr1/base_footprint", "pr1/left_laser_link", rospy.Time(0), rospy.Duration(1))
        (self.trans, self.rot) = self.tf_listener.lookupTransform("/pr1/base_footprint", "pr1/left_laser_link",rospy.Time(0))
        self.laser_footprint_transform.transform.translation.x = self.trans[0]
        self.laser_footprint_transform.transform.translation.y = self.trans[1]
        self.laser_footprint_transform.transform.rotation.x = self.rot[0]
        self.laser_footprint_transform.transform.rotation.y = self.rot[1]
        self.laser_footprint_transform.transform.rotation.z = self.rot[2]
        self.laser_footprint_transform.transform.rotation.w = self.rot[3]

        #rospy.Subscriber("/pr1/amcl_pose", PoseWithCovarianceStamped, self.robot_pose_cb)
        rospy.Subscriber("map", OccupancyGrid, self.map_cb)
        rospy.sleep(1)
        self.laser_projection = LaserProjection()
        rospy.Subscriber("/pr1/l_scan", LaserScan, self.laser_cb)
        self.pc2_pub = rospy.Publisher("pointcloud", pc2, queue_size=1)
        
        
        rospy.spin()
コード例 #4
0
 def __init__(self):
     self.laserProj = LaserProjection()
     self.pcPub = rospy.Publisher("/laserPointCloud", pc, queue_size=1)
     self.laserSub = rospy.Subscriber("/scan",
                                      LaserScan,
                                      self.laserCallback,
                                      queue_size=1)
コード例 #5
0
 def __init__(self):
     self.laserProj = LaserProjection()
     self.pcPub = rospy.Publisher('Pointy_Cloud',
                                  PointCloud2,
                                  queue_size=10)
     self.laserSub = rospy.Subscriber("lidar", LaserScan,
                                      self.laserCallback)
コード例 #6
0
    def __init__(self):
        self.global_x = 0
        self.global_y = 0
        self.global_yaw = 0
        self.global_ang_z = 0
        self.RESOLUTION = 0.05
        self.width = int(20 / self.RESOLUTION)
        self.height = int(20 / self.RESOLUTION)
        self.robot_in_grid = [0, 0]

        self.full_scan = []
        self.full_scan.append((0, 0, 0, 0, 0))
        self.scanned_map = np.zeros(
            (self.width, self.height)
        )  # initialize map to zero. Each cell represent a RESOLUTION squared area
        self.occupancy_grid = np.ones(
            (self.width, self.height), dtype=np.int) * -1

        self.laserProj = LaserProjection()
        self.pcPub = rospy.Publisher("/laserPointCloud",
                                     PointCloud2,
                                     queue_size=1)
        self.laserSub = rospy.Subscriber("/scan", LaserScan,
                                         self.laserCallback)
        self.laserSub = rospy.Subscriber("/odom", Odometry,
                                         self.positionCallback)

        self.occup_grid_pub = rospy.Publisher("/my_map",
                                              OccupancyGrid,
                                              queue_size=1)
コード例 #7
0
 def __init__(self):
     self.laserProj = LaserProjection()
     self.pcPub = rospy.Publisher("/prometheus/sensors/pcl2",
                                  pc2,
                                  queue_size=1)
     self.laserSub = rospy.Subscriber("/prometheus/sensors/2Dlidar_scan",
                                      LaserScan, self.laserCallback)
コード例 #8
0
    def __init__(self):

        # Variables
        self.laserProj = LaserProjection()
        self.isWater = False

        # Publishers
        self.pub = rospy.Publisher("/sonar_lidar_state",
                                   LaserScanFiltered,
                                   queue_size=10)
        self.pcWaterPub = rospy.Publisher("/water_pc", pc2, queue_size=1)

        # Subscribers
        rospy.Subscriber("/scan", LaserScan, self.callback_get_lidar_scan)
        rospy.Subscriber("/sonar", Range, self.callback_get_sonar_scan)

        # Services
        self.service_set_isWater = rospy.Service(
            'is_water', SetBool, self.callback_isWater
        )  # Toggle for isWater when water is classified

        # Defining variables:
        self.scanFiltered = LaserScanFiltered()
        self.scanFiltered.laser_range = 0.0
        self.scanFiltered.laser_intensity = 0.0
        self.scanFiltered.sonar_range = 0.0
        self.sensor_offset = 13.5

        rospy.spin()
コード例 #9
0
 def __init__(self,state):
     rospy.loginfo("Scan done")
     self.state = state
     self.done = False
     self.laser_msg = rospy.Subscriber("/scan", LaserScan, self.update_laser)
     self.lp = LaserProjection()
     pc = self.lp.projectLaser(scan)
     pc.rangemin = targetdistance
コード例 #10
0
    def __init__(self, verbose=False):
        self.rangeData = LaserScan()
        self.scan_sub = rospy.Subscriber("/base_scan", LaserScan,
                                         self.callback)
        self.listener = tf.TransformListener()
        self.laser_projector = LaserProjection()
        self.pc = []
        self.leg1 = []
        self.leg2 = []
        self.br = tf.TransformBroadcaster()
        self.rate = rospy.Rate(20.0)
        self.calibrated = False
        self.reCalibration = False
        self.priorOri_in_base_laser_link = []  # in base_laser_link frame
        self.priorLeft_in_base_laser_link = []  # in base_laser_link frame
        self.priorRight_in_base_laser_link = []  # in base_laser_link frame
        self.odomL = []  # in odom_combined frame
        self.odomR = []  # in odom_combined frame
        self.priorAvailable = False
        self.newObsWeight = 0.2
        self.offsetXY = [-0.044, 0]
        self.binOffset = 0.02
        self.pubShelfSep = rospy.Publisher('pubShelfSep', PoseStamped)
        self.reCalibrationCount = 4
        self.tolerance = 0.1
        self.updateRounds = 100
        self.asyncRate = 20
        self.limitInitX = True
        self.xLimit = 0.1
        self.emergencyThreshold = 30 * 20
        self.emergencyTimeWindow = 60

        while not rospy.is_shutdown():
            try:
                self.offsetXY = rospy.get_param('/base_scan/offset')
                break
            except:
                rospy.loginfo(
                    '[shelf publisher]: waiting for base scan offset param')
                rospy.sleep(random.uniform(0, 1))
                continue

        self.start_srv = rospy.Service(rospy.get_name() + '/start', Empty,
                                       self.start_srv_callback)
        self._start = False

        # backup human supervised info for emergency
        rp = rospkg.RosPack()
        try:
            dict_fp = rp.get_path('amazon_challenge_grasping')

        except:
            rospy.logerr('[shelf_publisher]: emergency file not found!')
            sys.exit(1)
        self.fileName = dict_fp + '/config/shelf_emergency.dat'
        self.emergency = {}
コード例 #11
0
ファイル: laser2pc.py プロジェクト: HyeonJaeGil/icra20
 def __init__(self):
     self.laserProj = LaserProjection()
     #        self.inMsg = rospy.get_param('~inMsg')
     #        self.outMsg = rospy.get_param('~outMsg')
     self.inMsg = '/scan'
     self.outMsg = '/converted_pc'
     self.pcPub = rospy.Publisher(self.outMsg, pc2, queue_size=1)
     self.laserSub = rospy.Subscriber(self.inMsg, LaserScan,
                                      self.laserCallback)
     self.count = 0
コード例 #12
0
    def __init__(self):

        self.laserProj = LaserProjection()

        #self.pub = rospy.Publisher("/own/true/sonar_PC2",PointCloud2, queue_size=1)
        #self.pub1 = rospy.Publisher("/own/simulated/sonar_PC2",PointCloud2, queue_size=1)
        self.pub2 = rospy.Publisher("/sonar/PC2", PointCloud2, queue_size=1)

        #self.sub = rospy.Subscriber("/desistek_saga/sonar",LaserScan, self.callback)
        #self.sub1 = rospy.Subscriber("/own/simulated/sonar_LS",LaserScan, self.callback1)
        self.sub2 = rospy.Subscriber("/sonar/LS", LaserScan, self.callback2)
コード例 #13
0
    def __init__(self, verbose=False):
        self.rangeData = LaserScan()
        self.scan_sub = rospy.Subscriber("/scan", LaserScan,
                                         self.callback_laser)
        rospy.sleep(1)
        self.listener = tf.TransformListener()
        self.laser_projector = LaserProjection()
        self.pc = []
        self.leg1 = []
        self.leg2 = []
        self.br = tf.TransformBroadcaster()
        self.rate = rospy.Rate(10.0)
        self.rate_measurements = rospy.Rate(0.5)
        self.priorOri_in_base = []
        self.priorLeft_in_base = []
        self.priorRight_in_base = []
        self.odomL = []
        self.odomR = []
        self.priorAvailable = False
        self.newObsWeight = 0.2
        self.offsetXY = [0.0, 0.0]
        self.binOffset = 0.02
        #self.pubShelfSep = rospy.Publisher('pubShelfSep', PoseStamped, queue_size=10)
        self.reCalibrationCount = 4
        self.tolerance = 0.1
        self.updateRounds = 100
        self.asyncRate = 20
        self.limitInitX = True
        self.xLimit = 0.1
        self.rp = rospkg.RosPack()
        self.writeFile = False
        self.calculateTF = False
        self.sendTF = False
        self._ready = False
        self.radius_th_leg = 0.1
        self.n_samples = 5
        self.sleep_between_scans = 2

        while not rospy.is_shutdown():
            try:
                self._shelfHeight = rospy.get_param(
                    "/apc/shelf_calibration/height")
                self._shelfRoll = rospy.get_param(
                    "/apc/shelf_calibration/roll")
                self._shelfPitch = rospy.get_param(
                    "/apc/shelf_calibration/pitch")
                break
            except:
                rospy.sleep(1.0)
                continue

        self.start_srv = rospy.Service(rospy.get_name() + '/calibrate', Empty,
                                       self.start_srv_callback)
コード例 #14
0
 def readscan(self):
     laser_projector = LaserProjection()
     for topic, msg, time_stamp in self.bag.read_messages(
             topics=[self.scan_topic]):
         cloud = laser_projector.projectLaser(msg)
         frame_points = np.zeros([0, 2])
         for p in pc2.read_points(cloud,
                                  field_names=("x", "y", "z"),
                                  skip_nans=True):
             p2d = np.array([p[0], p[1]])
             frame_points = np.vstack([frame_points, p2d])
         self.points.append([time_stamp, frame_points])
コード例 #15
0
    def __init__(self):
        self.tfr = TransformerROS()

        self.projector = LaserProjection()

        self.outputTopic = rospy.get_param('output_topic', 'cloud')
        self.inputTopic = rospy.get_param('input_topic', 'scan')
        self.outputFrame = rospy.get_param('output_frame', 'base_link')

        self.publisher = rospy.Publisher(self.outputTopic,
                                         PointCloud2,
                                         queue_size=10)
        self.subscriber = rospy.Subscriber(self.inputTopic, LaserScan,
                                           self.laser_callback)
コード例 #16
0
    def __init__(self,
                 slice_size=SLICE_SIZE_DEFAULT,
                 slice_offset=SLICE_OFFSET_DEFAULT,
                 max_dist_mult=MAX_DIST_MULT_DEFAULT,
                 publish_rate=PUBLISH_RATE_DEFAULT,
                 publish_pc=PUBLISH_PC_DEFAULT,
                 scan_topic=SCAN_TOPIC_DEFAULT,
                 contour_topic=CONTOUR_TOPIC_DEFAULT,
                 centroid_topic=CENTROID_TOPIC_DEFAULT,
                 pc_topic=PC_TOPIC_DEFAULT):
        self.__slice_size = slice_size
        self.__slice_offset = slice_offset
        self.__max_dist_mult = max_dist_mult
        self.__publish_point_cloud = publish_pc

        self.__rate = rospy.Rate(publish_rate)
        self.__laser_projector = LaserProjection()
        self.__vals_lock = Lock()
        self.__curr_msg = None
        self.__data_available = False
        self.__stopped = False

        # Create Slices once and reset them on each iteration
        self.__slices = [
            Slice(v, v + self.__slice_size)
            for v in range(0, 180, self.__slice_size)
        ]

        rospy.loginfo(
            "Publishing Contour vals to topic {}".format(contour_topic))
        self.__contour_pub = rospy.Publisher(contour_topic,
                                             Contour,
                                             queue_size=5)

        rospy.loginfo(
            "Publishing Point vals to topic {}".format(centroid_topic))
        self.__centroid_pub = rospy.Publisher(centroid_topic,
                                              Point,
                                              queue_size=5)

        if self.__publish_point_cloud:
            rospy.loginfo(
                "Publishing PointCloud2 vals to topic {}".format(pc_topic))
            self.__pc_pub = rospy.Publisher(pc_topic,
                                            PointCloud2,
                                            queue_size=5)

        rospy.loginfo("Subscribing to LaserScan topic {}".format(scan_topic))
        self.__scan_sub = rospy.Subscriber(scan_topic, LaserScan, self.on_msg)
コード例 #17
0
ファイル: laserprojector.py プロジェクト: harshad1600/ICP
    def callbac(self, msg):
        lst = []
        self.converter = LaserProjection()
        cloud = self.converter.projectLaser(msg)
        self.pub = rospy.Publisher("/pc", pc2, queue_size=1)
        self.pub.publish(cloud)

        for p in pc22.read_points(cloud,
                                  field_names=("x", "y", "z"),
                                  skip_nans=True):
            lst.append((p[0], p[1], p[2]))
        print(lst)
        print("")
        print("-------------------")
        print("")
コード例 #18
0
    def __init__(self):
        plt.ion()
        plt.show()
        self.proj = LaserProjection()
        rospy.init_node('nav_test', anonymous=False)
        self.bridge = CvBridge()
        self.OPENNING_ANGLE = np.deg2rad(60)
        self.has_box = False
        self.box_x = 0
        self.moving = False
        rospy.Subscriber("/usb_cam/image_raw", Image, self.img_callback)
        rospy.Subscriber('scan', LaserScan, self.scan_callback)

        rospy.on_shutdown(self.shutdown)

        self.state = state_start
        # Publisher to manually control the robot (e.g. to stop it)
        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)

        # Subscribe to the move_base action server
        self.move_base = actionlib.SimpleActionClient("move_base",
                                                      MoveBaseAction)

        rospy.loginfo("Waiting for move_base action server...")

        # Wait 60 seconds for the action server to become available
        self.move_base.wait_for_server(rospy.Duration(60))

        rospy.loginfo("Connected to move base server")
        rospy.loginfo("Starting navigation test")

        if not rospy.is_shutdown():
            rospy.spin()
コード例 #19
0
class Laser2PC():
    #translate laser to pc and publish point cloud to /cloud_in
    def __init__(self):
        self._measurements = [float('inf')]
        self.laserProj = LaserProjection()
        self.pcPub = rospy.Publisher('/point_cloud2', pc2, queue_size=5)
        self.laserSub = rospy.Subscriber('/scan', LaserScan,
                                         self.laserCallBack)

    def laserCallBack(self, data):
        # rospy.loginfo(rospy.get_caller_id() + 'I heard %s', data)
        cloud_out = self.laserProj.projectLaser(data)
        cloud_out.header.seq = data.header.frame_id
        cloud_out.header.stamp = data.header.stamp
        cloud_out.header.frame_id = 'laser0_frame'
        self._measurements = cloud_out
        self.pcPub.publish(cloud_out)

    @property
    def ready(self):
        return not np.isnan(self._measurements)

    @property
    def measurements(self):
        return self._measurements
コード例 #20
0
class Laser2PC():
    def __init__(self):
        self.laserProj = LaserProjection()
        self.pcPub = rospy.Publisher("/prometheus/sensors/pcl2",
                                     pc2,
                                     queue_size=1)
        self.laserSub = rospy.Subscriber("/prometheus/sensors/2Dlidar_scan",
                                         LaserScan, self.laserCallback)

    def laserCallback(self, data):

        # cc = LaserScan()

        # cc.header = data.header
        # cc.angle_max = data.angle_max
        # cc.angle_min = data.angle_min
        # cc.angle_increment = data.angle_increment
        # cc.time_increment = data.time_increment
        # cc.scan_time = data.scan_time
        # cc.range_max = data.range_max
        # cc.range_min = data.range_min
        # cc.ranges = data.ranges
        # cc.intensities = data.intensities

        # a = len(data.ranges)

        # for i in range(0,a):
        #     if data.ranges[i] != float("inf"):
        #         if data.ranges[i] < 0.4:
        #             cc.ranges[i] = 0.41

        cloud_out = self.laserProj.projectLaser(data)

        self.pcPub.publish(cloud_out)
コード例 #21
0
ファイル: rotate_to_3D.py プロジェクト: nivir/scan_volume
    def push(self,data_LaserScan):

        # rospy.loginfo('project data_LaserScan to PointCloud OK')
        pcl_LaserScan = LaserProjection().projectLaser(data_LaserScan)
        points_xyz = self.xyz_from_pcl(pcl_LaserScan)   # points_xyz: [N,3]  [:,1]=0
        # print "scan point N = ",points_xyz.shape[0],"   /  ", pcl_LaserScan.width, "    rangesN = ",len(data_LaserScan.ranges)

        if self.status == 'start' or self.status == 'scanning':
            if self.status == 'start':
                self.status = 'scanning'
            self.add_data( pcl_LaserScan )
            self.scanN += 1
            self.pcl_3d_pub.publish(self.pcl_3d)
            self.pcl3d_bag.write( 'pcl_3d', self.pcl_3d )

        elif self.status == 'stop':
            self.status = 'waitting'

            if self.separate_models:
                self.pcl_n = self.pcl_n + 1
                self.reset()

            self.pcl3d_bag.close()

            rospy.loginfo('stop recording, save this model: ' + self.res_bag_name )

        if self.status == 'scanning' and  self.theta > 181.0 * math.pi  / 180:
            self.stop()
        return self.scanN, self.theta
コード例 #22
0
class Laser2PC():
    def __init__(self):

        self.laserProj = LaserProjection()
        self.pcPub = rospy.Publisher("/laserPointCloud",
                                     PointCloud2,
                                     queue_size=1)
        self.laserSub = rospy.Subscriber("/scan",
                                         LaserScan,
                                         self.laserCallback,
                                         queue_size=1)

    def laserCallback(self, msg):

        pc2_msg = self.laserProj.projectLaser(msg)
        self.pcPub.publish(pc2_msg)

        # Convert it to a generator of the individual points
        point_generator = pc2.read_points(pc2_msg)

        # Access a generator in a loop
        sum_points = 0.0
        num = 0
        for point in point_generator:
            if not np.isnan(point[2]):
                sum_points += point[2]
                num += 1

        # Calculate the average z value for example
        print("Average height: " + str(sum_points / num))
コード例 #23
0
    def __init__(self):
	self.max_vel = 0.0
        self._sub = rospy.Subscriber('/scan', LaserScan, self.callback_scan)
        self._pub = rospy.Publisher('/max_speed', Float32, queue_size=1)
        self.laser_projector = LaserProjection()
        self.scancount = 0
        self.backcount = 0
        self.backing = 0
コード例 #24
0
    def __init__(self):
        """
        Class constructor
        """
        rospy.init_node('explore', anonymous=True)
        self.laser = LaserScan()
        self.laser_proj = LaserProjection()
        self.pc_pub = rospy.Publisher('/laserPointCloud',
                                      PointCloud2,
                                      queue_size=1)
        self.sacn_subscriber = rospy.Subscriber('/scan', LaserScan,
                                                self.callback)
        self.velocity_publisher = rospy.Publisher('/cmd_vel',
                                                  Twist,
                                                  queue_size=10)

        self.front_thresh = 0.3
コード例 #25
0
ファイル: laserScan.py プロジェクト: soohyun0907/serbot_pc
class Laser2PC():
    def __init__(self):
        self.laserProj = LaserProjection()
        self.pcPub = rospy.Publisher("/laserPointCLoud",pc2, queue_size=1)
        self.laserSub = rospy.Subscriber("scan",LaserScan,self.laserCallback)
    def laserCallback(self,data):
        cloud_out = self.laserProj.projectLaser(data)
        self.pcPub.publish(cloud_out)
コード例 #26
0
class LaserScanToPointCloud:
    def __init__(self):
        self.laserProj = LaserProjection()
        self.pointCloudPublisher = rospy.Publisher("/points", PointCloud2, queue_size = 1)
        self.laserScanSubscriber = rospy.Subscriber("/scan", LaserScan, self.laserScanCallback)

    def laserScanCallback(self, data):
        self.pointCloudPublisher.publish(self.laserProj.projectLaser(data))
コード例 #27
0
    def __init__(self):

        # Variables
        self.laserProj = LaserProjection()

        # Publishers
        self.pcWorldPub = rospy.Publisher("/world_pc", pc2, queue_size=1)
        #self.pcWaterPub = rospy.Publisher("/water_pc", pc2, queue_size=1)
        self.mkArrayPub = rospy.Publisher("/marker_array_water_recoloured",
                                          MarkerArray,
                                          queue_size=1)

        # Subscribers
        rospy.Subscriber("/scan", LaserScan, self.callback_lidar_scan)
        rospy.Subscriber("/marker_array_water", MarkerArray,
                         self.callback_recolour)

        rospy.spin()
コード例 #28
0
    def __init__(self):
        # Defining variables:
        self.laserProj = LaserProjection()

        self.scanFiltered = LaserScanFiltered()
        self.scanFiltered.laser_range = 0.0
        self.scanFiltered.laser_intensity = 0.0
        self.scanFiltered.sonar_range = 0.0
        self.sensor_offset = 10.0

        rospy.Subscriber("/scan", LaserScan, self.callback_get_lidar_scan)
        rospy.Subscriber("/sensor_state", SensorState,
                         self.callback_get_sonar_scan)
        self.pub = rospy.Publisher("/sonar_lidar_state",
                                   LaserScanFiltered,
                                   queue_size=10)
        self.pcWaterPub = rospy.Publisher("/water_pc", pc2, queue_size=1)
        rospy.spin()
コード例 #29
0
 def readscan(self):
     laser_projector = LaserProjection()
     for topic, msg, time_stamp in self.bag.read_messages(
             topics=[self.scan_topic]):
         cloud = laser_projector.projectLaser(msg)
         frame_points = np.zeros([0, 2])
         for p in pc2.read_points(cloud,
                                  field_names=("x", "y", "z"),
                                  skip_nans=True):
             #theta = math.atan2(p[0] , p[1])
             #laser_range = np.pi
             #if theta > np.pi/2 + np.pi/3 + laser_range/2 or theta < np.pi/2 + np.pi/3 - laser_range/2:
             #    continue
             #d = math.sqrt(p[0]*p[0] + p[1]*p[1])
             #if d > 1.5 and theta > np.pi/2:
             #    continue
             p2d = np.array([p[0], p[1]])
             frame_points = np.vstack([frame_points, p2d])
         self.points.append([time_stamp, frame_points])
コード例 #30
0
class Laser2PC():
    def __init__(self):
        self.laserProj= LaserProjection()
        self.pcPub = rospy.Publisher('Pointy_Cloud', PointCloud2, queue_size=10)
        self.laserSub = rospy.Subscriber("lidar", LaserScan, self.laserCallback)


    def laserCallback(self,msg):
        point_cloud =  self.laserProj.projectLaser(msg)
        self.pcPub.publish(point_cloud)
コード例 #31
0
ファイル: baseScan.py プロジェクト: fevb/team_cvap
    def __init__(self, verbose=False):
        self.rangeData = LaserScan()
        self.scan_sub = rospy.Subscriber("/base_scan", LaserScan, self.callback)
        self.listener = tf.TransformListener()
        self.laser_projector = LaserProjection()
        self.pc = []
        self.leg1 = []
        self.leg2 = []
        self.br = tf.TransformBroadcaster()
        self.rate = rospy.Rate(20.0)
        self.calibrated = False
        self.reCalibration = False
        self.priorOri_in_base_laser_link = [] # in base_laser_link frame
        self.priorLeft_in_base_laser_link = [] # in base_laser_link frame
        self.priorRight_in_base_laser_link = [] # in base_laser_link frame
        self.odomL = [] # in odom_combined frame
        self.odomR = [] # in odom_combined frame
        self.priorAvailable = False
        self.newObsWeight = 0.2
        self.offsetXY = [-0.044, 0]
        self.binOffset = 0.02
        self.pubShelfSep = rospy.Publisher('pubShelfSep', PoseStamped)
        self.reCalibrationCount = 4
        self.tolerance = 0.1
        self.updateRounds = 100
        self.asyncRate = 20
        self.limitInitX = True
        self.xLimit = 0.1
        self.emergencyThreshold = 30*20
        self.emergencyTimeWindow = 60

        while not rospy.is_shutdown():
            try:
                self.offsetXY = rospy.get_param('/base_scan/offset')
                break
            except:
                rospy.loginfo('[shelf publisher]: waiting for base scan offset param')
                rospy.sleep(random.uniform(0,1))
                continue

        self.start_srv = rospy.Service(rospy.get_name() + '/start', Empty, self.start_srv_callback)
        self._start = False

        # backup human supervised info for emergency
        rp = rospkg.RosPack()
        try:
            dict_fp = rp.get_path('amazon_challenge_grasping')

        except:
            rospy.logerr('[shelf_publisher]: emergency file not found!')
            sys.exit(1)
        self.fileName = dict_fp + '/config/shelf_emergency.dat'
        self.emergency = {}
コード例 #32
0
    def __init__(self, verbose=False):
        self.rangeData = LaserScan()
        self.scan_sub = rospy.Subscriber("/scan", LaserScan, self.callback_laser)
        rospy.sleep(1)
        self.listener = tf.TransformListener()
        self.laser_projector = LaserProjection()
        self.pc = []
        self.leg1 = []
        self.leg2 = []
        self.br = tf.TransformBroadcaster()
        self.rate = rospy.Rate(10.0)
        self.rate_measurements = rospy.Rate(0.5)
        self.priorOri_in_base = []
        self.priorLeft_in_base = []
        self.priorRight_in_base = []
        self.odomL = []
        self.odomR = []
        self.priorAvailable = False
        self.newObsWeight = 0.2
        self.offsetXY = [0.0, 0.0]
        self.binOffset = 0.02
        #self.pubShelfSep = rospy.Publisher('pubShelfSep', PoseStamped, queue_size=10)
        self.reCalibrationCount = 4
        self.tolerance = 0.1
        self.updateRounds = 100
        self.asyncRate = 20
        self.limitInitX = True
        self.xLimit = 0.0
        self.rp = rospkg.RosPack()
        self.writeFile = False
        self.calculateTF = False
        self.sendTF = False
        self._ready = False
        self.radius_th_leg = 0.1
        self.n_samples = 5
        self.sleep_between_scans = 2

        while not rospy.is_shutdown():
            try:
                self._shelfHeight = rospy.get_param("/apc/shelf_calibration/height")
                self._shelfRoll = rospy.get_param("/apc/shelf_calibration/roll")
                self._shelfPitch = rospy.get_param("/apc/shelf_calibration/pitch")
                break
            except:
                rospy.sleep(1.0)
                continue


        self.start_srv = rospy.Service(rospy.get_name() + '/calibrate', Empty, self.start_srv_callback)
コード例 #33
0
ファイル: behave_tree.py プロジェクト: ajayjain/husky_pursuit
class Lidar():
	def __init__(self):
		rospy.loginfo("Initializing Lidar class. Creating subscriber")
		self.scan_sub = rospy.Subscriber('/base_scan', LaserScan, self.on_scan)
		self.xyz_points = None
		rospy.loginfo("Initializing Lidar class. Creating LaserProjection")
		self.laser_projector = LaserProjection()

	def on_scan(self, scan):
		rospy.loginfo("Got scan, projecting")
		cloud = self.laser_projector.projectLaser(scan)
		gen = pc2.read_points(cloud, skip_nans=True, field_names=("x", "y", "z"))
		self.xyz_points = np.array(list(gen))
		rospy.sleep(1/Me.CMD_FREQ)
	
	def show_point_plot(self):
		x = self.xyz_points[:,0]
		y = self.xyz_points[:,1]
		plt.scatter(x, y)
		plt.show()
コード例 #34
0
ファイル: lineScan.py プロジェクト: fevb/team_cvap
	def __init__(self, verbose=False):
		self.rangeData = LaserScan()
		self.scan_sub = rospy.Subscriber("/base_scan", LaserScan, self.callback)
		self.listener = tf.TransformListener()
		self.laser_projector = LaserProjection()
		self.pc = []
		self.leg1 = []
		self.leg2 = []
		self.br = tf.TransformBroadcaster()
		self.rate = rospy.Rate(100.0)
		self.calibrated = False
		self.reCalibration = False
		self.priorOri = []
		self.priorLeft = []
		self.priorRight = []
		self.odomL = []
		self.odomR = []
		self.priorAvailable = False
		self.newObsWeight = 0.1
		self.offsetXY = [-0.044, 0]
コード例 #35
0
ファイル: behave_tree.py プロジェクト: ajayjain/husky_pursuit
	def __init__(self):
		rospy.loginfo("Initializing Lidar class. Creating subscriber")
		self.scan_sub = rospy.Subscriber('/base_scan', LaserScan, self.on_scan)
		self.xyz_points = None
		rospy.loginfo("Initializing Lidar class. Creating LaserProjection")
		self.laser_projector = LaserProjection()
コード例 #36
0
    def test_project_laser(self):
        tolerance = 6 # decimal places
        projector = LaserProjection()

        ranges = [-1.0, 1.0, 2.0, 3.0, 4.0, 5.0, 100.0]
        intensities = np.arange(1.0, 6.0).tolist()

        min_angles = -np.pi / np.array([1.0, 1.5, 2.0, 4.0, 8.0])
        max_angles = -min_angles

        angle_increments = np.pi / np.array([180., 360., 720.])

        scan_times = [rospy.Duration(1./i) for i in [40, 20]]

        for range_val, intensity_val, \
            angle_min, angle_max, angle_increment, scan_time in \
            product(ranges, intensities,
                min_angles, max_angles, angle_increments, scan_times):
            try:
                scan = build_constant_scan(
                    range_val, intensity_val,
                    angle_min, angle_max, angle_increment, scan_time)
            except BuildScanException:
                if (angle_max - angle_min)/angle_increment > 0:
                    self.fail()

            cloud_out = projector.projectLaser(scan, -1.0,
                    LaserProjection.ChannelOption.INDEX)
            self.assertEquals(len(cloud_out.fields), 4,
                    "PointCloud2 with channel INDEX: fields size != 4")

            cloud_out = projector.projectLaser(scan, -1.0,
                    LaserProjection.ChannelOption.INTENSITY)
            self.assertEquals(len(cloud_out.fields), 4,
                    "PointCloud2 with channel INDEX: fields size != 4")

            cloud_out = projector.projectLaser(scan, -1.0)
            self.assertEquals(len(cloud_out.fields), 5,
                    "PointCloud2 with channel INDEX: fields size != 5")
            cloud_out = projector.projectLaser(scan, -1.0,
                    LaserProjection.ChannelOption.INTENSITY |
                    LaserProjection.ChannelOption.INDEX)
            self.assertEquals(len(cloud_out.fields), 5,
                    "PointCloud2 with channel INDEX: fields size != 5")

            cloud_out = projector.projectLaser(scan, -1.0,
                    LaserProjection.ChannelOption.INTENSITY |
                    LaserProjection.ChannelOption.INDEX |
                    LaserProjection.ChannelOption.DISTANCE)
            self.assertEquals(len(cloud_out.fields), 6,
                    "PointCloud2 with channel INDEX: fields size != 6")

            cloud_out = projector.projectLaser(scan, -1.0,
                    LaserProjection.ChannelOption.INTENSITY |
                    LaserProjection.ChannelOption.INDEX |
                    LaserProjection.ChannelOption.DISTANCE |
                    LaserProjection.ChannelOption.TIMESTAMP)
            self.assertEquals(len(cloud_out.fields), 7,
                    "PointCloud2 with channel INDEX: fields size != 7")

            valid_points = 0
            for i in range(len(scan.ranges)):
                ri = scan.ranges[i]
                if (PROJECTION_TEST_RANGE_MIN <= ri and
                    ri <= PROJECTION_TEST_RANGE_MAX):
                    valid_points += 1

            self.assertEqual(valid_points, cloud_out.width,
                    "Valid points != PointCloud2 width")

            idx_x = idx_y = idx_z = 0
            idx_intensity = idx_index = 0
            idx_distance = idx_stamps = 0

            i = 0
            for f in cloud_out.fields:
                if f.name == "x":
                    idx_x = i
                elif f.name == "y":
                    idx_y = i
                elif f.name == "z":
                    idx_z = i
                elif f.name == "intensity":
                    idx_intensity = i
                elif f.name == "index":
                    idx_index = i
                elif f.name == "distances":
                    idx_distance = i
                elif f.name == "stamps":
                    idx_stamps = i
                i += 1

            i = 0
            for point in pc2.read_points(cloud_out):
                ri = scan.ranges[i]
                ai = scan.angle_min + i * scan.angle_increment

                self.assertAlmostEqual(point[idx_x], ri * np.cos(ai),
                        tolerance, "x not equal")
                self.assertAlmostEqual(point[idx_y], ri * np.sin(ai),
                        tolerance, "y not equal")
                self.assertAlmostEqual(point[idx_z], 0,
                        tolerance, "z not equal")
                self.assertAlmostEqual(point[idx_intensity],
                        scan.intensities[i],
                        tolerance, "Intensity not equal")
                self.assertAlmostEqual(point[idx_index], i,
                        tolerance, "Index not equal")
                self.assertAlmostEqual(point[idx_distance], ri,
                        tolerance, "Distance not equal")
                self.assertAlmostEqual(point[idx_stamps],
                        i * scan.time_increment,
                        tolerance, "Timestamp not equal")
                i += 1
コード例 #37
0
ファイル: lineScan.py プロジェクト: fevb/team_cvap
class lineScan:
	def __init__(self, verbose=False):
		self.rangeData = LaserScan()
		self.scan_sub = rospy.Subscriber("/base_scan", LaserScan, self.callback)
		self.listener = tf.TransformListener()
		self.laser_projector = LaserProjection()
		self.pc = []
		self.leg1 = []
		self.leg2 = []
		self.br = tf.TransformBroadcaster()
		self.rate = rospy.Rate(100.0)
		self.calibrated = False
		self.reCalibration = False
		self.priorOri = []
		self.priorLeft = []
		self.priorRight = []
		self.odomL = []
		self.odomR = []
		self.priorAvailable = False
		self.newObsWeight = 0.1
		self.offsetXY = [-0.044, 0]


	def callback(self,data):
		self.rangeData = data

	def refreshRangeData(self):
		self.rangeData = LaserScan() # flush
		while len(self.rangeData.ranges) == 0:
			rospy.sleep(0.04)

	def getStatus(self):
		return self.calibrated

	def getCloud(self):

		self.refreshRangeData()
		cloud2 = self.laser_projector.projectLaser(self.rangeData)

		xyz = pc2.read_points(cloud2, skip_nans=True, field_names=("x", "y", "z"))
		self.pc = []
		while True:
			try:
				self.pc.append(xyz.next())
			except:
				break
		return self.pc

	def segmentLength(self, m, i1, i2):
		v1 = m[i1]
		v2 = m[i2]
		return np.linalg.norm(v1-v2)


	def cloud2Input(self):


		while not rospy.is_shutdown():
			plt.clf()
			pc = self.getCloud()
			m = np.asarray(pc)

			segs = []
			r = split_and_merge(m, 0.1)
			for i in range(1, len(r)):
				if r[i] - r[i-1] < 20:
					continue
				l = self.segmentLength(m, r[i], r[i-1])
				if l > 0.8 and l < 0.9:
					print 'found'
					print l
					segs.append([r[i], r[i-1]])

			# Plot found lines
			plt.plot(m[:, 0], m[:, 1], '.r')
			plt.plot(m[r, 0], m[r, 1], '-')
			plt.plot(m[r, 0], m[r, 1], '+')
			for s in segs:
				plt.plot(m[s, 0], m[s, 1], 'g-+', linewidth=5)
			plt.axis('equal')
			plt.draw()
			plt.title("Laser scan")
			plt.show(block=False)
コード例 #38
0
ファイル: baseScan.py プロジェクト: fevb/team_cvap
class baseScan:
    def __init__(self, verbose=False):
        self.rangeData = LaserScan()
        self.scan_sub = rospy.Subscriber("/base_scan", LaserScan, self.callback)
        self.listener = tf.TransformListener()
        self.laser_projector = LaserProjection()
        self.pc = []
        self.leg1 = []
        self.leg2 = []
        self.br = tf.TransformBroadcaster()
        self.rate = rospy.Rate(20.0)
        self.calibrated = False
        self.reCalibration = False
        self.priorOri_in_base_laser_link = [] # in base_laser_link frame
        self.priorLeft_in_base_laser_link = [] # in base_laser_link frame
        self.priorRight_in_base_laser_link = [] # in base_laser_link frame
        self.odomL = [] # in odom_combined frame
        self.odomR = [] # in odom_combined frame
        self.priorAvailable = False
        self.newObsWeight = 0.2
        self.offsetXY = [-0.044, 0]
        self.binOffset = 0.02
        self.pubShelfSep = rospy.Publisher('pubShelfSep', PoseStamped)
        self.reCalibrationCount = 4
        self.tolerance = 0.1
        self.updateRounds = 100
        self.asyncRate = 20
        self.limitInitX = True
        self.xLimit = 0.1
        self.emergencyThreshold = 30*20
        self.emergencyTimeWindow = 60

        while not rospy.is_shutdown():
            try:
                self.offsetXY = rospy.get_param('/base_scan/offset')
                break
            except:
                rospy.loginfo('[shelf publisher]: waiting for base scan offset param')
                rospy.sleep(random.uniform(0,1))
                continue

        self.start_srv = rospy.Service(rospy.get_name() + '/start', Empty, self.start_srv_callback)
        self._start = False

        # backup human supervised info for emergency
        rp = rospkg.RosPack()
        try:
            dict_fp = rp.get_path('amazon_challenge_grasping')

        except:
            rospy.logerr('[shelf_publisher]: emergency file not found!')
            sys.exit(1)
        self.fileName = dict_fp + '/config/shelf_emergency.dat'
        self.emergency = {}


    def start_srv_callback(self, req):
        rospy.loginfo('[shelf_publisher]: starting shelf publisher!')
        if self._start:
            rospy.logwarn('[shelf_publisher]: already started!!!!')
        self._start = True
        return EmptyResponse()

    def raw_input_with_timeout(prompt, timeout=1.0):
        print prompt,    
        timer = threading.Timer(timeout, thread.interrupt_main)
        astring = None
        try:
            timer.start()
            astring = raw_input(prompt)
        except KeyboardInterrupt:
            pass
        timer.cancel()
        return astring

    def callback(self,data):
        self.rangeData = data

    def refreshRangeData(self):
        self.rangeData = LaserScan() # flush
        while len(self.rangeData.ranges) == 0 and not rospy.is_shutdown():
            rospy.sleep(0.04)

    def getStatus(self):
        return self.calibrated

    def getCloud(self):

        # self.refreshRangeData()
        cloud2 = self.laser_projector.projectLaser(self.rangeData)

        xyz = pc2.read_points(cloud2, skip_nans=True, field_names=("x", "y", "z"))
        self.pc = []
        while not rospy.is_shutdown():
            try:
                self.pc.append(xyz.next())
            except:
                break
        return self.pc

    def findLegsOnce(self):
        pc = self.getCloud()
        x = []
        y= []
        for i in range(len(pc)):
            x.append(pc[i][0])
            y.append(pc[i][1])
        radius = []

        if self.calibrated or self.reCalibration: # use prior to find legs
            for i in range(len(x)):
                radius.append(math.sqrt((x[i]-self.priorLeft_in_base_laser_link[0])**2 + (y[i] - self.priorLeft_in_base_laser_link[1])**2))
            n1 = radius.index(min(radius))

            radius = []
            for i in range(len(x)):
                radius.append(math.sqrt((x[i]-self.priorRight_in_base_laser_link[0])**2 + (y[i] - self.priorRight_in_base_laser_link[1])**2))
            n2 = radius.index(min(radius))


            leg1 = [x[n1], y[n1]]
            leg2 = [x[n2], y[n2]]
        else:
            # Assuming there is nothing between the robot and the shelf
            if self.limitInitX:
                y = [y[i] for i in range(len(y)) if x[i] >= self.xLimit]
                x = [x[i] for i in range(len(x)) if x[i] >= self.xLimit]

            for i in range(len(x)):
                radius.append(math.sqrt(x[i]**2 + y[i]**2))
            n = radius.index(min(radius))
            
            x2 = [x[i] for i in range(len(x)) if math.sqrt( (x[i]-x[n])**2 + (y[i]-y[n])**2 ) > 0.7]
            y2 = [y[i] for i in range(len(y)) if math.sqrt( (x[i]-x[n])**2 + (y[i]-y[n])**2 ) > 0.7]
            radius2 = []
            

            for i in range(len(x2)):
                radius2.append(math.sqrt(x2[i]**2 + y2[i]**2))
            n2 = radius2.index(min(radius2))

            leg1 = [x[n], y[n]]
            leg2 = [x2[n2], y2[n2]]

        leg1 = [leg1[0] + self.offsetXY[0], leg1[1] + self.offsetXY[1]]
        leg2 = [leg2[0] + self.offsetXY[0], leg2[1] + self.offsetXY[1]]
        return [leg1, leg2] # left, right

    def findLegs(self):
        '''
        accumulate new observations
        '''
        L1x = 0
        L1y = 0
        L2x = 0
        L2y = 0

        legs = self.findLegsOnce()

        if legs[0][1] < legs[1][1]:
            legs[0], legs[1] = legs[1], legs[0]

        if self.calibrated:
            self.leg1[0] = self.leg1[0] * (1-self.newObsWeight) + legs[0][0] * self.newObsWeight
            self.leg1[1] = self.leg1[1] * (1-self.newObsWeight) + legs[0][1] * self.newObsWeight
            self.leg2[0] = self.leg2[0] * (1-self.newObsWeight) + legs[1][0] * self.newObsWeight
            self.leg2[1] = self.leg2[1] * (1-self.newObsWeight) + legs[1][1] * self.newObsWeight
        else:
            self.leg1 = legs[0]
            self.leg2 = legs[1]

        return [self.leg1, self.leg2] # left, right




    def getShelfFrame(self):
        # with respect to the frame of /base_scan
        legs = self.findLegs()
        ori_x = (legs[0][0] + legs[1][0]) / 2.
        ori_y = (legs[0][1] + legs[1][1]) / 2.

        left_leg = legs[0]
        if legs[0][1] < legs[1][1]:
            left_leg = legs[1]

        rotAngle = atan2(ori_x - left_leg[0], left_leg[1] - ori_y)

        return [ori_x, ori_y], rotAngle, legs

    def tf2PoseStamped(self, xy, ori):
        shelfPoseMsg = PoseStamped()
        shelfPoseMsg.pose.position.x = xy[0]
        shelfPoseMsg.pose.position.y = xy[1]
        shelfPoseMsg.pose.position.z = 0.0
        shelfPoseMsg.pose.orientation.x = ori[0]
        shelfPoseMsg.pose.orientation.y = ori[1]
        shelfPoseMsg.pose.orientation.z = ori[2]
        shelfPoseMsg.pose.orientation.w = ori[3]
        shelfPoseMsg.header.frame_id = 'base_laser_link'
        shelfPoseMsg.header.stamp = rospy.Time.now()
        return shelfPoseMsg


    def saveEmergency(self):
        with open(self.fileName, 'wb') as handle:
            pickle.dump(self.emergency, handle)

    def loadEmergency(self):
       
        with open(self.fileName, 'rb') as handle:
            self.emergency = pickle.load(handle)


    def publish2TF(self):
        answer = 'n'
        ask = True
        u = 0
        shelfN = 0
        recalibrateCount = 0
        emergencyCount = 0
        t_init = rospy.Time.now()

        while not rospy.is_shutdown():
            self.rate.sleep()
            # check if human calibration is done
            shelfOri, shelfRot, legs = self.getShelfFrame()


            if self.reCalibration:
                u = 0

            if not self.calibrated:

                self.br.sendTransform((shelfOri[0], shelfOri[1], 0),
                                      tf.transformations.quaternion_from_euler(0, 0, shelfRot),
                                      rospy.Time.now(),
                                      "/shelf_frame",     # child
                                      "/base_laser_link"      # parent
                                      )

                self.br.sendTransform((legs[0][0], legs[0][1], 0), \
                                      tf.transformations.quaternion_from_euler(0, 0, shelfRot), \
                                      rospy.Time.now(), \
                                      "/left_leg", \
                                      "/base_laser_link")

                self.br.sendTransform((legs[1][0], legs[1][1], 0), \
                                      tf.transformations.quaternion_from_euler(0, 0, shelfRot), \
                                      rospy.Time.now(), \
                                      "/right_leg", \
                                      "/base_laser_link")


            
                
            
            if self.priorAvailable:
                while not rospy.is_shutdown():
                    try:
                        shelf_in_odom, shelf_rot_in_odom = self.listener.lookupTransform("/odom_combined", "/shelf_frame", rospy.Time(0))
                        self.priorLeft_in_base_laser_link, dummy = self.listener.lookupTransform("/base_laser_link", "/odomL", rospy.Time(0))
                        self.priorRight_in_base_laser_link, dummy = self.listener.lookupTransform("/base_laser_link", "/odomR", rospy.Time(0))
                        break
                    except:
                        continue
            else:
                try:
                    shelf_in_odom, shelf_rot_in_odom = self.listener.lookupTransform("/odom_combined", "/shelf_frame", rospy.Time(0))
                except Exception, e:
                    # print e
                    continue
                    
            if self.reCalibration and math.sqrt((shelf_in_odom[0]-self.priorOri_in_odom[0]) **2 + (shelf_in_odom[1]-self.priorOri_in_odom[1]) **2) <= self.tolerance:
                # rospy.sleep(2)
                recalibrateCount += 1
                if recalibrateCount == self.reCalibrationCount: # take recalibration only if it's stable
                    rospy.loginfo('reCalibrated!')
                    recalibrateCount = 0
                    u = 0
                    while not rospy.is_shutdown(): # make sure the odomL and odomR are updated
                        try:
                            ######## self.priorOri_in_odom, self.priorRot_in_odom = self.listener.lookupTransform("/odom_combined", "/shelf_frame", rospy.Time(0))
                            ######## self.odomL, self.odomL_rot = self.listener.lookupTransform("/odom_combined", "/left_leg", rospy.Time(0))
                            ######## self.odomR, self.odomR_rot = self.listener.lookupTransform("/odom_combined", "/right_leg", rospy.Time(0))
                            self.calibrated = True
                            self.reCalibration = False
                            rospy.loginfo("Prior origin in odom_combined: X = %4f, Y = %4f" % (self.priorOri_in_odom[0], self.priorOri_in_odom[1]))
                            break
                        except:
                            continue

            if not self.calibrated and ask:
                emergencyCount += 1
                if emergencyCount == self.emergencyThreshold:
                    self.loadEmergency()
                    self.odomL = self.emergency.odomL
                    self.odomL_rot = self.emergency.odomL_rot
                    self.odomR = self.emergency.odomR
                    self.odomR_rot = self.emergency.odomR_rot
                    emergency_timestamp = self.emergency.timestamp
                    if (rospy.Time.now() - emergency_timestamp).to_sec() > self.emergencyTimeWindow:
                        print ''
                        rospy.logwarn('***********************************************************************')
                        rospy.logwarn('[shelf_publisher] NOT ABLE TO RECOVER FROM CRASHING, GAME OVER')
                        rospy.logwarn('***********************************************************************')
                        continue

                    ask = False
                    self.calibrated = True
                    self.priorAvailable = True
                    self.priorOri_in_odom = shelf_in_odom
                    self.priorRot_in_odom = shelf_rot_in_odom
                    self.priorLeft_in_base_laser_link = legs[0]
                    self.priorRight_in_base_laser_link = legs[1]

                    print ''
                    rospy.logwarn('***********************************************************************')
                    rospy.logwarn('[shelf_publisher] EMERGENCY RECOVERY CALLED!!!!!!!!!!!!!!!!!!!!!!')
                    rospy.logwarn('***********************************************************************')
                else:
                    sys.stdout.write("\r [ROS time: %s] Is the current shelf pose estimation good? (y/n)" % rospy.get_time() )
                    sys.stdout.flush()


                    if self._start:
                        answer = 'y'

                    if answer == 'y' or answer == 'yes':
                        self.calibrated = True
                        ask = False
                        self.priorAvailable = True
                        self.priorOri_in_odom = shelf_in_odom
                        self.priorRot_in_odom = shelf_rot_in_odom
                        self.priorLeft_in_base_laser_link = legs[0]
                        self.priorRight_in_base_laser_link = legs[1]
                        print ""
                        rospy.loginfo("Human calibration finished")
                        rospy.loginfo("Prior origin in /odom_combined: X = %4f, Y = %4f" % (self.priorOri_in_odom[0], self.priorOri_in_odom[1]))
                        while not rospy.is_shutdown():
                            try:
                                self.odomL, self.odomL_rot = self.listener.lookupTransform("/odom_combined", "/left_leg", rospy.Time(0))
                                self.odomR, self.odomR_rot = self.listener.lookupTransform("/odom_combined", "/right_leg", rospy.Time(0))
                                break
                            except:
                                pass
                        self.emergency = shelfInfo(self.odomL, self.odomL_rot, self.odomR, self.odomR_rot, rospy.Time.now())
                        self.saveEmergency()
                    else:
                        continue

            
            # check in the odometry frame

            if self.priorAvailable:
                # print 'pub odom'
                self.br.sendTransform(self.odomL, \
                                 self.odomL_rot, \
                                 rospy.Time.now(),\
                                 "/odomL",   \
                                 "/odom_combined")

                self.br.sendTransform(self.odomR, \
                                 self.odomR_rot, \
                                 rospy.Time.now(),\
                                 "/odomR",   \
                                 "/odom_combined")
            if self.calibrated:
                u += 1
                shelfN += 1
                if math.sqrt((shelf_in_odom[0] - self.priorOri_in_odom[0]) **2 + (shelf_in_odom[1] - self.priorOri_in_odom[1]) **2) > self.tolerance:
                    rospy.logwarn('something is wrong with shelf pose estimation!!!!!!!!!! RECALIBRATING')
                    recalibrateCount = 0
                    u = 0
                    self.calibrated = False
                    self.reCalibration = True
                    continue
                else:
                    self.br.sendTransform((shelfOri[0], shelfOri[1], 0),
                                     tf.transformations.quaternion_from_euler(0, 0, shelfRot),
                                     rospy.Time.now(),
                                     "/shelf_frame",     # child
                                     "/base_laser_link"      # parent
                                     )

                    self.br.sendTransform((legs[0][0], legs[0][1], 0), \
                                     tf.transformations.quaternion_from_euler(0, 0, shelfRot), \
                                     rospy.Time.now(),\
                                     "/left_leg",   \
                                     "/base_laser_link")

                    self.br.sendTransform((legs[1][0], legs[1][1], 0), \
                                     tf.transformations.quaternion_from_euler(0, 0, shelfRot), \
                                     rospy.Time.now(),\
                                     "/right_leg",   \
                                     "/base_laser_link")

                    self.pubShelfSep.publish(self.tf2PoseStamped(shelfOri, tf.transformations.quaternion_from_euler(0, 0, shelfRot)))

                if u == self.updateRounds:
                    while not rospy.is_shutdown(): # make sure the odomL and odomR are updated
                        try:
                            self.priorOri_in_odom, self.priorRot_in_odom = self.listener.lookupTransform("/odom_combined", "/shelf_frame", rospy.Time(0))
                            self.odomL, self.odomL_rot = self.listener.lookupTransform("/odom_combined", "/left_leg", rospy.Time(0))
                            self.odomR, self.odomR_rot = self.listener.lookupTransform("/odom_combined", "/right_leg", rospy.Time(0))
                            rospy.loginfo("Prior origin in /odom_combined: X = %4f, Y = %4f" % (self.priorOri_in_odom[0], self.priorOri_in_odom[1]))
                            u = 0
                            break
                        except:
                            continue

                if (rospy.Time.now()-t_init).to_sec()>1.0:
                    t_init = rospy.Time.now()
                    self.emergency = shelfInfo(self.odomL, self.odomL_rot, self.odomR, self.odomR_rot, rospy.Time.now())
                    self.saveEmergency()
                    # print 'pub'
                    shelfN = 0
                    self.br.sendTransform((0, 0.1515 + self.binOffset, 1.21),
                                     tf.transformations.quaternion_from_euler(0, 0, 0),
                                     rospy.Time.now(),
                                     "/shelf_bin_A",     # child
                                     "/shelf_frame"      # parent
                                     )

                    self.br.sendTransform((0, - 0.1515 + self.binOffset, 1.21),
                                     tf.transformations.quaternion_from_euler(0, 0, 0),
                                     rospy.Time.now(),
                                     "/shelf_bin_B",     # child
                                     "/shelf_frame"      # parent
                                     )

                    self.br.sendTransform((0, - 0.4303 + self.binOffset, 1.21),
                                     tf.transformations.quaternion_from_euler(0, 0, 0),
                                     rospy.Time.now(),
                                     "/shelf_bin_C",     # child
                                     "/shelf_frame"      # parent
                                     )

                    self.br.sendTransform((0, 0.1515 + self.binOffset, 1),
                                     tf.transformations.quaternion_from_euler(0, 0, 0),
                                     rospy.Time.now(),
                                     "/shelf_bin_D",     # child
                                     "/shelf_frame"      # parent
                                     )

                    self.br.sendTransform((0,  - 0.1515 + self.binOffset, 1),
                                     tf.transformations.quaternion_from_euler(0, 0, 0),
                                     rospy.Time.now(),
                                     "/shelf_bin_E",     # child
                                     "/shelf_frame"      # parent
                                     )

                    self.br.sendTransform((0, - 0.4303 + self.binOffset, 1),
                                     tf.transformations.quaternion_from_euler(0, 0, 0),
                                     rospy.Time.now(),
                                     "/shelf_bin_F",     # child
                                     "/shelf_frame"      # parent
                                     )

                    self.br.sendTransform((0, 0.1515 + self.binOffset, 0.78),
                                     tf.transformations.quaternion_from_euler(0, 0, 0),
                                     rospy.Time.now(),
                                     "/shelf_bin_G",     # child
                                     "/shelf_frame"      # parent
                                     )

                    self.br.sendTransform((0, - 0.1515 + self.binOffset, 0.78),
                                     tf.transformations.quaternion_from_euler(0, 0, 0),
                                     rospy.Time.now(),
                                     "/shelf_bin_H",     # child
                                     "/shelf_frame"      # parent
                                     )

                    self.br.sendTransform((0, - 0.4303 + self.binOffset, 0.78),
                                     tf.transformations.quaternion_from_euler(0, 0, 0),
                                     rospy.Time.now(),
                                     "/shelf_bin_I",     # child
                                     "/shelf_frame"      # parent
                                     )

                    self.br.sendTransform((0, 0.1515 + self.binOffset, 0.51),
                                     tf.transformations.quaternion_from_euler(0, 0, 0),
                                     rospy.Time.now(),
                                     "/shelf_bin_J",     # child
                                     "/shelf_frame"      # parent
                                     )

                    self.br.sendTransform((0, - 0.1515 + self.binOffset, 0.51),
                                     tf.transformations.quaternion_from_euler(0, 0, 0),
                                     rospy.Time.now(),
                                     "/shelf_bin_K",     # child
                                     "/shelf_frame"      # parent
                                     )

                    self.br.sendTransform((0, - 0.4303 + self.binOffset, 0.51),
                                     tf.transformations.quaternion_from_euler(0, 0, 0),
                                     rospy.Time.now(),
                                     "/shelf_bin_L",     # child
                                     "/shelf_frame"      # parent
                                     )
コード例 #39
0
class baseScan:
    def __init__(self, verbose=False):
        self.rangeData = LaserScan()
        self.scan_sub = rospy.Subscriber("/scan", LaserScan, self.callback_laser)
        rospy.sleep(1)
        self.listener = tf.TransformListener()
        self.laser_projector = LaserProjection()
        self.pc = []
        self.leg1 = []
        self.leg2 = []
        self.br = tf.TransformBroadcaster()
        self.rate = rospy.Rate(10.0)
        self.rate_measurements = rospy.Rate(0.5)
        self.priorOri_in_base = []
        self.priorLeft_in_base = []
        self.priorRight_in_base = []
        self.odomL = []
        self.odomR = []
        self.priorAvailable = False
        self.newObsWeight = 0.2
        self.offsetXY = [0.0, 0.0]
        self.binOffset = 0.02
        #self.pubShelfSep = rospy.Publisher('pubShelfSep', PoseStamped, queue_size=10)
        self.reCalibrationCount = 4
        self.tolerance = 0.1
        self.updateRounds = 100
        self.asyncRate = 20
        self.limitInitX = True
        self.xLimit = 0.0
        self.rp = rospkg.RosPack()
        self.writeFile = False
        self.calculateTF = False
        self.sendTF = False
        self._ready = False
        self.radius_th_leg = 0.1
        self.n_samples = 5
        self.sleep_between_scans = 2

        while not rospy.is_shutdown():
            try:
                self._shelfHeight = rospy.get_param("/apc/shelf_calibration/height")
                self._shelfRoll = rospy.get_param("/apc/shelf_calibration/roll")
                self._shelfPitch = rospy.get_param("/apc/shelf_calibration/pitch")
                break
            except:
                rospy.sleep(1.0)
                continue


        self.start_srv = rospy.Service(rospy.get_name() + '/calibrate', Empty, self.start_srv_callback)


    def start_srv_callback(self, req):
        while not self._ready:
            rospy.logwarn('Shelf published waiting for scan callback')
            rospy.sleep(1.0)

        rospy.loginfo('[shelf_publisher]: starting shelf publisher')
        self.calculateTF = True
        return EmptyResponse()

    def callback_laser(self, data):
        self._ready = True
        self.rangeData = data

    def getCloud(self):
        cloud2 = self.laser_projector.projectLaser(self.rangeData)
        xyz = pc2.read_points(cloud2, skip_nans=True, field_names=("x", "y", "z"))
        self.pc = []
        while not rospy.is_shutdown():
            try:
                self.pc.append(xyz.next())
            except:
                break
        return self.pc

    def findLegsOnce(self):
        pc = self.getCloud()
        x = []
        y = []
        for i in range(len(pc)):
            x.append(pc[i][0])
            y.append(pc[i][1])
        radius = []

        if self.limitInitX:
            y = [y[i] for i in range(len(y)) if x[i] >= self.xLimit]
            x = [x[i] for i in range(len(x)) if x[i] >= self.xLimit]

        for i in range(len(x)):
            radius.append(math.sqrt(x[i]**2 + y[i]**2))
        n = radius.index(min(radius))

        x1 = [x[i] for i in range(len(x)) if y[i] > 0.0]
        y1 = [y[i] for i in range(len(y)) if y[i] > 0.0]
        radius2 = []

        x2 = [x[i] for i in range(len(x)) if y[i] < 0.0]
        y2 = [y[i] for i in range(len(y)) if y[i] < 0.0]

        for i in range(len(x2)):
            radius2.append(math.sqrt(x2[i]**2 + y2[i]**2))
        n2 = radius2.index(min(radius2))

        leg1_p = [x[n], y[n]]
        leg2_p = [x2[n2], y2[n2]]

        x1_avg = [x[i] for i in range(len(x)) if math.sqrt( (x[i]-x[n])**2 + (y[i]-y[n])**2 ) < self.radius_th_leg]
        y1_avg = [y[i] for i in range(len(y)) if math.sqrt( (x[i]-x[n])**2 + (y[i]-y[n])**2 ) < self.radius_th_leg]

        x2_avg = [x[i] for i in range(len(x)) if math.sqrt( (x[i]-x[n2])**2 + (y[i]-y[n2])**2 ) < self.radius_th_leg]
        y2_avg = [y[i] for i in range(len(y)) if math.sqrt( (x[i]-x[n2])**2 + (y[i]-y[n2])**2 ) < self.radius_th_leg]

        leg1 = [numpy.mean(x1) + self.offsetXY[0], numpy.mean(y1) + self.offsetXY[1]]
        leg2 = [numpy.mean(x2) + self.offsetXY[0], numpy.mean(y2) + self.offsetXY[1]]

        #leg1 = [leg1[0] + self.offsetXY[0], leg1[1] + self.offsetXY[1]]
        #leg2 = [leg2[0] + self.offsetXY[0], leg2[1] + self.offsetXY[1]]
        return [leg1, leg2] # left, right

    def findLegs(self):
        leg1_x = numpy.zeros(self.n_samples)
        leg1_y = numpy.zeros(self.n_samples)
        leg2_x = numpy.zeros(self.n_samples)
        leg2_y = numpy.zeros(self.n_samples)
        for i in range(0, self.n_samples):
            rospy.loginfo('taking laser scan: ')
            rospy.loginfo(i)
            legs_sample = self.findLegsOnce()
            leg1_x[i] = legs_sample[0][0]
            leg1_y[i] = legs_sample[0][1]
            leg2_x[i] = legs_sample[1][0]
            leg2_y[i] = legs_sample[1][1]
            time.sleep(self.sleep_between_scans)
            #self.rate_measurements.sleep()

        leg1_m = [numpy.mean(leg1_x), numpy.mean(leg1_y)]
        leg2_m = [numpy.mean(leg2_x), numpy.mean(leg2_y)]
        rospy.loginfo('done scan')
        rospy.loginfo(leg1_m)
        rospy.loginfo(leg2_m)

        legs = [leg1_m, leg2_m]
        if legs[0][1] < legs[1][1]:
            legs[0], legs[1] = legs[1], legs[0]
        self.leg1 = legs[0]
        self.leg2 = legs[1]
        return [self.leg1, self.leg2] # left, right

    def getShelfFrame(self):
        legs = self.findLegs()
        ori_x = (legs[0][0] + legs[1][0]) / 2.
        ori_y = (legs[0][1] + legs[1][1]) / 2.
        left_leg = legs[0]
        if legs[0][1] < legs[1][1]:
            left_leg = legs[1]
        rotAngle = atan2(ori_x - left_leg[0], left_leg[1] - ori_y)
        return [ori_x, ori_y], rotAngle, legs

    def tf2PoseStamped(self, xy, ori):
        shelfPoseMsg = PoseStamped()
        shelfPoseMsg.pose.position.x = xy[0]
        shelfPoseMsg.pose.position.y = xy[1]
        shelfPoseMsg.pose.position.z = 0.0
        shelfPoseMsg.pose.orientation.x = ori[0]
        shelfPoseMsg.pose.orientation.y = ori[1]
        shelfPoseMsg.pose.orientation.z = ori[2]
        shelfPoseMsg.pose.orientation.w = ori[3]
        shelfPoseMsg.header.frame_id = 'base'
        shelfPoseMsg.header.stamp = rospy.Time.now()
        return shelfPoseMsg

    def estimateShelfFrame(self):

        while not rospy.is_shutdown():
            self.rate.sleep()
            t_init = rospy.Time.now()

            if self.calculateTF:
                try:
                    shelfOri, shelfRot, legs = self.getShelfFrame()
                except:
                    rospy.logerr("Shelf calibration is not getting points")
                    continue

                self.calculateTF = False
                self.sendTF = True
                self.writeFile = True

            if self.sendTF:
                # F1 = kdl.Frame(kdl.Rotation.RPY(0, 0, shelfRot),
                #                     kdl.Vector(shelfOri[0], shelfOri[0], self._shelfHeight))
                # F2 = kdl.Frame(kdl.Rotation.RPY(self._shelfRoll, self._shelfPitch, 0), kdl.Vector(0,0,0))
                # F3 = F1*F2
                # position_xyz3 = F3.p
                # orientation_xyzw3 = F3.M.GetQuaternion()
                # self.br.sendTransform((position_xyz3[0], position_xyz3[1], position_xyz3[2]),
                #              orientation_xyzw3,
                #              rospy.Time.now(),
                #              "/shelf_front",     # child
                #              "/base"             # parent
                #              )

                self.br.sendTransform((shelfOri[0], shelfOri[1], self._shelfHeight),
                             tf.transformations.quaternion_from_euler(self._shelfRoll, self._shelfPitch, shelfRot),
                             rospy.Time.now(),
                             "/shelf_front",     # child
                             "/base"             # parent
                             )

                self.br.sendTransform((legs[0][0], legs[0][1], self._shelfHeight), \
                             tf.transformations.quaternion_from_euler(self._shelfRoll, self._shelfPitch, shelfRot),
                             rospy.Time.now(),
                             "/left_leg",        # child
                             "/base"             # parent
                             )

                self.br.sendTransform((legs[1][0], legs[1][1], self._shelfHeight), \
                             tf.transformations.quaternion_from_euler(self._shelfRoll, self._shelfPitch, shelfRot),
                             rospy.Time.now(),
                             "/right_leg",       # child
                             "/base"             # parent
                             )

                #self.pubShelfSep.publish(self.tf2PoseStamped(shelfOri, tf.transformations.quaternion_from_euler(0, 0, shelfRot)))

            if self.writeFile:
                file_path = self.rp.get_path('calibration_data')
                f = open(file_path + "/extrinsics/shelf_front.txt", 'w')
                f.write(str(shelfOri[0])+'\t')
                f.write(str(shelfOri[1])+'\t')
                f.write(str(0.0)+'\t')
                quaternion_shelf = tf.transformations.quaternion_from_euler(0, 0, shelfRot)
                f.write(str(quaternion_shelf[0])+'\t')
                f.write(str(quaternion_shelf[1])+'\t')
                f.write(str(quaternion_shelf[2])+'\t')
                f.write(str(quaternion_shelf[3])+'\n')
                f.close()
                self.writeFile = False