#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_ros/static_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 <mavros_msgs/StatusText.h>
#include <mavros_msgs/ManualControl.h>
#include <clover/GetTelemetry.h>
#include <clover/Navigate.h>
#include <clover/NavigateGlobal.h>
#include <clover/SetPosition.h>
#include <clover/SetVelocity.h>
#include <clover/SetAttitude.h>
#include <clover/SetRates.h>

Include dependency graph for simple_offboard.cpp:

Macros
#define	ENSURE_FINITE(var) { if (!std::isfinite(var)) throw std::runtime_error(#var " argument cannot be NaN or Inf"); }

#define	TIMEOUT(msg, timeout) (ros::Time::now() - msg.header.stamp > timeout)

Enumerations
enum	{ YAW, YAW_RATE, TOWARDS }

enum	setpoint_type_t { NONE, NAVIGATE, NAVIGATE_GLOBAL, POSITION, VELOCITY, ATTITUDE, RATES }

Functions
void	checkManualControl ()

void	checkState ()

float	getDistance (const Point &from, const Point &to)

void	getNavigateSetpoint (const ros::Time &stamp, float speed, Point &nav_setpoint)

bool	getTelemetry (GetTelemetry::Request &req, GetTelemetry::Response &res)

PoseStamped	globalToLocal (double lat, double lon)

void	handleLocalPosition (const PoseStamped &pose)

template<typename T , T & STORAGE>
void	handleMessage (const T &msg)

void	handleState (const mavros_msgs::State &s)

double	hypot (double x, double y, double z)

bool	land (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)

int	main (int argc, char **argv)

bool	navigate (Navigate::Request &req, Navigate::Response &res)

bool	navigateGlobal (NavigateGlobal::Request &req, NavigateGlobal::Response &res)

void	offboardAndArm ()

void	publish (const ros::Time stamp)

void	publishBodyFrame ()

void	publishSetpoint (const ros::TimerEvent &event)

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, float lat, float lon, float thrust, float speed, string frame_id, bool auto_arm, uint8_t &success, string &message)

bool	setAttitude (SetAttitude::Request &req, SetAttitude::Response &res)

bool	setPosition (SetPosition::Request &req, SetPosition::Response &res)

bool	setRates (SetRates::Request &req, SetRates::Response &res)

bool	setVelocity (SetVelocity::Request &req, SetVelocity::Response &res)

bool	waitTransform (const string &target, const string &source, const ros::Time &stamp, const ros::Duration &timeout)

Variables
ros::ServiceClient	arming

ros::Duration	arming_timeout

AttitudeTarget	att_raw_msg

ros::Publisher	attitude_pub

ros::Publisher	attitude_raw_pub

bool	auto_release

BatteryState	battery

ros::Duration	battery_timeout

geometry_msgs::TransformStamped	body

bool	busy = false

bool	check_kill_switch

float	default_speed

string	fcu_frame

NavSatFix	global_position

ros::Duration	global_position_timeout

bool	land_only_in_offboard

ros::Duration	land_timeout

string	local_frame

PoseStamped	local_position

ros::Duration	local_position_timeout

mavros_msgs::ManualControl	manual_control

ros::Duration	manual_control_timeout

bool	nav_from_sp

bool	nav_from_sp_flag = false

float	nav_speed

PoseStamped	nav_start

ros::Duration	offboard_timeout

PoseStamped	position_msg

ros::Publisher	position_pub

PositionTarget	position_raw_msg

ros::Publisher	position_raw_pub

TwistStamped	rates_msg

ros::Publisher	rates_pub

std::map< string, string >	reference_frames

ros::ServiceClient	set_mode

TransformStamped	setpoint

QuaternionStamped	setpoint_attitude

QuaternionStamped	setpoint_attitude_transformed

PoseStamped	setpoint_position

PoseStamped	setpoint_position_transformed

ros::Timer	setpoint_timer

enum setpoint_type_t	setpoint_type = NONE

Vector3Stamped	setpoint_velocity

Vector3Stamped	setpoint_velocity_transformed

float	setpoint_yaw_rate

enum { ... }	setpoint_yaw_type

mavros_msgs::State	state

ros::Duration	state_timeout

std::shared_ptr< tf2_ros::StaticTransformBroadcaster >	static_transform_broadcaster

mavros_msgs::StatusText	statustext

TransformStamped	target

ros::Duration	telemetry_transform_timeout

tf2_ros::Buffer	tf_buffer

Thrust	thrust_msg

ros::Publisher	thrust_pub

tf::Quaternion	tq

std::shared_ptr< tf2_ros::TransformBroadcaster >	transform_broadcaster

