def _convert_obs(self):
robot = self.soadrl_sim.robot
# lidar obs
lidar_pos = np.array([robot.px, robot.py, robot.theta], dtype=np.float32)
ranges = np.ones((self.n_angles,), dtype=np.float32) * 25.
angles = np.linspace(self.kLidarMergedMinAngle,
self.kLidarMergedMaxAngle-self.kLidarAngleIncrement,
self.n_angles) + lidar_pos[2]
render_contours_in_lidar(ranges, angles, self.flat_contours, lidar_pos[:2])
# agents
other_agents = []
for i, human in enumerate(self.soadrl_sim.humans):
pos = np.array([human.px, human.py, human.theta], dtype=np.float32)
dist = self.distances_travelled_in_base_frame[i].astype(np.float32)
vel = np.array([human.vx, human.vy], dtype=np.float32)
if self.lidar_legs:
agent = CSimAgent(pos, dist, vel)
else:
agent = CSimAgent(pos, dist, vel, type_="trunk", radius=human.radius)
other_agents.append(agent)
# apply through converter map (res 1., origin 0,0 -> i,j == x,y)
self.converter_cmap2d.render_agents_in_lidar(ranges, angles, other_agents, lidar_pos[:2])
self.lidar_scan = ranges
self.lidar_angles = angles
# robotstate obs
# shape (n_agents, 5 [grx, gry, vx, vy, vtheta]) - all in base frame
baselink_in_world = np.array([robot.px, robot.py, robot.theta])
world_in_baselink = inverse_pose2d(baselink_in_world)
robotvel_in_world = np.array([robot.vx, robot.vy, 0]) # TODO: actual robot rot vel?
robotvel_in_baselink = apply_tf_to_vel(robotvel_in_world, world_in_baselink)
goal_in_world = np.array([robot.gx, robot.gy, 0])
goal_in_baselink = apply_tf_to_pose(goal_in_world, world_in_baselink)
robotstate_obs = np.hstack([goal_in_baselink[:2], robotvel_in_baselink])
obs = (self.lidar_scan, robotstate_obs)
return obs
def _update_dist_travelled(self):
""" update dist travel var used for animating legs """
# for each human, get vel in base frame
for i, human in enumerate(self.soadrl_sim.humans):
# dig up rotational velocity from past states log
vrot = 0.
if len(self.soadrl_sim.states) > 1:
vrot = (self.soadrl_sim.states[-1][1][i].theta
- self.soadrl_sim.states[-2][1][i].theta) / self._get_dt()
# transform world vel to base vel
baselink_in_world = np.array([human.px, human.py, human.theta])
world_in_baselink = inverse_pose2d(baselink_in_world)
vel_in_world_frame = np.array([human.vx, human.vy, vrot])
vel_in_baselink_frame = apply_tf_to_vel(vel_in_world_frame, world_in_baselink)
self.distances_travelled_in_base_frame[i, :] += vel_in_baselink_frame * self._get_dt()
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)