Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
ROS2 Piksi Multi Driver
#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;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment