visualize_navigation_goals.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
00004 #
00005 # Licensed under the Apache License, Version 2.0 (the "License");
00006 # you may not use this file except in compliance with the License.
00007 # You may obtain a copy of the License at
00008 #
00009 #   http://www.apache.org/licenses/LICENSE-2.0
00010 #
00011 # Unless required by applicable law or agreed to in writing, software
00012 # distributed under the License is distributed on an "AS IS" BASIS,
00013 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00014 # See the License for the specific language governing permissions and
00015 # limitations under the License.
00016 
00017 
00018 import sys
00019 import rospy
00020 import tf
00021 from visualization_msgs.msg import Marker
00022 from visualization_msgs.msg import MarkerArray
00023 from dynamic_reconfigure.server import Server
00024 from cob_helper_tools.cfg import HelperToolsConfig
00025 
00026 class VisualizerNavigationGoals():
00027     def __init__(self):
00028         self.text_size = 0.5
00029         self.srv = Server(HelperToolsConfig, self.reconfigure_callback)
00030         self.pubGoals = rospy.Publisher('visualize_navigation_goals', MarkerArray, queue_size=1, latch=True)
00031 
00032     def reconfigure_callback(self, config, level):
00033         self.text_size = config.text_size
00034         return config
00035 
00036     def pubMarker(self):
00037         navigation_goals = rospy.get_param("/script_server/base", {})
00038 
00039         markerarray = MarkerArray()
00040         i=0
00041         for name, pose in navigation_goals.items():
00042 
00043             # check if pose is valid
00044             if len(pose) != 3:
00045                 continue
00046             
00047             # arrow
00048             marker_arrow = Marker()
00049             marker_arrow.header.stamp = rospy.Time.now()
00050             marker_arrow.header.frame_id = "/map"
00051             marker_arrow.ns = "/pose"
00052             marker_arrow.id = i
00053             marker_arrow.type = Marker.ARROW
00054             marker_arrow.action = Marker.ADD
00055             marker_arrow.scale.x = 1.0
00056             marker_arrow.scale.y = 0.1
00057             marker_arrow.scale.z = 1.0
00058             marker_arrow.color.r = 0.0
00059             marker_arrow.color.g = 0.0
00060             marker_arrow.color.b = 1.0
00061             marker_arrow.color.a = 1.0
00062             marker_arrow.pose.position.x = pose[0]
00063             marker_arrow.pose.position.y = pose[1]
00064             marker_arrow.pose.position.z = 0.2
00065             quaternion = tf.transformations.quaternion_from_euler(0, 0, pose[2])
00066             marker_arrow.pose.orientation.x = quaternion[0]
00067             marker_arrow.pose.orientation.y = quaternion[1]
00068             marker_arrow.pose.orientation.z = quaternion[2]
00069             marker_arrow.pose.orientation.w = quaternion[3]
00070             markerarray.markers.append(marker_arrow)
00071             
00072             # text
00073             marker_text = Marker()
00074             marker_text.header.stamp = rospy.Time.now()
00075             marker_text.header.frame_id = "/map"
00076             marker_text.ns = "/name"
00077             marker_text.id = i + 1000000
00078             marker_text.type = Marker.TEXT_VIEW_FACING
00079             marker_text.action = Marker.ADD
00080             marker_text.scale.z = self.text_size
00081             marker_text.color.r = 0.0
00082             marker_text.color.g = 0.0
00083             marker_text.color.b = 1.0
00084             marker_text.color.a = 1.0
00085             marker_text.pose.position.x = pose[0]
00086             marker_text.pose.position.y = pose[1] + 0.2
00087             marker_text.pose.position.z = 0.2
00088             quaternion = tf.transformations.quaternion_from_euler(0, 0, pose[2])
00089             marker_text.pose.orientation.x = quaternion[0]
00090             marker_text.pose.orientation.y = quaternion[1]
00091             marker_text.pose.orientation.z = quaternion[2]
00092             marker_text.pose.orientation.w = quaternion[3]
00093             marker_text.text = name
00094             markerarray.markers.append(marker_text)
00095             
00096             i = i + 1
00097 
00098         self.pubGoals.publish(markerarray)
00099 
00100 if __name__ == "__main__":
00101     rospy.init_node('navigation_goal_visualizer')
00102     p = VisualizerNavigationGoals()
00103     r = rospy.Rate(1)
00104     while not rospy.is_shutdown():
00105         p.pubMarker()
00106         try:
00107             r.sleep()
00108         except rospy.exceptions.ROSInterruptException as e:
00109             pass


cob_helper_tools
Author(s): Felix Messmer
autogenerated on Sun Jun 9 2019 20:20:17