Ros basics part 3 project

ROSDS Support
1

i keep getting that error

: started with pid [11747]
[INFO] [1678223798.285249, 1831.599000]: OdomRecord action started
[INFO] [1678223798.367049, 1831.601000]: the robot does not start moving yet
[INFO] [1678223798.459553, 1831.625000]: the robot does not start moving yet
[ERROR] [1678223798.468999, 1831.650000]: Exception in your execute callback: ‘Point’ object has no attribute ‘pose’
Traceback (most recent call last):
File “/home/simulations/public_sim_ws/src/all/actionlib/actionlib/src/actionlib/simple_action_server.py”, line 289, in executeLoop
self.execute_callback(goal)
File “/home/user/catkin_ws/src/project/src/action_server.py”, line 82, in callback_action
distance_x_home = (self.current_position.pose.pose.position.x) - (home_position.pose.pose.position.x)
AttributeError: ‘Point’ object has no attribute ‘pose’

#! /usr/bin/env python 

import math
import rospy
import actionlib
from project.msg import OdomRecordAction ,OdomRecordResult , OdomRecordFeedback
from nav_msgs.msg import Odometry
import tf


class record_odomClass(object):
    _feedback = OdomRecordFeedback()
    _result = OdomRecordResult()

    def __init__(self):
        
        self.server = actionlib.SimpleActionServer('record_odom',OdomRecordAction,self.callback_action,False)
        rospy.sub = rospy.Subscriber('/odom' , Odometry  , self.callback)
        rospy.wait_for_message('/odom' , Odometry)
        self.server.start()
        self.odom = Odometry()
        self.current_position = Odometry() 
        self.last_position = Odometry()

    def callback(self,msg):
        self.odom = msg
        
    
    def callback_action(self,goal):
        r =rospy.Rate(1)
        success = True
        distance = 0 
        distance_from_home_position = 0
        home_position = Odometry()
        home_position = self.odom.pose.pose.position
        self._feedback.current_total = 0 
        self._result.list_of_odoms = [] 
        
        rospy.loginfo("OdomRecord action started")
        
        #theta 
        orientation_= Odometry() 
        orientation_.pose.pose.orientation = self.current_position.pose.pose.orientation
        quaternion = [orientation_.pose.pose.orientation.x, orientation_.pose.pose.orientation.y,orientation_.pose.pose.orientation.z, orientation_.pose.pose.orientation.w]
        euler = tf.transformations.euler_from_quaternion(quaternion)
        angle = euler[2] # Get the yaw angle from the euler angles 

        #x and y 
        self.current_position.pose.pose.position = self.odom.pose.pose.position
        self.last_position.pose.pose.position = self.odom.pose.pose.position

        self._result.list_of_odoms.append(self.odom.pose.pose.position.x)
        self._result.list_of_odoms.append(self.odom.pose.pose.position.y)
        self._result.list_of_odoms.append(angle)

        while(1):

            if self.server.is_preempt_requested():
                rospy.loginfo("the goal is cnaceled")
                self.server.set_preempted()
                success = False
                break
            
            self.current_position.pose.pose.position = self.odom.pose.pose.position

            if self.current_position == self.last_position:
                rospy.loginfo("the robot does not start moving yet")
            
            else:

                distance_x = self.current_position.pose.pose.position.x - self.last_position.pose.pose.position.x
                distance_y = self.current_position.pose.pose.position.y - self.last_position.pose.pose.position.y
                #theta 
                orientation_.pose.pose.orientation = self.current_position.pose.pose.orientation
                quaternion = [orientation_.pose.pose.orientation.x, orientation_.pose.pose.orientation.y,orientation_.pose.pose.orientation.z, orientation_.pose.pose.orientation.w]
                euler = tf.transformations.euler_from_quaternion(quaternion)
                angle = euler[2] # Get the yaw angle from the euler angles 
                #distance

                distance += math.sqrt(distance_x**2 + distance_y**2)
                #distance from home 
                distance_x_home = (self.current_position.pose.pose.position.x) - (home_position.pose.pose.position.x)
                distance_y_home = (self.current_position.pose.pose.position.y) - (home_position.pose.pose.position.y)
                distance_from_home_position = math.sqrt(distance_x_home**2 + distance_y_home**2)

                #publish the feedback
                self._feedback = distance
                self.server.publish_feedback(self._feedback)
                #store the recorded odometery 
                self._result.list_of_odoms.append(self.odom.pose.pose.position.x)
                self._result.list_of_odoms.append(self.odom.pose.pose.position.y)
                self._result.list_of_odoms.append(angle)

                
                #see if the turtlebot done a complete loop
                if ((distance_from_home_position < 0.3) and (distance_from_home_position > 0 )):
                    break

                r.sleep()
        
        if success:
            rospy.loginfo("action successd")
            self.server.set_succeeded(self._result)
            


    


