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/nodes/commander/commander.cpp | |
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/nodes/commander/commander.cpp')
-rw-r--r-- | src/platforms/ros/nodes/commander/commander.cpp | 36 |
1 files changed, 35 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"); |