0
$\begingroup$

Rosanswers logo

Hi, I am new to python and ros and i am trying to get to different goals defined by me but I get the error: list imdex out of range and i dont know why because as far as i know, the list index is not out of range. my code is below and also the error:

#!/usr/bin/env python
import rospy
import math
from std_msgs.msg import String
from std_msgs.msg import Int32
from geometry_msgs.msg import Twist          # Import the Twist message from the geometry_msgs
from sensor_msgs.msg import LaserScan       # Import the LaserScan message from the sensor
from gazebo_msgs.msg import ModelStates  #import ModelState message from gazebo_msgs
from geometry_msgs.msg import Pose,Point         # Import the Pose message from the geometry_msgs
from tf.transformations import euler_from_quaternion
from goal_publisher.msg import PointArray
from math import pow, atan2, sqrt


PI= 3.1415926535897
repeat = 0
theta = 0.0
x = 0.0
y = 0.0
msg = 0.0
index_same=0.0
target = 90
kp=0.5

class MovetoGoals:
    def __init__(self):

        # Creates a node with name 'final_projekt_node' and make sure it is a
        # unique node (using anonymous=True).
        rospy.init_node('final_projekt_node', anonymous=True)

        # Publisher which will publish to the topic 'Twist'.
        self.pub = rospy.Publisher('cmd_vel',Twist, queue_size=10)

        # A subscriber to the topic 'Modelstate'. self.update_pose is called
        # when a message of type Pose is received.
        self.pose_sub = rospy.Subscriber('/gazebo/model_states',ModelStates, self.update_pose)
        self.goals_sub = rospy.Subscriber('/goals',PointArray,self.get_goals)
        self.dist_sub = rospy.Subscriber('/distance',Int32,self.distance_tracker)
        self.laser_sub = rospy.Subscriber('scan', LaserScan,self.detect_obstacle)# subscriber to subscribe to laser messages
        self.pose = ModelStates()
        self.current_distance = Int32()
        self.the_goals = PointArray()
        self.rate = rospy.Rate(10)

    def update_pose(self, data):
        """Callback function which is called when a new message of type Pose is
        received by the subscriber."""
        #print('in update_pose function')

        self.pose = data
        self.pose.pose[1].position.x = round(self.pose.pose[1].position.x, 4)
        self.pose.pose[1].position.y = round(self.pose.pose[1].position.y, 4)

        #print ('robots x position' self.pose.pose[1].position.x)

    def euclidean_distance(self,goal_pose):

        return sqrt(pow((goal_pose.x - self.pose.pose[1].position.x), 2) +
                    pow((goal_pose.y - self.pose.pose[1].position.y), 2))

    def linear_vel(self,goal_pose, constant=1):

        #return constant * (self.euclidean_distance(goal_pose))
        return abs(1.0)

    def steering_angle(self, goal_pose):

        return atan2(goal_pose.y - self.pose.pose[1].position.y,goal_pose.x - self.pose.pose[1].position.x)

    def angular_vel(self,goal_pose, constant=2):

        global theta
        self.pose.pose[1].orientation = self.pose.pose[1].orientation
        (roll, pitch, theta) = euler_from_quaternion([self.pose.pose[1].orientation.x, self.pose.pose[1].orientation.y, self.pose.pose[1].orientation.z, self.pose.pose[1].orientation.w])
        #print ('theta at update pose is', + theta)
        #print ('theta is', + theta)
        return constant * (self.steering_angle(goal_pose) - theta)
        #return 0.3

    def get_goals(self,the_goals):
        global x
        global y
        x = the_goals.goals[0].x
        y = the_goals.goals[0].y
        #print x
        #print y

    def distance_tracker(self,distance):
        self.current_distance = distance.data


    def detect_obstacle(self,laser_msg):
        global msg
        msg = laser_msg.ranges[0]
        #index_same = all(ele == laser_msg.ranges[0] for ele in laser_msg.ranges[0:10])

    #function to rotate robot approx. 90 degrees
    def rotate(self):
        vel_msg = Twist()
        print("in rotate function")
        speed = 30
        angle = 90
        angular_speed = speed*2*PI/360
        relative_angle = angle*2*PI/360

        vel_msg.linear.x=0
        vel_msg.linear.y=0
        vel_msg.linear.z=0
        vel_msg.angular.x = 0
        vel_msg.angular.y = 0

        vel_msg.angular.z = -abs(angular_speed)

        t0 = rospy.Time.now().to_sec()
        current_angle = 0

        while(current_angle < relative_angle):
            self.pub.publish(vel_msg)
            t1 = rospy.Time.now().to_sec()
            current_angle = angular_speed*(t1-t0)

        vel_msg.angular.z = 0
        self.pub.publish(vel_msg)

    def move2goal(self):
        """Moves the turtle to the goal."""
        goal_pose = Point()
        #goal_pose.x = input("Set your x goal: ")
        #goal_pose.y = input("Set your y goal: ")
        goal_pose.x = 3.5
        goal_pose.y = 9.0


        # Please, insert a number slightly greater than 0 (e.g. 0.01).
        distance_tolerance = 0.1

        vel_msg = Twist()

        while True:
            if self.euclidean_distance(goal_pose) >= distance_tolerance and msg > 1.5:
                print msg
                print self.current_distance

                # Linear velocity in the x-axis.
                vel_msg.linear.x = self.linear_vel(goal_pose)
                vel_msg.linear.y = 0
                vel_msg.linear.z = 0

                # Angular velocity in the z-axis.
                vel_msg.angular.x = 0
                vel_msg.angular.y = 0
                vel_msg.angular.z = self.angular_vel(goal_pose)

            elif self.euclidean_distance(goal_pose) < distance_tolerance and msg > 1.5:
                print('goal reached')
                break

            else:
                print('obstacle detected')
                vel_msg.linear.x = 0
                #target_rad = target*math.pi/180
                #vel_msg.linear.z = kp*(target_rad-theta)
                self.rotate()

                # Publishing our vel_msg

            self.pub.publish(vel_msg)


            # Publish at the desired rate.
            self.rate.sleep()

            # Stopping our robot after the movement is over.
        print('stopping robot')
        vel_msg.linear.x = 0
        vel_msg.angular.z = 0
        self.pub.publish(vel_msg)

        rospy.spin()


