adding noise to /odom topic
hi, I am new to ROS, I am using ros melodic.in my project, i need to add and change noise type(to poisson distribution). The odometry topic is called in differential drive controller plugin in launch file, which is described in this web site link text, in this web site I applied changings on imu but, using odom how can i change the noise on it i don't know. A little help please.
Asked by emrebaltaci on 2021-02-01 17:19:16 UTC
Answers
If you want to add specially noise you can change your odom topic with the another name maybe old_odom and than subscribe this topic. After that you can add what ever want and publish your odom.
https://blog.lxsang.me/post/id/16
#! /usr/bin/env python
import rospy
from math import *
import numpy as np
from nav_msgs.msg import Odometry
import tf
last_odom = None
pose = [0.0,0.0,0.0]
a1 = 0.0
a2 = 0.0
a3 = 0.0
a4 = 0.0
base_frame = ""
# a callback for Gazebo odometry data
def callback(data):
global last_odom
global base_frame
global pose
global a1
global a2
global a3
global a4
if(last_odom == None):
last_odom = data
pose[0] = data.pose.pose.position.x
pose[1] = data.pose.pose.position.y
q = [ data.pose.pose.orientation.x,
data.pose.pose.orientation.y,
data.pose.pose.orientation.z,
data.pose.pose.orientation.w ]
(r, p, y) = tf.transformations.euler_from_quaternion(q)
pose[2] = y
else:
# calculate new odometry pose using the motion model
dx = data.pose.pose.position.x - last_odom.pose.pose.position.x
dy = data.pose.pose.position.y - last_odom.pose.pose.position.y
trans = sqrt(dx*dx + dy*dy)
q = [ last_odom.pose.pose.orientation.x,
last_odom.pose.pose.orientation.y,
last_odom.pose.pose.orientation.z,
last_odom.pose.pose.orientation.w ]
(r,p, theta1) = tf.transformations.euler_from_quaternion(q)
q = [ data.pose.pose.orientation.x,
data.pose.pose.orientation.y,
data.pose.pose.orientation.z,
data.pose.pose.orientation.w ]
(r,p, theta2) = tf.transformations.euler_from_quaternion(q)
rot1 = atan2(dy, dx) - theta1
rot2 = theta2-theta1-rot1
sd_rot1 = a1*abs(rot1) + a2*trans
sd_rot2 = a1*abs(rot2) + a2*trans
sd_trans = a3*trans + a4*(abs(rot1) + abs(rot2))
trans += np.random.normal(0,sd_trans*sd_trans)
rot1 += np.random.normal(0, sd_rot1*sd_rot1)
rot2 += np.random.normal(0, sd_rot2*sd_rot2)
pose[0] += trans*cos(theta1+rot1)
pose[1] += trans*sin(theta1+rot1)
pose[2] = theta1 + rot1 + rot2
last_odom = data
# publish the tf
br = tf.TransformBroadcaster()
br.sendTransform((pose[0], pose[1], 0),
tf.transformations.quaternion_from_euler(0, 0, pose[2]),
data.header.stamp,
base_frame,
"odom")
if __name__ == '__main__':
rospy.init_node('noisy_odometry', anonymous=True)
# alpha 1 is degree/degree
if rospy.has_param("~alpha1"):
a1 = rospy.get_param("~alpha1")
else:
rospy.logwarn("alpha1 is set to default")
a1 = 15.0*pi/180.0
# alpha 2 is degree/m
if rospy.has_param("~alpha2"):
a2 = rospy.get_param("~alpha2")
else:
a2 = 15.0*pi/180.0
rospy.logwarn("alpha2 is set to default")
# alpha 3 is m/meter
if rospy.has_param("~alpha3"):
a3 = rospy.get_param("~alpha3")
else:
a3 = 0.2
rospy.logwarn("alpha3 is set to default")
# alpha 4 is m/degree
if rospy.has_param("~alpha4"):
a4 = rospy.get_param("~alpha4")
else:
a4 = 0.01
rospy.logwarn("alpha4 is set to default")
# get odom topic
if rospy.has_param("~old_odom_topic"):
odom_topic = rospy.get_param("~old_odom_topic")
else:
odom_topic = "/odom"
# get base frame
if rospy.has_param("~base_frame"):
base_frame = rospy.get_param("~base_frame")
else:
base_frame = "/base_footprint"
rospy.Subscriber(odom_topic, Odometry, callback)
rospy.spin()
Asked by bekirbostanci on 2021-02-02 12:22:04 UTC
Comments
I think it would be a nice addition if this script would include filling up the odometry noise covariance matrix.
Asked by Roberto Z. on 2021-02-12 03:44:13 UTC
Hi there. This did this actually work?. The values of a1,a2,a3 and a4 cause my localization to jump all over the place.
Asked by psilva on 2021-03-16 10:12:50 UTC
Thanks for the information! Please, to select the correct ALPHAs odometry model noise: Is it correct to calculate the RMSE between /cmd_vel/linear/x and /odom/twist/twsit/linear/x for translation and RMSE between /cmd_vel/angular/z and /odom/twist/twist/angular/z for rotation ?
Can you suggest for me the correct way to tune them to the reasonable values ?
Asked by Zuhair95 on 2022-08-15 13:44:41 UTC
Comments