diff --git a/src/kite_control/kiteNMPF.h b/src/kite_control/kiteNMPF.h index 33b949b..5776fdc 100644 --- a/src/kite_control/kiteNMPF.h +++ b/src/kite_control/kiteNMPF.h @@ -1,105 +1,106 @@ #ifndef KITENMPF_H #define KITENMPF_H #include "kite.h" #include enum CollocationType{GLOBAL, MULTIPLE_SHOOTING}; class KiteNMPF { public: //KiteNMPF(const KiteProperties &KiteProps, const AlgorithmProperties &AlgoProps); KiteNMPF(std::shared_ptr _Kite, const casadi::Function &_Path); //KiteNMPF(const KiteProperties &KiteProps, const AlgorithmProperties &AlgoProps, const casadi::Function &_Path); virtual ~KiteNMPF(){} /** contsraints setters */ void setLBX(const casadi::DM &_lbx){this->LBX = _lbx;} void setUBX(const casadi::DM &_ubx){this->UBX = _ubx;} void setLBG(const casadi::DM &_lbg){this->LBG = _lbg;} void setUBG(const casadi::DM &_ubg){this->UBG = _ubg;} void setLBU(const casadi::DM &_lbu){this->LBU = _lbu;} void setUBU(const casadi::DM &_ubu){this->UBU = _ubu;} void setStateScaling(const casadi::DM &Scaling){Scale_X = Scaling; invSX = casadi::DM::solve(Scale_X, casadi::DM::eye(Scale_X.size1()));} void setControlScaling(const casadi::DM &Scaling){Scale_U = Scaling; invSU = casadi::DM::solve(Scale_U, casadi::DM::eye(Scale_U.size1()));} void setReferenceVelocity(const casadi::DM &vel_ref){reference_velocity = vel_ref;} void setPath(const casadi::SX &_path); void createNLP(); void enableWarmStart(){WARM_START = true;} void disableWarmStart(){WARM_START = false;} void computeControl(const casadi::DM &_X0); casadi::DM findClosestPointOnPath(const casadi::DM &position); casadi::DM getOptimalControl(){return OptimalControl;} casadi::DM getOptimalTrajetory(){return OptimalTrajectory;} + casadi::Function getPathFunction(){return PathFunc;} bool initialized(){return _initialized;} /** lower-upper bounds for state variables */ static const casadi::DM DEFAULT_LBX; static const casadi::DM DEFAULT_UBX; /** lower-upper bounds for nonlinear constraints */ static const casadi::DM DEFAULT_LBG; static const casadi::DM DEFAULT_UBG; /** lower-upper bounds for control variables */ static const casadi::DM DEFAULT_LBU; static const casadi::DM DEFAULT_UBU; private: std::shared_ptr Kite; casadi::SX Path; casadi::Function PathFunc; casadi::SX Contraints; casadi::Function ContraintsFunc; casadi::SX reference_velocity; /** state box constraints */ casadi::DM LBX, UBX; /** nonlinear inequality constraints */ casadi::DM LBG, UBG; /** control box constraints */ casadi::DM LBU, UBU; /** state and control scaling matrixces */ casadi::DM Scale_X, invSX; casadi::DM Scale_U, invSU; /** cost function weight matrices */ casadi::SX Q, R, W, Wq; casadi::DM NLP_X; casadi::Function NLP_Solver; casadi::SXDict NLP; casadi::Dict OPTS; casadi::DMDict ARG; casadi::DM OptimalControl; casadi::DM OptimalTrajectory; unsigned NUM_SHOOTING_INTERVALS; bool WARM_START; bool _initialized; /** TRACE FUNCTIONS */ casadi::Function DynamicsFunc; casadi::Function DynamicConstraints; casadi::Function CostFunction; }; #endif // KITENMPF_H diff --git a/src/kite_control/nmpf_node.cpp b/src/kite_control/nmpf_node.cpp index 3063012..8e8f208 100644 --- a/src/kite_control/nmpf_node.cpp +++ b/src/kite_control/nmpf_node.cpp @@ -1,173 +1,230 @@ #include "nmpf_node.hpp" #include "ekf_node.h" using namespace casadi; void KiteNMPF_Node::filterCallback(const sensor_msgs::MultiDOFJointState::ConstPtr &msg) { //boost::unique_lock scoped_lock(m_mutex, boost::try_to_lock); kite_state = convertToDM(*msg); if(!is_initialized()) initialize(); } KiteNMPF_Node::KiteNMPF_Node(const ros::NodeHandle &_nh, const KiteProperties &kite_props, const AlgorithmProperties &algo_props ) { std::shared_ptr kite = std::make_shared(kite_props, algo_props); /** @badcode : parametrize path outside NMPF node constructor*/ SX x = SX::sym("x"); double radius = 2.5; double altitude = 0.00; SX Path = SX::vertcat(SXVector{radius * cos(x), radius * sin(x), altitude}); Function path = Function("path", {x}, {Path}); controller = std::make_shared(kite, path); nh = std::make_shared(_nh); /** set control constraints */ double angle_sat = kmath::deg2rad(8.0); DM lbu = DM::vertcat({0, -angle_sat, -angle_sat, -10}); DM ubu = DM::vertcat({0.2, angle_sat, angle_sat, 10}); controller->setLBU(lbu); controller->setUBU(ubu); /** set variable constraints */ DM lbx = DM::vertcat({1.0, -0.5, -DM::inf(1), -2 * M_PI, -2 * M_PI, -2 * M_PI, -DM::inf(1), -DM::inf(1), -DM::inf(1), -1, -1, -1, -1, -DM::inf(1), -DM::inf(1)}); DM ubx = DM::vertcat({12, 5, DM::inf(1), 2 * M_PI, 2 * M_PI, 2 * M_PI, DM::inf(1), DM::inf(1), DM::inf(1), 1, 1, 1, 1, DM::inf(1), DM::inf(1)}); controller->setLBX(lbx); controller->setUBX(ubx); DM vel_ref = 0.05; controller->setReferenceVelocity(vel_ref); /** create NLP */ controller->createNLP(); /** create solver for delay compensation */ nh->param("delay", transport_delay, 0.0); Dict opts; opts["tf"] = transport_delay; opts["poly_order"] = 2; opts["tol"] = 1e-4; opts["method"] = IntType::CVODES; Function system = kite->getNumericDynamics(); solver = std::make_shared(system, opts); /** initialize subscribers and publishers */ control_pub = nh->advertise("kite_controls", 100); + traj_pub = nh->advertise("/opt_traj", 10); - std::string state_topic = "ekf_node/kite_state"; + 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 */ DM opt_ctl = controller->getOptimalControl(); control = opt_ctl(Slice(0,3), 0); std::vector controls = control.get_nonzeros(); test_package::aircraft_controls control_msg; if(!controls.empty()) { 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::publish_trajectory() +{ + DM opt_trajectory = controller->getOptimalTrajetory(); + int array_size = opt_trajectory.size2(); + if(array_size == 0) + return; + + DMVector split_traj = DM::horzsplit(opt_trajectory, 1); + sensor_msgs::MultiDOFJointState opt_msg; + opt_msg.twist.resize(array_size); + opt_msg.transforms.resize(array_size); + opt_msg.joint_names.resize(array_size); + opt_msg.wrench.resize(array_size); + opt_msg.header.frame_id = "optimal_trajectory"; + int idx = 0; + + for(DMVector::const_iterator it = split_traj.begin(); it != split_traj.end(); std::advance(it, 1)) + { + std::vector row = (*it).nonzeros(); + opt_msg.twist[idx].linear.x = row[0]; + opt_msg.twist[idx].linear.y = row[1]; + opt_msg.twist[idx].linear.z = row[2]; + + opt_msg.twist[idx].angular.x = row[3]; + opt_msg.twist[idx].angular.y = row[4]; + opt_msg.twist[idx].angular.z = row[5]; + + opt_msg.transforms[idx].translation.x = row[6]; + opt_msg.transforms[idx].translation.y = row[7]; + opt_msg.transforms[idx].translation.z = row[8]; + + opt_msg.transforms[idx].rotation.w = row[9]; + opt_msg.transforms[idx].rotation.x = row[10]; + opt_msg.transforms[idx].rotation.y = row[11]; + opt_msg.transforms[idx].rotation.z = row[12]; + + /** virtual state */ + Function path = controller->getPathFunction(); + DM point = path(DMVector{row[13]})[0]; + std::vector virt_point = point.nonzeros(); + + opt_msg.wrench[idx].force.x = virt_point[0]; + opt_msg.wrench[idx].force.y = virt_point[1]; + opt_msg.wrench[idx].force.z = virt_point[2]; + idx++; + } + + traj_pub.publish(opt_msg); +} + void KiteNMPF_Node::compute_control() { /** augment state with pseudo state*/ DM augmented_state; DM opt_traj = controller->getOptimalTrajetory(); + kite_state = DM({1.5,0,0,0,0,0,-3,0,-3,1,0,0,0}); + control = DM({0.3, 0.0, 0.0}); + if(!opt_traj.is_empty()) { /** transport delay compensation */ DM predicted_state = solver->solve(kite_state, control, transport_delay); augmented_state = DM::vertcat({predicted_state, opt_traj(Slice(13, opt_traj.size1()), opt_traj.size1() - 2)}); } else { DM closest_point = controller->findClosestPointOnPath(kite_state(Slice(6,9))); augmented_state = DM::vertcat({kite_state, closest_point, 0}); } /** @badcode : dirty hack : zero-speed workaround */ - DM minimal_speed = DM({1.1}); + DM minimal_speed = DM(1.1); if (augmented_state(0, 0).nonzeros()[0] < minimal_speed.nonzeros()[0]) augmented_state(0, 0) = minimal_speed; /** compute control */ controller->computeControl(augmented_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("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(18); /** 18 Hz */ while (ros::ok()) { if(tracker.is_initialized()) { - std::cout << "COMPUTE" << "\n"; tracker.compute_control(); tracker.publish(); - std::cout << "PUBLISHED" << "\n"; + tracker.publish_trajectory(); + std::cout << "Control published..." << "\n"; } - std::cout << "LOX" << "\n"; + + tracker.compute_control(); + tracker.publish(); + tracker.publish_trajectory(); ros::spinOnce(); loop_rate.sleep(); } return 0; } diff --git a/src/kite_control/nmpf_node.hpp b/src/kite_control/nmpf_node.hpp index 687d38e..6074d0a 100644 --- a/src/kite_control/nmpf_node.hpp +++ b/src/kite_control/nmpf_node.hpp @@ -1,54 +1,56 @@ #ifndef NMPF_NODE_HPP #define NMPF_NODE_HPP #include "ros/ros.h" #include "sensor_msgs/MultiDOFJointState.h" #include "std_msgs/Int32MultiArray.h" #include "test_package/aircraft_controls.h" #include "geometry_msgs/PoseStamped.h" #include "boost/thread/mutex.hpp" #include "kiteNMPF.h" #include "integrator.h" class KiteNMPF_Node { public: KiteNMPF_Node(const ros::NodeHandle &_nh, const KiteProperties &kite_props, const AlgorithmProperties &algo_props ); virtual ~KiteNMPF_Node(){} ros::Time last_computed_control; void filterCallback(const sensor_msgs::MultiDOFJointState::ConstPtr &msg); //void compute_control(const geometry_msgs::PoseStamped &_pose); void compute_control(); void publish(); + void publish_trajectory(); void initialize(){m_initialized = true;} bool is_initialized(){return m_initialized;} boost::mutex m_mutex; private: casadi::DM control; casadi::DM kite_state; ros::Publisher control_pub; + ros::Publisher traj_pub; ros::Subscriber state_sub; /** handle instance to access node params */ std::shared_ptr nh; /** NMPF instance */ std::shared_ptr controller; std::shared_ptr solver; bool m_initialized; double transport_delay; casadi::DM convertToDM(const sensor_msgs::MultiDOFJointState &_value); }; #endif // NMPF_NODE_HPP diff --git a/src/kite_estimation/ekf_node.cpp b/src/kite_estimation/ekf_node.cpp index 00052f5..b6f98c7 100644 --- a/src/kite_estimation/ekf_node.cpp +++ b/src/kite_estimation/ekf_node.cpp @@ -1,263 +1,263 @@ #include "ekf_node.h" using namespace casadi; void KiteEKF_Node::filterCallback(const geometry_msgs::PoseStamped::ConstPtr &msg) { /** update measurement */ last_measurement_arrived = (*msg).header.stamp; //boost::unique_lock scoped_lock(m_mutex); /** @todo: revise sending mechanism instead */ if ( !measurements.empty() ) { double time_lapsed = (last_measurement_arrived - measurements.back().header.stamp).toSec(); if( time_lapsed >= 0.01 ) { measurements.push_back(*msg); //std::cout << "callback: measurement came at: " << last_measurement_arrived // << " it was: " << convertToDM(*msg) << "\n"; } } else { measurements.push_back(*msg); //std::cout << "callback: first measurement came at: " << last_measurement_arrived // << " it was: " << convertToDM(*msg) << "\n"; } } void KiteEKF_Node::controlCallback(const std_msgs::Int16MultiArray::ConstPtr &msg) { double throttle = static_cast(msg->data[0]); double elevator = static_cast(msg->data[1]); double rudder = static_cast(msg->data[2]); control = DM::vertcat({throttle, elevator, rudder}); } KiteEKF_Node::KiteEKF_Node(const ros::NodeHandle &_nh, const Function &Integrator, const Function &Jacobian) { /** @redo model initialization*/ nh = std::make_shared(_nh); measurements.set_capacity(2); kite_state.twist.resize(1); kite_state.transforms.resize(1); kite_state.joint_names.resize(1); kite_state.header.frame_id = "kite"; kite_state.joint_names[0] = "lox"; brf_offset = DMVector{-0.09, 0, -0.02}; brf_rotation = DMVector{0.0, -1.0, 0.0, 0.0}; /** create filter object */ filter = std::make_shared(Integrator, Jacobian); /** initialize subscribers and publishers */ - state_pub = nh->advertise("kite_state", 100); + state_pub = nh->advertise("/kite_state", 100); /** @todo : parametrize topics */ std::string pose_topic = "/optitrack/Kite/pose"; std::string control_topic = "/chatter"; pose_sub = nh->subscribe(pose_topic, 1000, &KiteEKF_Node::filterCallback, this); control_sub = nh->subscribe(control_topic, 1000, &KiteEKF_Node::controlCallback, this); m_initialized = false; } void KiteEKF_Node::initialize() { /** initialize kite state based on 2 sequential pose measurements */ if (measurements.size() == 2) { DM m_prev, m_new; double dt; /** @todo: work out proper mutex lock */ //boost::unique_lock scoped_lock(m_mutex); m_prev = optitrack2world(convertToDM(measurements[0])); m_new = optitrack2world(convertToDM(measurements[1])); dt = measurements[1].header.stamp.toSec() - measurements[0].header.stamp.toSec(); std::cout << "time: " << measurements[0].header.stamp << " meas_prev: " << m_prev << "\n"; std::cout << "time: " << measurements[1].header.stamp << " meas_new: " << m_new << "\n"; /** linear velocity estimation */ DM rdot = ( m_new(Slice(0, 3), 0) - m_prev(Slice(0, 3), 0) ) / dt; SX att = m_prev( Slice(3, 7), 0); DM att_inv = kmath::quat_inverse(att); SX rdot_b = kmath::quat_multiply(att_inv, SX::vertcat({0, rdot})); DM v_body = kmath::quat_multiply(rdot_b, att); v_body = v_body(Slice(1, 4), 0); /** estimate angular rates */ SX att2 = m_new( Slice(3, 7), 0); DM dq = kmath::quat_multiply(SX(att_inv), att2); DM identity = DM::vertcat({1,0,0,0}); DM qw = (2.0 / dt) * (dq - identity); DM w_body = qw(Slice(1, 4), 0); std::cout << "Initialized at: " << v_body << " " << w_body << "\n"; std::vector v_brf = v_body.nonzeros(); std::vector w_brf = w_body.nonzeros(); std::vector pose_irf = m_new.nonzeros(); kite_state.twist[0].linear.x = v_brf[0]; kite_state.twist[0].linear.y = v_brf[1]; kite_state.twist[0].linear.z = v_brf[2]; kite_state.twist[0].angular.x = w_brf[3]; kite_state.twist[0].angular.y = w_brf[4]; kite_state.twist[0].angular.z = w_brf[5]; kite_state.transforms[0].translation.x = pose_irf[0]; kite_state.transforms[0].translation.y = pose_irf[1]; kite_state.transforms[0].translation.z = pose_irf[2]; kite_state.transforms[0].rotation.w = pose_irf[3]; kite_state.transforms[0].rotation.x = pose_irf[4]; kite_state.transforms[0].rotation.y = pose_irf[5]; kite_state.transforms[0].rotation.z = pose_irf[6]; /** update filter time */ filter->setTime( measurements.back().header.stamp.toSec() ); /** initialize filter */ filter->setEstimation( DM::vertcat({v_body, w_body, m_new}) ); m_initialized = true; } } DM KiteEKF_Node::convertToDM(const geometry_msgs::PoseStamped &_value) { DM value = DM::zeros(7); value[0] = _value.pose.position.x; value[1] = _value.pose.position.y; value[2] = _value.pose.position.z; value[3] = _value.pose.orientation.w; value[4] = _value.pose.orientation.x; value[5] = _value.pose.orientation.y; value[6] = _value.pose.orientation.z; return value; } DM KiteEKF_Node::optitrack2world(const DM &opt_pose) { DM position = opt_pose(Slice(0, 3), 0); DM attitude = opt_pose(Slice(3, 7), 0); DM tmp = kmath::quat_multiply(attitude, DM::vertcat({0, brf_offset})); DM offset_b = kmath::quat_multiply(tmp, kmath::quat_inverse(attitude)); position += offset_b(Slice(1, 4), 0); /** transform to world ref frame */ tmp = kmath::quat_multiply(brf_rotation, DM::vertcat({0, position})); DM world_pos = kmath::quat_multiply(tmp, kmath::quat_inverse(brf_rotation)); world_pos = world_pos(Slice(1, 4), 0); tmp = kmath::quat_multiply(brf_rotation, attitude); DM world_att = kmath::quat_multiply(tmp, kmath::quat_inverse(brf_rotation)); /** @todo : remove this hack */ world_pos(2) = world_pos(2) + 3; return DM::vertcat({world_pos, world_att}); } void KiteEKF_Node::estimate() { //get observation DM observation = optitrack2world(convertToDM(measurements.back())); double tstamp = measurements.back().header.stamp.toSec(); filter->estimate(observation, tstamp); /** update filter time */ filter->setTime(tstamp); } void KiteEKF_Node::publish() { /** pack estimation to ROS message */ std::vector state_vec = filter->getEstimation().nonzeros(); kite_state.header.stamp = ros::Time::now(); kite_state.twist[0].linear.x = state_vec[0]; kite_state.twist[0].linear.y = state_vec[1]; kite_state.twist[0].linear.z = state_vec[2]; kite_state.twist[0].angular.x = state_vec[3]; kite_state.twist[0].angular.y = state_vec[4]; kite_state.twist[0].angular.z = state_vec[5]; kite_state.transforms[0].translation.x = state_vec[6]; kite_state.transforms[0].translation.y = state_vec[7]; kite_state.transforms[0].translation.z = state_vec[8]; kite_state.transforms[0].rotation.w = state_vec[9]; kite_state.transforms[0].rotation.x = state_vec[10]; kite_state.transforms[0].rotation.y = state_vec[11]; kite_state.transforms[0].rotation.z = state_vec[12]; /** publish current state estimation */ state_pub.publish(kite_state); } int main(int argc, char **argv) { ros::init(argc, argv, "ekf_node"); ros::NodeHandle n("~"); unsigned tries = 0; unsigned locks = 0; /** create a kite object */ std::string kite_params_file; n.param("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; KiteDynamics kite = KiteDynamics(kite_props, algo_props); /** create a rigid body topic */ RigidBodyKinematics rigid_body = RigidBodyKinematics(algo_props); Function fIntegrator = rigid_body.getNumericIntegrator(); Function fJacobian = rigid_body.getNumericJacobian(); /** create an EKF instance */ KiteEKF_Node filter(n, fIntegrator, fJacobian); ros::Rate loop_rate(50); /** 50 Hz */ while (ros::ok()) { //std::cout << "TRIES: " << tries << " LOCKS: " << locks << "\n"; /** Some complicated logic here please */ if (!filter.initialized()) { std::cout << "CANNOT INITIALIZE \n"; //boost::unique_lock scoped_lock(filter.m_mutex); filter.initialize(); } else { /** perform estimation */ //tries++; //boost::unique_lock scoped_lock(filter.m_mutex); ///locks++; filter.estimate(); filter.publish(); } ros::spinOnce(); loop_rate.sleep(); } return 0; } diff --git a/src/nodes/kite_visualization_node.cpp b/src/nodes/kite_visualization_node.cpp index 415bca1..cca0e1d 100644 --- a/src/nodes/kite_visualization_node.cpp +++ b/src/nodes/kite_visualization_node.cpp @@ -1,400 +1,409 @@ #include #include #include #include "sensor_msgs/MultiDOFJointState.h" #include "geometry_msgs/Vector3.h" #include "geometry_msgs/Quaternion.h" #include "geometry_msgs/Point.h" #include "geometry_msgs/Transform.h" -#include "kite.h" +#include "kitemath.h" using namespace casadi; using namespace kmath; struct Scale { float x; float y; float z; Scale(const float &_x=1, const float &_y=1, const float &_z=1) : x(_x), y(_y), z(_z) {} virtual ~Scale(){} }; struct Color { float r; float g; float b; float transparency; Color(const float &_r=0, const float &_g=0, const float &_b=1, const float &_t=0) : r(_r), g(_g), b(_b), transparency(_t){} virtual ~Color(){} }; struct MarkerProperties { int type; int id; int action; std::string frame_id; std::string ns; std::string mesh_resource; Scale scale; Color color; geometry_msgs::Point position; geometry_msgs::Quaternion orientation; MarkerProperties() : type(visualization_msgs::Marker::SPHERE), id(0), action(visualization_msgs::Marker::ADD), frame_id("/kite"), ns("awe"), scale(Scale()), color(Color()), mesh_resource("") { position.x = position.y = position.z = 0; orientation.x = orientation.y = orientation.z = 0; orientation.w = 1; } void configureMarker(visualization_msgs::Marker &marker); }; void MarkerProperties::configureMarker(visualization_msgs::Marker &marker) { marker.type = type; marker.id = id; + marker.ns = ns; marker.header.frame_id = frame_id; marker.header.stamp = ros::Time::now(); marker.action = action; marker.color.r = color.r; marker.color.g = color.g; marker.color.b = color.b; marker.color.a = color.transparency; marker.scale.x = scale.x; marker.scale.y = scale.y; marker.scale.z = scale.z; marker.pose.position = position; marker.pose.orientation = orientation; if(type == visualization_msgs::Marker::MESH_RESOURCE) marker.mesh_resource = mesh_resource; } class KiteVisualizer { public: KiteVisualizer(const ros::NodeHandle &_nh); virtual ~KiteVisualizer(){} geometry_msgs::Point getTranslation(){geometry_msgs::Vector3 point = kite_state.transforms[0].translation; geometry_msgs::Point p; p.x = point.x; p.y = point.y; p.z = point.z; return p;} geometry_msgs::Quaternion getRotation(){return kite_state.transforms[0].rotation;} bool is_initialized(){return initialized;} uint32_t getNumPublishers(){return state_sub.getNumPublishers();} DM world2rviz(const casadi::DM &pose); void state2marker(const casadi::DM &kite_pose, visualization_msgs::Marker &_marker); visualization_msgs::Marker getPose(){return m_kite_marker;} visualization_msgs::MarkerArray getOptimalTrajectory(); visualization_msgs::Marker getVirtualTrajectory(); visualization_msgs::Marker getKiteMarker(){return m_kite_marker;} void setPath(const Function &path){PathFunction = path;} private: std::shared_ptr nh; ros::Subscriber state_sub; ros::Subscriber opt_traj_sub; void filterCallback(const sensor_msgs::MultiDOFJointState::ConstPtr &msg); void controlCallback(const sensor_msgs::MultiDOFJointState::ConstPtr &msg); sensor_msgs::MultiDOFJointState kite_state; visualization_msgs::Marker m_kite_marker; /** optimal trajectory visualisation */ visualization_msgs::MarkerArray kite_traj_markers; visualization_msgs::Marker virtual_state_markers; DM optimal_trajectory; Function PathFunction; bool initialized; - DM convertToDM(const sensor_msgs::MultiDOFJointState &_value, const bool &with_virtual = 0); + DM convertToDM(const sensor_msgs::MultiDOFJointState &_value, const bool &with_virtual = false); }; KiteVisualizer::KiteVisualizer(const ros::NodeHandle &_nh) { nh = std::make_shared(_nh); kite_state.twist.resize(1); kite_state.transforms.resize(1); kite_state.joint_names.resize(1); kite_state.header.frame_id = "kite"; kite_state.joint_names[0] = "lox"; std::string state_topic = "/kite_state"; state_sub = nh->subscribe(state_topic, 100, &KiteVisualizer::filterCallback, this); opt_traj_sub = nh->subscribe("/opt_traj", 100, &KiteVisualizer::controlCallback, this); /** configure kite_marker */ MarkerProperties m_props; m_props.id = 101; m_props.type = visualization_msgs::Marker::MESH_RESOURCE; m_props.mesh_resource = "package://test_package/meshes/kite_rviz.dae"; m_props.scale = Scale(0.1, 0.1, 0.1); - m_props.color = Color(0.0, 1.0, 0.0, 0.5); + m_props.color = Color(0.0, 1.0, 0.0, 1.0); m_props.configureMarker(m_kite_marker); m_kite_marker.lifetime = ros::Duration(); /** configure virtual state trajectory */ m_props.id = 201; m_props.type = visualization_msgs::Marker::SPHERE_LIST; m_props.color = Color(1.0, 0.0, 0.0, 0.5); m_props.configureMarker(virtual_state_markers); virtual_state_markers.lifetime = ros::Duration(); initialized = false; } void KiteVisualizer::filterCallback(const sensor_msgs::MultiDOFJointState::ConstPtr &msg) { /** transform kite pose to Rviz reference frame */ /** @badcode: */ DM pose = convertToDM(*msg); DM kite_state_dm = world2rviz(pose); state2marker(kite_state_dm, m_kite_marker); initialized = true; } void KiteVisualizer::controlCallback(const sensor_msgs::MultiDOFJointState::ConstPtr &msg) { - DM pose = convertToDM(*msg); - optimal_trajectory = world2rviz(pose); + optimal_trajectory = convertToDM(*msg, true); + //optimal_trajectory = world2rviz(pose); } DM KiteVisualizer::convertToDM(const sensor_msgs::MultiDOFJointState &_value, const bool &with_virtual) { uint num_poses = _value.transforms.size(); int size = with_virtual ? 10 : 7; DM poses = DM::zeros(size, num_poses); for(uint i = 0; i < num_poses; ++i) { poses(0, i) = _value.transforms[i].translation.x; poses(1, i) = _value.transforms[i].translation.y; poses(2, i) = _value.transforms[i].translation.z; poses(3, i) = _value.transforms[i].rotation.w; poses(4, i) = _value.transforms[i].rotation.x; poses(5, i) = _value.transforms[i].rotation.y; poses(6, i) = _value.transforms[i].rotation.z; if(with_virtual) { poses(7, i) = _value.wrench[i].force.x; poses(8, i) = _value.wrench[i].force.y; poses(9, i) = _value.wrench[i].force.z; } } return poses; } DM KiteVisualizer::world2rviz(const DM &world_pose) { DM pose = DM::zeros(world_pose.size()); DM transform = DM::vertcat({0, 1, 0, 0}); /** do only position transform if receive a point*/ if(pose.size1() == 3) { DM q_pose = quat_multiply(transform, DM::vertcat({0, world_pose}) ); DM tmp = quat_multiply(q_pose, quat_inverse(transform) ); pose = tmp(Slice(1,4), 0); return pose; } /** position transform */ DM q_pose = quat_multiply(transform, DM::vertcat({0, world_pose(Slice(0,3), 0) }) ); DM tmp = quat_multiply(q_pose, quat_inverse(transform) ); pose(Slice(0,3), 0) = tmp(Slice(1,4), 0); /** attitude transform */ pose(Slice(3,7),0) = quat_multiply(quat_multiply(transform, world_pose(Slice(3,7), 0)), quat_inverse(transform)); if(pose.size1() == 10) { q_pose = quat_multiply(transform, DM::vertcat({0, world_pose(Slice(7,10), 0) }) ); tmp = quat_multiply(q_pose, quat_inverse(transform) ); pose(Slice(7,10), 0) = tmp(Slice(1,4), 0); } return pose; } visualization_msgs::MarkerArray KiteVisualizer::getOptimalTrajectory() { visualization_msgs::MarkerArray m_array; int array_size = optimal_trajectory.size2(); if(array_size == 0) return m_array; DMVector split_traj = DM::horzsplit(optimal_trajectory, 1); visualization_msgs::Marker tmp_marker = m_kite_marker; + int id_counter = m_kite_marker.id; - for(DMVector::const_iterator it = split_traj.begin(); it != split_traj.end() - 1; std::advance(it, 2)) + for(DMVector::const_iterator it = split_traj.begin(); it != split_traj.end(); std::advance(it, 2)) { /** transform to rviz ref frame */ DM state = world2rviz(*it); + //std::cout << state << "\n"; + id_counter += 2; std::vector row = state.nonzeros(); - tmp_marker.pose.position.x = row[6]; - tmp_marker.pose.position.y = row[7]; - tmp_marker.pose.position.z = row[8]; + tmp_marker.pose.position.x = row[0]; + tmp_marker.pose.position.y = row[1]; + tmp_marker.pose.position.z = row[2]; + + tmp_marker.pose.orientation.w = row[3]; + tmp_marker.pose.orientation.x = row[4]; + tmp_marker.pose.orientation.y = row[5]; + tmp_marker.pose.orientation.z = row[6]; - tmp_marker.pose.orientation.w = row[9]; - tmp_marker.pose.orientation.x = row[10]; - tmp_marker.pose.orientation.y = row[11]; - tmp_marker.pose.orientation.z = row[12]; + tmp_marker.id = id_counter; m_array.markers.push_back(tmp_marker); } return m_array; } visualization_msgs::Marker KiteVisualizer::getVirtualTrajectory() { visualization_msgs::Marker m_array; int array_size = optimal_trajectory.size2(); - if(array_size == 0 || PathFunction.is_null()) + if(array_size == 0) return m_array; DMVector split_traj = DM::horzsplit(optimal_trajectory, 1); virtual_state_markers.points.clear(); - for(DMVector::const_iterator it = split_traj.begin(); it != split_traj.end(); std::advance(it, 1)) + for(DMVector::const_iterator it = split_traj.begin(); it != split_traj.end(); std::advance(it, 2)) { - DM theta = (*it)(13); - DMVector res = PathFunction(DMVector{theta}); - DM point = world2rviz(res[0]); + /** REDO */ + DM point = (*it)(Slice(7, 10)); + point = world2rviz(point); geometry_msgs::Point p; std::vector coords = point.nonzeros(); p.x = coords[0]; p.y = coords[1]; p.z = coords[2]; + std::cout << p << "\n"; + virtual_state_markers.points.push_back(p); } return m_array; } void KiteVisualizer::state2marker(const DM &kite_pose, visualization_msgs::Marker &_marker) { std::vector pose = kite_pose.nonzeros(); _marker.pose.position.x = pose[0]; _marker.pose.position.y = pose[1]; _marker.pose.position.z = pose[2]; _marker.pose.orientation.w = pose[3]; _marker.pose.orientation.x = pose[4]; _marker.pose.orientation.y = pose[5]; _marker.pose.orientation.z = pose[6]; } int main( int argc, char** argv ) { ros::init(argc, argv, "kite_visualization_node"); ros::NodeHandle n; ros::Rate r(10); - ros::Publisher marker_pub = n.advertise("visualization_marker", 10); - ros::Publisher trajec_pub = n.advertise("trajectory_marker", 10); + ros::Publisher marker_pub = n.advertise("/visualization_marker", 10); + ros::Publisher trajec_pub = n.advertise("/trajectory_marker", 10); KiteVisualizer kite(n); visualization_msgs::Marker gs_marker; visualization_msgs::Marker kite_marker; visualization_msgs::Marker path_marker; MarkerProperties marker_props; marker_props.type = visualization_msgs::Marker::CYLINDER; marker_props.scale = Scale(0.1, 0.1, 2.0); marker_props.color = Color(0.0, 1.0, 0.0, 1.0); marker_props.configureMarker(gs_marker); gs_marker.pose.position.z = 1.0; gs_marker.lifetime = ros::Duration(); /** kite marker set up */ marker_props.id = 1; marker_props.type = visualization_msgs::Marker::MESH_RESOURCE; marker_props.mesh_resource = "package://test_package/meshes/kite_rviz.dae"; marker_props.scale = Scale(0.1, 0.1, 0.1); marker_props.configureMarker(kite_marker); kite_marker.lifetime = ros::Duration(); /** path markers set up */ marker_props.id = 3; marker_props.type = visualization_msgs::Marker::LINE_STRIP; marker_props.scale.x = 0.025; + marker_props.color = Color(0, 1, 0, 0.5); marker_props.configureMarker(path_marker); path_marker.lifetime = ros::Duration(); /** create a path */ SX x = SX::sym("x"); double radius = 2.31; double altitude = 3.0; SX Path = SX::vertcat(SXVector{radius * cos(x), radius * sin(x), altitude}); Function path_fun = Function("path", {x}, {Path}); for(double alpha = 0; alpha < 2 * M_PI; alpha += 0.2) { DMVector res = path_fun( DMVector{DM(alpha)} ); geometry_msgs::Point p; DM point = res[0]; p.x = point.nonzeros()[0]; p.y = point.nonzeros()[1]; p.z = point.nonzeros()[2]; path_marker.points.push_back(p); } bool published_once = false; while (ros::ok()) { // Publish the marker while (marker_pub.getNumSubscribers() < 1) { if (!ros::ok()) { return 0; } ROS_WARN_ONCE("Please create a subscriber to the marker"); sleep(1); } if(kite.is_initialized()) { kite_marker = kite.getKiteMarker(); marker_pub.publish(kite_marker); } /** draw optimal trajectories */ - visualization_msgs::Marker virt_markers = kite.getVirtualTrajectory(); + visualization_msgs::Marker virt_markers = kite.getVirtualTrajectory(); visualization_msgs::MarkerArray opt_markers = kite.getOptimalTrajectory(); if(!opt_markers.markers.empty()) trajec_pub.publish(opt_markers); if(!virt_markers.points.empty()) marker_pub.publish(virt_markers); if(!published_once) { marker_pub.publish(gs_marker); marker_pub.publish(path_marker); published_once = true; } ros::spinOnce(); r.sleep(); } }