Skip to content

Commit

Permalink
Fix ros2 component topics
Browse files Browse the repository at this point in the history
Emit gps/navsat fix messages on timer tick.
  • Loading branch information
daveshed committed Oct 27, 2021
1 parent fc78281 commit 1ddc750
Showing 1 changed file with 11 additions and 1 deletion.
12 changes: 11 additions & 1 deletion gpsd_client/src/client.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -5,8 +5,11 @@
#include <sensor_msgs/msg/nav_sat_status.hpp>
#include <libgpsmm.h>

#include <chrono>
#include <cmath>

using namespace std::chrono_literals;

namespace gpsd_client
{
class GPSDClientComponent : public rclcpp::Node
Expand All @@ -18,7 +21,11 @@ namespace gpsd_client
use_gps_time_(true),
check_fix_by_variance_(true),
frame_id_("gps")
{}
{
timer_ = create_wall_timer(1s, std::bind(&GPSDClientComponent::step, this));
start();
RCLCPP_INFO(this->get_logger(), "Instantiated.");
}

bool start()
{
Expand Down Expand Up @@ -207,6 +214,7 @@ namespace gpsd_client

fix.status = status;

RCLCPP_DEBUG(this->get_logger(), "Publishing gps fix...");
gps_fix_pub_->publish(fix);
}

Expand Down Expand Up @@ -271,6 +279,7 @@ namespace gpsd_client

fix->position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;

RCLCPP_DEBUG(this->get_logger(), "Publishing navsatfix...");
navsatfix_pub_->publish(std::move(fix));
}

Expand All @@ -282,6 +291,7 @@ namespace gpsd_client
bool use_gps_time_;
bool check_fix_by_variance_;
std::string frame_id_;
rclcpp::TimerBase::SharedPtr timer_;
};
}

Expand Down

0 comments on commit 1ddc750

Please sign in to comment.