Esempio n. 1
0
    def trigger(self):

        co = Color()
        co.header.frame_id = self.frame_id
        co.header.stamp    = rospy.Time.now()
        co.r = co.g = co.b = 0.0

        if self.sensor.light_mode:
            co.intensity = self.sensor.get_reflected_light()
            color_light  = self.sensor.get_light_color()
            if color_light == Type.COLORRED:
                co.r = 1.0
                co.g = 0.0
                co.b = 0.0
            elif color_light == Type.COLORGREEN:
                co.r = 0.0
                co.g = 1.0
                co.b = 0.0
            elif color_light == Type.COLORBLUE:
                co.r = 0.0
                co.g = 0.0
                co.b = 1.0
            else:
                co.r = 1.0
                co.g = 1.0
                co.b = 1.0
        else:
            co.intensity = 0.0
            color = self.sensor.get_color()
            if color == 1:  # black
                co.r = 0.0
                co.g = 0.0
                co.b = 0.0
            elif color == 2: # blue
                co.r = 0.0
                co.g = 0.0
                co.b = 1.0
            elif color == 3: # green
                co.r = 0.0
                co.g = 1.0
                co.b = 0.0
            elif color == 4: # yellow
                co.r = 1.0
                co.g = 1.0
                co.b = 0.0
            elif color == 5: # red
                co.r = 1.0
                co.g = 0.0
                co.b = 1.0
            elif color == 6: # white
                co.r = 1.0
                co.g = 1.0
                co.b = 1.0
            else:
                rospy.logerr('Undefined color detected by the Color sensor')

        self.pub.publish(co)
Esempio n. 2
0
 def trigger(self):
     co = Color()
     co.header.frame_id = self.frame_id
     co.header.stamp = rospy.Time.now()
     co.r = self.color_r
     co.g = self.color_g
     co.b = self.color_b
     co.intensity = self.intensity.get_reflected_light(self.color)
     self.pub.publish(co)
Esempio n. 3
0
 def trigger(self):
     co = Color()
     co.header.frame_id = self.frame_id
     co.header.stamp = rospy.Time.now()
     co.r = self.color_r
     co.g = self.color_g
     co.b = self.color_b
     co.intensity = self.intensity.get_reflected_light(self.color)
     self.pub.publish(co)
Esempio n. 4
0
 def trigger(self):
     co = Color()
     co.header.frame_id = self.frame_id
     co.header.stamp = rospy.Time.now()
     co.r = self.color_r
     co.g = self.color_g
     co.b = self.color_b
     # Get the reflected light reading.
     # This turns on the LED if color = red/green/blue
     co.intensity = self.intensity.get_reflected_light(self.color)
     self.pub.publish(co)
Esempio n. 5
0
 def trigger(self):
     co = Color()
     co.header.frame_id = self.frame_id
     co.header.stamp = rospy.Time.now()
     co.intensity = 0.0
     color = self.color.get_color()
     if color == 1:  # black
         co.r = 0.0
         co.g = 0.0
         co.b = 0.0
     elif color == 2: # blue
         co.r = 0.0
         co.g = 0.0
         co.b = 1.0
     elif color == 3: # green
         co.r = 0.0
         co.g = 1.0
         co.b = 0.0
     elif color == 4: # yellow
         co.r = 1.0
         co.g = 1.0
         co.b = 0.0
     elif color == 5: # red
         co.r = 1.0
         co.g = 0.0
         co.b = 1.0
     elif color == 6: # white
         co.r = 1.0
         co.g = 1.0
         co.b = 1.0
     else:
         rospy.logerr('Undefined color of color sensor')
     self.pub.publish(co)
Esempio n. 6
0
 def trigger(self):
     co = Color()
     co.header.frame_id = self.frame_id
     co.header.stamp = rospy.Time.now()
     co.intensity = 0.0
     color = self.color.get_color()
     if color == 1:  # black
         co.r = 0.0
         co.g = 0.0
         co.b = 0.0
     elif color == 2:  # blue
         co.r = 0.0
         co.g = 0.0
         co.b = 1.0
     elif color == 3:  # green
         co.r = 0.0
         co.g = 1.0
         co.b = 0.0
     elif color == 4:  # yellow
         co.r = 1.0
         co.g = 1.0
         co.b = 0.0
     elif color == 5:  # red
         co.r = 1.0
         co.g = 0.0
         co.b = 1.0
     elif color == 6:  # white
         co.r = 1.0
         co.g = 1.0
         co.b = 1.0
     else:
         rospy.logerr('Undefined color of color sensor')
     self.pub.publish(co)
Esempio n. 7
0
rospy.init_node('send_test_msgs')

print "Publishing test Color messages to topic ", color_topic
print "Publishing test Range messages to topic ", range_topic

br = tf.TransformBroadcaster()

rate = rospy.Rate(1)
while not rospy.is_shutdown():

    color_msg = Color()
    color_msg.header.frame_id = "/color_sensor_frame"
    color_msg.header.stamp = rospy.Time.now()
    color_msg.r = color_values[color_count][0]
    color_msg.g = color_values[color_count][1]
    color_msg.b = color_values[color_count][2]
    color_publisher.publish(color_msg)
    color_count = color_count + 1
    if color_count > (len(color_values) - 1):
        color_count = 0

    range_msg = Range()
    range_msg.header.frame_id = "/range_sensor_frame"
    range_msg.header.stamp = rospy.Time.now()
    range_msg.range = range_values[range_count]
    range_msg.spread_angle = 0.2
    range_publisher.publish(range_msg)
    range_count = range_count + 1
    if range_count > (len(range_values) - 1):
        range_count = 0