8000 Add Input gnss speed thereshold by rsasaki0109 · Pull Request #231 · MapIV/eagleye · GitHub
[go: up one dir, main page]
More Web Proxy on the site http://driver.im/
Skip to content

Add Input gnss speed thereshold #231

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 2 commits into from
Mar 14, 2023
Merged

Add Input gnss speed thereshold #231

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
e91dcd0
Add Input gnss speed thereshold
rsasaki0109 Mar 14, 2023
c4cee9e
Update twist_covariance_thresh description
rsasaki0109 Mar 14, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions eagleye_util/gnss_converter/launch/gnss_converter.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,10 @@
<push-ros-namespace namespace="gnss"/>
<node pkg="eagleye_gnss_converter" name="gnss_converter_node" exec="gnss_converter" output="screen">
<param from="$(find-pkg-share eagleye_rt)/config/$(var config_yaml)"/>
<!-- Twist Covariance Threshold in geometry_msgs/TwistWithCovarianceStamped -->
<param name="twist_covariance_thresh" value="0.2"/>
<!-- Vertical Accuracy Estimate Threshold [mm/s] in ublox_msgs/NavPVT -->
<param name="ublox_vacc_thresh" value="200.0"/>
</node>
</group>

Expand Down
13 changes: 12 additions & 1 deletion eagleye_util/gnss_converter/src/gnss_converter_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,9 @@ sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_msg_ptr;
static std::string sub_topic_name, pub_fix_topic_name = "fix",
pub_gga_topic_name = "gga", pub_rmc_topic_name = "rmc" ,pub_rtklib_nav_topic = "rtklib_nav";

double twist_covariance_thresh = 0.2;
double ublox_vacc_thresh = 200.0;

void nmea_callback(const nmea_msgs::msg::Sentence::ConstSharedPtr msg)
{
nmea_msgs::msg::Gpgga gga;
Expand Down Expand Up @@ -53,6 +56,7 @@ void navsatfix_callback(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg) {

void navpvt_callback(const ublox_msgs::msg::NavPVT::ConstSharedPtr msg)
{
if(msg->s_acc > ublox_vacc_thresh) return;
rtklib_msgs::msg::RtklibNav r;
r.header.frame_id = "gps";
r.header.stamp.sec = msg->sec;
Expand Down Expand Up @@ -84,6 +88,7 @@ void navpvt_callback(const ublox_msgs::msg::NavPVT::ConstSharedPtr msg)

void gnss_velocity_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr msg)
{
if(msg->twist.covariance[0] > twist_covariance_thresh) return;
if (nav_msg_ptr == nullptr) return;
rtklib_msgs::msg::RtklibNav r;
r.header.frame_id = "gps";
Expand Down Expand Up @@ -131,16 +136,22 @@ int main(int argc, char** argv)
node->declare_parameter("gnss.velocity_source_topic",velocity_source_topic);
node->declare_parameter("gnss.llh_source_type",llh_source_type);
node->declare_parameter("gnss.llh_source_topic",llh_source_topic);
node->declare_parameter("twist_covariance_thresh",twist_covariance_thresh);
node->declare_parameter("ublox_vacc_thresh",ublox_vacc_thresh);
node->get_parameter("gnss.velocity_source_type",velocity_so 6DCB urce_type);
node->get_parameter("gnss.velocity_source_topic",velocity_source_topic);
node->get_parameter("gnss.llh_source_type",llh_source_type);
node->get_parameter("gnss.llh_source_topic",llh_source_topic);
node->get_parameter("twist_covariance_thresh",twist_covariance_thresh);
node->get_parameter("ublox_vacc_thresh",ublox_vacc_thresh);

std::cout<< "velocity_source_type "<<velocity_source_type<<std::endl;
std::cout<< "velocity_source_topic "<<velocity_source_topic<<std::endl;
std::cout<< "llh_source_type "<<llh_source_type<<std::endl;
std::cout<< "llh_source_topic "<<llh_source_topic<<std::endl;

std::cout<< "twist_covariance_thresh "<<twist_covariance_thresh<<std::endl;
std::cout<< "ublox_vacc_thresh "<<ublox_vacc_thresh<<std::endl;

if(velocity_source_type == 0)
{
rtklib_nav_sub = node->create_subscription<rtklib_msgs::msg::RtklibNav>(velocity_source_topic, 1000, rtklib_nav_callback);
Expand Down
0