Ask Your Question
0

adding noise to /odom topic

asked 2021-02-01 16:19:16 -0500

emrebaltaci gravatar image

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.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2021-02-02 11:22:04 -0500

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()
edit flag offensive delete link more

Comments

1

I think it would be a nice addition if this script would include filling up the odometry noise covariance matrix.

Roberto Z. gravatar image Roberto Z.  ( 2021-02-12 02:44:13 -0500 )edit

Hi there. This did this actually work?. The values of a1,a2,a3 and a4 cause my localization to jump all over the place.

psilva gravatar image psilva  ( 2021-03-16 10:12:50 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2021-02-01 16:19:16 -0500

Seen: 432 times

Last updated: Feb 02 '21