Crayzyflie_driver

CrazyflieServer

class CrazyflieServer
{
public:
  CrazyflieServer()
  {

  }

  void run()
  {
    ros::NodeHandle n;
    ros::CallbackQueue callback_queue;
    n.setCallbackQueue(&callback_queue);

    ros::ServiceServer serviceAdd = n.advertiseService("add_crazyflie", &CrazyflieServer::add_crazyflie, this);
    ros::ServiceServer serviceRemove = n.advertiseService("remove_crazyflie", &CrazyflieServer::remove_crazyflie, this);

    while(ros::ok()) {
      // Execute any ROS related functions now
      callback_queue.callAvailable(ros::WallDuration(0.0));
      std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }
  }

private:

  bool add_crazyflie(
    crazyflie_driver::AddCrazyflie::Request  &req,
    crazyflie_driver::AddCrazyflie::Response &res)
  {
    ROS_INFO("Adding %s as %s with trim(%f, %f). Logging: %d, Parameters: %d, Use ROS time: %d",
      req.uri.c_str(),
      req.tf_prefix.c_str(),
      req.roll_trim,
      req.pitch_trim,
      req.enable_parameters,
      req.enable_logging,
      req.use_ros_time);

    // Ignore if the uri is already in use
    if (m_crazyflies.find(req.uri) != m_crazyflies.end()) {
      ROS_ERROR("Cannot add %s, already added.", req.uri.c_str());
      return false;
    }

    CrazyflieROS* cf = new CrazyflieROS(
      req.uri,  //地址
      req.tf_prefix,   //前缀
      req.roll_trim,   //滚转修正
      req.pitch_trim,	//仰俯修正
      req.enable_logging,  //是否允许机载传感器
      req.enable_parameters, //是否允许从ROS参数服务器获取参数
      req.log_blocks,  // 从参数服务器获取并定制需要发布的topic 
                       // std::vector<crazyflie_driver::LogBlock>& log_blocks 
                        //string topic_name
                        //int16 frequency
                        //string[] variables
      req.use_ros_time,  //使用ros时间
      req.enable_logging_imu,  //从IMU获取姿态
      req.enable_logging_temperature,  //获取温度
      req.enable_logging_magnetic_field, //磁力计
      req.enable_logging_pressure,  //气压计
      req.enable_logging_battery, //电池容量
      req.enable_logging_pose,   //从传感器获取位置信息
      req.enable_logging_packets);   //发送RSSI Ack消息，属于通信部分

    m_crazyflies[req.uri] = cf;

    return true;
  }

  bool remove_crazyflie(
    crazyflie_driver::RemoveCrazyflie::Request  &req,
    crazyflie_driver::RemoveCrazyflie::Response &res)
  {

    if (m_crazyflies.find(req.uri) == m_crazyflies.end()) {
      ROS_ERROR("Cannot remove %s, not connected.", req.uri.c_str());
      return false;
    }

    ROS_INFO("Removing crazyflie at uri %s.", req.uri.c_str());

    m_crazyflies[req.uri]->stop();
    delete m_crazyflies[req.uri];
    m_crazyflies.erase(req.uri);

    ROS_INFO("Crazyflie %s removed.", req.uri.c_str());

    return true;
  }

private:
  std::map<std::string, CrazyflieROS*> m_crazyflies; //存储每个crazyflie信息的map容器
};

CrazyflieRos

class CrazyflieROS
{
private:
  std::string m_tf_prefix; //无人机前缀
  Crazyflie m_cf;  //无人机类， Crazyflie.cpp
  bool m_isEmergency; //紧急停机
  float m_roll_trim; 
  float m_pitch_trim;
  bool m_enableLogging;
  bool m_enableParameters;
  std::vector<crazyflie_driver::LogBlock> m_logBlocks;
  bool m_use_ros_time;
  bool m_enable_logging_imu;
  bool m_enable_logging_temperature;
  bool m_enable_logging_magnetic_field;
  bool m_enable_logging_pressure;
  bool m_enable_logging_battery;
  bool m_enable_logging_pose;
  bool m_enable_logging_packets;

  ros::ServiceServer m_serviceEmergency;
  ros::ServiceServer m_serviceUpdateParams;
  ros::ServiceServer m_sendPacketServer;

  // High-level setpoints
  ros::ServiceServer m_serviceSetGroupMask;
  ros::ServiceServer m_serviceTakeoff;
  ros::ServiceServer m_serviceLand;
  ros::ServiceServer m_serviceStop;
  ros::ServiceServer m_serviceGoTo;
  ros::ServiceServer m_serviceUploadTrajectory; 
  ros::ServiceServer m_serviceStartTrajectory;
  ros::ServiceServer m_serviceNotifySetpointsStop;

  ros::Subscriber m_subscribeCmdVel;
  ros::Subscriber m_subscribeCmdFullState;
  ros::Subscriber m_subscribeCmdHover;
  ros::Subscriber m_subscribeCmdStop;
  ros::Subscriber m_subscribeCmdPosition;
  ros::Subscriber m_subscribeExternalPosition;
  ros::Subscriber m_subscribeExternalPose;
  ros::Subscriber m_subscribeCmdVelocityWorld;
  ros::Publisher m_pubImu;
  ros::Publisher m_pubTemp;
  ros::Publisher m_pubMag;
  ros::Publisher m_pubPressure;
  ros::Publisher m_pubBattery;
  ros::Publisher m_pubPose;
  ros::Publisher m_pubPackets;
  ros::Publisher m_pubRssi;
  std::vector<ros::Publisher> m_pubLogDataGeneric;

  bool m_sentSetpoint, m_sentExternalPosition;

  std::thread m_thread;
  ros::CallbackQueue m_callback_queue;
}

Crazyflie