ros::Duration	transform_timeout

TwistStamped	velocity

ros::Duration	velocity_timeout

bool	wait_armed = false

Macro Definition Documentation

◆ ENSURE_FINITE

#define ENSURE_FINITE ( var ) { if (!std::isfinite(var)) throw std::runtime_error(#var " argument cannot be NaN or Inf"); }

Definition at line 522 of file simple_offboard.cpp.

◆ TIMEOUT

#define TIMEOUT	(	msg,
		timeout
	)	(ros::Time::now() - msg.header.stamp > timeout)

Definition at line 186 of file simple_offboard.cpp.

Enumeration Type Documentation

◆ anonymous enum

anonymous enum

Enumerator
YAW
YAW_RATE
TOWARDS

Definition at line 122 of file simple_offboard.cpp.

◆ setpoint_type_t

enum setpoint_type_t

Enumerator
NONE
NAVIGATE
NAVIGATE_GLOBAL
POSITION
VELOCITY
ATTITUDE
RATES

Definition at line 110 of file simple_offboard.cpp.

Function Documentation

◆ checkManualControl()

void checkManualControl ( )

inline

Definition at line 492 of file simple_offboard.cpp.

◆ checkState()

void checkState ( )

inline

Definition at line 513 of file simple_offboard.cpp.

◆ getDistance()

float getDistance	(	const Point &	from,
		const Point &	to
	)

inline

Definition at line 334 of file simple_offboard.cpp.

◆ getNavigateSetpoint()

void getNavigateSetpoint	(	const ros::Time &	stamp,
		float	speed,
		Point &	nav_setpoint
	)

Definition at line 339 of file simple_offboard.cpp.

◆ getTelemetry()

bool getTelemetry	(	GetTelemetry::Request &	req,
		GetTelemetry::Response &	res
	)

Definition at line 188 of file simple_offboard.cpp.

◆ globalToLocal()

PoseStamped globalToLocal	(	double	lat,
		double	lon
	)

Definition at line 355 of file simple_offboard.cpp.

◆ handleLocalPosition()

void handleLocalPosition ( const PoseStamped & pose )

Definition at line 165 of file simple_offboard.cpp.

◆ handleMessage()

template<typename T , T & STORAGE>

void handleMessage ( const T & msg )

Definition at line 135 of file simple_offboard.cpp.

◆ handleState()

void handleState ( const mavros_msgs::State & s )

Definition at line 140 of file simple_offboard.cpp.

◆ hypot()

double hypot	(	double	x,
		double	y,
		double	z
	)

inline

Definition at line 329 of file simple_offboard.cpp.

◆ land()

bool land	(	std_srvs::Trigger::Request &	req,
		std_srvs::Trigger::Response &	res
	)

Definition at line 800 of file simple_offboard.cpp.

◆ main()

int main	(	int	argc,
		char **	argv
	)

Definition at line 848 of file simple_offboard.cpp.

◆ navigate()

bool navigate	(	Navigate::Request &	req,
		Navigate::Response &	res
	)

Definition at line 776 of file simple_offboard.cpp.

◆ navigateGlobal()

bool navigateGlobal	(	NavigateGlobal::Request &	req,
		NavigateGlobal::Response &	res
	)

Definition at line 780 of file simple_offboard.cpp.

◆ offboardAndArm()

void offboardAndArm ( )

Definition at line 274 of file simple_offboard.cpp.

◆ publish()

void publish ( const ros::Time stamp )

Definition at line 383 of file simple_offboard.cpp.

◆ publishBodyFrame()

void publishBodyFrame ( )

inline

Definition at line 149 of file simple_offboard.cpp.

◆ publishSetpoint()

void publishSetpoint ( const ros::TimerEvent & event )

Definition at line 487 of file simple_offboard.cpp.

◆ serve()

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,
		float	lat,
		float	lon,
		float	thrust,
		float	speed,
		string	frame_id,
		bool	auto_arm,
		uint8_t &	success,
		string &	message
	)

Definition at line 524 of file simple_offboard.cpp.

◆ setAttitude()

bool setAttitude	(	SetAttitude::Request &	req,
		SetAttitude::Response &	res
	)

Definition at line 792 of file simple_offboard.cpp.

◆ setPosition()

bool setPosition	(	SetPosition::Request &	req,
		SetPosition::Response &	res
	)

Definition at line 784 of file simple_offboard.cpp.

◆ setRates()

bool setRates	(	SetRates::Request &	req,
		SetRates::Response &	res
	)

Definition at line 796 of file simple_offboard.cpp.

◆ setVelocity()

