Ask Your Question
0

Processing PoseStamped message as an integer (or something similar)

asked 2020-06-25 12:30:14 -0500

bionic_socks gravatar image

Hello,

I'm using ROS melodic on Ubuntu 18.04 and I'm having some problems using an absolute tolerance function called isclose() with a PoseStamped message. When I try to run this code I get the error:

x_tol = isclose(vehicle_x, x_target, abs_tol=0.25)
  File "./example_code.py", line 13, in isclose
    return abs(a-b) <= max(rel_tol * max(abs(a), abs(b)), abs_tol)
TypeError: unsupported operand type(s) for -: 'PoseStamped' and 'int'

I included just a small sample of what I am trying to do, just for clarity sake. Towards the bottom you'll notice the intVehicleX function, which obviously doesn't work, but I think it gets across what I am trying to acomplish. Anyone have any ideas of how I should approach this? Thanks in advance.

#! /usr/bin/python

import rospy
import numpy as np
from geometry_msgs.msg import PoseStamped, TwistStamped

def isclose(a, b, rel_tol=1e-09, abs_tol=0.0):
    return abs(a-b) <= max(rel_tol * max(abs(a), abs(b)), abs_tol)

def local_pose_callback(msg):
    global quad_pose_x, quad_pose_y, quad_pose_z
    vehicle_x = msg.pose.position.x
    vehicle_y = msg.pose.position.y
    vehicle_z = msg.pose.position.z

rospy.init_node('practice', anonymous=False)

vehicle_x = PoseStamped()
vehicle_y = PoseStamped()
vehicle_z = PoseStamped()

x_target = int(raw_input('x = '))
y_target = int(raw_input('y = '))

intVehicleX= int(vehicle_x)

x_tol = isclose(int_vehicle_x, x_target, abs_tol=0.25)

pose_sub = rospy.Subscriber('/mavros/local_position/pose', PoseStamped,
                            local_pose_callback, queue_size = 10)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-06-27 06:05:54 -0500

Weasfas gravatar image

Hi @bionic_socks,

First of all you will need to know that the PoseStamped msg type already contains the Pose of the robot, that means, position (x,y,z) and orientation (x,y,z,w) in quaternion form. So knowing that, your example is not coherent with this since you are using three PoseStamped objects to store individual pose coordinates. Lets assume you are using only one: vehicle_pose in which you store the full msg refering to the current robot pose.

As the error says, python interpreter is not able to compare the different data types int and PoseStamped, and the static cast to int will not work since the msg instance itself is not a number. Since, with that example, you are working only with x coordinates you will need to extract from the PoseStamped msg, the proper pose coordinate and feed it to your function. Here is a minimal working example of what I think you are trying to achieve:

#! /usr/bin/python

import rospy
import numpy as np
from geometry_msgs.msg import PoseStamped, TwistStamped

vehicle_pose = PoseStamped()

def isclose(a, b, rel_tol=1e-09, abs_tol=0.0):
    return abs(a-b) <= max(rel_tol * max(abs(a), abs(b)), abs_tol)

def local_pose_callback(msg):
    global vehicle_pose
    vehicle_pose = msg

rospy.init_node('practice', anonymous=False)

x_target = int(raw_input('x = '))
y_target = int(raw_input('y = '))

int_vehicle_x= int(vehicle_pose.pose.position.x)

x_tol = isclose(int_vehicle_x, x_target, abs_tol=0.25)

print("isclose returned: " + str(x_tol))

pose_sub = rospy.Subscriber('/mavros/local_position/pose', PoseStamped,
                            local_pose_callback, queue_size = 10)

Hope that helps.

edit flag offensive delete link more

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: 2020-06-25 12:30:14 -0500

Seen: 18 times

Last updated: Jun 27