if __name__ == '__main__':
    rospy.init_node('action_node')
    record_odomClass()
    rospy.spin()
2

distance_x_home = (self.current_position.pose.pose.position.x) - (home_position.x)

1 Like
4

thanks it worked one last question i am finding some trouble in knowing when the robot has done a full lab in my code if the robot current_position - home position is less than 0.1 the loop will break and the action is succeed but this is not valid as the second the robot moves this condition becomes true is there is another way to done this ?

#! /usr/bin/env python 

import math
import rospy
import actionlib
from project.msg import OdomRecordAction ,OdomRecordResult , OdomRecordFeedback
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Point 
import tf


class record_odomClass(object):
    _feedback = OdomRecordFeedback()
    _result = OdomRecordResult()

    def __init__(self):
        
        self.server = actionlib.SimpleActionServer('record_odom',OdomRecordAction,self.callback_action,False)
        rospy.sub = rospy.Subscriber('/odom' , Odometry  , self.callback)
        rospy.wait_for_message('/odom' , Odometry)
        self.server.start()
        self.odom = Odometry()
        self.current_position = Odometry() 
        self.last_position = Odometry()

    def callback(self,msg):
        self.odom = msg
        
    
    def callback_action(self,goal):
        r =rospy.Rate(1)
        success = True
        count = 0 
        distance = 0 
        distance_from_home_position = 0
        home_position = Odometry()
        home_position = self.odom.pose.pose.position
        self._feedback = 0.0
        self._result.list_of_odoms = [] 
        
        rospy.loginfo("OdomRecord action started")
        
        #theta 
        orientation_= Odometry() 
        orientation_.pose.pose.orientation = self.current_position.pose.pose.orientation
        quaternion = [orientation_.pose.pose.orientation.x, orientation_.pose.pose.orientation.y,orientation_.pose.pose.orientation.z, orientation_.pose.pose.orientation.w]
        euler = tf.transformations.euler_from_quaternion(quaternion)
        angle = euler[2] # Get the yaw angle from the euler angles 

        #x and y 
        self.current_position.pose.pose.position = self.odom.pose.pose.position
        self.last_position.pose.pose.position = self.odom.pose.pose.position

        self._result.list_of_odoms.append(self.odom.pose.pose.position.x)
        self._result.list_of_odoms.append(self.odom.pose.pose.position.y)
        self._result.list_of_odoms.append(angle)

        while(1):

            
            self.current_position.pose.pose.position = self.odom.pose.pose.position

            while self.current_position == self.last_position:
                rospy.loginfo("the robot does not start moving yet")
                rospy.sleep(1)
            
            if self.server.is_preempt_requested():
                rospy.loginfo("the goal is cnaceled")
                self.server.set_preempted()
                success = False
                break

            else:

                distance_x = self.current_position.pose.pose.position.x - self.last_position.pose.pose.position.x
                distance_y = self.current_position.pose.pose.position.y - self.last_position.pose.pose.position.y
                #theta 
                orientation_.pose.pose.orientation = self.current_position.pose.pose.orientation
                quaternion = [orientation_.pose.pose.orientation.x, orientation_.pose.pose.orientation.y,orientation_.pose.pose.orientation.z, orientation_.pose.pose.orientation.w]
                euler = tf.transformations.euler_from_quaternion(quaternion)
                angle = euler[2] # Get the yaw angle from the euler angles 
               
                #distance
   
                distance += math.sqrt(distance_x**2 + distance_y**2)
                #distance from home 
                distance_x_home = (self.current_position.pose.pose.position.x) - (home_position.x)
                distance_y_home = (self.current_position.pose.pose.position.y) - (home_position.y)
                distance_from_home_position = math.sqrt(distance_x_home**2 + distance_y_home**2)

                #publish the feedback
                self._feedback = distance
                self.server.publish_feedback(self._feedback)



                
                #update position 
                self.last_position = self.current_position

                #store the recorded odometery 
                my_point = Point()
                my_point.x = distance_x
                my_point.y = distance_y
                my_point.z = angle
                self._result.list_of_odoms.append(my_point)
                
                
                

                #see if the turtlebot done a complete loop
                if (0.3 > distance_from_home_position > 0.1):
                        rospy.loginfo(distance_from_home_position)
                        break

                r.sleep()
        
        if success:
            rospy.loginfo("action successd")
            self.server.set_succeeded(self._result)
            


    


