Processing PoseStamped message as an integer (or something similar)
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)