def fill_joy_marker(self, joy_input): """ joy_input: type ROS Odometry() """ marker = Marker() orientation = tf.transformations.quaternion_from_euler( 0, 0, joy_input.twist.twist.angular.z) marker.header.frame_id = joy_input.header.frame_id marker.header.stamp = rospy.Time.now() marker.type = Marker.ARROW marker.pose.orientation.x = orientation[0] marker.pose.orientation.y = orientation[1] marker.pose.orientation.z = orientation[2] marker.pose.orientation.w = orientation[3] marker.scale.x = 2.0 * joy_input.twist.twist.linear.x marker.scale.y = 2.0 marker.scale.z = 2.0 marker.lifetime.from_sec(10.0) marker.color.r = 1.0 marker.color.g = 0.0 marker.color.a = 1.0 marker.pose.position.x = 0.0 marker.pose.position.y = 0.0 marker.pose.position.z = 0.0 return marker
def fill_goal_marker_array(self, goalPoseArray, proba_values, tags, lifetime=0.5): marker_array = MarkerArray() sphere_marker = Marker() text_marker = Marker() tag_marker = Marker() count = 0 for goal in goalPoseArray: text_marker = self.fill_goal_id_text(count, goal, lifetime=lifetime) sphere_marker = self.fill_goal_marker(count, goal, proba_values[count], lifetime=lifetime) tag_marker = self.fill_goal_tag_text(count, goal, tags[count], lifetime=lifetime) marker_array.markers.append(deepcopy(sphere_marker)) marker_array.markers.append(deepcopy(text_marker)) marker_array.markers.append(deepcopy(tag_marker)) count += 1 return marker_array
def fill_vel_marker(self, orientation, vel): """ This function is different to fill_dir_marker because the size of the arrow changes accoring to the linear vel value vel: Twist(), It contains the angular and linear direction orientation: Quaternion() """ marker = Marker() marker.header.frame_id = self.tf_prefix + '/base_link' marker.header.stamp = rospy.Time.now() marker.id = self.vel_marker_id # For zero command, display a ball if [orientation.x, orientation.y, orientation.z, orientation.w] == [0, 0, 0, 0]: marker.type = Marker.SPHERE marker.scale.x = 0.2 marker.scale.y = 0.2 marker.scale.z = 0.2 marker.color.a = 0.1 # For non zero dir we display arrows else: marker.type = Marker.ARROW marker.scale.x = 1.5 * np.abs(vel.linear.x) + 0.4 marker.scale.y = 0.2 marker.scale.z = 0.01 marker.color.a = 0.1 marker.pose.orientation = orientation marker.lifetime = rospy.Duration.from_sec(5.0) marker.color.r = 0.0 marker.color.g = 0.0 marker.color.b = 1.0 marker.pose.position.x = 0.0 marker.pose.position.y = 0.0 marker.pose.position.z = 1.8 self.vel_marker_id += 1 return marker
def fill_command_marker(self, command, tf_prefix): """ command: Twist(), I contains the angular and linear speed of the command that we want to plot in rviz tf_prefix: String, The namespace to be added to the name of the base_link, ex. robot_0 or wheelchair """ marker = Marker() orientation = tf.transformations.quaternion_from_euler( 0, 0, command.angular.z) marker.header.frame_id = tf_prefix + '/base_link' marker.header.stamp = rospy.Time.now() # For zero command, display a ball if command.angular.z == 0 and command.linear.x == 0: marker.type = Marker.SPHERE marker.scale.x = 0.3 marker.scale.y = 0.3 marker.scale.z = 0.3 # For non zero command we display arrows else: marker.type = Marker.ARROW marker.scale.x = 2.0 marker.scale.y = 0.3 marker.scale.z = 0.3 marker.pose.orientation.x = orientation[0] marker.pose.orientation.y = orientation[1] marker.pose.orientation.z = orientation[2] marker.pose.orientation.w = orientation[3] marker.lifetime = rospy.Duration.from_sec(0.3) marker.color.r = 1.0 marker.color.g = 0.0 marker.color.a = 1.0 marker.pose.position.x = 0.0 marker.pose.position.y = 0.0 marker.pose.position.z = 0.0 return marker
def fill_command_marker(self, command, tf_prefix): """ command: Twist(), I contains the angular and linear speed of the command that we want to plot in rviz tf_prefix: String, The namespace to be added to the name of the base_link, ex. robot_0 or wheelchair """ marker = Marker() orientation = tf.transformations.quaternion_from_euler(0, 0, command.angular.z) marker.header.frame_id = tf_prefix + '/base_link' marker.header.stamp = rospy.Time.now() # For zero command, display a ball if command.angular.z == 0 and command.linear.x == 0: marker.type = Marker.SPHERE marker.scale.x = 0.3 marker.scale.y = 0.3 marker.scale.z = 0.3 # For non zero command we display arrows else: marker.type = Marker.ARROW marker.scale.x = 2.0 marker.scale.y = 0.3 marker.scale.z = 0.3 marker.pose.orientation.x = orientation[0] marker.pose.orientation.y = orientation[1] marker.pose.orientation.z = orientation[2] marker.pose.orientation.w = orientation[3] marker.lifetime = rospy.Duration.from_sec(0.3) marker.color.r = 1.0 marker.color.g = 0.0 marker.color.a = 1.0 marker.pose.position.x = 0.0 marker.pose.position.y = 0.0 marker.pose.position.z = 0.0 return marker
def makeBox(self, msg): marker = Marker() marker.type = Marker.SPHERE marker.scale.x = msg.scale * 0.2 marker.scale.y = msg.scale * 0.2 marker.scale.z = msg.scale * 0.2 marker.color.r = 0.5 marker.color.g = 0.5 marker.color.b = 0.5 marker.color.a = 0.5 return marker
def fill_people_marker(self, marker_id): """Fill a marker of a woman marker_id the number of the human """ marker = Marker() frame = "/robot_" + num2strg(marker_id) marker.id = marker_id marker.header.frame_id = frame + '/base_link' # marker.header.frame_id = '/robot_0/base_link' marker.header.stamp = rospy.Time.now() marker.type = Marker.MESH_RESOURCE if marker_id % 2 == 0: marker.mesh_resource = "package://social_filter/meshes/man2/models/man1.dae" marker.scale.x = 40 marker.scale.y = 35 marker.scale.z = 50 marker.pose.position.y += 0.04 quaternion = tf.transformations.quaternion_from_euler(0, 0, 1.82) else: marker.mesh_resource = "package://social_filter/meshes/female1/models/female1.dae" marker.scale.x = 0.45 marker.scale.y = 0.35 marker.scale.z = 0.5 quaternion = tf.transformations.quaternion_from_euler(0, 0, 1.57) marker.pose.orientation.x = quaternion[0] marker.pose.orientation.y = quaternion[1] marker.pose.orientation.z = quaternion[2] marker.pose.orientation.w = quaternion[3] marker.mesh_use_embedded_materials = True marker.lifetime = rospy.Duration.from_sec(1.0) return marker
def get_marker(self): if self.is_initialized(): if self.__marker is None: self.__marker = Marker() self.__marker.id = int( random() * 1000000000) # FIXME find a better way to generate ids self.__marker.ns = 'belief_state' self.__marker.action = Marker.ADD self.__marker.frame_locked = True self.__marker.header.frame_id = self.__transform.header.frame_id self.__marker.pose.position = self.__transform.transform.translation self.__marker.pose.orientation = self.__transform.transform.rotation if self.__mesh_path != '' and self.__mesh_path != "''": self.__marker.type = self.__marker.MESH_RESOURCE self.__marker.mesh_resource = self.__mesh_path.replace( '\'', '') self.__marker.mesh_use_embedded_materials = True self.__marker.scale = Vector3(1, 1, 1) if self.__marker.mesh_resource.endswith('.stl'): # TODO I think we need this special case for stl meshes, try to remove this line if they are buggy self.__marker.color = self.__color else: self.__marker.color = self.__color self.__marker.type = self.__shape self.__marker.scale = self.__scale return self.__marker
def fill_humans_marker_array(self): marker_array = MarkerArray() marker = Marker() for n in range(1, 10): # We will display maximum 10 markers marker = self.fill_people_marker(n) marker_array.markers.append(deepcopy(marker)) return marker_array
def fill_text_marker(self, text): marker = Marker() marker.header.frame_id = self.tf_prefix + '/base_link' marker.header.stamp = rospy.Time.now() marker.type = Marker.TEXT_VIEW_FACING marker.text = text marker.scale.z = 1.0 marker.scale.y = 1.0 marker.scale.x = 1.0 marker.lifetime = rospy.Duration.from_sec(1.0) marker.color.r = 0.0 marker.color.g = 0.0 marker.color.b = 1.0 marker.color.a = 1.0 marker.pose.position.x = 1.0 marker.pose.position.y = 1.0 marker.pose.position.z = 1.0 return marker
def make_sphere(self, msg): """ :param msg: :return: :rtype: Marker """ marker = Marker() marker.type = Marker.SPHERE marker.scale.x = msg.scale * MARKER_SCALE * 2 marker.scale.y = msg.scale * MARKER_SCALE * 2 marker.scale.z = msg.scale * MARKER_SCALE * 2 marker.color.r = 0.5 marker.color.g = 0.5 marker.color.b = 0.5 marker.color.a = 0.5 return marker
def fill_text_marker(text, tf_prefix): marker = Marker() marker.header.frame_id = tf_prefix + '/base_link' marker.header.stamp = rospy.Time.now() marker.type = Marker.TEXT_VIEW_FACING marker.text = text marker.scale.z = 1.0 marker.scale.y = 1.0 marker.scale.x = 1.0 marker.lifetime = rospy.Duration.from_sec(1.0) marker.color.r = 0.0 marker.color.g = 0.0 marker.color.b = 1.0 marker.color.a = 1.0 marker.pose.position.x = 1.0 marker.pose.position.y = 1.0 marker.pose.position.z = 1.0 return marker
def fill_dir_marker(self, orientation, discrete_mode_flag): """ This function is different to fill_vel_marker in which the size of the arrow is constant @param orientation: ROS Quaternion(), I contains the angular and linear direction @param discrete_mode_flag: bool, To change the markers to specifically show in discrete mode and continuous mode. """ marker = Marker() marker.id = self.dir_marker_id marker.header.frame_id = self.tf_prefix + '/base_link' marker.header.stamp = rospy.Time.now() #In continuous mode we don't like to print the markers very strong. if discrete_mode_flag: marker.color.a = 0.8 number_of_arrows_to_show = 1 else: marker.color.a = 0.1 number_of_arrows_to_show = 30 # For zero command, display a ball if [orientation.x, orientation.y, orientation.z, orientation.w] == [0, 0, 0, 0]: marker.type = Marker.SPHERE marker.scale.x = 0.3 marker.scale.y = 0.3 marker.scale.z = 0.3 # For non zero command we display arrows else: marker.type = Marker.ARROW marker.scale.x = 0.9 marker.scale.y = 0.15 marker.scale.z = 0.15 marker.pose.orientation = orientation marker.lifetime = rospy.Duration.from_sec(60.0) marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 marker.pose.position.x = 0.0 marker.pose.position.y = 0.0 marker.pose.position.z = 1.7 self.dir_marker_id += 1 if self.dir_marker_id == number_of_arrows_to_show: #To show up to this number of arrows self.dir_marker_id = 0 return marker
def get_marker(self): marker = Marker() marker.header.frame_id = self.ref_frame marker.type = marker.MESH_RESOURCE marker.action = Marker.ADD marker.id = 1337 marker.ns = self.get_short_name() marker.color = self.color marker.scale.x = 1 marker.scale.y = 1 marker.scale.z = 1 marker.frame_locked = True marker.pose.position = Point(*self.transform[-2]) marker.pose.orientation = Quaternion(*self.transform[-1]) marker.mesh_resource = self.mesh_path[:-4] + '.dae' return marker
def fill_text_marker(self, text, lifetime=1.0, r=0.0, g=0.0, b=1.0): marker = Marker() marker.id = self.text_marker_id #self.text_marker_id += 1 marker.header.frame_id = self.tf_prefix + '/base_link' marker.header.stamp = rospy.Time.now() marker.type = Marker.TEXT_VIEW_FACING marker.text = text marker.scale.z = 1.0 marker.lifetime = rospy.Duration.from_sec(lifetime) marker.color.r = r marker.color.g = g marker.color.b = b marker.color.a = 1.0 marker.pose.position.x = 0.0 marker.pose.position.y = 1.0 marker.pose.position.z = 2.0 return marker
def get_marker(self): marker = Marker() marker.header.frame_id = self.ref_frame marker.type = marker.MESH_RESOURCE marker.action = Marker.ADD marker.id = 1337 marker.ns = self.object_name marker.color = self.color marker.scale.x = 1 marker.scale.y = 1 marker.scale.z = 1 marker.frame_locked = True marker.pose.position = Point(*self.transform[-2]) marker.pose.orientation = Quaternion(*self.transform[-1]) if len(self.mesh_path) > 2 and self.mesh_path[0] == "'": marker.mesh_resource = self.mesh_path[1:-1] else: marker.mesh_resource = self.mesh_path return marker
def get_marker(self): marker = Marker() marker.header.frame_id = self.ref_frame marker.type = marker.MESH_RESOURCE marker.action = Marker.ADD marker.id = 1337 marker.ns = self.object_name marker.color = self.color marker.scale.x = 1 marker.scale.y = 1 marker.scale.z = 1 marker.frame_locked = True marker.pose.position = Point(*self.transform[-2]) marker.pose.orientation = Quaternion(*self.transform[-1]) if len(self.mesh_path)>2 and self.mesh_path[0] == "'": marker.mesh_resource = self.mesh_path[1:-1] else: marker.mesh_resource = self.mesh_path return marker
def fill_joy_marker(self, joy_input): """ joy_input: type ROS Odometry() """ marker = Marker() orientation = tf.transformations.quaternion_from_euler(0, 0, joy_input.twist.twist.angular.z) marker.header.frame_id = joy_input.header.frame_id marker.header.stamp = rospy.Time.now() marker.type = Marker.ARROW marker.pose.orientation.x = orientation[0] marker.pose.orientation.y = orientation[1] marker.pose.orientation.z = orientation[2] marker.pose.orientation.w = orientation[3] marker.scale.x = 2.0 * joy_input.twist.twist.linear.x marker.scale.y = 2.0 marker.scale.z = 2.0 marker.lifetime.from_sec(10.0) marker.color.r = 1.0 marker.color.g = 0.0 marker.color.a = 1.0 marker.pose.position.x = 0.0 marker.pose.position.y = 0.0 marker.pose.position.z = 0.0 return marker
def fill_goal_marker(self, marker_id, goal, value, lifetime=0.5): marker = Marker() marker.ns = "sphere" marker.lifetime = rospy.Duration.from_sec(lifetime) marker.id = marker_id marker.header.frame_id = self.tf_prefix + '/map' marker.header.stamp = rospy.Time.now() marker.type = Marker.SPHERE marker.text = str(round(value, 4)) marker.pose = goal.pose marker.pose.position.z = 1.5 * value + 0.1 marker.scale.x = 3.0 * value + 0.1 marker.scale.y = 3.0 * value + 0.1 marker.scale.z = 3.0 * value + 0.1 marker.color.a = 0.5 marker.color.r = value * 6 marker.color.g = 0.0 marker.color.b = 1 - value * 6 return marker
def fill_wheelchair_marker(self): marker = Marker() marker.header.frame_id = self.tf_prefix + '/base_link' # marker.header.frame_id = '/robot_0/base_link' marker.header.stamp = rospy.Time.now() marker.id = 1 marker.type = Marker.MESH_RESOURCE marker.mesh_resource = "package://social_filter/meshes/wheelchair/Wheelchair1.dae" marker.mesh_use_embedded_materials = True marker.pose.position.x = -0.2 marker.scale.x = 1.7 marker.scale.y = 1.1 marker.scale.z = 1.2 marker.lifetime = rospy.Duration.from_sec(1.0) return marker
def fill_sit_human_marker(self): marker = Marker() marker.header.frame_id = self.tf_prefix + '/base_link' marker.header.stamp = rospy.Time.now() marker.id = 2 marker.type = Marker.MESH_RESOURCE marker.mesh_resource = "package://social_filter/meshes/human/3D_Man_Seated.dae" marker.mesh_use_embedded_materials = True marker.scale.x = .025 marker.scale.y = .025 marker.scale.z = .025 marker.pose.position.x -= 0.2 marker.pose.position.z += .3 marker.lifetime = rospy.Duration.from_sec(1.0) return marker
def fill_goal_id_text(self, marker_id, goal, text="i=", lifetime=0.5): marker = Marker() marker.ns = "index" marker.lifetime = rospy.Duration.from_sec(lifetime) marker.id = marker_id marker.header.frame_id = self.tf_prefix + '/map' marker.header.stamp = rospy.Time.now() marker.type = Marker.TEXT_VIEW_FACING marker.text = text + str(marker_id + 1) # index should start in one marker.pose.position.x = goal.pose.position.x marker.pose.position.y = goal.pose.position.y marker.pose.position.z = 1.0 marker.scale.x = 0.6 marker.scale.y = 0.6 marker.scale.z = 0.6 marker.color.a = 0.9 marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 return marker
def fill_goal_tag_text(self, marker_id, goal, tag_string="tag", lifetime=0.5): marker = Marker() marker.ns = "tag" marker.lifetime = rospy.Duration.from_sec(lifetime) marker.id = marker_id marker.header.frame_id = self.tf_prefix + '/map' marker.header.stamp = rospy.Time.now() marker.type = Marker.TEXT_VIEW_FACING marker.text = tag_string marker.pose.position.x = goal.pose.position.x marker.pose.position.y = goal.pose.position.y + 0.6 marker.pose.position.z = 2.0 marker.scale.x = 0.5 marker.scale.y = 0.5 marker.scale.z = 0.5 marker.color.a = 0.9 marker.color.r = 0.0 marker.color.g = 0.3 marker.color.b = 0.0 return marker
def displayWaypoints(w_array): global marker_pub global map #delete old markers for i in range(2,2+displayWaypoints.prevWaypoints): marker = Marker() marker.ns = "lab3markers" marker.id = i marker.action = Marker.DELETE marker.header.frame_id = "map" marker.type = Marker.ARROW marker_pub.publish(marker) #publish new markers for i in range(1,len(w_array)): marker = Marker() marker.header.frame_id = "map" marker.type = Marker.ARROW marker.header.stamp = rospy.Time.now() marker.ns = "lab3markers" marker.id = 2+i-1 marker.color.r = 1 marker.color.a = 1 marker.action = Marker.ADD marker.scale.x = map.info.resolution/3 marker.scale.y = 2*map.info.resolution/3 marker.points = [] x1 = (w_array[i-1].x + map.info.width+0.5)*map.info.resolution + map.info.origin.position.x y1 = -((w_array[i-1].y+0.5)*map.info.resolution - map.info.origin.position.y) x2 = (w_array[i].x + map.info.width+0.5)*map.info.resolution + map.info.origin.position.x y2 = -((w_array[i].y+0.5)*map.info.resolution - map.info.origin.position.y) marker.points.append(Point(x1, y1, 0)) marker.points.append(Point(x2, y2, 0)) marker_pub.publish(marker) displayWaypoints.prevWaypoints = i
def get_del_marker(self): marker = Marker() marker.action = Marker.DELETE marker.id = 1337 marker.ns = self.object_name return marker
def get_del_marker(self): marker = Marker() marker.action = Marker.DELETE marker.id = self.marker_id marker.ns = self.marker_ns return marker
def pub_goal_marker(self, header, pose): """ :param header: :type header: std_msgs.msg._Header.Header :param pose: :type pose: Pose """ ma = MarkerArray() m = Marker() m.action = Marker.ADD m.type = Marker.CYLINDER m.header = header old_q = [ pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w ] # x m.pose = deepcopy(pose) m.scale.x = 0.05 * MARKER_SCALE m.scale.y = 0.05 * MARKER_SCALE m.scale.z = MARKER_SCALE muh = qv_mult(old_q, [m.scale.z / 2, 0, 0]) m.pose.position.x += muh[0] m.pose.position.y += muh[1] m.pose.position.z += muh[2] m.pose.orientation = Quaternion(*quaternion_multiply( old_q, quaternion_about_axis(np.pi / 2, [0, 1, 0]))) m.color.r = 1 m.color.g = 0 m.color.b = 0 m.color.a = 1 m.ns = u'interactive_marker_{}_{}'.format(self.root_link, self.tip_link) m.id = 0 ma.markers.append(m) # y m = deepcopy(m) m.pose = deepcopy(pose) muh = qv_mult(old_q, [0, m.scale.z / 2, 0]) m.pose.position.x += muh[0] m.pose.position.y += muh[1] m.pose.position.z += muh[2] m.pose.orientation = Quaternion(*quaternion_multiply( old_q, quaternion_about_axis(-np.pi / 2, [1, 0, 0]))) m.color.r = 0 m.color.g = 1 m.color.b = 0 m.color.a = 1 m.ns = u'interactive_marker_{}_{}'.format(self.root_link, self.tip_link) m.id = 1 ma.markers.append(m) # z m = deepcopy(m) m.pose = deepcopy(pose) muh = qv_mult(old_q, [0, 0, m.scale.z / 2]) m.pose.position.x += muh[0] m.pose.position.y += muh[1] m.pose.position.z += muh[2] m.color.r = 0 m.color.g = 0 m.color.b = 1 m.color.a = 1 m.ns = u'interactive_marker_{}_{}'.format(self.root_link, self.tip_link) m.id = 2 ma.markers.append(m) self.marker_pub.publish(ma)
def publish_marker(ean): global marker_pub, barcode_to_object if marker_pub is None: marker_pub = rospy.Publisher('shelf_objects', Marker, queue_size=100) m = Marker() text = Marker() text.header.frame_id = ean m.header.frame_id = ean m.pose.orientation.w = 1 m.action = Marker.ADD text.action = Marker.ADD text.type = Marker.TEXT_VIEW_FACING text.text = ean text.scale = Vector3(0,0,.05) text.color = ColorRGBA(1,1,1,1) text.pose.position.z = 0.05 if ean in barcode_to_object and barcode_to_object[ean].split('.')[-1]=='dae': m.pose.orientation = Quaternion(*quaternion_from_euler(0,0,np.pi/2)) m.type = Marker.MESH_RESOURCE m.mesh_resource = refills_models_path + barcode_to_object[ean] m.scale = Vector3(1,1,1) m.color = ColorRGBA(0,0,0,0) m.mesh_use_embedded_materials = True else: m.type = Marker.CUBE m.scale = Vector3(.05,.05,.05) m.color = ColorRGBA(.5,.5,.5,1.0) m.text = ean m.ns = ean text.ns = ean m.id = 2 marker_pub.publish(m) marker_pub.publish(text)
def getMarker(self): m = Marker() m.pose = self.pose return m
def get_del_marker(self): marker = Marker() marker.action = Marker.DELETE marker.id = self.__marker.id marker.ns = self.__marker.ns return marker