why odom values are continously increasing even if my robot is not moving in simulation environment ?

asked 2020-04-23 07:24:00 -0500

akshaywifi gravatar image

updated 2020-04-23 08:42:13 -0500

gvdhoorn gravatar image

it increases from .0.0123 even if my robot stop


Edit: thanks for your response. I am using a normal gazebo environment you can launch this using gazebo_ros gazebo . i am using the two-wheel robot and some python scripts to measure the distance traveled by a robot all those script files i uploaded below distance is for measuring the distance traveled by robot and movement script used to move the robot to desired distance. there are basically two different scripts ----------------------------------------------------------------distannce--------------------------------------

#!/usr/bin/env python

import rospy
import math
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Point
from std_msgs.msg import Float64

class MovementDetector(object):
    def __init__(self):
        self._mved_distance = Float64()
        self._mved_distance.data = 0.0
        self.get_init_position()

        self.distance_moved_pub = rospy.Publisher('/moved_distance', Float64, queue_size=1)
        rospy.Subscriber("/odom", Odometry, self.odom_callback)


    def get_init_position(self):
        data_odom = None
        while data_odom is None:
            try:
                data_odom = rospy.wait_for_message("/odom", Odometry, timeout=1)
            except:
                rospy.loginfo("Current odom not ready yet, retrying for setting up init pose")

        self._current_position = Point()
        self._current_position.x = data_odom.pose.pose.position.x
        self._current_position.y = data_odom.pose.pose.position.y
        self._current_position.z = data_odom.pose.pose.position.z

    def odom_callback(self,msg):
        NewPosition = msg.pose.pose.position
        self._mved_distance.data += self.calculate_distance(NewPosition, self._current_position)
        self.updatecurrent_positin(NewPosition)
        if self._mved_distance.data < 0.000001:
            aux = Float64()
            aux.data = 0.0
            self.distance_moved_pub.publish(aux)

       # elif self._mved_distance.data>2.23:

           # self._mved_distance.data=0.0
           # rospy.signal_shutdown("quit")



        else:
            self.distance_moved_pub.publish(self._mved_distance)

    def updatecurrent_positin(self, new_position):
        self._current_position.x = new_position.x
        self._current_position.y = new_position.y
        self._current_position.z = new_position.z

    def calculate_distance(self, new_position, old_position):
        x2 = new_position.x
        x1 = old_position.x
        y2 = new_position.y
        y1 = old_position.y
        dist = math.hypot(x2 - x1, y2 - y1)
        return dist

    def publish_moved_distance(self):


        rospy.spin()
        """

        Loops untils closed, publishing moved distance
        """




        # spin() simply keeps python from exiting until this node is stopped

if __name__ == '__main__':
    rospy.init_node('movement_detector_node', anonymous=True)
    movement_obj = MovementDetector()
    movement_obj.publish_moved_distance()

-----------------------------------------------movement-----------------------------------------------------------------

#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Twist
from std_msgs.msg import Float64

def my_callback(msg):

    distance_moved = msg.data

    if msg.data < 2:
        move.linear.x = 0.1

    if msg.data>2.0:
        move.linear.x = 0.0
        if msg.data > 2.01:
            msg.data=0.0
            rospy.signal_shutdown("quit")




    pub.publish(move)

rospy.init_node('test_movement')

sub = rospy.Subscriber('/moved_distance', Float64, my_callback)
pub = rospy.Publisher('/cmd_vel', Twist, queue_size="1")
move = Twist()

rospy.spin()
edit retag flag offensive close merge delete

Comments

you need to provide more info, which simulation platform, which robot, is your robot really moving or just the odom?

Procópio gravatar image Procópio  ( 2020-04-23 07:31:25 -0500 )edit

odom is increasing

akshaywifi gravatar image akshaywifi  ( 2020-04-23 09:01:34 -0500 )edit