ros: commander dummy node: set control flags in offboard mode

2 files changed, 43 insertions, 1 deletions

diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp
index 2673122c7..b0f905d23 100644
--- a/src/platforms/ros/nodes/commander/commander.cpp
+++ b/src/platforms/ros/nodes/commander/commander.cpp
@@ -45,13 +45,15 @@
 Commander::Commander() :
 	_n(),
 	_man_ctrl_sp_sub(_n.subscribe("manual_control_setpoint", 10, &Commander::ManualControlInputCallback, this)),
+	_offboard_control_mode_sub(_n.subscribe("offboard_control_mode", 10, &Commander::OffboardControlModeCallback, this)),
 	_vehicle_control_mode_pub(_n.advertise<px4::vehicle_control_mode>("vehicle_control_mode", 10)),
 	_actuator_armed_pub(_n.advertise<px4::actuator_armed>("actuator_armed", 10)),
 	_vehicle_status_pub(_n.advertise<px4::vehicle_status>("vehicle_status", 10)),
 	_parameter_update_pub(_n.advertise<px4::parameter_update>("parameter_update", 10)),
 	_msg_parameter_update(),
 	_msg_actuator_armed(),
-	_msg_vehicle_control_mode()
+	_msg_vehicle_control_mode(),
+	_msg_offboard_control_mode()
 {
 }
 
@@ -107,6 +109,33 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg,
 	// XXX this is a minimal implementation. If more advanced functionalities are
 	// needed consider a full port of the commander
 
+
+	if (msg->offboard_switch)
+	{
+		msg_vehicle_control_mode.flag_control_rates_enabled = !_msg_offboard_control_mode.ignore_bodyrate ||
+			!_msg_offboard_control_mode.ignore_attitude ||
+			!_msg_offboard_control_mode.ignore_position ||
+			!_msg_offboard_control_mode.ignore_velocity ||
+			!_msg_offboard_control_mode.ignore_acceleration_force;
+
+		msg_vehicle_control_mode.flag_control_attitude_enabled = !_msg_offboard_control_mode.ignore_attitude ||
+			!_msg_offboard_control_mode.ignore_position ||
+			!_msg_offboard_control_mode.ignore_velocity ||
+			!_msg_offboard_control_mode.ignore_acceleration_force;
+
+
+		msg_vehicle_control_mode.flag_control_velocity_enabled = !_msg_offboard_control_mode.ignore_velocity ||
+			!_msg_offboard_control_mode.ignore_position;
+
+		msg_vehicle_control_mode.flag_control_climb_rate_enabled = !_msg_offboard_control_mode.ignore_velocity ||
+			!_msg_offboard_control_mode.ignore_position;
+
+		msg_vehicle_control_mode.flag_control_position_enabled = !_msg_offboard_control_mode.ignore_position;
+
+		msg_vehicle_control_mode.flag_control_altitude_enabled = !_msg_offboard_control_mode.ignore_position;
+		return;
+	}
+
 	switch (msg->mode_switch) {
 		case px4::manual_control_setpoint::SWITCH_POS_NONE:
 		ROS_WARN("Joystick button mapping error, main mode not set");
@@ -152,6 +181,11 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg,
 
 }
 
+void Commander::OffboardControlModeCallback(const px4::offboard_control_modeConstPtr &msg)
+{
+	_msg_offboard_control_mode = *msg;
+}
+
 int main(int argc, char **argv)
 {
 	ros::init(argc, argv, "commander");
diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h
index 58b7257b7..3152055ae 100644
--- a/src/platforms/ros/nodes/commander/commander.h
+++ b/src/platforms/ros/nodes/commander/commander.h
@@ -44,6 +44,7 @@
 #include <px4/vehicle_status.h>
 #include <px4/parameter_update.h>
 #include <px4/actuator_armed.h>
+#include <px4/offboard_control_mode.h>
 
 class Commander
 {
@@ -59,6 +60,11 @@ protected:
 	void ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg);
 
 	/**
+	 * Stores the offboard control mode
+	 */
+	void OffboardControlModeCallback(const px4::offboard_control_modeConstPtr &msg);
+
+	/**
 	 * Set control mode flags based on stick positions (equiv to code in px4 commander)
 	 */
 	void EvalSwitches(const px4::manual_control_setpointConstPtr &msg,
@@ -67,6 +73,7 @@ protected:
 
 	ros::NodeHandle _n;
 	ros::Subscriber _man_ctrl_sp_sub;
+	ros::Subscriber _offboard_control_mode_sub;
 	ros::Publisher _vehicle_control_mode_pub;
 	ros::Publisher _actuator_armed_pub;
 	ros::Publisher _vehicle_status_pub;
@@ -75,5 +82,6 @@ protected:
 	px4::parameter_update _msg_parameter_update;
 	px4::actuator_armed _msg_actuator_armed;
 	px4::vehicle_control_mode _msg_vehicle_control_mode;
+	px4::offboard_control_mode _msg_offboard_control_mode;
 
 };