ModelPredictiveControlTrajectories::ModelPredictiveControlTrajectories(ros::NodeHandle& nh_)
:nh(nh_),
fmin(5.0),
fmax(50.0),
wmax(40.0),
minTimeSec(0.02),
min_high_upon_base_(0.5)
{
initializeParams();
desired_trajectory_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("desired_trajectory", 1);
}
FlyerInterface::FlyerInterface(ros::NodeHandle nh, ros::NodeHandle nh_private):
nh_(nh),
nh_private_(nh_private),
cfg_ctrl_srv_(ros::NodeHandle(nh_private_, "ctrl")),
cfg_x_srv_(ros::NodeHandle(nh_private_, "ctrl/pid_x")),
cfg_y_srv_(ros::NodeHandle(nh_private_, "ctrl/pid_y")),
cfg_z_srv_(ros::NodeHandle(nh_private_, "ctrl/pid_z")),
cfg_vx_srv_(ros::NodeHandle(nh_private_, "ctrl/pid_vx")),
cfg_vy_srv_(ros::NodeHandle(nh_private_, "ctrl/pid_vy")),
cfg_vz_srv_(ros::NodeHandle(nh_private_, "ctrl/pid_vz")),
cfg_yaw_srv_(ros::NodeHandle(nh_private_, "ctrl/pid_yaw")),
cfg_comm_srv_(ros::NodeHandle(nh_private_, "comm")),
connected_(false)
{
ROS_INFO("Starting FlyerInterface");
ros::NodeHandle nh_mav (nh_, "mav");
// **** initialize vaiables
cmd_roll_ = 0.0;
cmd_pitch_ = 0.0;
cmd_yaw_rate_ = 0.0;
cmd_thrust_ = 0.0;
cpu_load_index_ = 0;
cpu_loads_.resize(50);
// **** get parameters
initializeParams();
// **** dynamic reconfigure services
CfgZServer::CallbackType f1 = boost::bind(&FlyerInterface::reconfig_z_callback, this, _1, _2);
cfg_z_srv_.setCallback(f1);
CfgXServer::CallbackType f2 = boost::bind(&FlyerInterface::reconfig_x_callback, this, _1, _2);
cfg_x_srv_.setCallback(f2);
CfgYServer::CallbackType f3 = boost::bind(&FlyerInterface::reconfig_y_callback, this, _1, _2);
cfg_y_srv_.setCallback(f3);
CfgYawServer::CallbackType f4 = boost::bind(&FlyerInterface::reconfig_yaw_callback, this, _1, _2);
cfg_yaw_srv_.setCallback(f4);
CfgCommServer::CallbackType f5 = boost::bind(&FlyerInterface::reconfig_comm_callback, this, _1, _2);
cfg_comm_srv_.setCallback(f5);
CfgCtrlServer::CallbackType f6 = boost::bind(&FlyerInterface::reconfig_ctrl_callback, this, _1, _2);
cfg_ctrl_srv_.setCallback(f6);
CfgVXServer::CallbackType f7 = boost::bind(&FlyerInterface::reconfig_vx_callback, this, _1, _2);
cfg_vx_srv_.setCallback(f7);
CfgVYServer::CallbackType f8 = boost::bind(&FlyerInterface::reconfig_vy_callback, this, _1, _2);
cfg_vy_srv_.setCallback(f8);
CfgVZServer::CallbackType f9 = boost::bind(&FlyerInterface::reconfig_vz_callback, this, _1, _2);
cfg_vz_srv_.setCallback(f9);
// **** connect
connected_ = comm_.connect(serial_port_rx_, serial_port_tx_, baudrate_);
if (!connected_)
{
ROS_ERROR("unable to connect");
return;
}
// **** configure
sendCommConfig();
sendKFConfig(true); // configure and reset to 0 state
sendPIDConfig();
sendCtrlConfig();
// *** register publishers
if (publish_pose_)
{
pose_publisher_ = nh_mav.advertise<geometry_msgs::PoseStamped>(
"pose", 10);
}
vel_publisher_ = nh_mav.advertise<geometry_msgs::TwistStamped>(
"vel", 10);
imu_publisher_ = nh_mav.advertise<sensor_msgs::Imu>(
"imu", 10);
flight_state_publisher_ = nh_mav.advertise<std_msgs::UInt8>(
"status/flight_state", 1);
battery_voltage_publisher_ = nh_mav.advertise<std_msgs::Float32>(
"status/battery_voltage", 1);
cpu_load_publisher_ = nh_mav.advertise<std_msgs::Float32>(
"status/cpu_load", 1);
cpu_load_avg_publisher_ = nh_mav.advertise<std_msgs::Float32>(
"status/cpu_load_avg", 1);
if (publish_debug_)
{
debug_cmd_roll_publisher_ = nh_mav.advertise<std_msgs::Float32>(
"debug/cmd/roll", 1);
debug_cmd_pitch_publisher_ = nh_mav.advertise<std_msgs::Float32>(
"debug/cmd/pitch", 1);
debug_cmd_yaw_rate_publisher_ = nh_mav.advertise<std_msgs::Float32>(
"debug/cmd/yaw_rate", 1);
debug_cmd_thrust_publisher_ = nh_mav.advertise<std_msgs::Float32>(
"debug/cmd/thrust", 1);
debug_cmd_yaw_publisher_ = nh_mav.advertise<std_msgs::Float32>(
"debug/cmd/yaw", 1);
debug_pid_x_i_term_publisher_ = nh_mav.advertise<std_msgs::Float32>(
"debug/pid/x_i_term", 1);
debug_pid_y_i_term_publisher_ = nh_mav.advertise<std_msgs::Float32>(
"debug/pid/y_i_term", 1);
debug_pid_yaw_i_term_publisher_ = nh_mav.advertise<std_msgs::Float32>(
"debug/pid/yaw_i_term", 1);
debug_pid_z_i_term_publisher_ = nh_mav.advertise<std_msgs::Float32>(
"debug/pid/z_i_term", 1);
debug_roll_publisher_ = nh_mav.advertise<std_msgs::Float32>(
"debug/roll", 1);
debug_pitch_publisher_ = nh_mav.advertise<std_msgs::Float32>(
"debug/pitch", 1);
debug_yaw_publisher_ = nh_mav.advertise<std_msgs::Float32>(
"debug/yaw", 1);
debug_err_x_bf_publisher_= nh_mav.advertise<std_msgs::Float32>(
"debug/pid/err_x_bf", 1);
debug_err_y_bf_publisher_= nh_mav.advertise<std_msgs::Float32>(
"debug/pid/err_y_bf", 1);
debug_err_vx_bf_publisher_= nh_mav.advertise<std_msgs::Float32>(
"debug/pid/err_vx_bf", 1);
debug_err_vy_bf_publisher_= nh_mav.advertise<std_msgs::Float32>(
"debug/pid/err_vy_bf", 1);
debug_ax_bf_publisher_= nh_mav.advertise<std_msgs::Float32>(
"debug/pid/acc_x_bf", 1);
debug_ay_bf_publisher_= nh_mav.advertise<std_msgs::Float32>(
"debug/pid/acc_y_bf", 1);
debug_az_publisher_= nh_mav.advertise<std_msgs::Float32>(
"debug/pid/dvz", 1);
debug_vx_bf_publisher_= nh_mav.advertise<std_msgs::Float32>(
"debug/pid/vx_bf", 1);
debug_vy_bf_publisher_= nh_mav.advertise<std_msgs::Float32>(
"debug/pid/vy_bf", 1);
debug_imu_wf_publisher_ = nh_mav.advertise<sensor_msgs::Imu>(
"debug/imu_wf", 1);
}
// **** register callbacks for packets from serial port
comm_.registerCallback(MAV_IMU_PKT_ID, &FlyerInterface::processImuData, this);
comm_.registerCallback(MAV_POSE_PKT_ID, &FlyerInterface::processPoseData, this);
comm_.registerCallback(MAV_RCDATA_PKT_ID, &FlyerInterface::processRCData, this);
comm_.registerCallback(MAV_FLIGHT_STATE_PKT_ID, &FlyerInterface::processFlightStateData, this);
comm_.registerCallback(MAV_TIMESYNC_PKT_ID, &FlyerInterface::processTimeSyncData, this);
comm_.registerCallback(MAV_STATUS_PKT_ID, &FlyerInterface::processStatusData, this);
if (publish_debug_)
comm_.registerCallback(MAV_CTRL_DEBUG_PKT_ID, &FlyerInterface::processCtrlDebugData, this);
// **** register subscribers
// Synchronize inputs. Topic subscriptions happen on demand in the connection callback.
int queue_size = 5;
laser_pose_subscriber_.reset(new PoseStampedSubscriber(
nh_mav, "laser_pose_f", queue_size));
laser_vel_subscriber_.reset(new TwistStampedSubscriber(
nh_mav, "laser_vel_f", queue_size));
sync_.reset(new Synchronizer(
SyncPolicy(queue_size), *laser_pose_subscriber_, *laser_vel_subscriber_));
sync_->registerCallback(boost::bind(&FlyerInterface::laserCallback, this, _1, _2));
height_subscriber_ = nh_mav.subscribe(
"laser_height_f", 10, &FlyerInterface::heightCallback, this);
cmd_roll_subscriber_ = nh_mav.subscribe(
"cmd/roll", 10, &FlyerInterface::cmdRollCallback, this);
cmd_pitch_subscriber_ = nh_mav.subscribe(
"cmd/pitch", 10, &FlyerInterface::cmdPitchCallback, this);
cmd_yaw_rate_subscriber_ = nh_mav.subscribe(
"cmd/yaw_rate", 10, &FlyerInterface::cmdYawRateCallback, this);
cmd_thrust_subscriber_ = nh_mav.subscribe(
"cmd/thrust", 10, &FlyerInterface::cmdThrustCallback, this);
cmd_pose_subscriber_ = nh_mav.subscribe(
"cmd/pose", 10, &FlyerInterface::cmdPoseCallback, this);
cmd_vel_subscriber_ = nh_mav.subscribe(
"cmd/vel", 10, &FlyerInterface::cmdVelCallback, this);
// **** timers
cmd_timer_ = nh_private_.createTimer(
ros::Duration(0.05), &FlyerInterface::cmdTimerCallback, this);
// **** services
advance_state_srv_ = nh_mav.advertiseService(
"advanceState", &FlyerInterface::advanceState, this);
retreat_state_srv_ = nh_mav.advertiseService(
"retreatState", &FlyerInterface::retreatState, this);
estop_srv_ = nh_mav.advertiseService(
"estop", &FlyerInterface::estop, this);
toggle_engage_srv_ = nh_mav.advertiseService(
"toggleEngage", &FlyerInterface::toggleEngage, this);
takeoff_srv_ = nh_mav.advertiseService(
"takeoff", &FlyerInterface::takeoff, this);
land_srv_ = nh_mav.advertiseService(
"land", &FlyerInterface::land, this);
get_flight_state_srv_ = nh_mav.advertiseService(
"getFlightState", &FlyerInterface::getFlightState, this);
set_ctrl_type_srv_ = nh_mav.advertiseService(
"setCtrlType", &FlyerInterface::setCtrlType, this);
}
CtrlInterface::CtrlInterface(ros::NodeHandle nh, ros::NodeHandle nh_private):
nh_(nh),
nh_private_(nh_private),
tf_listener_(ros::Duration(10.0))
{
ROS_INFO("Starting CtrlInterface");
ros::NodeHandle nh_mav (nh_, "mav");
// **** get parameters
initializeParams();
// **** initialize vaiables
ctrl_type_ = mav::PositionCtrl;
des_vel_.linear.x = 0.0;
des_vel_.linear.y = 0.0;
des_vel_.linear.z = 0.0;
des_vel_.angular.x = 0.0;
des_vel_.angular.y = 0.0;
des_vel_.angular.z = 0.0;
tf::Pose identity;
identity.setIdentity();
tf::poseTFToMsg(identity, cur_pose_.pose);
cur_pose_.header.frame_id = fixed_frame_;
cur_goal_ = des_pose_ = cur_pose_;
////////navfn_.initialize("my_navfn_planner", &costmap_);
// *** register publishers
cmd_pose_publisher_ = nh_mav.advertise<geometry_msgs::PoseStamped>(
"cmd/pose", 10);
cmd_vel_publisher_ = nh_mav.advertise<geometry_msgs::TwistStamped>(
"cmd/vel", 10);
goal_publisher_= nh_mav.advertise<geometry_msgs::PoseStamped>(
"cmd/goal", 10);
/*plan_publisher_ = nh_mav.advertise<nav_msgs::Path>(
"plan", 10);
array_publisher_ = nh_mav.advertise<geometry_msgs::PoseArray>(
"plan_array", 10);*/
printf("ASDFF!\n");
// **** register subscribers
cur_pose_subscriber_ = nh_mav.subscribe(
"pose", 10, &CtrlInterface::curPoseCallback, this);
goal2D_subscriber_ = nh_mav.subscribe(
"goal2D_rviz", 10, &CtrlInterface::goal2Dcallback, this);
cmd_joy_vel_subscriber_ = nh_mav.subscribe(
"cmd_joy/vel", 10, &CtrlInterface::cmdJoyVelCallback, this);
/*cmd_plan_vel_subscriber_ = nh_mav.subscribe(
"cmd_plan/vel", 10, &CtrlInterface::cmdPlanVelCallback, this);*/
// **** register service servers
change_des_pose_srv_ = nh_mav.advertiseService(
"changeDesPose", &CtrlInterface::changeDesPose, this);
pos_hold_srv_ = nh_mav.advertiseService(
"positionHold", &CtrlInterface::positionHold, this);
vel_hold_srv_ = nh_mav.advertiseService(
"velocityHold", &CtrlInterface::velocityHold, this);
set_ctrl_type_client_ = nh_mav.serviceClient<mav_srvs::SetCtrlType>(
"setCtrlType");
// **** timers
cmd_timer_ = nh_private_.createTimer(
ros::Duration(0.100), &CtrlInterface::cmdTimerCallback, this);
/*plan_timer_ = nh_private_.createTimer(
ros::Duration(0.100), &CtrlInterface::planTimerCallback, this);*/
}
Mav2Ivy::Mav2Ivy(ros::NodeHandle nh, ros::NodeHandle nh_private):
nh_(nh),
nh_private_(nh_private)
{
ROS_INFO("Starting Mav2Ivy");
// NodeHandle for mav data
ros::NodeHandle nh_data (nh_, mav::ROS_NAMESPACE);
// **** get parameters
initializeParams();
// **** initialize vaiables
//motors_on_ = false;
//engaging_ = false;
// TODO handle motors on
motors_on_ = true;
engaging_ = false;
ctrl_roll_ = 0;
ctrl_pitch_ = 0;
ctrl_yaw_ = 0;
ctrl_thrust_ = 0;
// *** register publishers
imu_publisher_ = nh_data.advertise<sensor_msgs::Imu>(mav::IMU_TOPIC, 10);
height_publisher_ = nh_data.advertise<mav_msgs::Height>(mav::P_HEIGHT_TOPIC, 10);
// **** register subscribers
// Replace by IVY stuff
IvyBindMsg(ivyFPCallback,this, "^(\\S*) ROTORCRAF_FP (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*");
IvyBindMsg(ivyImuCallback, this, "^(\\S*) AHRS_LKF (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
IvyBindMsg(ivyStatusCallback, this, "^(\\S*) ROTORCRAFT_STATUS (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
//imu_calcdata_subscriber_ = nh_rawdata.subscribe(
// asctec::IMU_CALCDATA_TOPIC, 10, &Mav2Ivy::imuCalcDataCallback, this);
//ll_status_subscriber_ = nh_rawdata.subscribe(
// asctec::LL_STATUS_TOPIC, 5, &Mav2Ivy::llStatusCallback, this);
cmd_thrust_subscriber_ = nh_data.subscribe(
mav::CMD_THRUST_TOPIC, 1, &Mav2Ivy::cmdThrustCallback, this);
cmd_roll_subscriber_ = nh_data.subscribe(
mav::CMD_ROLL_TOPIC, 1, &Mav2Ivy::cmdRollCallback, this);
cmd_pitch_subscriber_ = nh_data.subscribe(
mav::CMD_PITCH_TOPIC, 1, &Mav2Ivy::cmdPitchCallback, this);
cmd_yaw_subscriber_ = nh_data.subscribe(
mav::CMD_YAW_RATE_TOPIC, 5, &Mav2Ivy::cmdYawCallback, this);
// **** services
//set_motors_on_off_srv_ = nh_data.advertiseService(
// "setMotorsOnOff", &Mav2Ivy::setMotorsOnOff, this);
get_motors_on_off_srv_ = nh_data.advertiseService(
"getMotorsOnOff", &Mav2Ivy::getMotorsOnOff, this);
}
cSQLiteDatabase::cSQLiteDatabase(){
initializeParams();
}
AsctecProc::AsctecProc(ros::NodeHandle nh, ros::NodeHandle nh_private):
nh_(nh),
nh_private_(nh_private)
{
ROS_INFO("Starting AsctecProc");
ros::NodeHandle nh_rawdata (nh_, asctec::ROS_NAMESPACE);
ros::NodeHandle nh_procdata (nh_, "mav");
// **** get parameters
initializeParams();
// **** initialize vaiables
motors_on_ = false;
engaging_ = false;
ctrl_roll_ = 0;
ctrl_pitch_ = 0;
ctrl_yaw_ = 0;
ctrl_thrust_ = 0;
assembleCtrlCommands();
// *** register publishers
imu_publisher_ = nh_procdata.advertise<sensor_msgs::Imu>(
"imu", 10);
height_publisher_ = nh_procdata.advertise<mav_msgs::Height>(
"pressure_height", 10);
height_filtered_publisher_ = nh_procdata.advertise<mav_msgs::Height>(
"pressure_height_filtered", 10);
ctrl_input_publisher_ = nh_rawdata.advertise<asctec_msgs::CtrlInput>(
asctec::CTRL_INPUT_TOPIC, 10);
// **** register subscribers
imu_calcdata_subscriber_ = nh_rawdata.subscribe(
asctec::IMU_CALCDATA_TOPIC, 10, &AsctecProc::imuCalcDataCallback, this);
ll_status_subscriber_ = nh_rawdata.subscribe(
asctec::LL_STATUS_TOPIC, 5, &AsctecProc::llStatusCallback, this);
if (enable_ctrl_thrust_)
{
cmd_thrust_subscriber_ = nh_procdata.subscribe(
"cmd_thrust", 1, &AsctecProc::cmdThrustCallback, this);
}
if (enable_ctrl_roll_)
{
cmd_roll_subscriber_ = nh_procdata.subscribe(
"cmd_roll", 1, &AsctecProc::cmdRollCallback, this);
}
if (enable_ctrl_pitch_)
{
cmd_pitch_subscriber_ = nh_procdata.subscribe(
"cmd_pitch", 1, &AsctecProc::cmdPitchCallback, this);
}
if (enable_ctrl_yaw_)
{
cmd_yaw_subscriber_ = nh_procdata.subscribe(
"cmd_yaw", 5, &AsctecProc::cmdYawCallback, this);
}
// **** services
if(enable_state_changes_)
{
set_motors_on_off_srv_ = nh_procdata.advertiseService(
"setMotorsOnOff", &AsctecProc::setMotorsOnOff, this);
}
get_motors_on_off_srv_ = nh_procdata.advertiseService(
"getMotorsOnOff", &AsctecProc::getMotorsOnOff, this);
}
