0
$\begingroup$

how do i use /odom as a subscription in python script, i have a controller file in which i have to publish cmd_vel and then move my model. then i have to get the position and velocity back to move further. i cant seem to find how to get position and theta using /odom. this is the code i have written so far.

def __init__(self):
    super().__init__("controller_car")
  
    self.cmd_vel_pub_ = self.create_publisher(Twist, "/cmd_vel", 10)
    self.odom_sub = self.create_subscription(Odometry, "/odom", self.odometryCb, 10)
    self.vel = Twist()

    self.rate = self.create_rate(100)


    self.car_x = 0
    self.car_y = 0
    self.car_theta = 0
    self.x_goals = [1, -1, -1, 1, 0]
    self.y_goals = [1, 1, -1, -1, 0]
    self.theta_goals = [0, math.pi/2, -math.pi, -math.pi/2, 

    self.cli = self.create_client(NextGoal, 'next_goal')      
    self.req = NextGoal.Request() 
    self.index = 0
     
def odometryCb(self,msg):
    #i have to add the code here, so that i can get values of x,y,theta from /odom
    pass
$\endgroup$

1 Answer 1

0
$\begingroup$

To access position values as in x-y-z:

   pos_x = msg.pose.pose.position.x
   pos_y = msg.pose.pose.position.y
   pos_z = msg.pose.pose.position.z

To get theta/yaw (assuming you want the values in euler format, not quaternion which are given by default):

   or_x  = msg.pose.pose.orientation.x
   or_y  = msg.pose.pose.orientation.y
   or_z  = msg.pose.pose.orientation.z
   or_w  = msg.pose.pose.orientation.w
   # To transform these to euler format
   roll, pitch, yaw = euler_from_quaternion([or_x, or_y, or_z, or_w])

The function euler_from_quaternion can be used by:

   from tf.transformations import euler_from_quaternion
$\endgroup$

Your Answer

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

Not the answer you're looking for? Browse other questions tagged or ask your own question.