Skip to content

Commit c073d28

Browse files
szepilotszepilot
committed
Novatel gps update
1 parent 09093cf commit c073d28

2 files changed

Lines changed: 92 additions & 58 deletions

File tree

‎launch/drivers/gps_nova.launch.py‎

Lines changed: 77 additions & 54 deletions
Original file line numberDiff line numberDiff line change
@@ -3,89 +3,112 @@
33
import yaml
44

55
from launch import LaunchDescription
6-
from launch.substitutions import LaunchConfiguration
6+
from launch.substitutions import LaunchConfiguration
77
from launch.actions import DeclareLaunchArgument
88
from launch_ros.actions import Node
99
from ament_index_python.packages import get_package_share_directory
1010

1111
PKG = "novatel_oem7_driver"
12+
lx_nova_namespace = '/lexus3/gps/nova'
13+
1214

1315
def get_cfg_path(cfg):
14-
return os.path.join(get_package_share_directory(PKG), "config", cfg )
15-
16+
return os.path.join(get_package_share_directory(PKG), "config", cfg)
17+
1618

1719
def load_yaml(p):
1820
with open(p, 'r') as f:
1921
return yaml.safe_load(f)
2022

23+
2124
def get_params(cfg):
2225
return load_yaml(get_cfg_path(cfg))
2326

24-
def get_override_params():
27+
28+
def get_override_params():
2529
try:
2630
oem7_param_override_path = os.environ['NOVATEL_OEM7_DRIVER_PARAM_OVERRIDES_PATH']
27-
except KeyError: # No overrides specified.
31+
except KeyError: # No overrides specified.
2832
return {}
29-
33+
3034
return load_yaml(oem7_param_override_path)
3135

32-
36+
3337
def arg(name, default_value, description):
34-
return DeclareLaunchArgument(name = name, description = description, default_value = default_value)
35-
38+
return DeclareLaunchArgument(name=name, description=description, default_value=default_value)
39+
3640

37-
3841
def generate_launch_description():
3942

40-
node = Node(
41-
package=PKG,
42-
namespace='/lexus3/nova',
43+
nova_node = Node(
44+
package = PKG,
45+
namespace = lx_nova_namespace,
4346
name='main',
4447
executable='novatel_oem7_driver_exe',
45-
48+
4649
parameters=[
47-
get_params("std_msg_handlers.yaml" ),
48-
get_params("std_oem7_raw_msgs.yaml" ),
49-
get_params("std_msg_topics.yaml" ),
50-
get_params("oem7_supported_imus.yaml" ),
51-
get_params("std_init_commands.yaml" ),
52-
{
53-
# Refer to README for more information on the configuration parameters
54-
# Receiver IO Parameters
55-
'oem7_max_io_errors' : 10,
56-
'oem7_msg_decoder': 'Oem7MessageDecoder',
57-
'oem7_if' : LaunchConfiguration('oem7_if'),
58-
'oem7_ip_addr' : LaunchConfiguration('oem7_ip_addr'),
59-
'oem7_port' : LaunchConfiguration('oem7_port'),
60-
61-
# Topic Parameters
62-
'oem7_position_source' : '', # If not set, then uses INSPVA or BESTPOS based on quality
63-
'oem7_imu_rate' : 0,
64-
'oem7_odometry_zero_origin' : False,
65-
'oem7_odometry_transform' : False,
66-
67-
# Debug/Other Parameters
68-
'oem7_receiver_log_file' : '',
69-
'oem7_decoder_log_file' : '',
70-
'oem7_strict_receiver_init' : True,
71-
'oem7_publish_unknown_oem7raw' : False,
72-
'oem7_publish_delay' : 0.0
73-
74-
},
75-
get_override_params() # Must be last to override
76-
],
77-
50+
get_params("std_msg_handlers.yaml"),
51+
get_params("std_oem7_raw_msgs.yaml"),
52+
get_params("std_msg_topics.yaml"),
53+
get_params("oem7_supported_imus.yaml"),
54+
get_params("std_init_commands.yaml"),
55+
{
56+
# Refer to README for more information on the configuration parameters
57+
# Receiver IO Parameters
58+
'oem7_max_io_errors': 10,
59+
'oem7_msg_decoder': 'Oem7MessageDecoder',
60+
'oem7_if': LaunchConfiguration('oem7_if'),
61+
'oem7_ip_addr': LaunchConfiguration('oem7_ip_addr'),
62+
'oem7_port': LaunchConfiguration('oem7_port'),
63+
64+
# Topic Parameters
65+
# If not set, then uses INSPVA or BESTPOS based on quality
66+
'oem7_position_source': '',
67+
'oem7_imu_rate': 0,
68+
'oem7_odometry_zero_origin': False,
69+
'oem7_odometry_transform': False,
70+
71+
# Debug/Other Parameters
72+
'oem7_receiver_log_file': '',
73+
'oem7_decoder_log_file': '',
74+
'oem7_strict_receiver_init': True,
75+
'oem7_publish_unknown_oem7raw': False,
76+
'oem7_publish_delay': 0.0
77+
78+
},
79+
get_override_params() # Must be last to override
80+
],
81+
7882
output='screen',
7983
)
80-
81-
ip_arg = arg('oem7_ip_addr', '192.168.10.12', 'IP Address of Oem7 Receiver, e.g. 192.168.1.2')
82-
port_arg = arg('oem7_port', '3001', 'TCP or UDP port, e.g. 3002')
83-
if_arg = arg('oem7_if', 'Oem7ReceiverUdp', 'Interface Type: Oem7ReceiverTcp or Oem7ReceiverUdp')
84-
85-
return LaunchDescription([ip_arg, port_arg, if_arg,
86-
node])
8784

