diff options
-rw-r--r-- | src/platforms/ros/nodes/commander/commander.cpp | 70 | ||||
-rw-r--r-- | src/platforms/ros/nodes/commander/commander.h | 8 |
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; + }; |