def __init__(self):
# FIELDS
#self.goal_pos = None
#self.start_pos = None
#self.g_map = None
#self.map_x_offset = None
#self.map_y_offset = None
#self.map_resolution = None
# Euler angnles:
#self.yaw = 0
#self.pitch = 0
#self.roll = 0
#self.CCW_rotation_matrix = []
#self.CW_rotation_matrix = []
# SUBSCRIBERS and PUBLISHERS
#self.odom_topic = rospy.get_param("~odom_topic")
self.map_sub = rospy.Subscriber("/map", OccupancyGrid, self.map_cb)
self.trajectory = LineTrajectory("/planned_trajectory")
self.goal_sub = rospy.Subscriber("/move_base_simple/goal",
PoseStamped,
self.goal_cb,
queue_size=10)
self.traj_pub = rospy.Publisher("/trajectory/current",
PoseArray,
queue_size=10)
self.odom_sub = rospy.Subscriber("/tesse/odom", Odometry, self.odom_cb)
def __init__(self):
self.path = rospy.get_param(
"~trajectory",
default=
"/home/nvidia/workspace/catkin_ws/src/ddl_0725/pf_localization/trajectories/2018-08-09-16-16-58.traj"
)
self.should_publish = bool(rospy.get_param("~publish", default=True))
self.pub_topic = rospy.get_param(
"~topic", default="/loaded_trajectory/recorded_path")
# initialize and load the trajectory
self.trajectory = LineTrajectory("/loaded_trajectory")
self.trajectory.load(self.path)
if self.should_publish:
self.traj_pub = rospy.Publisher(self.pub_topic, Path, queue_size=1)
# need to wait a short period of time before publishing the first message
time.sleep(0.5)
# visualize the loaded trajectory for 5 seconds
self.trajectory.publish_viz(duration=3.0)
self.duration = rospy.Duration(1)
self.times = 0
self.srv_get_raw_path_ = rospy.Service('/get_raw_path', GetRawPath,
self.handle_get_raw_path)
# send the trajectory
if self.should_publish:
self.publish_trajectory()
def __init__(self):
# Define Adjustaable Parameters
self.min_speed = rospy.get_param("~min_speed")
self.max_speed = rospy.get_param("~max_speed")
self.max_acceleration = rospy.get_param("~max_acceleration")
self.max_decceleration = rospy.get_param("~max_decceleration")
# Internal USE Variables - Do not modify without consultation
self.dynamic_model = utils.ApproximateDynamicsModel()
self.ackermann_model = utils.AckermannModel(0.36)
self.trajectory = LineTrajectory("/speed_track")
self.counts = 0
# Publishers
self.viz_track_pub = rospy.Publisher("/speed_track/viz",
MarkerArray,
queue_size=1)
self.traj_pub = rospy.Publisher(rospy.get_param("~pub_topic"),
PolygonStamped,
queue_size=1)
# Subscriber
self.traj_sub = rospy.Subscriber(rospy.get_param("~sub_topic"),
String,
self.trajCB,
queue_size=1)
print "Initialized. Waiting on messages..."
# need to wait a short period of time before publishing the first message
time.sleep(0.5)
def __init__(self):
self.current_pose = None
self.goal_pose = None
# The map data, in row-major order, starting with (0,0). Occupancy
# probabilities are in the range [0,100]. Unknown is -1.
self.map_occupancy_prob = None
self.obstacles = None
# The origin of the map [m, m, rad]. This is the real-world pose of the
# cell (0,0) in the map.
self.map_origin_pose = None
# Map width, height [cells]
self.map_dimensions = (0, 0)
# The map resolution [m/cell]
self.map_res = None
# MAKE SURE YOU UNCOMMENT THIS AND SET THE ODOM TOPIC RIGHT
# self.odom_topic = rospy.get_param("/particle_filter/odom_topic", "/odom")
self.trajectory = LineTrajectory("/planned_trajectory")
self.map_sub = rospy.Subscriber("/map", OccupancyGrid, self.map_cb)
self.traj_pub = rospy.Publisher("/trajectory/current",
PoseArray,
queue_size=10)
self.goal_sub = rospy.Subscriber("/move_base_simple/goal",
PoseStamped,
self.goal_cb,
queue_size=10)
self.odom_sub = rospy.Subscriber("/pf/pose/odom", Odometry,
self.odom_cb)
def __init__(self):
self.odom_topic = rospy.get_param("~odom_topic")
self.map_sub = rospy.Subscriber("/obstacles", OccupancyGrid,
self.obstacle_map_cb)
self.trajectory = LineTrajectory("/final_trajectory")
self.traj_sub = rospy.Subscriber("/trajectory/current",
PoseArray,
self.trajectory_cb,
queue_size=10)
self.goal_sub = rospy.Subscriber("/move_base_simple/goal",
PoseStamped,
self.goal_cb,
queue_size=10)
self.traj_pub = rospy.Publisher("/trajectory/final",
PoseArray,
queue_size=10)
self.odom_sub = rospy.Subscriber(self.odom_topic,
Odometry,
self.odom_cb,
queue_size=10)
self.rest_of_trajectory = LineTrajectory("/rest_of_trajectory")
self.planning_dist = 50.0
# FOR TESTING #
self.vis_pub = rospy.Publisher("/particles", Marker, queue_size=1)
self.nearest_pub = rospy.Publisher("/nearest", Marker, queue_size=1)
self.start_pose = None
self.goal_pose = None
#self.max_iterations = 1000 # Max size of RRT* tree before breaking.
self.dim = 2 # Configuration space dimensionality ([x, y] -> 2, [x, y, theta] -> 3)
self.gamma = 10 # I don't actually know what this is called lol, it's used in rrt_star.radius()
self.eta = .75
self.goal_bias = 0.15
self.map = None
# FOR TESTING PURPOSES ONLY -- set to true if you want to see the tree grow
self.enable_vis = True
def __init__(self):
self.odom_sub = rospy.Subscriber("/pf/pose/odom", Odometry,
self.odom_callback)
self.traj = LineTrajectory()
rospack = rospkg.RosPack()
fc_path = rospack.get_path("final_challenge")
self.save_path = os.path.join(
fc_path + "/trajectories/",
time.strftime("%Y-%m-%d-%H-%M-%S") + ".traj")
rospy.on_shutdown(self.traj.save(self.save_path))
def __init__(self):
self.odom_topic = rospy.get_param("~odom_topic")
self.map_sub = rospy.Subscriber("/map", OccupancyGrid, self.map_cb)
self.trajectory = LineTrajectory("/planned_trajectory")
self.goal_sub = rospy.Subscriber("/move_base_simple/goal",
PoseStamped,
self.goal_cb,
queue_size=10)
self.traj_pub = rospy.Publisher("/trajectory/current",
PoseArray,
queue_size=10)
self.odom_sub = rospy.Subscriber(self.odom_topic, Odometry,
self.odom_cb)
def __init__(self):
rospack = rospkg.RosPack()
lab6_path = rospack.get_path("lab6")
self.save_path = os.path.join(lab6_path+"/trajectories/", time.strftime("%Y-%m-%d-%H-%M-%S") + ".traj") #%Y-%m-%d-%H-%M-%S
self.trajectory = LineTrajectory("/built_trajectory")
self.data_points = []
self.count = 0
self.click_sub = rospy.Subscriber("/clicked_point", PointStamped, self.clicked_pose, queue_size=10)
self.traj_pub = rospy.Publisher("/trajectory/current", PoseArray, queue_size=10)
self.trajectory_points = rospy.Publisher("/traj_pts", Marker, queue_size=20)
self.trajectory.publish_viz()
# save the built trajectory on shutdown
rospy.on_shutdown(self.saveTrajectory)
def __init__(self):
self.save_path = os.path.join(
rospy.get_param("~save_path"),
time.strftime("%Y-%m-%d-%H-%M-%S") + ".traj")
self.trajectory = LineTrajectory("/built_trajectory")
'''
Insert appropriate subscribers/publishers here
'''
self.point_listener = rospy.Subscriber("/clicked_point",
PointStamped,
self.point_cb,
queue_size=1)
# save the built trajectory on shutdown
rospy.on_shutdown(self.saveTrajectory)
def __init__(self):
self.save_path = os.path.join(rospy.get_param("~save_path"), time.strftime("%Y-%m-%d-%H-%M-%S") + ".traj") #%Y-%m-%d-%H-%M-%S
self.trajectory = LineTrajectory("/built_trajectory")
'''
Insert appropriate subscribers/publishers here
'''
self.data_points = []
self.count = 0
self.click_sub = rospy.Subscriber("/clicked_point", PointStamped, self.clicked_pose, queue_size=10)
self.traj_pub = rospy.Publisher("/trajectory/current", PolygonStamped, queue_size=10)
self.trajectory_points = rospy.Publisher("/traj_pts", Marker, queue_size=20)
self.trajectory.publish_viz() #duration=40.0
# save the built trajectory on shutdown
rospy.on_shutdown(self.saveTrajectory)
def __init__(self):
self.save_path = os.path.join(
rospy.get_param("~save_path"),
time.strftime("%Y-%m-%d-%H-%M-%S") + ".traj")
self.trajectory = LineTrajectory("/built_trajectory")
# receive either goal points or clicked points
self.publish_point_sub = rospy.Subscriber("/clicked_point",
PointStamped,
self.clicked_point_callback,
queue_size=1)
self.nav_goal_sub = rospy.Subscriber("/move_base_simple/goal",
PoseStamped,
self.clicked_point_callback,
queue_size=1)
# save the built trajectory on shutdown
rospy.on_shutdown(self.saveTrajectory)
def __init__(self):
self.save_path = os.path.join(rospy.get_param("~save_path"), time.strftime("%Y-%m-%d-%H-%M-%S") + ".traj") #%Y-%m-%d-%H-%M-%S
self.trajectory = LineTrajectory("/built_trajectory")
'''
Insert appropriate subscribers/publishers here
'''
self.data_points = []
self.count = 0
self.num_waypoints = 4
self.waypoints = []
self.click_sub = rospy.Subscriber("/clicked_point", PointStamped, self.clicked_pose, queue_size=10)
self.traj_pub = rospy.Publisher("/trajectory/current", PolygonStamped, queue_size=10)
self.trajectory_points = rospy.Publisher("/traj_pts", Marker, queue_size=20)
self.trajectory.publish_viz() #duration=40.0
self.rtt_pub = rospy.Publisher("/rtt/final", Marker, queue_size = 10)
self.rtt_tree_pub = rospy.Publisher("/rtt/tree", Marker, queue_size = 10000)
# save the built trajectory on shutdown
rospy.on_shutdown(self.saveTrajectory)
# Create a simulated laser scan
self.scan_sim = PyScanSimulator2D(
2, # number of beams to send out
0, # field of view centered around theta = 0
0, # don't add noise
0.01, # used as an epsilon
500) # discretize the theta space for faster ray tracing
#subscribe to map
self.map_set = False
self.permissible_region = None
self.resolution = None
self.origin = None
self.width = None
self.height = None
self.permissible_indices = None
rospy.Subscriber(
"/map",
OccupancyGrid,
self.map_callback,
queue_size = 1)
self.rrt = RRT(self.rtt_tree_pub,self.scan_sim)
def __init__(self):
self.path = rospy.get_param("~trajectory")
# initialize and load the trajectory
self.trajectory = LineTrajectory("/loaded_trajectory")
self.trajectory.load(self.path)
self.pub_topic = "/trajectory/current"
self.traj_pub = rospy.Publisher(self.pub_topic, PoseArray, queue_size=1)
# need to wait a short period of time before publishing the first message
time.sleep(0.5)
# visualize the loaded trajectory
self.trajectory.publish_viz()
# send the trajectory
self.publish_trajectory()
def __init__(self):
self.save_path = os.path.join(
rospy.get_param(
"~save_path",
default=
"/home/lenovo/zh/workspace/catkin_ws/src/ddl_0725/localization_0725/trajectories"
),
time.strftime("%Y-%m-%d-%H-%M-%S") + ".traj")
self.trajectory = LineTrajectory("/built_trajectory")
self.odom_topic = rospy.get_param("~odom_topic", default="/pf/odom")
self.sub_odom = rospy.Subscriber(self.odom_topic,
Odometry,
self.odomCB,
queue_size=2)
# save the built trajectory on shutdown
rospy.on_shutdown(self.saveTrajectory)
def __init__(self):
# Define Adjustable Parameters
self.stop_pt = Point(float(rospy.get_param("~stop_pt_x")), float(rospy.get_param("~stop_pt_y")), 0)
self.check_pt = Point(float(rospy.get_param("~check_pt_x")), float(rospy.get_param("~check_pt_y")), 0)
self.finish_pt = Point(float(rospy.get_param("~finish_pt_x")), float(rospy.get_param("~finish_pt_y")), 0)
self.heading_min = float(rospy.get_param("~heading_min"))
self.heading_max = float(rospy.get_param("~heading_max"))
self.charging_route = rospy.get_param("~charging_route")
self.charge_stop_voltage = int(rospy.get_param("~charge_stop_voltage"))
self.charge_stop_offset = int(rospy.get_param("~charge_stop_offset"))
# Internal Use Variables - Do not modify without consultation
self.trajectory = LineTrajectory("/charging_trajectory")
self.init = False
self.step_counter = 0
self.first_time = True
self.contactor_counter = 0
self.finish_charging_counter = 0
self.mag_data = []
self.mag_end = False
self.cnt = 0
self.voltage_percent = 0
self.spike_cnt = 0
self.charge_OK = False
self.iters = 0
# Subscribers
self.init_charging_sub = rospy.Subscriber(rospy.get_param("~init_charging_topic"), Bool, self.init_chargingCB, queue_size=1)
self.battery_sub = rospy.Subscriber(rospy.get_param("~battery_topic"), String, self.batteryCB, queue_size=1)
self.pose_sub = rospy.Subscriber(rospy.get_param("~pose_topic"), Odometry, self.poseCB, queue_size=1)
self.magnetic_sub = rospy.Subscriber(rospy.get_param("~magnetic_topic"), String, self.magCB, queue_size=1)
# Publishers
self.drive_pub = rospy.Publisher(rospy.get_param("~drive_topic"), Twist, queue_size=1)
self.mag_drive_pub = rospy.Publisher(rospy.get_param("~mag_drive_topic"), Twist, queue_size=1)
self.contactor_pub = rospy.Publisher(rospy.get_param("~contactor_topic"), Bool, queue_size=1)
self.stop_charging_pub = rospy.Publisher(rospy.get_param("~stop_charging_topic"), Bool, queue_size=1)
self.finish_charging_pub = rospy.Publisher(rospy.get_param("~finish_charging_topic"), Bool, queue_size=1)
self.traj_pub = rospy.Publisher("/charging_navi/trajectory", PolygonStamped, queue_size=1)
self.suspend_pub = rospy.Publisher(rospy.get_param("~suspend_topic"), Bool, queue_size=1)
self.notify_pub = rospy.Publisher(rospy.get_param("~notify_topic"), String, queue_size=1)
self.init_turn_pub = rospy.Publisher("/turning/init", String, queue_size=1)
def __init__(self):
# Define Adjustable Parameters
# - Scan Region (Rectangular)
self.x_min = float(rospy.get_param("~x_min")) # [m]
self.x_max = float(rospy.get_param("~x_max")) # [m]
self.y_min = float(rospy.get_param("~y_min")) # [m]
self.y_max = float(rospy.get_param("~y_max")) # [m]
# - Table Size
self.table_width = float(rospy.get_param("~table_width")) # [m]
self.table_length = float(rospy.get_param("~table_length")) # [m]
self.table_diagonal = float(rospy.get_param("~table_diagonal")) # [m]
self.table_tolerance = float(rospy.get_param("~table_tolerance")) # [m]
# - Route Trim
self.route_trim_x = rospy.get_param("~route_trim_x") # [m]
self.route_trim_y = rospy.get_param("~route_trim_y") # [m]
# Internal Use Variables - Do not modify without consultation
self.init = False # Need to be "False" when integrated with others
self.cluster_tolerance = 0.1 # [m]
self.table = False
self.trajectory = LineTrajectory("/table_trajectory")
self.tfTL = TransformListener()
self.pathpoint_1 = None
self.pathpoint_2 = None
self.refresh_rate = rospy.Rate(10)
self.stop_counter = 0
self.centerpoint = None
# Subscribers
self.scan_sub = rospy.Subscriber(rospy.get_param("~scan_topic"), LaserScan, self.scanCB, queue_size=1)
self.pose_sub = rospy.Subscriber(rospy.get_param("~pose_topic"), Odometry, self.poseCB, queue_size=1)
self.init_table_sub = rospy.Subscriber(rospy.get_param("~init_table_topic"), Bool, self.init_tableCB, queue_size=1)
# Publishers
self.route_pub = rospy.Publisher(rospy.get_param("~route_topic"), PolygonStamped, queue_size=1)
self.drive_pub = rospy.Publisher(rospy.get_param("~drive_topic"), Twist, queue_size=1)
self.finish_table_pub = rospy.Publisher(rospy.get_param("~finish_table_topic"), Bool, queue_size=1)
self.viz_table_pub = rospy.Publisher(rospy.get_param("~viz_table_topic"), Marker, queue_size=1)
self.viz_points_pub = rospy.Publisher(rospy.get_param("~viz_points_topic"), Marker, queue_size=1)
# Publish Route for Navigating Under Table
self.route_publisher()
def __init__(self):
# Define Topics for Publishing and Subscribing Messages
self.sub_topic = rospy.get_param("~sub_topic", "trajectory/new")
self.pub_topic = rospy.get_param("~pub_topic", "trajectory/current")
# Internal Use Variables - Do not modify without consultation
self.counts = 0
# Initialize and Load the trajectory
self.trajectory = LineTrajectory("/called_trajectory")
# Subscriber
self.traj_sub = rospy.Subscriber(self.sub_topic,
String,
self.trajCB,
queue_size=1)
# Publisher
self.traj_pub = rospy.Publisher(self.pub_topic,
PolygonStamped,
queue_size=1)
def __init__(self):
self.path = rospy.get_param("~trajectory")
self.should_publish = bool(rospy.get_param("~publish"))
self.pub_topic = rospy.get_param("~topic")
# initialize and load the trajectory
self.trajectory = LineTrajectory("/loaded_trajectory")
self.trajectory.load(self.path)
if self.should_publish:
self.traj_pub = rospy.Publisher(self.pub_topic,
PolygonStamped,
queue_size=1)
# need to wait a short period of time before publishing the first message
time.sleep(0.5)
# visualize the loaded trajectory for 5 seconds
self.trajectory.publish_viz(duration=3.0)
# send the trajectory
if self.should_publish:
self.publish_trajectory()
def handle_smooth_path(self, req):
''' 1. Save raw path to raw_path_.
2. Smooth path by gradient descending.
3. Remove point from smoothed path which is too close to its neighborhood points ,or its neighborhood points is too close(which means there is probably a peak in path).
4. Publish the result path
'''
raw_path = req.raw_path
tolerance = self.param_tolerance_
max_iterations = self.param_iterations_
alpha = self.param_alpha_
beta = self.param_beta_
margin = self.param_margin_
min_point_dist = self.param_min_point_dist_
if not isinstance(self.map_data_, OccupancyGrid):
print 'Received smooth path request, but cannot smooth when map data not received.'
return
diff = tolerance + 1
step = 0
np_path = self.makeNpArray(raw_path)
if not isinstance(np_path, object):
return
new_path = deepcopy(np_path)
while step < max_iterations:
if diff < tolerance:
print 'smooth ok'
break
step += 1
diff = 0.
pre_path = deepcopy(new_path)
i = 1
while i != new_path.shape[0] - 2:
new_path[i] += alpha * (pre_path[i] - new_path[i]) + beta * (
new_path[i - 1] + new_path[i + 1] - 2 * new_path[i])
if self.isCollision(new_path[i], self.map_data_, margin):
new_path[i] = deepcopy(pre_path[i])
i += 1
continue
if np.sum(
(new_path[i - 1] - new_path[i + 1])**2) < min_point_dist:
new_path = np.delete(new_path, i, axis=0)
pre_path = np.delete(pre_path, i, axis=0)
i -= 1
i += 1
diff += np.sum((new_path - pre_path)**2)
print 'round: ', step, '; diff: ', diff, '; origin # of points: ', len(
req.raw_path.poses
), '; result # of points: ', new_path.shape[
0], '; # of deleted points: ', np_path.shape[0] - new_path.shape[0]
smoothed_path = Path()
smoothed_path.header = make_header(self.param_map_frame_)
for i in xrange(new_path.shape[0]):
pose = PoseStamped()
pose.pose.position.x = new_path[i][0]
pose.pose.position.y = new_path[i][1]
pose.pose.position.z = 0
pose.pose.orientation = angle_to_quaternion(0)
smoothed_path.poses.append(pose)
resp = SmoothPathResponse(smoothed_path)
if self.param_save:
trajectory = LineTrajectory("/bulid_trajectory")
trajectory.fromPath(smoothed_path)
trajectory.save(self.param_save_path)
return resp
def handle_get_raw_path(self, req):
if not isinstance(self.trajectory, LineTrajectory):
self.trajectory = LineTrajectory("/loaded_trajectory")
self.trajectory.load(self.path)
return GetRawPathResponse(self.trajectory.toPath())
