Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
R
robot_loc
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Requirements
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Locked files
Deploy
Releases
Package Registry
Model registry
Operate
Terraform modules
Monitor
Incidents
Service Desk
Analyze
Value stream analytics
Contributor analytics
Repository analytics
Code review analytics
Issue analytics
Insights
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
Emil Harlan
robot_loc
Commits
18e3ef41
Commit
18e3ef41
authored
1 year ago
by
Emil Harlan
Browse files
Options
Downloads
Patches
Plain Diff
a
parent
f6ab7fd5
No related branches found
No related tags found
No related merge requests found
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
config/ekf.yaml
+105
-0
105 additions, 0 deletions
config/ekf.yaml
launch/test.launch.py
+78
-0
78 additions, 0 deletions
launch/test.launch.py
with
183 additions
and
0 deletions
config/ekf.yaml
0 → 100644
+
105
−
0
View file @
18e3ef41
ekf_filter_node
:
ros__parameters
:
# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
# computation until it receives at least one message from one of the inputs. It will then run continuously at the
# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
frequency
:
50.0
# The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict
# cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the
# filter will generate new output. Defaults to 1 / frequency if not specified.
sensor_timeout
:
0.1
# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
# by, for example, an IMU. Defaults to false if unspecified.
two_d_mode
:
true
# Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for
# future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if
# unspecified.
transform_time_offset
:
0.0
# Use this parameter to provide specify how long the tf listener should wait for a transform to become available.
# Defaults to 0.0 if unspecified.
transform_timeout
:
0.0
# If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is
# unhappy with any settings or data.
print_diagnostics
:
true
# Whether we'll allow old measurements to cause a re-publication of the updated state
permit_corrected_publication
:
false
# Whether to publish the acceleration state. Defaults to false if unspecified.
publish_acceleration
:
false
# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
publish_tf
:
true
# REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and
# earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames.
# The robot's position in the odom frame will drift over time, but is accurate in the short term and should be
# continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom
# frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your
# robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based
# localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame.
# ekf_localization_node and ukf_localization_node are not concerned with the earth frame.
# Here is how to use the following settings:
# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
# 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of
# odom_frame.
# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set
# "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.
# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates
# from landmark observations) then:
# 3a. Set your "world_frame" to your map_frame value
# 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state
# estimation node from robot_localization! However, that instance should *not* fuse the global data.
map_frame
:
map
# Defaults to "map" if unspecified
odom_frame
:
odom
# Defaults to "odom" if unspecified
base_link_frame
:
base_footprint
# Defaults to "base_link" if unspecified
world_frame
:
odom
# Defaults to the value of odom_frame if unspecified
# The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry,
# geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped,
# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0,
# odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no
# default values, and must be specified.
odom0
:
encoder_odometry
# Each sensor reading updates some or all of the filter's state. These options give you greater control over which
# values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only
# want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the
# values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types
# do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message
# has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false
# if unspecified, effectively making this parameter required for each sensor.
odom0_config
:
[
true
,
true
,
false
,
false
,
false
,
false
,
false
,
false
,
false
,
false
,
false
,
true
,
false
,
false
,
false
]
# [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under-
# report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they
# arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also
# measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't
# always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose
# data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then
# integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true
# for twist measurements has no effect.
odom0_differential
:
false
imu0
:
Imu
imu0_config
:
[
false
,
false
,
false
,
true
,
true
,
true
,
false
,
false
,
false
,
true
,
true
,
true
,
true
,
true
,
true
]
imu0_differential
:
false
\ No newline at end of file
This diff is collapsed.
Click to expand it.
launch/test.launch.py
0 → 100644
+
78
−
0
View file @
18e3ef41
import
os
from
ament_index_python.packages
import
get_package_share_directory
from
launch
import
LaunchDescription
from
launch.substitutions
import
LaunchConfiguration
,
Command
from
launch.actions
import
DeclareLaunchArgument
from
launch_ros.actions
import
Node
import
xacro
def
generate_launch_description
():
# Process the URDF file
pkg_path
=
os
.
path
.
join
(
get_package_share_directory
(
'
robot_loc
'
))
xacro_file
=
os
.
path
.
join
(
pkg_path
,
'
urdf
'
,
'
robomaster_ep.urdf.xacro
'
)
# robot_description_config = xacro.process_file(xacro_file).toxml()
robot_description_config
=
Command
([
'
xacro
'
,
xacro_file
])
# Create a robot_state_publisher node
params
=
{
'
robot_description
'
:
robot_description_config
}
node_robot_state_publisher
=
Node
(
package
=
'
robot_state_publisher
'
,
executable
=
'
robot_state_publisher
'
,
output
=
'
screen
'
,
parameters
=
[
params
]
)
node_lidar
=
Node
(
package
=
'
rplidar_ros
'
,
executable
=
'
rplidar_composition
'
,
output
=
'
screen
'
,
parameters
=
[{
'
serial_port
'
:
'
/dev/serial/by-path/platform-fd500000.pcie-pci-0000:01:00.0-usb-0:1.2:1.0-port0
'
,
'
frame_id
'
:
'
laser_link
'
,
'
angle_compensate
'
:
True
,
'
scan_mode
'
:
'
Sensitivity
'
}]
)
node_odom
=
Node
(
package
=
'
robomaster_pi
'
,
executable
=
'
Odom
'
,
)
node_vel
=
Node
(
package
=
'
robomaster_pi
'
,
executable
=
'
conn_vel
'
,
)
node_IMU
=
Node
(
package
=
'
robomaster_pi
'
,
executable
=
'
IMU
'
,
)
node_ekf
=
Node
(
package
=
'
robot_localization
'
,
executable
=
'
ekf_node
'
,
name
=
'
ekf_filter_node
'
,
output
=
'
screen
'
,
parameters
=
[
os
.
path
.
join
(
get_package_share_directory
(
"
robot_loc
"
),
'
config
'
,
'
ekf.yaml
'
)]
)
# Launch!
return
LaunchDescription
([
node_robot_state_publisher
,
node_lidar
,
node_odom
,
node_vel
,
node_IMU
,
node_ekf
])
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment