mirror of
https://github.com/CopterExpress/clover.git
synced 2026-05-26 11:43:25 +00:00
Rewrite simple_offboard node in C++
* update_frame parameter removed * lat, lon type changed to float64 * add reference_frames parameters * misc parameters changes
This commit is contained in:
Oleg Kalachev
@@ -26,6 +26,9 @@ find_package(catkin REQUIRED COMPONENTS
|
||||
cv_bridge
|
||||
)
|
||||
|
||||
list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/cmake")
|
||||
|
||||
find_package(GeographicLib REQUIRED)
|
||||
|
||||
## System dependencies are found with CMake's conventions
|
||||
# find_package(Boost REQUIRED COMPONENTS system)
|
||||
@@ -138,6 +141,7 @@ catkin_package(
|
||||
include_directories(
|
||||
# include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
${GeographicLib_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
# Declare a C++ library
|
||||
@@ -161,12 +165,19 @@ add_library(aruco_vpe
|
||||
## Declare a C++ executable
|
||||
## With catkin_make all packages are built within a single CMake context
|
||||
## The recommended prefix ensures that target names across packages don't collide
|
||||
add_executable(simple_offboard src/simple_offboard.cpp)
|
||||
|
||||
add_executable(rc src/rc.cpp)
|
||||
|
||||
add_executable(camera_markers src/camera_markers.cpp)
|
||||
|
||||
add_executable(frames src/frames.cpp)
|
||||
|
||||
target_link_libraries(simple_offboard
|
||||
${catkin_LIBRARIES}
|
||||
${GeographicLib_LIBRARIES}
|
||||
)
|
||||
|
||||
target_link_libraries(rc ${catkin_LIBRARIES})
|
||||
|
||||
target_link_libraries(camera_markers ${catkin_LIBRARIES})
|
||||
|
||||
18
clever/cmake/FindGeographicLib.cmake
Normal file
18
clever/cmake/FindGeographicLib.cmake
Normal file
@@ -0,0 +1,18 @@
|
||||
# taken from: https://github.com/mavlink/mavros/blob/master/libmavconn/cmake/Modules/FindGeographicLib.cmake
|
||||
|
||||
# Look for GeographicLib
|
||||
#
|
||||
# Set
|
||||
# GEOGRAPHICLIB_FOUND = TRUE
|
||||
# GeographicLib_INCLUDE_DIRS = /usr/local/include
|
||||
# GeographicLib_LIBRARIES = /usr/local/lib/libGeographic.so
|
||||
# GeographicLib_LIBRARY_DIRS = /usr/local/lib
|
||||
|
||||
find_path (GeographicLib_INCLUDE_DIRS NAMES GeographicLib/Config.h)
|
||||
|
||||
find_library (GeographicLib_LIBRARIES NAMES Geographic)
|
||||
|
||||
include (FindPackageHandleStandardArgs)
|
||||
find_package_handle_standard_args (GeographicLib DEFAULT_MSG
|
||||
GeographicLib_LIBRARIES GeographicLib_INCLUDE_DIRS)
|
||||
mark_as_advanced (GeographicLib_LIBRARIES GeographicLib_INCLUDE_DIRS)
|
||||
@@ -39,7 +39,13 @@
|
||||
<node pkg="tf2_ros" type="static_transform_publisher" name="local_origin_upside_down_frame" args="0 0 0 3.1415926 3.1415926 0 local_origin local_origin_upside_down"/>
|
||||
|
||||
<!-- simplified offboard control -->
|
||||
<node name="simple_offboard" pkg="clever" type="simple_offboard.py" output="screen"/>
|
||||
<node name="simple_offboard" pkg="clever" type="simple_offboard" output="screen" clear_params="true">
|
||||
<rosparam param="reference_frames">
|
||||
body: local_origin
|
||||
base_link: local_origin
|
||||
</rosparam>
|
||||
</node>
|
||||
|
||||
<!-- Auxiliary frames -->
|
||||
<node name="frames" pkg="clever" type="frames" output="screen">
|
||||
<param name="body/frame_id" value="fcu_horiz"/>
|
||||
|
||||
@@ -27,6 +27,9 @@
|
||||
<!-- default px4 params -->
|
||||
<rosparam command="load" file="$(find mavros)/launch/px4_config.yaml"/>
|
||||
|
||||
<!-- enable setpoint_attitude/attitude -->
|
||||
<param name="setpoint_attitude/use_quaternion" value="true"/>
|
||||
|
||||
<!-- rangefinders -->
|
||||
<rosparam>
|
||||
distance_sensor:
|
||||
|
||||
@@ -16,6 +16,7 @@
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>visualization_msgs</build_depend>
|
||||
<build_depend>tf2_geometry_msgs</build_depend>
|
||||
<build_depend>geographiclib</build_depend>
|
||||
<!-- Use buildtool_depend for build tool packages: -->
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<!-- Use run_depend for packages you need at runtime: -->
|
||||
@@ -30,6 +31,7 @@
|
||||
<run_depend>rosbridge_server</run_depend>
|
||||
<run_depend>web_video_server</run_depend>
|
||||
<run_depend>ros_comm</run_depend>
|
||||
<run_depend>geographiclib</run_depend>
|
||||
<!-- Use test_depend for packages you need only for testing: -->
|
||||
<!-- <test_depend>gtest</test_depend> -->
|
||||
|
||||
|
||||
736
clever/src/simple_offboard.cpp
Normal file
736
clever/src/simple_offboard.cpp
Normal file
@@ -0,0 +1,736 @@
|
||||
/* Simplified copter control in OFFBOARD mode
|
||||
*
|
||||
* @author: Oleg Kalachev <okalachev@gmail.com>
|
||||
*/
|
||||
|
||||
#include <algorithm>
|
||||
#include <string>
|
||||
#include <cmath>
|
||||
#include <boost/format.hpp>
|
||||
#include <stdexcept>
|
||||
#include <GeographicLib/Geodesic.hpp>
|
||||
#include <ros/ros.h>
|
||||
#include <tf/transform_datatypes.h>
|
||||
#include <tf2/utils.h>
|
||||
#include <tf2_ros/transform_listener.h>
|
||||
#include <tf2_ros/transform_broadcaster.h>
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
||||
#include <std_srvs/Trigger.h>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <geometry_msgs/TwistStamped.h>
|
||||
#include <geometry_msgs/Vector3Stamped.h>
|
||||
#include <geometry_msgs/QuaternionStamped.h>
|
||||
#include <sensor_msgs/NavSatFix.h>
|
||||
#include <sensor_msgs/BatteryState.h>
|
||||
#include <mavros_msgs/CommandBool.h>
|
||||
#include <mavros_msgs/SetMode.h>
|
||||
#include <mavros_msgs/PositionTarget.h>
|
||||
#include <mavros_msgs/AttitudeTarget.h>
|
||||
#include <mavros_msgs/Thrust.h>
|
||||
#include <mavros_msgs/State.h>
|
||||
|
||||
#include <clever/GetTelemetry.h>
|
||||
#include <clever/Navigate.h>
|
||||
#include <clever/NavigateGlobal.h>
|
||||
#include <clever/SetPosition.h>
|
||||
#include <clever/SetVelocity.h>
|
||||
#include <clever/SetAttitude.h>
|
||||
#include <clever/SetRates.h>
|
||||
|
||||
using std::string;
|
||||
using namespace geometry_msgs;
|
||||
using namespace sensor_msgs;
|
||||
using namespace clever;
|
||||
using mavros_msgs::PositionTarget;
|
||||
using mavros_msgs::AttitudeTarget;
|
||||
using mavros_msgs::Thrust;
|
||||
|
||||
// tf2
|
||||
tf2_ros::Buffer tf_buffer;
|
||||
|
||||
// Parameters
|
||||
string local_frame;
|
||||
string fcu_frame;
|
||||
ros::Duration transform_timeout;
|
||||
ros::Duration telemetry_transform_timeout;
|
||||
ros::Duration offboard_timeout;
|
||||
ros::Duration land_timeout;
|
||||
ros::Duration arming_timeout;
|
||||
ros::Duration local_position_timeout;
|
||||
ros::Duration state_timeout;
|
||||
ros::Duration velocity_timeout;
|
||||
ros::Duration global_position_timeout;
|
||||
ros::Duration battery_timeout;
|
||||
float default_speed;
|
||||
bool auto_release;
|
||||
std::map<string, string> reference_frames;
|
||||
|
||||
// Publishers
|
||||
ros::Publisher attitude_pub, attitude_raw_pub, position_pub, position_raw_pub, rates_pub, thrust_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;
|
||||
TransformStamped target;
|
||||
|
||||
// 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;
|
||||
float nav_speed;
|
||||
bool busy = false;
|
||||
bool wait_armed = false;
|
||||
|
||||
enum setpoint_type_t {
|
||||
NONE,
|
||||
NAVIGATE,
|
||||
NAVIGATE_GLOBAL,
|
||||
POSITION,
|
||||
VELOCITY,
|
||||
ATTITUDE,
|
||||
RATES
|
||||
};
|
||||
|
||||
enum setpoint_type_t setpoint_type = NONE;
|
||||
|
||||
enum { YAW, YAW_RATE, TOWARDS } setpoint_yaw_type;
|
||||
|
||||
// Last received telemetry messages
|
||||
mavros_msgs::State state;
|
||||
PoseStamped local_position;
|
||||
TwistStamped velocity;
|
||||
NavSatFix global_position;
|
||||
BatteryState battery;
|
||||
|
||||
// Common subcriber callback template that stores message to the variable
|
||||
template<typename T, T& STORAGE>
|
||||
void handleMessage(const T& msg)
|
||||
{
|
||||
STORAGE = msg;
|
||||
}
|
||||
|
||||
// wait for transform without interrupting publishing setpoints
|
||||
inline bool waitTransform(const string& target, const string& source,
|
||||
const ros::Time& stamp, const ros::Duration& timeout)
|
||||
{
|
||||
ros::Rate r(10);
|
||||
auto start = ros::Time::now();
|
||||
while (ros::ok()) {
|
||||
if (ros::Time::now() - start > timeout) return false;
|
||||
if (tf_buffer.canTransform(target, source, stamp)) return true;
|
||||
ros::spinOnce();
|
||||
r.sleep();
|
||||
}
|
||||
}
|
||||
|
||||
#define TIMEOUT(msg, timeout) (ros::Time::now() - msg.header.stamp > timeout)
|
||||
|
||||
bool getTelemetry(GetTelemetry::Request& req, GetTelemetry::Response& res)
|
||||
{
|
||||
ros::Time stamp = ros::Time::now();
|
||||
|
||||
if (req.frame_id.empty())
|
||||
req.frame_id = local_frame;
|
||||
|
||||
res.frame_id = req.frame_id;
|
||||
res.x = NAN;
|
||||
res.y = NAN;
|
||||
res.z = NAN;
|
||||
res.lat = NAN;
|
||||
res.lon = NAN;
|
||||
res.alt = NAN;
|
||||
res.vx = NAN;
|
||||
res.vy = NAN;
|
||||
res.vz = NAN;
|
||||
res.pitch = NAN;
|
||||
res.roll = NAN;
|
||||
res.yaw = NAN;
|
||||
res.pitch_rate = NAN;
|
||||
res.roll_rate = NAN;
|
||||
res.yaw_rate = NAN;
|
||||
res.voltage = NAN;
|
||||
res.cell_voltage = NAN;
|
||||
|
||||
if (!TIMEOUT(state, state_timeout)) {
|
||||
res.connected = state.connected;
|
||||
res.armed = state.armed;
|
||||
res.mode = state.mode;
|
||||
}
|
||||
|
||||
waitTransform(local_frame, req.frame_id, stamp, telemetry_transform_timeout);
|
||||
|
||||
if (!TIMEOUT(local_position, local_position_timeout)) {
|
||||
try {
|
||||
// transform pose
|
||||
PoseStamped pose;
|
||||
tf_buffer.transform(local_position, pose, req.frame_id);
|
||||
res.x = pose.pose.position.x;
|
||||
res.y = pose.pose.position.y;
|
||||
res.z = pose.pose.position.z;
|
||||
|
||||
// Tait-Bryan angles, order z-y-x
|
||||
double yaw, pitch, roll;
|
||||
tf2::getEulerYPR(pose.pose.orientation, yaw, pitch, roll);
|
||||
res.yaw = yaw;
|
||||
res.pitch = pitch;
|
||||
res.roll = roll;
|
||||
} catch (const tf2::TransformException& e) {}
|
||||
}
|
||||
|
||||
if (!TIMEOUT(velocity, velocity_timeout)) {
|
||||
try {
|
||||
// transform velocity
|
||||
Vector3Stamped vec, vec_out;
|
||||
vec.header = velocity.header;
|
||||
vec.vector = velocity.twist.linear;
|
||||
tf_buffer.transform(vec, vec_out, req.frame_id);
|
||||
|
||||
res.vx = vec_out.vector.x;
|
||||
res.vy = vec_out.vector.y;
|
||||
res.vz = vec_out.vector.z;
|
||||
} catch (const tf2::TransformException& e) {}
|
||||
|
||||
// use angular velocities as they are
|
||||
res.yaw_rate = velocity.twist.angular.z;
|
||||
res.pitch_rate = velocity.twist.angular.y;
|
||||
res.roll_rate = velocity.twist.angular.x;
|
||||
}
|
||||
|
||||
if (!TIMEOUT(global_position, global_position_timeout)) {
|
||||
res.lat = global_position.latitude;
|
||||
res.lon = global_position.longitude;
|
||||
res.alt = global_position.altitude;
|
||||
}
|
||||
|
||||
if (!TIMEOUT(battery, battery_timeout)) {
|
||||
res.voltage = battery.voltage;
|
||||
if (!battery.cell_voltage.empty()) {
|
||||
res.cell_voltage = battery.cell_voltage[0];
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// throws std::runtime_error
|
||||
void offboardAndArm()
|
||||
{
|
||||
ros::Rate r(10);
|
||||
|
||||
if (state.mode != "OFFBOARD") {
|
||||
auto start = ros::Time::now();
|
||||
ROS_INFO("simple_offboard: switch to OFFBOARD");
|
||||
static mavros_msgs::SetMode sm;
|
||||
sm.request.custom_mode = "OFFBOARD";
|
||||
|
||||
if (!set_mode.call(sm))
|
||||
throw std::runtime_error("Error calling set_mode service");
|
||||
|
||||
// wait for OFFBOARD mode
|
||||
while (ros::ok()) {
|
||||
ros::spinOnce();
|
||||
if (state.mode == "OFFBOARD") {
|
||||
break;
|
||||
} else if (ros::Time::now() - start > offboard_timeout) {
|
||||
throw std::runtime_error("OFFBOARD request timed out");
|
||||
}
|
||||
ros::spinOnce();
|
||||
r.sleep();
|
||||
}
|
||||
}
|
||||
|
||||
if (!state.armed) {
|
||||
ros::Time start = ros::Time::now();
|
||||
ROS_INFO("simple_offboard: arming");
|
||||
mavros_msgs::CommandBool srv;
|
||||
srv.request.value = true;
|
||||
if (!arming.call(srv)) {
|
||||
throw std::runtime_error("Error calling arming service");
|
||||
}
|
||||
|
||||
// wait until armed
|
||||
while (ros::ok()) {
|
||||
ros::spinOnce();
|
||||
if (state.armed) {
|
||||
break;
|
||||
} else if (ros::Time::now() - start > arming_timeout) {
|
||||
throw std::runtime_error("Arming timed out");
|
||||
}
|
||||
ros::spinOnce();
|
||||
r.sleep();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
inline double hypot(double x, double y, double z)
|
||||
{
|
||||
return std::sqrt(x * x + y * y + z * z);
|
||||
}
|
||||
|
||||
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)
|
||||
{
|
||||
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 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;
|
||||
}
|
||||
|
||||
PoseStamped globalToLocal(double lat, double lon)
|
||||
{
|
||||
auto earth = GeographicLib::Geodesic::WGS84();
|
||||
|
||||
// Determine azimuth and distance between current and destination point
|
||||
double _, distance, azimuth;
|
||||
earth.Inverse(global_position.latitude, global_position.longitude, lat, lon, distance, _, azimuth);
|
||||
|
||||
double x_offset, y_offset;
|
||||
double azimuth_radians = azimuth * M_PI / 180;
|
||||
x_offset = distance * sin(azimuth_radians);
|
||||
y_offset = distance * cos(azimuth_radians);
|
||||
|
||||
auto local = tf_buffer.lookupTransform(local_frame, fcu_frame, global_position.header.stamp);
|
||||
|
||||
PoseStamped pose;
|
||||
pose.header.stamp = global_position.header.stamp; // TODO: ?
|
||||
pose.header.frame_id = local_frame;
|
||||
pose.pose.position.x = local.transform.translation.x + x_offset;
|
||||
pose.pose.position.y = local.transform.translation.y + y_offset;
|
||||
pose.pose.orientation.w = 1;
|
||||
return pose;
|
||||
}
|
||||
|
||||
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 == ATTITUDE) {
|
||||
setpoint_position.header.stamp = stamp;
|
||||
tf_buffer.transform(setpoint_position, setpoint_position_transformed, local_frame, ros::Duration(0.05));
|
||||
}
|
||||
|
||||
// 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));
|
||||
}
|
||||
|
||||
} catch (const tf2::TransformException& e) {
|
||||
ROS_WARN_THROTTLE(10, "simple_offboard: can't transform");
|
||||
}
|
||||
|
||||
if (!target.child_frame_id.empty()) {
|
||||
if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) {
|
||||
static tf2_ros::TransformBroadcaster tf_broadcaster;
|
||||
target.header = setpoint_position_transformed.header;
|
||||
target.transform.translation.x = setpoint_position_transformed.pose.position.x;
|
||||
target.transform.translation.y = setpoint_position_transformed.pose.position.y;
|
||||
target.transform.translation.z = setpoint_position_transformed.pose.position.z;
|
||||
target.transform.rotation = setpoint_position_transformed.pose.orientation;
|
||||
tf_broadcaster.sendTransform(target);
|
||||
}
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
if (setpoint_type == POSITION) {
|
||||
position_msg = setpoint_position_transformed;
|
||||
}
|
||||
|
||||
if (setpoint_type == POSITION || setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL) {
|
||||
if (setpoint_yaw_type == YAW || setpoint_yaw_type == TOWARDS) {
|
||||
position_msg.header.stamp = stamp;
|
||||
position_pub.publish(position_msg);
|
||||
|
||||
} else {
|
||||
position_raw_msg.type_mask = PositionTarget::IGNORE_VX +
|
||||
PositionTarget::IGNORE_VY +
|
||||
PositionTarget::IGNORE_VZ +
|
||||
PositionTarget::IGNORE_AFX +
|
||||
PositionTarget::IGNORE_AFY +
|
||||
PositionTarget::IGNORE_AFZ +
|
||||
PositionTarget::IGNORE_YAW;
|
||||
position_raw_msg.yaw_rate = setpoint_yaw_rate;
|
||||
position_raw_msg.position = position_msg.pose.position;
|
||||
position_raw_pub.publish(position_raw_msg);
|
||||
}
|
||||
}
|
||||
|
||||
if (setpoint_type == VELOCITY) {
|
||||
position_raw_msg.type_mask = PositionTarget::IGNORE_PX +
|
||||
PositionTarget::IGNORE_PY +
|
||||
PositionTarget::IGNORE_PZ +
|
||||
PositionTarget::IGNORE_AFX +
|
||||
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_pub.publish(position_raw_msg);
|
||||
}
|
||||
|
||||
if (setpoint_type == ATTITUDE) {
|
||||
attitude_pub.publish(setpoint_position_transformed);
|
||||
thrust_pub.publish(thrust_msg);
|
||||
}
|
||||
|
||||
if (setpoint_type == RATES) {
|
||||
// rates_pub.publish(rates_msg);
|
||||
// thrust_pub.publish(thrust_msg);
|
||||
// mavros rates topics waits for rates in local frame
|
||||
// use rates in body frame for simplicity
|
||||
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;
|
||||
attitude_raw_pub.publish(att_raw_msg);
|
||||
}
|
||||
}
|
||||
|
||||
void publishSetpoint(const ros::TimerEvent& event)
|
||||
{
|
||||
publish(event.current_real);
|
||||
}
|
||||
|
||||
inline void checkState()
|
||||
{
|
||||
if (TIMEOUT(state, state_timeout))
|
||||
throw std::runtime_error("State timeout, check mavros settings");
|
||||
|
||||
if (!state.connected)
|
||||
throw std::runtime_error("No connection to FCU, https://clever.copterexpress.com/connection.html");
|
||||
}
|
||||
|
||||
inline void 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,
|
||||
float lat, float lon, float thrust, float speed, string frame_id, bool auto_arm,
|
||||
uint8_t& success, string& message)
|
||||
{
|
||||
auto stamp = ros::Time::now();
|
||||
|
||||
try {
|
||||
if (busy)
|
||||
throw std::runtime_error("Busy");
|
||||
|
||||
busy = true;
|
||||
|
||||
// Checks
|
||||
checkState();
|
||||
|
||||
if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL) {
|
||||
if (TIMEOUT(local_position, local_position_timeout))
|
||||
throw std::runtime_error("No local position, check settings");
|
||||
|
||||
if (speed < 0)
|
||||
throw std::runtime_error("Navigate speed must be positive, " + std::to_string(speed) + " passed");
|
||||
|
||||
if (speed == 0)
|
||||
speed = default_speed;
|
||||
}
|
||||
|
||||
if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION) {
|
||||
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");
|
||||
}
|
||||
|
||||
// default frame is local frame
|
||||
if (frame_id.empty())
|
||||
frame_id = local_frame;
|
||||
|
||||
// look up for reference frame
|
||||
auto search = reference_frames.find(frame_id);
|
||||
const string& reference_frame = search == reference_frames.end() ? frame_id : search->second;
|
||||
|
||||
if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY || sp_type == ATTITUDE) {
|
||||
// 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);
|
||||
|
||||
// make sure transform from reference frame to local frame available
|
||||
if (!waitTransform(local_frame, reference_frame, stamp, transform_timeout))
|
||||
throw std::runtime_error("Can't transform from " + reference_frame + " to " + local_frame);
|
||||
}
|
||||
|
||||
if (sp_type == NAVIGATE_GLOBAL) {
|
||||
// Calculate x and from lat and lot in request's frame
|
||||
auto xy_in_req_frame = tf_buffer.transform(globalToLocal(lat, lon), frame_id);
|
||||
x = xy_in_req_frame.pose.position.x;
|
||||
y = xy_in_req_frame.pose.position.y;
|
||||
}
|
||||
|
||||
// Everything fine - switch setpoint type
|
||||
setpoint_type = sp_type;
|
||||
|
||||
if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL) {
|
||||
// starting point
|
||||
nav_start = local_position;
|
||||
nav_speed = speed;
|
||||
}
|
||||
|
||||
if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION) {
|
||||
if (std::isnan(yaw) && yaw_rate == 0) {
|
||||
// keep yaw unchanged
|
||||
yaw = tf2::getYaw(local_position.pose.orientation);
|
||||
}
|
||||
}
|
||||
|
||||
if (sp_type == POSITION || sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == VELOCITY || sp_type == ATTITUDE) {
|
||||
// destination point and/or yaw
|
||||
static 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;
|
||||
|
||||
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::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
|
||||
}
|
||||
|
||||
tf_buffer.transform(ps, setpoint_position, reference_frame);
|
||||
}
|
||||
|
||||
if (sp_type == VELOCITY) {
|
||||
static 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);
|
||||
}
|
||||
|
||||
if (sp_type == ATTITUDE || sp_type == RATES) {
|
||||
thrust_msg.thrust = thrust;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
wait_armed = auto_arm;
|
||||
|
||||
publish(stamp); // calculate initial transformed messages first
|
||||
setpoint_timer.start();
|
||||
|
||||
if (auto_arm) {
|
||||
offboardAndArm();
|
||||
wait_armed = false;
|
||||
} else if (state.mode != "OFFBOARD") {
|
||||
setpoint_timer.stop();
|
||||
throw std::runtime_error("Copter is not in OFFBOARD mode, use auto_arm?");
|
||||
} else if (!state.armed) {
|
||||
setpoint_timer.stop();
|
||||
throw std::runtime_error("Copter is not armed, use auto_arm?");
|
||||
}
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
message = e.what();
|
||||
ROS_INFO("simple_offboard: %s", message.c_str());
|
||||
busy = false;
|
||||
return;
|
||||
}
|
||||
|
||||
success = true;
|
||||
busy = false;
|
||||
return;
|
||||
}
|
||||
|
||||
bool navigate(Navigate::Request& req, Navigate::Response& res) {
|
||||
serve(NAVIGATE, req.x, req.y, req.z, 0, 0, 0, 0, 0, req.yaw, 0, 0, req.yaw_rate, 0, 0, 0, req.speed, req.frame_id, req.auto_arm, res.success, res.message);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool navigateGlobal(NavigateGlobal::Request& req, NavigateGlobal::Response& res) {
|
||||
serve(NAVIGATE_GLOBAL, 0, 0, req.z, 0, 0, 0, 0, 0, req.yaw, 0, 0, req.yaw_rate, req.lat, req.lon, 0, req.speed, req.frame_id, req.auto_arm, res.success, res.message);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool setPosition(SetPosition::Request& req, SetPosition::Response& res) {
|
||||
serve(POSITION, req.x, req.y, req.z, 0, 0, 0, 0, 0, req.yaw, 0, 0, req.yaw_rate, 0, 0, 0, 0, req.frame_id, req.auto_arm, res.success, res.message);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool setVelocity(SetVelocity::Request& req, SetVelocity::Response& res) {
|
||||
serve(VELOCITY, 0, 0, 0, req.vx, req.vy, req.vz, 0, 0, req.yaw, 0, 0, req.yaw_rate, 0, 0, 0, 0, req.frame_id, req.auto_arm, res.success, res.message);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool setAttitude(SetAttitude::Request& req, SetAttitude::Response& res) {
|
||||
serve(ATTITUDE, 0, 0, 0, 0, 0, 0, req.pitch, req.roll, req.yaw, 0, 0, 0, 0, 0, req.thrust, 0, req.frame_id, req.auto_arm, res.success, res.message);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool setRates(SetRates::Request& req, SetRates::Response& res) {
|
||||
serve(RATES, 0, 0, 0, 0, 0, 0, 0, 0, 0, req.pitch_rate, req.roll_rate, req.yaw_rate, 0, 0, req.thrust, 0, "", req.auto_arm, res.success, res.message);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
|
||||
{
|
||||
try {
|
||||
if (busy)
|
||||
throw std::runtime_error("Busy");
|
||||
|
||||
busy = true;
|
||||
|
||||
checkState();
|
||||
|
||||
static mavros_msgs::SetMode sm;
|
||||
sm.request.custom_mode = "AUTO.LAND";
|
||||
|
||||
if (!set_mode.call(sm))
|
||||
throw std::runtime_error("Can't call set_mode service");
|
||||
|
||||
if (!sm.response.mode_sent)
|
||||
throw std::runtime_error("Can't send set_mode request");
|
||||
|
||||
static ros::Rate r(10);
|
||||
auto start = ros::Time::now();
|
||||
while (ros::ok()) {
|
||||
if (state.mode == "AUTO.LAND") {
|
||||
res.success = true;
|
||||
busy = false;
|
||||
return true;
|
||||
}
|
||||
if (ros::Time::now() - start > land_timeout)
|
||||
throw std::runtime_error("Land request timed out");
|
||||
|
||||
ros::spinOnce();
|
||||
r.sleep();
|
||||
}
|
||||
|
||||
} catch (const std::exception& e) {
|
||||
res.message = e.what();
|
||||
ROS_INFO("simple_offboard: %s", e.what());
|
||||
busy = false;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "simple_offboard");
|
||||
ros::NodeHandle nh, nh_priv("~");
|
||||
|
||||
tf2_ros::TransformListener tf_listener(tf_buffer);
|
||||
|
||||
// Params
|
||||
nh.param<string>("mavros/local_position/tf/frame_id", local_frame, "map");
|
||||
nh.param<string>("mavros/local_position/tf/child_frame_id", fcu_frame, "base_link");
|
||||
nh_priv.param("target_frame", target.child_frame_id, string("target"));
|
||||
nh_priv.param("auto_release", auto_release, true);
|
||||
nh_priv.param("default_speed", default_speed, 0.5f);
|
||||
nh_priv.getParam("reference_frames", reference_frames);
|
||||
|
||||
state_timeout = ros::Duration(nh_priv.param("state_timeout", 3.0));
|
||||
local_position_timeout = ros::Duration(nh_priv.param("local_position_timeout", 2.0));
|
||||
velocity_timeout = ros::Duration(nh_priv.param("velocity_timeout", 2.0));
|
||||
global_position_timeout = ros::Duration(nh_priv.param("global_position_timeout", 10.0));
|
||||
battery_timeout = ros::Duration(nh_priv.param("battery_timeout", 2.0));
|
||||
|
||||
transform_timeout = ros::Duration(nh_priv.param("transform_timeout", 0.5));
|
||||
telemetry_transform_timeout = ros::Duration(nh_priv.param("telemetry_transform_timeout", 0.5));
|
||||
offboard_timeout = ros::Duration(nh_priv.param("offboard_timeout", 3.0));
|
||||
land_timeout = ros::Duration(nh_priv.param("land_timeout", 3.0));
|
||||
arming_timeout = ros::Duration(nh_priv.param("arming_timeout", 4.0));
|
||||
|
||||
// Service clients
|
||||
arming = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
|
||||
set_mode = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");
|
||||
|
||||
// Telemetry subscribers
|
||||
auto state_sub = nh.subscribe("mavros/state", 1, &handleMessage<mavros_msgs::State, state>);
|
||||
auto local_position_sub = nh.subscribe("mavros/local_position/pose", 1, &handleMessage<PoseStamped, local_position>);
|
||||
auto velocity_sub = nh.subscribe("mavros/local_position/velocity", 1, &handleMessage<TwistStamped, velocity>);
|
||||
auto global_position_sub = nh.subscribe("mavros/global_position/global", 1, &handleMessage<NavSatFix, global_position>);
|
||||
auto battery_sub = nh.subscribe("mavros/battery", 1, &handleMessage<BatteryState, battery>);
|
||||
|
||||
// Setpoint publishers
|
||||
position_pub = nh.advertise<PoseStamped>("mavros/setpoint_position/local", 1);
|
||||
position_raw_pub = nh.advertise<PositionTarget>("mavros/setpoint_raw/local", 1);
|
||||
attitude_pub = nh.advertise<PoseStamped>("mavros/setpoint_attitude/attitude", 1);
|
||||
attitude_raw_pub = nh.advertise<AttitudeTarget>("mavros/setpoint_raw/attitude", 1);
|
||||
rates_pub = nh.advertise<TwistStamped>("mavros/setpoint_attitude/cmd_vel", 1);
|
||||
thrust_pub = nh.advertise<Thrust>("mavros/setpoint_attitude/thrust", 1);
|
||||
|
||||
// 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 sp_serv = nh.advertiseService("set_position", &setPosition);
|
||||
auto sv_serv = nh.advertiseService("set_velocity", &setVelocity);
|
||||
auto sa_serv = nh.advertiseService("set_attitude", &setAttitude);
|
||||
auto sr_serv = nh.advertiseService("set_rates", &setRates);
|
||||
auto ld_serv = nh.advertiseService("land", &land);
|
||||
|
||||
// Setpoint timer
|
||||
setpoint_timer = nh.createTimer(ros::Duration(1 / nh_priv.param("setpoint_rate", 30.0)), &publishSetpoint, false, false);
|
||||
|
||||
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;
|
||||
|
||||
ROS_INFO("simple_offboard: ready");
|
||||
ros::spin();
|
||||
}
|
||||
@@ -1,480 +0,0 @@
|
||||
#!/usr/bin/env python
|
||||
from __future__ import division
|
||||
|
||||
import rospy
|
||||
from geometry_msgs.msg import TransformStamped, PoseStamped, Point, PointStamped, Vector3, \
|
||||
Vector3Stamped, TwistStamped, QuaternionStamped
|
||||
from sensor_msgs.msg import NavSatFix, BatteryState
|
||||
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)
|
||||
|
||||
|
||||
position_pub = rospy.Publisher('/mavros/setpoint_raw/local', PositionTarget, queue_size=1)
|
||||
attitude_pub = rospy.Publisher('/mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=1)
|
||||
target_pub = rospy.Publisher('~target', PoseStamped, queue_size=1)
|
||||
|
||||
arming = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool, persistent=True)
|
||||
set_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode, persistent=True)
|
||||
|
||||
|
||||
pose = None
|
||||
global_position = None
|
||||
velocity = None
|
||||
state = None
|
||||
battery = None
|
||||
|
||||
|
||||
def pose_update(data):
|
||||
global pose
|
||||
pose = data
|
||||
|
||||
|
||||
def global_position_update(data):
|
||||
global global_position
|
||||
global_position = data
|
||||
|
||||
|
||||
def velocity_update(data):
|
||||
global velocity
|
||||
velocity = data
|
||||
|
||||
|
||||
def state_update(data):
|
||||
global state
|
||||
state = data
|
||||
|
||||
|
||||
def battery_update(data):
|
||||
global battery
|
||||
battery = data
|
||||
|
||||
|
||||
rospy.Subscriber('/mavros/state', State, state_update)
|
||||
rospy.Subscriber('/mavros/local_position/pose', PoseStamped, pose_update)
|
||||
rospy.Subscriber('/mavros/local_position/velocity', TwistStamped, velocity_update)
|
||||
rospy.Subscriber('/mavros/global_position/global', NavSatFix, global_position_update)
|
||||
rospy.Subscriber('/mavros/battery', BatteryState, battery_update)
|
||||
|
||||
|
||||
PT = PositionTarget
|
||||
AT = AttitudeTarget
|
||||
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))
|
||||
LOCAL_POSITION_TIMEOUT = rospy.Duration(rospy.get_param('~local_position_timeout', 0.5))
|
||||
NAVIGATE_AFTER_ARMED = rospy.Duration(rospy.get_param('~navigate_after_armed', True))
|
||||
TRANSFORM_TIMEOUT = rospy.Duration(rospy.get_param('~transform_timeout', 3))
|
||||
SETPOINT_RATE = rospy.get_param('~setpoint_rate', 30)
|
||||
LOCAL_FRAME = rospy.get_param('mavros/local_position/frame_id', 'local_origin')
|
||||
LAND_MODE = rospy.get_param('~land_mode', 'AUTO.LAND')
|
||||
LAND_TIMEOUT = rospy.Duration(rospy.get_param('~land_timeout', 2))
|
||||
DEFAULT_SPEED = rospy.get_param('~default_speed', 0.5)
|
||||
|
||||
|
||||
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')
|
||||
rospy.sleep(0.1)
|
||||
|
||||
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')
|
||||
rospy.sleep(0.1)
|
||||
|
||||
|
||||
ps = PoseStamped()
|
||||
vs = Vector3Stamped()
|
||||
pt = PositionTarget()
|
||||
at = AttitudeTarget()
|
||||
|
||||
|
||||
BRAKE_TIME = rospy.Duration(0)
|
||||
|
||||
|
||||
def get_navigate_setpoint(stamp, start, finish, start_stamp, speed):
|
||||
distance = math.sqrt((finish.z - start.z)**2 + (finish.x - start.x)**2 + (finish.y - start.y)**2)
|
||||
time = rospy.Duration(distance / speed)
|
||||
if time == rospy.Duration(0):
|
||||
k = 0
|
||||
else:
|
||||
k = (stamp - start_stamp) / time
|
||||
time_left = start_stamp + time - stamp
|
||||
|
||||
if BRAKE_TIME and time_left < BRAKE_TIME:
|
||||
# time to brake
|
||||
time_before_braking = time - BRAKE_TIME
|
||||
brake_time_passed = (stamp - start_stamp - time_before_braking)
|
||||
|
||||
if brake_time_passed > 2 * BRAKE_TIME:
|
||||
# finish
|
||||
k = 1
|
||||
else:
|
||||
# brake!
|
||||
k_before_braking = time_before_braking / time
|
||||
k_after_braking = (speed * brake_time_passed.to_sec() - brake_time_passed.to_sec() ** 2 * speed / 4 / BRAKE_TIME.to_sec()) / distance
|
||||
k = k_before_braking + k_after_braking
|
||||
|
||||
k = min(k, 1)
|
||||
|
||||
p = Point()
|
||||
p.x = start.x + (finish.x - start.x) * k
|
||||
p.y = start.y + (finish.y - start.y) * k
|
||||
p.z = start.z + (finish.z - start.z) * k
|
||||
return p
|
||||
|
||||
|
||||
def get_publisher_and_message(req, stamp, continued=True, update_frame=True):
|
||||
ps.header.stamp = stamp
|
||||
vs.header.stamp = stamp
|
||||
|
||||
# don't block on setpoints publishing
|
||||
transform_timeout = rospy.Duration(0.1) if continued else TRANSFORM_TIMEOUT
|
||||
|
||||
if isinstance(req, (srv.NavigateRequest, srv.NavigateGlobalRequest)):
|
||||
global current_nav_start, current_nav_start_stamp, current_nav_finish
|
||||
|
||||
if update_frame:
|
||||
ps.header.frame_id = req.frame_id or LOCAL_FRAME
|
||||
ps.pose.position = Point(getattr(req, 'x', 0), getattr(req, 'y', 0), req.z)
|
||||
ps.pose.orientation = orientation_from_euler(0, 0, req.yaw, axes='sxyz')
|
||||
current_nav_finish = tf_buffer.transform(ps, LOCAL_FRAME, transform_timeout)
|
||||
|
||||
if isinstance(req, srv.NavigateGlobalRequest):
|
||||
# Recalculate x and y from lat and lon
|
||||
current_nav_finish.pose.position.x, current_nav_finish.pose.position.y = \
|
||||
global_to_local(req.lat, req.lon)
|
||||
|
||||
if not continued:
|
||||
current_nav_start = pose.pose.position
|
||||
current_nav_start_stamp = stamp
|
||||
|
||||
if NAVIGATE_AFTER_ARMED and not state.armed:
|
||||
current_nav_start_stamp = stamp
|
||||
|
||||
setpoint = get_navigate_setpoint(stamp, current_nav_start, current_nav_finish.pose.position,
|
||||
current_nav_start_stamp, req.speed)
|
||||
|
||||
yaw_rate_flag = math.isnan(req.yaw)
|
||||
msg = pt
|
||||
msg.coordinate_frame = PT.FRAME_LOCAL_NED
|
||||
msg.type_mask = PT.IGNORE_VX + PT.IGNORE_VY + PT.IGNORE_VZ + \
|
||||
PT.IGNORE_AFX + PT.IGNORE_AFY + PT.IGNORE_AFZ + \
|
||||
(PT.IGNORE_YAW if yaw_rate_flag else PT.IGNORE_YAW_RATE)
|
||||
msg.position = setpoint
|
||||
msg.yaw = euler_from_orientation(current_nav_finish.pose.orientation, 'sxyz')[2]
|
||||
msg.yaw_rate = req.yaw_rate
|
||||
return position_pub, msg
|
||||
|
||||
elif isinstance(req, (srv.SetPositionRequest, srv.SetPositionGlobalRequest)):
|
||||
ps.header.frame_id = req.frame_id or LOCAL_FRAME
|
||||
ps.pose.position = Point(getattr(req, 'x', 0), getattr(req, 'y', 0), req.z)
|
||||
ps.pose.orientation = orientation_from_euler(0, 0, req.yaw)
|
||||
pose_local = tf_buffer.transform(ps, LOCAL_FRAME, transform_timeout)
|
||||
|
||||
if isinstance(req, srv.SetPositionGlobalRequest):
|
||||
pose_local.pose.position.x, pose_local.pose.position.y = global_to_local(req.lat, req.lon)
|
||||
|
||||
yaw_rate_flag = math.isnan(req.yaw)
|
||||
msg = pt
|
||||
msg.coordinate_frame = PT.FRAME_LOCAL_NED
|
||||
msg.type_mask = PT.IGNORE_VX + PT.IGNORE_VY + PT.IGNORE_VZ + \
|
||||
PT.IGNORE_AFX + PT.IGNORE_AFY + PT.IGNORE_AFZ + \
|
||||
(PT.IGNORE_YAW if yaw_rate_flag else PT.IGNORE_YAW_RATE)
|
||||
msg.position = pose_local.pose.position
|
||||
msg.yaw = euler_from_orientation(pose_local.pose.orientation, 'sxyz')[2]
|
||||
msg.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_FRAME
|
||||
ps.header.frame_id = req.frame_id or LOCAL_FRAME
|
||||
ps.pose.orientation = orientation_from_euler(0, 0, req.yaw)
|
||||
pose_local = tf_buffer.transform(ps, LOCAL_FRAME, transform_timeout)
|
||||
vector_local = tf_buffer.transform(vs, LOCAL_FRAME, transform_timeout)
|
||||
|
||||
yaw_rate_flag = math.isnan(req.yaw)
|
||||
msg = pt
|
||||
msg.coordinate_frame = PT.FRAME_LOCAL_NED
|
||||
msg.type_mask = PT.IGNORE_PX + PT.IGNORE_PY + PT.IGNORE_PZ + \
|
||||
PT.IGNORE_AFX + PT.IGNORE_AFY + PT.IGNORE_AFZ + \
|
||||
(PT.IGNORE_YAW if yaw_rate_flag else PT.IGNORE_YAW_RATE)
|
||||
msg.velocity = vector_local.vector
|
||||
msg.yaw = euler_from_orientation(pose_local.pose.orientation, 'sxyz')[2]
|
||||
msg.yaw_rate = req.yaw_rate
|
||||
return position_pub, msg
|
||||
|
||||
elif isinstance(req, srv.SetAttitudeRequest):
|
||||
ps.header.frame_id = req.frame_id or LOCAL_FRAME
|
||||
ps.pose.orientation = orientation_from_euler(req.roll, req.pitch, req.yaw)
|
||||
pose_local = tf_buffer.transform(ps, LOCAL_FRAME, transform_timeout)
|
||||
msg = at
|
||||
msg.orientation = pose_local.pose.orientation
|
||||
msg.thrust = req.thrust
|
||||
msg.type_mask = AT.IGNORE_YAW_RATE + AT.IGNORE_PITCH_RATE + AT.IGNORE_ROLL_RATE
|
||||
return attitude_pub, msg
|
||||
|
||||
elif isinstance(req, srv.SetRatesRequest):
|
||||
msg = at
|
||||
msg.thrust = req.thrust
|
||||
msg.type_mask = AT.IGNORE_ATTITUDE
|
||||
msg.body_rate.x = req.roll_rate
|
||||
msg.body_rate.y = req.pitch_rate
|
||||
msg.body_rate.z = req.yaw_rate
|
||||
return attitude_pub, msg
|
||||
|
||||
|
||||
current_pub = None
|
||||
current_msg = None
|
||||
current_req = None
|
||||
current_nav_start = None
|
||||
current_nav_finish = None
|
||||
current_nav_start_stamp = None
|
||||
handle_lock = Lock()
|
||||
|
||||
|
||||
def handle(req):
|
||||
global current_pub, current_msg, current_req
|
||||
|
||||
if not state or not state.connected:
|
||||
rospy.logwarn('No connection to the FCU')
|
||||
return {'message': 'No connection to the FCU'}
|
||||
|
||||
if isinstance(req, (srv.NavigateRequest, srv.NavigateGlobalRequest)):
|
||||
if req.speed < 0:
|
||||
rospy.logwarn('Navigate speed must be positive, %s passed')
|
||||
return {'message': 'Navigate speed must be positive, %s passed' % req.speed}
|
||||
elif req.speed == 0:
|
||||
req.speed = DEFAULT_SPEED
|
||||
|
||||
if isinstance(req, (srv.NavigateRequest, srv.NavigateGlobalRequest)) and \
|
||||
(pose is None or rospy.get_rostime() - pose.header.stamp > LOCAL_POSITION_TIMEOUT):
|
||||
rospy.logwarn('No local position')
|
||||
return {'message': 'No local position'}
|
||||
|
||||
if getattr(req, 'yaw_rate', 0) != 0 and not math.isnan(getattr(req, 'yaw')):
|
||||
rospy.logwarn('Yaw value should be NaN for setting yaw rate')
|
||||
return {'message': 'Yaw value should be NaN for setting yaw rate'}
|
||||
|
||||
if math.isnan(getattr(req, 'yaw', 0)) and math.isnan(getattr(req, 'yaw_rate', 0)):
|
||||
rospy.logwarn('Both yaw and yaw_rate cannot be NaN')
|
||||
return {'message': 'Both yaw and yaw_rate cannot be NaN'}
|
||||
|
||||
try:
|
||||
# check frame_id existance
|
||||
# (for non-blocking setpoint's publishing in get_publisher_and_message)
|
||||
stamp = rospy.get_rostime()
|
||||
if hasattr(req, 'frame_id'):
|
||||
tf_buffer.lookup_transform(req.frame_id or LOCAL_FRAME, LOCAL_FRAME, stamp, TRANSFORM_TIMEOUT)
|
||||
|
||||
with handle_lock:
|
||||
current_req = req
|
||||
current_pub, current_msg = get_publisher_and_message(req, stamp, False)
|
||||
rospy.loginfo('Topic: %s, message: %s', current_pub.name, current_msg)
|
||||
|
||||
current_msg.header.stamp = stamp
|
||||
current_pub.publish(current_msg)
|
||||
|
||||
if req.auto_arm:
|
||||
offboard_and_arm()
|
||||
else:
|
||||
if state.mode != 'OFFBOARD':
|
||||
return {'message': 'Copter is not in OFFBOARD mode, use auto_arm?'}
|
||||
if not state.armed:
|
||||
return {'message': 'Copter is not armed, use auto_arm?'}
|
||||
|
||||
return {'success': True}
|
||||
|
||||
except Exception as e:
|
||||
rospy.logerr(str(e))
|
||||
return {'success': False, 'message': str(e)}
|
||||
|
||||
|
||||
def land(req):
|
||||
if not state or not state.connected:
|
||||
rospy.logwarn('No connection to the FCU')
|
||||
return {'message': 'No connection to the FCU'}
|
||||
|
||||
rospy.loginfo('Set %s mode', LAND_MODE)
|
||||
res = set_mode(custom_mode=LAND_MODE)
|
||||
if not res.mode_sent:
|
||||
return {'message': 'Cannot send %s mode request' % LAND_MODE}
|
||||
|
||||
start = rospy.get_rostime()
|
||||
while True:
|
||||
if state.mode == LAND_MODE:
|
||||
return {'success': True}
|
||||
if rospy.get_rostime() - start > LAND_TIMEOUT:
|
||||
return {'message': '%s mode request timed out' % LAND_MODE}
|
||||
rospy.sleep(0.1)
|
||||
|
||||
|
||||
def release(req):
|
||||
global current_pub
|
||||
current_pub = None
|
||||
rospy.loginfo('simple_offboard: release')
|
||||
return {'success': True}
|
||||
|
||||
|
||||
rospy.Service('navigate', srv.Navigate, handle)
|
||||
rospy.Service('navigate_global', srv.NavigateGlobal, handle)
|
||||
rospy.Service('set_position', srv.SetPosition, handle)
|
||||
rospy.Service('set_position_global', srv.SetPositionGlobal, handle)
|
||||
rospy.Service('set_velocity', srv.SetVelocity, handle)
|
||||
rospy.Service('set_attitude', srv.SetAttitude, handle)
|
||||
rospy.Service('set_rates', srv.SetRates, handle)
|
||||
rospy.Service('land', Trigger, land)
|
||||
rospy.Service('release', Trigger, release)
|
||||
|
||||
|
||||
def get_telemetry(req):
|
||||
res = {
|
||||
'frame_id': req.frame_id or LOCAL_FRAME,
|
||||
'x': float('nan'),
|
||||
'y': float('nan'),
|
||||
'z': float('nan'),
|
||||
'lat': float('nan'),
|
||||
'lon': float('nan'),
|
||||
'alt': float('nan'),
|
||||
'vx': float('nan'),
|
||||
'vy': float('nan'),
|
||||
'vz': float('nan'),
|
||||
'pitch': float('nan'),
|
||||
'roll': float('nan'),
|
||||
'yaw': float('nan'),
|
||||
'pitch_rate': float('nan'),
|
||||
'roll_rate': float('nan'),
|
||||
'yaw_rate': float('nan'),
|
||||
'voltage': float('nan'),
|
||||
'cell_voltage': float('nan')
|
||||
}
|
||||
frame_id = req.frame_id or LOCAL_FRAME
|
||||
stamp = rospy.get_rostime()
|
||||
|
||||
transform_timeout = rospy.Duration(0.4)
|
||||
try:
|
||||
if pose:
|
||||
p = tf_buffer.transform(pose, frame_id, transform_timeout)
|
||||
res['x'] = p.pose.position.x
|
||||
res['y'] = p.pose.position.y
|
||||
res['z'] = p.pose.position.z
|
||||
|
||||
# Calculate roll pitch and yaw as Tait-Bryan angles, order z-y-x
|
||||
res['yaw'], res['pitch'], res['roll'] = euler_from_orientation(p.pose.orientation, axes='rzyx')
|
||||
except:
|
||||
pass
|
||||
|
||||
if velocity:
|
||||
try:
|
||||
v = Vector3Stamped()
|
||||
v.header.stamp = velocity.header.stamp
|
||||
v.header.frame_id = velocity.header.frame_id
|
||||
v.vector = velocity.twist.linear
|
||||
linear = tf_buffer.transform(v, frame_id, transform_timeout)
|
||||
res['vx'] = linear.vector.x
|
||||
res['vy'] = linear.vector.y
|
||||
res['vz'] = linear.vector.z
|
||||
except:
|
||||
pass
|
||||
|
||||
res['yaw_rate'] = velocity.twist.angular.z
|
||||
res['pitch_rate'] = velocity.twist.angular.y
|
||||
res['roll_rate'] = velocity.twist.angular.x
|
||||
|
||||
if global_position and stamp - global_position.header.stamp < rospy.Duration(5):
|
||||
res['lat'] = global_position.latitude
|
||||
res['lon'] = global_position.longitude
|
||||
res['alt'] = global_position.altitude
|
||||
|
||||
if state:
|
||||
res['connected'] = state.connected
|
||||
res['armed'] = state.armed
|
||||
res['mode'] = state.mode
|
||||
|
||||
if battery:
|
||||
res['voltage'] = battery.voltage
|
||||
try:
|
||||
res['cell_voltage'] = battery.cell_voltage[0]
|
||||
except:
|
||||
pass
|
||||
|
||||
return res
|
||||
|
||||
|
||||
rospy.Service('get_telemetry', srv.GetTelemetry, get_telemetry)
|
||||
|
||||
|
||||
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) or \
|
||||
isinstance(current_req, (srv.NavigateRequest, srv.NavigateGlobalRequest)):
|
||||
current_pub, current_msg = get_publisher_and_message(current_req, stamp, True,
|
||||
getattr(current_req, 'update_frame', False))
|
||||
|
||||
except Exception as e:
|
||||
rospy.logwarn_throttle(10, str(e))
|
||||
|
||||
current_msg.header.stamp = stamp
|
||||
current_pub.publish(current_msg)
|
||||
|
||||
# For monitoring
|
||||
if isinstance(current_msg, PositionTarget):
|
||||
p = PoseStamped()
|
||||
p.header.frame_id = LOCAL_FRAME
|
||||
p.header.stamp = stamp
|
||||
p.pose.position = current_msg.position
|
||||
p.pose.orientation = orientation_from_euler(0, 0, current_msg.yaw)
|
||||
target_pub.publish(p)
|
||||
|
||||
r.sleep()
|
||||
|
||||
|
||||
start_loop()
|
||||
@@ -5,7 +5,6 @@ float32 yaw
|
||||
float32 yaw_rate
|
||||
float32 speed
|
||||
string frame_id
|
||||
bool update_frame
|
||||
bool auto_arm
|
||||
---
|
||||
bool success
|
||||
|
||||
@@ -1,11 +1,10 @@
|
||||
float32 lat
|
||||
float32 lon
|
||||
float64 lat
|
||||
float64 lon
|
||||
float32 z
|
||||
float32 yaw
|
||||
float32 yaw_rate
|
||||
float32 speed
|
||||
string frame_id
|
||||
bool update_frame
|
||||
bool auto_arm
|
||||
---
|
||||
bool success
|
||||
|
||||
@@ -3,7 +3,6 @@ float32 roll
|
||||
float32 yaw
|
||||
float32 thrust
|
||||
string frame_id
|
||||
bool update_frame
|
||||
bool auto_arm
|
||||
---
|
||||
bool success
|
||||
|
||||
@@ -4,7 +4,6 @@ float32 z
|
||||
float32 yaw
|
||||
float32 yaw_rate
|
||||
string frame_id
|
||||
bool update_frame
|
||||
bool auto_arm
|
||||
---
|
||||
bool success
|
||||
|
||||
@@ -1,11 +0,0 @@
|
||||
float32 lat
|
||||
float32 lon
|
||||
float32 z
|
||||
float32 yaw
|
||||
float32 yaw_rate
|
||||
string frame_id
|
||||
bool update_frame
|
||||
bool auto_arm
|
||||
---
|
||||
bool success
|
||||
string message
|
||||
@@ -4,7 +4,6 @@ float32 vz
|
||||
float32 yaw
|
||||
float32 yaw_rate
|
||||
string frame_id
|
||||
bool update_frame
|
||||
bool auto_arm
|
||||
---
|
||||
bool success
|
||||
|
||||
Reference in New Issue
Block a user