diff --git a/clover/CMakeLists.txt b/clover/CMakeLists.txt index a2bfb3dc..7eca3f4c 100644 --- a/clover/CMakeLists.txt +++ b/clover/CMakeLists.txt @@ -80,11 +80,10 @@ catkin_python_setup() ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) ## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) +add_message_files( + FILES + State.msg +) ## Generate services in the 'srv' folder add_service_files( @@ -92,6 +91,9 @@ add_service_files( GetTelemetry.srv Navigate.srv NavigateGlobal.srv + SetAltitude.srv + SetYaw.srv + SetYawRate.srv SetPosition.srv SetVelocity.srv SetAttitude.srv @@ -306,4 +308,5 @@ endif() if (CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) add_rostest(test/basic.test) + add_rostest(test/offboard.test) endif() diff --git a/clover/examples/navigate_wait.py b/clover/examples/navigate_wait.py index 99637caf..acb37f70 100644 --- a/clover/examples/navigate_wait.py +++ b/clover/examples/navigate_wait.py @@ -16,11 +16,8 @@ set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude) set_rates = rospy.ServiceProxy('set_rates', srv.SetRates) land = rospy.ServiceProxy('land', Trigger) -def navigate_wait(x=0, y=0, z=0, yaw=float('nan'), yaw_rate=0, speed=0.5, \ - frame_id='body', tolerance=0.2, auto_arm=False): - - res = navigate(x=x, y=y, z=z, yaw=yaw, yaw_rate=yaw_rate, speed=speed, \ - frame_id=frame_id, auto_arm=auto_arm) +def navigate_wait(x=0, y=0, z=0, yaw=math.nan, speed=0.5, frame_id='body', tolerance=0.2, auto_arm=False): + res = navigate(x=x, y=y, z=z, yaw=yaw, speed=speed, frame_id=frame_id, auto_arm=auto_arm) if not res.success: return res diff --git a/clover/msg/State.msg b/clover/msg/State.msg new file mode 100644 index 00000000..038df4fc --- /dev/null +++ b/clover/msg/State.msg @@ -0,0 +1,38 @@ +uint8 MODE_NONE = 0 +uint8 MODE_NAVIGATE = 1 +uint8 MODE_NAVIGATE_GLOBAL = 2 +uint8 MODE_POSITION = 3 +uint8 MODE_VELOCITY = 4 +uint8 MODE_ATTITUDE = 5 +uint8 MODE_RATES = 6 + +uint8 YAW_MODE_YAW = 0 +uint8 YAW_MODE_YAW_RATE = 1 +uint8 YAW_MODE_YAW_TOWARDS = 2 + +# type of offboard control +uint8 mode +uint8 yaw_mode + +# targets +float32 x +float32 y +float32 z +float32 speed +float32 lat +float32 lon +float32 vx +float32 vy +float32 vz +float32 roll +float32 pitch +float32 yaw +float32 roll_rate +float32 pitch_rate +float32 yaw_rate +float32 thrust + +# frames of reference +string xy_frame_id +string z_frame_id +string yaw_frame_id diff --git a/clover/src/autotest/autotest_aruco.py b/clover/src/autotest/autotest_aruco.py index d53c98ca..0fbe6fa7 100755 --- a/clover/src/autotest/autotest_aruco.py +++ b/clover/src/autotest/autotest_aruco.py @@ -35,11 +35,8 @@ def print_current_map_position(): dist = rospy.wait_for_message('rangefinder/range', Range).range print('Map position:\tx={:.1f}\ty={:.1f}\tz={:.1f}\tyaw={:.1f}\tdist={:.2f}'.format(telem.x, telem.y, telem.z, telem.yaw, dist)) -def navigate_wait(x=0, y=0, z=0, yaw=float('nan'), yaw_rate=0, speed=0.5, \ - frame_id='body', tolerance=0.2, auto_arm=False): - - res = navigate(x=x, y=y, z=z, yaw=yaw, yaw_rate=yaw_rate, speed=speed, \ - frame_id=frame_id, auto_arm=auto_arm) +def navigate_wait(x=0, y=0, z=0, yaw=math.nan, speed=0.5, frame_id='body', tolerance=0.2, auto_arm=False): + res = navigate(x=x, y=y, z=z, yaw=yaw, speed=speed, frame_id=frame_id, auto_arm=auto_arm) if not res.success: return res diff --git a/clover/src/autotest/autotest_flight.py b/clover/src/autotest/autotest_flight.py index 19adfdbf..371db7df 100755 --- a/clover/src/autotest/autotest_flight.py +++ b/clover/src/autotest/autotest_flight.py @@ -2,7 +2,7 @@ import rospy import math -from math import nan +from math import nan, inf import signal import sys from clover import srv @@ -15,6 +15,8 @@ rospy.init_node('autotest_flight', disable_signals=True) # disable signals to al get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry) navigate = handle_response(rospy.ServiceProxy('navigate', srv.Navigate)) navigate_global = handle_response(rospy.ServiceProxy('navigate_global', srv.NavigateGlobal)) +set_yaw = handle_response(rospy.ServiceProxy('set_yaw', srv.SetYaw)) +set_yaw_rate = handle_response(rospy.ServiceProxy('set_yaw_rate', srv.SetYawRate)) set_position = handle_response(rospy.ServiceProxy('set_position', srv.SetPosition)) set_velocity = handle_response(rospy.ServiceProxy('set_velocity', srv.SetVelocity)) set_attitude = handle_response(rospy.ServiceProxy('set_attitude', srv.SetAttitude)) @@ -28,11 +30,8 @@ def interrupt(sig, frame): signal.signal(signal.SIGINT, interrupt) -def navigate_wait(x=0, y=0, z=0, yaw=nan, yaw_rate=0, speed=0.5, \ - frame_id='body', tolerance=0.2, auto_arm=False): - - res = navigate(x=x, y=y, z=z, yaw=yaw, yaw_rate=yaw_rate, speed=speed, \ - frame_id=frame_id, auto_arm=auto_arm) +def navigate_wait(x=0, y=0, z=0, yaw=nan, speed=0.5, frame_id='body', tolerance=0.2, auto_arm=False): + res = navigate(x=x, y=y, z=z, yaw=yaw, speed=speed, frame_id=frame_id, auto_arm=auto_arm) if not res.success: return res @@ -69,17 +68,17 @@ set_velocity(vx=1, vy=0.0, vz=0, frame_id='body') rospy.sleep(2) set_position(frame_id='body') -input('Rotate right 90° [enter] ') -navigate(yaw=-math.pi / 2, frame_id='navigate_target') +input('Rotate right 90° using set_yaw [enter] ') +set_yaw(yaw=-math.pi / 2, frame_id='navigate_target') rospy.sleep(3) input('Use set_attitude to fly backwards [enter]') -set_attitude(pitch=-0.3, roll=0, yaw=0, thrust=0.5, frame_id='body') +set_attitude(roll=0, pitch=-0.3, yaw=0, thrust=0.5, frame_id='body') rospy.sleep(0.3) set_position(frame_id='body') input('Use set_attitude to fly right [enter]') -set_attitude(pitch=0, roll=0.3, yaw=0, thrust=0.5, frame_id='body') +set_attitude(roll=0.3, pitch=0, yaw=0, thrust=0.5, frame_id='body') rospy.sleep(0.5) set_position(frame_id='body') @@ -88,13 +87,13 @@ set_rates(roll_rate=1.2, thrust=0.5) rospy.sleep(0.4) set_position(frame_id='body') -input('Rotate 360° to the right using yaw_rate [enter]') -set_position(x=nan, y=nan, z=nan, frame_id='body', yaw=nan, yaw_rate=-1) +input('Rotate 360° to the right using set_yaw_rate [enter]') +set_yaw_rate(yaw_rate=-1) rospy.sleep(2 * math.pi) set_position(frame_id='body') -input('Return to start point [enter]') -navigate_wait(x=start.x, y=start.y, z=start.z, yaw=start.yaw, speed=1, frame_id='map') +input('Return to start point heading forward [enter]') +navigate_wait(x=start.x, y=start.y, z=start.z, yaw=inf, speed=1, frame_id='map') input('Land [enter]') land() diff --git a/clover/src/simple_offboard.cpp b/clover/src/simple_offboard.cpp index fc755d77..0781d78d 100644 --- a/clover/src/simple_offboard.cpp +++ b/clover/src/simple_offboard.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include #include @@ -37,14 +38,19 @@ #include #include #include +#include #include #include #include +#include +#include +#include #include #include #include #include +#include using std::string; using std::isnan; @@ -54,6 +60,7 @@ using namespace clover; using mavros_msgs::PositionTarget; using mavros_msgs::AttitudeTarget; using mavros_msgs::Thrust; +using mavros_msgs::Altitude; // tf2 tf2_ros::Buffer tf_buffer; @@ -81,33 +88,40 @@ bool land_only_in_offboard, nav_from_sp, check_kill_switch; std::map reference_frames; // Publishers -ros::Publisher attitude_pub, attitude_raw_pub, position_pub, position_raw_pub, rates_pub, thrust_pub; +ros::Publisher attitude_pub, attitude_raw_pub, position_pub, position_raw_pub, rates_pub, thrust_pub, state_pub; // Service clients ros::ServiceClient arming, set_mode; // Containers ros::Timer setpoint_timer; -tf::Quaternion tq; PoseStamped position_msg; PositionTarget position_raw_msg; -AttitudeTarget att_raw_msg; -Thrust thrust_msg; -TwistStamped rates_msg; +//TwistStamped rates_msg; TransformStamped target, setpoint; geometry_msgs::TransformStamped body; +geometry_msgs::TransformStamped terrain; // State PoseStamped nav_start; -PoseStamped setpoint_position, setpoint_position_transformed; -Vector3Stamped setpoint_velocity, setpoint_velocity_transformed; -QuaternionStamped setpoint_attitude, setpoint_attitude_transformed; -float setpoint_yaw_rate; +PointStamped setpoint_position; +PointStamped setpoint_altitude; +Vector3Stamped setpoint_velocity; +float setpoint_yaw, setpoint_roll, setpoint_pitch; +Vector3 setpoint_rates; +string yaw_frame_id; +float setpoint_thrust; float nav_speed; +float setpoint_lat = NAN, setpoint_lon = NAN; bool busy = false; bool wait_armed = false; bool nav_from_sp_flag = false; +// Last published +PoseStamped setpoint_pose_local; +Vector3Stamped setpoint_velocity_local; +float yaw_local; + enum setpoint_type_t { NONE, NAVIGATE, @@ -115,7 +129,10 @@ enum setpoint_type_t { POSITION, VELOCITY, ATTITUDE, - RATES + RATES, + _ALTITUDE, + _YAW, + _YAW_RATE, }; enum setpoint_type_t setpoint_type = NONE; @@ -170,7 +187,7 @@ void handleLocalPosition(const PoseStamped& pose) { local_position = pose; publishBodyFrame(); - // TODO: terrain?, home? + // TODO: home? } // wait for transform without interrupting publishing setpoints @@ -188,6 +205,20 @@ inline bool waitTransform(const string& target, const string& source, return false; } +void handleAltitude(const Altitude& alt) +{ + // publish terrain frame + if (!std::isfinite(alt.bottom_clearance)) return; + // terrain.header.stamp = alt.header.stamp; + + if (!waitTransform(local_frame, body.child_frame_id, alt.header.stamp, ros::Duration(0.1))) return; + + auto t = tf_buffer.lookupTransform(local_frame, body.child_frame_id, alt.header.stamp); + t.child_frame_id = terrain.child_frame_id; + t.transform.translation.z -= alt.bottom_clearance; + static_transform_broadcaster->sendTransform(t); +} + #define TIMEOUT(msg, timeout) (msg.header.stamp.isZero() || (ros::Time::now() - msg.header.stamp > timeout)) bool getTelemetry(GetTelemetry::Request& req, GetTelemetry::Response& res) @@ -207,11 +238,11 @@ bool getTelemetry(GetTelemetry::Request& req, GetTelemetry::Response& res) res.vx = NAN; res.vy = NAN; res.vz = NAN; - res.pitch = NAN; res.roll = NAN; + res.pitch = NAN; res.yaw = NAN; - res.pitch_rate = NAN; res.roll_rate = NAN; + res.pitch_rate = NAN; res.yaw_rate = NAN; res.voltage = NAN; res.cell_voltage = NAN; @@ -341,20 +372,20 @@ inline float getDistance(const Point& from, const Point& to) return hypot(from.x - to.x, from.y - to.y, from.z - to.z); } -void getNavigateSetpoint(const ros::Time& stamp, float speed, Point& nav_setpoint) +void getNavigateSetpoint(const ros::Time& stamp, const float speed, Point& nav_setpoint) { if (wait_armed) { // don't start navigating if we're waiting arming nav_start.header.stamp = stamp; } - float distance = getDistance(nav_start.pose.position, setpoint_position_transformed.pose.position); + float distance = getDistance(nav_start.pose.position, setpoint_pose_local.pose.position); float time = distance / speed; float passed = std::min((stamp - nav_start.header.stamp).toSec() / time, 1.0); - nav_setpoint.x = nav_start.pose.position.x + (setpoint_position_transformed.pose.position.x - nav_start.pose.position.x) * passed; - nav_setpoint.y = nav_start.pose.position.y + (setpoint_position_transformed.pose.position.y - nav_start.pose.position.y) * passed; - nav_setpoint.z = nav_start.pose.position.z + (setpoint_position_transformed.pose.position.z - nav_start.pose.position.z) * passed; + nav_setpoint.x = nav_start.pose.position.x + (setpoint_pose_local.pose.position.x - nav_start.pose.position.x) * passed; + nav_setpoint.y = nav_start.pose.position.y + (setpoint_pose_local.pose.position.y - nav_start.pose.position.y) * passed; + nav_setpoint.z = nav_start.pose.position.z + (setpoint_pose_local.pose.position.z - nav_start.pose.position.z) * passed; } PoseStamped globalToLocal(double lat, double lon) @@ -385,44 +416,101 @@ PoseStamped globalToLocal(double lat, double lon) return pose; } +// publish navigate_target frame +void publishTarget(ros::Time stamp, bool _static = false) +{ + bool single_frame = (setpoint_position.header.frame_id == setpoint_altitude.header.frame_id); + + // handle yaw for target frame + if (setpoint_yaw_type == YAW || setpoint_yaw_type == YAW_RATE) { // use last set yaw for yaw_rate + if (setpoint_altitude.header.frame_id == yaw_frame_id) { + target.transform.rotation = tf::createQuaternionMsgFromYaw(setpoint_yaw); + } else { + single_frame = false; + target.transform.rotation = tf::createQuaternionMsgFromYaw(yaw_local); + } + } else if (setpoint_yaw_type == TOWARDS) { + single_frame = false; + target.transform.rotation = tf::createQuaternionMsgFromYaw(yaw_local); + } + + if (_static && single_frame) { + // publish at user's command, if all frames are the same + target.header.frame_id = setpoint_position.header.frame_id; + target.header.stamp = stamp; + target.transform.translation.x = setpoint_position.point.x; + target.transform.translation.y = setpoint_position.point.y; + target.transform.translation.z = setpoint_position.point.z; + + } else if (!_static) { + // publish at each iteration, if frames are different + target.header = setpoint_pose_local.header; + target.transform.translation.x = setpoint_pose_local.pose.position.x; + target.transform.translation.y = setpoint_pose_local.pose.position.y; + target.transform.translation.z = setpoint_pose_local.pose.position.z; + } + + static_transform_broadcaster->sendTransform(target); +} + void publish(const ros::Time stamp) { if (setpoint_type == NONE) return; position_raw_msg.header.stamp = stamp; - thrust_msg.header.stamp = stamp; - rates_msg.header.stamp = stamp; - try { - // transform position and/or yaw - if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION || setpoint_type == VELOCITY || setpoint_type == ATTITUDE) { - setpoint_position.header.stamp = stamp; - tf_buffer.transform(setpoint_position, setpoint_position_transformed, local_frame, ros::Duration(0.05)); + // transform position + if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) { + setpoint_position.header.stamp = stamp; + setpoint_altitude.header.stamp = stamp; + // transform xy + try { + auto xy = tf_buffer.transform(setpoint_position, local_frame, ros::Duration(0.05)); + setpoint_pose_local.header = xy.header; + setpoint_pose_local.pose.position.x = xy.point.x; + setpoint_pose_local.pose.position.y = xy.point.y; + } catch (tf2::TransformException& ex) { + // can't transform xy, use last known + ROS_WARN_THROTTLE(10, "can't transform: %s", ex.what()); } - - // transform velocity - if (setpoint_type == VELOCITY) { - setpoint_velocity.header.stamp = stamp; - tf_buffer.transform(setpoint_velocity, setpoint_velocity_transformed, local_frame, ros::Duration(0.05)); + // transform altitude + try { + setpoint_pose_local.pose.position.z = tf_buffer.transform(setpoint_altitude, local_frame, ros::Duration(0.05)).point.z; + } catch (tf2::TransformException& ex) { + // can't transform altitude, use last known + ROS_WARN_THROTTLE(10, "can't transform: %s", ex.what()); } - - } catch (const tf2::TransformException& e) { - ROS_WARN_THROTTLE(10, "can't transform"); } + // transform yaw + if (setpoint_yaw_type == YAW) { + try { + QuaternionStamped q; + q.header.stamp = stamp; + q.header.frame_id = yaw_frame_id; + q.quaternion = tf::createQuaternionMsgFromYaw(setpoint_yaw); + yaw_local = tf2::getYaw(tf_buffer.transform(q, local_frame, ros::Duration(0.05)).quaternion); + } catch (tf2::TransformException& ex) { + // can't transform yaw, use last known + ROS_WARN_THROTTLE(10, "can't transform: %s", ex.what()); + } + } + + // compute navigate setpoint if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL) { - position_msg.pose.orientation = setpoint_position_transformed.pose.orientation; // copy yaw getNavigateSetpoint(stamp, nav_speed, position_msg.pose.position); if (setpoint_yaw_type == TOWARDS) { - double yaw_towards = atan2(position_msg.pose.position.y - nav_start.pose.position.y, - position_msg.pose.position.x - nav_start.pose.position.x); - position_msg.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, yaw_towards); + yaw_local = atan2(position_msg.pose.position.y - nav_start.pose.position.y, + position_msg.pose.position.x - nav_start.pose.position.x); } + + position_msg.pose.orientation = tf::createQuaternionMsgFromYaw(yaw_local); } if (setpoint_type == POSITION) { - position_msg = setpoint_position_transformed; + position_msg = setpoint_pose_local; + position_msg.pose.orientation = tf::createQuaternionMsgFromYaw(yaw_local); } if (setpoint_type == POSITION || setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL) { @@ -439,14 +527,14 @@ void publish(const ros::Time stamp) PositionTarget::IGNORE_AFY + PositionTarget::IGNORE_AFZ + PositionTarget::IGNORE_YAW; - position_raw_msg.yaw_rate = setpoint_yaw_rate; + position_raw_msg.yaw_rate = setpoint_rates.z; position_raw_msg.position = position_msg.pose.position; position_raw_pub.publish(position_raw_msg); } // publish setpoint frame if (!setpoint.child_frame_id.empty()) { - if (setpoint.header.stamp == position_msg.header.stamp) { + if (setpoint.header.stamp >= position_msg.header.stamp) { return; // avoid TF_REPEATED_DATA warnings } @@ -458,9 +546,22 @@ void publish(const ros::Time stamp) setpoint.header.stamp = position_msg.header.stamp; transform_broadcaster->sendTransform(setpoint); } + + // publish dynamic target frame + publishTarget(stamp); } if (setpoint_type == VELOCITY) { + // transform velocity to local frame + setpoint_velocity.header.stamp = stamp; + try { + setpoint_velocity_local = tf_buffer.transform(setpoint_velocity, local_frame, ros::Duration(0.05)); + } catch (tf2::TransformException& ex) { + // can't transform velocity, use last known + ROS_WARN_THROTTLE(10, "can't transform: %s", ex.what()); + } + + // publish velocity position_raw_msg.type_mask = PositionTarget::IGNORE_PX + PositionTarget::IGNORE_PY + PositionTarget::IGNORE_PZ + @@ -468,14 +569,22 @@ void publish(const ros::Time stamp) PositionTarget::IGNORE_AFY + PositionTarget::IGNORE_AFZ; position_raw_msg.type_mask += setpoint_yaw_type == YAW ? PositionTarget::IGNORE_YAW_RATE : PositionTarget::IGNORE_YAW; - position_raw_msg.velocity = setpoint_velocity_transformed.vector; - position_raw_msg.yaw = tf2::getYaw(setpoint_position_transformed.pose.orientation); - position_raw_msg.yaw_rate = setpoint_yaw_rate; + position_raw_msg.velocity = setpoint_velocity_local.vector; + position_raw_msg.yaw = yaw_local; + position_raw_msg.yaw_rate = setpoint_rates.z; position_raw_pub.publish(position_raw_msg); } if (setpoint_type == ATTITUDE) { - attitude_pub.publish(setpoint_position_transformed); + PoseStamped msg; + msg.header.stamp = stamp; + msg.header.frame_id = local_frame; + msg.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(setpoint_roll, setpoint_pitch, yaw_local); + attitude_pub.publish(msg); + + Thrust thrust_msg; + thrust_msg.header.stamp = stamp; + thrust_msg.thrust = setpoint_thrust; thrust_pub.publish(thrust_msg); } @@ -484,11 +593,12 @@ void publish(const ros::Time stamp) // thrust_pub.publish(thrust_msg); // mavros rates topics waits for rates in local frame // use rates in body frame for simplicity + AttitudeTarget att_raw_msg; att_raw_msg.header.stamp = stamp; att_raw_msg.header.frame_id = fcu_frame; att_raw_msg.type_mask = AttitudeTarget::IGNORE_ATTITUDE; - att_raw_msg.body_rate = rates_msg.twist.angular; - att_raw_msg.thrust = thrust_msg.thrust; + att_raw_msg.body_rate = setpoint_rates; + att_raw_msg.thrust = setpoint_thrust; attitude_raw_pub.publish(att_raw_msg); } } @@ -528,10 +638,59 @@ inline void checkState() throw std::runtime_error("No connection to FCU, https://clover.coex.tech/connection"); } +void publishState() +{ + clover::State msg; + msg.mode = setpoint_type; + msg.yaw_mode = setpoint_yaw_type; + + if (setpoint_position.header.frame_id.empty()) { + msg.x = NAN; + msg.y = NAN; + msg.z = NAN; + } else { + msg.x = setpoint_position.point.x; + msg.y = setpoint_position.point.y; + msg.z = setpoint_altitude.point.z; + } + + msg.speed = nav_speed; + msg.lat = setpoint_lat; + msg.lon = setpoint_lon; + msg.vx = setpoint_velocity.vector.x; + msg.vy = setpoint_velocity.vector.y; + msg.vz = setpoint_velocity.vector.z; + msg.roll = setpoint_roll; + msg.pitch = setpoint_pitch; + msg.yaw = !yaw_frame_id.empty() ? setpoint_yaw : NAN; + + msg.roll_rate = setpoint_rates.x; + msg.pitch_rate = setpoint_rates.y; + msg.yaw_rate = setpoint_rates.z; + msg.thrust = setpoint_thrust; + + if (setpoint_type == VELOCITY) { + msg.xy_frame_id = setpoint_velocity.header.frame_id; + msg.z_frame_id = setpoint_velocity.header.frame_id; + } else { + msg.xy_frame_id = setpoint_position.header.frame_id; + msg.z_frame_id = setpoint_altitude.header.frame_id; + } + msg.yaw_frame_id = yaw_frame_id; + + state_pub.publish(msg); +} + +inline float safe(float value) { + return std::isfinite(value) ? value : 0; +} + #define ENSURE_FINITE(var) { if (!std::isfinite(var)) throw std::runtime_error(#var " argument cannot be NaN or Inf"); } +#define ENSURE_NON_INF(var) { if (std::isinf(var)) throw std::runtime_error(#var " argument cannot be Inf"); } + bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, float vy, float vz, - float pitch, float roll, float yaw, float pitch_rate, float roll_rate, float yaw_rate, // editorconfig-checker-disable-line + float roll, float pitch, float yaw, float roll_rate, float pitch_rate, float yaw_rate, // editorconfig-checker-disable-line float lat, float lon, float thrust, float speed, string frame_id, bool auto_arm, // editorconfig-checker-disable-line uint8_t& success, string& message) // editorconfig-checker-disable-line { @@ -558,69 +717,40 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl auto search = reference_frames.find(frame_id); const string& reference_frame = search == reference_frames.end() ? frame_id : search->second; - // Serve "partial" commands + ENSURE_NON_INF(x); + ENSURE_NON_INF(y); + ENSURE_NON_INF(z); + ENSURE_NON_INF(speed); // TODO: allow inf + ENSURE_NON_INF(vx); + ENSURE_NON_INF(vy); + ENSURE_NON_INF(vz); + ENSURE_NON_INF(roll); + ENSURE_NON_INF(pitch); + ENSURE_NON_INF(roll_rate); + ENSURE_NON_INF(pitch_rate); + ENSURE_NON_INF(yaw_rate); + ENSURE_NON_INF(thrust); - if (!auto_arm && std::isfinite(yaw) && - isnan(x) && isnan(y) && isnan(z) && isnan(vx) && isnan(vy) && isnan(vz) && - isnan(pitch) && isnan(roll) && isnan(thrust) && - isnan(lat) && isnan(lon)) { - // change only the yaw - if (setpoint_type == POSITION || setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == VELOCITY) { - if (!waitTransform(setpoint_position.header.frame_id, frame_id, stamp, transform_timeout)) - throw std::runtime_error("Can't transform from " + frame_id + " to " + setpoint_position.header.frame_id); - - message = "Changing yaw only"; - - QuaternionStamped q; - q.header.frame_id = frame_id; - q.header.stamp = stamp; - q.quaternion = tf::createQuaternionMsgFromYaw(yaw); // TODO: pitch=0, roll=0 is not totally correct - setpoint_position.pose.orientation = tf_buffer.transform(q, setpoint_position.header.frame_id).quaternion; - setpoint_yaw_type = YAW; - goto publish_setpoint; - } else { - throw std::runtime_error("Setting yaw is possible only when position or velocity setpoints active"); - } - } - - if (!auto_arm && std::isfinite(yaw_rate) && - isnan(x) && isnan(y) && isnan(z) && isnan(vx) && isnan(vy) && isnan(vz) && - isnan(pitch) && isnan(roll) && isnan(yaw) && isnan(thrust) && - isnan(lat) && isnan(lon)) { - // change only the yaw rate - if (setpoint_type == POSITION || setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == VELOCITY) { - message = "Changing yaw rate only"; - - setpoint_yaw_type = YAW_RATE; - setpoint_yaw_rate = yaw_rate; - goto publish_setpoint; - } else { - throw std::runtime_error("Setting yaw rate is possible only when position or velocity setpoints active"); - } - } - - // Serve normal commands - - if (sp_type == NAVIGATE || sp_type == POSITION) { - ENSURE_FINITE(x); - ENSURE_FINITE(y); - ENSURE_FINITE(z); - } else if (sp_type == NAVIGATE_GLOBAL) { + if (sp_type == NAVIGATE_GLOBAL) { ENSURE_FINITE(lat); ENSURE_FINITE(lon); - ENSURE_FINITE(z); - } else if (sp_type == VELOCITY) { - ENSURE_FINITE(vx); - ENSURE_FINITE(vy); - ENSURE_FINITE(vz); - } else if (sp_type == ATTITUDE) { - ENSURE_FINITE(pitch); - ENSURE_FINITE(roll); - ENSURE_FINITE(thrust); - } else if (sp_type == RATES) { - ENSURE_FINITE(pitch_rate); - ENSURE_FINITE(roll_rate); - ENSURE_FINITE(thrust); + } + + if (isfinite(x) != isfinite(y)) { + throw std::runtime_error("x and y can be set only together"); + } + + if (isfinite(yaw_rate)) { + if (sp_type > RATES && setpoint_type == ATTITUDE) { + throw std::runtime_error("Yaw rate cannot be set in attitude mode."); + } + } + + // set_altitude + if (sp_type == _ALTITUDE) { + if (setpoint_type == VELOCITY || setpoint_type == ATTITUDE || setpoint_type == RATES) { + throw std::runtime_error("Altitude cannot be set in velocity, attitude or rates mode."); + } } if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL) { @@ -634,20 +764,13 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl speed = default_speed; } - if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY) { - if (yaw_rate != 0 && !std::isnan(yaw)) - throw std::runtime_error("Yaw value should be NaN for setting yaw rate"); - - if (std::isnan(yaw_rate) && std::isnan(yaw)) - throw std::runtime_error("Both yaw and yaw_rate cannot be NaN"); - } - if (sp_type == NAVIGATE_GLOBAL) { if (TIMEOUT(global_position, global_position_timeout)) throw std::runtime_error("No global position"); } - if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY || sp_type == ATTITUDE) { + // if any value need to be transformed to reference frame + if (isfinite(x) || isfinite(y) || isfinite(z) || isfinite(vx) || isfinite(vy) || isfinite(vz) || isfinite(yaw)) { // make sure transform from frame_id to reference frame available if (!waitTransform(reference_frame, frame_id, stamp, transform_timeout)) throw std::runtime_error("Can't transform from " + frame_id + " to " + reference_frame); @@ -664,15 +787,26 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl auto xy_in_req_frame = tf_buffer.transform(pose_local, frame_id); x = xy_in_req_frame.pose.position.x; y = xy_in_req_frame.pose.position.y; + setpoint_lat = lat; + setpoint_lon = lon; } // Everything fine - switch setpoint type - setpoint_type = sp_type; + if (sp_type <= RATES) { + setpoint_type = sp_type; + } - if (sp_type != NAVIGATE && sp_type != NAVIGATE_GLOBAL) { + if (setpoint_type != NAVIGATE && setpoint_type != NAVIGATE_GLOBAL) { nav_from_sp_flag = false; } + if (auto_arm || setpoint_type == VELOCITY || setpoint_type == ATTITUDE || setpoint_type == RATES) { + // invalidate position setpoint + setpoint_position.header.frame_id = ""; + setpoint_altitude.header.frame_id = ""; + yaw_frame_id = ""; + } + if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL) { // starting point if (nav_from_sp && nav_from_sp_flag) { @@ -681,89 +815,139 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl } else { nav_start = local_position; } - nav_speed = speed; + + if (!isnan(speed)) { + nav_speed = speed; + } + nav_from_sp_flag = true; } - // if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY) { - // if (std::isnan(yaw) && yaw_rate == 0) { - // // keep yaw unchanged - // // TODO: this is incorrect, because we need yaw in desired frame - // yaw = tf2::getYaw(local_position.pose.orientation); - // } - // } + // handle position + if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) { - if (sp_type == POSITION || sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == VELOCITY || sp_type == ATTITUDE) { - // destination point and/or attitude - PoseStamped ps; - ps.header.frame_id = frame_id; - ps.header.stamp = stamp; - ps.pose.position.x = x; - ps.pose.position.y = y; - ps.pose.position.z = z; - ps.pose.orientation.w = 1.0; // Ensure quaternion is always valid + PointStamped desired; + desired.header.frame_id = frame_id; + desired.header.stamp = stamp; + desired.point.x = safe(x); + desired.point.y = safe(y); + desired.point.z = safe(z); - if (sp_type == ATTITUDE) { - ps.pose.position.x = 0; - ps.pose.position.y = 0; - ps.pose.position.z = 0; - ps.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw); - } else if (std::isnan(yaw)) { - setpoint_yaw_type = YAW_RATE; - setpoint_yaw_rate = yaw_rate; - } else if (std::isinf(yaw) && yaw > 0) { - // yaw towards - setpoint_yaw_type = TOWARDS; - yaw = 0; - setpoint_yaw_rate = 0; - } else { - setpoint_yaw_type = YAW; - setpoint_yaw_rate = 0; - ps.pose.orientation = tf::createQuaternionMsgFromYaw(yaw); + // transform to reference frame + desired = tf_buffer.transform(desired, reference_frame); + + // set horizontal position + if (isfinite(x) && isfinite(y)) { + setpoint_position = desired; + } else if (setpoint_position.header.frame_id.empty()) { + // TODO: use transform for current stamp + setpoint_position.header = local_position.header; + setpoint_position.point = local_position.pose.position; } - tf_buffer.transform(ps, setpoint_position, reference_frame); + // set altitude + if (isfinite(z)) { + setpoint_altitude = desired; + } else if (setpoint_altitude.header.frame_id.empty()) { + setpoint_altitude.header = local_position.header; + setpoint_altitude.point = local_position.pose.position; + } } + // handle velocity if (sp_type == VELOCITY) { - Vector3Stamped vel; - vel.header.frame_id = frame_id; - vel.header.stamp = stamp; - vel.vector.x = vx; - vel.vector.y = vy; - vel.vector.z = vz; - tf_buffer.transform(vel, setpoint_velocity, reference_frame); + // TODO: allow setting different modes by altitude and xy + Vector3Stamped desired; + desired.header.frame_id = frame_id; + desired.header.stamp = stamp; + desired.vector.x = safe(vx); + desired.vector.y = safe(vy); + desired.vector.z = safe(vz); + + // transform to reference frame + desired = tf_buffer.transform(desired, reference_frame); + setpoint_velocity.header = desired.header; + + // set horizontal velocity + if (isfinite(vx) && isfinite(vy)) { + setpoint_velocity.vector.x = desired.vector.x; + setpoint_velocity.vector.y = desired.vector.y; + } + + // set vertical velocity + if (isfinite(vz)) { + setpoint_velocity.vector.z = desired.vector.z; + } } - if (sp_type == ATTITUDE || sp_type == RATES) { - thrust_msg.thrust = thrust; + // handle yaw + if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY || sp_type == ATTITUDE || sp_type == _YAW) { + if (isfinite(yaw)) { + setpoint_yaw_type = YAW; + QuaternionStamped desired; + desired.header.frame_id = frame_id; + desired.header.stamp = stamp; + desired.quaternion = tf::createQuaternionMsgFromYaw(yaw); + + // transform to reference frame + desired = tf_buffer.transform(desired, reference_frame); + setpoint_yaw = tf2::getYaw(desired.quaternion); + yaw_frame_id = reference_frame; + + } else if (isinf(yaw) && yaw > 0) { + // yaw towards + setpoint_yaw_type = TOWARDS; + + } else if (yaw_frame_id.empty() || sp_type == _YAW) { + // yaw is nan and not set previously OR set_yaw(yaw=nan) was called + setpoint_yaw_type = YAW; + setpoint_yaw = tf2::getYaw(local_position.pose.orientation); // set yaw to current yaw + yaw_frame_id = local_position.header.frame_id; + } } - if (sp_type == RATES) { - rates_msg.twist.angular.x = roll_rate; - rates_msg.twist.angular.y = pitch_rate; - rates_msg.twist.angular.z = yaw_rate; + // handle roll + if (isfinite(roll)) { + setpoint_roll = roll; + } + + // handle pitch + if (isfinite(pitch)) { + setpoint_pitch = pitch; + } + + // handle yaw rate + if (isfinite(yaw_rate)) { + setpoint_yaw_type = YAW_RATE; + setpoint_rates.z = yaw_rate; + } + + // handle pitch rate + if (isfinite(roll_rate)) { + setpoint_rates.x = roll_rate; + } + + // handle roll rate + if (isfinite(pitch_rate)) { + setpoint_rates.y = pitch_rate; + } + + // handle thrust + if (isfinite(thrust)) { + setpoint_thrust = thrust; } wait_armed = auto_arm; -publish_setpoint: publish(stamp); // calculate initial transformed messages first setpoint_timer.start(); - // publish target frame - if (!target.child_frame_id.empty()) { - if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) { - target.header.frame_id = setpoint_position.header.frame_id; - target.header.stamp = stamp; - target.transform.translation.x = setpoint_position.pose.position.x; - target.transform.translation.y = setpoint_position.pose.position.y; - target.transform.translation.z = setpoint_position.pose.position.z; - target.transform.rotation = setpoint_position.pose.orientation; - static_transform_broadcaster->sendTransform(target); - } + if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) { + publishTarget(stamp, true); } + publishState(); + if (auto_arm) { offboardAndArm(); wait_armed = false; @@ -788,27 +972,39 @@ publish_setpoint: } bool navigate(Navigate::Request& req, Navigate::Response& res) { - return serve(NAVIGATE, req.x, req.y, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, req.yaw_rate, NAN, NAN, NAN, req.speed, req.frame_id, req.auto_arm, res.success, res.message); + return serve(NAVIGATE, req.x, req.y, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, NAN, NAN, NAN, NAN, req.speed, req.frame_id, req.auto_arm, res.success, res.message); } bool navigateGlobal(NavigateGlobal::Request& req, NavigateGlobal::Response& res) { - return serve(NAVIGATE_GLOBAL, NAN, NAN, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, req.yaw_rate, req.lat, req.lon, NAN, req.speed, req.frame_id, req.auto_arm, res.success, res.message); + return serve(NAVIGATE_GLOBAL, NAN, NAN, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, NAN, req.lat, req.lon, NAN, req.speed, req.frame_id, req.auto_arm, res.success, res.message); +} + +bool setAltitude(SetAltitude::Request& req, SetAltitude::Response& res) { + return serve(_ALTITUDE, NAN, NAN, req.z, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.frame_id, false, res.success, res.message); +} + +bool setYaw(SetYaw::Request& req, SetYaw::Response& res) { + return serve(_YAW, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.frame_id, false, res.success, res.message); +} + +bool setYawRate(SetYawRate::Request& req, SetYawRate::Response& res) { + return serve(_YAW_RATE, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.yaw_rate, NAN, NAN, NAN, NAN, "", false, res.success, res.message); } bool setPosition(SetPosition::Request& req, SetPosition::Response& res) { - return serve(POSITION, req.x, req.y, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, req.yaw_rate, NAN, NAN, NAN, NAN, req.frame_id, req.auto_arm, res.success, res.message); + return serve(POSITION, req.x, req.y, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.frame_id, req.auto_arm, res.success, res.message); } bool setVelocity(SetVelocity::Request& req, SetVelocity::Response& res) { - return serve(VELOCITY, NAN, NAN, NAN, req.vx, req.vy, req.vz, NAN, NAN, req.yaw, NAN, NAN, req.yaw_rate, NAN, NAN, NAN, NAN, req.frame_id, req.auto_arm, res.success, res.message); + return serve(VELOCITY, NAN, NAN, NAN, req.vx, req.vy, req.vz, NAN, NAN, req.yaw, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.frame_id, req.auto_arm, res.success, res.message); } bool setAttitude(SetAttitude::Request& req, SetAttitude::Response& res) { - return serve(ATTITUDE, NAN, NAN, NAN, NAN, NAN, NAN, req.pitch, req.roll, req.yaw, NAN, NAN, NAN, NAN, NAN, req.thrust, NAN, req.frame_id, req.auto_arm, res.success, res.message); + return serve(ATTITUDE, NAN, NAN, NAN, NAN, NAN, NAN, req.roll, req.pitch, req.yaw, NAN, NAN, NAN, NAN, NAN, req.thrust, NAN, req.frame_id, req.auto_arm, res.success, res.message); } bool setRates(SetRates::Request& req, SetRates::Response& res) { - return serve(RATES, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.pitch_rate, req.roll_rate, req.yaw_rate, NAN, NAN, req.thrust, NAN, "", req.auto_arm, res.success, res.message); + return serve(RATES, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.roll_rate, req.pitch_rate, req.yaw_rate, NAN, NAN, req.thrust, NAN, "", req.auto_arm, res.success, res.message); } bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) @@ -840,9 +1036,7 @@ bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) auto start = ros::Time::now(); while (ros::ok()) { if (state.mode == "AUTO.LAND") { - res.success = true; - busy = false; - return true; + break; } if (ros::Time::now() - start > land_timeout) throw std::runtime_error("Land request timed out"); @@ -851,6 +1045,18 @@ bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) r.sleep(); } + // stop setpoints and invalidate position setpoint + setpoint_timer.stop(); + setpoint_type = NONE; + setpoint_position.header.frame_id = ""; + setpoint_altitude.header.frame_id = ""; + yaw_frame_id = ""; + publishState(); + + res.success = true; + busy = false; + return true; + } catch (const std::exception& e) { res.message = e.what(); ROS_INFO("%s", e.what()); @@ -863,6 +1069,11 @@ bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) bool release(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) { setpoint_timer.stop(); + setpoint_type = NONE; + setpoint_position.header.frame_id = ""; + setpoint_altitude.header.frame_id = ""; + yaw_frame_id = ""; + publishState(); res.success = true; return true; } @@ -888,6 +1099,7 @@ int main(int argc, char **argv) nh_priv.param("check_kill_switch", check_kill_switch, true); nh_priv.param("default_speed", default_speed, 0.5f); nh_priv.param("body_frame", body.child_frame_id, "body"); + nh_priv.param("terrain_frame", terrain.child_frame_id, "terrain"); nh_priv.getParam("reference_frames", reference_frames); // Default reference frames @@ -923,6 +1135,12 @@ int main(int argc, char **argv) auto manual_control_sub = nh.subscribe(mavros + "/manual_control/control", 1, &handleMessage); auto local_position_sub = nh.subscribe(mavros + "/local_position/pose", 1, &handleLocalPosition); + ros::Subscriber altitude_sub; + if (!body.child_frame_id.empty() && !terrain.child_frame_id.empty()) { + terrain.header.frame_id = local_frame; + altitude_sub = nh.subscribe(mavros + "/altitude", 1, &handleAltitude); + } + // Setpoint publishers position_pub = nh.advertise(mavros + "/setpoint_position/local", 1); position_raw_pub = nh.advertise(mavros + "/setpoint_raw/local", 1); @@ -931,10 +1149,16 @@ int main(int argc, char **argv) rates_pub = nh.advertise(mavros + "/setpoint_attitude/cmd_vel", 1); thrust_pub = nh.advertise(mavros + "/setpoint_attitude/thrust", 1); + // State publisher + state_pub = nh_priv.advertise("state", 1, true); + // Service servers auto gt_serv = nh.advertiseService("get_telemetry", &getTelemetry); auto na_serv = nh.advertiseService("navigate", &navigate); auto ng_serv = nh.advertiseService("navigate_global", &navigateGlobal); + auto sl_serv = nh.advertiseService("set_altitude", &setAltitude); + auto ya_serv = nh.advertiseService("set_yaw", &setYaw); + auto yr_serv = nh.advertiseService("set_yaw_rate", &setYawRate); auto sp_serv = nh.advertiseService("set_position", &setPosition); auto sv_serv = nh.advertiseService("set_velocity", &setVelocity); auto sa_serv = nh.advertiseService("set_attitude", &setAttitude); @@ -948,7 +1172,7 @@ int main(int argc, char **argv) position_msg.header.frame_id = local_frame; position_raw_msg.header.frame_id = local_frame; position_raw_msg.coordinate_frame = PositionTarget::FRAME_LOCAL_NED; - rates_msg.header.frame_id = fcu_frame; + //rates_msg.header.frame_id = fcu_frame; ROS_INFO("ready"); ros::spin(); diff --git a/clover/srv/GetTelemetry.srv b/clover/srv/GetTelemetry.srv index b9ae484e..a5b6e9e7 100644 --- a/clover/srv/GetTelemetry.srv +++ b/clover/srv/GetTelemetry.srv @@ -13,11 +13,11 @@ float32 alt float32 vx float32 vy float32 vz -float32 pitch float32 roll +float32 pitch float32 yaw -float32 pitch_rate float32 roll_rate +float32 pitch_rate float32 yaw_rate float32 voltage float32 cell_voltage diff --git a/clover/srv/Navigate.srv b/clover/srv/Navigate.srv index f607a437..7a4943ed 100644 --- a/clover/srv/Navigate.srv +++ b/clover/srv/Navigate.srv @@ -2,7 +2,6 @@ float32 x float32 y float32 z float32 yaw -float32 yaw_rate float32 speed string frame_id bool auto_arm diff --git a/clover/srv/NavigateGlobal.srv b/clover/srv/NavigateGlobal.srv index 69fbc444..5822f2b0 100644 --- a/clover/srv/NavigateGlobal.srv +++ b/clover/srv/NavigateGlobal.srv @@ -2,7 +2,6 @@ float64 lat float64 lon float32 z float32 yaw -float32 yaw_rate float32 speed string frame_id bool auto_arm diff --git a/clover/srv/SetAltitude.srv b/clover/srv/SetAltitude.srv new file mode 100644 index 00000000..1b817ec5 --- /dev/null +++ b/clover/srv/SetAltitude.srv @@ -0,0 +1,5 @@ +float32 z +string frame_id +--- +bool success +string message diff --git a/clover/srv/SetAttitude.srv b/clover/srv/SetAttitude.srv index b41c1072..ef8cf8b2 100644 --- a/clover/srv/SetAttitude.srv +++ b/clover/srv/SetAttitude.srv @@ -1,5 +1,5 @@ -float32 pitch float32 roll +float32 pitch float32 yaw float32 thrust string frame_id diff --git a/clover/srv/SetPosition.srv b/clover/srv/SetPosition.srv index 20c73f05..80a01b29 100644 --- a/clover/srv/SetPosition.srv +++ b/clover/srv/SetPosition.srv @@ -2,7 +2,6 @@ float32 x float32 y float32 z float32 yaw -float32 yaw_rate string frame_id bool auto_arm --- diff --git a/clover/srv/SetRates.srv b/clover/srv/SetRates.srv index f6ebddf9..b284a5c8 100644 --- a/clover/srv/SetRates.srv +++ b/clover/srv/SetRates.srv @@ -1,5 +1,5 @@ -float32 pitch_rate float32 roll_rate +float32 pitch_rate float32 yaw_rate float32 thrust bool auto_arm diff --git a/clover/srv/SetVelocity.srv b/clover/srv/SetVelocity.srv index 25e174ac..908be1b8 100644 --- a/clover/srv/SetVelocity.srv +++ b/clover/srv/SetVelocity.srv @@ -2,7 +2,6 @@ float32 vx float32 vy float32 vz float32 yaw -float32 yaw_rate string frame_id bool auto_arm --- diff --git a/clover/srv/SetYaw.srv b/clover/srv/SetYaw.srv new file mode 100644 index 00000000..3e61d323 --- /dev/null +++ b/clover/srv/SetYaw.srv @@ -0,0 +1,5 @@ +float32 yaw +string frame_id +--- +bool success +string message diff --git a/clover/srv/SetYawRate.srv b/clover/srv/SetYawRate.srv new file mode 100644 index 00000000..82a21640 --- /dev/null +++ b/clover/srv/SetYawRate.srv @@ -0,0 +1,4 @@ +float32 yaw_rate +--- +bool success +string message diff --git a/clover/test/offboard.py b/clover/test/offboard.py new file mode 100755 index 00000000..426fb453 --- /dev/null +++ b/clover/test/offboard.py @@ -0,0 +1,402 @@ +import rospy +import pytest +from pytest import approx +import threading +import mavros_msgs.msg +from geometry_msgs.msg import PoseStamped +from clover import srv +from clover.msg import State +from math import nan, inf +import tf2_ros +import tf2_geometry_msgs + +@pytest.fixture() +def node(): + return rospy.init_node('offboard_test', anonymous=True) + +@pytest.fixture +def tf_buffer(): + buf = tf2_ros.Buffer() + tf2_ros.TransformListener(buf) + return buf + +def get_state(): + return rospy.wait_for_message('/simple_offboard/state', State, timeout=1) + +def get_navigate_target(tf_buffer): + target = tf_buffer.lookup_transform('map', 'navigate_target', rospy.get_rostime(), rospy.Duration(1)) + assert target.child_frame_id == 'navigate_target' + return target + +def test_offboard(node, tf_buffer): + navigate = rospy.ServiceProxy('navigate', srv.Navigate) + set_position = rospy.ServiceProxy('set_position', srv.SetPosition) + set_altitude = rospy.ServiceProxy('set_altitude', srv.SetAltitude) + set_yaw = rospy.ServiceProxy('set_yaw', srv.SetYaw) + set_yaw_rate = rospy.ServiceProxy('set_yaw_rate', srv.SetYawRate) + set_velocity = rospy.ServiceProxy('set_velocity', srv.SetVelocity) + set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude) + set_rates = rospy.ServiceProxy('set_rates', srv.SetRates) + get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry) + res = navigate() + assert res.success == False + assert res.message.startswith('State timeout') + + telem = get_telemetry() + assert telem.connected == False + + state_pub = rospy.Publisher('/mavros/state', mavros_msgs.msg.State, latch=True, queue_size=1) + state_msg = mavros_msgs.msg.State(mode='OFFBOARD', armed=True) + + def publish_state(): + r = rospy.Rate(2) + while not rospy.is_shutdown(): + state_msg.header.stamp = rospy.Time.now() + state_pub.publish(state_msg) + r.sleep() + + # start publishing state + threading.Thread(target=publish_state, daemon=True).start() + rospy.sleep(0.5) + + telem = get_telemetry() + assert telem.connected == False + + res = navigate() + assert res.success == False + assert res.message.startswith('No connection to FCU') + + state_msg.connected = True + rospy.sleep(1) + + telem = get_telemetry() + assert telem.connected == True + + res = navigate() + assert res.success == False + assert res.message.startswith('No local position') + + local_position_pub = rospy.Publisher('/mavros/local_position/pose', PoseStamped, latch=True, queue_size=1) + local_position_msg = PoseStamped() + local_position_msg.header.frame_id = 'map' + local_position_msg.pose.position.x = 1 + local_position_msg.pose.position.y = 2 + local_position_msg.pose.position.z = 3 + local_position_msg.pose.orientation.w = 1 + + def publish_local_position(): + r = rospy.Rate(30) + while not rospy.is_shutdown(): + local_position_msg.header.stamp = rospy.Time.now() + local_position_pub.publish(local_position_msg) + r.sleep() + + # start publishing local position + threading.Thread(target=publish_local_position, daemon=True).start() + rospy.sleep(0.5) + + # check body frame + body = tf_buffer.lookup_transform('map', 'body', rospy.get_rostime(), rospy.Duration(1)) + assert body.child_frame_id == 'body' + assert body.transform.translation.x == approx(1) + assert body.transform.translation.y == approx(2) + assert body.transform.translation.z == approx(3) + + res = navigate(x=3, y=2, z=1, frame_id='map') + assert res.success == True + state = get_state() + assert state.mode == State.MODE_NAVIGATE + assert state.yaw_mode == State.YAW_MODE_YAW + assert state.x == 3 + assert state.y == 2 + assert state.z == 1 + assert state.yaw == 0 + assert state.xy_frame_id == 'map' + assert state.z_frame_id == 'map' + assert state.yaw_frame_id == 'map' + target = get_navigate_target(tf_buffer) + assert target.header.frame_id == 'map' + assert target.transform.translation.x == approx(3) + assert target.transform.translation.y == approx(2) + assert target.transform.translation.z == approx(1) + assert target.transform.rotation.x == 0 + assert target.transform.rotation.y == 0 + assert target.transform.rotation.z == 0 + assert target.transform.rotation.w == 1 + + # try to set only the y + res = navigate(x=nan, y=1, z=nan) + assert res.success == False + assert res.message.startswith('x and y can be set only together') + + # set z in body frame + res = navigate(x=nan, y=nan, z=1, frame_id='body') + assert res.success == True + state = get_state() + assert state.mode == State.MODE_NAVIGATE + assert state.yaw_mode == State.YAW_MODE_YAW + assert state.x == 3 + assert state.y == 2 + assert state.z == 4 + assert state.yaw == 0 + assert state.xy_frame_id == 'map' + assert state.z_frame_id == 'map' + assert state.yaw_frame_id == 'map' + + # set xy in test frame + res = navigate(x=1, y=2, z=nan, frame_id='test') + assert res.success == True + state = get_state() + assert state.mode == State.MODE_NAVIGATE + assert state.yaw_mode == State.YAW_MODE_YAW + assert state.x == 1 + assert state.y == 2 + assert state.z == 4 + assert state.yaw == 0 + assert state.xy_frame_id == 'test' + assert state.z_frame_id == 'map' + assert state.yaw_frame_id == 'test' + + # auto_arm should invalidate the setpoint + res = navigate(x=nan, y=nan, z=1, frame_id='map', auto_arm=True) + assert res.success == True + state = get_state() + assert state.mode == State.MODE_NAVIGATE + assert state.yaw_mode == State.YAW_MODE_YAW + assert state.x == 1 + assert state.y == 2 + assert state.z == 1 + assert state.yaw == 0 + assert state.xy_frame_id == 'map' + assert state.z_frame_id == 'map' + assert state.yaw_frame_id == 'map' + + # set_attitude should invalidate the setpoint + res = set_attitude() + assert res.success == True + + res = navigate(x=5, y=6, z=nan, yaw=nan, frame_id='map') + assert res.success == True + state = get_state() + assert state.mode == State.MODE_NAVIGATE + assert state.yaw_mode == State.YAW_MODE_YAW + assert state.x == 5 + assert state.y == 6 + assert state.z == 3 + assert state.yaw == 0 + assert state.xy_frame_id == 'map' + assert state.z_frame_id == 'map' + assert state.yaw_frame_id == 'map' + + # test set_altitude + res = set_altitude(z=7, frame_id='test') + assert res.success == True + state = get_state() + assert state.mode == State.MODE_NAVIGATE + assert state.yaw_mode == State.YAW_MODE_YAW + assert state.x == 5 + assert state.y == 6 + assert state.z == 7 + assert state.yaw == 0 + assert state.xy_frame_id == 'map' + assert state.z_frame_id == 'test' + assert state.yaw_frame_id == 'map' + + # test set_yaw + res = set_yaw(yaw=0.5, frame_id='test2') + assert res.success == True + state = get_state() + assert state.mode == State.MODE_NAVIGATE + assert state.yaw_mode == State.YAW_MODE_YAW + assert state.x == 5 + assert state.y == 6 + assert state.z == 7 + assert state.yaw == 0.5 + assert state.xy_frame_id == 'map' + assert state.z_frame_id == 'test' + assert state.yaw_frame_id == 'test2' + + # test set_yaw_rate + res = set_yaw_rate(yaw_rate=2) + assert res.success == True + state = get_state() + assert state.mode == State.MODE_NAVIGATE + assert state.yaw_mode == State.YAW_MODE_YAW_RATE + assert state.x == 5 + assert state.y == 6 + assert state.z == 7 + assert state.yaw_rate == 2 + assert state.xy_frame_id == 'map' + assert state.z_frame_id == 'test' + + # navigate(yaw=nan) should keep yaw rate mode + res = navigate(x=nan, y=nan, z=nan, yaw=nan) + assert res.success == True + state = get_state() + assert state.mode == State.MODE_NAVIGATE + assert state.yaw_mode == State.YAW_MODE_YAW_RATE + assert state.x == 5 + assert state.y == 6 + assert state.z == 7 + assert state.yaw_rate == 2 + assert state.xy_frame_id == 'map' + assert state.z_frame_id == 'test' + + # set_yaw(nan) should change back to yaw mode + res = set_yaw(yaw=nan) + assert res.success == True + state = get_state() + assert state.mode == State.MODE_NAVIGATE + assert state.yaw_mode == State.YAW_MODE_YAW + assert state.yaw == 0 + assert state.yaw_frame_id == 'map' + + # test set_position + res = set_position(x=nan, y=nan, z=13, yaw=nan, frame_id='test2') + assert res.success == True + state = get_state() + assert state.mode == State.MODE_POSITION + assert state.yaw_mode == State.YAW_MODE_YAW + assert state.x == 5 + assert state.y == 6 + assert state.z == 13 + assert state.yaw == 0 + assert state.xy_frame_id == 'map' + assert state.z_frame_id == 'test2' + assert state.yaw_frame_id == 'map' + + # set_altitude should not change the mode + res = set_altitude(z=3, frame_id='test') + assert res.success == True + state = get_state() + assert state.mode == State.MODE_POSITION + assert state.yaw_mode == State.YAW_MODE_YAW + assert state.x == 5 + assert state.y == 6 + assert state.z == 3 + assert state.yaw == 0 + assert state.xy_frame_id == 'map' + assert state.z_frame_id == 'test' + assert state.yaw_frame_id == 'map' + + # set_yaw should not change the main mode + res = set_yaw(yaw=1, frame_id='test2') + assert res.success == True + state = get_state() + assert state.mode == State.MODE_POSITION + assert state.yaw_mode == State.YAW_MODE_YAW + assert state.x == 5 + assert state.y == 6 + assert state.z == 3 + assert state.yaw == 1 + assert state.xy_frame_id == 'map' + assert state.z_frame_id == 'test' + assert state.yaw_frame_id == 'test2' + + # test set_velocity + res = set_velocity(vx=1, frame_id='body') + state = get_state() + assert state.mode == State.MODE_VELOCITY + assert state.yaw_mode == State.YAW_MODE_YAW + assert state.vx == 1 + assert state.vy == 0 + assert state.vz == 0 + assert state.yaw == 0 + assert state.xy_frame_id == 'map' + assert state.z_frame_id == 'map' + assert state.yaw_frame_id == 'map' + + # set_altitude should not work in velocity mode + res = set_altitude(z=3, frame_id='test') + assert res.success == False + assert res.message.startswith('Altitude cannot be set in') + + # test set_attitude + res = set_attitude(roll=0.1, pitch=0.2, yaw=0.3, thrust=0.5) + assert res.success == True + state = get_state() + assert state.mode == State.MODE_ATTITUDE + assert state.yaw_mode == State.YAW_MODE_YAW + assert state.roll == approx(0.1) + assert state.pitch == approx(0.2) + assert state.yaw == approx(0.3) + assert state.thrust == approx(0.5) + assert state.yaw_frame_id == 'map' + msg = rospy.wait_for_message('/mavros/setpoint_attitude/attitude', PoseStamped, timeout=3) + # Tait-Bryan ZYX angle (rzyx) converted to quaternion + assert msg.pose.orientation.x == approx(0.0342708) + assert msg.pose.orientation.y == approx(0.10602051) + assert msg.pose.orientation.z == approx(0.14357218) + assert msg.pose.orientation.w == approx(0.98334744) + msg = rospy.wait_for_message('/mavros/setpoint_attitude/thrust', mavros_msgs.msg.Thrust, timeout=3) + assert msg.thrust == approx(0.5) + + # set_yaw should work in attitude mode + res = set_yaw(yaw=0.7, frame_id='test2') + assert res.success == True + state = get_state() + assert state.mode == State.MODE_ATTITUDE + assert state.yaw_mode == State.YAW_MODE_YAW + assert state.roll == approx(0.1) + assert state.pitch == approx(0.2) + assert state.yaw == approx(0.7) + assert state.thrust == approx(0.5) + assert state.yaw_frame_id == 'test2' + + # set_yaw_rate should not work in attitude mode + res = set_yaw_rate(yaw_rate=0.3) + assert res.success == False + assert res.message.startswith('Yaw rate cannot be set in') + + # test set_rates + res = set_rates(roll_rate=nan, pitch_rate=nan, yaw_rate=0.3, thrust=0.6) + assert res.success == True + state = get_state() + assert state.mode == State.MODE_RATES + assert state.yaw_mode == State.YAW_MODE_YAW_RATE + assert state.roll_rate == approx(0) + assert state.pitch_rate == approx(0) + assert state.yaw_rate == approx(0.3) + assert state.thrust == approx(0.6) + msg = rospy.wait_for_message('/mavros/setpoint_raw/attitude', mavros_msgs.msg.AttitudeTarget, timeout=3) + assert msg.thrust == approx(0.6) + + res = set_rates(roll_rate=0.3, pitch_rate=0.2, yaw_rate=0.1, thrust=0.4) + assert res.success == True + state = get_state() + assert state.mode == State.MODE_RATES + assert state.yaw_mode == State.YAW_MODE_YAW_RATE + assert state.roll_rate == approx(0.3) + assert state.pitch_rate == approx(0.2) + assert state.yaw_rate == approx(0.1) + assert state.thrust == approx(0.4) + + res = set_rates(roll_rate=nan, pitch_rate=nan, yaw_rate=nan, thrust=0.3) + assert res.success == True + state = get_state() + assert state.mode == State.MODE_RATES + assert state.yaw_mode == State.YAW_MODE_YAW_RATE + assert state.roll_rate == approx(0.3) + assert state.pitch_rate == approx(0.2) + assert state.yaw_rate == approx(0.1) + assert state.thrust == approx(0.3) + msg = rospy.wait_for_message('/mavros/setpoint_raw/attitude', mavros_msgs.msg.AttitudeTarget, timeout=3) + assert msg.type_mask == mavros_msgs.msg.AttitudeTarget.IGNORE_ATTITUDE + assert msg.body_rate.x == approx(0.3) + assert msg.body_rate.y == approx(0.2) + assert msg.body_rate.z == approx(0.1) + + # set_yaw_rate should work in rates mode + res = set_yaw_rate(yaw_rate=0.4) + assert res.success == True + state = get_state() + assert state.mode == State.MODE_RATES + assert state.yaw_mode == State.YAW_MODE_YAW_RATE + assert state.roll_rate == approx(0.3) + assert state.pitch_rate == approx(0.2) + assert state.yaw_rate == approx(0.4) + assert state.thrust == approx(0.3) + + res = set_rates(roll_rate=inf) + assert res.success == False + assert res.message == 'roll_rate argument cannot be Inf' diff --git a/clover/test/offboard.test b/clover/test/offboard.test new file mode 100644 index 00000000..470fe4fe --- /dev/null +++ b/clover/test/offboard.test @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/clover_blocks/www/blocks.js b/clover_blocks/www/blocks.js index b10e315f..f5062b3d 100644 --- a/clover_blocks/www/blocks.js +++ b/clover_blocks/www/blocks.js @@ -15,6 +15,7 @@ const COLOR_GPIO = 200; const DOCS_URL = 'https://clover.coex.tech/en/blocks.html'; var frameIds = [["body", "BODY"], ["markers map", "ARUCO_MAP"], ["marker", "ARUCO"], ["last navigate target", "NAVIGATE_TARGET"], ["map", "MAP"]]; +var frameIdsWithTerrain = frameIds.concat([["terrain", "TERRAIN"]]); function considerFrameId(e) { if (!(e instanceof Blockly.Events.Change || e instanceof Blockly.Events.Create)) return; @@ -22,7 +23,7 @@ function considerFrameId(e) { var frameId = this.getFieldValue('FRAME_ID'); // set appropriate coordinates labels if (this.getInput('X')) { // block has x-y-z fields - if (frameId == 'BODY' || frameId == 'NAVIGATE_TARGET' || frameId == 'BASE_LINK') { + if (frameId == 'BODY' || frameId == 'NAVIGATE_TARGET' || frameId == 'BASE_LINK' || frameId == 'TERRAIN') { this.getInput('X').fieldRow[0].setValue('forward'); this.getInput('Y').fieldRow[0].setValue('left'); this.getInput('Z').fieldRow[0].setValue('up'); @@ -59,8 +60,8 @@ function updateSetpointBlock(e) { this.getInput('VY').setVisible(velocity); this.getInput('VZ').setVisible(velocity); this.getInput('YAW').setVisible(attitude); - this.getInput('PITCH').setVisible(attitude); this.getInput('ROLL').setVisible(attitude); + this.getInput('PITCH').setVisible(attitude); this.getInput('THRUST').setVisible(attitude); this.getInput('RELATIVE_TO').setVisible(type != 'RATES'); @@ -73,7 +74,7 @@ function updateSetpointBlock(e) { Blockly.Blocks['navigate'] = { init: function () { - let navFrameId = frameIds.slice(); + let navFrameId = frameIdsWithTerrain.slice(); navFrameId.push(['global', 'GLOBAL_LOCAL']) navFrameId.push(['global, WGS 84 alt.', 'GLOBAL']) this.appendDummyInput() @@ -163,14 +164,14 @@ Blockly.Blocks['setpoint'] = { this.appendValueInput("VZ") .setCheck("Number") .appendField("vz"); - this.appendValueInput("PITCH") - .setCheck("Number") - .appendField("pitch") - .setVisible(false); this.appendValueInput("ROLL") .setCheck("Number") .appendField("roll") .setVisible(false); + this.appendValueInput("PITCH") + .setCheck("Number") + .appendField("pitch") + .setVisible(false); this.appendValueInput("YAW") .setCheck("Number") .appendField("yaw") @@ -213,7 +214,7 @@ Blockly.Blocks['get_position'] = { .appendField("current") .appendField(new Blockly.FieldDropdown([["x", "X"], ["y", "Y"], ["z", "Z"], ["vx", "VX"], ["vy", "VY"], ["vz", "VZ"]]), "FIELD") .appendField("relative to") - .appendField(new Blockly.FieldDropdown(frameIds), "FRAME_ID"); + .appendField(new Blockly.FieldDropdown(frameIdsWithTerrain), "FRAME_ID"); this.appendValueInput("ID") .setCheck("Number") .appendField("with ID") @@ -247,7 +248,7 @@ Blockly.Blocks['get_attitude'] = { init: function () { this.appendDummyInput() .appendField("current") - .appendField(new Blockly.FieldDropdown([["pitch", "PITCH"], ["roll", "ROLL"], ["pitch rate", "PITCH_RATE"], ["roll rate", "ROLL_RATE"], ["yaw rate", "YAW_RATE"]]), "FIELD"); + .appendField(new Blockly.FieldDropdown([["roll", "ROLL"], ["pitch", "PITCH"], ["roll rate", "ROLL_RATE"], ["pitch rate", "PITCH_RATE"], ["yaw rate", "YAW_RATE"]]), "FIELD"); this.setOutput(true, "Number"); this.setColour(COLOR_STATE); this.setTooltip("Returns current orientation or angle rates in degree or degree per second (not radian)."); @@ -509,7 +510,7 @@ Blockly.Blocks['distance'] = { .appendField("z"); this.appendDummyInput() .appendField("relative to") - .appendField(new Blockly.FieldDropdown([["markers map", "ARUCO_MAP"], ["marker", "ARUCO"], ["last navigate target", "NAVIGATE_TARGET"]]), "FRAME_ID"); + .appendField(new Blockly.FieldDropdown([["markers map", "ARUCO_MAP"], ["marker", "ARUCO"], ["last navigate target", "NAVIGATE_TARGET"], ["terrain", "TERRAIN"]]), "FRAME_ID"); this.appendValueInput("ID") .setCheck("Number") .appendField("with ID") diff --git a/clover_blocks/www/index.html b/clover_blocks/www/index.html index 8f394198..fcd4060f 100644 --- a/clover_blocks/www/index.html +++ b/clover_blocks/www/index.html @@ -69,8 +69,8 @@ 0 0 0 - 0 0 + 0 0 0.5 0 diff --git a/clover_blocks/www/python.js b/clover_blocks/www/python.js index 9cf67938..42de72a2 100644 --- a/clover_blocks/www/python.js +++ b/clover_blocks/www/python.js @@ -276,10 +276,11 @@ Blockly.Python.angle = function(block) { } Blockly.Python.set_yaw = function(block) { + rosDefinitions.setYaw = true; simpleOffboard(); let yaw = Blockly.Python.valueToCode(block, 'YAW', Blockly.Python.ORDER_NONE); let frameId = buildFrameId(block); - let code = `navigate(x=float('nan'), y=float('nan'), z=float('nan'), yaw=${yaw}, frame_id=${frameId})\n`; + let code = `set_yaw(yaw=${yaw}, frame_id=${frameId})\n`; if (block.getFieldValue('WAIT') == 'TRUE') { rosDefinitions.waitYaw = true; simpleOffboard(); @@ -328,11 +329,11 @@ Blockly.Python.setpoint = function(block) { } else if (type == 'ATTITUDE') { rosDefinitions.setAttitude = true; simpleOffboard(); - return `set_attitude(pitch=${pitch}, roll=${roll}, yaw=${yaw}, thrust=${thrust}, frame_id=${frameId})\n`; + return `set_attitude(roll=${roll}, pitch=${pitch}, yaw=${yaw}, thrust=${thrust}, frame_id=${frameId})\n`; } else if (type == 'RATES') { rosDefinitions.setRates = true; simpleOffboard(); - return `set_rates(pitch_rate=${pitch}, roll_rate=${roll}, yaw_rate=${yaw}, thrust=${thrust})\n`; + return `set_rates(roll_rate=${roll}, pitch_rate=${pitch}, yaw_rate=${yaw}, thrust=${thrust})\n`; } } diff --git a/docs/en/parameters.md b/docs/en/parameters.md index caef83fd..6bf4b2d3 100644 --- a/docs/en/parameters.md +++ b/docs/en/parameters.md @@ -60,8 +60,8 @@ The `SYS_MC_EST_GROUP` parameter defines the estimator subsystem to use. Estimator subsystem is a group of modules that calculates the current state of the copter using readings from the sensors. The copter state includes: -* Angle rate of the copter – pitch_rate, roll_rate, yaw_rate; -* Copter orientation (in the local coordinate system) – pitch, roll, yaw (one of presentations); +* Angle rate of the copter – roll_rate, pitch_rate, yaw_rate; +* Copter orientation (in the local coordinate system) – roll, pitch, yaw (one of presentations); * Copter position (in the local coordinate system) – x, y, z; * Copter speed (in the local coordinate system) – vx, vy, vz; * Global coordinates of the copter – latitude, longitude, altitude; diff --git a/docs/en/simple_offboard.md b/docs/en/simple_offboard.md index 14985dfa..de0a8186 100644 --- a/docs/en/simple_offboard.md +++ b/docs/en/simple_offboard.md @@ -51,11 +51,11 @@ Response format: * `lat, lon` – drone latitude and longitude *(degrees)*, requires [GPS](gps.md) module; * `alt` – altitude in the global coordinate system (according to [WGS-84](https://ru.wikipedia.org/wiki/WGS_84) standard, not AMSL!), requires [GPS](gps.md) module; * `vx, vy, vz` – drone velocity *(m/s)*; -* `pitch` – pitch angle *(radians)*; * `roll` – roll angle *(radians)*; +* `pitch` – pitch angle *(radians)*; * `yaw` — yaw angle *(radians)*; -* `pitch_rate` — angular pitch velocity *(rad/s)*; * `roll_rate` – angular roll velocity *(rad/s)*; +* `pitch_rate` — angular pitch velocity *(rad/s)*; * `yaw_rate` – angular yaw velocity *(rad/s)*; * `voltage` – total battery voltage *(V)*; * `cell_voltage` – battery cell voltage *(V)*. @@ -261,22 +261,22 @@ set_velocity(vx=1, vy=0.0, vz=0, frame_id='body') ### set_attitude -Set pitch, roll, yaw and throttle level (similar to [the `STABILIZED` mode](modes.md)). This service may be used for lower level control of the drone behavior, or controlling the drone when no reliable data on its position is available. +Set roll, pitch, yaw and throttle level (similar to [the `STABILIZED` mode](modes.md)). This service may be used for lower level control of the drone behavior, or controlling the drone when no reliable data on its position is available. Parameters: -* `pitch`, `roll`, `yaw` – requested pitch, roll, and yaw angle *(radians)*; +* `roll`, `pitch`, `yaw` – requested roll, pitch, and yaw angle *(radians)*; * `thrust` — throttle level, ranges from 0 (no throttle, propellers are stopped) to 1 (full throttle). * `auto_arm` – switch the drone to `OFFBOARD` mode and arm automatically (**the drone will take off**); * `frame_id` – [coordinate system](frames.md) for `yaw` (Default value: `map`). ### set_rates -Set pitch, roll, and yaw rates and the throttle level (similar to [the `ACRO` mode](modes.md)). This is the lowest drone control level (excluding direct control of motor rotation speed). This service may be used to automatically perform aerobatic tricks (e.g., flips). +Set roll, pitch, and yaw rates and the throttle level (similar to [the `ACRO` mode](modes.md)). This is the lowest drone control level (excluding direct control of motor rotation speed). This service may be used to automatically perform aerobatic tricks (e.g., flips). Parameters: -* `pitch_rate`, `roll_rate`, `yaw_rate` – pitch, roll, and yaw rates *(rad/s)*; +* `roll_rate`, `pitch_rate`, `yaw_rate` – pitch, roll, and yaw rates *(rad/s)*; * `thrust` — throttle level, ranges from 0 (no throttle, propellers are stopped) to 1 (full throttle). * `auto_arm` – switch the drone to `OFFBOARD` and arm automatically (**the drone will take off**); diff --git a/docs/en/snippets.md b/docs/en/snippets.md index 315e1f55..052270b5 100644 --- a/docs/en/snippets.md +++ b/docs/en/snippets.md @@ -144,7 +144,7 @@ Determine whether the copter is turned upside-down: PI_2 = math.pi / 2 telem = get_telemetry() -flipped = abs(telem.pitch) > PI_2 or abs(telem.roll) > PI_2 +flipped = abs(telem.roll) > PI_2 or abs(telem.pitch) > PI_2 ``` ### # {#angle-hor} @@ -155,8 +155,8 @@ Calculate the copter horizontal angle: PI_2 = math.pi / 2 telem = get_telemetry() -flipped = not -PI_2 <= telem.pitch <= PI_2 or not -PI_2 <= telem.roll <= PI_2 -angle_to_horizon = math.atan(math.hypot(math.tan(telem.pitch), math.tan(telem.roll))) +flipped = not -PI_2 <= telem.roll <= PI_2 or not -PI_2 <= telem.pitch <= PI_2 +angle_to_horizon = math.atan(math.hypot(math.tan(telem.roll), math.tan(telem.pitch))) if flipped: angle_to_horizon = math.pi - angle_to_horizon ``` @@ -324,7 +324,7 @@ def flip(): while True: telem = get_telemetry() - flipped = abs(telem.pitch) > PI_2 or abs(telem.roll) > PI_2 + flipped = abs(telem.roll) > PI_2 or abs(telem.pitch) > PI_2 if flipped: break diff --git a/docs/ru/parameters.md b/docs/ru/parameters.md index 312de3e0..089a3742 100644 --- a/docs/ru/parameters.md +++ b/docs/ru/parameters.md @@ -60,8 +60,8 @@ Estimator это подсистема, которая вычисляет текущее состояние (state) коптера, используя показания с датчиков. В состояние коптера входит: -* угловая скорость коптера – pitch_rate, roll_rate, yaw_rate; -* ориентация коптера (в локальной системе координат) – pitch (тангаж), roll (крен), yaw (рысканье) (одно из представлений); +* угловая скорость коптера – roll_rate, pitch_rate, yaw_rate; +* ориентация коптера (в локальной системе координат) – roll (крен), pitch (тангаж), yaw (рысканье) (одно из представлений); * позиция коптера (в локальной системе координат) – x, y, z; * скорость коптера (в локальной системе координат) – vx, vy, vz; * глобальные координаты коптера – latitude, longitude, altitude; diff --git a/docs/ru/simple_offboard.md b/docs/ru/simple_offboard.md index c9d51474..36a948d9 100644 --- a/docs/ru/simple_offboard.md +++ b/docs/ru/simple_offboard.md @@ -51,11 +51,11 @@ land = rospy.ServiceProxy('land', Trigger) * `lat, lon` – широта, долгота *(градусы)*, необходимо наличие [GPS](gps.md); * `alt` – высота в глобальной системе координат (стандарт [WGS-84](https://ru.wikipedia.org/wiki/WGS_84), не AMSL!), необходимо наличие [GPS](gps.md); * `vx, vy, vz` – скорость коптера *(м/с)*; -* `pitch` – угол по тангажу *(радианы)*; * `roll` – угол по крену *(радианы)*; +* `pitch` – угол по тангажу *(радианы)*; * `yaw` – угол по рысканью *(радианы)*; -* `pitch_rate` – угловая скорость по тангажу *(рад/с)*; * `roll_rate` – угловая скорость по крену *(рад/с)*; +* `pitch_rate` – угловая скорость по тангажу *(рад/с)*; * `yaw_rate` – угловая скорость по рысканью *(рад/с)*; * `voltage` – общее напряжение аккумулятора *(В)*; * `cell_voltage` – напряжение аккумулятора на ячейку *(В)*. @@ -265,7 +265,7 @@ set_velocity(vx=1, vy=0.0, vz=0, frame_id='body') Параметры: -* `pitch`, `roll`, `yaw` – необходимый угол по тангажу, крену и рысканью *(радианы)*; +* `roll`, `pitch`, `yaw` – необходимый угол по тангажу, крену и рысканью *(радианы)*; * `thrust` – уровень газа от 0 (нет газа, пропеллеры остановлены) до 1 (полный газ); * `auto_arm` – перевести коптер в `OFFBOARD` и заармить автоматически (**коптер взлетит**); * `frame_id` – [система координат](frames.md), в которой задан `yaw` (по умолчанию: `map`). @@ -276,7 +276,7 @@ set_velocity(vx=1, vy=0.0, vz=0, frame_id='body') Параметры: -* `pitch_rate`, `roll_rate`, `yaw_rate` – угловая скорость по тангажу, крену и рыканью *(рад/с)*; +* `roll_rate`, `pitch_rate`, `yaw_rate` – угловая скорость по тангажу, крену и рыканью *(рад/с)*; * `thrust` – уровень газа от 0 (нет газа, пропеллеры остановлены) до 1 (полный газ). * `auto_arm` – перевести коптер в `OFFBOARD` и заармить автоматически (**коптер взлетит**); diff --git a/docs/ru/snippets.md b/docs/ru/snippets.md index 45b1be6f..89d15d60 100644 --- a/docs/ru/snippets.md +++ b/docs/ru/snippets.md @@ -154,7 +154,7 @@ new_pose = tf_buffer.transform(pose, frame_id, transform_timeout) PI_2 = math.pi / 2 telem = get_telemetry() -flipped = abs(telem.pitch) > PI_2 or abs(telem.roll) > PI_2 +flipped = abs(telem.roll) > PI_2 or abs(telem.pitch) > PI_2 ``` ### # {#angle-hor} @@ -165,7 +165,7 @@ flipped = abs(telem.pitch) > PI_2 or abs(telem.roll) > PI_2 PI_2 = math.pi / 2 telem = get_telemetry() -flipped = not -PI_2 <= telem.pitch <= PI_2 or not -PI_2 <= telem.roll <= PI_2 +flipped = not -PI_2 <= telem.roll <= PI_2 or not -PI_2 <= telem.pitch <= PI_2 angle_to_horizon = math.atan(math.hypot(math.tan(telem.pitch), math.tan(telem.roll))) if flipped: angle_to_horizon = math.pi - angle_to_horizon @@ -335,7 +335,7 @@ def flip(): while True: telem = get_telemetry() - flipped = abs(telem.pitch) > PI_2 or abs(telem.roll) > PI_2 + flipped = abs(telem.roll) > PI_2 or abs(telem.pitch) > PI_2 if flipped: break