yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Thomas Gubler
2015-02-14 16:49:06 +01:00
parent 01b8a18ad5
commit 582c664a9c
2 changed files with 43 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

36
src/platforms/ros/nodes/commander/commander.cpp
View File

@@ -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");

8
src/platforms/ros/nodes/commander/commander.h
View File

@@ -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
{
@@ -58,6 +59,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)
*/
@@ -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;
};