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)
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)
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)
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)
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)
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)
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