ssdk_interface.cpp
Go to the documentation of this file.
00001 /*
00002 
00003 Copyright (c) 2011, Markus Achtelik, ASL, ETH Zurich, Switzerland
00004 You can contact the author at <markus dot achtelik at mavt dot ethz dot ch>
00005 
00006 All rights reserved.
00007 
00008 Redistribution and use in source and binary forms, with or without
00009 modification, are permitted provided that the following conditions are met:
00010 * Redistributions of source code must retain the above copyright
00011 notice, this list of conditions and the following disclaimer.
00012 * Redistributions in binary form must reproduce the above copyright
00013 notice, this list of conditions and the following disclaimer in the
00014 documentation and/or other materials provided with the distribution.
00015 * Neither the name of ETHZ-ASL nor the
00016 names of its contributors may be used to endorse or promote products
00017 derived from this software without specific prior written permission.
00018 
00019 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00020 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00021 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00022 DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY
00023 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00024 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00025 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00026 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00027 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00028 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00029 
00030 */
00031 
00032 #include "ssdk_interface.h"
00033 #include "helper.h"
00034 #include <asctec_hl_comm/mav_ctrl.h>
00035 
00036 SSDKInterface::SSDKInterface(ros::NodeHandle & nh, CommPtr & comm):
00037   nh_(nh),
00038   pnh_("~/ssdk"),
00039   comm_(comm),
00040   have_config_(false)
00041 {
00042   ros::NodeHandle _pnh("~");
00043   _pnh.param("frame_id", frame_id_, std::string("fcu"));
00044 
00045   ROS_WARN_COND(!pnh_.hasParam("omega_0_xy"), "no ssdk parameters available, position control on the HLP will not work!");
00046 
00047   pose_sub_ = nh_.subscribe("pose", 1, &SSDKInterface::cbPose, this);
00048   state_sub_ = nh_.subscribe("state", 1, &SSDKInterface::cbState, this);
00049   odometry_sub_ = nh_.subscribe("odometry", 1, &SSDKInterface::cbOdometry, this);
00050   debug_pub_ = nh_.advertise<asctec_hl_comm::DoubleArrayStamped> ("debug", 1);
00051   pose_pub_ = nh_.advertise<geometry_msgs::PoseStamped> ("current_pose", 1);
00052 
00053   config_server_ = new SSDKConfigServer(pnh_);
00054   SSDKConfigServer::CallbackType f = boost::bind(&SSDKInterface::cbSSDKConfig, this, _1, _2);
00055   config_server_->setCallback(f);
00056 
00057   comm_->registerCallback(HLI_PACKET_ID_SSDK_DEBUG, &SSDKInterface::processDebugData, this);
00058   comm_->registerCallback(HLI_PACKET_ID_SSDK_STATUS, &SSDKInterface::processStatusData, this);
00059 }
00060 
00061 SSDKInterface::~SSDKInterface()
00062 {
00063   delete config_server_;
00064 }
00065 
00066 
00067 void SSDKInterface::tfCallback()
00068 {
00069   tf::StampedTransform pose;
00070   static tf::StampedTransform last_pose;
00071   ros::Time tf_time(0);
00072   if (tf_listener_.canTransform(config_.tf_ref_frame_id, config_.tf_tracked_frame_id, tf_time))
00073   {
00074     tf_listener_.lookupTransform(config_.tf_ref_frame_id, config_.tf_tracked_frame_id, tf_time, pose);
00075 
00076     // only send new poses to HLP checking one coordinate should be sufficient since it will never be the same
00077     if (pose.getOrigin() != last_pose.getOrigin())
00078     {
00079       const tf::Vector3 & pos = pose.getOrigin();
00080       const tf::Quaternion & q = pose.getRotation();
00081       sendPoseToAP(pos.x(), pos.y(), pos.z(), tf::getYaw(q), 100);
00082       last_pose = pose;
00083     }
00084   }
00085 }
00086 
00087 void SSDKInterface::cbPose(const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg)
00088 {
00089   const geometry_msgs::Point & pos = msg->pose.pose.position;
00090   double yaw = tf::getYaw(msg->pose.pose.orientation);
00091 
00092   char qual = 100;
00093 
00094   for (unsigned int i = 0; i < msg->pose.covariance.size(); i++)
00095   {
00096     if (msg->pose.covariance[i] > 1)
00097     {
00098       qual = 0;
00099     }
00100   }
00101 
00102   sendPoseToAP(pos.x, pos.y, pos.z, yaw, qual);
00103 }
00104 
00105 
00106 void SSDKInterface::sendPoseToAP(const double & x, const double & y, const double & z, const double & yaw,
00107                                const unsigned char & qual)
00108 {
00109   static HLI_EXT_POSITION pose;
00110   static uint8_t count = 0;
00111 
00112   pose.x = (int)(x * 1000);
00113   pose.y = (int)(y * 1000);
00114   pose.z = (int)(z * 1000);
00115   pose.heading = helper::yaw2asctec(yaw);
00116   pose.qualX = qual;
00117   pose.qualY = qual;
00118   pose.qualZ = qual;
00119 
00120   pose.vX = 0;
00121   pose.vY = 0;
00122   pose.vZ = 0;
00123   pose.qualVx = 0;
00124   pose.qualVy = 0;
00125   pose.qualVz = 0;
00126 
00127   pose.bitfield = 0;
00128 
00129   pose.count = count++;
00130 
00131   comm_->sendPacket(HLI_PACKET_ID_POSITION_UPDATE, pose);
00132 }
00133 
00134 
00135 void SSDKInterface::cbSSDKConfig(asctec_hl_interface::SSDKConfig & config, uint32_t level)
00136 {
00137   config.p31 = config.omega_0_xy * config.omega_0_xy;
00138   config.p32 = config.omega_0_z * config.omega_0_z;
00139   config.p33 = config.omega_0_xy * config.zeta_xy * 2.0;
00140   config.p34 = config.omega_0_z * config.zeta_z * 2.0;
00141 
00142   if (level & asctec_hl_interface::SSDK_SEND_PARAMETERS)
00143   {
00144     if(!sendParameters(config)){
00145       // TODO: not sure if this is maybe too harsh, but we would be in a somewhat undefined state
00146       ROS_FATAL("Could not send SSDK parameters to the HLP! Shutting down the node");
00147       ros::shutdown();
00148     }
00149     config.send = false;
00150   }
00151 
00152   if (level & asctec_hl_interface::SSDK_TF_CHANGED)
00153   {
00154     if(tf_callback_.connected())
00155     {
00156       tf_listener_.removeTransformsChangedListener(tf_callback_);
00157     }
00158   }
00159 
00160   config_ = config;
00161   have_config_ = true;
00162 
00163   if ((level & asctec_hl_interface::SSDK_TF_CHANGED) && config.listen_on_tf)
00164   {
00165     if(!tf_callback_.connected())
00166       tf_callback_ = tf_listener_.addTransformsChangedListener(boost::bind(&SSDKInterface::tfCallback, this));
00167   }
00168 }
00169 
00170 
00171 void SSDKInterface::cbState(const sensor_fusion_comm::ExtStatePtr & msg)
00172 {
00173   // TODO: untested, use that with care !!!
00174 
00175   // receive state (position, yaw, velocity) and directly pass it to the HL position controller by bypassing the
00176   // state observer on the HL
00177 
00178   HLI_EXT_POSITION state;
00179 
00180   double yaw = tf::getYaw(msg->pose.orientation);
00181 
00182   char qual = 100;
00183 
00184   state.x = static_cast<int> (msg->pose.position.x * 1000);
00185   state.y = static_cast<int> (msg->pose.position.y * 1000);
00186   state.z = static_cast<int> (msg->pose.position.z * 1000);
00187   state.heading = static_cast<int> (helper::yaw2asctec(yaw));
00188   state.qualX = qual;
00189   state.qualY = qual;
00190   state.qualZ = qual;
00191 
00192   state.vX = static_cast<short> (msg->velocity.x * 1000);
00193   state.vY = static_cast<short> (msg->velocity.y * 1000);
00194   state.vZ = static_cast<short> (msg->velocity.z * 1000);
00195   state.qualVx = qual;
00196   state.qualVy = qual;
00197   state.qualVz = qual;
00198 
00199   state.bitfield = EXT_POSITION_BYPASS_FILTER;
00200 
00201   comm_->sendPacket(HLI_PACKET_ID_POSITION_UPDATE, state);
00202 }
00203 
00204 void SSDKInterface::cbOdometry(const nav_msgs::OdometryConstPtr& msg)
00205 {
00206   // TODO: untested, use that with care !!!
00207 
00208   // receive state (position, yaw, velocity) and directly pass it to the HL position controller by bypassing the
00209   // state observer on the HL
00210 
00211   if(msg->child_frame_id != msg->header.frame_id){
00212     ROS_WARN_STREAM_THROTTLE(1, "header.frame_id and child_frame_id don't match, not sending to HLP. got "<<msg->child_frame_id << " / "<<msg->header.frame_id);
00213     return;
00214   }
00215 
00216   HLI_EXT_POSITION state;
00217 
00218   double yaw = tf::getYaw(msg->pose.pose.orientation);
00219 
00220   char qual = 100;
00221 
00222   state.x = static_cast<int> (msg->pose.pose.position.x * 1000);
00223   state.y = static_cast<int> (msg->pose.pose.position.y * 1000);
00224   state.z = static_cast<int> (msg->pose.pose.position.z * 1000);
00225   state.heading = static_cast<int> (helper::yaw2asctec(yaw));
00226   state.qualX = qual;
00227   state.qualY = qual;
00228   state.qualZ = qual;
00229 
00230   // TODO: rotate or not?
00231   geometry_msgs::Vector3 v = msg->twist.twist.linear;
00232 
00233   state.vX = static_cast<short> (v.x * 1000);
00234   state.vY = static_cast<short> (v.y * 1000);
00235   state.vZ = static_cast<short> (v.z * 1000);
00236   state.qualVx = qual;
00237   state.qualVy = qual;
00238   state.qualVz = qual;
00239 
00240   state.bitfield = EXT_POSITION_BYPASS_FILTER;
00241 
00242   comm_->sendPacket(HLI_PACKET_ID_POSITION_UPDATE, state);
00243 }
00244 
00245 void SSDKInterface::processStatusData(uint8_t * buf, uint32_t length)
00246 {
00247   HLI_SSDK_STATUS *status = (HLI_SSDK_STATUS*)buf;
00248   if(status->have_parameters == 0 && have_config_)
00249     sendParameters(config_);
00250 }
00251 
00252 
00253 void SSDKInterface::processDebugData(uint8_t * buf, uint32_t length)
00254 {
00255   static unsigned int seq = 0;
00256   HLI_SSDK_DEBUG *data = (HLI_SSDK_DEBUG*)buf;
00257   int size = sizeof(HLI_SSDK_DEBUG) / sizeof(short);
00258 
00259   if (pose_pub_.getNumSubscribers() > 0)
00260   {
00261     // TODO: currently limited to +- 65m, move this to somewhere else
00262     // this is still NED, so convert to ENU
00263     geometry_msgs::PoseStampedPtr msg(new geometry_msgs::PoseStamped);
00264 
00265     msg->header.stamp = ros::Time(data->timestamp * 1.0e-6);
00266     msg->header.frame_id = frame_id_;
00267     msg->header.seq = seq;
00268 
00269     msg->pose.position.x = helper::debug2Double(data->debug14);
00270     msg->pose.position.y = -helper::debug2Double(data->debug15);
00271     msg->pose.position.z = -helper::debug2Double(data->debug16);
00272 
00273     geometry_msgs::Quaternion & q = msg->pose.orientation;
00274     helper::angle2quaternion(helper::debug2Double(data->debug29), -helper::debug2Double(data->debug30),
00275                              -helper::debug2Double(data->debug31), &q.w, &q.x, &q.y, &q.z);
00276     pose_pub_.publish(msg);
00277   }
00278 
00279   if (debug_pub_.getNumSubscribers() > 0)
00280   {
00281     asctec_hl_comm::DoubleArrayStampedPtr msg(new asctec_hl_comm::DoubleArrayStamped);
00282 
00283     msg->data.resize(size);
00284 
00285     msg->header.stamp = ros::Time(data->timestamp * 1.0e-6);
00286     msg->header.frame_id = frame_id_;
00287     msg->header.seq = seq;
00288 
00289     msg->data[0] = helper::debug2Double(data->debug01);
00290     msg->data[1] = helper::debug2Double(data->debug02);
00291     msg->data[2] = helper::debug2Double(data->debug03);
00292     msg->data[3] = helper::debug2Double(data->debug04);
00293     msg->data[4] = helper::debug2Double(data->debug05);
00294     msg->data[5] = helper::debug2Double(data->debug06);
00295     msg->data[6] = helper::debug2Double(data->debug07);
00296     msg->data[7] = helper::debug2Double(data->debug08);
00297     msg->data[8] = helper::debug2Double(data->debug09);
00298     msg->data[9] = helper::debug2Double(data->debug10);
00299     msg->data[10] = helper::debug2Double(data->debug11);
00300     msg->data[11] = helper::debug2Double(data->debug12);
00301     msg->data[12] = helper::debug2Double(data->debug13);
00302     msg->data[13] = helper::debug2Double(data->debug14);
00303     msg->data[14] = helper::debug2Double(data->debug15);
00304     msg->data[15] = helper::debug2Double(data->debug16);
00305     msg->data[16] = helper::debug2Double(data->debug17);
00306     msg->data[17] = helper::debug2Double(data->debug18);
00307     msg->data[18] = helper::debug2Double(data->debug19);
00308     msg->data[19] = helper::debug2Double(data->debug20);
00309     msg->data[20] = helper::debug2Double(data->debug21);
00310     msg->data[21] = helper::debug2Double(data->debug22);
00311     msg->data[22] = helper::debug2Double(data->debug23);
00312     msg->data[23] = helper::debug2Double(data->debug24);
00313     msg->data[24] = helper::debug2Double(data->debug25);
00314     msg->data[25] = helper::debug2Double(data->debug26);
00315     msg->data[26] = helper::debug2Double(data->debug27);
00316     msg->data[27] = helper::debug2Double(data->debug28);
00317     msg->data[28] = helper::debug2Double(data->debug29);
00318     msg->data[29] = helper::debug2Double(data->debug30);
00319     msg->data[30] = helper::debug2Double(data->debug31);
00320     msg->data[31] = helper::debug2Double(data->debug32);
00321     msg->data[32] = helper::debug2Double(data->debug33);
00322     msg->data[33] = helper::debug2Double(data->debug34);
00323     msg->data[34] = helper::debug2Double(data->debug35);
00324     msg->data[35] = helper::debug2Double(data->debug36);
00325     msg->data[36] = helper::debug2Double(data->debug37);
00326     msg->data[37] = helper::debug2Double(data->debug38);
00327     msg->data[38] = helper::debug2Double(data->debug39);
00328 
00329     debug_pub_.publish(msg);
00330   }
00331   seq++;
00332 }
00333 
00334 bool SSDKInterface::sendParameters(const asctec_hl_interface::SSDKConfig & config)
00335 {
00336   HLI_SSDK_PARAMS params;
00337   params.p01 = helper::param2Fixpoint(config.p1);
00338   params.p02 = helper::param2Fixpoint(config.p2);
00339   params.p03 = helper::param2Fixpoint(config.p3);
00340   params.p04 = helper::param2Fixpoint(config.p4);
00341   params.p05 = helper::param2Fixpoint(config.p5);
00342   params.p06 = helper::param2Fixpoint(config.p6);
00343   params.p07 = helper::param2Fixpoint(config.p7);
00344   params.p08 = helper::param2Fixpoint(config.p8);
00345   params.p09 = helper::param2Fixpoint(config.p9);
00346   params.p10 = helper::param2Fixpoint(config.p10);
00347   params.p11 = helper::param2Fixpoint(config.p11);
00348   params.p12 = helper::param2Fixpoint(config.p12);
00349   params.p13 = helper::param2Fixpoint(config.p13);
00350   params.p14 = helper::param2Fixpoint(config.p14);
00351   params.p15 = helper::param2Fixpoint(config.p15);
00352   params.p16 = helper::param2Fixpoint(config.p16);
00353   params.p17 = helper::param2Fixpoint(config.p17);
00354   params.p18 = helper::param2Fixpoint(config.p18);
00355   params.p19 = helper::param2Fixpoint(config.p19);
00356   params.p20 = helper::param2Fixpoint(config.p20);
00357   params.p21 = helper::param2Fixpoint(config.p21);
00358   params.p22 = helper::param2Fixpoint(config.p22);
00359   params.p23 = helper::param2Fixpoint(config.p23);
00360   params.p24 = helper::param2Fixpoint(config.p24);
00361   params.p25 = helper::param2Fixpoint(config.p25);
00362   params.p26 = helper::param2Fixpoint(config.p26);
00363   params.p27 = helper::param2Fixpoint(config.p27);
00364   params.p28 = helper::param2Fixpoint(config.p28);
00365   params.p29 = helper::param2Fixpoint(config.p29);
00366   params.p30 = helper::param2Fixpoint(config.p30);
00367   params.p31 = helper::param2Fixpoint(config.p31);
00368   params.p32 = helper::param2Fixpoint(config.p32);
00369   params.p33 = helper::param2Fixpoint(config.p33);
00370   params.p34 = helper::param2Fixpoint(config.p34);
00371   params.p35 = helper::param2Fixpoint(config.p35);
00372   params.p36 = helper::param2Fixpoint(config.p36);
00373   params.p37 = helper::param2Fixpoint(config.p37);
00374   params.p38 = helper::param2Fixpoint(config.p38);
00375   params.p39 = helper::param2Fixpoint(config.p39);
00376   params.p40 = helper::param2Fixpoint(config.p40);
00377   params.p41 = helper::param2Fixpoint(config.p41);
00378   params.p42 = helper::param2Fixpoint(config.p42);
00379   params.p43 = helper::param2Fixpoint(config.p43);
00380   params.p44 = helper::param2Fixpoint(config.p44);
00381   params.p45 = helper::param2Fixpoint(config.p45);
00382   params.p46 = helper::param2Fixpoint(config.p46);
00383   params.p47 = helper::param2Fixpoint(config.p47);
00384   params.p48 = helper::param2Fixpoint(config.p48);
00385   params.p49 = helper::param2Fixpoint(config.p49);
00386   params.p50 = helper::param2Fixpoint(config.p50);
00387 
00388   for(int i=0; i<5; i++){
00389     if(comm_->sendPacketAck(HLI_PACKET_ID_SSDK_PARAMETERS, params)){
00390       ROS_INFO("sent SSDK parameters");
00391       return true;
00392     }
00393   }
00394 
00395   ROS_ERROR("sending SSDK parameters failed, tried 5 times");
00396   return false;
00397 }


asctec_hl_interface
Author(s): Markus Achtelik, Michael Achtelik, Stephan Weiss, Laurent Kneip
autogenerated on Thu Jun 6 2019 20:57:17