ROS: Fixes for ROS build

The ROS build included some files that used isfinite vs PX4_ISFINITE.

The AppState class also needed to be supported for ROS.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>

src/examples/subscriber/subscriber_example.cpp

@@ -45,7 +45,7 @@ using namespace px4;
 
 void rc_channels_callback_function(const px4_rc_channels &msg)
 {
-	PX4_INFO("I heard: [%llu]", msg.data().timestamp_last_valid);
+	PX4_INFO("I heard: [%lu]", msg.data().timestamp_last_valid);
 }
 
 SubscriberExample::SubscriberExample() :
@@ -84,21 +84,21 @@ SubscriberExample::SubscriberExample() :
  */
 void SubscriberExample::rc_channels_callback(const px4_rc_channels &msg)
 {
-	PX4_INFO("rc_channels_callback (method): [%llu]",
+	PX4_INFO("rc_channels_callback (method): [%lu]",
 		 msg.data().timestamp_last_valid);
-	PX4_INFO("rc_channels_callback (method): value of _sub_rc_chan: [%llu]",
+	PX4_INFO("rc_channels_callback (method): value of _sub_rc_chan: [%lu]",
 		 _sub_rc_chan->data().timestamp_last_valid);
 }
 
 void SubscriberExample::vehicle_attitude_callback(const px4_vehicle_attitude &msg)
 {
-	PX4_INFO("vehicle_attitude_callback (method): [%llu]",
+	PX4_INFO("vehicle_attitude_callback (method): [%lu]",
 		 msg.data().timestamp);
 }
 
 void SubscriberExample::parameter_update_callback(const px4_parameter_update &msg)
 {
-	PX4_INFO("parameter_update_callback (method): [%llu]",
+	PX4_INFO("parameter_update_callback (method): [%lu]",
 		 msg.data().timestamp);
 	_p_sub_interv.update();
 	PX4_INFO("Param SUB_INTERV = %d", _p_sub_interv.get());

src/modules/mc_att_control_multiplatform/mc_att_control.cpp

@@ -236,10 +236,10 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const px4_vehicle_atti
 		control_attitude_rates(dt);
 
 		/* publish actuator controls */
-		_actuators.data().control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f;
-		_actuators.data().control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
-		_actuators.data().control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
-		_actuators.data().control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
+		_actuators.data().control[0] = (PX4_ISFINITE(_att_control(0))) ? _att_control(0) : 0.0f;
+		_actuators.data().control[1] = (PX4_ISFINITE(_att_control(1))) ? _att_control(1) : 0.0f;
+		_actuators.data().control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f;
+		_actuators.data().control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f;
 		_actuators.data().timestamp = px4::get_time_micros();
 
 		if (!_actuators_0_circuit_breaker_enabled) {

src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp

@@ -537,7 +537,7 @@ MulticopterPositionControl::control_auto(float dt)
 		_pos_sp = pos_sp_s.edivide(scale);
 
 		/* update yaw setpoint if needed */
-		if (isfinite(_pos_sp_triplet->data().current.yaw)) {
+		if (PX4_ISFINITE(_pos_sp_triplet->data().current.yaw)) {
 			_att_sp_msg.data().yaw_body = _pos_sp_triplet->data().current.yaw;
 		}
 
@@ -554,9 +554,9 @@ void MulticopterPositionControl::handle_parameter_update(const px4_parameter_upd
 void MulticopterPositionControl::handle_position_setpoint_triplet(const px4_position_setpoint_triplet &msg)
 {
 	/* Make sure that the position setpoint is valid */
-	if (!isfinite(_pos_sp_triplet->data().current.lat) ||
-			!isfinite(_pos_sp_triplet->data().current.lon) ||
-			!isfinite(_pos_sp_triplet->data().current.alt)) {
+	if (!PX4_ISFINITE(_pos_sp_triplet->data().current.lat) ||
+			!PX4_ISFINITE(_pos_sp_triplet->data().current.lon) ||
+			!PX4_ISFINITE(_pos_sp_triplet->data().current.alt)) {
 		_pos_sp_triplet->data().current.valid = false;
 	}
 }

src/platforms/px4_defines.h

@@ -66,6 +66,8 @@
 /* Get value of parameter by name, which is equal to the handle for ros */
 #define PX4_PARAM_GET_BYNAME(_name, _destpt) ros::param::get(_name, *_destpt)
 
+#define PX4_ISFINITE(x) std::isfinite(x)
+
 #elif defined(__PX4_NUTTX) || defined(__PX4_POSIX)
 /*
  * Building for NuttX or POSIX

src/platforms/px4_nodehandle.h

@@ -42,7 +42,6 @@
 #include "px4_subscriber.h"
 #include "px4_publisher.h"
 #include "px4_middleware.h"
-#include "px4_posix.h"
 #include "px4_app.h"
 
 #if defined(__PX4_ROS)
@@ -64,10 +63,11 @@ class NodeHandle :
 	private ros::NodeHandle
 {
 public:
-	NodeHandle() :
+	NodeHandle(AppState &a) :
 		ros::NodeHandle(),
 		_subs(),
-		_pubs()
+		_pubs(),
+		_appState(a)
 	{}
 
 	~NodeHandle()
@@ -138,6 +138,9 @@ public:
 protected:
 	std::list<SubscriberBase *> _subs;				/**< Subcriptions of node */
 	std::list<PublisherBase *> _pubs;				/**< Publications of node */
+
+	AppState	&_appState;
+
 };
 #else //Building for NuttX
 class __EXPORT NodeHandle

src/platforms/ros/geo.cpp

@@ -55,7 +55,7 @@
 __EXPORT float _wrap_pi(float bearing)
 {
 	/* value is inf or NaN */
-	if (!isfinite(bearing)) {
+	if (!PX4_ISFINITE(bearing)) {
 		return bearing;
 	}
 
@@ -85,7 +85,7 @@ __EXPORT float _wrap_pi(float bearing)
 __EXPORT float _wrap_2pi(float bearing)
 {
 	/* value is inf or NaN */
-	if (!isfinite(bearing)) {
+	if (!PX4_ISFINITE(bearing)) {
 		return bearing;
 	}
 
@@ -115,7 +115,7 @@ __EXPORT float _wrap_2pi(float bearing)
 __EXPORT float _wrap_180(float bearing)
 {
 	/* value is inf or NaN */
-	if (!isfinite(bearing)) {
+	if (!PX4_ISFINITE(bearing)) {
 		return bearing;
 	}
 
@@ -145,7 +145,7 @@ __EXPORT float _wrap_180(float bearing)
 __EXPORT float _wrap_360(float bearing)
 {
 	/* value is inf or NaN */
-	if (!isfinite(bearing)) {
+	if (!PX4_ISFINITE(bearing)) {
 		return bearing;
 	}