Move interactive.py node to clever_tools

clever/src/interactive.py

@@ -1,85 +0,0 @@
-#!/usr/bin/env python
-import copy
-
-import rospy
-import tf.transformations as t
-from interactive_markers.interactive_marker_server import InteractiveMarkerServer
-from visualization_msgs.msg import Marker, InteractiveMarker, InteractiveMarkerControl, InteractiveMarkerFeedback
-from clever import srv
-
-
-def make_box(msg):
-    marker = Marker()
-
-    marker.type = Marker.CUBE
-    marker.scale.x = msg.scale * 0.3
-    marker.scale.y = msg.scale * 0.3
-    marker.scale.z = msg.scale * 0.3
-    marker.color.r = 0.5
-    marker.color.g = 0.5
-    marker.color.b = 0.5
-    marker.color.a = 1.0
-    marker.pose.orientation.w = 1
-
-    return marker
-
-
-def make_box_control(msg):
-    control = InteractiveMarkerControl()
-    control.always_visible = True
-    control.orientation.w = 1
-    control.markers.append(make_box(msg))
-    msg.controls.append(control)
-    return control
-
-
-def make_quadcopter_marker():
-    marker = InteractiveMarker()
-    marker.header.frame_id = 'base_link'
-    marker.header.stamp = rospy.get_rostime()
-    marker.scale = 1
-    marker.pose.orientation.w = 1
-
-    marker.name = 'quadcopter'
-    marker.description = 'Quadcopter'
-
-    make_box_control(marker)
-
-    control = InteractiveMarkerControl()
-    control.orientation.w = 1
-    control.orientation.x = 0
-    control.orientation.y = 1
-    control.orientation.z = 0
-    control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE
-    marker.controls.append(copy.deepcopy(control))
-    control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
-    marker.controls.append(control)
-
-    return marker
-
-
-navigate = rospy.ServiceProxy('navigate', srv.Navigate)
-
-
-def process_feedback(feedback):
-    if feedback.event_type != InteractiveMarkerFeedback.MOUSE_UP:
-        return
-
-    p = feedback.pose.position
-    o = feedback.pose.orientation
-    yaw = t.euler_from_quaternion((o.x, o.y, o.z, o.w), axes='rzyx')[0]
-    rospy.loginfo('Navigate to %s', p)
-    rospy.loginfo(navigate(x=p.x, y=p.y, z=p.z, yaw=yaw, speed=2,
-                           frame_id=feedback.header.frame_id, auto_arm=True))
-
-
-rospy.init_node('quadcopter_im')
-
-server = InteractiveMarkerServer('quadcopter_im')
-
-int_marker = make_quadcopter_marker()
-server.insert(int_marker, process_feedback)
-server.applyChanges()
-
-rospy.loginfo('Interactive quadcopter marker initialized')
-rospy.spin()