if __name__ == '__main__':
    rospy.init_node('action_node')
    record_odomClass()
    rospy.spin()
5

Hi @80601 ,

The best way to fix this is to move the robot for a few seconds, say 30 seconds, and then start calculating the distance between the starting pose and current pose. This way you can complete a lap successfully.

You are calculating the distance between the robot and start pose from the very beginning which causes this problem.

This is what I did in my project too. So I am telling this from my experience. This will fix your problem.

Regards,
Girish

1 Like
6

thanks your advise really helped me but i want to ask another simple question the self.last_position is seems to be updated at every call so that the current and last values will be always be the same how can i fix this?

#! /usr/bin/env python 

import math
#import time
import rospy
import actionlib
from project.msg import OdomRecordAction ,OdomRecordResult , OdomRecordFeedback
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Point 
import tf


class record_odomClass(object):
    _feedback = OdomRecordFeedback()
    _result = OdomRecordResult()

    def __init__(self):
        
        self.server = actionlib.SimpleActionServer('record_odom',OdomRecordAction,self.callback_action,False)
        rospy.sub = rospy.Subscriber('/odom' , Odometry  , self.callback)
        rospy.wait_for_message('/odom' , Odometry)
        self.server.start()
        self.odom = Odometry()
        self.current_position = Odometry() 
        self.last_position = Odometry()

    def callback(self,msg):
        self.odom = msg
        
    
    def callback_action(self,goal):
        r =rospy.Rate(1)
        success = True
        count = 0 
        distance = 0 
        distance_from_home_position = 0
        home_position = Odometry()
        home_position = self.odom.pose.pose.position
        self._feedback = 0.0
        self._result.list_of_odoms = [] 
        
        rospy.loginfo("OdomRecord action started")
        
        #theta 
        orientation_= Odometry() 
        orientation_.pose.pose.orientation = self.current_position.pose.pose.orientation
        quaternion = [orientation_.pose.pose.orientation.x, orientation_.pose.pose.orientation.y,orientation_.pose.pose.orientation.z, orientation_.pose.pose.orientation.w]
        euler = tf.transformations.euler_from_quaternion(quaternion)
        angle = euler[2] # Get the yaw angle from the euler angles 

        #x and y 
        self.current_position.pose.pose.position = self.odom.pose.pose.position
        self.last_position.pose.pose.position = self.odom.pose.pose.position

        self._result.list_of_odoms.append(self.odom.pose.pose.position.x)
        self._result.list_of_odoms.append(self.odom.pose.pose.position.y)
        self._result.list_of_odoms.append(angle)
        t_start = rospy.Time.now()
        rospy.logwarn(t_start)
        rospy.logwarn(t_start.to_sec())
           


        while(1):   

            
            self.current_position.pose.pose.position = self.odom.pose.pose.position
            while round(self.current_position.pose.pose.position.x, 3) == round(self.last_position.pose.pose.position.x, 3) and \
            round(self.current_position.pose.pose.position.y, 3) == round(self.last_position.pose.pose.position.y, 3):

                self.current_position.pose.pose.position = self.odom.pose.pose.position
                rospy.logwarn(self.current_position.pose.pose.position)
                rospy.logwarn(self.last_position.pose.pose.position)
                rospy.loginfo("the robot is not moving")
                rospy.sleep(1)
            
            if self.server.is_preempt_requested():
                rospy.loginfo("the goal is cnaceled")
                self.server.set_preempted()
                success = False
                break

            else:

                distance_x = self.current_position.pose.pose.position.x - self.last_position.pose.pose.position.x
                distance_y = self.current_position.pose.pose.position.y - self.last_position.pose.pose.position.y
                #theta 
                orientation_.pose.pose.orientation = self.current_position.pose.pose.orientation
                quaternion = [orientation_.pose.pose.orientation.x, orientation_.pose.pose.orientation.y,orientation_.pose.pose.orientation.z, orientation_.pose.pose.orientation.w]
                euler = tf.transformations.euler_from_quaternion(quaternion)
                angle = euler[2] # Get the yaw angle from the euler angles 
               
                #distance
   
                distance += math.sqrt(distance_x**2 + distance_y**2)
                #distance from home 
                distance_x_home = (self.current_position.pose.pose.position.x) - (home_position.x)
                distance_y_home = (self.current_position.pose.pose.position.y) - (home_position.y)
                distance_from_home_position = math.sqrt(distance_x_home**2 + distance_y_home**2)

                #publish the feedback
                self._feedback = distance
                self.server.publish_feedback(self._feedback)



                
                #update position 
                self.last_position = self.current_position

                #store the recorded odometery 
                my_point = Point()
                my_point.x = distance_x
                my_point.y = distance_y
                my_point.z = angle
                self._result.list_of_odoms.append(my_point)
                
                
                t_before_end = rospy.Time.now()
                
                if((t_before_end - t_start).to_sec() > 20):
                    rospy.loginfo(t_before_end-t_start)
                    #see if the turtlebot done a complete loop
                    if (0.3 > round(distance_from_home_position,3) > 0.1):
                            rospy.loginfo(distance_from_home_position)
                            break

                r.sleep()
        
        if success:
            rospy.loginfo("action successd")
            self.server.set_succeeded(self._result)
            


    