class Crazyflie
{
public:
  Crazyflie(
    const std::string& link_uri,
    Logger& logger = EmptyLogger,
    std::function<void(const char*)> consoleCb = nullptr);

  int getProtocolVersion();

  std::string ge  tFirmwareVersion();

  std::string getDeviceTypeName();

  void logReset();

  void sendSetpoint(
    float roll,
    float pitch,
    float yawrate,
    uint16_t thrust); //局部速度变化控制

  void cmdVelChanged(
    const geometry_msgs::Twist::ConstPtr& msg)
  {
    if (!m_isEmergency) {
      float roll = msg->linear.y + m_roll_trim;
      float pitch = - (msg->linear.x + m_pitch_trim);
      float yawrate = msg->angular.z;
      uint16_t thrust = std::min<uint16_t>(std::max<float>(msg->linear.z, 0.0), 60000);

      m_cf.sendSetpoint(roll, pitch, yawrate, thrust);
      m_sentSetpoint = true;
    }
  }//局部速度变化控制
    
  void sendFullStateSetpoint(
    float x, float y, float z,
    float vx, float vy, float vz,
    float ax, float ay, float az,
    float qx, float qy, float qz, float qw,
    float rollRate, float pitchRate, float yawRate); //全状态控制

  void sendVelocityWorldSetpoint(
    float x, float y, float z, float yawRate); //全局速度控制

  void sendHoverSetpoint(
    float vx,
    float vy,
    float yawrate,
    float zDistance); //hover控制

  void sendPositionSetpoint(
    float x,
    float y,
    float z,
    float yaw); //位置控制

  void notifySetpointsStop(uint32_t remainValidMillisecs);

  void sendStop();

  void emergencyStop();

  void emergencyStopWatchdog();

  void sendExternalPositionUpdate(
    float x,
    float y,
    float z);

  void sendExternalPoseUpdate(
    float x, float y, float z,
    float qx, float qy, float qz, float qw);
    
  void setGenericPacketCallback(
    std::function<void(const ITransport::Ack&)> cb) {
    m_genericPacketCallback = cb;
  }

  // High-Level setpoints
  void setGroupMask(uint8_t groupMask);

  void takeoff(float height, float duration, uint8_t groupMask = 0);

  void land(float height, float duration, uint8_t groupMask = 0);

  void stop(uint8_t groupMask = 0);

  void goTo(float x, float y, float z, float yaw, float duration, bool relative = false, uint8_t groupMask = 0);

  void uploadTrajectory(
    uint8_t trajectoryId,
    uint32_t pieceOffset,
    const std::vector<poly4d>& pieces);

  void startTrajectory(
    uint8_t trajectoryId,
    float timescale = 1.0,
    bool reversed = false,
    bool relative = true,
    uint8_t groupMask = 0);

  // Memory subsystem
  void readUSDLogFile(
    std::vector<uint8_t>& data);
    
private:
  Crazyradio* m_radio;
  ITransport* m_transport;
  int m_devId;

  uint8_t m_channel;
  uint64_t m_address;
  Crazyradio::Datarate m_datarate;

  std::vector<LogTocEntry> m_logTocEntries;
  std::map<uint8_t, std::function<void(crtpLogDataResponse*, uint8_t)> > m_logBlockCb;

  std::vector<ParamTocEntry> m_paramTocEntries;
  std::map<uint16_t, ParamValue> m_paramValues;

  std::vector<MemoryTocEntry> m_memoryTocEntries;

  std::function<void(const crtpPlatformRSSIAck*)> m_emptyAckCallback;
  std::function<void(float)> m_linkQualityCallback;
  std::function<void(const char*)> m_consoleCallback;
  std::function<void(const ITransport::Ack&)> m_genericPacketCallback;

  Logger& m_logger;
};

Crazyrflie_add

客户端节点，参数由launch文件传入

#include "ros/ros.h"
#include "crazyflie_driver/AddCrazyflie.h"
#include "crazyflie_driver/LogBlock.h"

int main(int argc, char **argv)
{
  ros::init(argc, argv, "crazyflie_add", ros::init_options::AnonymousName);
  ros::NodeHandle n("~");
  // read paramaters
  n.getParam("uri", uri);
  n.getParam("tf_prefix", tf_prefix);
  n.param("roll_trim", roll_trim, 0.0);
  n.param("pitch_trim", pitch_trim, 0.0);
  n.param("enable_logging", enable_logging, true);
  n.param("enable_parameters", enable_parameters, true);
  n.param("use_ros_time", use_ros_time, true);
  n.param("enable_logging_imu", enable_logging_imu, true);
  n.param("enable_logging_temperature", enable_logging_temperature, true);
  n.param("enable_logging_magnetic_field", enable_logging_magnetic_field, true);
  n.param("enable_logging_pressure", enable_logging_pressure, true);
  n.param("enable_logging_battery", enable_logging_battery, true);
  n.param("enable_logging_pose", enable_logging_pose, false);
  n.param("enable_logging_packets", enable_logging_packets, true);

  ros::ServiceClient addCrazyflieService = n.serviceClient<crazyflie_driver::AddCrazyflie>("/add_crazyflie");
  addCrazyflieService.waitForExistence();
  crazyflie_driver::AddCrazyflie addCrazyflie;
  addCrazyflie.request.uri = uri; // 将其余参数均传入req
    
  std::vector<std::string> genericLogTopics;
  n.param("genericLogTopics", genericLogTopics, std::vector<std::string>());
  std::vector<int> genericLogTopicFrequencies;
  n.param("genericLogTopicFrequencies", genericLogTopicFrequencies, std::vector<int>()); //定制发布topic， 默认为空
  addCrazyflieService.call(addCrazyflie); // 请求服务
    
  return 0;
}