示例#1
0
def random_sonar(delay=1):
    print('Starting random sonar...')
    pub = Publisher('/sensors/range', Range)
    msg = Range()
    while not rospy.is_shutdown():
        msg.header = rospy.Header(frame_id='right_sonar_frame')
        msg.range = random.random() * 20
        pub.publish(msg)
        sleep(delay)
        msg.header = rospy.Header(frame_id='left_sonar_frame')
        msg.range = random.random() * 20
        pub.publish(msg)
    def test_map_received(self):
        poseMsg = PoseStamped()
        poseMsg.header = Header()
        poseMsg.pose.position.x = 0
        poseMsg.pose.position.y = 0
        poseMsg.pose.position.z = 0
        poseMsg.pose.orientation.x = 0
        poseMsg.pose.orientation.y = 0
        poseMsg.pose.orientation.z = 0
        poseMsg.pose.orientation.w = 1
        self.pose_pub.publish(poseMsg)

        rangeMsg = RangeArray()
        rangeMsg.header = Header()
        ranges = [0, 0.2, -1, 1, 1.5, 8, 3, 1, 8, 4, 2.3, 0.2, 0.1, 2, 3, 1, 9]
        for rangerange in ranges:
            range = Range()
            range.header = Header()
            range.range = rangerange
            range.field_of_view = 1
            range.min_range = 0.1
            range.max_range = 2.1
            rangeMsg.ranges.append(range)
        rangeMsg.ranges = rangeMsg.ranges[:self.NUM_RANGE_SENSORS]

        self.range_pub.publish(rangeMsg)
        rospy.sleep(0.2)
        self.assertEqual(self.rx.receivedMsg, True)
        self.assertGreater(len(self.rx.map.points), 0)
    def out_message(self):
        msg = Range()
        msg.max_range = self.max_range
        msg.min_range = self.min_range
        msg.range = self.return_median()
        msg.header = self.header
        self.publisher.publish(msg)
	print "Last four ranges recived: ", np.around(self.ranges, 4), "\t\t\t\r"
示例#4
0
    def onLidarLiteUpdate(self, msg):
        inMsg = msg
        outMsg = Range()
        outMsg.header = inMsg.header
        outMsg.min_range = inMsg.range_min
        outMsg.max_range = inMsg.range_max

        outMsg.range = msg.ranges[1]
        self.LidarLitePub.publish(outMsg)
示例#5
0
 async def publish_data(self, data):
     # Although the initialization of this Range message
     # including some of the values could be placed in the
     # constructor for efficiency reasons. This does
     # for some reason not work though.
     range = Range()
     range.radiation_type = range.ULTRASOUND
     range.field_of_view = math.pi * 5
     range.min_range = 0.02
     range.max_range = 1.5
     range.header = self.get_header()
     range.range = data[2]
     await self.publish(range)
示例#6
0
def lidar_callback(data):
    rg = Range()
    rg.header = Header()

    rg.radiation_type = Range().ULTRASOUND
    rg.min_range = 0.00  # 2cm
    rg.max_range = 13.0  # 2m
    for i in range(4):
        rg.header.frame_id = frame[i]
        rg.header.stamp = rospy.Time.now()
        rg.field_of_view = (20.0 / 180.0) * 3.14
        rg.range = data.ranges[90 * i]
        pub[i].publish(rg)
示例#7
0
文件: infrared.py 项目: mitya57/morse
    def default(self, ci='unused'):
        msg = Range()
        msg.header = self.get_ros_header()
        msg.radiation_type = Range.INFRARED
        msg.field_of_view = 20
        msg.min_range = 0
        msg.max_range = self.component_instance.bge_object['laser_range']
        tmp = msg.max_range
        for ray in self.data['range_list']:
            if tmp > ray:
                tmp = ray
        msg.range = tmp

        self.publish(msg)
示例#8
0
    def default(self, ci='unused'):
        msg = Range()
        msg.header = self.get_ros_header()

        try:
            msg.header.frame_id = self.kwargs['frame']
        except KeyError:
            msg.header.frame_id = ''

        msg.radiation_type = Range.INFRARED
        msg.min_range = self.component_instance.bge_object['min_range']
        msg.max_range = self.component_instance.bge_object['max_range']
        msg.range = self.data['range']
        self.publish(msg)
