Skip to content

Commit

Permalink
Connection check
Browse files Browse the repository at this point in the history
  • Loading branch information
souryavarenya authored Jan 24, 2024
1 parent 5cb29a1 commit 260bc23
Show file tree
Hide file tree
Showing 2 changed files with 19 additions and 10 deletions.
12 changes: 11 additions & 1 deletion fixposition_driver_lib/src/fixposition_driver.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -287,7 +287,11 @@ void FixpositionDriver::NovConvertAndPublish(const uint8_t* msg, int size) {
}

bool FixpositionDriver::CreateTCPSocket() {
struct sockaddr_in server_address;
if (client_fd_ != -1) {
std::cerr << "TCP connection already exists" << "\n";
return true;
}

client_fd_ = socket(AF_INET, SOCK_STREAM, 0);

if (client_fd_ < 0) {
Expand All @@ -297,6 +301,7 @@ bool FixpositionDriver::CreateTCPSocket() {
std::cout << "Client created.\n";
}

struct sockaddr_in server_address;
server_address.sin_family = AF_INET;
server_address.sin_addr.s_addr = INADDR_ANY;
server_address.sin_port = htons(std::stoi(params_.fp_output.port));
Expand All @@ -312,6 +317,11 @@ bool FixpositionDriver::CreateTCPSocket() {
}

bool FixpositionDriver::CreateSerialConnection() {
if (client_fd_ != -1) {
std::cerr << "Serial connection already exists" << "\n";
return true;
}

client_fd_ = open(params_.fp_output.port.c_str(), O_RDWR | O_NOCTTY);

struct termios options;
Expand Down
17 changes: 8 additions & 9 deletions fixposition_driver_ros1/src/fixposition_driver_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -52,14 +52,13 @@ FixpositionDriverNode::FixpositionDriverNode(const FixpositionDriverParams& para
odometry_enu0_pub_(nh_.advertise<nav_msgs::Odometry>("/fixposition/odometry_enu", 100)),
eul_pub_(nh_.advertise<geometry_msgs::Vector3Stamped>("/fixposition/ypr", 100)),
eul_imu_pub_(nh_.advertise<geometry_msgs::Vector3Stamped>("/fixposition/imu_ypr", 100)) {

ws_sub_ = nh_.subscribe<fixposition_driver_ros1::Speed>(params_.customer_input.speed_topic, 100,
&FixpositionDriverNode::WsCallback, this,
ros::TransportHints().tcpNoDelay());



Connect();

RegisterObservers();
}

Expand Down Expand Up @@ -195,23 +194,23 @@ void FixpositionDriverNode::PublishNmea(NmeaMessage data) {
if (data.checkEpoch()) {
// Generate new message
fixposition_driver_ros1::NMEA msg;

// ROS Header
msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.gpzda.stamp));
msg.header.frame_id = "LLH";

// Latitude [degrees]. Positive is north of equator; negative is south
msg.latitude = data.gpgga.latitude;

// Longitude [degrees]. Positive is east of prime meridian; negative is west
msg.longitude = data.gpgga.longitude;

// Altitude [m]. Positive is above the WGS 84 ellipsoid
msg.altitude = data.gpgga.altitude;

// Speed over ground [m/s]
msg.speed = data.gprmc.speed;

// Course over ground [deg]
msg.course = data.gprmc.course;

Expand All @@ -224,7 +223,7 @@ void FixpositionDriverNode::PublishNmea(NmeaMessage data) {

// Positioning system mode indicator, R (RTK fixed), F (RTK float), A (no RTK), E, N
msg.mode = data.gprmc.mode;

// Publish message
nmea_pub_.publish(msg);
}
Expand Down

0 comments on commit 260bc23

Please sign in to comment.