class BuildTrajectory(object):
""" Listens for points published by RViz and uses them to build a trajectory. Saves the output to the file system.
"""
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 clicked_point_callback(self, msg):
if isinstance(msg, PointStamped):
self.trajectory.addPoint(msg.point)
elif isinstance(msg, PoseStamped):
self.trajectory.addPoint(msg.pose.position)
# publish visualization of the path being built
self.trajectory.publish_viz()
def saveTrajectory(self):
self.trajectory.save(self.save_path)
class BuildTrajectory(object):
""" Listens for points published by RViz and uses them to build a trajectory. Saves the output to the file system.
"""
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 point_cb(self, clicked_point):
self.trajectory.addPoint(clicked_point.point)
self.trajectory.publish_start_point()
self.trajectory.publish_end_point()
self.trajectory.publish_trajectory()
def saveTrajectory(self):
self.trajectory.save(self.save_path)
class BuildTrajectory(object):
""" Listens for points published by RViz and uses them to build a trajectory. Saves the output to the file system.
"""
def __init__(self):
self.save_path = os.path.join("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 publish_trajectory(self):
self.traj_pub.publish(self.trajectory.toPoseArray())
def saveTrajectory(self):
self.trajectory.save(self.save_path)
def clicked_pose(self, msg):
self.count += 1
point = Point()
point.x = msg.point.x
point.y = msg.point.y
self.trajectory.addPoint(point)
self.data_points.append(point)
self.mark_pt(self.trajectory_points, (0, 1, 0), self.data_points)
if self.count > 2:
rospy.loginfo("PUBLISH TRAJ")
self.publish_trajectory()
self.trajectory.publish_viz()
def mark_pt(self, subscriber, color_tup, data):
mark_pt = Marker()
mark_pt.header.frame_id = "/map"
mark_pt.header.stamp = rospy.Time.now()
mark_pt.type = mark_pt.SPHERE_LIST
mark_pt.action = mark_pt.ADD
mark_pt.scale.x = .5
mark_pt.scale.y = .5
mark_pt.scale.z = .5
mark_pt.color.a = 1.0
mark_pt.color.r = color_tup[0]
mark_pt.color.g = color_tup[1]
mark_pt.color.b = color_tup[2]
mark_pt.points = data
subscriber.publish(mark_pt)
class BuildTrajectory(object):
""" Listens for points published by RViz and uses them to build a trajectory. Saves the output to the file system.
"""
def __init__(self):
self.save_path = os.path.join(
rospy.get_param(
"~save_path",
default=
"/home/nvidia/workspace/catkin_ws/src/ddl_0725/pf_localization/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)
self.sub_pose = rospy.Subscriber("/current_pose",
PoseStamped,
self.poseCB,
queue_size=1)
# save the built trajectory on shutdown
rospy.on_shutdown(self.saveTrajectory)
def odomCB(self, msg):
self.trajectory.addPoint(
msg.pose.pose.position.x, msg.pose.pose.position.y,
quaternion_to_angle(msg.pose.pose.orientation))
self.trajectory.publish_viz()
def poseCB(self, msg):
self.trajectory.addPoint(msg.pose.position.x, msg.pose.position.y,
quaternion_to_angle(msg.pose.orientation))
self.trajectory.publish_viz()
def saveTrajectory(self):
self.trajectory.save(self.save_path)
class BuildTrajectory(object):
""" Listens for points published by RViz and uses them to build a trajectory. Saves the output to the file system.
"""
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 clicked_point_callback(self, msg):
if isinstance(msg, PointStamped):
self.trajectory.addPoint(msg.point)
elif isinstance(msg, PoseStamped):
self.trajectory.addPoint(msg.pose.position)
# publish visualization of the path being built
self.trajectory.publish_viz()
def odomCB(self, msg):
self.trajectory.addPoint(
msg.pose.pose.position.x, msg.pose.pose.position.y,
quaternion_to_angle(msg.pose.pose.orientation))
self.trajectory.publish_viz()
def saveTrajectory(self):
self.trajectory.save(self.save_path)
class PathBuilder(object):
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 odom_callback(self, msg):
point = msg.pose.pose.position #get Point from Odometry msg
self.traj.addPoint(point)
class BuildTrajectory(object):
""" Listens for points published by RViz and uses them to build a trajectory. Saves the output to the file system.
"""
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 map_callback(self,map_msg):
"""
#convert the map to a numpy array
print('HI')
self.resolution = map_msg.info.resolution
self.width = map_msg.info.width
self.height = map_msg.info.height
#ccmap_ = np.array(map_msg.data, np.double)/100
#map_ = np.clip(map_,0,1)
#print(map_.shape)
"""
# assign map-based attributes
self.resolution = map_msg.info.resolution
self.width = map_msg.info.width
self.height = map_msg.info.height
# Convert the origin to a tuple
origin_p = map_msg.info.origin.position
origin_o = map_msg.info.origin.orientation
origin_o = tf.transformations.euler_from_quaternion((
origin_o.x,
origin_o.y,
origin_o.z,
origin_o.w))
self.origin = (origin_p.x, origin_p.y, origin_o[2])
# 0: permissible, -1: unmapped, 100: blocked
array_255 = np.array(map_msg.data).reshape((map_msg.info.height, map_msg.info.width))
# 0: not permissible, 1: permissible
self.permissible_region = np.zeros_like(array_255, dtype = bool)
self.permissible_region[array_255 == 0] = 1
# dilate the maps
self.permissible_region = 1 - binary_dilation(1 - self.permissible_region, square(15))
scan_map = 1 - np.array(self.permissible_region.reshape((self.permissible_region.size,)), np.double)
# Initialize a map with the laser scan
self.scan_sim.set_map(
scan_map,
map_msg.info.height,
map_msg.info.width,
map_msg.info.resolution,
self.origin,
0.5) # Consider anything < 0.5 to be free
self.permissible_indices = [(r, c) for r in xrange(self.height) for c in xrange(self.width) if self.permissible_region[r][c] == 1]
self.map_set = True
print("Map initialized")
def publish_trajectory(self):
self.traj_pub.publish(self.trajectory.toPolygon())
def saveTrajectory(self):
self.trajectory.save(self.save_path)
def clicked_pose(self,msg):
"""
self.trajectory.addPoint(point)
self.data_points.append(point)
self.mark_pt(self.trajectory_points, (0,1,0), self.data_points)
if self.count > 2:
rospy.loginfo("PUBLISH TRAJ")
print("publish traj")
self.publish_trajectory()
"""
point = Point()
point.x = msg.point.x
point.y = msg.point.y
self.waypoints.append(point)
self.mark_pt(self.trajectory_points, (0,0,1), [point])
self.count +=1
print("Point detected")
print(self.count)
print(self.waypoints)
if self.count == self.num_waypoints:
#do stuff
self.data_points = []
start = self.waypoints[0]
for pt in self.waypoints[1:]:
rospy.loginfo("Building rrt")
result = self.rrt.run(start,pt)
self.data_points = self.data_points + result
start = result[-1]
print("Publishing")
print(result)
mark = Marker()
mark.header.frame_id = "/map"
mark.header.stamp = rospy.Time.now()
mark.type = Marker.LINE_STRIP
mark.ns = "final_rtt_path"
mark.id = 9000
mark.scale.x = 0.5
mark.scale.y = 0.5
mark.scale.z = 0.5
mark.points = self.data_points
mark.color = ColorRGBA(1,0,0,1)
self.rtt_pub.publish(mark)
print("published...")
self.count = 0
self.waypoints = []
def mark_pt(self, subscriber, color_tup, data):
mark_pt = Marker()
mark_pt.header.frame_id = "/map"
mark_pt.id = self.count
mark_pt.header.stamp = rospy.Time.now()
mark_pt.type = mark_pt.SPHERE_LIST
mark_pt.action = mark_pt.ADD
mark_pt.scale.x = 1.0
mark_pt.scale.y = 1.0
mark_pt.scale.z= 1.0
mark_pt.color.a =1.0
mark_pt.color.r=color_tup[0]
mark_pt.color.g = color_tup[1]
mark_pt.color.b = color_tup[2]
mark_pt.points = data
subscriber.publish(mark_pt)
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