if __name__ == '__main__':
    rospy.init_node('action_node')
    record_odomClass()
    rospy.spin()
7

Hi @80601 ,

That is because of your own program logic as shown below:

The above two lines has the problem. You are updating current_position and last_position with the same odometry value.

You need to fix your program yourself. I have pointed out your mistake. Refactor your code logic.

Regards,
Girish

8

i actually know the mistake but i asked about a differrent way so they did not get the same value as you can see at each callback of the odom msg they both get update despite there place in the code

9

Hi @80601 ,

In my opinion, you do not require those two variables : current_position and last_position.

The reason being, you should return the list of all poses travelled by the robot when the action server returns the result. So, the better way to do this is to store the odometry readings in a list and use the last two pose values from the list to calculate the distance moved every second.

So, the best way to remove this error would be to refactor your code logic.

Regards,
Girish

10

thanks girish i will try that

11

i did made what you say but nothing changes

#! /usr/bin/env python 

import math
#import time
import rospy
import actionlib
from project.msg import OdomRecordAction ,OdomRecordResult , OdomRecordFeedback
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Point 
import tf



def update_theta(theta):
    #theta 
    orientation_= Odometry() 
    orientation_.pose.pose.orientation = theta
    quaternion = [orientation_.pose.pose.orientation.x, orientation_.pose.pose.orientation.y,orientation_.pose.pose.orientation.z, orientation_.pose.pose.orientation.w]
    euler = tf.transformations.euler_from_quaternion(quaternion)
    angle = euler[2] # Get the yaw angle from the euler angles 
    return angle
            

def update_position_x(x):
    return x

def update_position_y(y):
    return y

