Skip to content

Commit df6758f

Browse files
javizqhjavizqh
authored
Merge pull request #763 from JdeRobot/reduce-odometry-noise-turtlebot-low
Reduce odometry noise turtlebot low
2 parents 20c3621 + eced37a commit df6758f

4 files changed

Lines changed: 41 additions & 37 deletions

File tree

‎CustomRobots/turtlebot3/models/turtlebot3/turtlebot3.urdf.xacro‎

Lines changed: 13 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -6,25 +6,26 @@
66
<xacro:arg name="noise_level" default="none"/>
77
<xacro:property name="noise_level" value="$(arg noise_level)"/>
88

9+
<!-- Odometry noise per difficulty (drift std ~ alpha*sqrt(distance)). -->
910
<xacro:if value="${noise_level == 'low'}">
10-
<xacro:property name="a1" value="0.1" />
11-
<xacro:property name="a2" value="0.1" />
12-
<xacro:property name="a3" value="0.05" />
13-
<xacro:property name="a4" value="0.05" />
11+
<xacro:property name="a1" value="0.01" />
12+
<xacro:property name="a2" value="0.01" />
13+
<xacro:property name="a3" value="0.01" />
14+
<xacro:property name="a4" value="0.01" />
1415
</xacro:if>
1516

1617
<xacro:if value="${noise_level == 'med'}">
17-
<xacro:property name="a1" value="0.5" />
18-
<xacro:property name="a2" value="0.5" />
19-
<xacro:property name="a3" value="0.25" />
20-
<xacro:property name="a4" value="0.25" />
18+
<xacro:property name="a1" value="0.05" />
19+
<xacro:property name="a2" value="0.05" />
20+
<xacro:property name="a3" value="0.05" />
21+
<xacro:property name="a4" value="0.05" />
2122
</xacro:if>
2223

2324
<xacro:if value="${noise_level == 'high'}">
24-
<xacro:property name="a1" value="1.5" />
25-
<xacro:property name="a2" value="1.5" />
26-
<xacro:property name="a3" value="0.8" />
27-
<xacro:property name="a4" value="0.8" />
25+
<xacro:property name="a1" value="0.25" />
26+
<xacro:property name="a2" value="0.25" />
27+
<xacro:property name="a3" value="0.25" />
28+
<xacro:property name="a4" value="0.25" />
2829
</xacro:if>
2930

3031
<xacro:include filename="$(find custom_robots)/models/turtlebot3/turtlebot3_gz.urdf.xacro"/>

‎Industrial/gz_noisy_odometry/src/gz_noisy_odometry/NoisyOdometryPlugin.cpp‎

Lines changed: 4 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -127,11 +127,10 @@ namespace custom_plugins
127127
const double ds = wheel_radius_ * (d_right + d_left) / 2.0;
128128
const double dtheta = wheel_radius_ * (d_right - d_left) / wheel_separation_;
129129

130-
// Odometry motion-model noise (Probabilistic Robotics, Ch. 5).
131-
// Translation error grows with distance and heading change; rotation error
132-
// grows with heading change and distance — both accumulate over time.
133-
const double sigma_trans = alpha3_ * std::abs(ds) + alpha4_ * std::abs(dtheta);
134-
const double sigma_rot = alpha1_ * std::abs(dtheta) + alpha2_ * std::abs(ds);
130+
// Motion-model noise: per-tick std scales with sqrt(increment), so drift
131+
// variance grows linearly with travelled distance (step-rate independent).
132+
const double sigma_trans = alpha3_ * std::sqrt(std::abs(ds)) + alpha4_ * std::sqrt(std::abs(dtheta));
133+
const double sigma_rot = alpha1_ * std::sqrt(std::abs(dtheta)) + alpha2_ * std::sqrt(std::abs(ds));
135134

136135
const double ds_noisy = ds + gz::math::Rand::DblNormal(0.0, sigma_trans);
137136
const double dtheta_noisy = dtheta + gz::math::Rand::DblNormal(0.0, sigma_rot);

‎Launchers/drone_cat_mouse.launch.py‎

Lines changed: 2 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -14,9 +14,7 @@
1414

1515
def generate_launch_description():
1616
custom_robots_share = get_package_share_directory("custom_robots")
17-
bridges_path = os.path.join(
18-
custom_robots_share, "bridges", "drone_cat_mouse.yaml"
19-
)
17+
bridges_path = os.path.join(custom_robots_share, "bridges", "drone_cat_mouse.yaml")
2018

