Homec4science
Diffusion test_package (master) Browse

R1517/src/nodes/nmpf_node.cpp
8c836b7e8628master

Last Change
Raw File

nmpf_node.cpp
View Options

#include "nmpf_node.hpp"
#include "ekf_node.h"
using namespace casadi;
void KiteNMPF_Node::filterCallback(const sensor_msgs::MultiDOFJointState::ConstPtr &msg)
{
boost::unique_lock<boost::mutex> scoped_lock(m_mutex, boost::try_to_lock);
kite_state = convertToDM(*msg);
}
KiteNMPF_Node::KiteNMPF_Node(const ros::NodeHandle &_nh, const KiteProperties &kite_props,
const AlgorithmProperties &algo_props )
{
controller = std::make_shared<KiteNMPF>(kite_props, algo_props);
nh = std::make_shared<ros::NodeHandle>(_nh);
/** set control constraints */
double angle_sat = math_utils::deg2rad(7.0);
DM lbu = DM::vertcat({0, -angle_sat, -angle_sat, 0});
DM ubu = DM::vertcat({0.3, angle_sat, angle_sat, 1});
controller->setLBU(lbu);
controller->setUBU(ubu);
/** set variable constraints */
DM lbx = DM::vertcat({5, -DM::inf(1), -DM::inf(1), -M_PI, -M_PI, -M_PI, -DM::inf(1), -DM::inf(1), -DM::inf(1),
-DM::inf(1), -DM::inf(1), -DM::inf(1), -DM::inf(1), -DM::inf(1), -1});
DM ubx = DM::vertcat({12, DM::inf(1), DM::inf(1), M_PI, M_PI, M_PI, DM::inf(1), DM::inf(1), DM::inf(1),
DM::inf(1), DM::inf(1), DM::inf(1), DM::inf(1), DM::inf(1), 12});
controller->setLBX(lbx);
controller->setUBX(ubx);
/** test cheb */
controller->createNLP();
/** initialize subscribers and publishers */
control_pub = nh->advertise<test_package::aircraft_controls>("kite_controls", 100);
std::string state_topic = "/kite_state";
state_sub = nh->subscribe(state_topic, 100, &KiteNMPF_Node::filterCallback, this);
m_initialized = false;
}
DM KiteNMPF_Node::convertToDM(const sensor_msgs::MultiDOFJointState &_value)
{
DM value = DM::zeros(13);
value[0] = _value.twist.back().linear.x;
value[1] = _value.twist.back().linear.y;
value[2] = _value.twist.back().linear.z;
value[3] = _value.twist.back().angular.x;
value[4] = _value.twist.back().angular.y;
value[5] = _value.twist.back().angular.z;
value[6] = _value.transforms.back().translation.x;
value[7] = _value.transforms.back().translation.y;
value[8] = _value.transforms.back().translation.z;
value[9] = _value.transforms.back().rotation.w;
value[10] = _value.transforms.back().rotation.x;
value[11] = _value.transforms.back().rotation.y;
value[12] = _value.transforms.back().rotation.z;
return value;
}
void KiteNMPF_Node::publish()
{
/** pack estimation to ROS message */
std::vector<double> controls = controller->getOptimalControl().get_nonzeros();
test_package::aircraft_controls control_msg;
control_msg.header.stamp = ros::Time::now();
control_msg.thrust = controls[0];
control_msg.elevator = controls[1];
control_msg.rudder = controls[2];
/** publish current control */
control_pub.publish(control_msg);
}
void KiteNMPF_Node::compute_control()
{
controller->computeControl(kite_state);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "nmpf_node");
ros::NodeHandle n;
/** create a kite object */
std::string kite_params_file;
n.param<std::string>("kite_params", kite_params_file, "umx_radian.yaml");
std::cout << kite_params_file << "\n";
KiteProperties kite_props = kite_utils::LoadProperties(kite_params_file);
AlgorithmProperties algo_props;
algo_props.Integrator = RK4;
algo_props.sampling_time = 0.02;
/** create a NMPF instance */
KiteNMPF_Node tracker(n, kite_props, algo_props);
ros::Rate loop_rate(20); /** 20 Hz */
while (ros::ok())
{
tracker.compute_control();
tracker.publish();
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
c4science · Help