bool setVelocity	(	SetVelocity::Request &	req,
		SetVelocity::Response &	res
	)

Definition at line 788 of file simple_offboard.cpp.

◆ waitTransform()

bool waitTransform	(	const string &	target,
		const string &	source,
		const ros::Time &	stamp,
		const ros::Duration &	timeout
	)

inline

Definition at line 173 of file simple_offboard.cpp.

Variable Documentation

◆ arming

ros::ServiceClient arming

Definition at line 86 of file simple_offboard.cpp.

◆ arming_timeout

ros::Duration arming_timeout

Definition at line 70 of file simple_offboard.cpp.

◆ att_raw_msg

AttitudeTarget att_raw_msg

Definition at line 93 of file simple_offboard.cpp.

◆ attitude_pub

ros::Publisher attitude_pub

Definition at line 83 of file simple_offboard.cpp.

◆ attitude_raw_pub

ros::Publisher attitude_raw_pub

Definition at line 83 of file simple_offboard.cpp.

◆ auto_release

bool auto_release

Definition at line 78 of file simple_offboard.cpp.

◆ battery

BatteryState battery

Definition at line 131 of file simple_offboard.cpp.

◆ battery_timeout

ros::Duration battery_timeout

Definition at line 75 of file simple_offboard.cpp.

◆ body

geometry_msgs::TransformStamped body

Definition at line 97 of file simple_offboard.cpp.

◆ busy

bool busy = false

Definition at line 106 of file simple_offboard.cpp.

◆ check_kill_switch

bool check_kill_switch

Definition at line 79 of file simple_offboard.cpp.

◆ default_speed

float default_speed

Definition at line 77 of file simple_offboard.cpp.

◆ fcu_frame

string fcu_frame

Definition at line 65 of file simple_offboard.cpp.

◆ global_position

NavSatFix global_position

Definition at line 130 of file simple_offboard.cpp.

◆ global_position_timeout

ros::Duration global_position_timeout

Definition at line 74 of file simple_offboard.cpp.

◆ land_only_in_offboard

bool land_only_in_offboard

Definition at line 79 of file simple_offboard.cpp.

◆ land_timeout

ros::Duration land_timeout

Definition at line 69 of file simple_offboard.cpp.

◆ local_frame

string local_frame

Definition at line 64 of file simple_offboard.cpp.

◆ local_position

PoseStamped local_position

Definition at line 128 of file simple_offboard.cpp.

◆ local_position_timeout

ros::Duration local_position_timeout

Definition at line 71 of file simple_offboard.cpp.

◆ manual_control

mavros_msgs::ManualControl manual_control

Definition at line 127 of file simple_offboard.cpp.

◆ manual_control_timeout

ros::Duration manual_control_timeout

Definition at line 76 of file simple_offboard.cpp.

◆ nav_from_sp

bool nav_from_sp

Definition at line 79 of file simple_offboard.cpp.

◆ nav_from_sp_flag

bool nav_from_sp_flag = false

Definition at line 108 of file simple_offboard.cpp.

◆ nav_speed

float nav_speed

Definition at line 105 of file simple_offboard.cpp.

◆ nav_start

PoseStamped nav_start

Definition at line 100 of file simple_offboard.cpp.

◆ offboard_timeout

ros::Duration offboard_timeout

Definition at line 68 of file simple_offboard.cpp.

◆ position_msg

PoseStamped position_msg

Definition at line 91 of file simple_offboard.cpp.

◆ position_pub

ros::Publisher position_pub

Definition at line 83 of file simple_offboard.cpp.

◆ position_raw_msg

PositionTarget position_raw_msg

Definition at line 92 of file simple_offboard.cpp.

◆ position_raw_pub

ros::Publisher position_raw_pub

Definition at line 83 of file simple_offboard.cpp.

◆ rates_msg

TwistStamped rates_msg

Definition at line 95 of file simple_offboard.cpp.

◆ rates_pub

ros::Publisher rates_pub

Definition at line 83 of file simple_offboard.cpp.

◆ reference_frames

std::map<string, string> reference_frames

Definition at line 80 of file simple_offboard.cpp.

◆ set_mode

ros::ServiceClient set_mode

Definition at line 86 of file simple_offboard.cpp.

◆ setpoint

TransformStamped setpoint

Definition at line 96 of file simple_offboard.cpp.

◆ setpoint_attitude

QuaternionStamped setpoint_attitude

Definition at line 103 of file simple_offboard.cpp.

◆ setpoint_attitude_transformed

QuaternionStamped setpoint_attitude_transformed

Definition at line 103 of file simple_offboard.cpp.

◆ setpoint_position