if __name__ == '__main__':
    try:
        object = MovetoGoals()
        object.move2goal()
    except rospy.ROSInterruptException:
        pass

The error is:

core service [/rosout] found
process[final_projekt_node-1]: started with pid [17840]
process[goal_publisher-2]: started with pid [17841]
process[track-3]: started with pid [17842]
[INFO] [1560706012.005172, 0.000000]: Configuration file name: config.yaml
Traceback (most recent call last):
  File "/home/amr/catkin_ws/src/oe-161016_tier4/oe_161016_prj/src/turtle_sim.py", line 207, in <module>
    object.move2goal()
  File "/home/amr/catkin_ws/src/oe-161016_tier4/oe_161016_prj/src/turtle_sim.py", line 143, in move2goal
    if self.euclidean_distance(goal_pose) >= distance_tolerance and msg > 1.5:
  File "/home/amr/catkin_ws/src/oe-161016_tier4/oe_161016_prj/src/turtle_sim.py", line 61, in euclidean_distance
    return sqrt(pow((goal_pose.x - self.pose.pose[1].position.x), 2) +
IndexError: list index out of range

The structure of the ModelStates topic So the list is made up of more different positions and orientations of obejects in the gazebo world, like the robot e.t.c and I did rostopic echo to make sure and i got this

pose: 
  
    position: 
      x: 0.0
      y: 0.0
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
  
    position: 
      x: -3.40717160297e-05
      y: 9.40266793714e-05
      z: -0.00100139287064
    orientation: 
      x: -9.90025971612e-06
      y: 0.00385309557022
      z: 0.000737195142211
      w: 0.99999230502
twist: 
  
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  
    linear: 
      x: 7.42159494544e-08
      y: 6.63937672672e-07
      z: 3.5974708812e-05
    angular: 
      x: 3.90254538909e-05
      y: -0.000180313959702
      z: 9.82618596517e-06

so please, how is the list index out of range?

The error is in def euclidean_distance.

thanks


Originally posted by Jojo on ROS Answers with karma: 3 on 2019-06-17

Post score: 0


Original comments

Comment by jayess on 2019-06-17:
Please make sure to use the preformatted text (101010) button when posting code or terminal output. It makes your question much easier to read when you use it

Comment by jayess on 2019-06-17:
What topic are you showing exactly? Can you please update your question with the output of

rostopic echo /gazebo/model_states

with just one message of the topic printed? Something doesn't look right because when I do it, i get

name: [ground_plane]
pose: 
  - 
    position: 
      x: 0.0
      y: 0.0
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
twist: 
  - 
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0

notice the - in there which is YAML syntax for a list.

Comment by Jojo on 2019-06-20:
Thank you for edting my question and the tips on posting a code here. I have solved the problem with the answer given below.

Comment by jayess on 2019-06-20:
If that answered your question, then please click the to the left of the answer to mark it as correct.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

You need to allow some time for poses to be received from Gazebo before you try to use them. Although you set up the subscriber for /gazebo/model_states in the MovetoGoals object's constructor, you do not give ROS any time to receive anything on the topic before you start calling move2goal which internally via the euclidean_distance function tries to use the value that will be set by update_pose. This means that update_pose has not yet been called when euclidean_distance is called so it tries to access a pose in the empty ModelStates object's array.

To solve this, you need to give some time to ROS to receive the data you want to use before you start trying to send movement commands based on received data. Add a function that allows ROS to receive a few methods (e.g. creating a rate object of 10 Hz and doing a rate.sleep() on it) until you see that the size of the ModelStates object has data.

Also make sure that the second pose in the list is the one you want. In Python, list indices are zero-based.


Originally posted by Geoff with karma: 4203 on 2019-06-17

This answer was ACCEPTED on the original site

Post score: 2


Original comments

Comment by Jojo on 2019-06-20:
Solved it .Thanks

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.