コード例 #1
0
 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
コード例 #2
0
 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
コード例 #3
0
ファイル: _Markers.py プロジェクト: artur84/catkin_ws
    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
コード例 #4
0
 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
コード例 #5
0
ファイル: voice_and_head.py プロジェクト: artur84/catkin_ws
 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
コード例 #6
0
    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
コード例 #7
0
    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
コード例 #8
0
    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
コード例 #9
0
 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
コード例 #10
0
 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
コード例 #11
0
    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
コード例 #12
0
ファイル: keyboard.py プロジェクト: artur84/catkin_ws
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
コード例 #13
0
ファイル: _Markers.py プロジェクト: artur84/catkin_ws
    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
コード例 #14
0
 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
コード例 #15
0
ファイル: _Markers.py プロジェクト: artur84/catkin_ws
    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
コード例 #16
0
    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
コード例 #17
0
    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
コード例 #18
0
ファイル: _Markers.py プロジェクト: artur84/catkin_ws
 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
コード例 #19
0
 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
コード例 #20
0
    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
コード例 #21
0
    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
コード例 #22
0
ファイル: _Markers.py プロジェクト: artur84/catkin_ws
 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
コード例 #23
0
 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
コード例 #24
0
    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
コード例 #25
0
ファイル: _Markers.py プロジェクト: artur84/catkin_ws
 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
コード例 #26
0
ファイル: _Markers.py プロジェクト: artur84/catkin_ws
 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
コード例 #27
0
 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
コード例 #28
0
    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
コード例 #29
0
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
コード例 #30
0
 def get_del_marker(self):
     marker = Marker()
     marker.action = Marker.DELETE
     marker.id = 1337
     marker.ns = self.object_name
     return marker
コード例 #31
0
 def get_del_marker(self):
     marker = Marker()
     marker.action = Marker.DELETE
     marker.id = self.marker_id
     marker.ns = self.marker_ns
     return marker
コード例 #32
0
 def get_del_marker(self):
     marker = Marker()
     marker.action = Marker.DELETE
     marker.id = 1337
     marker.ns = self.object_name
     return marker
コード例 #33
0
 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)
コード例 #34
0
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)
コード例 #35
0
	def getMarker(self):
		m = Marker()
		m.pose = self.pose
		return m
コード例 #36
0
 def get_del_marker(self):
     marker = Marker()
     marker.action = Marker.DELETE
     marker.id = self.__marker.id
     marker.ns = self.__marker.ns
     return marker
コード例 #37
0
 def getMarker(self):
     m = Marker()
     m.pose = self.pose
     return m