External position update

crazyflie_cpp/include/crazyflie_cpp/Crazyflie.h

@@ -75,6 +75,11 @@ class Crazyflie
     float yawrate,
     uint16_t thrust);
 
+  void sendExternalPositionUpdate(
+    float x,
+    float y,
+    float z);
+
   void sendPing();
 
   void reboot();

crazyflie_cpp/include/crazyflie_cpp/crtp.h

@@ -362,6 +362,27 @@ struct crtpLogDataResponse
 } __attribute__((packed));
 
 
+// Port 0x06 (External Position Update)
+
+struct crtpExternalPositionUpdate
+{
+  crtpExternalPositionUpdate(
+    float x,
+    float y,
+    float z)
+    : header(0x06, 0)
+    , x(x)
+    , y(y)
+    , z(z)
+  {
+  }
+  const crtp header;
+  float x;
+  float y;
+  float z;
+}  __attribute__((packed));
+
+
 
 // Port 13 (Platform)
 

crazyflie_cpp/src/Crazyflie.cpp

@@ -108,6 +108,15 @@ void Crazyflie::sendSetpoint(
   sendPacket((const uint8_t*)&request, sizeof(request));
 }
 
+void Crazyflie::sendExternalPositionUpdate(
+  float x,
+  float y,
+  float z)
+{
+  crtpExternalPositionUpdate position(x, y, z);
+  sendPacket((const uint8_t*)&position, sizeof(position));
+}
+
 void Crazyflie::sendPing()
 {
   uint8_t ping = 0xFF;

crazyflie_driver/src/crazyflie_server.cpp

@@ -6,6 +6,7 @@
 #include "crazyflie_driver/UpdateParams.h"
 #include "std_srvs/Empty.h"
 #include "geometry_msgs/Twist.h"
+#include "geometry_msgs/PointStamped.h"
 #include "sensor_msgs/Imu.h"
 #include "sensor_msgs/Temperature.h"
 #include "sensor_msgs/MagneticField.h"
@@ -20,7 +21,7 @@
 
 #include <crazyflie_cpp/Crazyflie.h>
 
-constexpr double pi() { return std::atan(1)*4; }
+constexpr double pi() { return 3.141592653589793238462643383279502884; }
 
 double degToRad(double deg) {
     return deg / 180.0 * pi();
@@ -64,16 +65,19 @@ class CrazyflieROS
     , m_serviceEmergency()
     , m_serviceUpdateParams()
     , m_subscribeCmdVel()
+    , m_subscribeExternalPosition()
     , m_pubImu()
     , m_pubTemp()
     , m_pubMag()
     , m_pubPressure()
     , m_pubBattery()
     , m_pubRssi()
     , m_sentSetpoint(false)
+    , m_sentExternalPosition(false)
   {
     ros::NodeHandle n;
     m_subscribeCmdVel = n.subscribe(tf_prefix + "/cmd_vel", 1, &CrazyflieROS::cmdVelChanged, this);
+    m_subscribeExternalPosition = n.subscribe(tf_prefix + "/external_position", 1, &CrazyflieROS::positionMeasurementChanged, this);
     m_serviceEmergency = n.advertiseService(tf_prefix + "/emergency", &CrazyflieROS::emergency, this);
 
     if (m_enable_logging_imu) {
@@ -204,6 +208,13 @@ class CrazyflieROS
     }
   }
 
+  void positionMeasurementChanged(
+    const geometry_msgs::PointStamped::ConstPtr& msg)
+  {
+    m_cf.sendExternalPositionUpdate(msg->point.x, msg->point.y, msg->point.z);
+    m_sentExternalPosition = true;
+  }
+
   void run()
   {
     // m_cf.reboot();
@@ -331,10 +342,11 @@ class CrazyflieROS
 
     while(!m_isEmergency) {
       // make sure we ping often enough to stream data out
-      if (m_enableLogging && !m_sentSetpoint) {
+      if (m_enableLogging && !m_sentSetpoint && !m_sentExternalPosition) {
         m_cf.sendPing();
       }
       m_sentSetpoint = false;
+      m_sentExternalPosition = false;
       std::this_thread::sleep_for(std::chrono::milliseconds(1));
     }
 
@@ -464,6 +476,7 @@ class CrazyflieROS
   ros::ServiceServer m_serviceEmergency;
   ros::ServiceServer m_serviceUpdateParams;
   ros::Subscriber m_subscribeCmdVel;
+  ros::Subscriber m_subscribeExternalPosition;
   ros::Publisher m_pubImu;
   ros::Publisher m_pubTemp;
   ros::Publisher m_pubMag;
@@ -472,7 +485,7 @@ class CrazyflieROS
   ros::Publisher m_pubRssi;
   std::vector<ros::Publisher> m_pubLogDataGeneric;
 
-  bool m_sentSetpoint;
+  bool m_sentSetpoint, m_sentExternalPosition;
 
   std::thread m_thread;
 };