Skip to content
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

Add pose_with_covariance output #3

Merged
merged 1 commit into from
Oct 31, 2022
Merged

Add pose_with_covariance output #3

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
2 changes: 2 additions & 0 deletions gnss_compass/include/gnss_compass_core/gnss_compass_core.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@

#include <nmea_msgs/Gpgga.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_msgs/Odometry.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/convert.h>
Expand Down Expand Up @@ -36,6 +37,7 @@ class GnssCompass
ros::Subscriber subgga_sub_;

ros::Publisher pose_pub_;
ros::Publisher pose_with_covariance_pub_;
ros::Publisher odom_pub_;
ros::Publisher illigal_odom_pub_;
ros::Publisher diagnostics_pub_;
Expand Down
4 changes: 3 additions & 1 deletion gnss_compass/launch/gnss_compass.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,12 +6,14 @@

<arg name="input_main_gnss_topic" default="/main/mosaic/gga" doc="Main gnss topic"/>
<arg name="input_sub_gnss_topic" default="/sub/mosaic/gga" doc="Sub gnss topic"/>
<arg name="output_pose_topic" default="gnss_pose" doc="Estimated self position" />
<arg name="output_pose_topic" default="gnss_compass_pose" doc="Estimated self position" />
<arg name="output_pose_with_covariance_topic" default="gnss_compass_pose_with_covariance" doc="Estimated self position" />

<node pkg="gnss_compass" name="gnss_compass" type="gnss_compass">
<remap from="main_gnss_gga" to="$(arg input_main_gnss_topic)" />
<remap from="sub_gnss_gga" to="$(arg input_sub_gnss_topic)" />
<remap from="gnss_compass_pose" to="$(arg output_pose_topic)" />
<remap from="gnss_compass_pose_with_covariance" to="$(arg output_pose_with_covariance_topic)" />
<rosparam command="load" file="$(arg param_file)" />
</node>

Expand Down
18 changes: 18 additions & 0 deletions gnss_compass/src/gnss_compass_core.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@ GnssCompass::GnssCompass(ros::NodeHandle nh, ros::NodeHandle private_nh)
subgga_sub_ = nh_.subscribe("sub_gnss_gga", 100, &GnssCompass::callbackSubGga, this);

pose_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("gnss_compass_pose", 10);
pose_with_covariance_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("gnss_compass_pose_with_covariance", 10);
odom_pub_ = nh_.advertise<nav_msgs::Odometry>("gnss_compass_odom", 10);
illigal_odom_pub_ = nh_.advertise<nav_msgs::Odometry>("illigal_gnss_compass_odom", 10);
diagnostics_pub_ = nh_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 10);
Expand Down Expand Up @@ -221,6 +222,23 @@ void GnssCompass::callbackSubGga(const nmea_msgs::Gpgga::ConstPtr & subgga_msg_p
pose_pub_.publish(transformed_pose_msg_ptr);
odom_pub_.publish(odom_msg_);

geometry_msgs::PoseWithCovarianceStamped transformed_pose_with_covariance_msg;
transformed_pose_with_covariance_msg.header = transformed_pose_msg_ptr->header;
transformed_pose_with_covariance_msg.pose.pose = transformed_pose_msg_ptr->pose;

double std_dev_xyz = 0.03; // [m]
double std_dev_rp = 100; // [rad]
double std_dev_yaw = 0.2 / 180 * M_PI; // [rad]
transformed_pose_with_covariance_msg.pose.covariance[0] = std_dev_xyz * std_dev_xyz;
transformed_pose_with_covariance_msg.pose.covariance[7] = std_dev_xyz * std_dev_xyz;
transformed_pose_with_covariance_msg.pose.covariance[14] = std_dev_xyz * std_dev_xyz;
transformed_pose_with_covariance_msg.pose.covariance[21] = std_dev_rp * std_dev_rp;
transformed_pose_with_covariance_msg.pose.covariance[28] = std_dev_rp * std_dev_rp;
transformed_pose_with_covariance_msg.pose.covariance[35] = std_dev_yaw * std_dev_yaw;


pose_with_covariance_pub_.publish(transformed_pose_with_covariance_msg);

}

void GnssCompass::publishTF(const std::string & frame_id, const std::string & child_frame_id,
Expand Down