88-
85+
convert_node = Node(
86+
package = "lexus_bringup",
87+
namespace = lx_nova_namespace,
88+
name='gps_convert',
89+
executable='nova_oem7_to_tf',
90+
parameters=[
91+
{
92+
'use_sim_time': False,
93+
'x_coord_offset': -697237.0,
94+
'y_coord_offset': -5285644.0,
95+
'z_coord_exact_height': 1.9,
96+
'frame_id': 'map',
97+
'child_frame_id': 'lexus3/base_link',
98+
'z_coord_ref_switch': 'orig',
99+
'pose_topic_pub': 'current_pose',
100+
'utm_topic_sub': 'bestutm',
101+
'inspva_topic_sub': 'inspva'
102+
}
103+
]
104+
)
89105

106+
ip_arg = arg('oem7_ip_addr', '192.168.10.12',
107+
'IP Address of Oem7 Receiver, e.g. 192.168.1.2')
108+
port_arg = arg('oem7_port', '3001',
109+
'TCP or UDP port, e.g. 3002')
110+
if_arg = arg('oem7_if', 'Oem7ReceiverUdp',
111+
'Interface Type: Oem7ReceiverTcp or Oem7ReceiverUdp')
90112

91-
113+
return LaunchDescription([ip_arg, port_arg, if_arg,
114+
nova_node, convert_node])

‎src/nova_oem7_to_tf.cpp‎

Lines changed: 15 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -31,21 +31,29 @@ class CurrentPoseFromTf : public rclcpp::Node
3131
this->declare_parameter<std::string>("frame_id", "map");
3232
this->declare_parameter<std::string>("child_frame_id", "lexus3/base_link");
3333
this->declare_parameter<std::string>("z_coord_ref_switch", "orig");
34+
this->declare_parameter<std::string>("pose_topic_pub", "lexus3/gps/nova/current_pose");
35+
this->declare_parameter<std::string>("utm_topic_sub", "lexus3/gps/nova/bestutm");
36+
this->declare_parameter<std::string>("inspva_topic_sub", "lexus3/gps/nova/inspva");
37+
3438
this->get_parameter("x_coord_offset", m_x_coord_offset_);
3539
this->get_parameter("y_coord_offset", m_y_coord_offset_);
3640
this->get_parameter("z_coord_exact_height", m_z_coord_exact_height_);
3741
this->get_parameter("frame_id", m_frame_id_);
3842
this->get_parameter("child_frame_id", m_child_frame_id_);
3943
this->get_parameter("z_coord_ref_switch", m_z_coord_ref_switch_);
44+
this->get_parameter("pose_topic_pub", m_pose_topic_);
45+
this->get_parameter("utm_topic_sub", m_utm_topic_);
46+
this->get_parameter("inspva_topic_sub", m_inspva_topic_);
4047

41-
42-
m_pose_pub_ = this->create_publisher<geometry_msgs::msg::PoseStamped>("lexus3/gps/nova/current_pose", 10);
43-
m_utm_sub_ = this->create_subscription<novatel_oem7_msgs::msg::BESTUTM>("/novatel/oem7/bestutm", 10, std::bind(&CurrentPoseFromTf::utm_callback, this, _1));
44-
m_inspva_sub_ = this->create_subscription<novatel_oem7_msgs::msg::INSPVA>("/novatel/oem7/inspva", 10, std::bind(&CurrentPoseFromTf::inspva_callback, this, _1));
48+
m_pose_pub_ = this->create_publisher<geometry_msgs::msg::PoseStamped>(m_pose_topic_, 10);
49+
m_utm_sub_ = this->create_subscription<novatel_oem7_msgs::msg::BESTUTM>(m_utm_topic_, 10, std::bind(&CurrentPoseFromTf::utm_callback, this, _1));
50+
m_inspva_sub_ = this->create_subscription<novatel_oem7_msgs::msg::INSPVA>(m_inspva_topic_, 10, std::bind(&CurrentPoseFromTf::inspva_callback, this, _1));
4551
m_tfBroadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
4652
RCLCPP_INFO_STREAM(this->get_logger(), "nova_oem7_to_tf node started");
4753
RCLCPP_INFO_STREAM(this->get_logger(), "nova coord offset: " << m_x_coord_offset_ << ", " << m_y_coord_offset_ << ", " << m_z_coord_exact_height_);
4854
RCLCPP_INFO_STREAM(this->get_logger(), "nova frame_id: " << m_frame_id_ << ", child_frame_id: " << m_child_frame_id_);
55+
RCLCPP_INFO_STREAM(this->get_logger(), "nova utm_topic: " << m_utm_topic_);
56+
4957
if (m_z_coord_ref_switch_.compare("exact") == 0){
5058
RCLCPP_INFO_STREAM(this->get_logger(), "nova exact height (z): " << m_z_coord_exact_height_);
5159
}
@@ -127,6 +135,9 @@ class CurrentPoseFromTf : public rclcpp::Node
127135
double m_x_coord_offset_ = 0.0;
128136
double m_y_coord_offset_ = 0.0;
129137
double m_z_coord_exact_height_ = 1.9;
138+
std::string m_pose_topic_ = "/lexus3/gps/nova/current_pose";
139+
std::string m_utm_topic_ = "/lexus3/nova/bestutm";
140+
std::string m_inspva_topic_ = "/lexus3/nova/inspva";
130141

131142
};
132143

0 commit comments

Comments
 (0)