triangle_screenpoint.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import math
00004 import os
00005 import subprocess
00006 
00007 import roslib
00008 import rospy
00009 
00010 roslib.load_manifest("hrpsys_gazebo_atlas")
00011 
00012 from visualization_msgs.msg import Marker
00013 from image_view2.msg import ImageMarker2
00014 from geometry_msgs.msg import PointStamped, Point
00015 from std_msgs.msg import ColorRGBA
00016 from hrpsys_gazebo_atlas.msg import TrianglePoints
00017 from std_srvs.srv import Empty, EmptyResponse
00018 # global variables
00019 
00020 g_offset = [-0.2, 0.0, 0.0]
00021 g_rviz_marker_pub = None
00022 g_image_marker_pub = None
00023 g_triangle_point_state = None
00024 
00025 def publishMode():
00026   global g_image_marker_pub
00027   marker = ImageMarker2()
00028   marker.header.stamp = rospy.Time(0.0)
00029   marker.id = 1
00030   marker.type = ImageMarker2.TEXT
00031   marker.text = "mode: triangle_screenpoint.py"
00032   marker.position.x = 300
00033   marker.position.y = 50
00034   g_image_marker_pub.publish(marker)
00035 
00036 class TrianglePointsState():
00037   def __init__(self):
00038     self.reset()
00039     self.triangle_publisher = rospy.Publisher("/trianglepoints", 
00040                                               TrianglePoints)
00041     self.point_a_publisher = rospy.Publisher("/triangle_point_a",
00042                                              PointStamped)
00043     self.point_b_publisher = rospy.Publisher("/triangle_point_b", 
00044                                              PointStamped)
00045     self.point_c_publisher = rospy.Publisher("/triangle_point_c", 
00046                                              PointStamped)
00047   def reset(self):
00048     self.a = None
00049     self.b = None
00050     self.c = None
00051   def makeImageMarker(self, p):
00052     now = rospy.Time(0.0)
00053     marker = ImageMarker2()
00054     marker.header.stamp = now
00055     marker.header.frame_id = p.header.frame_id
00056     marker.type = ImageMarker2.POLYGON3D
00057     marker.points3D.header.frame_id = p.header.frame_id
00058     marker.points3D.header.stamp = now
00059     R = 0.01
00060     N = 10
00061     for i in range(N) + [0]:
00062       point = Point()
00063       point.x = p.point.x + R * math.cos(2 * math.pi * i / N)
00064       point.y = p.point.y + R * math.sin(2 * math.pi * i / N)
00065       point.z = p.point.z
00066       marker.points3D.points.append(point)
00067     marker.outline_colors = [ColorRGBA(1.0, .0, .0, 1.0)]
00068     return marker
00069   def publishTriangles(self):
00070     msg = TrianglePoints()
00071     msg.header.stamp = rospy.Time()
00072     msg.a = self.a
00073     msg.b = self.b
00074     msg.c = self.c
00075     self.triangle_publisher.publish(msg)
00076   def publishHelperTopics(self):
00077     global g_image_marker_pub
00078     if self.a:
00079       self.point_a_publisher.publish(self.a)
00080       m = self.makeImageMarker(self.a)
00081       m.id = 2
00082       g_image_marker_pub.publish(m)
00083     if self.b:
00084       self.point_b_publisher.publish(self.b)
00085       m = self.makeImageMarker(self.b)
00086       m.id = 3
00087       m.outline_colors[0].r = 0.0
00088       m.outline_colors[0].g = 1.0
00089       g_image_marker_pub.publish(m)
00090     if self.c:
00091       self.point_c_publisher.publish(self.c)
00092       m = self.makeImageMarker(self.c)
00093       m.outline_colors[0].r = 0.0
00094       m.outline_colors[0].b = 1.0
00095       m.id = 4
00096       g_image_marker_pub.publish(m)
00097     publishMode()
00098 
00099 def pointCB(msg):
00100   global g_rviz_marker_pub, g_image_marker_pub, g_triangle_point_state
00101   if not g_triangle_point_state.a:
00102     rospy.loginfo("setting A")
00103     g_triangle_point_state.a = msg
00104     g_triangle_point_state.a.header.stamp = rospy.Time(0.0)
00105   elif not g_triangle_point_state.b:
00106     rospy.loginfo("setting B")
00107     g_triangle_point_state.b = msg
00108     g_triangle_point_state.b.header.stamp = rospy.Time(0.0)
00109   elif not g_triangle_point_state.c:
00110     rospy.loginfo("setting C")
00111     g_triangle_point_state.c = msg
00112     g_triangle_point_state.c.header.stamp = rospy.Time(0.0)
00113   g_triangle_point_state.publishHelperTopics()
00114 
00115 def cancelCB(req):
00116   global g_triangle_point_state
00117   rospy.loginfo("canceled")
00118   g_triangle_point_state.reset()
00119   return EmptyResponse()
00120   
00121 def goCB(req):
00122   global g_triangle_point_state
00123   rospy.loginfo("go")
00124   g_triangle_point_state.publishTriangles()
00125   return EmptyResponse()
00126   
00127 def main():
00128   global g_rviz_marker_pub, g_image_marker_pub, g_triangle_point_state
00129   rospy.init_node("triangle_screenpoint")
00130   g_triangle_point_state = TrianglePointsState()
00131   g_rviz_marker_pub = rospy.Publisher("/triangle_marker", Marker)
00132   g_image_marker_pub = rospy.Publisher("/multisense_sl/camera/left/image_marker",
00133                                        ImageMarker2)
00134   gui_process = subprocess.Popen([os.path.join(os.path.dirname(__file__), 
00135                                                "triangle_gui.py")])
00136   rospy.Subscriber("/pointcloud_screenpoint_nodelet/output_point", PointStamped, 
00137                    pointCB)
00138   cancel = rospy.Service("~cancel", Empty, cancelCB)
00139   go = rospy.Service("~go", Empty, goCB)
00140   publishMode()
00141   rospy.spin()
00142 
00143 if __name__ == "__main__":
00144    main()


hrpsys_gazebo_atlas
Author(s): Yohei Kakiuchi
autogenerated on Thu Jun 6 2019 20:57:50