SMACC2
ekf_gps_launch.py
Go to the documentation of this file.
1# Copyright 2018 Open Source Robotics Foundation, Inc.
2# Licensed under the Apache License, Version 2.0 (the "License");
3# you may not use this file except in compliance with the License.
4# You may obtain a copy of the License at
5#
6# http://www.apache.org/licenses/LICENSE-2.0
7#
8# Unless required by applicable law or agreed to in writing, software
9# distributed under the License is distributed on an "AS IS" BASIS,
10# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
11# See the License for the specific language governing permissions and
12# limitations under the License.
13
14from launch import LaunchDescription
15import launch_ros.actions
16import os
17from launch.substitutions import LaunchConfiguration
18import launch.actions
19
20# from launch.actions import DeclareLaunchArgument
21from ament_index_python.packages import get_package_share_directory
22
23
25 robot_localization_dir = get_package_share_directory("robot_localization")
26 parameters_file_dir = os.path.join(robot_localization_dir, "params")
27 # parameters_file_path = os.path.join("sm_husky_barrel_search_1", 'dual_ekf_navsat.yaml')
28
29 parameters_file_path = os.path.join(parameters_file_dir, "dual_ekf_navsat_example.yaml")
30 os.environ["FILE_PATH"] = str(parameters_file_dir)
31
32 xtermprefix = "xterm -xrm 'XTerm*scrollBar: true' -xrm 'xterm*rightScrollBar: true' -hold -geometry 1000x600 -sl 10000 -e"
33
34 return LaunchDescription(
35 [
36 launch.actions.DeclareLaunchArgument("use_sim_time", default_value="false"),
37 launch.actions.DeclareLaunchArgument("output_final_position", default_value="false"),
38 launch.actions.DeclareLaunchArgument(
39 "output_location", default_value="~/dual_ekf_navsat_example_debug.txt"
40 ),
41 # launch_ros.actions.Node(
42 # package='robot_localization',
43 # executable='ekf_node',
44 # name='ekf_filter_node_odom',
45 # output='screen',
46 # parameters=[parameters_file_path],
47 # remappings=[('odometry/filtered', 'odometry/local')]
48 # ),
49 launch_ros.actions.Node(
50 package="robot_localization",
51 executable="ekf_node",
52 name="ekf_filter_node_map",
53 output="screen",
54 parameters=[
55 {"use_sim_time": LaunchConfiguration("use_sim_time")},
56 parameters_file_path,
57 ],
58 remappings=[("odometry/filtered", "odometry/global"), ("odometry/wheel", "/odom")],
59 prefix=xtermprefix,
60 ),
61 launch_ros.actions.Node(
62 package="robot_localization",
63 executable="navsat_transform_node",
64 name="navsat_transform",
65 output="screen",
66 parameters=[
67 {"use_sim_time": LaunchConfiguration("use_sim_time")},
68 parameters_file_path,
69 ],
70 remappings=[
71 ("imu/data", "imu"),
72 ("gps/fix", "gps/data"),
73 ("gps/filtered", "gps/filtered"),
74 ("odometry/gps", "odometry/gps"),
75 ("odometry/filtered", "odometry/global"),
76 ],
77 prefix=xtermprefix,
78 ),
79 ]
80 )
def generate_launch_description()