class LoadTrajectory(object):
""" Loads a trajectory from the file system and publishes it to a ROS topic.
"""
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 publish_trajectory(self):
print "Publishing trajectory to:", self.pub_topic
self.traj_pub.publish(self.trajectory.toPolygon())
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):
# 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)
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)
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()
class LoadTrajectory(object):
""" Loads a trajectory from the file system and publishes it to a ROS topic.
"""
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
rospy.loginfo("waiting to publish trajectory")
time.sleep(3.0)
# visualize the loaded trajectory
self.trajectory.publish_viz()
# send the trajectory
self.publish_trajectory()
def publish_trajectory(self):
print "Publishing trajectory to:", self.pub_topic
self.traj_pub.publish(self.trajectory.toPoseArray())
class PathPlan(object):
""" Listens for goal pose published by RViz and uses it to plan a path from
current car pose.
"""
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 map_cb(self, msg):
pass ## REMOVE AND FILL IN ##
def odom_cb(self, msg):
pass ## REMOVE AND FILL IN ##
def goal_cb(self, msg):
pass ## REMOVE AND FILL IN ##
def plan_path(self, start_point, end_point, map):
## CODE FOR PATH PLANNING ##
# publish trajectory
self.traj_pub.publish(self.trajectory.toPoseArray())
# visualize trajectory Markers
self.trajectory.publish_viz()
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)
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)
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):
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):
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)
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)
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)
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)
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()
class TrajectoryCaller(object):
""" Call a pre-defined trajectory from the file system and publishes it to a ROS topic.
"""
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)
# Callback Function for receiving path
def trajCB(self, msg):
path_id = msg.data
if self.counts == 1:
self.publish_trajectory(path_id)
self.counts = 0
self.counts = self.counts + 1
# Republish the trajectory after being called
def publish_trajectory(self, path_name):
# clear the trajectory whenever going to load new path
self.trajectory.clear()
# load a new received path
self.trajectory.load(path_name)
print "Publishing trajectory to:", self.pub_topic
self.traj_pub.publish(self.trajectory.toPolygon())
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)
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 LoadTrajectory(object):
""" Loads a trajectory from the file system and publishes it to a ROS topic.
"""
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 publish_trajectory(self):
print "Publishing trajectory to:", self.pub_topic
while not rospy.is_shutdown():
if self.traj_pub.get_num_connections() > 0:
self.traj_pub.publish(self.trajectory.toPath())
time.sleep(self.duration.to_sec())
self.times = self.times + 1
if self.times > 20:
break
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())
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.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()
class LoadTrajectory(object):
""" Loads a trajectory from the file system and publishes it to a ROS topic.
"""
def __init__(self):
self.path = rospy.get_param(
"~trajectory",
default=
"/home/lenovo/zh/workspace/catkin_ws/src/ddl_0725/localization_0725/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)
# send the trajectory
if self.should_publish:
self.publish_trajectory()
def publish_trajectory(self):
print "Publishing trajectory to:", self.pub_topic
while not rospy.is_shutdown():
self.traj_pub.publish(self.trajectory.toPath())
time.sleep(self.duration.to_sec())
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)