示例#9
0
    def to_msg(self, sensor_id, distance):

        msg = Range()
        msg.header = Header()

        msg.header.stamp = self.get_clock().now().to_msg()
        # need to define transforms for all sensors
        msg.header.frame_id = f"ultrasonic_sensor_{sensor_id}"

        msg.radiation_type = 0

        msg.field_of_view = 0.0
        msg.min_range = self.min_range
        msg.max_range = self.max_range
        msg.range = distance

        return msg
    def handle_estimate_range(self, req):
        ret = estimate_rangeResponse()
        if self.estimated_distance == None:
            ret.detected = False
        else:
            ret.detected = True

        current_estimated_range = Range()
        if (self.estimated_distance == None):
            self.estimated_distance = 0
        else:
            current_estimated_range.range = self.estimated_distance
        h = std_msgs.msg.Header()
        h.stamp = rospy.Time.now()
        current_estimated_range.header = h
        self.sampled_range_pub.publish(current_estimated_range)
        ret.range = current_estimated_range
        return ret
示例#11
0
def callback_relay(in_msg):
    global pub

    # find the main axis projection of each range, and save the minimum
    ang = in_msg.angle_min
    inc = in_msg.angle_increment
    min_range = math.inf
    for range in in_msg.ranges:
        val = math.cos(ang) * range
        ang = ang + inc
        if val < min_range:
            min_range = val

    # create and publish the message with noise
    msg = Range()
    msg.header = in_msg.header
    msg.radiation_type = msg.INFRARED
    msg.field_of_view = in_msg.angle_max - in_msg.angle_min
    msg.min_range = in_msg.range_min
    msg.max_range = in_msg.range_max
    msg.range = min_range

    pub.publish(msg)
示例#12
0
    def publishRangeData(self, sensor, byte1, byte2, dataLength, publisher):
        if len(self.byte) == dataLength:
            # Get data (decimal) from data bytes
            raw = int.from_bytes(self.byte[byte1] + self.byte[byte2],
                                 "big",
                                 signed=False)
            # Populate ROS message
            h = Header()
            h.stamp = rospy.Time.now()
            h.frame_id = sensor
            rangeMsg = Range()
            rangeMsg.header = h
            rangeMsg.radiation_type = 1
            rangeMsg.field_of_view = 0.3
            rangeMsg.min_range = 0.0
            rangeMsg.max_range = 0.2
            rangeMsg.range = (float(700 - raw) / 700.0) * 0.2

            # Publish data
            publisher.publish(rangeMsg)

        else:
            print("Data not formatted properly...")
示例#13
0
    def __init__(
        self,
        NUM_SENSORS=2,
        BAUDRATE=19200,
        PORT=None,
        READ_RATE=10,
    ):
        if PORT is None:
            PORT = check_output(
                ['rosrun', 'gudrun', 'get_usb_device_by_ID',
                 'encoder_mega']).strip(),

        rospy.init_node('listen_to_ultrasound')

        self.NUM_SENSORS = NUM_SENSORS
        self.BAUDRATE = BAUDRATE
        self.PORT = PORT
        self.READ_RATE = READ_RATE

        self.ser = serial.Serial(self.PORT, self.BAUDRATE)

        self.publishers = [
            rospy.Publisher('sensors/ultrasound_%d' % i, Range, queue_size=1)
            for i in range(self.NUM_SENSORS)
        ]

        m = Range()
        m.radiation_type = Range.ULTRASOUND
        m.field_of_view = 22.5 * 3.14159 / 180.
        m.min_range = 6.
        m.max_range = 200.
        m.range = 0
        m.header = Header()
        self.message = m

        self.loop()
