def toXYHVPath(waypoints, desired_speed=1.0):
h = Header()
h.stamp = rospy.Time.now()
speeds = np.zeros(len(waypoints))
speeds[:] = desired_speed
speeds[-1] = 0.0
path = XYHVPath(h,[XYHV(*[waypoint[0], waypoint[1], waypoint[2], speed]) \
for waypoint, speed in zip(waypoints, speeds)])
return path
def hit_waypoints(self, dubins_path):
w_dubins_path = util.map_to_world(dubins_path, self.map.info)
h = Header()
h.stamp = rospy.Time.now()
# waypoints = np.concatenate((w_dubins_path, w_dubins_path[::-1]), axis=0)
# speeds = np.empty(2*len(dubins_path))
# speeds[:len(dubins_path)] = self.SPEED
# speeds[len(dubins_path):] = -self.SPEED
# path = XYHVPath(h,[XYHV(*[waypoint[0], waypoint[1], waypoint[2], speed]) \
# for waypoint, speed in zip(waypoints, speeds)])
waypoints = w_dubins_path
forward_path = XYHVPath(h,[XYHV(*[waypoint[0], waypoint[1], waypoint[2], speed]) \
for waypoint, speed in zip(waypoints, speeds)])
backward_path = XYHVPath(h,[XYHV(*[waypoint[0], waypoint[1], waypoint[2], -speed]) \
for waypoint, speed in zip(waypoints[::-1], speeds)])
print "Found path to a waypoint."
success = self.controller(forward_path)
print "Controller started.", success
def send_path(self, waypoints):
print("prepare to send waypoints", len(waypoints))
h = Header()
h.stamp = rospy.Time.now()
desired_speed = 1.0
speeds = np.zeros(len(waypoints))
speeds[:] = desired_speed
speeds[-1] = 0.0
path = XYHVPath(h,[XYHV(*[waypoint[0], waypoint[1], waypoint[2], speed]) \
for waypoint, speed in zip(waypoints, speeds)])
print "Sending path..."
controller = rospy.ServiceProxy("/controller/follow_path",
FollowPath())
success = controller(path)
print "Controller started.", success
return success
print "{} ({})".format(name, i)
index = int(raw_input("num: "))
if index >= len(plan_names):
print "Wrong number. Exiting."
exit()
return plans[plan_names[index]]()
if __name__ == '__main__':
rospy.init_node("controller_runner")
configs = generate_plan()
if type(configs) == XYHVPath:
path = configs
else:
h = Header()
h.stamp = rospy.Time.now()
desired_speed = 2.0
ramp_percent = 0.1
ramp_up = np.linspace(0.0, desired_speed, int(ramp_percent * len(configs)))
ramp_down = np.linspace(desired_speed, 0.3, int(ramp_percent * len(configs)))
speeds = np.zeros(len(configs))
speeds[:] = desired_speed
speeds[0:len(ramp_up)] = ramp_up
speeds[-len(ramp_down):] = ramp_down
path = XYHVPath(h, [XYHV(*[config[0], config[1], config[2], speed]) for config, speed in zip(configs, speeds)])
print "Sending path..."
controller = rospy.ServiceProxy("/controller/follow_path", FollowPath())
success = controller(path)
print "Controller started."