sample_parser.h

Go to the documentation of this file.

#pragma once

#include <parsernode/common.h>
#include <parsernode/parser.h>

class SampleParser : public parsernode::Parser {
public:
  SampleParser() {}
  void setRC(int16_t channels[4]) {
    for (int i = 0; i < 4; i++) {
      quad_data.servo_in[i] = channels[i];
    }
  }
  virtual bool takeoff() {
    quad_data.quadstate = "takeoff";
    return true;
  }
  virtual bool land() {
    quad_data.quadstate = "land";
    return true;
  }
  virtual bool disarm() {
    quad_data.quadstate = "disarm";
    return true;
  }
  virtual bool flowControl(bool) {
    quad_data.quadstate = "flowcontrol";
    return true;
  }
  virtual bool calibrateimubias() {
    quad_data.quadstate = "calibrateimu";
    return true;
  }
  virtual bool cmdrpythrust(geometry_msgs::Quaternion &rpytmsg,
                            bool sendyaw = false) {
    quad_data.rpydata.x = rpytmsg.x;
    quad_data.rpydata.y = rpytmsg.y;
    quad_data.rpydata.z = rpytmsg.z;
    return true;
  }
  virtual bool cmdvelguided(geometry_msgs::Vector3 &vel_cmd, double &yaw_ang) {
    quad_data.linvel = vel_cmd;
    quad_data.rpydata.z = yaw_ang;
    return true;
  }
  virtual bool cmdwaypoint(geometry_msgs::Vector3 &desired_pos,
                           double desired_yaw = 0) {
    quad_data.localpos = desired_pos;
    quad_data.rpydata.z = desired_yaw;
    return true;
  }
  virtual void grip(int state) {}
  virtual void reset_attitude(double roll, double pitch, double yaw) {}
  virtual void setmode(std::string mode) {}

  // PluginLib initialization function
  virtual void initialize(ros::NodeHandle &nh_) {}
  virtual void getquaddata(parsernode::common::quaddata &d1) { d1 = quad_data; }
  virtual ~SampleParser() {}
  virtual void setBatteryPercent(double percent) {
    quad_data.batterypercent = percent;
  }
  virtual void setaltitude(double altitude_) {
    quad_data.altitude = altitude_;
    quad_data.localpos.z = altitude_;
  }
  virtual void setlogdir(string logdir) {}
  virtual void controllog(bool logswitch) {}

private:
  parsernode::common::quaddata quad_data;
};