示例#14
0
    def update_physics(self, delay, quadcopterId, quadcopter_controller,
                       cmd_sub, imu_pub, range_pub):
        vol_meas = p.getBaseVelocity(quadcopterId)
        one_over_sample_rate = 1 / delay
        previous_publish = time.perf_counter()
        try:
            while not rospy.is_shutdown():
                start = time.perf_counter()
                # the controllers are evaluated at a slower rate, only once in the
                # control_subsample times the controller is evaluated
                pos_meas, quaternion_meas = p.getBasePositionAndOrientation(
                    quadcopterId)
                previous_vol_meas = np.array(vol_meas)
                vol_meas = np.array(p.getBaseVelocity(quadcopterId))
                #print(vol_meas, type(vol_meas))
                acc = (vol_meas - previous_vol_meas) * one_over_sample_rate
                #print("VOL MEAS", previous_vol_meas, acc)

                if quadcopter_controller.sample == 0:
                    quadcopter_controller.sample = control_subsample

                    #force_act1,force_act2,force_act3,force_act4,moment_yaw = quadcopter_controller.update_control(pos_meas,quaternion_meas)
                    actual_values = cmd_sub.get_control()
                    vectorizer = np.zeros([5, 3])
                    vectorizer[:, 2] = actual_values
                    #print("input forces ", actual_values)
                    force_act1, force_act2, force_act3, force_act4, moment_yaw = vectorizer
                    #print("Well well well\t\t", vectorizer, force_act1, moment_yaw)
                else:
                    quadcopter_controller.sample -= 1

                # apply forces/moments from controls etc:
                # (do this each time, because forces and moments are reset to zero after a stepSimulation())

                #assuming right is -y

                p.applyExternalForce(quadcopterId, -1, force_act1,
                                     [arm_length, -arm_length, 0.],
                                     p.LINK_FRAME)
                p.applyExternalForce(quadcopterId, -1, force_act2,
                                     [-arm_length, -arm_length, 0.],
                                     p.LINK_FRAME)
                p.applyExternalForce(quadcopterId, -1, force_act3,
                                     [-arm_length, arm_length, 0.],
                                     p.LINK_FRAME)
                p.applyExternalForce(quadcopterId, -1, force_act4,
                                     [arm_length, arm_length, 0.],
                                     p.LINK_FRAME)

                # for the yaw-control:
                p.applyExternalTorque(quadcopterId, -1, moment_yaw,
                                      p.LINK_FRAME)

                # do simulation with pybullet:

                p.stepSimulation()

                header = Header()
                header.frame_id = 'Body'
                header.stamp = rospy.Time.now()
                Tsample_physics

                imu_message = Imu()
                imu_message.header = header
                imu_message.orientation.x = quaternion_meas[0]
                imu_message.orientation.y = quaternion_meas[1]
                imu_message.orientation.z = quaternion_meas[2]
                imu_message.orientation.w = quaternion_meas[3]
                imu_message.linear_acceleration.x = acc[0, 0]
                imu_message.linear_acceleration.y = acc[0, 1]
                imu_message.linear_acceleration.z = acc[0, 2]

                msg = Range()
                msg.header = header
                msg.max_range = 200
                msg.min_range = 0
                msg.range = pos_meas[2]

                if start - previous_publish > 1 / 100:
                    imu_pub.publish(imu_message)
                    range_pub.publish(msg)
                    previous_publish = start

                #print("BUBLISHED RANON /simulation/infrared_sensor_node", msg.range)
                # delay than repeat
                calc_time = time.perf_counter() - start
                if (calc_time > delay):
                    #print("Time to update physics is {} and is more than 20% of the desired update time ({}).".format(calc_time,delay))
                    pass
                else:
                    # print("calc_time = ",calc_time)
                    while (time.perf_counter() - start < delay):
                        time.sleep(delay / 10)

        except KeyboardInterrupt:

            self.ctrl_c_reset()
示例#15
0
top_topic = "top_camera_range"
front_topic = "front_camera_range"
s_range = 4
fov = pi/4

top_pub = rospy.Publisher(top_topic, Range, queue_size = 10)
front_pub = rospy.Publisher(front_topic, Range, queue_size = 10)
rospy.init_node('range_publisher')
r = rospy.Rate(10)

while not rospy.is_shutdown():
    h = Header()
    h.stamp = rospy.Time.now()
    h.frame_id = top_frame

    rg_top = Range()
    rg_top.header = h
    rg_top.field_of_view = fov
    rg_top.max_range = s_range
    rg_top.min_range = s_range
    rg_top.range = s_range
    rg_top.radiation_type = Range.INFRARED

    rg_front = copy.deepcopy(rg_top)
    rg_front.header.frame_id = front_frame

    top_pub.publish(rg_top)
    front_pub.publish(rg_front)

    r.sleep()
示例#16
0
from sensor_msgs.msg import Range
from std_msgs.msg import Header

rospy.init_node('lidar_range')

pub = [None, None, None, None]

# 4개의 토픽 발행
for i in range(4):
    name = 'scan' + str(i)
    pub[i] = rospy.Publisher(name, Range, queue_size=1)

msg = Range()
h = Header()
h.frame_id = "sensorXY"
msg.header = h
msg.radiation_type = Range().ULTRASOUND
msg.min_range = 0.02  # 2cm
msg.max_range = 2.0  # 2m
msg.field_of_view = (30.0 / 180.0) * 3.14

while not rospy.is_shutdown():
    msg.header.stamp = rospy.Time.now()

    # msg.range에 장애물까지의 거리를 미터 단위로 넣고 토픽을 발행한다.
    msg.range = 0.4

    for i in range(4):
        pub[i].publish(msg)
        msg.range += 0.4