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())
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.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 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.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")
print("publish traj")
self.publish_trajectory()
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 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())
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)
class AutoCharging():
def __init__(self):
# Define Topics for publishing or subscribing messages
self.init_charging_topic = rospy.get_param("~init_charging_topic")
self.battery_topic = rospy.get_param("~battery_topic")
self.pose_topic = rospy.get_param("~pose_topic")
self.drive_topic = rospy.get_param("~drive_topic")
self.mag_drive_topic = rospy.get_param("~mag_drive_topic")
self.contactor_topic = rospy.get_param("~contactor_topic")
self.stop_charging_topic = rospy.get_param("~stop_charging_topic")
self.finish_charging_topic = rospy.get_param("~finish_charging_topic")
self.suspend_topic = rospy.get_param("~suspend_topic")
self.notify_topic = rospy.get_param("~notify_topic")
self.magnetic_topic = rospy.get_param("~magnetic_topic")
# 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
# Subscribers
self.init_charging_sub = rospy.Subscriber(self.init_charging_topic,
Bool,
self.init_chargingCB,
queue_size=1)
self.battery_sub = rospy.Subscriber(self.battery_topic,
String,
self.batteryCB,
queue_size=1)
self.pose_sub = rospy.Subscriber(self.pose_topic,
Odometry,
self.poseCB,
queue_size=1)
self.mag_sub = rospy.Subscriber(self.magnetic_topic,
String,
self.magCB,
queue_size=1)
# Publishers
self.drive_pub = rospy.Publisher(self.drive_topic, Twist, queue_size=1)
self.mag_drive_pub = rospy.Publisher(self.mag_drive_topic,
Twist,
queue_size=1)
self.contactor_pub = rospy.Publisher(self.contactor_topic,
Bool,
queue_size=1)
self.stop_charging_pub = rospy.Publisher(self.stop_charging_topic,
Bool,
queue_size=1)
self.finish_charging_pub = rospy.Publisher(self.finish_charging_topic,
Bool,
queue_size=1)
self.traj_pub = rospy.Publisher("/charging_navi/trajectory",
PolygonStamped,
queue_size=1)
self.suspend_pub = rospy.Publisher(self.suspend_topic,
Bool,
queue_size=1)
self.notify_pub = rospy.Publisher(self.notify_topic,
String,
queue_size=1)
def init_chargingCB(self, msg):
self.init = msg.data
def batteryCB(self, msg):
self.voltage_percent = float(msg.data)
def poseCB(self, msg):
print "step_counter : " + str(self.step_counter) + ", Batt(%):" + str(
self.voltage_percent)
now_position = msg.pose.pose.position
now_heading = self.quaternion_to_angle(msg.pose.pose.orientation)
# Initiate self charging procedure
if (self.init == True and self.first_time == True):
self.step_counter = 1
self.trajectory.clear()
print "Step 0 - Load Trajectory"
self.trajectory.load(self.charging_route)
time.sleep(0.5)
self.traj_pub.publish(self.trajectory.toPolygon())
self.first_time = False
self.stop_charging_pub.publish(False)
# Step 1 - When arriving check point, disable the charging navi and start to tune heading
if (self.step_counter == 1 and self.dist_checker(
now_position, self.check_pt, 0.25) == True):
print "Step 1 - Tune heading"
print np.rad2deg(now_heading)
self.init = False
if (now_heading > np.deg2rad(self.heading_max)
or now_heading < np.deg2rad(self.heading_min)):
self.step_counter = 2
else:
self.tuning_heading(0.3)
self.stop_charging_pub.publish(True)
self.finish_charging_pub.publish(False)
# Step 2 - Continue enable Charging Navi after the checking point
elif (self.step_counter == 2
and self.dist_checker(now_position, self.stop_pt, 0.1) == False):
print "Step 2 - Continue enable Charging Navi"
self.stop_charging_pub.publish(False)
self.step_counter = 3
#------------------Stat
# Step 3 - Stop
elif (self.step_counter == 3
and self.dist_checker(now_position, self.stop_pt, 0.1) == True):
print "Step 3 - Stop Vehicle at Charging Point"
self.stopping()
self.stop_charging_pub.publish(True)
self.step_counter = 4
# Step 4 - Python Magnetic
elif (self.step_counter == 4):
if (self.mag_end == True):
print "Magnetic Drive... Charging"
self.contactor_pub.publish(True)
self.stopping()
self.stop_charging_pub.publish(True)
if (self.contactor_counter >= 60):
self.step_counter = 5
self.contactor_counter = 0
self.mag_end = False
self.contactor_counter = self.contactor_counter + 1
else:
self.notify_pub.publish("1")
self.mag_drive()
print "Magnetic Drive... Moving To Charger"
#Step 5 - Turn Off the Contactor after finish charging
elif (self.step_counter == 5 and self.charge_OK == True):
print "Step 5 - Turn OFF Contactor-----" + str(
self.finish_charging_counter)
if (self.finish_charging_counter >= 30):
self.contactor_pub.publish(False)
self.charge_OK = False
self.step_counter = 6
self.finish_charging_counter = 0
self.finish_charging_counter = self.finish_charging_counter + 1
#------------------END
elif (self.step_counter == 6):
print "Step 6 - Navi to Finish Location"
self.stop_charging_pub.publish(False)
self.step_counter = 7
# Step 7 - Report finish self_charging procedure, Reset counter
elif (self.step_counter == 7 and self.dist_checker(
now_position, self.finish_pt, 1.0) == True):
print "Step 7 - Finish self_charging"
self.finish_charging_pub.publish(True)
self.step_counter = 0
self.first_time = True
self.contactor_counter = 0
self.finish_charging_counter = 0
#measure voltage 10 times before confirming it full prevent false reading from spike
if (self.voltage_percent >=
(self.charge_stop_voltage + self.charge_stop_offset)):
if (self.spike_cnt > 100):
self.charge_OK = True
self.spike_cnt = 0
self.spike_cnt = self.spike_cnt + 1
print "Done Charged Count:" + str(self.spike_cnt)
else:
print "Done Charge:" + str(self.charge_OK)
def magCB(self, msg):
raw = map(int, msg.data.split("/"))
self.mag_data = raw
def mag_drive(self):
x = 0
z = 0
mg = self.mag_data
j = [i for i in mg if i > 150]
self.cnt = self.cnt + 1
if (len(j) > 8):
#stop
self.notify_pub.publish("0,0") #Boot = 0, Low_level_Drive = 0
x = 0
z = 0
self.mag_end = True
print str(len(j)) + "----STOP, " + str(self.mag_end)
else:
self.notify_pub.publish("0,1") #Boot = 0, Low_level_Drive = 1
# line_trace speed set
x = 0.18
drive_msg = Twist()
drive_msg.linear = Vector3(x, 0, 0)
drive_msg.angular = Vector3(0, 0, z)
self.mag_drive_pub.publish(drive_msg)
# Distance Checker, Checking whether 2 points are within distance tolerance or not
def dist_checker(self, a, b, dist_tolerance):
distance = np.sqrt((a.x - b.x)**2 + (a.y - b.y)**2)
if (distance <= dist_tolerance):
return True
else:
return False
# Tuning the Heading of Vehicle
def tuning_heading(self, rotspeed):
drive_msg = Twist()
drive_msg.linear = Vector3(0, 0, 0)
drive_msg.angular = Vector3(0, 0, rotspeed)
self.drive_pub.publish(drive_msg)
# Stopping Vehicle
def stopping(self):
drive_msg = Twist()
drive_msg.linear = Vector3(0, 0, 0)
drive_msg.angular = Vector3(0, 0, 0)
self.drive_pub.publish(drive_msg)
# Convert Quaternion to Angle
def quaternion_to_angle(self, q):
x, y, z, w = q.x, q.y, q.z, q.w
roll, pitch, yaw = tf.transformations.euler_from_quaternion(
(x, y, z, w))
return yaw
class PsaAutoChargingV2():
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.amcl_sub = rospy.Subscriber(rospy.get_param("~amcl_topic"),
PoseWithCovarianceStamped,
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_chargingCB(self, msg):
self.init = msg.data
def batteryCB(self, msg):
self.voltage_percent = float(msg.data)
def poseCB(self, msg):
print "step_counter : " + str(self.step_counter) + ", Batt(%):" + str(
self.voltage_percent)
if isinstance(msg, Odometry):
now_position = msg.pose.pose.position
now_heading = self.quaternion_to_yaw(msg.pose.pose.orientation)
elif isinstance(msg, PoseWithCovarianceStamped):
now_position = msg.pose.pose.position
now_heading = self.quaternion_to_yaw(msg.pose.pose.orientation)
# Initiate self charging procedure
if (self.init == True and self.first_time == True):
self.step_counter = 1
self.trajectory.clear()
print "Step 0 - Load Trajectory"
self.trajectory.load(self.charging_route)
time.sleep(0.5)
self.traj_pub.publish(self.trajectory.toPolygon())
self.first_time = False
self.stop_charging_pub.publish(False)
# Step 1 - When arriving check point, disable the charging navi and start to tune heading
if (self.step_counter == 1 and self.dist_checker(
now_position, self.check_pt, 0.25) == True):
print "Step 1 - Tune heading"
print "current_heading : " + str(now_heading)
self.init = False
if (self.heading_min < now_heading < self.heading_max):
self.step_counter = 2
else:
self.init_turn_pub.publish("90")
self.stop_charging_pub.publish(True)
self.finish_charging_pub.publish(False)
# Step 2 - Continue enable Charging Navi after the checking point
elif (self.step_counter == 2
and self.dist_checker(now_position, self.stop_pt, 0.1) == False):
print "Step 2 - Continue enable Charging Navi"
self.stop_charging_pub.publish(False)
self.step_counter = 3
# Step 3 - Pass over to Magnetic Following Mode
elif (self.step_counter == 3
and self.dist_checker(now_position, self.stop_pt, 0.1) == True):
print "Step 3 - Ready to Swap into Magnetic Following"
self.stopping()
self.stop_charging_pub.publish(True)
self.step_counter = 4
# Step 4 - Magnetic Following and ON Charging after arriving the end of magnetic line
elif (self.step_counter == 4):
if (self.mag_end == True):
print "Turn ON - Charging"
if (self.contactor_counter % 50 == 0):
self.contactor_pub.publish(True)
self.stopping()
self.stop_charging_pub.publish(True)
if (self.contactor_counter >= 60 and self.charge_OK == True):
self.step_counter = 5
self.contactor_counter = 0
self.mag_end = False
self.contactor_counter += 1
else:
self.mag_drive()
print "Step 4 - Magnetic Drive... Moving To Charger"
# Step 5 - Turn Off the Contactor after finish charging
elif (self.step_counter == 5 and self.charge_OK == True):
print "Step 5 - Turn OFF Contactor-----" + str(
self.finish_charging_counter)
self.contactor_pub.publish(False)
if (self.finish_charging_counter >= 50):
self.charge_OK = False
self.step_counter = 6
self.finish_charging_counter = -1
self.finish_charging_counter += 1
# Step 6 - Revert back to LIDAR Navigation Mode
elif (self.step_counter == 6):
print "Step 6 - LIDAR Navi to Finished Location"
self.stop_charging_pub.publish(False)
self.step_counter = 7
# Step 7 - Report finish self_charging procedure, Reset counter
elif (self.step_counter == 7 and self.dist_checker(
now_position, self.finish_pt, 1.0) == True):
print "Step 7 - Finish self_charging"
self.finish_charging_pub.publish(True)
self.step_counter = 0
self.first_time = True
self.contactor_counter = 0
self.finish_charging_counter = 0
# Measure voltage 100 times before confirming it full prevent false reading from spike
if (self.voltage_percent >=
(self.charge_stop_voltage + self.charge_stop_offset)):
if (self.spike_cnt > 100):
self.charge_OK = True
self.spike_cnt = 0
self.spike_cnt += 1
print "Done Charged Count:" + str(self.spike_cnt)
else:
print "Done Charge:" + str(self.charge_OK)
def magCB(self, msg):
raw = map(int, msg.data.split("/"))
self.mag_data = raw
def mag_drive(self):
x = 0
z = 0
mg = self.mag_data
j = [i for i in mg if i > 130] #threshold between magnetic tape or not
self.cnt = self.cnt + 1
if (len(j) > 8): #if magnetic detected more than 8 line
#stop
self.notify_pub.publish("0,0") #Boot = 0, Low_level_Drive = 0
x = 0
self.mag_end = True
print str(len(j)) + "----STOP, " + str(self.mag_end)
else:
self.notify_pub.publish("0,1") #Boot = 0, Low_level_Drive = 1
# line_trace speed set
x = 0.2
print str(len(j)) + "----MOVING, " + str(self.mag_end)
drive_msg = Twist()
drive_msg.linear = Vector3(x, 0, 0)
drive_msg.angular = Vector3(0, 0, 0)
self.mag_drive_pub.publish(drive_msg)
# Distance Checker, Checking whether 2 points are within distance tolerance or not
def dist_checker(self, a, b, dist_tolerance):
distance = np.sqrt((a.x - b.x)**2 + (a.y - b.y)**2)
if (distance <= dist_tolerance):
return True
else:
return False
# Stopping Vehicle
def stopping(self):
drive_msg = Twist()
drive_msg.linear = Vector3(0, 0, 0)
drive_msg.angular = Vector3(0, 0, 0)
self.drive_pub.publish(drive_msg)
# Convert Quaternion to Yaw Angle [deg]
def quaternion_to_yaw(self, q):
x, y, z, w = q.x, q.y, q.z, q.w
roll, pitch, yaw = np.rad2deg(tftr.euler_from_quaternion((x, y, z, w)))
return yaw
class TableFilteringV6():
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()
# Check when to start the Table-Filtering
def init_tableCB(self, msg):
self.init = msg.data
if(msg.data != True):
self.table = False
# Receive every LaserScan and do Filtering to find out Table
def scanCB(self, msg):
if(self.init == True):
maybe_pts = []
clusters = []
mean_pts = []
precheck_pts = []
checked_pts = []
table_pts = []
# Finding Points within selected Scan_Region (Rectangular)
for i in xrange(len(msg.ranges)):
pt = Point()
pt.x = msg.ranges[i]*np.cos(msg.angle_min + msg.angle_increment*i)
pt.y = msg.ranges[i]*np.sin(msg.angle_min + msg.angle_increment*i)
pt.z = 0.0
if(self.x_min <= pt.x <= self.x_max and self.y_min <= pt.y <= self.y_max):
maybe_pts.append(pt)
# Filter the maybe_pts into clusters of points
for j in range(0,10):
cluster_counter = 0
temp_cluster = []
if(len(maybe_pts) == 0):
break
for i in xrange(len(maybe_pts)):
if(self.dist(maybe_pts[i-1], maybe_pts[i]) <= self.cluster_tolerance):
temp_cluster.append(maybe_pts[i-1])
if(self.dist(maybe_pts[i-1], maybe_pts[i]) > self.cluster_tolerance):
cluster_counter = cluster_counter + 1
if(cluster_counter == 2):
temp_cluster.append(maybe_pts[i-1])
break
clusters.append(temp_cluster) # Save each clusters that meet the requirement
for i in xrange(len(temp_cluster)):
del maybe_pts[0]
# Filter and Find the Mean Point of survived clusters (possible as table legs)
for i in xrange(len(clusters)):
if(len(clusters[i]) > 5):
mean_pts.append(self.mean_point(clusters[i]))
# Visualize the detected Clusters
for i in xrange(len(mean_pts)):
self.viz_point(i, mean_pts[i], ColorRGBA(1,0,0,1))
# Filter the Possibilities and Find out which Sets of Clusters is equivalent as Table
for j in xrange(len(mean_pts)):
correct_counter = 0
for i in xrange(len(mean_pts)):
if(self.check_length_width_diagonal(mean_pts[j], mean_pts[i])):
correct_counter = correct_counter + 1
if(correct_counter == 3):
precheck_pts.append(mean_pts[j])
break
checked_pts = self.remove_duplicates(precheck_pts)
# Visualize the Table
print len(checked_pts)
if(len(checked_pts) == 4):
for i in xrange(len(checked_pts)):
table_pts.append(checked_pts[i])
table_pts.append(checked_pts[0])
self.viz_table(table_pts)
self.table = True
self.pathpoint_1 = self.middle_point(checked_pts[0], checked_pts[3])
self.pathpoint_2 = self.middle_point(checked_pts[1], checked_pts[2])
else:
self.pathpoint_1 = self.pathpoint_1
self.pathpoint_2 = self.pathpoint_2
# Checking Position (Map Frame) for when to stop
def poseCB(self, msg):
if(self.init == True):
current_pose = msg.pose.pose.position
current_yaw = utils.quaternion_to_angle(msg.pose.pose.orientation)
if(self.centerpoint != None):
# print "current_pose: " + str(current_pose)
# print "centerpoint : " + str(self.centerpoint)
print self.dist(current_pose, self.centerpoint)
if(self.dist(current_pose, self.centerpoint) <= 0.17): # 0.141 / 0.145 / 0.17 / 0.28 / 0.31
self.stopping()
self.stop_counter = self.stop_counter + 1
if(self.stop_counter == 30):
self.finish_table_pub.publish(True)
self.stop_counter = 0
print "----------------------------- reporting finish --------------------------------------" # Testing USE - find hidden bug
# Keep on Publishing the Latest Route for the Table-Navi
def route_publisher(self):
while not rospy.is_shutdown():
if(self.tfTL.frameExists("base_link")):
if(self.init == True and self.pathpoint_1 != None and self.pathpoint_2 != None and self.table == True):
self.trajectory.clear()
# Transform the table mid-pts (base_link) into (map)
traj_pt_1 = self.transforming_point(self.pathpoint_1, "/map")
traj_pt_1.x += self.route_trim_x
traj_pt_1.y += self.route_trim_y
traj_pt_2 = self.transforming_point(self.pathpoint_2, "/map")
traj_pt_2.x += self.route_trim_x
traj_pt_2.y += self.route_trim_y
# Line Extension(2m before the table)
traj_pt_0 = Point()
traj_pt_0.x = traj_pt_1.x + (2.0*(traj_pt_1.x - traj_pt_2.x)/self.dist(traj_pt_1, traj_pt_2)) + self.route_trim_x
traj_pt_0.y = traj_pt_1.y + (2.0*(traj_pt_1.y - traj_pt_2.y)/self.dist(traj_pt_1, traj_pt_2)) + self.route_trim_y
traj_pt_0.z = 0.0
# Line Extension (0.3m after the table)
traj_pt_3 = Point()
traj_pt_3.x = traj_pt_2.x - (0.3*(traj_pt_1.x - traj_pt_2.x)/self.dist(traj_pt_1, traj_pt_2)) + self.route_trim_x
traj_pt_3.y = traj_pt_2.y - (0.3*(traj_pt_1.y - traj_pt_2.y)/self.dist(traj_pt_1, traj_pt_2)) + self.route_trim_y
traj_pt_3.z = 0.0
self.trajectory.addPoint(traj_pt_0)
self.trajectory.addPoint(traj_pt_1)
self.trajectory.addPoint(traj_pt_2)
self.trajectory.addPoint(traj_pt_3)
self.route_pub.publish(self.trajectory.toPolygon())
self.centerpoint = self.middle_point(traj_pt_1, traj_pt_2)
# Visualize the Path Points
self.viz_point(100, traj_pt_0, ColorRGBA(0,0,1,1))
self.viz_point(101, traj_pt_1, ColorRGBA(0,0,1,1))
self.viz_point(102, traj_pt_2, ColorRGBA(0,0,1,1))
self.viz_point(103, traj_pt_3, ColorRGBA(0,0,1,1))
else:
self.centerpoint = self.centerpoint
self.refresh_rate.sleep()
# Distance Calculator
def dist(self, a, b):
distance = np.sqrt((a.x-b.x)**2 + (a.y-b.y)**2)
return distance
# Mean Point Calculator for each Cluster
def mean_point(self, point_set):
mean_pt = Point()
for i in xrange(len(point_set)):
mean_pt.x = mean_pt.x + point_set[i].x # x-position
mean_pt.y = mean_pt.y + point_set[i].y # y-position
mean_pt.x = mean_pt.x/len(point_set)
mean_pt.y = mean_pt.y/len(point_set)
mean_pt.z = 0.0
return mean_pt
# a & b Middle Point Calculator
def middle_point(self, a, b):
middle_point = Point((a.x + b.x)/2.0, (a.y + b.y)/2.0, 0.0)
return middle_point
# Table Length or Width Checker
def check_length_width_diagonal(self, a, b):
if(0.0001<abs(self.dist(a,b) - self.table_length)<=self.table_tolerance or 0.0001<abs(self.dist(a,b) - self.table_width)<=self.table_tolerance or 0.0001<abs(self.dist(a,b) - self.table_diagonal)<self.table_tolerance):
return True
else:
return False
# Duplicates Remover
def remove_duplicates(self, values):
output = []
seen = set()
for value in values:
if value not in seen:
output.append(value)
seen.add(value)
return output
# Transform Point from coordinate frame (base_link) to (map)
def transforming_point(self, pre_pos, frame):
pre_transform_pt = PointStamped()
pre_transform_pt.header.frame_id = "base_link"
pre_transform_pt.header.stamp = rospy.Time(0)
pre_transform_pt.point = pre_pos
transformed_pt = self.tfTL.transformPoint(frame, pre_transform_pt)
after_transform_pt = Point(transformed_pt.point.x, transformed_pt.point.y, 0.0)
return after_transform_pt
# Use for Visualizing the Points
def viz_point(self, sid, pt_position, color):
point = Marker()
point.header.frame_id = "base_link"
point.header.stamp = rospy.Time.now()
point.ns = "points"
point.id = sid
point.type = 1 # CUBE
point.action = 0 # add/modify
point.lifetime = rospy.Duration(0.1)
point.scale = Vector3(0.1,0.1,0.1)
point.color = color
point.pose.orientation = Quaternion(0,0,0,1)
point.pose.position = pt_position
self.viz_points_pub.publish(point)
# Use for Visualizing the Table
def viz_table(self, table_points):
table = Marker()
table.header.frame_id = "base_link"
table.header.stamp = rospy.Time.now()
table.ns = "table"
table.id = 0
table.type = 4 # LINE STRIP
table.action = 0 # add/modify
table.lifetime = rospy.Duration(0.1)
table.scale = Vector3(0.05,0,0)
table.color = ColorRGBA(1,1,0,1) # Yellow
table.pose.orientation = Quaternion(0,0,0,1)
for p in table_points:
t_point = Point(p.x, p.y, 0.0)
table.points.append(t_point)
self.viz_table_pub.publish(table)
# Stopping the vehicle
def stopping(self):
stop_cmd = Twist()
stop_cmd.linear = Vector3(0, 0, 0)
stop_cmd.angular = Vector3(0, 0, 0)
self.drive_pub.publish(stop_cmd)
class TrajectoryWithSpeedLoader():
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)
# Callback Function for receiving path
def trajCB(self, msg):
path_id = msg.data
if (self.counts == 1):
self.generate_speed_profile(path_id)
self.counts = 0
self.counts += 1
# Generating Speed Profile
def generate_speed_profile(self, path):
self.trajectory.clear()
self.trajectory.load(path)
self.trajectory.make_np_array()
points = self.trajectory.np_points
local_polar_points = utils.piecewise_linear_local_waypoints_polar(
points)
speeds = np.zeros(points.shape[0])
for i in xrange(local_polar_points.shape[0]):
max_speed = self.dynamic_model.max_speed_dist(
local_polar_points[i, 0])
speeds[i] = max_speed
speeds[0] = 0 # speed of the beginning of the track
speeds[-1] = 0 # speed of the last of the track
speeds = np.clip(speeds, self.min_speed, self.max_speed)
# ensure that acceleration bounds are respected
for i in xrange(speeds.shape[0] - 1):
x = local_polar_points[i, 0]
v_i = speeds[i] # initial speed of a straight line
v_f = speeds[i + 1] # final speed of a straight line
a = (v_f**2 - v_i**2) / (2.0 * x) # vf^2 - vi^2 = 2ax
if a > self.max_acceleration:
a = self.max_acceleration
v_f = np.sqrt(v_i**2 + 2.0 * a * x)
speeds[i + 1] = v_f
# ensure that decceleration bounds are respected
for i in xrange(speeds.shape[0] - 2, -1, -1):
x = local_polar_points[i, 0]
v_i = speeds[i]
v_f = speeds[i + 1]
a = (v_f**2 - v_i**2) / (2.0 * x) # vf^2 - vi^2 = 2ax
if a < self.max_decceleration:
a = self.max_decceleration
v_i = np.sqrt(v_f**2 - 2.0 * a * x)
speeds[i] = v_i
# counting estimated travesal time
t_traversal = 0.0
for i in xrange(speeds.shape[0] - 1):
v, x = speeds[i], local_polar_points[i, 0]
t = x / v
t_traversal += t
print "Estimated traversal time: ", t_traversal
print speeds
# Publish the generated Speed Profile
self.trajectory.speed_profile = speeds.tolist()
self.viz_track_pub.publish(
viz_speed_track(self.trajectory.speed_profile,
self.trajectory.np_points))
self.traj_pub.publish(self.trajectory.toPolygon())
