class PredictionSummarizer(object):
"""Node that decides whether to ask a question from uncertainty metrics."""
def __init__(self):
"""Init."""
super(PredictionSummarizer, self).__init__()
rospy.on_shutdown(self.shutdown)
self.pub = rospy.Publisher('prediction_summary', PredictionSummary)
self.tss = TimeSynchronizer(subscribers, queue_size=3)
# self.tss = \
# ApproximateTimeSynchronizer(subscribers, queue_size=3, slop=5)
self.tss.registerCallback(self.callback)
def callback(self, prediction, entropy, margin, all_predictions,
named_predictions):
"""Produce a summary msg and publish it."""
summary = PredictionSummary(predicted=prediction,
entropy=entropy,
margin=margin,
all_predictions=all_predictions,
named_predictions=named_predictions)
self.pub.publish(summary)
def shutdown(self):
"""Hook to be executed when rospy.shutdown is called."""
pass
def run(self):
"""Run the node."""
rospy.spin()
def __init__(self, name):
rospy.loginfo("Starting %s..." % name)
self.vis_pub = rospy.Publisher("~visualization_marker",
Marker,
queue_size=1)
self.tf = TransformListener()
self.cc = CostmapCreator(
rospy.Publisher("/velocity_costmap",
OccupancyGrid,
queue_size=10,
latch=True),
rospy.Publisher("~origin", PoseStamped, queue_size=10))
self.dyn_srv = DynServer(VelocityCostmapsConfig, self.dyn_callback)
subs = [
Subscriber(
rospy.get_param("~qtc_topic",
"/qtc_state_predictor/prediction_array"),
QTCPredictionArray),
Subscriber(
rospy.get_param("~ppl_topic", "/people_tracker/positions"),
PeopleTracker)
]
ts = TimeSynchronizer(fs=subs, queue_size=60)
ts.registerCallback(self.callback)
rospy.loginfo("... all done.")
def test_synchronizer(self):
m0 = MockFilter()
m1 = MockFilter()
ts = TimeSynchronizer([m0, m1], 1)
ts.registerCallback(self.cb_collector_2msg)
if 0:
# Simple case, pairs of messages, make sure that they get combined
for t in range(10):
self.collector = []
msg0 = MockMessage(t, 33)
msg1 = MockMessage(t, 34)
m0.signalMessage(msg0)
self.assertEqual(self.collector, [])
m1.signalMessage(msg1)
self.assertEqual(self.collector, [(msg0, msg1)])
# Scramble sequences of length N. Make sure that TimeSequencer recombines them.
random.seed(0)
for N in range(1, 10):
m0 = MockFilter()
m1 = MockFilter()
seq0 = [MockMessage(t, random.random()) for t in range(N)]
seq1 = [MockMessage(t, random.random()) for t in range(N)]
# random.shuffle(seq0)
ts = TimeSynchronizer([m0, m1], N)
ts.registerCallback(self.cb_collector_2msg)
self.collector = []
for msg in random.sample(seq0, N):
m0.signalMessage(msg)
self.assertEqual(self.collector, [])
for msg in random.sample(seq1, N):
m1.signalMessage(msg)
self.assertEqual(set(self.collector), set(zip(seq0, seq1)))
def test_synchronizer(self):
m0 = MockFilter()
m1 = MockFilter()
ts = TimeSynchronizer([m0, m1], 1)
ts.registerCallback(self.cb_collector_2msg)
if 0:
# Simple case, pairs of messages, make sure that they get combined
for t in range(10):
self.collector = []
msg0 = MockMessage(t, 33)
msg1 = MockMessage(t, 34)
m0.signalMessage(msg0)
self.assertEqual(self.collector, [])
m1.signalMessage(msg1)
self.assertEqual(self.collector, [(msg0, msg1)])
# Scramble sequences of length N. Make sure that TimeSequencer recombines them.
random.seed(0)
for N in range(1, 10):
m0 = MockFilter()
m1 = MockFilter()
seq0 = [MockMessage(t, random.random()) for t in range(N)]
seq1 = [MockMessage(t, random.random()) for t in range(N)]
# random.shuffle(seq0)
ts = TimeSynchronizer([m0, m1], N)
ts.registerCallback(self.cb_collector_2msg)
self.collector = []
for msg in random.sample(seq0, N):
m0.signalMessage(msg)
self.assertEqual(self.collector, [])
for msg in random.sample(seq1, N):
m1.signalMessage(msg)
self.assertEqual(set(self.collector), set(zip(seq0, seq1)))
def __init__(self):
"""Constructor."""
# Node initialization.
rospy.init_node("scan_filter", anonymous=True)
rospy.sleep(1)
# Parameter reading.
self.map_inflation_radius = float(
rospy.get_param('map_inflation_radius'))
self.robot_count = rospy.get_param('/robot_count')
# Robot ID starting from 1, as navigation_2d
self.robot_id = rospy.get_param('~robot_id')
rospy.logerr("Received Robot ID: {} Robot Count: {}".format(
self.robot_id, self.robot_count))
# Initialization of variables.
self.all_poses = {
} # Poses of other robots in the world reference frame.
# Get pose from other robots.
for i in xrange(0, self.robot_count):
if i != self.robot_id:
s = "def a_{0}(self, data): self.all_poses[{0}] = (data.pose.pose.position.x," \
"data.pose.pose.position.y," \
"(data.pose.pose.orientation.x,data.pose.pose.orientation.y,data.pose.pose.orientation.z," \
"data.pose.pose.orientation.w), data.header.stamp.to_sec())".format(i)
exec(s)
exec("setattr(ScanFilter, 'callback_pos_teammate{0}', a_{0})".
format(i))
exec(
"rospy.Subscriber('/robot_{0}/base_pose_ground_truth', Odometry, self.callback_pos_teammate{0}, "
"queue_size=1)".format(i))
# Assumes that apart from the readings, and timestamp, the rest won't change.
self.scan = rospy.wait_for_message('base_scan', LaserScan)
self.scan.ranges = list(self.scan.ranges)
self.scan_ranges_size = len(self.scan.ranges)
# Subscriber of the own robot.
self.base_pose_subscriber = message_filters.Subscriber(
'base_pose_ground_truth', Odometry)
self.scan_subscriber = message_filters.Subscriber(
'base_scan', LaserScan)
ats = TimeSynchronizer(
[self.base_pose_subscriber, self.scan_subscriber], 1) #, 0.01)
ats.registerCallback(self.pose_scan_callback)
# Publisher of the filtered scan.
self.scan_pub = rospy.Publisher('filtered_scan',
LaserScan,
queue_size=1)
# Creation of the message.
rospy.Subscriber('/shutdown', String, self.save_all_data)
class CameraSynchronizer:
def __init__(self, camera_name):
rospy.logerr("Initializing!")
self.camera_name = camera_name
if (self.camera_name == "right_hand"):
self.image_sub = Subscriber("/cameras/right_hand_camera/image",
Image)
self.info_sub = Subscriber(
"/cameras/right_hand_camera/camera_info_std", CameraInfo)
elif (self.camera_name == "top"):
rospy.loginfo("Initializing top camera!")
self.image_sub = Subscriber("/camera/rgb/image_raw", Image)
self.info_sub = Subscriber("/camera/rgb/camera_info", CameraInfo)
def subscribe(self):
# self.ats = ApproximateTimeSynchronizer(
# [self.image_sub, self.info_sub], queue_size=5, slop=0.5)
# self.ats.registerCallback(self.gotImage)
self.ts = TimeSynchronizer(
[self.image_sub, self.info_sub],
queue_size=5,
)
self.ts.registerCallback(self.gotImage)
def publish(self):
self.image_pub = rospy.Publisher("/" + self.camera_name +
"/synch_camera/image_rect",
Image,
queue_size=10)
self.info_pub = rospy.Publisher("/" + self.camera_name +
"/synch_camera/camera_info",
CameraInfo,
queue_size=10)
def gotImage(self, image, camerainfo):
rospy.loginfo("Got an image!")
print("GOT AN IMAGE")
assert image.header.stamp == camerainfo.header.stamp
rospy.loginfo("got an Image and CameraInfo")
self.image_pub.publish(image)
self.info_pub.publish(camerainfo)
class teleop_and_stop:
def __init__(self):
#self.move_sub = Subscriber()
self.scan_sub = Subscriber('/scan', LaserScan)
#self.scan_sub.registerCallback(self.test_scan_callback)
self.move_pub = rospy.Publisher('/cmd_vel_mux/input/teleop',
Twist,
queue_size=10)
self.teleop_sub = rospy.Subscriber('/my_teleop', Twist, self.add_stamp)
self.altered_input_pub = rospy.Publisher('/my_altered_input',
TwistStamped,
queue_size=10)
self.altered_input_sub = Subscriber('/my_altered_input', TwistStamped)
#self.altered_input_sub.registerCallback(self.altered_input_callback)
self.ats = TimeSynchronizer([self.altered_input_sub, self.scan_sub],
queue_size=10)
self.ats.registerCallback(self.scan_callback)
def add_stamp(self, input):
output = TwistStamped()
output.twist = input
output.header = Header()
output.header.stamp = rospy.Time.now()
self.altered_input_pub.publish(output)
#prin/cmd_vel_mux/input/teleopt "at line 33"
def altered_input_callback(self, input):
print "got altered input"
def test_scan_callback(self, scan):
print "got a scan"
def scan_callback(self, vel, scan): #For some reason it never gets here!
near_wall = False
for i in scan.ranges:
if i <= 0.5 and vel.twist.linear.x >= 0:
#print True
self.move_pub.publish(Twist())
near_wall = True
elif (not near_wall):
self.move_pub.publish(vel.twist)
if __name__ == '__main__':
rospy.init_node('schedule_status_printer')
all_topics = rospy.get_published_topics()
all_tasks = 'task_executor/all_tasks'
current_schedule = 'current_schedule'
use_all_tasks = False
for topic, type in all_topics:
if topic.endswith(all_tasks):
use_all_tasks = True
break
if use_all_tasks:
rospy.Subscriber('mdp_plan_exec/execute_policy/goal',
ExecutePolicyActionGoal, policy_goal_callback)
rospy.Subscriber('mdp_plan_exec/execute_policy/feedback',
ExecutePolicyActionFeedback, policy_feedback_callback)
from message_filters import Subscriber, TimeSynchronizer
ts = TimeSynchronizer([
Subscriber(all_tasks, ExecutionStatus),
Subscriber(current_schedule, ExecutionStatus)
], 1)
ts.registerCallback(all_tasks_cb)
else:
rospy.Subscriber('current_schedule', ExecutionStatus, callback)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
class MappingNode(Mapping):
def __init__(self):
super(MappingNode, self).__init__()
self.lock = threading.RLock()
self.use_slam_traj = True
def init_node(self, ns="~"):
self.use_slam_traj = rospy.get_param(ns + "use_slam_traj", True)
self.x0, self.y0 = rospy.get_param(ns + "origin")
self.width, self.height = rospy.get_param(ns + "size")
self.resolution = rospy.get_param(ns + "resolution")
self.inc = rospy.get_param(ns + "inc")
self.pub_occupancy1 = rospy.get_param(ns + "pub_occupancy1")
self.hit_prob = rospy.get_param(ns + "hit_prob")
self.miss_prob = rospy.get_param(ns + "miss_prob")
self.inflation_angle = rospy.get_param(ns + "inflation_angle")
self.inflation_radius = rospy.get_param(ns + "inflation_range")
self.pub_occupancy2 = rospy.get_param(ns + "pub_occupancy2")
self.inflation_radius = rospy.get_param(ns + "inflation_radius")
self.outlier_filter_radius = rospy.get_param(ns +
"outlier_filter_radius")
self.outlier_filter_min_points = rospy.get_param(
ns + "outlier_filter_min_points")
self.pub_intensity = rospy.get_param(ns + "pub_intensity")
# Only update keyframe that has significant movement
self.min_translation = rospy.get_param(ns + "min_translation")
self.min_rotation = rospy.get_param(ns + "min_rotation")
self.sonar_sub = Subscriber(SONAR_TOPIC, OculusPing)
if self.use_slam_traj:
self.traj_sub = Subscriber(SLAM_TRAJ_TOPIC, PointCloud2)
else:
self.traj_sub = Subscriber(LOCALIZATION_TRAJ_TOPIC, PointCloud2)
# Method 1
if self.pub_occupancy1:
self.feature_sub = Subscriber(SONAR_FEATURE_TOPIC, PointCloud2)
# Method 2
if self.pub_occupancy2:
self.feature_sub = Subscriber(SLAM_CLOUD_TOPIC, PointCloud2)
# The time stamps for trajectory and ping have to be exactly the same
# A big queue_size is required to assure no keyframe is missed especially
# for offline playing.
self.ts = TimeSynchronizer(
[self.traj_sub, self.sonar_sub, self.feature_sub], 100)
self.ts.registerCallback(self.tpf_callback)
self.intensity_map_pub = rospy.Publisher(MAPPING_INTENSITY_TOPIC,
OccupancyGrid,
queue_size=1,
latch=True)
self.occupancy_map_pub = rospy.Publisher(MAPPING_OCCUPANCY_TOPIC,
OccupancyGrid,
queue_size=1,
latch=True)
self.get_map_srv = rospy.Service(ns + "get_map", GetOccupancyMap,
self.get_map)
self.configure()
loginfo("Mapping node is initialized")
def get_map(self, req):
resp = GetOccupancyMapResponse()
with self.lock:
occ_msg = self.get_occupancy_grid(req.frames, req.resolution)
resp.occ = occ_msg
return resp
@add_lock
def tpf_callback(self, traj_msg, ping, feature_msg):
self.lock.acquire()
with CodeTimer("Mapping - add keyframe"):
traj = r2n(traj_msg)
pose = pose322(n2g(traj[-1, :6], "Pose3"))
points = r2n(feature_msg)
self.add_keyframe(len(traj) - 1, pose, ping, points)
with CodeTimer("Mapping - update keyframe"):
for x in range(len(traj) - 1):
pose = pose322(n2g(traj[x, :6], "Pose3"))
self.update_pose(x, pose)
if self.pub_intensity:
with CodeTimer("Mapping - publish intensity map"):
intensity_msg = self.get_intensity_grid()
intensity_msg.header.stamp = ping.header.stamp
if not self.use_slam_traj:
intensity_msg.header.frame_id = "odom"
self.intensity_map_pub.publish(intensity_msg)
if self.pub_occupancy1:
with CodeTimer("Mapping - publish occupancy map"):
occupancy_msg = self.get_occupancy_grid1()
occupancy_msg.header.stamp = ping.header.stamp
if not self.use_slam_traj:
occupancy_msg.header.frame_id = "odom"
self.occupancy_map_pub.publish(occupancy_msg)
if self.pub_occupancy2:
with CodeTimer("Mapping - publish occupancy map"):
occupancy_msg = self.get_occupancy_grid2()
occupancy_msg.header.stamp = ping.header.stamp
if not self.use_slam_traj:
occupancy_msg.header.frame_id = "odom"
self.occupancy_map_pub.publish(occupancy_msg)
# # Why doesn't this work?
# # Delete unnecessary ping cached in time synchronizer since we use a big queue.
# q = self.ts.queues[1]
# self.ts.queues[1] = {
# t: m for t, m in q.items() if t >= ping.header.stamp
# }
self.lock.release()
if self.save_fig:
self.save_submaps()
def save_submaps(self):
submaps = []
for keyframe in self.keyframes:
submap = (
g2n(keyframe.pose),
keyframe.r,
keyframe.c,
keyframe.l,
keyframe.i,
keyframe.cimg,
keyframe.limg,
)
submaps.append(submap)
np.savez(
"step-{}-submaps.npz".format(len(self.keyframes) - 1),
submaps=submaps,
map_size=(self.x0, self.y0, self.width, self.height,
self.resolution),
)
def __init__(self, args, n):
# Auxiliar variables
self.init_scene = False
self.use_gaussian_noise = False
self.filter_hdmap = True
self.display = args[0]
self.trajectory_prediction = args[1]
self.ros = args[2]
self.grid = False
self.image_width = 0 # This value will be updated
self.image_height = 1000 # Pixels
self.shapes = []
self.view_ahead = 0
self.n = np.zeros(
(n, 1)
) # The bounding boxes will be predicted n boxes ahead, regarding its velocity
self.ego_vehicle_x, self.ego_vehicle_y = 0.0, 0.0
self.ego_orientation_cumulative_diff = 0 # Cumulative orientation w.r.t the original odometry of the ego-vehicle (radians)
self.initial_angle = False
self.previous_angle = float(0)
self.pre_yaw = float(0)
self.current_yaw = float(0)
self.ego_trajectory_prediction_bb = np.zeros(
(5, self.n.shape[0])) # (x,y,s,r,theta) n times
self.write_video = False
self.video_flag = False
self.start = float(0)
self.end = float(0)
self.frame_no = 0
self.avg_fps = float(0)
self.colours = np.random.rand(32, 3)
# Emergency break
self.cont = 0
self.collision_flag = std_msgs.msg.Bool()
self.collision_flag.data = False
self.emergency_break_timer = float(0)
self.nearest_object_in_route = 50000
self.ego_braking_distance = 0
self.rc_max = rospy.get_param("/controller/rc_max")
self.geometric_monitorized_area = []
# ROS publishers
self.pub_monitorized_area = rospy.Publisher(
"/t4ac/perception/detection/monitorized_area_marker",
visualization_msgs.msg.Marker,
queue_size=20)
self.pub_bev_sort_tracking_markers_list = rospy.Publisher(
'/t4ac/perception/tracking/obstacles_markers',
visualization_msgs.msg.MarkerArray,
queue_size=20)
self.pub_particular_monitorized_area_markers_list = rospy.Publisher(
'/t4ac/perception/monitors/individual_monitorized_area',
visualization_msgs.msg.MarkerArray,
queue_size=20)
self.pub_collision = rospy.Publisher(
'/t4ac/perception/monitors/predicted_collision',
std_msgs.msg.Bool,
queue_size=20)
self.pub_nearest_object_distance = rospy.Publisher(
'/t4ac/perception/monitors/nearest_object_distance',
std_msgs.msg.Float64,
queue_size=20)
# ROS subscribers
if not self.filter_hdmap:
self.sub_road_curvature = rospy.Subscriber(
"/control/rc", std_msgs.msg.Float64,
self.road_curvature_callback)
self.detections_topic = "/t4ac/perception/detection/merged_obstacles"
self.odom_topic = "/localization/pose"
self.monitorized_lanes_topic = "/mapping_planning/monitor/lanes"
self.detections_subscriber = Subscriber(self.detections_topic,
BEV_detections_list)
self.odom_subscriber = Subscriber(self.odom_topic,
nav_msgs.msg.Odometry)
self.monitorized_lanes_subscriber = Subscriber(
self.monitorized_lanes_topic, MonitorizedLanes)
ts = TimeSynchronizer([
self.detections_subscriber, self.odom_subscriber,
self.monitorized_lanes_subscriber
], header_synchro)
ts.registerCallback(self.callback)
# Listeners
self.listener = tf.TransformListener()
def callback(self, detections_rosmsg, odom_rosmsg,
monitorized_lanes_rosmsg):
"""
"""
try: # Target # Pose
(translation, quaternion) = self.listener.lookupTransform(
'/map', '/ego_vehicle/lidar/lidar1', rospy.Time(0))
# rospy.Time(0) get us the latest available transform
rot_matrix = tf.transformations.quaternion_matrix(quaternion)
self.tf_map2lidar = rot_matrix
self.tf_map2lidar[:3,
3] = self.tf_map2lidar[:3,
3] + translation # This matrix transforms local to global coordinates
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
print("\033[1;33m" + "TF exception" + '\033[0;m')
self.start = time.time()
self.detections_subscriber.unregister()
self.odom_subscriber.unregister()
self.monitorized_lanes_subscriber.unregister()
# Initialize the scene
if not self.init_scene:
self.real_height = detections_rosmsg.front - detections_rosmsg.back # Grid height (m)
self.real_front = detections_rosmsg.front + self.view_ahead # To study obstacles (view_head) meters ahead
self.real_width = -detections_rosmsg.left + detections_rosmsg.right # Grid width (m)
self.real_left = -detections_rosmsg.left
r = float(self.image_height) / self.real_height
self.image_width = int(round(self.real_width *
r)) # Grid width (pixels)
self.image_front = int(round(self.real_front * r))
self.image_left = int(round(self.real_left * r))
print("Real width: ", self.real_width)
print("Real height: ", self.real_height)
print("Image width: ", self.image_width)
print("Image height: ", self.image_height)
self.shapes = (self.real_front, self.real_left, self.image_front,
self.image_left)
self.scale_factor = (self.image_height / self.real_height,
self.image_width / self.real_width)
# Our centroid is defined in BEV camera coordinates but centered in the ego-vehicle.
# We need to traslate it to the top-left corner (required by the SORT algorithm)
self.tf_bevtl2bevcenter_m = np.array(
[[1.0, 0.0, 0.0, self.shapes[1]],
[0.0, 1.0, 0.0, self.shapes[0]], [0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 1.0]])
# From BEV centered to BEV top-left (in pixels)
self.tf_bevtl2bevcenter_px = np.array(
[[1.0, 0.0, 0.0, -self.shapes[3]],
[0.0, 1.0, 0.0, -self.shapes[2]], [0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 1.0]])
# Rotation matrix from LiDAR frame to BEV frame (to convert to global coordinates)
self.tf_lidar2bev = np.array([[0.0, -1.0, 0.0, 0.0],
[-1.0, 0.0, 0.0, 0.0],
[0.0, 0.0, -1.0, 0.0],
[0.0, 0.0, 0.0, 1.0]])
# Initialize the regression model to calculate the braking distance
self.velocity_braking_distance_model = monitors_functions.fit_velocity_braking_distance_model(
)
# Tracker
self.mot_tracker = sort_functions.Sort(max_age, min_hits, n,
self.shapes,
self.trajectory_prediction,
self.filter_hdmap)
self.init_scene = True
output_image = np.ones(
(self.image_height, self.image_width, 3),
dtype="uint8") # Black image to represent the scene
print("----------------")
# Initialize ROS and monitors variables
self.trackers_marker_list = visualization_msgs.msg.MarkerArray(
) # Initialize trackers list
monitorized_area_colours = []
trackers_in_route = 0
nearest_distance = std_msgs.msg.Float64()
nearest_distance.data = float(self.nearest_object_in_route)
world_features = []
trackers = []
dynamic_trackers = []
static_trackers = []
ndt = 0
timer_rosmsg = detections_rosmsg.header.stamp.to_sec()
# Predict the ego-vehicle trajectory
monitors_functions.ego_vehicle_prediction(self, odom_rosmsg,
output_image)
print("Braking distance ego vehicle: ",
float(self.ego_braking_distance))
# Convert input data to bboxes to perform Multi-Object Tracking
#print("\nNumer of total detections: ", len(detections_rosmsg.bev_detections_list))
bboxes_features, types = sort_functions.bbox_to_xywh_cls_conf(
self, detections_rosmsg, odom_rosmsg, detection_threshold,
output_image)
#print("Number of relevant detections: ", len(bboxes_features)) # score > detection_threshold
# Emergency break (Only detection)
"""
nearest_object_in_route = 99999
if not self.collision_flag.data:
for bbox,type_object in zip(bboxes_features,types):
for lane in monitorized_lanes_rosmsg.lanes:
if (lane.role == "current" and len(lane.left.way) >= 2):
detection = Node()
bbox = bbox.reshape(1,-1)
global_pos = sort_functions.store_global_coordinates(self.tf_map2lidar,bbox)
detection.x = global_pos[0,0]
detection.y = -global_pos[1,0] # N.B. In CARLA this coordinate is the opposite
is_inside, particular_monitorized_area, dist2centroid = monitors_functions.inside_lane(lane,detection,type_object)
if is_inside:
#distance_to_object = monitors_functions.calculate_distance_to_nearest_object_inside_route(monitorized_lanes_rosmsg,global_pos)
ego_x_global = odom_rosmsg.pose.pose.position.x
ego_y_global = -odom_rosmsg.pose.pose.position.y
distance_to_object = math.sqrt(pow(ego_x_global-detection.x,2)+pow(ego_y_global-detection.y,2))
print("Distance to object: ", distance_to_object)
if distance_to_object < self.nearest_object_in_route:
self.nearest_object_in_route = distance_to_object
nearest_distance.data = float(self.nearest_object_in_route)
print("Braking distance: ", self.ego_braking_distance)
if distance_to_object < self.ego_braking_distance:
print("Break")
self.collision_flag.data = True
self.pub_collision.publish(self.collision_flag)
self.emergency_break_timer = time.time()
self.cont = 0
break
else:
continue
break
else:
dist = 99999
for bbox,type_object in zip(bboxes_features,types):
for lane in monitorized_lanes_rosmsg.lanes:
if (lane.role == "current" and len(lane.left.way) >= 2):
detection = Node()
bbox = bbox.reshape(1,-1)
global_pos = sort_functions.store_global_coordinates(self.tf_map2lidar,bbox)
detection.x = global_pos[0,0]
detection.y = -global_pos[1,0] # N.B. In CARLA this coordinate is the opposite
is_inside, particular_monitorized_area, dist2centroid = monitors_functions.inside_lane(lane,detection,type_object)
print("inside: ", is_inside)
if is_inside:
ego_x_global = odom_rosmsg.pose.pose.position.x
ego_y_global = -odom_rosmsg.pose.pose.position.y
aux = math.sqrt(pow(ego_x_global-detection.x,2)+pow(ego_y_global-detection.y,2))
if aux < dist:
dist = aux
break
if dist > 5:
self.cont += 1
else:
self.cont = 0
print("distance to object after collision: ", dist)
print("cont: ", self.cont)
if self.cont >= 3:
self.collision_flag.data = False
self.nearest_object_in_route = 50000
nearest_distance.data = float(self.nearest_object_in_route)
print("Data: ", nearest_distance.data)
print("Collision: ", self.collision_flag.data)
self.pub_nearest_object_distance.publish(nearest_distance)
self.pub_collision.publish(self.collision_flag)
"""
## Multi-Object Tracking
# TODO: Publish on tracked_obstacle message instead of visualization marker
# TODO: Evaluate the predicted position to predict its influence in a certain use case
if (len(bboxes_features) > 0): # At least one object was detected
trackers, object_types, object_scores, object_observation_angles, dynamic_trackers, static_trackers = self.mot_tracker.update(
bboxes_features, types, self.ego_vel_px, self.tf_map2lidar,
self.shapes, self.scale_factor, monitorized_lanes_rosmsg,
timer_rosmsg, self.angle_bb, self.geometric_monitorized_area)
#print("Number of trackers: ", len(trackers))
if len(dynamic_trackers.shape) == 3:
#print("Dynamic trackers", dynamic_trackers.shape[1])
ndt = dynamic_trackers.shape[1]
else:
#print("Dynamic trackers: ", 0)
ndt = 0
#print("Static trackers: ", static_trackers.shape[0])
id_nearest = -1
if (len(trackers) > 0): # At least one object was tracked
for i, tracker in enumerate(trackers):
object_type = object_types[i]
object_score = object_scores[i]
object_rotation = np.float64(object_observation_angles[i,
0])
object_observation_angle = np.float64(
object_observation_angles[i, 1])
color = self.colours[tracker[5].astype(int) % 32]
#print("hago append")
monitorized_area_colours.append(color)
if self.ros:
world_features = monitors_functions.tracker_to_topic(
self, tracker, object_type,
color) # world_features (w,l,h,x,y,z,id)
#print("WF: ", world_features)
if kitti:
num_image = detections_rosmsg.header.seq - 1 # Number of image in the dataset, e.g. 0000.txt -> 0
object_properties = object_observation_angle, object_rotation, object_score
monitors_functions.store_kitti(
num_image, path, object_type, world_features,
object_properties)
if (self.display):
my_thickness = -1
geometric_functions.compute_and_draw(
tracker, color, my_thickness, output_image)
label = 'ID %06d' % tracker[5].astype(int)
cv2.putText(
output_image, label,
(tracker[0].astype(int), tracker[1].astype(int) - 20),
cv2.FONT_HERSHEY_PLAIN, 1.5, [255, 255, 255], 2)
cv2.putText(
output_image, object_type,
(tracker[0].astype(int), tracker[1].astype(int) - 40),
cv2.FONT_HERSHEY_PLAIN, 1.5, [255, 255, 255], 2)
# Evaluate if there is some obstacle in lane and calculate nearest distance
if self.filter_hdmap:
if tracker[-1]: # In route, last element of the array
trackers_in_route += 1
obstacle_local_position = np.zeros((1, 9))
obstacle_local_position[0, 7] = world_features[3]
obstacle_local_position[0, 8] = world_features[4]
obstacle_global_position = sort_functions.store_global_coordinates(
self.tf_map2lidar, obstacle_local_position)
#distance_to_object = monitors_functions.calculate_distance_to_nearest_object_inside_route(monitorized_lanes_rosmsg,obstacle_global_position)
detection = Node()
detection.x = obstacle_global_position[0, 0]
detection.y = -obstacle_global_position[1, 0]
ego_x_global = odom_rosmsg.pose.pose.position.x
ego_y_global = -odom_rosmsg.pose.pose.position.y
distance_to_object = math.sqrt(
pow(ego_x_global - detection.x, 2) +
pow(ego_y_global - detection.y, 2))
distance_to_object -= 5 # QUITARLO, DEBERIA SER DISTANCIA CENTROIDE OBJETO A MORRO, EN VEZ DE LIDAR A LIDAR, POR ESO
# LE METO ESTE OFFSET
#print("Distance to object: ", distance_to_object)
if distance_to_object < self.nearest_object_in_route:
id_nearest = tracker[5]
self.nearest_object_in_route = distance_to_object
else:
# Evaluate in the geometric monitorized area
x = world_features[3]
y = world_features[4]
print("main")
print("goemetric area: ",
self.geometric_monitorized_area)
print("x y: ", x, y)
if (x < self.geometric_monitorized_area[0]
and x > self.geometric_monitorized_area[1]
and y < self.geometric_monitorized_area[2]
and y > self.geometric_monitorized_area[3]):
trackers_in_route += 1
self.cont = 0
print("\n\n\nDentro")
distance_to_object = math.sqrt(
pow(x, 2) + pow(y, 2))
print("Nearest: ", self.nearest_object_in_route)
print("distance: ", distance_to_object)
if distance_to_object < self.nearest_object_in_route:
self.nearest_object_in_route = distance_to_object
print("Collision: ", self.collision_flag.data)
print("trackers in route: ", trackers_in_route)
print("Distance nearest: ", self.nearest_object_in_route)
if self.collision_flag.data and (
trackers_in_route == 0 or
(self.nearest_object_in_route > 12 and self.abs_vel < 1)):
print("suma A")
self.cont += 1
else:
self.cont == 0
nearest_distance.data = self.nearest_object_in_route
if (self.trajectory_prediction):
collision_id_list = [[], []]
# Evaluate collision with dynamic trackers
for a in range(dynamic_trackers.shape[1]):
for j in range(self.n.shape[0]):
e = dynamic_trackers[j][a]
color = self.colours[e[5].astype(int) % 32]
my_thickness = 2
geometric_functions.compute_and_draw(
e, color, my_thickness, output_image)
if (self.ros):
object_type = "trajectory_prediction"
monitors_functions.tracker_to_topic(
self, e, object_type, color, j)
# Predict possible collision (including the predicted bounding boxes)
collision_id, index_bb = monitors_functions.predict_collision(
self.ego_predicted, dynamic_trackers[:, a])
if collision_id != -1:
collision_id_list[0].append(collision_id)
collision_id_list[1].append(index_bb)
# Evaluate collision with static trackers
for b in static_trackers:
if b[-1]: # In route, last element of the array
collision_id, index_bb = monitors_functions.predict_collision(
self.ego_predicted, b,
static=True) # Predict possible collision
if (collision_id != -1):
collision_id_list[0].append(collision_id)
collision_id_list[1].append(index_bb)
#if self.nearest_object_in_route < self.ego_braking_distance:
# self.collision_flag.data = True
# Collision
if not self.collision_flag.data:
if not collision_id_list[0]: # Empty
#if id_nearest == -1:
collision_id_list[0].append(
-1
) # The ego-vehicle will not collide with any object
collision_id_list[1].append(-1)
self.collision_flag.data = False
elif collision_id_list[0] and (
self.nearest_object_in_route <
self.ego_braking_distance
or self.nearest_object_in_route < 12):
#elif id_nearest != -1 and (self.nearest_object_in_route < self.ego_braking_distance or self.nearest_object_in_route < 12):
self.collision_flag.data = True
self.cont = 0
#print("Collision id list: ", collision_id_list)
if (len(collision_id_list) > 1):
message = 'Predicted collision with objects: ' + str(
collision_id_list[0])
else:
message = 'Predicted collision with object: ' + str(
collision_id_list[0])
cv2.putText(output_image, message, (30, 140),
cv2.FONT_HERSHEY_PLAIN, 1.5, [255, 255, 255],
2) # Predicted collision message
else:
print("\033[1;33m" + "No object to track" + '\033[0;m')
monitors_functions.empty_trackers_list(self)
if self.collision_flag.data:
print("suma B")
self.cont += 1
else:
print("\033[1;33m" + "No objects detected" + '\033[0;m')
monitors_functions.empty_trackers_list(self)
if self.collision_flag.data:
print("suma C")
self.cont += 1
print("cont: ", self.cont)
if self.cont >= 3:
self.collision_flag.data = False
self.nearest_object_in_route = 50000
nearest_distance.data = float(self.nearest_object_in_route)
self.end = time.time()
fps = 1 / (self.end - self.start)
self.avg_fps += fps
self.frame_no += 1
print("SORT time: {}s, fps: {}, avg fps: {}".format(
round(self.end - self.start, 3), round(fps, 3),
round(self.avg_fps / self.frame_no, 3)))
message = 'Trackers: ' + str(len(trackers))
cv2.putText(output_image, message, (30, 20), cv2.FONT_HERSHEY_PLAIN,
1.5, [255, 255, 255], 2)
message = 'Dynamic trackers: ' + str(ndt)
cv2.putText(output_image, message, (30, 50), cv2.FONT_HERSHEY_PLAIN,
1.5, [255, 255, 255], 2)
try:
message = 'Static trackers: ' + str(static_trackers.shape[0])
except:
message = 'Static trackers: ' + str(0)
cv2.putText(output_image, message, (30, 80), cv2.FONT_HERSHEY_PLAIN,
1.5, [255, 255, 255], 2)
# Publish the list of tracked obstacles and predicted collision
print("Data: ", nearest_distance.data)
print("Collision: ", self.collision_flag.data)
self.pub_bev_sort_tracking_markers_list.publish(
self.trackers_marker_list)
self.pub_collision.publish(self.collision_flag)
self.pub_nearest_object_distance.publish(nearest_distance)
print("moni: ", len(monitorized_area_colours))
self.particular_monitorized_area_list = self.mot_tracker.get_particular_monitorized_area_list(
detections_rosmsg.header.stamp, monitorized_area_colours)
self.pub_particular_monitorized_area_markers_list.publish(
self.particular_monitorized_area_list)
if self.write_video:
if not self.video_flag:
fourcc = cv2.VideoWriter_fourcc(*'MJPG')
self.output = cv2.VideoWriter(
"map-filtered-mot.avi", fourcc, 20,
(self.image_width, self.image_height))
self.output.write(output_image)
# Add a grid to appreciate the obstacles coordinates
if (self.grid):
gap = int(
round(self.view_ahead * (self.image_width / self.real_width)))
geometric_functions.draw_basic_grid(gap, output_image, pxstep=50)
if (self.display):
cv2.imshow("SORT tracking", output_image)
cv2.waitKey(1)
self.detections_subscriber = Subscriber(self.detections_topic,
BEV_detections_list)
self.odom_subscriber = Subscriber(self.odom_topic,
nav_msgs.msg.Odometry)
self.monitorized_lanes_subscriber = Subscriber(
self.monitorized_lanes_topic, MonitorizedLanes)
ts = TimeSynchronizer([
self.detections_subscriber, self.odom_subscriber,
self.monitorized_lanes_subscriber
], header_synchro)
ts.registerCallback(self.callback)
class LaserPublisher(object):
def __init__(self):
if not rospy.core.is_initialized():
rospy.init_node('laser_test')
rospy.loginfo("Initialised rospy node: laser_test")
self.tl = TransformListener()
self.lp = LaserProjection()
# Publishers
self.all_laser_pub = rospy.Publisher('/pepper/laser_2',
LaserScan,
queue_size=1)
self.pc_pub = rospy.Publisher('/cloud', PointCloud2, queue_size=1)
self.pcl_pub = rospy.Publisher('/cloudl', PointCloud2, queue_size=1)
self.pcr_pub = rospy.Publisher('/cloudr', PointCloud2, queue_size=1)
self.pc_redone_pub = rospy.Publisher('/cloud_redone',
PointCloud2,
queue_size=1)
self.pc_rereprojected_pub = rospy.Publisher('/cloud_rereprojected',
PointCloud2,
queue_size=1)
# Subscribers
left_sub = Subscriber('/pepper/scan_left', LaserScan)
front_sub = Subscriber('/pepper/scan_front', LaserScan)
right_sub = Subscriber('/pepper/scan_right', LaserScan)
self.ts = TimeSynchronizer([left_sub, front_sub, right_sub], 10)
rospy.loginfo("Finished intialising")
self.ddr = DDR('increment')
default_increment = radians(120.0 * 2.0) / 61.0
self.ddr.add_variable('angle_increment', '', default_increment, 0.05,
0.08)
# 130.665
self.ddr.add_variable('half_max_angle', '', 120., 115., 145.0)
self.ddr.start(self.dyn_rec_callback)
self.ts.registerCallback(self.scan_cb)
rospy.loginfo("Ready to go.")
def add_variables_to_self(self):
var_names = self.ddr.get_variable_names()
for var_name in var_names:
self.__setattr__(var_name, None)
def dyn_rec_callback(self, config, level):
rospy.loginfo("Received reconf call: " + str(config))
# Update all variables
var_names = self.ddr.get_variable_names()
for var_name in var_names:
self.__dict__[var_name] = config[var_name]
return config
def scan_cb(self, left, front, right):
rospy.loginfo("We got scan_cb")
translated_points = []
try:
pc_left = self.lp.projectLaser(left, channel_options=0x00)
pc_front = self.lp.projectLaser(front, channel_options=0x00)
pc_right = self.lp.projectLaser(right, channel_options=0x00)
except Exception as e:
rospy.logerr("Failed to transform laser scan because: " + str(e))
pc_left.header.stamp = rospy.Time.now()
pc_left.header.frame_id = 'SurroundingLeftLaser_frame'
self.pcl_pub.publish(pc_left)
self.pcr_pub.publish(pc_right)
transform_right_to_front = self.tl.lookupTransform(
'base_footprint', 'SurroundingRightLaser_frame', rospy.Time.now())
rospy.logdebug("Transform Right to Front:")
rospy.logdebug(transform_right_to_front)
ts = TransformStamped()
ts.transform.translation = Vector3(*transform_right_to_front[0])
ts.transform.rotation = Quaternion(*transform_right_to_front[1])
ts.header.stamp = rospy.Time.now()
transformed_cloud = do_transform_cloud(pc_right, ts)
# right point cloud translation
for p in read_points(transformed_cloud,
field_names=('x', 'y', 'z'),
skip_nans=False):
translated_points.append(p)
for i in range(8):
translated_points.append(
(float('nan'), float('nan'), float('nan')))
transform_front_to_front = self.tl.lookupTransform(
'base_footprint', 'SurroundingFrontLaser_frame', rospy.Time.now())
rospy.logdebug("Transform Front to Front:")
rospy.logdebug(transform_front_to_front)
ts = TransformStamped()
ts.transform.translation = Vector3(*transform_front_to_front[0])
ts.transform.rotation = Quaternion(*transform_front_to_front[1])
ts.header.stamp = rospy.Time.now()
transformed_cloud_f = do_transform_cloud(pc_front, ts)
# front point cloud
for p in read_points(transformed_cloud_f,
field_names=('x', 'y', 'z'),
skip_nans=False):
translated_points.append(p)
transform_left_to_front = self.tl.lookupTransform(
'base_footprint', 'SurroundingLeftLaser_frame', rospy.Time.now())
rospy.logdebug("Transform Left to Front:")
rospy.logdebug(transform_left_to_front)
ts = TransformStamped()
ts.transform.translation = Vector3(*transform_left_to_front[0])
ts.transform.rotation = Quaternion(*transform_left_to_front[1])
ts.header.stamp = rospy.Time.now()
from copy import deepcopy
transformed_cloud_l = do_transform_cloud(deepcopy(pc_left), ts)
for i in range(8):
translated_points.append(
(float('nan'), float('nan'), float('nan')))
# left pc translation
for p in read_points(transformed_cloud_l,
field_names=('x', 'y', 'z'),
skip_nans=False):
translated_points.append(p)
# Create a point cloud from the combined points wrt the front
# laser frame
pc_front.header.frame_id = 'base_footprint'
point_cloud = create_cloud_xyz32(pc_front.header, translated_points)
self.pc_pub.publish(point_cloud)
rospy.logdebug("pointcloud all together len: " +
str(point_cloud.width))
# # double check we have the same thing
# compare_str = "\n"
# for idx, (tp, pcp) in enumerate(zip(translated_points, read_points(point_cloud))):
# compare_str += str(idx).zfill(2) + ":\n"
# compare_str += " tp : " + str(tp)
# compare_str += "\n pcp: " + str(pcp) + "\n"
# rospy.loginfo(compare_str)
# # OK we know they are the same
# # translated_points and point_cloud contain virtually the same data
# Convert combined point cloud into LaserScan
all_laser_msg = LaserScan()
laser_ranges, angle_min, angle_max, angle_increment = self.pc_to_laser(
point_cloud)
all_laser_msg.header.frame_id = 'base_footprint'
all_laser_msg.header.stamp = rospy.Time.now()
all_laser_msg.ranges = laser_ranges
all_laser_msg.angle_min = angle_min
all_laser_msg.angle_max = angle_max
all_laser_msg.angle_increment = angle_increment
all_laser_msg.range_min = 0.1
all_laser_msg.range_max = 7.0
all_laser_msg.intensities = []
self.all_laser_pub.publish(all_laser_msg)
rospy.logdebug("all_laser_msg len: " + str(len(all_laser_msg.ranges)))
pc_redone = self.lp.projectLaser(all_laser_msg, channel_options=0x00)
rospy.logdebug("all_laser pc_redone len: " + str(pc_redone.width))
self.pc_redone_pub.publish(pc_redone)
# compare what came in and what came out
rospy.logdebug("point_cloud frame_id, pc_redone frame_id:")
rospy.logdebug(
(point_cloud.header.frame_id, pc_redone.header.frame_id))
rospy.logdebug("point_cloud is correct, pc_redone is incorrect")
compare_str = "\n"
for idx, (point_in, point_out) in enumerate(
zip(read_points(point_cloud), read_points(pc_redone))):
point_out = [point_out[0], point_out[1], 0.0]
point_in = [point_in[0], point_in[1], 0.0]
compare_str += str(idx).zfill(2) + ":\n"
compare_str += " in : " + str(point_in)
compare_str += "\n out: " + str(point_out) + "\n"
dist = np.linalg.norm(np.array(point_out) - np.array(point_in))
compare_str += " dist: " + str(dist) + "\n"
# angle
angle1 = atan2(point_in[1], point_in[0])
angle2 = atan2(point_out[1], point_out[0])
angle_dif = angle2 - angle1
compare_str += " angle dif: " + str(angle_dif) + "\n"
rospy.logdebug(compare_str)
def pc_to_laser(self, cloud):
laser_points = []
points_rereprojected = []
multiply_num_rays = 8
num_rays = 61 * multiply_num_rays
laser_points2 = [float('nan')] * num_rays
min_angle = -radians(self.half_max_angle)
max_angle = radians(self.half_max_angle)
# angle_increment = self.angle_increment
angle_increment = (radians(self.half_max_angle) *
2.0) / float(num_rays)
big_str = "\n"
for idx, p in enumerate(read_points(cloud, skip_nans=False)):
#dist = self.get_dist(p[0], p[1])
p = [p[0], p[1], 0.0]
dist = np.linalg.norm(np.array((0., 0., 0.)) - np.array(p))
# dist1 = self.get_dist(p[0], p[1])
big_str += " " + str(idx).zfill(2) + ": x: " + str(round(
p[0], 2)) + ", y: " + str(round(p[1], 2)) + ", z: " + str(
round(p[2], 2)) + " = " + str(round(
dist, 2)) + "m (at " + str(
round(degrees(idx * angle_increment + min_angle),
2)) + "deg)\n"
laser_points.append(dist)
# coords from dist
x = dist * cos(idx * angle_increment + min_angle)
y = dist * sin(idx * angle_increment + min_angle)
print(" [ px, py, are the correct points ] ")
print("dist, px, py: " + str(dist) + " " + str(p[0])) + " " + str(
p[1])
print("dist, x, y: " + str(dist) + " " + str(x) + " " + str(y))
dist_from_rereproj = self.get_dist(x, y)
print("dist rereproj: " + str(dist_from_rereproj))
# print("dist1 : " + str(dist1))
# what if a make a pointcloud based in the cos sin version
points_rereprojected.append((x, y, 0.0))
# angle from point
angle = atan2(p[1], p[0])
# angle2 = atan2(y, x)
expected_angle = idx * self.angle_increment + min_angle
if not isnan(angle):
tmp_angle = angle - min_angle
print("tmp_angle: " + str(degrees(tmp_angle))) + " deg"
print("angle_increment: " + str(degrees(angle_increment)))
closest_index = int(tmp_angle / angle_increment)
print("closest index: " + str(closest_index))
if closest_index >= len(laser_points2):
laser_points2[-1] = dist
elif closest_index < 0:
laser_points2[0] = dist
else:
laser_points2[closest_index] = dist
else:
print("nan, not adding anything to scan")
# laser_points[]
print("Angle from p : " + str(round(degrees(angle), 2)))
# print("Angle from xy: " + str(round(degrees(angle2), 2)))
print("Expected angle: " + str(round(degrees(expected_angle), 2)))
rospy.logdebug("Lasered cloud")
rospy.logdebug(big_str)
laser_points = laser_points2
print("Len of laser points after new technique: " +
str(len(laser_points)))
rereprojected_pc = PointCloud2()
rereprojected_pc.header.frame_id = 'base_footprint'
rereprojected_pc.header.stamp = rospy.Time.now()
point_cloud_rere = create_cloud_xyz32(rereprojected_pc.header,
points_rereprojected)
self.pc_rereprojected_pub.publish(point_cloud_rere)
return laser_points, min_angle, max_angle, angle_increment
def get_dist(self, x0, y0, x1=0.0, y1=0.0):
return sqrt((x1 - x0)**2 + (y1 - y0)**2)
class PCLChecker(object):
def __init__(self):
rospy.init_node('pcl_checker', anonymous=True, disable_signals=True)
# Original laser scan
lleft_sub = Subscriber('/pepper/scan_left', LaserScan)
lfront_sub = Subscriber('/pepper/scan_front', LaserScan)
lright_sub = Subscriber('/pepper/scan_right', LaserScan)
# Original point cloud
pcleft_sub = Subscriber('/pepper/pc_left', PointCloud2)
pcfront_sub = Subscriber('/pepper/pc_front', PointCloud2)
pcright_sub = Subscriber('/pepper/pc_right', PointCloud2)
# Translated point cloud
pcall_sub = Subscriber('/cloud_in', PointCloud2)
self.ts = TimeSynchronizer([
lleft_sub, lfront_sub, lright_sub, pcleft_sub, pcfront_sub,
pcright_sub, pcall_sub
], 10)
self.ts.registerCallback(self.check_values)
def get_dist(self, x0, y0, x1=0, y1=0):
return sqrt((x1 - x0)**2 + (y1 - y0)**2)
def check_values(self, olleft, olfront, olright, opc_right, opc_left,
opc_front, allpc):
# Front
olfront_angles = []
opc_dist = []
opc_angles = []
# Left
olleft_angles = []
opcleft_dist = []
opcleft_angles = []
# Right
olright_angles = []
opcright_dist = []
opcright_angles = []
# Combined
alllaser_dist = []
alllaser_angles = []
allpc_dist = []
allpc_angles = []
# Right
for i in range(len(olright.ranges)):
alllaser_dist.append(olright.ranges[i])
alllaser_angles.append(olright.angle_min +
i * olright.angle_increment - 1.757)
olright_angles.append(olright.angle_min +
i * olright.angle_increment - 1.757)
for p in read_points(opc_right, skip_nans=True):
distance = self.get_dist(p[0], p[1])
opcright_dist.append(distance)
opcright_angles.append(atan2(p[1], p[0]) - 1.757)
# Front
for i in range(len(olfront.ranges)):
alllaser_dist.append(olfront.ranges[i])
alllaser_angles.append(olfront.angle_min +
i * olfront.angle_increment)
olfront_angles.append(olfront.angle_min +
i * olfront.angle_increment)
for p in read_points(opc_front, skip_nans=True):
opc_dist.append(self.get_dist(p[0], p[1]))
opc_angles.append(atan2(p[1], p[0]))
# Left
for i in range(len(olleft.ranges)):
alllaser_dist.append(olleft.ranges[i])
alllaser_angles.append(olleft.angle_min +
i * olleft.angle_increment + 1.757)
olleft_angles.append(olleft.angle_min +
i * olleft.angle_increment + 1.757)
for p in read_points(opc_left, skip_nans=True):
distance = self.get_dist(p[0], p[1])
opcleft_dist.append(distance)
opcleft_angles.append(atan2(p[1], p[0]) + 1.757)
# All combined
for p in read_points(allpc, skip_nans=True):
allpc_dist.append(self.get_dist(p[0], p[1]))
allpc_angles.append(atan2(p[1], p[0]))
rospy.loginfo(
"===============================================================")
rospy.loginfo("original right laser")
rospy.loginfo(olright.ranges)
rospy.loginfo(olright_angles)
rospy.loginfo(
"===============================================================")
rospy.loginfo("original front laser")
rospy.loginfo(olfront.ranges)
rospy.loginfo(olfront_angles)
rospy.loginfo(
"===============================================================")
rospy.loginfo("original left laser")
rospy.loginfo(olleft.ranges)
rospy.loginfo(olleft_angles)
rospy.loginfo(
"===============================================================")
rospy.loginfo("all combined laser")
rospy.loginfo(alllaser_dist)
rospy.loginfo(alllaser_angles)
rospy.loginfo(
"==============================================================")
rospy.loginfo("original right point cloud")
rospy.loginfo(opcright_dist)
rospy.loginfo(opcright_angles)
rospy.loginfo(
"===============================================================")
rospy.loginfo("original front point cloud")
rospy.loginfo(opc_dist)
rospy.loginfo(opc_angles)
rospy.loginfo(
"===============================================================")
rospy.loginfo("original left point cloud")
rospy.loginfo(opcleft_dist)
rospy.loginfo(opcleft_angles)
rospy.loginfo(
"===============================================================")
rospy.loginfo("all combined point cloud")
rospy.loginfo(allpc_dist)
rospy.loginfo(allpc_angles)
rospy.loginfo(
"===============================================================")
class ClassifierNode:
def __init__(self):
self.cv_bridge = CvBridge()
self.path_sign_model = rospy.get_param('~traffic_sign_path')
self.path_light_model = rospy.get_param('~traffic_light_path')
self.cuda_devices = {
0: 'cuda:0',
1: 'cuda:1',
2: 'cuda:2',
3: 'cuda:3'
}
gpu_id = rospy.get_param('~gpu_id')
self.device = torch.device(self.cuda_devices[gpu_id])
self.num_traffic_light = rospy.get_param('~num_traffic_light_class')
self.model_traffic_light = STN(self.num_traffic_light).to(self.device)
state_dict_light = torch.load(self.path_light_model)
self.model_traffic_light.load_state_dict(state_dict_light)
self.model_traffic_light.eval().to(self.device)
self.model_traffic_light.cuda().to(self.device)
self.num_traffic_sign = rospy.get_param('~num_traffic_sign_class')
self.model_traffic_sign = STN(self.num_traffic_sign).to(self.device)
state_dict_sign = torch.load(self.path_sign_model)
self.model_traffic_sign.load_state_dict(state_dict_sign)
self.model_traffic_sign.eval().to(self.device)
self.model_traffic_sign.cuda().to(self.device)
self.pub_fm01_new_bbox = rospy.Publisher(
node_handle_name + '/cam_fm_01/detection_2d_array/',
Detection2DArray,
queue_size=1)
self.pub_fl01_new_bbox = rospy.Publisher(
node_handle_name + '/cam_fl_01/detection_2d_array/',
Detection2DArray,
queue_size=1)
self.pub_fr01_new_bbox = rospy.Publisher(
node_handle_name + '/cam_fr_01/detection_2d_array/',
Detection2DArray,
queue_size=1)
self.topic_fm01_img = rospy.get_param('~topic_name_fm01_img')
self.topic_fm01_bbox = rospy.get_param('~topic_name_fm01_bbox')
self.sub_image_fm01 = Subscriber(self.topic_fm01_img, Image)
self.sub_bbox_fm01 = Subscriber(self.topic_fm01_bbox, Detection2DArray)
self.time_sync_fm01 = TimeSynchronizer(
[self.sub_image_fm01, self.sub_bbox_fm01], 10)
self.time_sync_fm01.registerCallback(self.callback_synchronize,
"cam_fm_01")
self.topic_fl01_img = rospy.get_param('~topic_name_fl01_img')
self.topic_fl01_bbox = rospy.get_param('~topic_name_fl01_bbox')
self.sub_image_fl01 = Subscriber(self.topic_fl01_img, Image)
self.sub_bbox_fl01 = Subscriber(self.topic_fl01_bbox, Detection2DArray)
self.time_sync_fl01 = TimeSynchronizer(
[self.sub_image_fl01, self.sub_bbox_fl01], 10)
self.time_sync_fl01.registerCallback(self.callback_synchronize,
"cam_fl_01")
self.topic_fr01_img = rospy.get_param('~topic_name_fr01_img')
self.topic_fr01_bbox = rospy.get_param('~topic_name_fr01_bbox')
self.sub_image_fr01 = Subscriber(self.topic_fr01_img, Image)
self.sub_bbox_fr01 = Subscriber(self.topic_fr01_bbox, Detection2DArray)
self.time_sync_fr01 = TimeSynchronizer(
[self.sub_image_fr01, self.sub_bbox_fr01], 10)
self.time_sync_fr01.registerCallback(self.callback_synchronize,
"cam_fr_01")
def callback_synchronize(self, msg_img, msg_bbox, cam_id):
cv_img = self.cv_bridge.imgmsg_to_cv2(msg_img)
new_bbox = Detection2DArray()
print("aaaaa")
for detection in msg_bbox.detections:
box = Detection2D()
box.bbox = detection.bbox
pose = ObjectHypothesisWithPose()
print(detection.results[0].id)
if detection.results[0].id == 7: # id:7 Traffic light
x_min = int(detection.bbox.center.x -
(detection.bbox.size_x / 2))
y_min = int(detection.bbox.center.y -
(detection.bbox.size_y / 2))
x_max = int(detection.bbox.center.x +
(detection.bbox.size_x / 2))
y_max = int(detection.bbox.center.y +
(detection.bbox.size_y / 2))
img_cropped = cv_img[y_min:y_max, x_min:x_max]
preprocessed_img = self.preprocess_img(img=img_cropped)
predict_f = self.model_traffic_light(preprocessed_img)
predict = predict_f.data.max(1, keepdim=True)[1]
pose.id = int(1000 + int(predict))
cnn_prob = float(torch.exp(predict_f)[0][int(predict)])
pose.score = cnn_prob
box.results.append(pose)
# pose.id = int(1000 + int(predict))
# box.results.append(pose)
if self.validate_light_with_brightness_region(
img_cropped, int(predict)):
new_bbox.detections.append(box)
elif detection.results[0].id == 8: # id:8 Traffic sign
x_min = int(detection.bbox.center.x -
(detection.bbox.size_x / 2))
y_min = int(detection.bbox.center.y -
(detection.bbox.size_y / 2))
x_max = int(detection.bbox.center.x +
(detection.bbox.size_x / 2))
y_max = int(detection.bbox.center.y +
(detection.bbox.size_y / 2))
img_cropped = cv_img[y_min:y_max, x_min:x_max]
preprocessed_img = self.preprocess_img(img=img_cropped)
predict_f = self.model_traffic_sign(preprocessed_img)
predict = predict_f.data.max(1, keepdim=True)[1]
cnn_prob = float(torch.exp(predict_f)[0][int(predict)])
pose.score = cnn_prob
pose.id = int(2000 + int(predict))
box.results.append(pose)
new_bbox.detections.append(box)
else:
box = detection
new_bbox.detections.append(box)
new_bbox.header = msg_bbox.header
if cam_id == "cam_fm_01":
self.pub_fm01_new_bbox.publish(new_bbox)
elif cam_id == "cam_fr_01":
self.pub_fr01_new_bbox.publish(new_bbox)
elif cam_id == "cam_fl_01":
self.pub_fl01_new_bbox.publish(new_bbox)
def preprocess_img(self, img):
loader = transforms.Compose([
transforms.Resize((32, 32)),
transforms.ToTensor(),
transforms.Normalize((0.3337, 0.3064, 0.3171),
(0.2672, 0.2564, 0.2629))
])
img = PilImage.fromarray(img)
img = loader(img).float()
img = Variable(img)
img = img.unsqueeze(0)
return img.cuda(device=self.device)
def validate_light_with_brightness_region(self, rgb_image, light_id):
if light_id == 0:
return True
hsv = cv2.cvtColor(rgb_image, cv2.COLOR_RGB2HSV)
v = hsv[:, :, 2]
height, width = v.shape
height_div3 = int((height - 6) / 3)
red_region = v[3:3 + height_div3, 3:width - 3]
yellow_region = v[3 + height_div3:3 + height_div3 * 2, 3:width - 3]
green_region = v[3 + height_div3 * 2:3 + height_div3 * 3, 3:width - 3]
sum_brightness_red = np.sum(red_region)
avg_brightness_red = sum_brightness_red / (height_div3 * width)
sum_brightness_yellow = np.sum(yellow_region)
avg_brightness_yellow = sum_brightness_yellow / (height_div3 * width)
sum_brightness_green = np.sum(green_region)
avg_brightness_green = sum_brightness_green / (height_div3 * width)
# print(sum_brightness_red)
# print(sum_brightness_yellow)
# print(sum_brightness_green)
if avg_brightness_red >= avg_brightness_yellow and avg_brightness_red >= avg_brightness_green:
brightness_predict = 1
elif avg_brightness_yellow >= avg_brightness_green:
brightness_predict = 2
else:
brightness_predict = 3
if brightness_predict == light_id:
return True
else:
return False
Image,
queue_size=100)
vis_render_0 = rospy.Publisher(CONFIG["vis_topic_0"],
Image,
queue_size=100)
kps_pub_1 = rospy.Publisher(CONFIG["kps_topic_1"],
KeypointsList,
queue_size=100)
hm_render_1 = rospy.Publisher(CONFIG["hm_topic_1"], Image, queue_size=100)
img_render_1 = rospy.Publisher(CONFIG["hm_img_topic_1"],
Image,
queue_size=100)
vis_render_1 = rospy.Publisher(CONFIG["vis_topic_1"],
Image,
queue_size=100)
# define the sync subscriber(google `message filter` for more info)
tss_0 = TimeSynchronizer([
Subscriber(CONFIG["image_src_0"], Image),
Subscriber(CONFIG["bbxes_src_0"], BoundingBoxes)
], 200)
tss_0.registerCallback(callback_0)
tss_1 = TimeSynchronizer([
Subscriber(CONFIG["image_src_1"], Image),
Subscriber(CONFIG["bbxes_src_1"], BoundingBoxes)
], 200)
tss_1.registerCallback(callback_1)
rospy.spin()
exit(0)
if __name__ == "__main__":
parser = init_argparse()
args, unknown = parser.parse_known_args()
rospy.init_node('object_finder')
pose_pub = rospy.Publisher(POS_TOPIC, PoseStamped, queue_size=1)
line_pub = rospy.Publisher(MARKER_TOPIC, Marker, queue_size=1)
tss = TimeSynchronizer([
Subscriber(IMAGE_TOPIC, Image),
Subscriber(CAMERA_INFO_TOPIC, CameraInfo)
], 10)
tss.registerCallback(image_callback)
bridge = CvBridge()
print('----------------------------------------------')
print('OpenCV object detector')
print('Controls:')
print(' - S Save current config')
print(' - ESC Exit without saving current config')
print('----------------------------------------------')
config = Settings.load(args.config, default=make_default_config())
if config == None:
exit(1)
if args.gui == 'full':
KeypointsList,
queue_size=100)
kps_pub_1 = rospy.Publisher(CONFIG["kps_data_1"],
KeypointsList,
queue_size=100)
rospy.init_node('cropper', anonymous=True)
crop_length = CONFIG["crop_length"]
tss_0 = TimeSynchronizer([
Subscriber(CONFIG["img_topic_0_remap"], Image),
Subscriber(CONFIG["img_topic_1_remap"], Image),
Subscriber(CONFIG["kps_data_0_remap"], KeypointsList),
Subscriber(CONFIG["kps_data_1_remap"], KeypointsList)
], 200)
tss_0.registerCallback(callback)
rospy.spin()
# rate = rospy.Rate(CONFIG["hz"])
# # I don't know why we have to set image scale fixed
# # Or else it seems like the yolo will fail at recognize varied scales imgs(you can try comment cv2.resize(..))
# pre_width=CONFIG["pre_width"]
# pre_height=CONFIG["pre_height"]
# # fetch the image name lists
# name_list_0=[]
# name_list_1=[]
# if CONFIG["list_path_0"]!="":
# with open(CONFIG["list_path_0"],"r") as f_0:
# name_list_0 = f_0.readlines()
image6_sub = Subscriber('/camera6/color/image_raw_throttle_sync', Image)
aligned_depth1_sub = Subscriber('/camera1/aligned_depth_to_color/image_raw',
Image)
aligned_depth2_sub = Subscriber('/camera2/aligned_depth_to_color/image_raw',
Image)
aligned_depth3_sub = Subscriber('/camera3/aligned_depth_to_color/image_raw',
Image)
aligned_depth4_sub = Subscriber('/camera4/aligned_depth_to_color/image_raw',
Image)
aligned_depth5_sub = Subscriber('/camera5/aligned_depth_to_color/image_raw',
Image)
aligned_depth6_sub = Subscriber('/camera6/aligned_depth_to_color/image_raw',
Image)
ts1 = TimeSynchronizer([image1_sub, aligned_depth1_sub], 10)
ts2 = TimeSynchronizer([image2_sub, aligned_depth2_sub], 10)
ts3 = TimeSynchronizer([image3_sub, aligned_depth3_sub], 10)
ts4 = TimeSynchronizer([image4_sub, aligned_depth4_sub], 10)
ts5 = TimeSynchronizer([image5_sub, aligned_depth5_sub], 10)
ts6 = TimeSynchronizer([image6_sub, aligned_depth6_sub], 10)
ts1.registerCallback(pubLishImages1)
ts2.registerCallback(pubLishImages2)
ts3.registerCallback(pubLishImages3)
ts4.registerCallback(pubLishImages4)
ts5.registerCallback(pubLishImages5)
ts6.registerCallback(pubLishImages6)
rospy.spin()
