ROS2 Piksi Multi Driver

ns_piksi.cpp

#include <ns_piksi_gps/ns_piksi_gps.hpp>

PiksiGps::PiksiGps():
Node("piksi"),
socket_desc_(-1)
{
  // Parameters declaration
  this->declare_parameter<std::string>("addr", "192.168.50.222");
  this->declare_parameter<std::string>("port", "55555");

  // Get parameters
  this->get_parameter("addr", tcp_port_addr_);
  this->get_parameter("port", tcp_port_);

  // Publishers setups
  heartbeat_pub_ = this->create_publisher<ns_msgs::msg::Heartbeat>("piksi/heartbeat", 5);
  fix_pub_ = this->create_publisher<sensor_msgs::msg::NavSatFix>("piksi/navsatfix_best_fix",5);
  vel_pub_ = this->create_publisher<geometry_msgs::msg::Vector3Stamped>("piksi/velned", 5);

  // Setup piksi connection
  setup_socket();
  sbp_state_init(&sbp_state_);
  sbp_state_set_io_context(&sbp_state_, this);
  using namespace std::placeholders;
//   auto fp = std::bind(&PiksiGps::heartbeat_callback, this, _1, _2, _3, _4);
  sbp_register_callback(&sbp_state_, SBP_MSG_HEARTBEAT, &PiksiGps::heartbeat_callback, this, &heartbeat_callback_node_);
  sbp_register_callback(&sbp_state_, SBP_MSG_POS_LLH, &PiksiGps::sbp_pos_llh_callback, this, &pos_llh_callback_node_);
  sbp_register_callback(&sbp_state_, SBP_MSG_VEL_NED, &PiksiGps::vel_ned_callback, this, &velned_callback_node_);

  run();
}

void PiksiGps::run(){
    RCLCPP_INFO(this->get_logger(), "Running Piksi driver");
    while(rclcpp::ok()){
      sbp_process(&sbp_state_, &PiksiGps::read_redirect);
    }
}

PiksiGps::~PiksiGps() {
    close_socket();
}

void PiksiGps::setup_socket() {
    struct sockaddr_in server;
    socket_desc_ = socket(AF_INET, SOCK_STREAM, 0);
    if(socket_desc_ == -1)
    {
        std::cerr << "Could not create socket" << std::endl;
    }
    memset(&server, '0', sizeof(server));
    server.sin_addr.s_addr = inet_addr(tcp_port_addr_.c_str());
    server.sin_family = AF_INET;
    server.sin_port = htons(std::stoi(tcp_port_));

    if(connect(socket_desc_, (struct sockaddr *)&server, sizeof(server)) < 0) {
        std::cerr << "Connection error" << std::endl;
    }
}

s32 PiksiGps::socket_read(u8 *buff, u32 n) {
    u32 result; 
    result = read(socket_desc_, buff, n);
    return result;
}

void PiksiGps::heartbeat_process(u16 sender_id, u8 len, u8 msg[]) {
    (void)sender_id, (void)len, (void)msg;
    msg_heartbeat_t *p_heartbeat;
    p_heartbeat = (msg_heartbeat_t *)msg;
    ns_msgs::msg::Heartbeat beat;
    beat.header.stamp = rclcpp::Clock().now();
    beat.system_error = p_heartbeat->flags & 0x01;
    beat.io_error = p_heartbeat->flags & 0x02;
    beat.swift_nap_error = p_heartbeat->flags & 0x04;
    beat.sbp_minor_version = (p_heartbeat->flags & 0xFF00)>>8;
    beat.sbp_major_version = (p_heartbeat->flags & 0xFF0000) >> 16;
    beat.external_antenna_present = (p_heartbeat->flags & 0x80000000) >> 31;
    heartbeat_pub_->publish(beat);
}

void PiksiGps::sbp_pos_llh_process(u16 sender_id, u8 len, u8 msg[]) {
    (void)sender_id, (void)len;
    msg_pos_llh_t *p_pos_llh = (msg_pos_llh_t *)msg;

    sensor_msgs::msg::NavSatFix fix;
    fix.header.stamp = rclcpp::Clock().now();
    fix.header.frame_id = "base_link";
    fix.status.status = p_pos_llh->flags;
    fix.latitude = p_pos_llh->lat;
    fix.longitude = p_pos_llh->lon;
    fix.altitude = p_pos_llh->height;
    fix.position_covariance_type = 0;
    fix_pub_->publish(fix);
}

void PiksiGps::vel_ned_process(u16 sender_id, u8 len, u8 msg[]) {
    msg_vel_ned_t *p_vel_ned = (msg_vel_ned_t *)msg;
    geometry_msgs::msg::Vector3Stamped vel;
    vel.header.stamp = rclcpp::Clock().now();
    vel.vector.y = p_vel_ned->n;
    vel.vector.x = p_vel_ned->e;
    vel.vector.z = p_vel_ned->d;
    vel_pub_->publish(vel);
    // ns_msgs::msg::VelNed vel;
    // vel.header.stamp = rclcpp::Clock().now();
    // vel.tow = p_vel_ned->tow;
    // vel.n = p_vel_ned->n;
    // vel.e = p_vel_ned->e;
    // vel.d = p_vel_ned->d;
    // vel.h_accuracy = p_vel_ned->h_accuracy;
    // vel.v_accuracy = p_vel_ned->v_accuracy;
    // vel.n_sats = p_vel_ned->n_sats;
    // vel.flags = p_vel_ned->flags;
    // vel_pub_->publish(vel); 
}

void PiksiGps::heartbeat_callback(u16 sender_id, u8 len, u8 msg[], void* context) {
    if(!context) {
        std::cout << "No context set for heartbeat callback" << std::endl;
        return;
    }
    PiksiGps* instance = static_cast<PiksiGps*>(context);
    return instance->heartbeat_process(sender_id, len, msg);
}

void PiksiGps::sbp_pos_llh_callback(u16 sender_id, u8 len, u8 msg[], void* context) {
    if(!context) {
        std::cout << "No context set for llh callback" << std::endl;
        return;
    }
    PiksiGps* instance = static_cast<PiksiGps*>(context);
    return instance->sbp_pos_llh_process(sender_id, len, msg);
}

void PiksiGps::vel_ned_callback(u16 sender_id, u8 len, u8 msg[], void* context) {
    if(!context) {
        std::cout << "No context set for vel ned callback" << std::endl;
        return;
    }
    PiksiGps* instance = static_cast<PiksiGps*>(context);
    return instance->vel_ned_process(sender_id, len, msg);
}

s32 PiksiGps::read_redirect(u8 *buff, u32 n, void *context) {
    if(!context) {
        std::cout << "No context set for socket read" << std::endl;
        return 0;
    }
    PiksiGps* instance = static_cast<PiksiGps*>(context);
    return instance->socket_read(buff, n);
}

int main(int argc, char* argv[]) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<PiksiGps>());
    rclcpp::shutdown();
    return 0;
}