def callLedsForDancing(self, blinking_freq):
"""
fColor : the first Color
sColor : the second Color
By defining LedGroup, we will have RGBA of color of each
set of colours.
I suppose that the last parameter of LEDS_BLINK is the frequency,
although its not on the server definition that I have.
"""
fColor = ColorRGBA()
sColor = ColorRGBA()
fColor.r = 0.5
fColor.g = 1.0
fColor.b = 0.5
sColor.r = 0.0
sColor.g = 0.0
sColor.b = 0.0
ledGroup = LedGroup()
first_color_duration = rospy.Duration(FIRST_COLOR_DURATION)
second_color_duration = rospy.Duration(SECOND_COLOR_DURATION)
try:
self.LEDS_BLINK(ledGroup, fColor, sColor, first_color_duration, second_color_duration, BLINK_EFECT_DURATION_DANCING, blinking_freq)
except rospy.ServiceException, e:
print "Service call To Blinking LEDs failed: %s" % e
def callback(joy): motor = ColorRGBA() color = ColorRGBA() if joy.buttons[0]>0: color.r = 0 color.g = 0 color.b = 150 if joy.buttons[1]>0: color.r = 0 color.g = 150 color.b = 0 if joy.buttons[2]>0: color.r = 150 color.g = 0 color.b = 0 if joy.buttons[3]>0: color.r = 150 color.g = 80 color.b = 0 y=joy.axes[3]*100.0 x=joy.axes[2]*-100.0 ##Ouputs throttle from -100 to 100 f = math.fabs(y/100.0) left = y*f+(1-f)*x right = y*f -(1-f)*x #Change to 0..127..255 motor.b = left*254.0/200.0 +127.0 motor.a = left*254.0/200.0 +127.0 motor.r = right*254.0/200.0 +127.0 motor.g = right*254.0/200.0 +127.0 pub_motor.publish(motor) pub_color.publish(color)
def SetColour(colour):
rgb_colour = ColorRGBA()
rgb_colour.a = 1.0
if colour == "RED":
rgb_colour.r = 1.0
rgb_colour.g = 0.0
rgb_colour.b = 0.0
return rgb_colour
if colour == "GREEN":
rgb_colour.r = 0.0
rgb_colour.g = 1.0
rgb_colour.b = 0.0
return rgb_colour
if colour == "BLUE":
rgb_colour.r = 0.0
rgb_colour.g = 0.0
rgb_colour.b = 1.0
return rgb_colour
if colour == "RANDOM":
rgb_colour.r = random.random()
rgb_colour.g = random.random()
rgb_colour.b = random.random()
return rgb_colour
assert (), "COLOUR not supported, changed to default RED type"
rgb_colour.r = 1.0
rgb_colour.g = 0.0
rgb_colour.b = 0.0
return rgb_colour
def __init__(self):
self.marker_pub = rospy.Publisher('occupancy_marker', Marker)
self.color_free = ColorRGBA()
cf = ColorRGBA()
cf.r, cf.g, cf.b, cf.a = 0.0, 0.6, 0.2, 1.0
self.color_free = cf
co = ColorRGBA()
co.r, co.g, co.b, co.a = 0.8, 0.0, 0.2, 1.0
self.color_occupied = co
self.point_srv = rospy.ServiceProxy('/occupancy_query_server/'
'is_point_free',
IsPointFree)
self.line_srv = rospy.ServiceProxy('/occupancy_query_server/'
'is_line_segment_free',
IsLineSegmentFree)
def main():
display_pub = rospy.Publisher('leds/display', Keyframe, queue_size=10)
anim_pub = rospy.Publisher('leds/animation', Animation, queue_size=10)
set_pub = rospy.Publisher('leds/set', Command, queue_size=10)
rospy.init_node('led_anim_clear', anonymous = True)
time.sleep(0.5)
keyframe_msg = Keyframe()
command_msg = Command()
color_msg = ColorRGBA()
anim_msg = Animation()
rospy.loginfo("Single frame test ...")
keyframe_msg.color_pattern = []
color_msg.r = 0.0
color_msg.g = 0.0
color_msg.b = 0.0
keyframe_msg.color_pattern.append(copy.deepcopy(color_msg))
display_pub.publish(keyframe_msg)
time.sleep(0.5)
rospy.loginfo("Clearing the animation...")
anim_msg.iteration_count = 1
anim_pub.publish(anim_msg)
time.sleep(1)
def find_color(self, im, label_color, mask):
contours = cv2.findContours(mask, cv2.cv.CV_RETR_TREE, cv2.cv.CV_CHAIN_APPROX_SIMPLE)[0]
#cv2.drawContours(im, contours, -1, (255, 255, 255), 2)
approx_contours = []
for c in contours:
area = cv2.contourArea(c)
if area < 100: continue
perim = cv2.arcLength(c, True)
approx = cv2.approxPolyDP(c, .05*perim, True)
#if len(approx) == 4 and len(cv2.convexityDefects(c, cv2.convexHull(c))) <= 1:
if len(approx) == 4:
approx_contours.append(approx)
moments = cv2.moments(c)
center = (int(moments['m10']/moments['m00']), int(moments['m01']/moments['m00']))
cv2.circle(im, center, 3, (255, 100, 100), 4)
print "Moment: ({}, {})".format(center[0], center[1])
msg_color = ColorRGBA()
msg_color.r, msg_color.g, msg_color.b = label_color
self.msg.colors.append(msg_color)
print "Label color: {}".format(label_color)
msg_size = Float64()
#msg_size.data = max(math.sqrt((approx[1][0][0]-approx[0][0][0])**2+(approx[1][0][1]-approx[0][0][1])**2), math.sqrt((approx[2][0][0]-approx[1][0][0])**2+(approx[2][0][1]-approx[2][0][0])**2))
msg_size.data = float((max(approx, key=lambda x: x[0][0])[0][0] - min(approx, key=lambda x: x[0][0])[0][0])) / len(im[0])
print "Width: {}".format(msg_size.data)
self.msg.sizes.append(msg_size)
msg_loc = Point()
msg_loc.x, msg_loc.y = float(center[0]) / len(im[0]), float(center[1]) / len(im)
self.msg.locations.append(msg_loc)
def MoveToBlob(self): #Moves to the largest [choose a color] blob self.drive_msg.drive.speed = 2 #Sets a field of the drivemsg message. #Gets the position of the largest green blob. The first green blob will be the largest green blob because the blobs are sorted by size. (index 0 is biggest size). The next three lines #define a specific color that we want to find/move to ele = ColorRGBA() ele.r = 0 ele.g = 255 largest_green_blob_index = self.blob_detect.colors.index(ele) blob_pos = self.blob_detect.locations[largest_green_blob_index] blob_x_pos = blob_pos[0] #This will be in between 0 and 1 (inclusive) (0 is the left and 1 is the right (?)) blob_y_pos = blob_pos[1] #Same thing here blob_area = blob_detect.size[largest_green_blob_index] #Get the area of the largest block of the set of the blocks with the color we specified if (blob_area < 1000): #Keep moving unless the blob_area becomes too big (meaning that we must be very close to the blob, ie it is taking up more of our field of vision. #If the blob_area is "small, we want to keep moving toward the blob while continually centering our field of vision with the blob. if (blob_x_pos > 0.55): #If Blob is On the right side drive_msg.drive.steering_angle = .33 #Turn left elif (blob_x_pos < 0.45): #If Blob is on the left side. drive_msg.drive.steering_angle = -0.33 #Turn right else: #If blob_x_pos (the x component of the blob's centroid) is in the middle 200 pixels, just move forward drive_msg.drive.steering_angle = 0 self.pub_mov_func.publish(drive_msg) else: # if the contour is really big, we are close to the sign, so we do an emergency stop. This is a bit redundant with our emergency stop function that comes from the lidar scan. self.drive_msg.drive.speed = 0 self.pub_mov_func.publish(drive_msg)
def MoveToBlob(self,msg): #Moves to the largest blob drivemsg = AckermannDriveStamped() #Sets the drive messaged to the AckermannDriveStamped message type drivemsg.drive.speed = 2 #Sets a field of the drivemsg message. #Gets the position of the largest green blob. The first green blob will be the largest green blob because the blobs are sorted by size. (index 0 is biggest size) ele = ColorRGBA() ele.r = 0 ele.g = 255 largest_green_blob_index = self.blob_detect.colors.index(ele) blob_pos = self.blob_detec.locations[largest_green_bloc_index] #Get's the position of the largest green block blob_x_pos = blob_pos[0] blob_y_pos = blob_pos[1] biggest_blob_area = blob_detec.sizes[0] #The blob with index one is the biggest blob if (biggest_blob_area < 1000): #Keep moving if (blob_x_pos > self.img_width/2): #If Blob is On the right side drivemsg.drive.steering_angle = .33 #Turn left if (blob_x_pos < self.img_width/2): #If Blob is on the left side. drivemsg.drive.steering_angle = -0.33 #Turn right self.pub_mov_func.publish(drivemsg) else: # if the contour is really big, we are close to the sign, so we do an emergency stop. We can delete this if we were to use the lidar. drivemsg.drive.speed = 0 self.pub_mov_func.publish(drivemsg)
def __init__(self):
super(CCA, self).__init__()
#UIの初期化
self.initUI()
#ROSのパブリッシャなどの初期化
rospy.init_node('roscca', anonymous=True)
self.mpub = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=10)
self.ppub = rospy.Publisher('joint_diff', PointStamped, queue_size=10)
#rvizのカラー設定(未)
self.carray = []
clist = [[1,1,0,1],[0,1,0,1],[1,0,0,1]]
for c in clist:
color = ColorRGBA()
color.r = c[0]
color.g = c[1]
color.b = c[2]
color.a = c[3]
self.carray.append(color)
self.fnames=["20151222_a1.json","20151222_a2.json","20151222_a3.json","20151222_b1.json","20151222_b2.json","20151222_b3.json"]
#self.fnames=["20151222_a1.json"]
self.winSizes = [90]
def to_msg(self):
msg = ColorRGBA()
msg.r = self.r / 255.0
msg.g = self.g / 255.0
msg.b = self.b / 255.0
msg.a = 1.0
return msg
def __init__(self):
super(CCA, self).__init__()
#UIの初期化
self.initUI()
#ファイル入力
#self.jsonInput()
#ROSのパブリッシャなどの初期化
rospy.init_node('ros_cca_table', anonymous=True)
self.mpub = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=10)
self.ppub = rospy.Publisher('joint_diff', PointStamped, queue_size=10)
self.carray = []
clist = [[1,1,0,1],[0,1,0,1],[1,0,0,1]]
for c in clist:
color = ColorRGBA()
color.r = c[0]
color.g = c[1]
color.b = c[2]
color.a = c[3]
self.carray.append(color)
def __init__(self):
super(CCA, self).__init__()
#UIの初期化
self.initUI()
#クラス間のデータ渡しテスト
self.test = 100
#ROSのパブリッシャなどの初期化
rospy.init_node('roscca', anonymous=True)
self.mpub = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=10)
self.ppub = rospy.Publisher('joint_diff', PointStamped, queue_size=10)
#rvizのカラー設定(未)
self.carray = []
clist = [[1,1,0,1],[0,1,0,1],[1,0,0,1]]
for c in clist:
color = ColorRGBA()
color.r = c[0]
color.g = c[1]
color.b = c[2]
color.a = c[3]
self.carray.append(color)
def hex_to_color_msg(hex_str):
rgb = struct.unpack('BBB', hex_str.decode('hex'))
msg = ColorRGBA()
msg.r = rgb[0]
msg.g = rgb[1]
msg.b = rgb[2]
msg.a = 1
return msg
def get_color(self, obj):
if obj in self.objects:
return self.objects[obj]
else:
c = ColorRGBA()
c.r, c.g, c.b = self.COLORS[len(self.objects) % len(self.COLORS)]
c.a = 1.0
self.objects[obj] = c
return c
def robot_position_marker(self): color = ColorRGBA() scale = Point() scale.x = 0.1 scale.y = 0.1 scale.z = 0.1 color.r = 1.0 color.a = 1.0 self.robot_position = self.visual_test(self.pose, Marker.CUBE, color, scale)
def callLedsForCmplxMv(self):
fColor = ColorRGBA()
sColor = ColorRGBA()
fColor.r = 1.0
fColor.g = 1.0
fColor.b = 1.0
sColor.r = 0.0
sColor.g = 0.0
sColor.b = 0.0
ledGroup = LedGroup()
ledGroup.ledMask = 3 # binary mask to decide which leds are active
try:
self.LEDS_BLINK(ledGroup, fColor, sColor, rospy.Duration(0.75), rospy.Duration(0.25), rospy.Duration(5), 50)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def static_area_makers(self): color = ColorRGBA() scale = Point() scale.x = 0.05 scale.y = 0.05 color.r = 1.0 color.g = 1.0 color.b = 0.0 color.a = 1.0 self.points_marker = self.visual_test(self.static_area, Marker.POINTS, color, scale)
def set_light(self,parameter_name,blocking=False):
ah = action_handle("set", "light", parameter_name, blocking, self.parse)
if(self.parse):
return ah
else:
ah.set_active(mode="topic")
rospy.loginfo("Set light to <<%s>>",parameter_name)
# get joint values from parameter server
if type(parameter_name) is str:
if not rospy.has_param(self.ns_global_prefix + "/light/" + parameter_name):
rospy.logerr("parameter %s does not exist on ROS Parameter Server, aborting...",self.ns_global_prefix + "/light/" + parameter_name)
return 2
param = rospy.get_param(self.ns_global_prefix + "/light/" + parameter_name)
else:
param = parameter_name
# check color parameters
if not type(param) is list: # check outer list
rospy.logerr("no valid parameter for light: not a list, aborting...")
print "parameter is:",param
ah.error_code = 3
return ah
else:
if not len(param) == 3: # check dimension
rospy.logerr("no valid parameter for light: dimension should be 3 (r,g,b) and is %d, aborting...",len(param))
print "parameter is:",param
ah.error_code = 3
return ah
else:
for i in param:
#print i,"type1 = ", type(i)
if not ((type(i) is float) or (type(i) is int)): # check type
#print type(i)
rospy.logerr("no valid parameter for light: not a list of float or int, aborting...")
print "parameter is:",param
ah.error_code = 3
return ah
else:
rospy.logdebug("accepted parameter %f for light",i)
# convert to ColorRGBA message
color = ColorRGBA()
color.r = param[0]
color.g = param[1]
color.b = param[2]
color.a = 1 # Transparency
# publish color
self.pub_light.publish(color)
ah.set_succeeded()
ah.error_code = 0
return ah
def create_trajectory_marker(self, traj):
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "/map"
int_marker.name = traj.uuid
int_marker.description = traj.uuid
pose = Pose()
pose.position.x = traj.pose[0]['position']['x']
pose.position.y = traj.pose[0]['position']['y']
int_marker.pose = pose
line_marker = Marker()
line_marker.type = Marker.LINE_STRIP
line_marker.scale.x = 0.05
# random.seed(traj.uuid)
# val = random.random()
# line_marker.color.r = r_func(val)
# line_marker.color.g = g_func(val)
# line_marker.color.b = b_func(val)
# line_marker.color.a = 1.0
line_marker.points = []
MOD = 1
for i, point in enumerate(traj.pose):
if i % MOD == 0:
x = point['position']['x']
y = point['position']['y']
p = Point()
p.x = x - int_marker.pose.position.x
p.y = y - int_marker.pose.position.y
line_marker.points.append(p)
line_marker.colors = []
for i, vel in enumerate(traj.vel):
if i % MOD == 0:
color = ColorRGBA()
val = vel / traj.max_vel
color.r = r_func(val)
color.g = g_func(val)
color.b = b_func(val)
color.a = 1.0
line_marker.colors.append(color)
# create a control which will move the box
# this control does not contain any markers,
# which will cause RViz to insert two arrows
control = InteractiveMarkerControl()
control.markers.append(line_marker)
int_marker.controls.append(control)
return int_marker
def visual_markers(self): color=ColorRGBA() scale=Point() scale.x=0.05 scale.y=0.05 color.r=0.0 color.g=1.0 color.b=0.0 color.a=0.5 self.line_marker=self.visual_test(self.cut_points, Marker.LINE_LIST, color, scale)#标记出区域划分（testing） color=ColorRGBA() scale=Point() scale.x=0.1 scale.y=0.1 color.r=0.0 color.g=0.0 color.b=1.0 color.a=1.0 self.centre_points_marker=self.visual_test(self.centre_points,Marker.POINTS, color, scale)
def clear_area_makers(self): color = ColorRGBA() scale = Point() scale.x = 0.05 scale.y = 0.05 color.r = 0.0 color.g = 1.0 color.b = 0.0 color.a = 1.0 self.ClearPoints_marker = self.visual_test(self.clear_area, Marker.POINTS, color, scale) print 'finish build markers', len(self.ClearPoints_marker.points)
def LaserDataMarker(self,LaserData): ####################visual_test color=ColorRGBA() scale=Point() scale.x=0.04 scale.y=0.04 color.r=0.0 color.g=2.0 color.b=0.0 color.a=1.0 self.laser_points_marker=self.visual_test(LaserData, Marker.POINTS, color, scale)
def changeColor():
pub = rospy.Publisher('light_controller/command', ColorRGBA)
rospy.init_node('light_test')
rospy.sleep(1)
#color in rgb color space ranging from 0 to 1
red = ColorRGBA()
red.r = 1
red.g = 0
red.b = 0
red.a = 1
yellow = ColorRGBA()
yellow.r = 0.4
yellow.g = 1
yellow.b = 0
yellow.a = 1
green = ColorRGBA()
green.r = 0
green.g = 1
green.b = 0
green.a = 1
blue = ColorRGBA()
blue.r = 0
blue.g = 0
blue.b = 1
blue.a = 1
white = ColorRGBA()
white.r = 0.3
white.g = 1
white.b = 0.3
white.a = 1
for color in [red,yellow,green,white,blue,green]:
rospy.loginfo("Setting rgb to [%lf, %lf, %lf] with a [%lf]",color.r,color.g,color.b,color.a)
pub.publish(color)
time.sleep(3)
def createLabel(m_id, action, pose, target_frame): # Text showing person's ID number pose = deepcopy(pose) scale = Vector3() scale.z = 0.2 color = ColorRGBA() color.r = 1.0 color.g = 1.0 color.b = 1.0 color.a = 1.0 pose.position.z = 1.9 return createMarker(m_id, Marker.TEXT_VIEW_FACING, action, pose, scale, color, target_frame, text='Player')
def publisher():
pub = rospy.Publisher('/ledscape_ros/array', ColorRGBA, queue_size=10)
rospy.init_node('userstracker_vectors', anonymous=True)
rate=rospy.Rate(0.2)
msg = ColorRGBA()
msg.r = int(sys.argv[1])
msg.g = int(sys.argv[2])
msg.b = int(sys.argv[3])
while not rospy.is_shutdown():
rospy.loginfo('Publishing color = '+str(msg.r)+" "+ str(msg.g)+" "+str(msg.b))
pub.publish(msg)
rate.sleep()
def get_marker_color(self): # pylint: disable=no-self-use
"""
Function (override) to return the color for marker messages.
:return: the color used by a walkers marker
:rtpye : std_msgs.msg.ColorRGBA
"""
color = ColorRGBA()
color.r = 0
color.g = 0
color.b = 255
return color
def static_area_makers(self):
color = ColorRGBA()
scale = Point()
scale.x = 0.05
scale.y = 0.05
color.r = 1.0
color.g = 1.0
color.b = 0.0
color.a = 1.0
self.points_marker = self.visual_test(self.static_area, Marker.POINTS,
color, scale)
def LaserDataMarker(self, LaserData): ####################visual_test
color = ColorRGBA()
scale = Point()
scale.x = 0.04
scale.y = 0.04
color.r = 0.0
color.g = 2.0
color.b = 0.0
color.a = 1.0
self.laser_points_marker = self.visual_test(LaserData, Marker.POINTS,
color, scale)
def flag_makers(self): color = ColorRGBA() scale = Point() pose = Pose() scale.x = 0.01 scale.y = 0.01 scale.z = 0.2 pose = self.pose pose.position.z += 0.5 color.r = 1.0 color.a = 1.0 self.flag_marker = self.visual_test(pose, Marker.TEXT_VIEW_FACING, color, scale)
def get_marker_color(self):
"""
Function (override) to return the color for marker messages.
:return: the color used by a vehicle marker
:rtpye : std_msgs.msg.ColorRGBA
"""
color = ColorRGBA()
color.r = 255
color.g = 0
color.b = 0
return color
def get_color(color):
rgba = ColorRGBA()
if color is None:
color = [1.0] * 4
if hasattr(color, 'x'):
rgba.r = getattr(color, 'r', 1.0)
rgba.g = getattr(color, 'g', 1.0)
rgba.b = getattr(color, 'b', 1.0)
rgba.a = getattr(color, 'a', 1.0)
elif len(color) == 4:
rgba.r = color[0]
rgba.g = color[1]
rgba.b = color[2]
rgba.a = color[3]
else:
rgba.r = color[0]
rgba.g = color[1]
rgba.b = color[2]
rgba.a = 1.0
return rgba
def changeColor():
pub = rospy.Publisher('light_controller/command_mode', LightMode, queue_size=1)
rospy.init_node('light_test')
light_mode = LightMode()
#color in rgb color space ranging from 0 to 1
red = ColorRGBA()
red.r = 1
red.g = 0
red.b = 0
red.a = 1
yellow = ColorRGBA()
yellow.r = 0.4
yellow.g = 1
yellow.b = 0
yellow.a = 1
green = ColorRGBA()
green.r = 0
green.g = 1
green.b = 0
green.a = 1
blue = ColorRGBA()
blue.r = 0
blue.g = 0
blue.b = 1
blue.a = 1
white = ColorRGBA()
white.r = 0.3
white.g = 1
white.b = 0.3
white.a = 1
for color in [red,yellow,green,white,blue,green]:
rospy.loginfo("Setting rgb to %s [%d, %d, %d]",color.r,color.g,color.b,color.a)
light_mode.colors= 27*[color]
pub.publish(light_mode)
time.sleep(3)
def pub_3D_marker(self, id, r, x, y, z, w, h, d):
marker = Marker()
marker.header.frame_id = 'camera_aligned_depth_to_color_frame'
marker.ns = 'object'
marker.id = id
marker.type = 1 # cube
marker.action = 0
marker.lifetime = rospy.Duration(1)
# Pose
pose = Pose()
pose.position.x = x
pose.position.y = y
pose.position.z = z
pose.orientation.w = 1
marker.pose = pose
# Scale
scale = Vector3()
scale.x = w
scale.y = h
scale.z = d
marker.scale = scale
# Color
color = ColorRGBA()
color.a = 0.25
if r:
color.r = 0
color.g = 255
color.b = 0
else:
color.r = 255
color.g = 0
color.b = 0
marker.color = color
# Publish marker
self.rviz_pub.publish(marker)
def create_plan_marker(self, plan, plan_values):
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "/map"
int_marker.name = plan.ID
int_marker.description = plan.ID
pose = Pose()
#pose.position.x = traj.pose[0]['position']['x']
#pose.position.y = traj.pose[0]['position']['y']
int_marker.pose = pose
line_marker = Marker()
line_marker.type = Marker.LINE_STRIP
line_marker.scale.x = 0.1
# random.seed(float(plan.ID))
# val = random.random()
# line_marker.color.r = r_func(val)
# line_marker.color.g = g_func(val)
# line_marker.color.b = b_func(val)
# line_marker.color.a = 1.0
line_marker.points = []
for view in plan.views:
x = view.get_ptu_pose().position.x
y = view.get_ptu_pose().position.y
z = 0.0 # float(plan.ID) / 10
p = Point()
p.x = x - int_marker.pose.position.x
p.y = y - int_marker.pose.position.y
p.z = z - int_marker.pose.position.z
line_marker.points.append(p)
line_marker.colors = []
for i, view in enumerate(plan.views):
color = ColorRGBA()
val = float(i) / len(plan.views)
color.r = r_func(val)
color.g = g_func(val)
color.b = b_func(val)
color.a = 1.0
line_marker.colors.append(color)
# create a control which will move the box
# this control does not contain any markers,
# which will cause RViz to insert two arrows
control = InteractiveMarkerControl()
control.markers.append(line_marker)
int_marker.controls.append(control)
return int_marker
def createHead(m_id, action, pose, target_frame):
pose = deepcopy(pose)
scale = Vector3()
scale.x = 0.3
scale.y = 0.3
scale.z = 0.3
color = ColorRGBA()
color.a = 1.0
color.r = 233.0/255.0
color.g = 150.0/255.0
color.b = 122.0/255.0
pose.position.z = 1.6
return createMarker(m_id, Marker.SPHERE, action, pose, scale, color, target_frame)
def get_traj_msg(paths, opt_ind, traj_x, traj_y, maps):
ma = MarkerArray()
for id, path in enumerate(paths):
m = Marker()
m.header.frame_id = "/map"
m.header.stamp = rospy.Time.now()
m.id = (id + 1) * 10
m.type = m.POINTS
m.lifetime.secs = 1
c = ColorRGBA()
if opt_ind == id:
m.scale.x = 0.5
m.scale.y = 0.5
m.scale.z = 0.5
c.r = 0 / 255.0
c.g = 0 / 255.0
c.b = 255 / 255.0
c.a = 1
else:
m.scale.x = 0.2
m.scale.y = 0.2
m.scale.z = 0.2
c.r = 255 / 255.0
c.g = 0 / 255.0
c.b = 0 / 255.0
c.a = 0.2
for x, y in zip(path.x, path.y):
p = Point()
p.x, p.y = x, y
m.points.append(p)
m.colors.append(c)
ma.markers.append(m)
return ma
def createPointLine (marker, d=1.1, n=50.0):
t = 0.0
while t < d:
p = Point()
p.x = t
c = ColorRGBA()
c.r, c.a = 1, 0.5
marker.points.append(p)
marker.colors.append(c)
t += d/n
def get_marker_color(self): # pylint: disable=no-self-use
"""
Virtual (non-abstract) function to get the ROS std_msgs.msg.ColorRGBA used for rviz objects of this Actor
Reimplement this in the derived actor class if ROS visualization messages
(e.g. visualization_msgs.msg.Marker) are sent out and you want a different color than blue.
:return: blue color object
:rtype: std_msgs.msg.ColorRGBA
"""
color = ColorRGBA()
color.r = 0
color.g = 0
color.b = 255
return color
def get_marker_pc_for_track(self, lat_dist, lng_dist, valid_count):
p = Point()
p.x = float(lng_dist)
p.y = -float(lat_dist)
p.z = 0.0
c = ColorRGBA()
c.a = float(valid_count) / RADAR_VALID_MAX
if valid_count > 0:
c.r = 1.0
else:
c.a = 1.0
c.r = c.g = c.b = 0.5
return p, c
def changeColor():
pub = rospy.Publisher('light_controller/command', ColorRGBA, queue_size=1)
rospy.init_node('light_test')
#color in rgb color space ranging from 0 to 1
red = ColorRGBA()
red.r = 1
red.g = 0
red.b = 0
red.a = 1
yellow = ColorRGBA()
yellow.r = 0.4
yellow.g = 1
yellow.b = 0
yellow.a = 1
green = ColorRGBA()
green.r = 0
green.g = 1
green.b = 0
green.a = 1
blue = ColorRGBA()
blue.r = 0
blue.g = 0
blue.b = 1
blue.a = 1
white = ColorRGBA()
white.r = 0.3
white.g = 1
white.b = 0.3
white.a = 1
for color in [red, yellow, green, white, blue, green]:
rospy.loginfo("Setting rgb to %s [%d, %d, %d]", color.r, color.g,
color.b, color.a)
pub.publish(color)
time.sleep(3)
def flag_makers(self):
color = ColorRGBA()
scale = Point()
pose = Pose()
scale.x = 0.01
scale.y = 0.01
scale.z = 0.2
pose = self.pose
pose.position.z += 0.5
color.r = 1.0
color.a = 1.0
self.flag_marker = self.visual_test(pose, Marker.TEXT_VIEW_FACING,
color, scale)
def get_rgba_color_for_markers(color_255):
"""
color_255 is a ColorRGBA object with values between 0 and 255.
Return a ColorRGBA object with values between 0 and 1
"""
ratio = 1.0 / 255.0
color = ColorRGBA()
color.r = color_255.r * ratio
color.g = color_255.g * ratio
color.b = color_255.b * ratio
color.a = 0.8 # a little bit translucent
return color
def __init__(self):
#ROSのパブリッシャなどの初期化
#rospy.init_node('ccaviz', anonymous=True)
self.mpub = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=10)
self.carray = []
clist = [[1, 0, 0, 1], [0, 1, 0, 1], [1, 1, 0, 1], [1, 0.5, 0, 1]]
for c in clist:
color = ColorRGBA()
color.r = c[0]
color.g = c[1]
color.b = c[2]
color.a = c[3]
self.carray.append(color)
def pub_text_marker(self, id, label, r, x, y, z):
marker = Marker()
marker.header.frame_id = 'camera_aligned_depth_to_color_frame'
marker.ns = 'label'
marker.text = label
marker.id = id
marker.type = 9 # text
marker.action = 0
marker.lifetime = rospy.Duration(1)
# Pose
pose = Pose()
pose.position.x = x
pose.position.y = y
pose.position.z = z + 0.05
pose.orientation.w = 1
marker.pose = pose
# Scale
scale = Vector3()
scale.z = 0.035
marker.scale = scale
# Color
color = ColorRGBA()
color.a = 1
if r:
color.r = 0
color.g = 255
color.b = 0
else:
color.r = 255
color.g = 0
color.b = 0
marker.color = color
# Publish marker
self.rviz_pub.publish(marker)
def set_leds(self):
led_msg = FadeRGB()
color = ColorRGBA()
color.r = 0
color.g = 0
color.b = 0
color.a = 0
d = rospy.Duration.from_sec(0)
led_msg = FadeRGB()
led_msg.led_name = 'EarLeds'
led_msg.color = color
led_msg.fade_duration = d
led.publish(led_msg)
def callLedsForStraightMv(self, status):
fColor = ColorRGBA()
sColor = ColorRGBA()
fColor.r = 0
if status == 'STRAIGHT':
fColor.g = 0
fColor.b = 0.9
else:
fColor.g = 0.9
fColor.b = 0
sColor.r = 0
sColor.g = 0.9
sColor.b = 0
ledGroup = LedGroup()
ledGroup.ledMask = 3 # binary mask to decide which leds are active
try:
self.LEDS_FADE(ledGroup, fColor, sColor, rospy.Duration(0.5), True, rospy.Duration(2), 50)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def callLedsForTurningMv(self, side):
fColor = ColorRGBA()
sColor = ColorRGBA()
fColor.r = 0.5
fColor.g = 1.0
fColor.b = 0.5
sColor.r = 0.0
sColor.g = 0.0
sColor.b = 0.0
ledGroup = LedGroup()
if side == 'LEFT':
ledGroup.ledMask = 1 # binary mask to decide which leds are active
else:
ledGroup.ledMask = 2
try:
self.LEDS_BLINK(ledGroup, fColor, sColor, rospy.Duration(0.75), rospy.Duration(0.25), rospy.Duration(5), 50)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def visualise(self, nodes):
if self.visual_debug:
rospy.loginfo('visualising JPS nodes')
color = ColorRGBA()
scale = Point()
scale.x = 0.05
scale.y = 0.05
color.r = 0.0
color.g = 1.0
color.a = 1.0
result = maplib.visual_test(nodes, Marker.POINTS, color, scale, 0)
pub = rospy.Publisher('/JPS_nodes', Marker, queue_size=1)
pub.publish(result)
def __init__(self):
solid_blue = ColorRGBA()
solid_blue.r = 0.0 # Opaque blue
solid_blue.g = 0.0
solid_blue.b = 1.0
solid_blue.a = 1.0
self.seq = 0
self.color = solid_blue
self.bestParticlePose = None
self.pub = rospy.Publisher('/marker', MarkerArray, queue_size=10)
self.r = rospy.Rate(0.1) # 10 Hz, or 10 cycles per second.
def create_plan_marker(self, plan, plan_values):
# create an interactive marker for our server
int_marker = InteractiveMarker()
int_marker.header.frame_id = "/map"
int_marker.name = plan.ID
int_marker.description = plan.ID
pose = Pose()
#pose.position.x = traj.pose[0]['position']['x']
#pose.position.y = traj.pose[0]['position']['y']
int_marker.pose = pose
line_marker = Marker()
line_marker.type = Marker.LINE_STRIP
line_marker.scale.x = 0.1
# random.seed(float(plan.ID))
# val = random.random()
# line_marker.color.r = r_func(val)
# line_marker.color.g = g_func(val)
# line_marker.color.b = b_func(val)
# line_marker.color.a = 1.0
line_marker.points = []
for view in plan.views:
x = view.get_ptu_pose().position.x
y = view.get_ptu_pose().position.y
z = 0.0 # float(plan.ID) / 10
p = Point()
p.x = x - int_marker.pose.position.x
p.y = y - int_marker.pose.position.y
p.z = z - int_marker.pose.position.z
line_marker.points.append(p)
line_marker.colors = []
for i, view in enumerate(plan.views):
color = ColorRGBA()
val = float(i) / len(plan.views)
color.r = r_func(val)
color.g = g_func(val)
color.b = b_func(val)
color.a = 1.0
line_marker.colors.append(color)
# create a control which will move the box
# this control does not contain any markers,
# which will cause RViz to insert two arrows
control = InteractiveMarkerControl()
control.markers.append(line_marker)
int_marker.controls.append(control)
return int_marker
def get_marker_color(self):
"""
Function (override) to return the color for marker messages.
The ego vehicle uses a different marker color than other vehicles.
:return: the color used by a ego vehicle marker
:rtpye : std_msgs.msg.ColorRGBA
"""
color = ColorRGBA()
color.r = 0
color.g = 255
color.b = 0
return color
def createBody(m_id, action, pose, target_frame, color=None):
pose = deepcopy(pose)
scale = Vector3()
scale.x = 0.35
scale.y = 0.35
scale.z = 0.7
if color == None:
color = ColorRGBA()
color.a = 1.0
color.r = 139.0/255.0
color.g = 0.0/255.0
color.b = 0.0/255.0
pose.position.z = 1.1
return createMarker(m_id, Marker.CYLINDER, action, pose, scale, color, target_frame)
def callback(msg):
color = ColorRGBA()
color.a = rospy.get_param('~a', 1.0)
color.r = rospy.get_param('~r', np.random.random())
color.g = rospy.get_param('~g', np.random.random())
color.b = rospy.get_param('~b', np.random.random())
occ_stamped = OccupancyStamped()
occ_stamped.occupancy = msg
occ_stamped.header.frame_id = rospy.get_param('~frame_id', "base_frame")
occ_stamped.header.stamp = rospy.get_rostime()
occ_stamped.scale = rospy.get_param('~scale', 0.01)
pub.publish(occupancyStamped_to_cubelist(occ_stamped, color))
def get_color(color_in, alpha=1.0):
color_out = ColorRGBA()
color_out.a = alpha
color_out.r = 0.5 # Default is gray
color_out.g = 0.5
color_out.b = 0.5
if color_in == ObjectDetection.COLOR_RED:
color_out.r = 1.0
color_out.g = 0.0
color_out.b = 0.0
elif color_in == ObjectDetection.COLOR_GREEN:
color_out.r = 0.0
color_out.g = 1.0
color_out.b = 0.0
elif color_in == ObjectDetection.COLOR_BLUE:
color_out.r = 0.0
color_out.g = 0.0
color_out.b = 1.0
elif color_in == ObjectDetection.COLOR_ORANGE:
color_out.r = 1.0
color_out.g = 0.65
color_out.b = 0.0
return color_out
def pub_test():
#Aqui comeca o programa
rospy.init_node("pub_prof", anonymous=True)
pub = rospy.Publisher("pub_prof", ColorRGBA, queue_size=10)
taxa_pub = rospy.Rate(10)
while not rospy.is_shutdown():
cor = ColorRGBA()
cor.r = 255
cor.g = 255
cor.b = 255
cor.a = 0
pub.publish(cor)
taxa_pub.sleep()
def changeAllColor(self, r = 0.0, g = 0.0, b = 0.0):
if self.__alive().status:
if (r >= 0.0) and (r <= 255.0) and (g >= 0.0) and (g <= 255.0) and (b >= 0.0) and (b <= 255.0):
for i in range(len(self.__leds)):
color = ColorRGBA()
color.r = r
color.g = g
color.b = b
self.__leds[i] = color
return self.__led_service(self.__leds).status
else:
rospy.logerr("Color value must be between 0.0 and 255.0 inclusive")
else:
rospy.logwarn("Wait, connecting to flight controller")
def createGoalMarker(self, currentgoal, marker_container, x, y):
current_point = Point()
current_color = ColorRGBA()
current_color.a = 0.5
current_color.r = 1
current_color.g = 0
current_color.b = 0
current_point.x = currentgoal.pose.position.x
current_point.y = currentgoal.pose.position.y
marker_container.points.append(current_point)
marker_container.colors.append(current_color)
def pub_test():
rospy.init_node("node_pub_luan", anonymous=True)
pub = rospy.Publisher("topic_pub_luan", ColorRGBA, queue_size=10)
taxa_pub = rospy.Rate(10)
while not rospy.is_shutdown():
cor = ColorRGBA()
cor.r = 255
cor.g = 255
cor.b = 255
cor.a = 0
pub.publish(cor)
taxa_pub.sleep()
def line_centre_markers(self):
color = ColorRGBA()
scale = Point()
scale.x = 0.05
scale.y = 0.05
color.r = 0.0
color.g = 1.0
color.b = 0.0
color.a = 0.5
self.line_marker = self.visual_test(self.cut_points, Marker.LINE_LIST,
color, scale) #标记出区域划分（testing）
color = ColorRGBA()
scale = Point()
scale.x = 0.1
scale.y = 0.1
color.r = 0.0
color.g = 0.0
color.b = 1.0
color.a = 1.0
self.centre_points_marker = self.visual_test(self.centre_points,
Marker.POINTS, color,
scale)
def draw_state(self, state, time_step, nominal_model_error=0, action=None):
marker = self.make_marker()
marker.ns = "state_trajectory"
marker.id = time_step
p = Point()
p.x = state[0]
p.y = state[1]
p.z = state[2]
c = ColorRGBA()
c.a = 1
c.r = 0
c.g = 1.0 * max(0, self.max_nom_model_error -
nominal_model_error) / self.max_nom_model_error
c.b = 0
marker.colors.append(c)
marker.points.append(p)
self.marker_pub.publish(marker)
if action is not None:
action_marker = self.make_marker(marker_type=Marker.LINE_LIST)
action_marker.ns = "action"
action_marker.id = 0
action_marker.points.append(p)
p = Point()
p.x = state[0] + action[0] * self.action_scale
p.y = state[1] + action[1] * self.action_scale
p.z = state[2]
action_marker.points.append(p)
c = ColorRGBA()
c.a = 1
c.r = 1
c.g = 0
c.b = 0
action_marker.colors.append(c)
action_marker.colors.append(c)
self.marker_pub.publish(action_marker)
