def get_traj_msg(paths, opt_ind): 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 compute_rgb(data): # print(data) color = ColorRGBA() if data < 1.0 / 3: color.r = 1.0 color.g = 3.0 * data color.b = 0.0 if data >= 1.0 / 3 and data < 1.0 / 2: color.r = 3 - 6.0 * data color.g = 1.0 color.b = 0.0 if data >= 1.0 / 2 and data < 2.0 / 3: color.r = 0.0 color.g = 1.0 color.b = 6.0 * data - 3 if data >= 2.0 / 3 and data < 5.0 / 6: color.r = 0.0 color.g = 5 - 6.0 * data color.b = 1.0 if data >= 5.0 / 6 and data <= 1.0: color.r = (900.0 * data - 750) / 255.0 color.g = 0.0 color.b = 1.0 color.a = 1.0 return color
def switch_lights_parser(self, lights_dict): """ Parse and use action to switch lights Parameters ---------- lights_dict: dictionary of light switching parameters """ light_name = lights_dict["target_light"] diffuse = ColorRGBA() if lights_dict["on"] == True: diffuse.r = 0.5 diffuse.g = 0.5 diffuse.b = 0.5 diffuse.a = 1.0 attenuation_linear = 0.01 attenuation_quadratic = 0.001 elif lights_dict["on"] == False: diffuse.r = 0.0 diffuse.g = 0.0 diffuse.b = 0.0 diffuse.a = 0.0 attenuation_linear = 0.00 attenuation_quadratic = 0.0000 else: rospy.logwarn("invalid light on state parsed") return self.set_light_properties(light_name, diffuse, 0.0, attenuation_linear, attenuation_quadratic) return
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 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 Colormap(self, ii, jj): p = self._visualMap[ii, jj] c = ColorRGBA() if p == 2: c.r = 0.1 c.g = 1.0 c.b = 0.1 c.a = 0.75 elif p == 3: c.r = 1.0 c.g = 1.0 c.b = 0.1 c.a = 0.75 elif p == 4: c.r = 1.0 c.g = 1.0 c.b = 1.0 c.a = 0.75 elif p == 5: c.r = 1.0 c.g = 0.1 c.b = 1.0 c.a = 0.75 else: c.r = p c.g = 0.1 c.b = 1.0 - p c.a = 0.3 return c
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 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 get_color(self, weight, weights): color = ColorRGBA(0, 0, 0, 1) if max(weights) != min(weights): normalized_weight = round( (weight - min(weights)) / (max(weights) - min(weights)), 5) color.r = 1 - normalized_weight color.g = normalized_weight else: color.g = 1.0 #rospy.logwarn("normalized_weight is:{} - {} / {} - {} = {}".format(weight, min(weights), max(weights), min(weights), normalized_weight)) return color
def onBumper(data): rgb = ColorRGBA() byte = int(data.data) if byte & 1: rgb.r = 255 if byte & 2: rgb.g = 255 if byte & 4: rgb.b = 255 if byte & 8: rgb.r = 255 rgb.g = 255 rgb.b = 255 rgbPub.publish(rgb)
def get_color_discrete(self, signal_strength): color = ColorRGBA(0, 0, 0, 1) high_threshold = -55.0 low_threshold = -65.0 if signal_strength > high_threshold: color.g = 1 elif signal_strength < high_threshold and signal_strength > low_threshold: color.g = 0.5 color.r = 0.5 elif signal_strength < low_threshold: color.r = 1.0 return color
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 getLineColor(self): result = ColorRGBA() result.a = 1 result.r = 0.8 result.g = 0.1 result.b = 0.1 return result
def pub_color(self, r, g, b, a): color_msg = ColorRGBA() color_msg.r = r color_msg.g = g color_msg.b = b color_msg.a = a self.color_pub.publish(color_msg)
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 perform(self, reevaluate=False): pose_msg = self.blackboard.pathfinding.get_ball_goal( self.target, self.distance) self.blackboard.pathfinding.publish(pose_msg) approach_marker = Marker() approach_marker.pose.position.x = self.distance approach_marker.type = Marker.SPHERE approach_marker.action = Marker.MODIFY approach_marker.id = 1 color = ColorRGBA() color.r = 1.0 color.g = 1.0 color.b = 1.0 color.a = 1.0 approach_marker.color = color approach_marker.lifetime = rospy.Duration(nsecs=0.5) scale = Vector3(0.2, 0.2, 0.2) approach_marker.scale = scale approach_marker.header.stamp = rospy.Time.now() approach_marker.header.frame_id = self.blackboard.world_model.base_footprint_frame self.blackboard.pathfinding.approach_marker_pub.publish( approach_marker) if self.blackboard.pathfinding.status in [ GoalStatus.SUCCEEDED, GoalStatus.ABORTED ] or not self.blocking: self.pop()
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() #クラス間のデータ渡しテスト 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 meshToRos(mesh): triangles = np.asarray(mesh.triangles) vertices = np.asarray(mesh.vertices) vertex_colors = np.asarray(mesh.vertex_colors) out_msg = Marker() out_msg.type = out_msg.TRIANGLE_LIST out_msg.action = out_msg.ADD out_msg.id = 1 out_msg.scale.x = 1.0 out_msg.scale.y = 1.0 out_msg.scale.z = 1.0 out_msg.pose.position.x = 0 out_msg.pose.position.y = 0 out_msg.pose.position.z = 0 out_msg.pose.orientation.w = 1 out_msg.pose.orientation.x = 0 out_msg.pose.orientation.y = 0 out_msg.pose.orientation.z = 0 for triangle in triangles: for vertex_index in triangle: curr_point = Point() curr_point.x = vertices[vertex_index][0] curr_point.y = vertices[vertex_index][1] curr_point.z = vertices[vertex_index][2] curr_point_color = ColorRGBA() curr_point_color.r = vertex_colors[vertex_index][2] curr_point_color.g = vertex_colors[vertex_index][1] curr_point_color.b = vertex_colors[vertex_index][0] curr_point_color.a = 1 out_msg.points.append(curr_point) out_msg.colors.append(curr_point_color) return out_msg
def adddisplay(points): pub = rospy.Publisher("visualization_marker", Marker, queue_size=10) marker = Marker() marker.header.frame_id = "/world" marker.type = marker.POINTS marker.action = marker.ADD marker.id = 1 marker.scale.x = 0.25 marker.scale.y = 0.25 marker.scale.z = 0.25 result_v = addVoronoi(points) result = result_v storepoint=[] for i in range(len(result)): for j in range(len(result[i])): point = Point() color = ColorRGBA() color.b = 1 color.g = 1 color.r = 0 color.a = 1 point.x = result[i][j,0] point.y = result[i][j,1] point.z = result[i][j,2] if point.z >= 0: marker.points.append(point) marker.colors.append(color) storepoint.append(point) pub.publish(marker) return storepoint
def color(r,g,b,a=1): out = ColorRGBAMsg() out.r = r out.g = g out.b = b out.a = a return out
def pixel_to_color(x, y, image): color = ColorRGBA() color.r = image[x][y][0] color.g = image[x][y][1] color.b = image[x][y][2] color.a = 1.0 return color
def changeColor(self): command = ColorRGBA() command.r = int(self.redBox.checkState()) command.g = int(self.greenBox.checkState()) command.b = int(self.blueBox.checkState()) command.a = 0 self.pub2.publish(command)
def make_edge_marker(self): self.edge_marker = Marker() self.edge_marker.header.frame_id = self.map_data['MAP_FRAME'] self.edge_marker.header.stamp = rospy.get_rostime() self.edge_marker.id = 0 self.edge_marker.action = Marker().ADD self.edge_marker.type = Marker().LINE_LIST self.edge_marker.lifetime = rospy.Duration() self.edge_marker.pose.orientation = Quaternion(w=1, x=0, y=0, z=0) self.edge_marker.scale.x = 0.8 self.edge_marker.ns = "edge" self.edge_marker.frame_locked = True for edge in self.map_data['EDGE']: p0 = Point() p0.x = self.map_data['NODE'][self.get_index_from_id( edge['node_id'][0])]['point']['x'] p0.y = self.map_data['NODE'][self.get_index_from_id( edge['node_id'][0])]['point']['y'] p1 = Point() p1.x = self.map_data['NODE'][self.get_index_from_id( edge['node_id'][1])]['point']['x'] p1.y = self.map_data['NODE'][self.get_index_from_id( edge['node_id'][1])]['point']['y'] self.edge_marker.points.append(p0) self.edge_marker.points.append(p1) color = ColorRGBA() color.r = 0.0 color.g = 0.0 color.b = 1.0 color.a = 0.7 self.edge_marker.colors.append(color) self.edge_marker.colors.append(color)
def createMarkerVolume(name, points=[], color=[1.0, 1.0, 1.0], map_resolution=0.01, base_frame='world'): marker = Marker() marker.header.frame_id = base_frame marker.type = Marker.CUBE_LIST marker.id = 0 marker.action = marker.ADD marker.ns = name marker.scale.x = map_resolution marker.scale.y = map_resolution marker.scale.z = map_resolution for p in points: pp = Point() pp.x = p[0] pp.y = p[1] pp.z = p[2] marker.points.append(pp) cc = ColorRGBA() cc.r = color[0] cc.g = color[1] cc.b = color[2] cc.a = 1.0 marker.colors.append(cc) return marker
def __init__(self, node): self._node = node self._node.get_logger().info('Init ObjectMarker()') # Initial default marker properties self.marker = Marker() self.marker.color.r = 0.0 self.marker.color.g = 1.0 self.marker.color.b = 0.0 self.marker.color.a = 1.0 self.marker.scale.x = 0.0 self.marker.scale.y = 0.0 self.marker.scale.z = 0.0 line_color = ColorRGBA() line_color.r = 1.0 line_color.g = 1.0 line_color.b = 0.0 line_color.a = 1.0 self.marker.colors.append(line_color) self.marker.colors.append(line_color) # Marker topic on Rviz self._markers_pub = self._node.create_publisher( MarkerArray, '/object_map/Markers')
def __init__(self, parent=None, width=5, height=5, dpi=100): #pl.ion() self.fig = Figure(figsize=(width, height), dpi=dpi) self.canvas = FigureCanvas(self.fig) self.canvas.setParent(parent) self.canvas.mpl_connect('button_press_event', self.on_click) self.rho_area = self.fig.add_subplot(111) #self.rho_area = self.fig.add_subplot(211) self.rho_area.set_title("rho", fontsize=11) self.fig.tight_layout() #ROS rospy.init_node('cor_joints', anonymous=True) self.mpub = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=10) #rvizのカラー設定(未) 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) self.jidx = [ [11, 3, 2, 10, 1, 0], [10, 4, 5, 6], [10, 7, 8, 9], ]
def goal_parts_cb(self, msg: PoseWithCertaintyArray): arr = [] i = 0 for post in msg.poses: post_marker = Marker() pose = Pose() pose.position = post.pose.pose.position post_marker.pose = pose post_marker.pose.position.z = self.post_height / 2 post_marker.pose.orientation = Quaternion() post_marker.pose.orientation.x = 0 post_marker.pose.orientation.y = 0 post_marker.pose.orientation.z = 0 post_marker.pose.orientation.w = 1 post_marker.type = Marker.CYLINDER post_marker.action = Marker.MODIFY post_marker.id = i post_marker.ns = "rel_goal_parts" color = ColorRGBA() color.r = 1.0 color.g = 1.0 color.b = 1.0 color.a = 1.0 post_marker.color = color post_marker.lifetime = rospy.Duration(nsecs=self.goal_lifetime) scale = Vector3(self.post_diameter, self.post_diameter, self.post_height) post_marker.scale = scale post_marker.header = msg.header arr.append(post_marker) i += 1 self.goal_parts_marker.markers = arr self.marker_array_publisher.publish(self.goal_parts_marker)
def gen_color(r, g, b): c = ColorRGBA() c.r = float(r) c.g = float(g) c.b = float(b) c.a = 1.0 return c
def NewColor(r, g, b, a=1.0): col = ColorRGBA() col.r = r col.g = g col.b = b col.a = a return col
def draw_footprint(self, points): # print 'PRINT ARIS FOOTPRINT' marker = Marker() marker.header.stamp = rospy.Time.now() marker.header.frame_id = "/girona500" marker.ns = "aris_foot_print" marker.id = 1 marker.type = 4 # SPHERE marker.action = 0 # Add/Modify an object marker.pose.position.x = 0 marker.pose.position.y = 0 marker.pose.position.z = 0 marker.scale.x = 0.1 marker.scale.y = 0.1 marker.scale.z = 0.1 marker.color.r = 0.0 for i in points: p = Point() p.x = i[0] p.y = i[1] p.z = i[2] color = ColorRGBA() color.r = 0.0 color.g = 0.5 color.b = 0.5 color.a = 0.7 marker.points.append(p) marker.colors.append(color) marker.lifetime = rospy.Duration(0.5) marker.frame_locked = False self.pub_aris_footprint.publish(marker)
def findNextGoalPt(self, speed, angle=0.0): """ Finds the point on the subsampled trajectory that is closest to the desired lookahead distance (lookahead is proportional to speed), and returns that as a local goal. """ lookahead = self.getLookaheadDist(speed) rospy.loginfo('Lookahead Distance: %f' % lookahead) self.curr_traj_goal_pt = self.findPtAtDist(lookahead) self.goal_pose = self.traj_pts[self.curr_traj_goal_pt] if (self.curr_traj_goal_pt >= len(self.traj_pts) - 5): self.seeking_end_pt = True if self.end_time == 0.0: self.end_time = rospy.get_time() for p in self.robot_path: pt = Point32() pt.x = p[0] pt.y = p[1] pt.z = 0.0 color = ColorRGBA() color.r = 1.0 - p[2] color.g = p[2] color.b = 0.0 color.a = 0.75 self.robot_path_marker.points.append(pt) self.robot_path_marker.colors.append(color) self.robot_path_pub.publish(self.robot_path_marker) return
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 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 show_text_in_rviz_mullti_line(marker_pub, text): line_color = ColorRGBA() # a nice color for my line (royalblue) line_color.r = 0.254902 line_color.g = 0.411765 line_color.b = 0.882353 line_color.a = 1.0 start_point = Point() # start point start_point.x = 0.2 start_point.y = 0.0 start_point.z = 0.2 end_point = Point() # end point end_point.x = 20 end_point.y = 20 end_point.z = 20 marker3 = Marker() marker3.id = 3 marker3.header.frame_id = 'world' marker3.type = Marker.LINE_STRIP marker3.ns = 'Testline' marker3.action = Marker.ADD marker3.scale.x = 0.8 marker3.points.append(start_point) marker3.points.append(end_point) marker3.colors.append(line_color) marker3.colors.append(line_color) marker_pub.publish(marker3)
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 PubcmdCB(self, event): robot_orien_pub = rospy.Publisher("/robot_orientation", Marker, queue_size=1) color = ColorRGBA() scale = Point() scale.x = 0.05 scale.y = 0.05 scale.z = 0.05 color.r = 2.0 color.g = 0.0 color.b = 0.0 color.a = 1.0 point_marker = Marker() point_marker.header.frame_id = '/map' point_marker.header.stamp = rospy.Time.now() point_marker.ns = 'robot_uni_marker' point_marker.action = Marker.ADD point_marker.id = 0 point_marker.type = Marker.ARROW point_marker.scale.x = scale.x * 5 point_marker.scale.y = scale.y / 2 point_marker.scale.z = scale.z / 2 point_marker.pose = self.marker.pose point_marker.pose.position.z = self.height + 0.1 point_marker.color = color point_marker.lifetime = rospy.Duration(0.1) robot_orien_pub.publish(point_marker)
def make_color_msg(self, rgba): color = ColorRGBA() color.r = rgba[0] color.g = rgba[1] color.b = rgba[2] color.a = 1 if len(rgba) < 4 else rgba[3] return 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 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 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 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 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 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 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 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 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 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 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 __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 createPointsInMarker(self, data): # track_no_idx = 0 # for i in range(0,768,12): for track_no_idx in self.track_no_idxs: # if not self.rangerate[track_no_idx] == 81.91: point = Point() dist = self.dist [track_no_idx] #data.data[i+9] * .1 angle = self.angle[track_no_idx] #data.data[i+11]* .1 angle_rad = math.radians(angle) point.x = - dist * math.sin(angle_rad) point.y = - dist * math.cos(angle_rad) point.z = 0 color = ColorRGBA() if self.movingflags[track_no_idx]: color.r = 1.0 color.g = 0.0 color.b = 0.0 else: color.r = 0.0 color.g = 1.0 color.b = 0.0 color.a = 1.0 self.marker.points.append( point) self.marker.colors.append( color)
def talker(): lpub = rospy.Publisher('left_eye', ColorRGBA) rpub = rospy.Publisher('right_eye', ColorRGBA) rospy.init_node('led_tester') while not rospy.is_shutdown(): t = rospy.get_rostime().to_sec() k = 4.0 msg = ColorRGBA() msg.r = ( 1.0 + sin(0.0 + (k*t)) ) / 2.0 msg.g = ( 1.0 + sin(2.1 + (k*t)) ) / 2.0 msg.b = ( 1.0 + sin(4.2 + (k*t)) ) / 2.0 pub.publish(msg) rospy.sleep(.1)
def setLightCyanCircle_cb(req): rospy.wait_for_service('/light_torso/set_light') try: set_light_torso = rospy.ServiceProxy("/light_torso/set_light",SetLightMode) light_mode = LightMode() cyan_color = ColorRGBA() cyan_color.r = 0.0 cyan_color.g = 1.0 cyan_color.b = 0.5 cyan_color.a = 0.4 light_mode.colors.append(cyan_color) light_mode.mode = 7 resp = set_light_torso(light_mode) print resp return except rospy.ServiceException, e: print "Service call failed: %s"%e
def __init__(self): self.jsonInput() super(CCA, self).__init__() self.initUI() rospy.init_node('ros_cca', anonymous=True) self.mpub = rospy.Publisher('visualization_marker_array', MarkerArray, 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): #ROSのパブリッシャなどの初期化 #rospy.init_node('ccaviz', anonymous=True) fname = "/home/uema/catkin_ws/src/pose_cca/datas/20160520_a2.json" poses1, poses2, dts, dtd = self.json_pose_input(fname) self.poses1, self.poses2 = self.select_datas(poses1, poses2) #print poses1.shape 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)
import threading from std_msgs.msg import Float64, ColorRGBA from geometry_msgs.msg import Point from racecar.msg import BlobDetections global colors, color_dims, color_map colors = ["red", "yellow", "green"] color_dims = {"red":("r", 150, 1.7, 2.5),\ "yellow": ("g",150, .6, 2.2),\ "green": ("g", 60, 1.5, 1.5)} redB = ColorRGBA() redB.r = 255 yellowB = ColorRGBA() yellowB.r = 255 yellowB.g = 255 greenB = ColorRGBA() greenB.g = 255 color_map = {"red": redB,"yellow":yellowB,"green":greenB} #find all the blobs in the thresholded image def blobdetection(img, cond1, cond2, col): img2, contours, hierarchy = cv2.findContours(img.copy(), mode = cv2.RETR_CCOMP, method=cv2.CHAIN_APPROX_SIMPLE)#change later for cv Boxes = [] ImgBox = [] for co in contours: rect = cv2.minAreaRect(co) box = cv2.cv.BoxPoints(rect) box = np.int0(box) center = rect[0] (w,h) = rect[1]