def main():
rospy.init_node("pub_pose_array")
publisher = rospy.Publisher("pose_array", PoseArray, queue_size=1)
center = (1, 0)
radius = 2
pose = Pose2D(center[0] + radius * np.cos(0),
center[1] + radius * np.sin(0), np.pi / 2)
angle = np.pi / 18.
moved = Pose2D(center[0] + radius * np.cos(angle),
center[1] + radius * np.sin(angle), np.pi / 2 + angle)
motion = pose.motion_to(moved)
rate = rospy.Rate(2)
pose_arr = []
count = 0
while not rospy.is_shutdown():
pose3d = pose2d_to_3d(pose)
count += 1
pose3d.position.z = count * 0.01
header = Header(stamp=rospy.get_rostime(), frame_id="map")
pose_arr.append(pose3d)
message = PoseArray(header=header, poses=pose_arr)
publisher.publish(message)
pose = pose.move(motion)
rate.sleep()
def tracked_persons_callback(self, msg):
if self.refmap_manager.tf_frame_in_refmap is None:
# localization not available yet
return
if self.goal_xy_in_refmap is None:
# goal not available yet
return
pose2d_tracksframe_in_refmap = None
if msg.header.frame_id != self.refmap_manager.kRefMapFrame:
# get tf
try:
tf_frame_in_refmap = self.tf_listener.lookupTransform(
self.refmap_manager.kRefMapFrame, msg.header.frame_id,
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
rospy.logwarn_throttle(10., e)
raise ValueError(e)
# set for future use
pose2d_tracksframe_in_refmap = Pose2D(tf_frame_in_refmap)
robot_pos_in_refmap = Pose2D(self.refmap_manager.tf_frame_in_refmap)
robot_radius = self.kRobotRadius
robot_goal_xy_in_refmap = self.goal_xy_in_refmap
# state update
self.state_estimator_node.tracked_persons_update(
msg,
pose2d_tracksframe_in_refmap,
self.refmap_manager.map_8ds,
robot_pos_in_refmap,
robot_radius,
robot_goal_xy_in_refmap,
)
def visuals_loop(self):
while True:
xx, yy, clusters, is_legs, cogs, radii = self.clusters_data
# past positions
tf_past_in_rob = []
if self.tf_pastrobs_in_fix:
tf_rob_in_fix = self.tf_pastrobs_in_fix[-1]
for i, tf_a in enumerate(self.tf_pastrobs_in_fix[::-1]):
tf_b = apply_tf_to_pose(
Pose2D(tf_a), inverse_pose2d(Pose2D(tf_rob_in_fix)))
tf_past_in_rob.append(tf_b)
# Plotting
plt.figure("clusters")
plt.cla()
plt.scatter(xx,
yy,
zorder=1,
facecolor=(1, 1, 1, 1),
edgecolor=(0, 0, 0, 1),
color='k',
marker='.')
# for x, y in zip(xx, yy):
# plt.plot([0, x], [0, y], linewidth=0.01, color='red' , zorder=2)
plt.scatter([0], [0], marker='+', color='k', s=1000)
for c in clusters:
plt.plot(xx[c], yy[c], zorder=2)
for l, cog, r in zip(is_legs, cogs, radii):
if l:
patch = patches.Circle(cog,
r,
facecolor=(0, 0, 0, 0),
edgecolor=(0, 0, 0, 1),
linewidth=3)
else:
patch = patches.Circle(cog,
r,
facecolor=(0, 0, 0, 0),
edgecolor=(0, 0, 0, 1),
linestyle='--')
plt.gca().add_artist(patch)
if tf_past_in_rob:
xy_past = np.array(tf_past_in_rob)
plt.plot(xy_past[:, 0], xy_past[:, 1], color='red')
plt.gca().set_aspect('equal', adjustable='box')
LIM = 10
plt.ylim([-LIM, LIM])
plt.xlim([-LIM, LIM])
# pause
plt.pause(0.1)
rospy.timer.sleep(0.01)
def visuals_loop(self):
while True:
with self.lock:
if self.transport_data is None:
plt.pause(0.1)
rospy.timer.sleep(0.01)
continue
_, xx, yy, clusters, is_legs, cogs, radii, tf_rob_in_fix = self.transport_data
# transform data to fixed frame
pose2d_rob_in_fix = Pose2D(tf_rob_in_fix)
xy_f = apply_tf(np.vstack([xx, yy]).T, pose2d_rob_in_fix)
cogs_f = apply_tf(np.array(cogs), pose2d_rob_in_fix)
# past positions
pose2d_past_in_fix = []
if self.tf_pastrobs_in_fix:
for tf_a in self.tf_pastrobs_in_fix[::-1]:
pose2d_past_in_fix.append(Pose2D(tf_a))
# Plotting
plt.figure("clusters")
plt.cla()
# Robot
plt.scatter([pose2d_rob_in_fix[0]],[pose2d_rob_in_fix[1]],marker='+', color='k',s=1000)
# Lidar
# plt.scatter(xy_f[:,0], xy_f[:,1], zorder=1, facecolor=(1,1,1,1), edgecolor=(0,0,0,1), color='k', marker='.')
# for x, y in zip(xx, yy):
# plt.plot([0, x], [0, y], linewidth=0.01, color='red' , zorder=2)
# clusters
# for c in clusters:
# plt.plot(xx[c], yy[c], zorder=2)
for l, cog, r in zip(is_legs,cogs_f, radii):
if l:
# patch = patches.Circle(cog, r, facecolor=(0,0,0,0), edgecolor=(0,0,0,1), linewidth=3)
# plt.gca().add_artist(patch)
plt.scatter([cog[0]], [cog[1]], marker='+', color='green', s=500)
else:
patch = patches.Circle(cog, r, facecolor=(0,0,0,0), edgecolor=(0,0,0,1), linestyle='--')
plt.gca().add_artist(patch)
if pose2d_past_in_fix:
xy_past = np.array(pose2d_past_in_fix)
plt.plot(xy_past[:,0], xy_past[:,1], color='red')
# Tracks
with self.lock:
self.tracker.vizualize_tracks()
# Display options
plt.gca().set_aspect('equal',adjustable='box')
# LIM = 10
# plt.ylim([-LIM, LIM])
# plt.xlim([-LIM, LIM])
# pause
plt.pause(0.1)
rospy.timer.sleep(0.01)
def global_waypoint_callback(self, msg):
""" If a global path is received (in map frame), try to track it """
with self.lock:
waypoint_in_msg = np.array([
msg.pose.position.x,
msg.pose.position.y,
])
## msg frame to fixed frame
if self.kFixedFrame != msg.header.frame_id:
try:
tf_msg_in_fix = self.tf_listener.lookupTransform(
self.kFixedFrame, msg.header.frame_id,
msg.header.stamp - rospy.Duration(0.1))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
print(
"Could not find transform from waypoint frame to fixed."
)
print(e)
return
waypoint_in_fix = apply_tf(waypoint_in_msg,
Pose2D(tf_msg_in_fix))
else:
waypoint_in_fix = waypoint_in_msg
self.tf_goal_in_fix = (
np.array([waypoint_in_fix[0], waypoint_in_fix[1],
0.]), # trans
tf.transformations.quaternion_from_euler(0, 0, 0)) # quat
def set_goal_routine(self, event=None):
if self.latest_goal_msg is None:
return
if self.refmap_manager.tf_frame_in_refmap is None:
rospy.logwarn_throttle(
1.,
"IA: Reference map transform ({} -> {}) not available yet.".
format(
self.refmap_manager.kRefMapFrame,
self.refmap_manager.kFrame,
))
return
# goal might not be in reference map frame. find goal_xy in refmap frame
try:
time = rospy.Time.now()
tf_info = [
self.refmap_manager.kRefMapFrame,
self.latest_goal_msg.header.frame_id, time
]
self.tf_listener.waitForTransform(*(tf_info + [self.tf_timeout]))
tf_msg_in_refmap = self.tf_listener.lookupTransform(*tf_info)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException, TransformException) as e:
rospy.logwarn(
"[{}.{}] tf to refmap frame for time {}.{} not found: {}".
format(rospy.Time.now().secs,
rospy.Time.now().nsecs, time.secs, time.nsecs, e))
return
pose2d_msg_in_refmap = Pose2D(tf_msg_in_refmap)
goal_xy = apply_tf(
np.array([
self.latest_goal_msg.pose.position.x,
self.latest_goal_msg.pose.position.y
]), pose2d_msg_in_refmap)
# goal ij
goal_ij = self.refmap_manager.map_8ds.xy_to_ij(
[goal_xy[:2]], clip_if_outside=False)[0]
if not self.refmap_manager.map_8ds.is_inside_ij(
np.array([goal_ij], dtype=np.float32)):
rospy.logwarn(
"Goal (i,j : {}, {}) is outside of reference map (size: {}, {})"
.format(goal_ij[0], goal_ij[1],
*self.refmap_manager.map_8ds.occupancy().shape))
return
if self.goal_xy_in_refmap is not None:
if not np.allclose(self.goal_ij_in_refmap, goal_ij):
self.replan_due_to_new_goal_flag[0] = True
rospy.loginfo("Global goal has changed.")
self.goal_xy_in_refmap = goal_xy
self.goal_ij_in_refmap = goal_ij
# Goal as marker
marker = self.goal_as_marker(goal_xy)
goal_pub = rospy.Publisher("/ia_planner/goal", Marker, queue_size=1)
goal_pub.publish(marker)
self.goal_is_reached = False
rospy.loginfo("New global goal: {}[meters], {}[ij]".format(
goal_xy, goal_ij))
self.latest_goal_msg = None
def main():
rospy.init_node("pub_moving_pose")
publisher = rospy.Publisher("moving_pose", PoseStamped, queue_size=1)
center = (1, 0)
radius = 2
pose = Pose2D(center[0] + radius * np.cos(0), center[1] + radius * np.sin(0), np.pi / 2)
angle = np.pi / 18.
moved = Pose2D(center[0] + radius * np.cos(angle), center[1] + radius * np.sin(angle), np.pi / 2 + angle)
motion = pose.motion_to(moved)
rate = rospy.Rate(2)
while not rospy.is_shutdown():
pose3d = pose2d_to_3d(pose)
header = Header(stamp=rospy.get_rostime(), frame_id="map")
message = PoseStamped(header=header, pose=pose3d)
publisher.publish(message)
pose = pose.move(motion)
rate.sleep()
def turn_to_face_goal_callback(self, event=None):
with self.lock:
if self.plan_executor_node.task_being_executed is not None:
return
if self.refmap_manager.tf_frame_in_refmap is None:
return
if self.goal_xy_in_refmap is None:
return
p2_refmap_in_robot = inverse_pose2d(
Pose2D(self.refmap_manager.tf_frame_in_refmap))
goal_xy_in_robot = apply_tf(self.goal_xy_in_refmap,
p2_refmap_in_robot)
rospy.loginfo_throttle(5., "turning to face goal")
gx, gy = goal_xy_in_robot
angle_to_goal = np.arctan2(gy, gx) # [-pi, pi]
w = 0
if np.abs(angle_to_goal) > ((np.pi / 4) / 10): # deadzone
w = 0.3 * np.sign(angle_to_goal)
if not self.STOP:
cmd_vel_msg = Twist()
cmd_vel_msg.angular.z = w
self.cmd_vel_pub.publish(cmd_vel_msg)
def waypoint_callback(self, msg):
self.tf_timeout = rospy.Duration(0.1)
""" If a global path is received (in map frame), try to track it """
with self.lock:
ref_frame = msg.header.frame_id
wpt_ref_xy = [msg.pose.position.x, msg.pose.position.y]
try:
time = rospy.Time.now()
tf_info = [self.kFixedFrame, msg.header.frame_id, time]
self.tf_listener.waitForTransform(*(tf_info +
[self.tf_timeout]))
tf_ref_in_fix = self.tf_listener.lookupTransform(*tf_info)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException, TransformException) as e:
rospy.logwarn(
"[{}.{}] tf to refmap frame for time {}.{} not found: {}".
format(rospy.Time.now().secs,
rospy.Time.now().nsecs, time.secs, time.nsecs, e))
return
wpt_fix_xy = apply_tf(np.array(wpt_ref_xy), Pose2D(tf_ref_in_fix))
self.tf_wpt_in_fix = (
np.array([wpt_fix_xy[0], wpt_fix_xy[1], 0.]), # trans
tf.transformations.quaternion_from_euler(0, 0, 0)) # quat
def planner_routine(self, event=None):
nowstamp = rospy.Time.now()
if self.tf_wpt_in_fix is None:
rospy.logwarn_throttle(10., "Global path not available yet")
return
if self.tf_rob_in_fix is None:
rospy.logwarn_throttle(10., "tf_rob_in_fix not available yet")
return
if self.odom is None:
rospy.logwarn_throttle(10., "odom not available yet")
return
# remove old obstacles from buffer
k2s = 2.
with self.lock:
tic = timer()
dynamic_obstacles = []
self.obstacles_buffer = [
obst_msg for obst_msg in self.obstacles_buffer
if nowstamp - obst_msg.header.stamp < rospy.Duration(k2s)
]
# DEBUG remove
# for obst_msg in self.obstacles_buffer:
for obst_msg in self.obstacles_buffer[-1:]:
dynamic_obstacles.extend(obst_msg.obstacles)
# take into account static obstacles if not old
static_obstacles = []
if self.static_obstacles_buffer is not None:
if (nowstamp - self.static_obstacles_buffer.header.stamp
) < rospy.Duration(k2s):
static_obstacles = self.static_obstacles_buffer.obstacles
# vel to goal in fix
goal_in_fix = np.array(Pose2D(self.tf_wpt_in_fix)[:2])
toc = timer()
print("Lock preproc {:.1f} ms".format((toc - tic) * 1000.))
# robot position
tf_rob_in_fix = self.tf_rob_in_fix
tic = timer()
# RVO set up
positions = []
radii = []
velocities = []
obstacles = []
metadata = [] # agent metadata
# create agents for RVO
# first agent is the robot
positions.append(Pose2D(tf_rob_in_fix)[:2])
radii.append(self.kRadius)
velocities.append(
[self.odom.twist.twist.linear.x, self.odom.twist.twist.linear.y])
metadata.append({'source': 'robot'})
# create agent for every dynamic obstacle
for obst in dynamic_obstacles:
positions.append(
[obst.polygon.points[0].x, obst.polygon.points[0].y])
radii.append(obst.radius)
velocities.append([
obst.velocities.twist.linear.x, obst.velocities.twist.linear.y
])
metadata.append({'source': 'dynamic'})
# create agents for local static obstacles (table leg, chair, etc)
for obst in static_obstacles:
if obst.radius > 0.5:
continue # TODO big walls make big circles : but dangerous to do this!
positions.append(
[obst.polygon.points[0].x, obst.polygon.points[0].y])
radii.append(obst.radius)
velocities.append([
obst.velocities.twist.linear.x, obst.velocities.twist.linear.y
])
metadata.append({'source': 'nonleg'})
# create static obstacle vertices from refmap if available (walls, etc)
if self.refmap_manager.tf_frame_in_refmap is not None:
kMapObstMaxDist = 3.
obstacles_in_ref = self.refmap_manager.map_as_closed_obstacles
pose2d_ref_in_fix = inverse_pose2d(
Pose2D(self.refmap_manager.tf_frame_in_refmap))
near_obstacles = [
apply_tf(vertices, pose2d_ref_in_fix)
for vertices in obstacles_in_ref if len(vertices) > 1
]
near_obstacles = [
vertices for vertices in near_obstacles if np.mean(
np.linalg.norm(vertices - np.array(positions[0]), axis=1))
< kMapObstMaxDist
]
obstacles = near_obstacles
toc = timer()
print("Pre-RVO {:.1f} ms".format((toc - tic) * 1000.))
tic = timer()
# RVO ----------------
import rvo2
t_horizon = 10.
DT = 1 / 10.
#RVOSimulator(timeStep, neighborDist, maxNeighbors, timeHorizon, timeHorizonObst, radius, maxSpeed, velocity = [0,0]);
sim = rvo2.PyRVOSimulator(DT, 1.5, 5, 1.5, 2, 0.4, 2)
# Pass either just the position (the other parameters then use
# the default values passed to the PyRVOSimulator constructor),
# or pass all available parameters.
agents = []
for p, v, r, m in zip(positions, velocities, radii, metadata):
#addAgent(posxy, neighborDist, maxNeighbors, timeHorizon, timeHorizonObst, radius, maxSpeed, velocity = [0,0]);
a = sim.addAgent(tuple(p), radius=r, velocity=tuple(v))
agents.append(a)
# static agents can't move
if m['source'] != 'robot' and np.linalg.norm(v) == 0:
sim.setAgentMaxSpeed(a, 0)
# Obstacle(list of vertices), anticlockwise (clockwise for bounding obstacle)
for vert in obstacles:
o1 = sim.addObstacle(list(vert))
sim.processObstacles()
toc = timer()
print("RVO init {:.1f} ms".format((toc - tic) * 1000.))
tic = timer()
n_steps = int(t_horizon / DT)
pred_tracks = [[] for a in agents]
pred_vels = [[] for a in agents]
pred_t = []
for step in range(n_steps):
for i in range(len(agents)):
# TODO: early stop if agent 0 reaches goal
a = agents[i]
pref_vel = velocities[
i] # assume agents want to keep initial vel
if i == 0:
vector_to_goal = goal_in_fix - np.array(
sim.getAgentPosition(a))
pref_vel = self.kIdealVelocity * vector_to_goal / np.linalg.norm(
vector_to_goal)
sim.setAgentPrefVelocity(a, tuple(pref_vel))
pred_tracks[i].append(sim.getAgentPosition(a))
pred_vels[i].append(sim.getAgentVelocity(a))
pred_t.append(1. * step * DT)
sim.doStep()
# -------------------------
toc = timer()
print("RVO steps {} - {} agents - {} obstacles".format(
step, len(agents), len(obstacles)))
print("RVO sim {:.1f} ms".format((toc - tic) * 1000.))
# PLOT ---------------------------
# self.transport_data = (sim, agents, metadata, radii, obstacles, pred_tracks)
# -----------------------------
# publish predicted tracks as paths
rob_track = pred_tracks[0]
pub = rospy.Publisher("/rvo_robot_plan", Path, queue_size=1)
path_msg = Path()
path_msg.header.stamp = nowstamp
path_msg.header.frame_id = self.kFixedFrame
for pose, t in zip(rob_track, pred_t):
pose_msg = PoseStamped()
pose_msg.header.stamp = nowstamp + rospy.Duration(t)
pose_msg.header.frame_id = path_msg.header.frame_id
pose_msg.pose.position.x = pose[0]
pose_msg.pose.position.y = pose[1]
path_msg.poses.append(pose_msg)
pub.publish(path_msg)
# publish tracks
pub = rospy.Publisher("/rvo_simulated_tracks",
MarkerArray,
queue_size=1)
ma = MarkerArray()
id_ = 0
end_x = [sim.getAgentPosition(agent_no)[0] for agent_no in agents]
end_y = [sim.getAgentPosition(agent_no)[1] for agent_no in agents]
end_t = pred_t[-1]
for i in range(len(agents)):
color = (0, 0, 0, 1) # black
color = (1, .8, 0,
1) if metadata[i]['source'] == 'robot' else color # green
color = (1, 0, 0,
1) if metadata[i]['source'] == 'dynamic' else color # red
color = (0, 0, 1,
1) if metadata[i]['source'] == 'nonleg' else color # blue
# track line
mk = Marker()
mk.lifetime = rospy.Duration(0.1)
mk.header.frame_id = self.kFixedFrame
mk.ns = "tracks"
mk.id = i
mk.type = 4 # LINE_STRIP
mk.action = 0
mk.scale.x = 0.02
mk.color.r = color[0]
mk.color.g = color[1]
mk.color.b = color[2]
mk.color.a = color[3]
mk.frame_locked = True
xx = np.array(pred_tracks[i])[:, 0]
yy = np.array(pred_tracks[i])[:, 1]
for x, y, t in zip(xx, yy, pred_t):
pt = Point()
pt.x = x
pt.y = y
pt.z = t / t_horizon
mk.points.append(pt)
ma.markers.append(mk)
# endpoint
r = radii[i]
mk = Marker()
mk.lifetime = rospy.Duration(0.1)
mk.header.frame_id = self.kFixedFrame
mk.ns = "endpoints"
mk.id = i
mk.type = 3 # CYLINDER
mk.action = 0
mk.scale.x = r * 2.
mk.scale.y = r * 2.
mk.scale.z = 0.1
mk.color.r = color[0]
mk.color.g = color[1]
mk.color.b = color[2]
mk.color.a = color[3]
mk.frame_locked = True
mk.pose.position.x = end_x[i]
mk.pose.position.y = end_y[i]
mk.pose.position.z = end_t / t_horizon
ma.markers.append(mk)
pub.publish(ma)
pred_rob_vel_in_fix = np.array(
pred_vels[0]
[1]) # 2 is a cheat because 0 is usually the current speed
pred_end_in_fix = np.array([end_x[0], end_y[0]])
# in robot frame
pose2d_fix_in_rob = inverse_pose2d(Pose2D(tf_rob_in_fix))
pred_rob_vel_in_rob = apply_tf_to_vel(
np.array(list(pred_rob_vel_in_fix) + [0]), pose2d_fix_in_rob)[:2]
pred_end_in_rob = apply_tf(pred_end_in_fix, pose2d_fix_in_rob)
# resulting command vel, and path
best_u, best_v = pred_rob_vel_in_rob
# check if goal is reached
if np.linalg.norm(pred_end_in_rob) < 0.1:
best_u, best_v = (0, 0)
# Slow turn towards goal
# TODO
best_w = 0
WMAX = 0.5
gx, gy = pred_end_in_rob
angle_to_goal = np.arctan2(gy, gx) # [-pi, pi]
if np.sqrt(gx * gx + gy * gy) > 0.5: # turn only if goal is far away
if np.abs(angle_to_goal) > (np.pi / 4 / 10): # deadzone
best_w = np.clip(angle_to_goal, -WMAX, WMAX) # linear ramp
cmd_vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
if not self.STOP:
# publish cmd_vel
cmd_vel_msg = Twist()
cmd_vel_msg.linear.x = best_u
cmd_vel_msg.linear.y = best_v
cmd_vel_msg.angular.z = best_w
cmd_vel_pub.publish(cmd_vel_msg)
def currenttask_planner_update_routine(self, event=None):
with self.lock:
if self.refmap_manager.tf_frame_in_refmap is None:
return
if self.task_being_executed is None:
return
if self.STOP:
return
current_task = self.task_being_executed
taskinfo = current_task["taskinfo"]
current_task_action = taskinfo["taskaction"]
startidx = taskinfo["taskstartidx"]
resultidx = taskinfo["tasktargetpathidx"]
current_task_path = taskinfo["taskfullpath"][startidx:resultidx]
# current_task_target_pos = current_task_path[taskinfo["tasktargetpathidx"]]
# get next waypoint
robot_xy_in_refmap = Pose2D(
self.refmap_manager.tf_frame_in_refmap)[:2]
current_waypoint_in_refmap = np.array(
current_task_path[self.current_waypoint_index])
# move waypoint forward by Xm if reached
waypoint_dist = self.kWaypointDist_m[current_task_action.typestr()]
if SLIDING_WPT:
for index in range(self.current_waypoint_index,
len(current_task_path)):
new_pos = np.array(current_task_path[index])
if np.linalg.norm(new_pos -
robot_xy_in_refmap) >= waypoint_dist:
break
self.current_waypoint_index = index
current_waypoint_in_refmap = new_pos
else:
if np.linalg.norm(robot_xy_in_refmap -
current_waypoint_in_refmap) < (
self.kRobotRadius + self.kWaypointRadius
) or self.current_waypoint_index == 0:
for index in range(self.current_waypoint_index,
len(current_task_path)):
new_pos = np.array(current_task_path[index])
if np.linalg.norm(new_pos - current_waypoint_in_refmap
) >= waypoint_dist:
break
result_index = index
result_waypoint_in_refmap = new_pos
self.current_waypoint_index = result_index
current_waypoint_in_refmap = result_waypoint_in_refmap
# send waypoint
from visualization_msgs.msg import Marker
mk_msg = Marker()
mk_msg.header.frame_id = self.refmap_manager.kRefMapFrame
mk_msg.header.stamp = rospy.Time.now()
mk_msg.type = Marker.CUBE
mk_msg.color.a = 1
mk_msg.color.r = 1
mk_msg.color.g = 1
mk_msg.scale.x = self.kWaypointRadius
mk_msg.scale.y = self.kWaypointRadius
mk_msg.scale.z = self.kWaypointRadius
mk_msg.pose.position.x = current_waypoint_in_refmap[0]
mk_msg.pose.position.y = current_waypoint_in_refmap[1]
mk_msg.text = current_task_action.typestr()
# some planners also get a path
for xy in current_task_path:
p = Point()
p.x = xy[0]
p.y = xy[1]
mk_msg.points.append(p)
# publish on relevant topics
if current_task_action.typestr() in ["Intend", "Say"]:
pub = self.rvo_planner_wpt_pub
pub.publish(mk_msg)
elif current_task_action.typestr() in ["Crawl", "Nudge"]:
pub = self.responsive_wpt_pub
pub.publish(mk_msg)
else:
rospy.logerr("Unknown task action {}".format(
current_task_action.typestr()))
def replan_detector_routine(self, event=None):
# init, set plan as soon as available
if self.plan_executor_node.plan_being_executed is None:
if self.newest_plan is None:
return # no plan is found yet, do nothing and wait
self.plan_executor_node.switch_to_new_plan_requested = True
self.plan_executor_node.switch_to_plan = self.newest_plan
return
with self.lock:
pass
# newest_plan = self.newest_plan
# plan_being_executed = self.plan_executor_node.plan_being_executed
# has the global goal changed? Trigger replan
if self.replan_due_to_new_goal_flag[0]:
with self.lock:
self.replan_due_to_new_goal_flag[0] = False
self.planning_interrupt_flag[0] = True
self.state_estimator_node.latest_state_estimate_is_stale_flag = True
self.newest_plan = None
with self.plan_executor_node.lock:
self.plan_executor_node.switch_to_new_plan_requested = True
self.plan_executor_node.switch_to_plan = None
return
# Have we reached the goal? Stop planning.
if self.refmap_manager.tf_frame_in_refmap is not None and self.goal_xy_in_refmap is not None:
robot_xy_in_refmap = Pose2D(
self.refmap_manager.tf_frame_in_refmap)[:2]
robot_goal_xy_in_refmap = self.goal_xy_in_refmap
if np.linalg.norm(robot_xy_in_refmap - robot_goal_xy_in_refmap) < (
self.kRobotRadius + GOAL_INFLATION_RADIUS):
if self.plan_executor_node.plan_being_executed is not None:
rospy.loginfo("Goal reached, exiting current plan")
with self.plan_executor_node.lock:
self.plan_executor_node.switch_to_new_plan_requested = True
self.plan_executor_node.switch_to_plan = None
self.newest_plan = None
with self.lock:
self.goal_xy_in_refmap = None
self.goal_ij_in_refmap = None
# have we detected task success? Notify plan executor
task_being_executed = self.plan_executor_node.task_being_executed
if task_being_executed is not None:
tbe_taskinfo = task_being_executed["taskinfo"]
tbe_path = tbe_taskinfo["taskfullpath"]
tbe_target_pos = tbe_path[tbe_taskinfo["tasktargetpathidx"]]
if np.linalg.norm(robot_xy_in_refmap - tbe_target_pos) < (
self.kRobotRadius + GOAL_INFLATION_RADIUS):
with self.plan_executor_node.lock:
self.plan_executor_node.task_succeeded_flag = True
# is the current plan still valid? if not trigger replan
if False: # TODO
with self.plan_executor_node.lock:
self.plan_executor_node.switch_to_new_plan_requested = True
self.plan_executor_node.switch_to_plan = self.newest_plan
return
# is the newest plan very different from the current plan? trigger replan
# is the newest plan a much better best cost? a much worse worst cost?
if False: # TODO
with self.plan_executor_node.lock:
self.plan_executor_node.switch_to_new_plan_requested = True
self.plan_executor_node.switch_to_plan = self.newest_plan
return
# is the state i'm in similar to the state predicted by my plan?
if False: # TODO
with self.plan_executor_node.lock:
self.plan_executor_node.switch_to_new_plan_requested = True
self.plan_executor_node.switch_to_plan = self.newest_plan
return
def publish_obstacles(self):
with self.lock:
if self.transport_data is None:
return
# non-leg obstacles
timestamp, xx, yy, clusters, is_legs, cogs, radii, tf_rob_in_fix = self.transport_data
# tracks
track_ids = []
tracks_latest_pos, tracks_color = [], []
tracks_in_frame, tracks_velocities = [], []
tracks_radii = []
for trackid in self.tracker.active_tracks:
track = self.tracker.active_tracks[trackid]
xy = np.array(track.pos_history[-1])
is_track_in_frame = True
if trackid in self.tracker.latest_matches:
color = (0.,1.,0.,1.) # green
elif trackid in self.tracker.new_tracks:
color = (0.,0.,1.,1.) # blue
else:
color = (0.7, 0.7, 0.7, 1.)
is_track_in_frame = False
track_ids.append(trackid)
tracks_latest_pos.append(xy)
tracks_color.append(color)
tracks_in_frame.append(is_track_in_frame)
tracks_velocities.append(track.estimate_velocity())
tracks_radii.append(track.avg_radius())
# publish trackedpersons
from spencer_tracking_msgs.msg import TrackedPersons, TrackedPerson
pub = rospy.Publisher("/tracked_persons", TrackedPersons, queue_size=1)
tp_msg = TrackedPersons()
tp_msg.header.frame_id = self.kFixedFrame
tp_msg.header.stamp = timestamp
for trackid, xy, in_frame, vel, radius in zip(track_ids, tracks_latest_pos, tracks_in_frame, tracks_velocities, tracks_radii):
# if not in_frame:
# continue
tp = TrackedPerson()
tp.track_id = trackid
tp.is_occluded = False
tp.is_matched = in_frame
tp.detection_id = trackid
tp.pose.pose.position.x = xy[0]
tp.pose.pose.position.y = xy[1]
heading_angle = np.arctan2(vel[1], vel[0]) # guess heading from velocity
from geometry_msgs.msg import Quaternion
tp.pose.pose.orientation = Quaternion(
*tf.transformations.quaternion_from_euler(0, 0, heading_angle))
tp.twist.twist.linear.x = vel[0]
tp.twist.twist.linear.y = vel[1]
tp.twist.twist.angular.z = 0 # unknown
tp_msg.tracks.append(tp)
pub.publish(tp_msg)
pub = rospy.Publisher('/obstacles', ObstacleArrayMsg, queue_size=1)
obstacles_msg = ObstacleArrayMsg()
obstacles_msg.header.stamp = timestamp
obstacles_msg.header.frame_id = self.kFixedFrame
for trackid, xy, in_frame, vel, radius in zip(track_ids, tracks_latest_pos, tracks_in_frame, tracks_velocities, tracks_radii):
if not in_frame:
continue
# Add point obstacle
obst = ObstacleMsg()
obst.id = trackid
obst.polygon.points = [Point32()]
obst.polygon.points[0].x = xy[0]
obst.polygon.points[0].y = xy[1]
obst.polygon.points[0].z = 0
obst.radius = radius
yaw = np.arctan2(vel[1], vel[0])
q = tf.transformations.quaternion_from_euler(0,0,yaw)
obst.orientation = Quaternion(*q)
obst.velocities.twist.linear.x = vel[0]
obst.velocities.twist.linear.y = vel[1]
obst.velocities.twist.linear.z = 0
obst.velocities.twist.angular.x = 0
obst.velocities.twist.angular.y = 0
obst.velocities.twist.angular.z = 0
obstacles_msg.obstacles.append(obst)
pub.publish(obstacles_msg)
pub = rospy.Publisher('/close_nonleg_obstacles', ObstacleArrayMsg, queue_size=1)
MAX_DIST_STATIC_CLUSTERS_M = 3.
cogs_in_fix = []
for i in range(len(cogs)):
cogs_in_fix.append(apply_tf(cogs[i], Pose2D(tf_rob_in_fix)))
obstacles_msg = ObstacleArrayMsg()
obstacles_msg.header.stamp = timestamp
obstacles_msg.header.frame_id = self.kFixedFrame
for c, cog_in_fix, cog_in_rob, r, is_leg in zip(clusters, cogs_in_fix, cogs, radii, is_legs):
if np.linalg.norm(cog_in_rob) < self.kRobotRadius:
continue
if len(c) < self.kMinClusterSize:
continue
# leg obstacles are already published in the tracked obstacles topic
if is_leg:
continue
# close obstacles only
if np.linalg.norm(cog_in_rob) > MAX_DIST_STATIC_CLUSTERS_M:
continue
# Add point obstacle
obst = ObstacleMsg()
obst.id = 0
obst.polygon.points = [Point32()]
obst.polygon.points[0].x = cog_in_fix[0]
obst.polygon.points[0].y = cog_in_fix[1]
obst.polygon.points[0].z = 0
obst.radius = r
yaw = 0
q = tf.transformations.quaternion_from_euler(0,0,yaw)
from geometry_msgs.msg import Quaternion
obst.orientation = Quaternion(*q)
obst.velocities.twist.linear.x = 0
obst.velocities.twist.linear.y = 0
obst.velocities.twist.linear.z = 0
obst.velocities.twist.angular.x = 0
obst.velocities.twist.angular.y = 0
obst.velocities.twist.angular.z = 0
obstacles_msg.obstacles.append(obst)
pub.publish(obstacles_msg)
pub = rospy.Publisher('/obstacle_markers', MarkerArray, queue_size=1)
# delete all markers
ma = MarkerArray()
mk = Marker()
mk.header.frame_id = self.kFixedFrame
mk.ns = "obstacles"
mk.id = 0
mk.type = 0 # ARROW
mk.action = 3 # deleteall
ma.markers.append(mk)
pub.publish(ma)
# publish tracks
ma = MarkerArray()
id_ = 0
# track endpoint
for trackid, xy, in_frame, vel in zip(track_ids, tracks_latest_pos, tracks_in_frame, tracks_velocities):
if not in_frame:
continue
normvel = np.linalg.norm(vel)
if normvel == 0:
continue
mk = Marker()
mk.header.frame_id = self.kFixedFrame
mk.ns = "tracks"
mk.id = trackid
mk.type = 0 # ARROW
mk.action = 0
mk.scale.x = np.linalg.norm(vel)
mk.scale.y = 0.1
mk.scale.z = 0.1
mk.color.r = color[0]
mk.color.g = color[1]
mk.color.b = color[2]
mk.color.a = color[3]
mk.frame_locked = True
mk.pose.position.x = xy[0]
mk.pose.position.y = xy[1]
mk.pose.position.z = 0.03
yaw = np.arctan2(vel[1], vel[0])
q = tf.transformations.quaternion_from_euler(0,0,yaw)
mk.pose.orientation = Quaternion(*q)
ma.markers.append(mk)
pub.publish(ma)
def scan_callback(self, msg):
atic = timer()
with self.lock:
# edge case: first callback
if self.odom is None:
print("odom not received yet")
return
if self.tf_rob_in_fix is None:
rospy.logwarn_throttle(10., "tf_rob_in_fix not found yet")
return
# TODO check that odom and tf are not old
# add current position to path history
PATH_HISTORY_RESOLUTION = 0.05 # m
self.tf_pastrobs_in_fix.append(self.tf_rob_in_fix)
# downsample if too large
if len(self.tf_pastrobs_in_fix) > N_PAST_POS:
keep = [0]
for i in range(len(self.tf_pastrobs_in_fix)):
latest_xy = np.array(self.tf_pastrobs_in_fix[keep[-1]][0][:2])
xy = np.array(self.tf_pastrobs_in_fix[i][0][:2])
if np.linalg.norm(xy-latest_xy) > PATH_HISTORY_RESOLUTION:
keep.append(i)
self.tf_pastrobs_in_fix = [self.tf_pastrobs_in_fix[i] for i in keep]
# strict x2 downsample if still too large
if len(self.tf_pastrobs_in_fix) > 2*N_PAST_POS:
self.tf_pastrobs_in_fix = self.tf_pastrobs_in_fix[::2]
scan = np.array(msg.ranges, dtype=np.float32)
# clustering
angles = np.linspace(0, 2*np.pi, scan.shape[0]+1, dtype=np.float32)[:-1]
xx = np.cos(angles) * scan
yy = np.sin(angles) * scan
tic = timer()
clusters, _, _ = lidar_clustering.euclidean_clustering(scan, angles,
self.kEuclideanClusteringThresh)
cluster_sizes = lidar_clustering.cluster_sizes(len(scan), clusters)
toc = timer()
clustering_time = toc-tic
# filter small clusters
# clusters = [c for c, l in zip(clusters, cluster_sizes) if l >= self.kMinClusterSize]
# center of gravity
tic = timer()
cogs = [np.array([np.mean(xx[c]), np.mean(yy[c])]) for c in clusters]
radii = [np.max(np.sqrt((xx[c]-cog[0])**2 + (yy[c]-cog[1])**2)) for cog, c in zip(cogs, clusters)]
toc = timer()
cog_radii_time = toc-tic
# in fixed frame
cogs_in_fix = []
for i in range(len(cogs)):
cogs_in_fix.append(apply_tf(cogs[i], Pose2D(self.tf_rob_in_fix)))
# legs
is_legs = []
for r, c, cog_in_fix in zip(radii, clusters, cogs_in_fix):
is_leg = True if 0.03 < r and r < 0.15 and len(c) >= self.kMinClusterSize else False
# if a reference map and localization are available, filter detections which are in map
if self.refmap_manager.tf_frame_in_refmap is not None:
cog_in_refmap = apply_tf(cog_in_fix, Pose2D(self.refmap_manager.tf_frame_in_refmap))
ij = cog_in_refmap_ij = self.refmap_manager.map_.xy_to_ij([cog_in_refmap])[0]
# the distance between the c.o.g. and the nearest obstacle in the refmap
if self.refmap_manager.map_as_tsdf[ij[0], ij[1]] < r:
is_leg = False
is_legs.append(is_leg)
timestamp = msg.header.stamp
self.transport_data = (timestamp, xx, yy, clusters, is_legs, cogs, radii, self.tf_rob_in_fix)
leg_cogs_in_fix = []
leg_radii = []
for i in range(len(is_legs)):
if is_legs[i]:
leg_cogs_in_fix.append(cogs_in_fix[i])
leg_radii.append(radii[i])
# Tracks
self.tracker.match(leg_cogs_in_fix, leg_radii, msg.header.stamp)
# publish markers
self.publish_markers()
self.publish_obstacles()
atoc = timer()
if self.args.hz:
print("DWA callback: {:.2f} Hz | C.O.Gs, Radii: {:.1f} ms, Clustering: {:.1f} ms".format(
1/(atoc-atic), cog_radii_time*1000., clustering_time*1000.))
def scan_callback(self, msg):
atic = timer()
# edge case: first callback
if self.msg_prev is None:
self.msg_prev = msg
return
if self.odom is None:
print("odom not received yet")
return
if self.tf_rob_in_fix is None:
print("tf_rob_in_fix not found yet")
return
if self.tf_goal_in_fix is None:
self.tf_goal_in_fix = self.tf_rob_in_fix
print("goal set")
# TODO check that odom and tf are not old
# measure rotation TODO
s = np.array(msg.ranges)
# prediction
dt = (msg.header.stamp - self.msg_prev.header.stamp).to_sec()
s_prev = np.array(self.msg_prev.ranges)
ds = (s - s_prev)
max_ds = self.kMaxObstacleVel_ms * dt
ds_capped = ds
ds_capped[np.abs(ds) > max_ds] = 0
s_next = np.maximum(0, s + ds_capped).astype(np.float32)
# cluster
EUCLIDEAN_CLUSTERING_THRESH_M = 0.05
angles = np.linspace(0,
2 * np.pi,
s_next.shape[0] + 1,
dtype=np.float32)[:-1]
clusters, x, y = clustering.euclidean_clustering(
s_next, angles, EUCLIDEAN_CLUSTERING_THRESH_M)
cluster_sizes = clustering.cluster_sizes(len(s_next), clusters)
s_next[cluster_sizes <= 3] = 25
# dwa
# Get state
# goal in robot frame
goal_in_robot_frame = apply_tf_to_pose(
Pose2D(self.tf_goal_in_fix),
inverse_pose2d(Pose2D(self.tf_rob_in_fix)))
gx = goal_in_robot_frame[0]
gy = goal_in_robot_frame[1]
# robot speed in robot frame
u = self.odom.twist.twist.linear.x
v = self.odom.twist.twist.linear.y
w = self.odom.twist.twist.angular.z
DWA_DT = 0.5
COMFORT_RADIUS_M = self.kRobotComfortRadius_m
MAX_XY_VEL = 0.5
tic = timer()
best_u, best_v, best_score = dynamic_window.linear_dwa(
s_next,
angles,
u,
v,
gx,
gy,
DWA_DT,
DV=0.05,
UMIN=0 if self.args.forward_only else -MAX_XY_VEL,
UMAX=MAX_XY_VEL,
VMIN=-MAX_XY_VEL,
VMAX=MAX_XY_VEL,
AMAX=10.,
COMFORT_RADIUS_M=COMFORT_RADIUS_M,
)
toc = timer()
# print(best_u * DWA_DT, best_v * DWA_DT, best_score)
# print("DWA: {:.2f} Hz".format(1/(toc-tic)))
# Slow turn towards goal
# TODO
best_w = 0
WMAX = 0.5
angle_to_goal = np.arctan2(gy, gx) # [-pi, pi]
if np.sqrt(gx * gx + gy * gy) > 0.5: # turn only if goal is far away
if np.abs(angle_to_goal) > (np.pi / 4 / 10): # deadzone
best_w = np.clip(angle_to_goal, -WMAX, WMAX) # linear ramp
if not self.STOP:
# publish cmd_vel
cmd_vel_msg = Twist()
SIDEWAYS_FACTOR = 0.3 if self.args.forward_only else 1.
cmd_vel_msg.linear.x = np.clip(best_u * 0.5, -0.3, 0.3)
cmd_vel_msg.linear.y = np.clip(best_v * 0.5 * SIDEWAYS_FACTOR,
-0.3, 0.3)
cmd_vel_msg.angular.z = best_w
self.cmd_vel_pub.publish(cmd_vel_msg)
# publish cmd_vel vis
pub = rospy.Publisher("/dwa_cmd_vel", Marker, queue_size=1)
mk = Marker()
mk.header.frame_id = self.kRobotFrame
mk.ns = "arrows"
mk.id = 0
mk.type = 0
mk.action = 0
mk.scale.x = 0.02
mk.scale.y = 0.02
mk.color.b = 1
mk.color.a = 1
mk.frame_locked = True
pt = Point()
pt.x = 0
pt.y = 0
pt.z = 0.03
mk.points.append(pt)
pt = Point()
pt.x = 0 + best_u * DWA_DT
pt.y = 0 + best_v * DWA_DT
pt.z = 0.03
mk.points.append(pt)
pub.publish(mk)
pub = rospy.Publisher("/dwa_goal", Marker, queue_size=1)
mk = Marker()
mk.header.frame_id = self.kRobotFrame
mk.ns = "arrows"
mk.id = 0
mk.type = 0
mk.action = 0
mk.scale.x = 0.02
mk.scale.y = 0.02
mk.color.g = 1
mk.color.a = 1
mk.frame_locked = True
pt = Point()
pt.x = 0
pt.y = 0
pt.z = 0.03
mk.points.append(pt)
pt = Point()
pt.x = 0 + gx
pt.y = 0 + gy
pt.z = 0.03
mk.points.append(pt)
pub.publish(mk)
pub = rospy.Publisher("/dwa_radius", Marker, queue_size=1)
mk = Marker()
mk.header.frame_id = self.kRobotFrame
mk.ns = "radius"
mk.id = 0
mk.type = 3
mk.action = 0
mk.pose.position.z = -0.1
mk.scale.x = COMFORT_RADIUS_M * 2
mk.scale.y = COMFORT_RADIUS_M * 2
mk.scale.z = 0.01
mk.color.b = 1
mk.color.g = 1
mk.color.r = 1
mk.color.a = 1
mk.frame_locked = True
pub.publish(mk)
# publish scan prediction
msg_next = deepcopy(msg)
msg_next.ranges = s_next
# for pretty colors
cluster_ids = clustering.cluster_ids(len(x), clusters)
random_map = np.arange(len(cluster_ids))
np.random.shuffle(random_map)
cluster_ids = random_map[cluster_ids]
msg_next.intensities = cluster_ids
self.pubs[0].publish(msg_next)
# publish past
msg_prev = deepcopy(msg)
msg_prev.ranges = self.msg_prev.ranges
self.pubs[1].publish(msg_prev)
# ...
# finally, set up for next callback
self.msg_prev = msg
atoc = timer()
if self.args.hz:
print("DWA callback: {:.2f} Hz".format(1 / (atoc - atic)))
def publish_markers(self):
with self.lock:
if self.transport_data is None:
return
timestamp, xx, yy, clusters, is_legs, cogs, radii, tf_rob_in_fix = self.transport_data
# transform data to fixed frame
pose2d_rob_in_fix = Pose2D(tf_rob_in_fix)
xy_f = apply_tf(np.vstack([xx, yy]).T, pose2d_rob_in_fix)
cogs_f = apply_tf(np.array(cogs), pose2d_rob_in_fix)
# past positions
pose2d_past_in_fix = []
if self.tf_pastrobs_in_fix:
for tf_a in self.tf_pastrobs_in_fix[::-1]:
pose2d_past_in_fix.append(Pose2D(tf_a))
# tracks
tracks_xy, tracks_color, tracks_in_frame = [], [], []
tracks_avg_r = []
for trackid in self.tracker.active_tracks:
track = self.tracker.active_tracks[trackid]
xy = np.array(track.pos_history)
is_track_in_frame = True
if trackid in self.tracker.latest_matches:
color = (0.,1.,0.,1.) # green
elif trackid in self.tracker.new_tracks:
color = (0.,0.,1.,1.) # blue
else:
color = (0.7, 0.7, 0.7, 1.)
is_track_in_frame = False
tracks_xy.append(xy)
tracks_color.append(color)
tracks_in_frame.append(is_track_in_frame)
tracks_avg_r.append(track.avg_radius())
pub = rospy.Publisher("/detections", Marker, queue_size=1)
# ...
pub = rospy.Publisher("/tracks", MarkerArray, queue_size=1)
# delete all markers
ma = MarkerArray()
mk = Marker()
mk.header.frame_id = self.kFixedFrame
mk.ns = "tracks"
mk.id = 0
mk.type = 4 # LINE_STRIP
mk.action = 3 # deleteall
ma.markers.append(mk)
pub.publish(ma)
ma = MarkerArray()
mk = Marker()
mk.header.frame_id = self.kFixedFrame
mk.ns = "tracks"
mk.id = 0
mk.type = 3 # CYLINDER
mk.action = 3 # deleteall
ma.markers.append(mk)
pub.publish(ma)
# publish tracks
ma = MarkerArray()
id_ = 0
# track lines
for color, xy in zip(tracks_color, tracks_xy):
mk = Marker()
mk.header.frame_id = self.kFixedFrame
mk.ns = "tracks"
mk.id = id_
id_+=1
mk.type = 4 # LINE_STRIP
mk.action = 0
mk.scale.x = 0.02
mk.color.r = color[0]
mk.color.g = color[1]
mk.color.b = color[2]
mk.color.a = color[3]
mk.frame_locked = True
for x, y in xy:
pt = Point()
pt.x = x
pt.y = y
pt.z = 0.03
mk.points.append(pt)
ma.markers.append(mk)
# track endpoint
for color, xy, r in zip(tracks_color, tracks_xy, tracks_avg_r):
x, y = xy[-1]
mk = Marker()
mk.header.frame_id = self.kFixedFrame
mk.ns = "tracks"
mk.id = id_
id_+=1
mk.type = 3 # CYLINDER
mk.action = 0
mk.scale.x = r * 2.
mk.scale.y = r * 2.
mk.scale.z = 0.1
mk.color.r = color[0]
mk.color.g = color[1]
mk.color.b = color[2]
mk.color.a = color[3]
mk.frame_locked = True
mk.pose.position.x = x
mk.pose.position.y = y
mk.pose.position.z = 0.03
ma.markers.append(mk)
pub.publish(ma)
pub = rospy.Publisher("/robot_track", Path, queue_size=1)
path_msg = Path()
path_msg.header.stamp = rospy.Time.now()
path_msg.header.frame_id = self.kFixedFrame
for x, y, _ in pose2d_past_in_fix:
pose_msg = PoseStamped()
pose_msg.header.stamp = path_msg.header.stamp # TODO log actual time
pose_msg.header.frame_id = path_msg.header.frame_id
pose_msg.pose.position.x = x
pose_msg.pose.position.y = y
path_msg.poses.append(pose_msg)
pub.publish(path_msg)
cmd_vel = msg
# store cmd_vel
if topic in joy_topics:
joy = msg
# detect wether autonomous motion is active or not
# process messages
if topic in odom_topics:
# velocity
current_action = np.array([ # if msg is odometry
msg.twist.twist.linear.x, msg.twist.twist.linear.y, msg.twist.twist.angular.z])
# position
try:
p2_rob_in_fix = Pose2D(bag_transformer.lookupTransform(
FIXED_FRAME, ROBOT_FRAME, msg.header.stamp))
except: # noqa
continue
trajectory.append(p2_rob_in_fix)
if topic in reward_topics:
# goal is reached
reached = True
if topic in goal_topics:
goal_in_msg = np.array([msg.pose.position.x, msg.pose.position.y])
p2_msg_in_fix = Pose2D(bag_transformer.lookupTransform(
FIXED_FRAME, msg.header.frame_id, msg.header.stamp))
goal_in_fix = apply_tf(goal_in_msg[None, :], p2_msg_in_fix)[0]