PoseStamped setpoint_position

Definition at line 101 of file simple_offboard.cpp.

◆ setpoint_position_transformed

PoseStamped setpoint_position_transformed

Definition at line 101 of file simple_offboard.cpp.

◆ setpoint_timer

ros::Timer setpoint_timer

Definition at line 89 of file simple_offboard.cpp.

◆ setpoint_type

enum setpoint_type_t setpoint_type = NONE

Definition at line 120 of file simple_offboard.cpp.

◆ setpoint_velocity

Vector3Stamped setpoint_velocity

Definition at line 102 of file simple_offboard.cpp.

◆ setpoint_velocity_transformed

Vector3Stamped setpoint_velocity_transformed

Definition at line 102 of file simple_offboard.cpp.

◆ setpoint_yaw_rate

float setpoint_yaw_rate

Definition at line 104 of file simple_offboard.cpp.

◆ setpoint_yaw_type

enum { ... } setpoint_yaw_type

◆ state

mavros_msgs::State state

Definition at line 125 of file simple_offboard.cpp.

◆ state_timeout

ros::Duration state_timeout

Definition at line 72 of file simple_offboard.cpp.

◆ static_transform_broadcaster

std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_transform_broadcaster

Definition at line 61 of file simple_offboard.cpp.

◆ statustext

mavros_msgs::StatusText statustext

Definition at line 126 of file simple_offboard.cpp.

◆ target

TransformStamped target

Definition at line 96 of file simple_offboard.cpp.

◆ telemetry_transform_timeout

ros::Duration telemetry_transform_timeout

Definition at line 67 of file simple_offboard.cpp.

◆ tf_buffer

tf2_ros::Buffer tf_buffer

Definition at line 59 of file simple_offboard.cpp.

◆ thrust_msg

Thrust thrust_msg

Definition at line 94 of file simple_offboard.cpp.

◆ thrust_pub

ros::Publisher thrust_pub

Definition at line 83 of file simple_offboard.cpp.

◆ tq

tf::Quaternion tq

Definition at line 90 of file simple_offboard.cpp.

◆ transform_broadcaster

std::shared_ptr<tf2_ros::TransformBroadcaster> transform_broadcaster

Definition at line 60 of file simple_offboard.cpp.

◆ transform_timeout

ros::Duration transform_timeout

Definition at line 66 of file simple_offboard.cpp.

◆ velocity

TwistStamped velocity

Definition at line 129 of file simple_offboard.cpp.

◆ velocity_timeout

ros::Duration velocity_timeout

Definition at line 73 of file simple_offboard.cpp.

◆ wait_armed

bool wait_armed = false

Definition at line 107 of file simple_offboard.cpp.

Macros

Enumerations

Functions

Variables

Macro Definition Documentation

◆ ENSURE_FINITE

◆ TIMEOUT

Enumeration Type Documentation

◆ anonymous enum

◆ setpoint_type_t

Function Documentation

◆ checkManualControl()

◆ checkState()

◆ getDistance()

◆ getNavigateSetpoint()

◆ getTelemetry()

◆ globalToLocal()

◆ handleLocalPosition()

◆ handleMessage()

◆ handleState()

◆ hypot()

◆ land()

◆ main()

◆ navigate()

◆ navigateGlobal()

◆ offboardAndArm()

◆ publish()

◆ publishBodyFrame()

◆ publishSetpoint()

◆ serve()

◆ setAttitude()

◆ setPosition()

◆ setRates()

◆ setVelocity()

◆ waitTransform()

Variable Documentation

◆ arming

◆ arming_timeout

◆ att_raw_msg

◆ attitude_pub

◆ attitude_raw_pub

◆ auto_release

◆ battery

◆ battery_timeout

◆ body

◆ busy

◆ check_kill_switch

◆ default_speed

◆ fcu_frame

◆ global_position

◆ global_position_timeout

◆ land_only_in_offboard

◆ land_timeout

◆ local_frame

◆ local_position

◆ local_position_timeout

◆ manual_control

◆ manual_control_timeout

◆ nav_from_sp

◆ nav_from_sp_flag

◆ nav_speed

◆ nav_start

◆ offboard_timeout

◆ position_msg

◆ position_pub

◆ position_raw_msg

◆ position_raw_pub

◆ rates_msg

◆ rates_pub

◆ reference_frames

◆ set_mode

◆ setpoint

◆ setpoint_attitude

◆ setpoint_attitude_transformed

◆ setpoint_position

◆ setpoint_position_transformed

◆ setpoint_timer

◆ setpoint_type

◆ setpoint_velocity

◆ setpoint_velocity_transformed