Created
July 21, 2020 17:49
-
-
Save pedro1713/68bcd6bb693d48b646f70e6ddf77595f to your computer and use it in GitHub Desktop.
ROS2 Piksi Multi Driver
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#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