diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2015-02-14 16:49:06 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2015-02-28 18:25:35 +0100 |
commit | 582c664a9c61e3b6cb4762e90ce437e5843c5d14 (patch) | |
tree | 0a15ef057daaf72530338af6651d15aef8b73ad6 /src/platforms/ros | |
parent | 01b8a18ad520a9d7bfecd3eea9a2e1dfc76b0ab1 (diff) | |
download | px4-firmware-582c664a9c61e3b6cb4762e90ce437e5843c5d14.tar.gz px4-firmware-582c664a9c61e3b6cb4762e90ce437e5843c5d14.tar.bz2 px4-firmware-582c664a9c61e3b6cb4762e90ce437e5843c5d14.zip |
ros: commander dummy node: set control flags in offboard mode
Diffstat (limited to 'src/platforms/ros')
-rw-r--r-- | src/platforms/ros/nodes/commander/commander.cpp | 36 | ||||
-rw-r--r-- | src/platforms/ros/nodes/commander/commander.h | 8 |
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; }; |