ros: commander dummy node: fix offboard support

2 files changed, 55 insertions, 23 deletions

diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp
index b0f905d23..9ca54339d 100644
--- a/src/platforms/ros/nodes/commander/commander.cpp
+++ b/src/platforms/ros/nodes/commander/commander.cpp
@@ -53,12 +53,17 @@ Commander::Commander() :
 	_msg_parameter_update(),
 	_msg_actuator_armed(),
 	_msg_vehicle_control_mode(),
-	_msg_offboard_control_mode()
+	_msg_offboard_control_mode(),
+	_got_manual_control(false)
 {
+
+	/* Default to offboard control: when no joystick is connected offboard control should just work */
+
 }
 
 void Commander::ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg)
 {
+	_got_manual_control = true;
 	px4::vehicle_status msg_vehicle_status;
 
 	/* fill vehicle control mode based on (faked) stick positions*/
@@ -103,39 +108,51 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon
 	}
 }
 
-void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg,
-		px4::vehicle_status &msg_vehicle_status,
-		px4::vehicle_control_mode &msg_vehicle_control_mode) {
-	// XXX this is a minimal implementation. If more advanced functionalities are
-	// needed consider a full port of the commander
+void Commander::SetOffboardControl(const px4::offboard_control_mode &msg_offboard_control_mode,
+			px4::vehicle_control_mode &msg_vehicle_control_mode)
+{
+	msg_vehicle_control_mode.flag_control_manual_enabled = false;
+	msg_vehicle_control_mode.flag_control_offboard_enabled = true;
+	msg_vehicle_control_mode.flag_control_auto_enabled = false;
 
+	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;
 
-	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_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_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_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_position_enabled = !_msg_offboard_control_mode.ignore_position;
+	msg_vehicle_control_mode.flag_control_altitude_enabled = !msg_offboard_control_mode.ignore_position;
+}
 
-		msg_vehicle_control_mode.flag_control_altitude_enabled = !_msg_offboard_control_mode.ignore_position;
+void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg,
+		px4::vehicle_status &msg_vehicle_status,
+		px4::vehicle_control_mode &msg_vehicle_control_mode) {
+	// XXX this is a minimal implementation. If more advanced functionalities are
+	// needed consider a full port of the commander
+
+
+	if (msg->offboard_switch == px4::manual_control_setpoint::SWITCH_POS_ON)
+	{
+		SetOffboardControl(_msg_offboard_control_mode, msg_vehicle_control_mode);
 		return;
 	}
 
+	msg_vehicle_control_mode.flag_control_offboard_enabled = false;
+
 	switch (msg->mode_switch) {
 		case px4::manual_control_setpoint::SWITCH_POS_NONE:
 		ROS_WARN("Joystick button mapping error, main mode not set");
@@ -184,6 +201,13 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg,
 void Commander::OffboardControlModeCallback(const px4::offboard_control_modeConstPtr &msg)
 {
 	_msg_offboard_control_mode = *msg;
+
+	/* Force system into offboard control mode */
+	if (!_got_manual_control) {
+		SetOffboardControl(_msg_offboard_control_mode, _msg_vehicle_control_mode);
+		_msg_vehicle_control_mode.timestamp = px4::get_time_micros();
+		_vehicle_control_mode_pub.publish(_msg_vehicle_control_mode);
+	}
 }
 
 int main(int argc, char **argv)
diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h
index 3152055ae..c537c8419 100644
--- a/src/platforms/ros/nodes/commander/commander.h
+++ b/src/platforms/ros/nodes/commander/commander.h
@@ -71,6 +71,12 @@ protected:
 			px4::vehicle_status &msg_vehicle_status,
 			px4::vehicle_control_mode &msg_vehicle_control_mode);
 
+	/**
+	 * Sets offboard controll flags in msg_vehicle_control_mode
+	 */
+	void SetOffboardControl(const px4::offboard_control_mode &msg_offboard_control_mode,
+			px4::vehicle_control_mode &msg_vehicle_control_mode);
+
 	ros::NodeHandle _n;
 	ros::Subscriber _man_ctrl_sp_sub;
 	ros::Subscriber _offboard_control_mode_sub;
@@ -84,4 +90,6 @@ protected:
 	px4::vehicle_control_mode _msg_vehicle_control_mode;
 	px4::offboard_control_mode _msg_offboard_control_mode;
 
+	bool _got_manual_control;
+
 };