Robotics StackExchange | Archived questions

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

Comments

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