def getTransform(self):
try:
#transReal = self.tfBuffer.lookup_transform('odom', 'body', self.get_clock().now(), timeout=Duration(seconds=1.0))
trans = self.tfBuffer.lookup_transform('base_link', 'body',
tf2_ros.Time())
pos = self.tfBuffer.lookup_transform('base_link', 'odom',
tf2_ros.Time())
self.f.write("{}, {}, {}, {}\n".format(
trans.transform.translation.x, trans.transform.translation.y,
pos.transform.translation.x, pos.transform.translation.y))
except Exception as e:
self.get_logger().error(str(e))
def getTheta(self):
transform = self.tfBuffer.lookup_transform('odom', 'base_link', tf2_ros.Time())
q = transform.transform.rotation
siny_cosp = 2 * (q.w * q.z + q.x * q.y)
cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z)
yaw = np.arctan2(siny_cosp, cosy_cosp)
return yaw
def getPos(self):
transform = self.tfBuffer.lookup_transform('odom', 'base_link',
tf2_ros.Time())
return np.array([
transform.transform.translation.x,
transform.transform.translation.y
])
def obj_callback(self, msg):
A = (msg.x, msg.y)
cout("Got target")
transform = self.tfBuffer.lookup_transform('odom', 'base_link',
tf2_ros.Time())
self.pos = (transform.transform.translation.x,
transform.transform.translation.y)
if not self.terrain.contains(Point(A)):
cout("Objectif not in safe zone")
#box = Point(A).buffer(2*self.l_rob)
box = Polygon(
rect_from_center(3 * self.l_rob, 3 * self.l_rob, A[0], A[1]))
self.polys = self.polys.union(box)
#pts, d = self.rec_path(self.pos, A, 0)
I = self.terrain.exterior.intersection(box.exterior)
if len(I.geoms) == 2:
B = I.geoms[0]
C = I.geoms[1]
if cdist(
np.array(self.pos).reshape(1, -1),
np.array(B).reshape(1, -1)) > cdist(
np.array(self.pos).reshape(1, -1),
np.array(C).reshape(1, -1)):
B, C = C, B
pts, d1 = self.rec_path(self.pos, B, 0)
if pts != None:
pts.append(C)
self.path = LineString(pts)
self.send_wps()
# if pts != None:
# I = self.terrain.exterior.intersection(box.exterior)
# for B in I:
# cout(B)
# self.path = LineString(pts)
else:
pts, d = self.rec_path(self.pos, A, 0)
if pts != None:
self.path = LineString(pts)
self.send_wps()
def getTransform(self):
try:
#transReal = self.tfBuffer.lookup_transform('odom', 'body', self.get_clock().now(), timeout=Duration(seconds=1.0))
transform = self.tfBuffer.lookup_transform('odom', 'base_link', tf2_ros.Time())
q = transform.transform.rotation
siny_cosp = 2 * (q.w * q.z + q.x * q.y)
cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z)
yaw = arctan2(siny_cosp, cosy_cosp)
u = self.control(transform.transform.translation.x, transform.transform.translation.y, yaw, self.a, self.b)
m = Twist()
m.angular.z = u
m.linear.x = float(self.vel)
self.pub_cmd.publish(m)
except Exception as e:
self.get_logger().error(str(e))