class record_odomClass(object):
    _feedback = OdomRecordFeedback()
    _result = OdomRecordResult()
    odom = []

    def __init__(self):
        
        self.server = actionlib.SimpleActionServer('record_odom',OdomRecordAction,self.callback_action,False)
        rospy.sub = rospy.Subscriber('/odom' , Odometry  , self.callback)
        rospy.wait_for_message('/odom' , Odometry)
        self.server.start()
        

    def callback(self,msg):
        self.odom.append(msg.pose.pose)
        rospy.sleep(1)
        
    
    def callback_action(self,goal):

        current_position = Point()
        last_position = Point()

        while (len(self.odom) < 2):
           rospy.loginfo("no enough elements"+str(len(self.odom)))
           rospy.sleep(1)
           
        r =rospy.Rate(1)
        success = True
        distance = 0
        distance_from_home_position = 0
        home_position = Odometry()
        home_position = self.odom[0]
        self._feedback = 0.0
        self._result.list_of_odoms = [] 
        
        rospy.loginfo("OdomRecord action started")
        


        #x and y 
        current_position = Point()
        current_position.x = update_position_x(self.odom[-1].position.y)
        current_position.y =  update_position_y(self.odom[-1].position.y)
        #theta
        current_position.z =  update_theta(self.odom[-1].orientation) 
        
        #store odometry
        my_point = Point()
        my_point = current_position
        self._result.list_of_odoms.append(my_point)

        #start_time
        t_start = rospy.Time.now()

        #assume the robot is not moving 
        last_position = current_position

        while(1):  

            current_position.x = update_position_x(self.odom[-1].position.y)
            current_position.y =  update_position_y(self.odom[-1].position.y)
            
            #theta 
            current_position.z =  update_theta(self.odom[-1].orientation)  


            rospy.logwarn("current:   "+str(round(current_position.x, 5)))
            rospy.logwarn("last:      "+str(round(last_position.x, 5)))

            while round(current_position.x, 5) == round(last_position.x, 5) and \
            round(current_position.y, 5) == round(last_position.y, 5):

                current_position.x = update_position_x(self.odom[-1].position.y)
                current_position.y =  update_position_y(self.odom[-1].position.y)
                #theta 
                current_position.z =  update_theta(self.odom[-1].orientation) 

                rospy.logwarn("current:   "+str(round(current_position.x, 7)))
                rospy.logwarn("last:      "+str(round(last_position.x, 7)))
                rospy.loginfo("the robot is not moving")
                rospy.sleep(1)
            
            if self.server.is_preempt_requested():
                rospy.loginfo("the goal is cnaceled")
                self.server.set_preempted()
                success = False
                break
             
            else:
                
                distance_x = current_position.x - last_position.x
                distance_y = current_position.y - last_position.y
               
                
               
                #distance
                distance += math.sqrt(distance_x**2 + distance_y**2)
                  
                #publish the feedback
                self._feedback = distance
                self.server.publish_feedback(self._feedback)

                #distance from home 
                distance_x_home = (current_position.x) - (home_position.position.x)
                distance_y_home = (current_position.y) - (home_position.position.y)

                distance_from_home_position = math.sqrt(distance_x_home**2 + distance_y_home**2)

                #update position 
                last_position = current_position
                

                #store the recorded odometery 
                my_point.x = current_position
                

                self._result.list_of_odoms.append(my_point)
                
                
                t_before_end = rospy.Time.now()
                
                rospy.logwarn("current:   "+str(round(current_position.x, 5)))
                rospy.logwarn("last:      "+str(round(last_position.x, 5)))

                #rospy.loginfo(t_before_end-t_start)
                if((t_before_end - t_start).to_sec() > 10):
                    rospy.loginfo("about to finish")
                    #see if the turtlebot done a complete loop
                    if (0.3 > round(distance_from_home_position,3) > 0.1):
                            rospy.loginfo(distance_from_home_position)
                            break

                r.sleep()
        
        if success:
            rospy.loginfo("action successd")
            self.server.set_succeeded(self._result)
            


    


if __name__ == '__main__':
    rospy.init_node('action_node')
    record_odomClass()
    rospy.spin()