头文件
#include <ros/ros.h>
#include "PX4CtrlFSM.h"
#include <signal.h>
1.初始化节点
ros::init(argc, argv, "px4ctrl");
ros::NodeHandle nh("~"); //初始化句柄
signal(SIGINT, mySigintHandler); //触发退出终端
ros::Duration(1.0).sleep();
2.初始化参数和类
Parameter_t param; //Parameter_t在PX4CtrlFSM.h中被开放
param.config_from_ros_handle(nh); //用read_essential_param函数读取参数->使用getParam将参数读取到节点(初始化)
// Controller controller(param);
LinearControl controller(param); //LinearControl在contrller.h->LinearControl类中被开放,形参Parameter_t
PX4CtrlFSM fsm(param, controller);
3.订阅相关话题和服务
ros::Subscriber state_sub =
nh.subscribe<mavros_msgs::State>("/mavros/state",
10,
boost::bind(&State_Data_t::feed, &fsm.state_data, _1));
//订阅mavros连接状态
ros::Subscriber extended_state_sub =
nh.subscribe<mavros_msgs::ExtendedState>("/mavros/extended_state",
10,
boost::bind(&ExtendedState_Data_t::feed, &fsm.extended_state_data, _1));
//订阅mavros连接状态
ros::Subscriber odom_sub =
nh.subscribe<nav_msgs::Odometry>("odom",
100,
boost::bind(&Odom_Data_t::feed, &fsm.odom_data, _1),
ros::VoidConstPtr(),
ros::TransportHints().tcpNoDelay());
//订阅里程计
ros::Subscriber cmd_sub =
nh.subscribe<quadrotor_msgs::PositionCommand>("cmd",
100,
boost::bind(&Command_Data_t::feed, &fsm.cmd_data, _1),
ros::VoidConstPtr(),
ros::TransportHints().tcpNoDelay());
//cmd暂时不知道谁发布的
ros::Subscriber imu_sub =
nh.subscribe<sensor_msgs::Imu>("/mavros/imu/data", // Note: do NOT change it to /mavros/imu/data_raw !!!
100,
boost::bind(&Imu_Data_t::feed, &fsm.imu_data, _1),
ros::VoidConstPtr(),
ros::TransportHints().tcpNoDelay());
//订阅飞控imu数据
ros::Subscriber rc_sub;
//.........................................省略.................................................//
//下面是建立服务
fsm.ctrl_FCU_pub = nh.advertise<mavros_msgs::AttitudeTarget>("/mavros/setpoint_raw/attitude", 10);
//.........................................省略.................................................//
4.如果设置禁用遥控器则警告,未设置则err直至有信号输入
if (param.takeoff_land.no_RC)
{
ROS_WARN("PX4CTRL] Remote controller disabled, be careful!");
}
else
{
ROS_INFO("PX4CTRL] Waiting for RC");
while (ros::ok())
{
ros::spinOnce();
if (fsm.rc_is_received(ros::Time::now()))
{
ROS_INFO("[PX4CTRL] RC received.");
break;
}
ros::Duration(0.1).sleep();
}
}
5.检查ros状态及mavros连接状态,直至连接成功.执行 fsm.process()
while (ros::ok() && !fsm.state_data.current_state.connected)
{
ros::spinOnce();
ros::Duration(1.0).sleep();
if (trials++ > 5)
ROS_ERROR("Unable to connnect to PX4!!!");
}
ros::Rate r(param.ctrl_freq_max);
while (ros::ok())
{
r.sleep();
ros::spinOnce();
fsm.process(); // We DO NOT rely on feedback as trigger, since there is no significant performance difference through our test.
}
return 0;
}
State_Data_t在input中定义current_state变量类型为 mavros_msgs::State
class State_Data_t
{
public:
mavros_msgs::State current_state;
mavros_msgs::State state_before_offboard;
State_Data_t();
void feed(mavros_msgs::StateConstPtr pMsg);
};
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)