From e7249f86e76492407d2cfe1cb2c65a216424598c Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Fri, 17 Nov 2017 16:46:28 +0300 Subject: [PATCH] New version of simple_offboard node --- clever/CMakeLists.txt | 33 ++- clever/launch/clever.launch | 4 +- clever/requirements.txt | 1 + clever/src/global_local.py | 36 +++ clever/src/simple_offboard.py | 301 ++++++++++++++++++++++++ clever/src/util.py | 28 +++ clever/srv/SetAttitude.srv | 9 + clever/srv/SetAttitudeYawRate.srv | 7 + clever/srv/SetPosition.srv | 9 + clever/srv/SetPositionGlobal.srv | 9 + clever/srv/SetPositionGlobalYawRate.srv | 9 + clever/srv/SetPositionYawRate.srv | 9 + clever/srv/SetRates.srv | 7 + clever/srv/SetRatesYaw.srv | 9 + clever/srv/SetVelocity.srv | 9 + clever/srv/SetVelocityYawRate.srv | 9 + 16 files changed, 477 insertions(+), 12 deletions(-) create mode 100644 clever/src/global_local.py create mode 100644 clever/src/simple_offboard.py create mode 100644 clever/src/util.py create mode 100644 clever/srv/SetAttitude.srv create mode 100644 clever/srv/SetAttitudeYawRate.srv create mode 100644 clever/srv/SetPosition.srv create mode 100644 clever/srv/SetPositionGlobal.srv create mode 100644 clever/srv/SetPositionGlobalYawRate.srv create mode 100644 clever/srv/SetPositionYawRate.srv create mode 100644 clever/srv/SetRates.srv create mode 100644 clever/srv/SetRatesYaw.srv create mode 100644 clever/srv/SetVelocity.srv create mode 100644 clever/srv/SetVelocityYawRate.srv diff --git a/clever/CMakeLists.txt b/clever/CMakeLists.txt index b3cb2aba..c13ff221 100644 --- a/clever/CMakeLists.txt +++ b/clever/CMakeLists.txt @@ -7,7 +7,12 @@ project(clever) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages -find_package(catkin REQUIRED) +find_package(catkin REQUIRED COMPONENTS + rospy + std_msgs + message_generation +) + ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) @@ -50,11 +55,19 @@ find_package(catkin REQUIRED) # ) ## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) +add_service_files( + FILES + SetPosition.srv + SetPositionYawRate.srv + SetPositionGlobal.srv + SetPositionGlobalYawRate.srv + SetVelocity.srv + SetVelocityYawRate.srv + SetAttitude.srv + SetAttitudeYawRate.srv + SetRatesYaw.srv + SetRates.srv +) ## Generate actions in the 'action' folder # add_action_files( @@ -64,10 +77,10 @@ find_package(catkin REQUIRED) # ) ## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) +generate_messages( + DEPENDENCIES + std_msgs # Or other packages containing msgs +) ################################################ ## Declare ROS dynamic reconfigure parameters ## diff --git a/clever/launch/clever.launch b/clever/launch/clever.launch index 011db8e4..94ba7954 100644 --- a/clever/launch/clever.launch +++ b/clever/launch/clever.launch @@ -27,8 +27,8 @@ - - + + diff --git a/clever/requirements.txt b/clever/requirements.txt index 484d3c57..6cb6a548 100644 --- a/clever/requirements.txt +++ b/clever/requirements.txt @@ -1 +1,2 @@ flask==0.12.2 +geopy==1.11.0 diff --git a/clever/src/global_local.py b/clever/src/global_local.py new file mode 100644 index 00000000..a4f8f047 --- /dev/null +++ b/clever/src/global_local.py @@ -0,0 +1,36 @@ +import rospy +import math +import geopy +from geometry_msgs.msg import PoseStamped +from geopy.distance import VincentyDistance, vincenty +from sensor_msgs.msg import NavSatFix + + +def global_to_local(lat, lon): + # TODO: refactor + + position_global = rospy.wait_for_message('mavros/global_position/global', NavSatFix, timeout=5) + pose = rospy.wait_for_message('mavros/local_position/pose', PoseStamped, timeout=5) + + d = math.hypot(pose.pose.position.x, pose.pose.position.y) + + bearing = math.degrees(math.atan2(-pose.pose.position.x, -pose.pose.position.y)) + if bearing < 0: + bearing += 360 + + cur = geopy.Point(position_global.latitude, position_global.longitude) + origin = VincentyDistance(meters=d).destination(cur, bearing) + + _origin = origin.latitude, origin.longitude + olat_tlon = origin.latitude, lon + tlat_olon = lat, origin.longitude + + N = vincenty(_origin, tlat_olon) + if lat < origin.latitude: + N = -N + + E = vincenty(_origin, olat_tlon) + if lon < origin.longitude: + E = -E + + return E.meters, N.meters diff --git a/clever/src/simple_offboard.py b/clever/src/simple_offboard.py new file mode 100644 index 00000000..c091c068 --- /dev/null +++ b/clever/src/simple_offboard.py @@ -0,0 +1,301 @@ +#!/usr/bin/env python +from __future__ import division + +import rospy +from geometry_msgs.msg import TransformStamped, PoseStamped, Point, Vector3, Vector3Stamped +import tf2_ros +import tf2_geometry_msgs +from mavros_msgs.msg import PositionTarget, AttitudeTarget, State +from mavros_msgs.srv import CommandBool, SetMode +from threading import Lock +import math + +from global_local import global_to_local +from util import euler_from_orientation, vector3_from_point, orientation_from_euler +from std_srvs.srv import Trigger +from clever import srv + + +rospy.init_node('simple_offboard') + + +# TF2 stuff +tf_broadcaster = tf2_ros.TransformBroadcaster() +static_tf_broadcaster = tf2_ros.StaticTransformBroadcaster() + +tf_buffer = tf2_ros.Buffer() +tf_listener = tf2_ros.TransformListener(tf_buffer) + + +def init_fcu_horiz(): + # `fcu_horiz` frame publishing + + tr = TransformStamped() + tr.header.frame_id = 'local_origin' + tr.child_frame_id = 'fcu_horiz' + + def update_pose(data): + tr.header.stamp = data.header.stamp + tr.transform.translation = vector3_from_point(data.pose.position) + yaw = euler_from_orientation(data.pose.orientation)[2] + tr.transform.rotation = orientation_from_euler(0, 0, yaw) + tf_broadcaster.sendTransform(tr) + + rospy.Subscriber('/mavros/local_position/pose', PoseStamped, update_pose) + + +init_fcu_horiz() + + +position_pub = rospy.Publisher('/mavros/setpoint_raw/local', PositionTarget, queue_size=1) +attitude_pub = rospy.Publisher('/mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=1) +arming = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool) +set_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode) + + +state = None + + +def state_update(data): + global state + state = data + + +rospy.Subscriber('/mavros/state', State, state_update) + + +AUTO_OFFBOARD = rospy.get_param('~auto_offboard', True) +AUTO_ARM = AUTO_OFFBOARD and rospy.get_param('~auto_arm', True) +OFFBOARD_TIMEOUT = rospy.Duration(rospy.get_param('~offboard_timeout', 3)) +ARM_TIMEOUT = rospy.Duration(rospy.get_param('~arm_timeout', 5)) +TRANSFORM_TIMEOUT = rospy.Duration(rospy.get_param('~transform_timeout', 3)) +SETPOINT_RATE = rospy.get_param('~setpoint_rate', 50) + + +def offboard_and_arm(): + if AUTO_OFFBOARD and state.mode != 'OFFBOARD': + rospy.sleep(.3) + rospy.loginfo('Switch mode to OFFBOARD') + res = set_mode(base_mode=0, custom_mode='OFFBOARD') + + start = rospy.get_rostime() + while True: + if state.mode == 'OFFBOARD': + break + if rospy.get_rostime() - start > OFFBOARD_TIMEOUT: + raise Exception('OFFBOARD request timed out') + + if AUTO_ARM and not state.armed: + rospy.loginfo('Arming') + res = arming(True) + + start = rospy.get_rostime() + while True: + if state.armed: + return True + + if rospy.get_rostime() - start > ARM_TIMEOUT: + raise Exception('Arming timed out') + + +ps = PoseStamped() +vs = Vector3Stamped() + + +def get_publisher_and_message(req, stamp): + ps.header.stamp = stamp + vs.header.stamp = stamp + + if isinstance(req, srv.SetPositionRequest): + ps.header.frame_id = req.frame_id or 'local_origin' + ps.pose.position = Point(req.x, req.y, req.z) + ps.pose.orientation = orientation_from_euler(0, 0, req.yaw) + pose_local = tf_buffer.transform(ps, 'local_origin', TRANSFORM_TIMEOUT) + + msg = PositionTarget(coordinate_frame=PositionTarget.FRAME_LOCAL_NED, + type_mask=PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ + + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ + + PositionTarget.IGNORE_YAW_RATE, + position=pose_local.pose.position, + yaw=euler_from_orientation(pose_local.pose.orientation)[2] - math.pi / 2) + return position_pub, msg + + elif isinstance(req, srv.SetPositionYawRateRequest): + ps.header.frame_id = req.frame_id or 'local_origin' + ps.pose.position = Point(req.x, req.y, req.z) + pose_local = tf_buffer.transform(ps, 'local_origin', TRANSFORM_TIMEOUT) + msg = PositionTarget(coordinate_frame=PositionTarget.FRAME_LOCAL_NED, + type_mask=PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ + + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ + + PositionTarget.IGNORE_YAW, + position=pose_local.pose.position, + yaw_rate=req.yaw_rate) + return position_pub, msg + + elif isinstance(req, srv.SetPositionGlobalRequest): + x, y = global_to_local(req.lat, req.lon) + + ps.header.frame_id = req.frame_id or 'local_origin' + ps.pose.position = Point(0, 0, req.z) + ps.pose.orientation = orientation_from_euler(0, 0, req.yaw) + pose_local = tf_buffer.transform(ps, 'local_origin', TRANSFORM_TIMEOUT) + pose_local.pose.position.x = x + pose_local.pose.position.y = y + + msg = PositionTarget(coordinate_frame=PositionTarget.FRAME_LOCAL_NED, + type_mask=PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ + + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ + + PositionTarget.IGNORE_YAW_RATE, + position=pose_local.pose.position, + yaw=euler_from_orientation(pose_local.pose.orientation)[2] - math.pi / 2) + return position_pub, msg + + elif isinstance(req, srv.SetPositionGlobalYawRateRequest): + x, y = global_to_local(req.lat, req.lon) + + ps.header.frame_id = req.frame_id or 'local_origin' + ps.pose.position = Point(0, 0, req.z) + pose_local = tf_buffer.transform(ps, 'local_origin', TRANSFORM_TIMEOUT) + pose_local.pose.position.x = x + pose_local.pose.position.y = y + + msg = PositionTarget(coordinate_frame=PositionTarget.FRAME_LOCAL_NED, + type_mask=PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ + + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ + + PositionTarget.IGNORE_YAW, + position=pose_local.pose.position, + yaw_rate=req.yaw_rate) + return position_pub, msg + + elif isinstance(req, srv.SetVelocityRequest): + vs.vector = Vector3(req.vx, req.vy, req.vz) + vs.header.frame_id = req.frame_id or 'local_origin' + ps.header.frame_id = req.frame_id or 'local_origin' + ps.pose.orientation = orientation_from_euler(0, 0, req.yaw) + pose_local = tf_buffer.transform(ps, 'local_origin', TRANSFORM_TIMEOUT) + vector_local = tf_buffer.transform(vs, 'local_origin', TRANSFORM_TIMEOUT) + msg = PositionTarget(coordinate_frame=PositionTarget.FRAME_LOCAL_NED, + type_mask=PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ + + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ + + PositionTarget.IGNORE_YAW_RATE, + velocity=vector_local.vector, + yaw=euler_from_orientation(pose_local.pose.orientation)[2] - math.pi / 2) + return position_pub, msg + + elif isinstance(req, srv.SetVelocityYawRateRequest): + vs.vector = Vector3(req.vx, req.vy, req.vz) + vs.header.frame_id = req.frame_id or 'local_origin' + vector_local = tf_buffer.transform(vs, 'local_origin', TRANSFORM_TIMEOUT) + msg = PositionTarget(coordinate_frame=PositionTarget.FRAME_LOCAL_NED, + type_mask=PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ + + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ + + PositionTarget.IGNORE_YAW, + velocity=vector_local.vector, + yaw_rate=req.yaw_rate) + return position_pub, msg + + elif isinstance(req, srv.SetAttitudeRequest): + ps.header.frame_id = req.frame_id or 'local_origin' + ps.pose.orientation = orientation_from_euler(req.roll, req.pitch, req.yaw) + pose_local = tf_buffer.transform(ps, 'local_origin', TRANSFORM_TIMEOUT) + msg = AttitudeTarget(orientation=pose_local.pose.orientation, + thrust=req.thrust, + type_mask=AttitudeTarget.IGNORE_YAW_RATE + AttitudeTarget.IGNORE_PITCH_RATE + + AttitudeTarget.IGNORE_ROLL_RATE) + return attitude_pub, msg + + elif isinstance(req, srv.SetAttitudeYawRateRequest): + msg = AttitudeTarget(orientation=orientation_from_euler(req.roll, req.pitch, 0), + thrust=req.thrust, + type_mask=AttitudeTarget.IGNORE_PITCH_RATE + AttitudeTarget.IGNORE_ROLL_RATE) + msg.body_rate.z = req.yaw_rate + return attitude_pub, msg + + elif isinstance(req, srv.SetRatesYawRequest): + ps.header.frame_id = req.frame_id or 'local_origin' + ps.pose.orientation = orientation_from_euler(0, 0, req.yaw) + pose_local = tf_buffer.transform(ps, 'local_origin', TRANSFORM_TIMEOUT) + msg = AttitudeTarget(orientation=pose_local.pose.orientation, + thrust=req.thrust, + type_mask=AttitudeTarget.IGNORE_YAW_RATE, + body_rate=Vector3(req.roll_rate, req.pitch_rate, 0)) + return attitude_pub, msg + + elif isinstance(req, srv.SetRatesRequest): + msg = AttitudeTarget(thrust=req.thrust, + type_mask=AttitudeTarget.IGNORE_ATTITUDE, + body_rate=Vector3(req.roll_rate, req.pitch_rate, req.yaw_rate)) + return attitude_pub, msg + + +current_pub = None +current_msg = None +current_req = None +handle_lock = Lock() + + +def handle(req): + global current_pub, current_msg, current_req + with handle_lock: + try: + stamp = rospy.get_rostime() + current_pub, current_msg = get_publisher_and_message(req, stamp) + rospy.loginfo('Topic: %s, message: %s', current_pub.name, current_msg) + + current_msg.header.stamp = stamp + current_pub.publish(current_msg) + + offboard_and_arm() + return {'success': True} + + except Exception as e: + rospy.logerr(str(e)) + return {'success': False, 'message': str(e)} + + +def release(req): + global current_pub + current_pub = None + rospy.loginfo('simple_offboard: release') + return {'success': True} + + +rospy.Service('set_position', srv.SetPosition, handle) +rospy.Service('set_position/yaw_rate', srv.SetPositionYawRate, handle) +rospy.Service('set_position_global', srv.SetPositionGlobal, handle) +rospy.Service('set_position_global/yaw_rate', srv.SetPositionGlobalYawRate, handle) +rospy.Service('set_velocity', srv.SetVelocity, handle) +rospy.Service('set_velocity/yaw_rate', srv.SetVelocityYawRate, handle) +rospy.Service('set_attitude', srv.SetAttitude, handle) +rospy.Service('set_attitude/yaw_rate', srv.SetAttitudeYawRate, handle) +rospy.Service('set_rates', srv.SetRates, handle) +rospy.Service('set_rates/yaw', srv.SetRatesYaw, handle) +rospy.Service('release', Trigger, release) + + +rospy.loginfo('simple_offboard inited') + + +def start_loop(): + global current_pub, current_msg, current_req + r = rospy.Rate(SETPOINT_RATE) + + while not rospy.is_shutdown(): + with handle_lock: + if current_pub is not None: + try: + stamp = rospy.get_rostime() + + if getattr(current_req, 'update_frame', False): + current_pub, current_msg = get_publisher_and_message(current_req, stamp) + + current_msg.header.stamp = stamp + current_pub.publish(current_msg) + + except Exception as e: + rospy.logwarn_throttle(10, str(e)) + + r.sleep() + + +start_loop() diff --git a/clever/src/util.py b/clever/src/util.py new file mode 100644 index 00000000..eafa72fd --- /dev/null +++ b/clever/src/util.py @@ -0,0 +1,28 @@ +from geometry_msgs.msg import Quaternion, Vector3, Point +import tf.transformations as t + + +def orientation_from_quaternion(q): + return Quaternion(*q) + + +def orientation_from_euler(roll, pitch, yaw): + q = t.quaternion_from_euler(roll, pitch, yaw) + return orientation_from_quaternion(q) + + +def quaternion_from_orientation(o): + return o.x, o.y, o.z, o.w + + +def euler_from_orientation(o): + q = quaternion_from_orientation(o) + return t.euler_from_quaternion(q) + + +def vector3_from_point(p): + return Vector3(p.x, p.y, p.z) + + +def point_from_vector3(v): + return Point(v.x, v.y, v.z) diff --git a/clever/srv/SetAttitude.srv b/clever/srv/SetAttitude.srv new file mode 100644 index 00000000..9c41fff9 --- /dev/null +++ b/clever/srv/SetAttitude.srv @@ -0,0 +1,9 @@ +float32 pitch +float32 roll +float32 yaw +float32 thrust +string frame_id +bool update_frame +--- +bool success +string message diff --git a/clever/srv/SetAttitudeYawRate.srv b/clever/srv/SetAttitudeYawRate.srv new file mode 100644 index 00000000..80e68a6a --- /dev/null +++ b/clever/srv/SetAttitudeYawRate.srv @@ -0,0 +1,7 @@ +float32 roll +float32 pitch +float32 yaw_rate +float32 thrust +--- +bool success +string message diff --git a/clever/srv/SetPosition.srv b/clever/srv/SetPosition.srv new file mode 100644 index 00000000..53e375ed --- /dev/null +++ b/clever/srv/SetPosition.srv @@ -0,0 +1,9 @@ +float32 x +float32 y +float32 z +float32 yaw +string frame_id +bool update_frame +--- +bool success +string message diff --git a/clever/srv/SetPositionGlobal.srv b/clever/srv/SetPositionGlobal.srv new file mode 100644 index 00000000..4bbdfc95 --- /dev/null +++ b/clever/srv/SetPositionGlobal.srv @@ -0,0 +1,9 @@ +float32 lat +float32 lon +float32 z +float32 yaw +string frame_id +bool update_frame +--- +bool success +string message diff --git a/clever/srv/SetPositionGlobalYawRate.srv b/clever/srv/SetPositionGlobalYawRate.srv new file mode 100644 index 00000000..a7959e04 --- /dev/null +++ b/clever/srv/SetPositionGlobalYawRate.srv @@ -0,0 +1,9 @@ +float32 lat +float32 lon +float32 z +float32 yaw_rate +string frame_id +bool update_frame +--- +bool success +string message diff --git a/clever/srv/SetPositionYawRate.srv b/clever/srv/SetPositionYawRate.srv new file mode 100644 index 00000000..76feda1f --- /dev/null +++ b/clever/srv/SetPositionYawRate.srv @@ -0,0 +1,9 @@ +float32 x +float32 y +float32 z +float32 yaw_rate +string frame_id +bool update_frame +--- +bool success +string message diff --git a/clever/srv/SetRates.srv b/clever/srv/SetRates.srv new file mode 100644 index 00000000..7e7c1cb1 --- /dev/null +++ b/clever/srv/SetRates.srv @@ -0,0 +1,7 @@ +float32 pitch_rate +float32 roll_rate +float32 yaw_rate +float32 thrust +--- +bool success +string message diff --git a/clever/srv/SetRatesYaw.srv b/clever/srv/SetRatesYaw.srv new file mode 100644 index 00000000..fb9dbcc3 --- /dev/null +++ b/clever/srv/SetRatesYaw.srv @@ -0,0 +1,9 @@ +float32 pitch_rate +float32 roll_rate +float32 yaw +float32 thrust +string frame_id +bool update_frame +--- +bool success +string message diff --git a/clever/srv/SetVelocity.srv b/clever/srv/SetVelocity.srv new file mode 100644 index 00000000..88efaaa2 --- /dev/null +++ b/clever/srv/SetVelocity.srv @@ -0,0 +1,9 @@ +float32 vx +float32 vy +float32 vz +float32 yaw +string frame_id +bool update_frame +--- +bool success +string message diff --git a/clever/srv/SetVelocityYawRate.srv b/clever/srv/SetVelocityYawRate.srv new file mode 100644 index 00000000..965bdf68 --- /dev/null +++ b/clever/srv/SetVelocityYawRate.srv @@ -0,0 +1,9 @@ +float32 vx +float32 vy +float32 vz +float32 yaw_rate +string frame_id +bool update_frame +--- +bool success +string message