2119
world_file_name = "drone_cat_mouse_city.world"
2220
worlds_dir = "/opt/jderobot/Worlds"
@@ -34,9 +32,7 @@ def generate_launch_description():
3432

3533
# gazebo + the ros_gz bridges. the one bridges file covers both drones.
3634
gzsim = IncludeLaunchDescription(
37-
PythonLaunchDescriptionSource(
38-
[jderobot_drones_launch, "/gz_sim.launch.py"]
39-
),
35+
PythonLaunchDescriptionSource([jderobot_drones_launch, "/gz_sim.launch.py"]),
4036
condition=IfCondition(LaunchConfiguration("use_simulator")),
4137
launch_arguments={
4238
"namespace": "drone0",

‎jderobot_drones/launch/as2_default_gazebo_sim.launch.py‎

Lines changed: 22 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,11 @@
55
from ament_index_python.packages import get_package_share_directory
66

77
from launch import LaunchDescription
8-
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
8+
from launch.actions import (
9+
DeclareLaunchArgument,
10+
IncludeLaunchDescription,
11+
OpaqueFunction,
12+
)
913
from launch.launch_description_sources import PythonLaunchDescriptionSource
1014
from launch.substitutions import LaunchConfiguration
1115
from launch_ros.actions import Node, SetParameter
@@ -31,16 +35,17 @@ def _launch_setup(context, *args, **kwargs):
3135
# first and drone1 just inherits drone0's - both ended up driving
3236
# /gz/drone0/cmd_vel and the second drone never moved. easiest way out is to
3337
# skip that file and start the node ourselves with the real topic strings.
34-
cmd_vel = LaunchConfiguration("cmd_vel_topic").perform(context) \
38+
cmd_vel = (
39+
LaunchConfiguration("cmd_vel_topic").perform(context)
3540
or f"/gz/{namespace}/cmd_vel"
36-
arm = LaunchConfiguration("arm_topic").perform(context) \
37-
or f"/gz/{namespace}/arm"
38-
acro = LaunchConfiguration("acro_topic").perform(context) \
39-
or f"/gz/{namespace}/acro"
41+
)
42+
arm = LaunchConfiguration("arm_topic").perform(context) or f"/gz/{namespace}/arm"
43+
acro = LaunchConfiguration("acro_topic").perform(context) or f"/gz/{namespace}/acro"
4044

4145
control_modes = os.path.join(
4246
get_package_share_directory("as2_platform_gazebo"),
43-
"config", "control_modes.yaml",
47+
"config",
48+
"control_modes.yaml",
4449
)
4550
platform_gazebo = Node(
4651
package="as2_platform_gazebo",
@@ -51,11 +56,11 @@ def _launch_setup(context, *args, **kwargs):
5156
emulate_tty=True,
5257
parameters=[
5358
{
54-
"use_sim_time": True,
59+
"use_sim_time": True,
5560
"control_modes_file": control_modes,
56-
"cmd_vel_topic": cmd_vel,
57-
"arm_topic": arm,
58-
"acro_topic": acro,
61+
"cmd_vel_topic": cmd_vel,
62+
"arm_topic": arm,
63+
"acro_topic": acro,
5964
},
6065
as2_sim_config,
6166
],
@@ -111,15 +116,18 @@ def generate_launch_description():
111116
"namespace", default_value="drone0", description="Drone namespace."
112117
),
113118
DeclareLaunchArgument(
114-
"cmd_vel_topic", default_value="",
119+
"cmd_vel_topic",
120+
default_value="",
115121
description="Gazebo cmd_vel topic. Empty = auto-derive from namespace.",
116122
),
117123
DeclareLaunchArgument(
118-
"arm_topic", default_value="",
124+
"arm_topic",
125+
default_value="",
119126
description="Gazebo arm topic. Empty = auto-derive from namespace.",
120127
),
121128
DeclareLaunchArgument(
122-
"acro_topic", default_value="",
129+
"acro_topic",
130+
default_value="",
123131
description="Gazebo acro topic. Empty = auto-derive from namespace.",
124132
),
125133
OpaqueFunction(function=_launch_setup),

0 commit comments

Comments
 (0)