mirror of
https://github.com/CopterExpress/clover.git
synced 2026-05-27 05:29:32 +00:00
86 lines
2.3 KiB
Python
Executable File
86 lines
2.3 KiB
Python
Executable File
#!/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()
|