def make_line_path(p0, p1, n_pt=10):
_path = guidance.Path()
_pts = np.stack([np.linspace(p0[i], p1[i], n_pt) for i in range(2)], axis=-1)
disp = p1-p0
yaws = np.arctan2(disp[1], disp[0])*np.ones(len(_pts))
_path.append_points(_pts, yaws)
return _path
def __init__(
self,
camera_name='ueye_enac_ceiling_1_6mm',
camera_img_fmt='mono8',
map_path='/home/poine/work/rosmip.git/rosmip/rosmip_worlds/maps/track_ethz_2.yaml',
path_path='/home/poine/work/oscar.git/oscar/oscar_control/path_track_ethz_5.npz'
):
self.cam_img_type = camera_img_fmt
self.camera = smocap.Camera(camera_name)
self.retrieve_cam_info(camera_name)
self.retrieve_cam_localization(camera_name)
self.load_map(map_path)
if path_path is not None:
self.path = guidance.Path(load=path_path)
self.draw_decorations()
self.image_pub = rospy.Publisher("/smocap/image_map_debug",
sensor_msgs.msg.Image,
queue_size=1)
self.bridge = cv_bridge.CvBridge()
self.image_sub = rospy.Subscriber(camera_name + '/image_raw',
sensor_msgs.msg.Image,
self.img_callback,
queue_size=1)
def __init__(self):
twist_cmd_topic = rospy.get_param(
'~twist_cmd_topic', '/oscar_ackermann_controller/cmd_vel')
path_filenames = rospy.get_param(
'~path_filename',
'/home/poine/work/oscar.git/oscar/oscar_control/path_track_ethz_5.npz'
)
_path_filenames = path_filenames.split(',')
#pdb.set_trace()
self.vel_setpoint = rospy.get_param('~vel_setpoint', '0.4')
self.vel_adaptive = rospy.get_param('~vel_adaptive', 'false')
self.look_ahead = rospy.get_param('~look_ahead', '0.25')
self.paths = [guidance.Path(load=f) for f in _path_filenames]
#self.controller = PurePursuit(path_filename)
self.dt = 1. / 60.
self.l = rospy.get_param('~wheel_sep', 0.1) # wheelbase
self.pub_twist = rospy.Publisher(twist_cmd_topic,
geometry_msgs.msg.Twist,
queue_size=1)
self.pub_path = rospy.Publisher('pure_pursuit/path',
nav_msgs.msg.Path,
queue_size=1)
self.pub_curpath = rospy.Publisher('pure_pursuit/curpath',
nav_msgs.msg.Path,
queue_size=1)
self.pub_goal = rospy.Publisher('pure_pursuit/goal',
visualization_msgs.msg.Marker,
queue_size=1)
self.pub_arc = rospy.Publisher('pure_pursuit/arc',
nav_msgs.msg.Path,
queue_size=1)
use_gazebo_truth = rospy.get_param('~use_gazebo_truth', 'false')
truth_topic = rospy.get_param('~truth_topic',
'/rosmip/base_link_truth')
if use_gazebo_truth:
rospy.loginfo(' using gazebo truth: {}'.format(truth_topic))
self.smocap_listener = utils.GazeboTruthListener(truth_topic)
else:
rospy.loginfo(' using smocap input')
self.smocap_listener = utils.SmocapListener()
self.vel_ref = utils.FirstOrdLinRef(0.75)
rospy.loginfo(' using twist cmd topic: {}'.format(twist_cmd_topic))
rospy.loginfo(' using wheel_sep: {}'.format(self.l))
rospy.loginfo(' using path: {}'.format(path_filenames))
rospy.loginfo(' using vel_setpoint: {}'.format(self.vel_setpoint))
rospy.loginfo(' using look_ahead: {}'.format(self.look_ahead))
rospy.loginfo(' using wheel_sep: {}'.format(self.l))
rospy.loginfo(' using vel_adaptive: {}'.format(self.vel_adaptive))
def plot_path_list():
map_path = '/home/poine/work/rosmip.git/rosmip/rosmip_worlds/maps/enac_bench/track_test2.yaml'
_map = guidance.Map(yaml_path=map_path)
path_list = (
'/home/poine/work/oscar.git/oscar/oscar_control/paths/enac_bench/path_03.npz',
'/home/poine/work/oscar.git/oscar/oscar_control/paths/enac_bench/path_04.npz',
'/home/poine/work/oscar.git/oscar/oscar_control/paths/enac_bench/path_05.npz',
'/home/poine/work/oscar.git/oscar/oscar_control/paths/enac_bench/path_08.npz',
'/home/poine/work/oscar.git/oscar/oscar_control/paths/enac_bench/path_10.npz',
'/home/poine/work/oscar.git/oscar/oscar_control/paths/enac_bench/path_11.npz',
'/home/poine/work/oscar.git/oscar/oscar_control/paths/enac_bench/path_08.npz'
)
for i, path_path in enumerate(path_list):
_path = guidance.Path(load=path_path)
ax = plt.subplot(len(path_list), 1, i + 1)
guidance.plot(_map, _path)
def main():
map_path = sys.argv[1] if len(
sys.argv
) > 1 else '/home/poine/work/rosmip.git/rosmip/rosmip_worlds/maps/track_ethz_2.yaml'
path_path = sys.argv[2] if len(
sys.argv
) > 2 else '/home/poine/work/oscar.git/oscar/oscar_control/path_track_ethz_2.npz'
_map = guidance.Map(yaml_path=map_path)
_path = guidance.Path(load=path_path)
ax = plt.subplot(2, 1, 1)
guidance.plot(_map, _path)
ax = plt.subplot(2, 1, 2)
_path.compute_curvatures()
print _path.radius
print _path.dists
plt.plot(_path.radius)
ax.set_ylim(0, 3)
def make_circle_path(c, r, th0, th1, n_pt=10):
_path = guidance.Path()
ths = np.linspace(th0, th1, n_pt)
pts = pt_on_circle(c, r, ths)
_path.append_points(pts, ths+math.pi/2)
return _path
#!/usr/bin/env python
import sys, numpy as np
import guidance
if __name__ == '__main__':
p = guidance.Path(load=sys.argv[1])
p.append([guidance.Path(load=_p) for _p in sys.argv[2:]])
p.save('/tmp/foo.npz')
def __init__(self, path_file, params, look_ahead=0.3):
self.path = guidance.Path(load=path_file)
self.params = params
self.look_ahead = look_ahead
