class Map_Filtered_MOT:
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 road_curvature_callback(self, msg):
"""
"""
rc = msg.data
rc_ratio = rc / self.rc_max
self.geometric_monitorized_area
xmax = float(rc_ratio * self.ego_braking_distance * 1.4)
if xmax > 30:
xmax = 30
elif xmax < 12:
xmax = 12
lateral = 2.6
xmin = 0
ymin = rc_ratio * (-lateral)
ymax = rc_ratio * lateral
self.geometric_monitorized_area = [xmax, xmin, ymax, ymin]
geometric_monitorized_area_marker = visualization_msgs.msg.Marker()
geometric_monitorized_area_marker.header.frame_id = "/ego_vehicle/lidar/lidar1"
geometric_monitorized_area_marker.ns = "geometric_monitorized_area"
geometric_monitorized_area_marker.action = geometric_monitorized_area_marker.ADD
geometric_monitorized_area_marker.lifetime = rospy.Duration.from_sec(1)
geometric_monitorized_area_marker.type = geometric_monitorized_area_marker.CUBE
geometric_monitorized_area_marker.color.r = 1.0
geometric_monitorized_area_marker.color.g = 1.0
geometric_monitorized_area_marker.color.b = 1.0
geometric_monitorized_area_marker.color.a = 0.4
geometric_monitorized_area_marker.pose.position.x = xmax / 2
geometric_monitorized_area_marker.pose.position.y = (ymin + ymax) / 2
geometric_monitorized_area_marker.pose.position.z = -2.0
geometric_monitorized_area_marker.scale.x = xmax
geometric_monitorized_area_marker.scale.y = abs(ymin) + ymax
geometric_monitorized_area_marker.scale.z = 0.2
self.pub_monitorized_area.publish(geometric_monitorized_area_marker)
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 RosStateMachine(StateMachine):
def __init__(self,
states,
transitions,
start,
outcomes,
camera=None,
optical_flow=None):
StateMachine.__init__(self, states, transitions, start, outcomes)
self.__last_output = Userdata()
self.__bridge = CvBridge()
# Publishers
self.__delta_pub = tf.TransformBroadcaster()
self.__debug_pub = rospy.Publisher('/debug_image', Image, queue_size=1)
#self.__pid_input_pub = rospy.Publisher('/pid_input', controller_msg,
# queue_size=1)
self.__state_pub = rospy.Publisher('/statemachine',
String,
queue_size=1)
if camera is None:
self.__camera = Camera.from_ros()
else:
self.__camera = camera
#if optical_flow is None:
# self.__optical_flow = OpticalFlow.from_ros()
#else:
# self.__optical_flow = optical_flow
self.__config_max_height = rospy.get_param('config/max_height')
def execute(self):
self.__height_sub = Subscriber('height', Range)
self.__image_sub = Subscriber('image', Image)
self.__sync = ApproximateTimeSynchronizer(
[self.__height_sub, self.__image_sub], queue_size=1, slop=0.05)
self.__sync.registerCallback(self.__callback)
rospy.spin()
def __callback(self, range_msg, image_msg):
"""
Receives ROS messages and advances the state machine.
"""
self.__state_pub.publish(self.current_state)
if self.current_state == self.FINAL_STATE:
rospy.loginfo('Final state reached.')
rospy.signal_shutdown('Final state reached.')
self.__height_sub.unregister()
self.__image_sub.unregister()
del self.__sync
return
u = self.create_userdata(range_msg=range_msg, image_msg=image_msg)
self.__last_output = self(u)
def publish_debug(self, userdata, image_callback, *args, **kwargs):
if self.__debug_pub.get_num_connections() <= 0:
return
image = image_callback(userdata, *args, **kwargs)
if image is None:
return
img_msg = self.__bridge.cv2_to_imgmsg(image, encoding='bgr8')
self.__debug_pub.publish(img_msg)
def publish_delta(self, x, y, z, theta):
if np.isnan(x):
# TODO: Fix this
rospy.logerr('Publish delta : Got bad x: {0}'.format(x))
#print 'BAD X?', x, repr(x)
#traceback.print_stack()
return
x, y = -x, -y
rospy.loginfo('Publish delta : x {0}, y {1}, z {2}, theta {3}'.format(
x, y, z, theta))
q = tf.transformations.quaternion_from_euler(0, 0, theta)
# TODO: camera frame (down/forward)
camera_frame = 'downward_cam_optical_frame'
self.__delta_pub.sendTransform((x, y, z), q, rospy.Time.now(),
'waypoint', camera_frame)
# TODO: remove legacy topic publish
msg = controller_msg()
msg.x = -y
msg.y = x
msg.z = z
msg.t = theta
#self.__pid_input_pub.publish(msg)
def publish_delta__keep_height(self, userdata, x, y, theta, target_height):
delta_z = target_height - userdata.height_msg.range
self.publish_delta(x, y, delta_z, theta)
def create_userdata(self, **kwargs):
u = Userdata(self.__last_output)
u.publish_debug_image = lambda cb, *args, **kwargs: self.publish_debug(
u.image, cb, *args, **kwargs)
u.publish_delta = self.publish_delta
u.publish_delta__keep_height = lambda x, y, theta, target_height: self.publish_delta__keep_height(
u, x, y, theta, target_height)
u.height_msg = kwargs['range_msg']
u.max_height = self.__config_max_height
u.camera = self.__camera
#u.optical_flow = self.__optical_flow
try:
u.image = self.__bridge.imgmsg_to_cv2(kwargs['image_msg'],
desired_encoding='bgr8')
except CvBridgeError as error:
rospy.logerror(error)
return None
return u
class RosStateMachine(StateMachine):
def __init__(self, states, transitions, start, outcomes, camera=None,
optical_flow=None):
StateMachine.__init__(self, states, transitions, start, outcomes)
self.__last_output = Userdata()
self.__bridge = CvBridge()
# Publishers
self.__delta_pub = tf.TransformBroadcaster()
self.__debug_pub = rospy.Publisher('/debug_image', Image, queue_size=1)
#self.__pid_input_pub = rospy.Publisher('/pid_input', controller_msg,
# queue_size=1)
self.__state_pub = rospy.Publisher('/statemachine', String, queue_size=1)
if camera is None:
self.__camera = Camera.from_ros()
else:
self.__camera = camera
#if optical_flow is None:
# self.__optical_flow = OpticalFlow.from_ros()
#else:
# self.__optical_flow = optical_flow
self.__config_max_height = rospy.get_param('config/max_height')
def execute(self):
self.__height_sub = Subscriber('height', Range)
self.__image_sub = Subscriber('image', Image)
self.__sync = ApproximateTimeSynchronizer([self.__height_sub,
self.__image_sub],
queue_size=1,
slop=0.05)
self.__sync.registerCallback(self.__callback)
rospy.spin()
def __callback(self, range_msg, image_msg):
"""
Receives ROS messages and advances the state machine.
"""
self.__state_pub.publish(self.current_state)
if self.current_state == self.FINAL_STATE:
rospy.loginfo('Final state reached.')
rospy.signal_shutdown('Final state reached.')
self.__height_sub.unregister()
self.__image_sub.unregister()
del self.__sync
return
u = self.create_userdata(range_msg=range_msg,
image_msg=image_msg)
self.__last_output = self(u)
def publish_debug(self, userdata, image_callback, *args, **kwargs):
if self.__debug_pub.get_num_connections() <= 0:
return
image = image_callback(userdata, *args, **kwargs)
if image is None:
return
img_msg = self.__bridge.cv2_to_imgmsg(image, encoding='bgr8')
self.__debug_pub.publish(img_msg)
def publish_delta(self, x, y, z, theta):
if np.isnan(x):
# TODO: Fix this
rospy.logerr('Publish delta : Got bad x: {0}'.format(x))
#print 'BAD X?', x, repr(x)
#traceback.print_stack()
return
x, y = -x, -y
rospy.loginfo('Publish delta : x {0}, y {1}, z {2}, theta {3}'
.format(x, y, z, theta))
q = tf.transformations.quaternion_from_euler(0, 0, theta)
# TODO: camera frame (down/forward)
camera_frame = 'downward_cam_optical_frame'
self.__delta_pub.sendTransform((x, y, z),
q,
rospy.Time.now(),
'waypoint',
camera_frame)
# TODO: remove legacy topic publish
msg = controller_msg()
msg.x = -y
msg.y = x
msg.z = z
msg.t = theta
#self.__pid_input_pub.publish(msg)
def publish_delta__keep_height(self, userdata, x, y, theta, target_height):
delta_z = target_height - userdata.height_msg.range
self.publish_delta(x, y, delta_z, theta)
def create_userdata(self, **kwargs):
u = Userdata(self.__last_output)
u.publish_debug_image = lambda cb, *args, **kwargs: self.publish_debug(u.image, cb, *args, **kwargs)
u.publish_delta = self.publish_delta
u.publish_delta__keep_height = lambda x, y, theta, target_height: self.publish_delta__keep_height(u, x, y, theta, target_height)
u.height_msg = kwargs['range_msg']
u.max_height = self.__config_max_height
u.camera = self.__camera
#u.optical_flow = self.__optical_flow
try:
u.image = self.__bridge.imgmsg_to_cv2(kwargs['image_msg'],
desired_encoding='bgr8')
except CvBridgeError as error:
rospy.logerror(error)
return None
return u