def main():
# Declare the global variable
global result
global count
# Set the init
count = 1
result = {"detection": []}
# Using mesage filters to get multiple image
depth_image_sub = message_filters.Subscriber('/camera/aligned_depth_to_color/image_raw', Image)
bb_sub = message_filters.Subscriber('/darknet_ros/bounding_boxes', BoundingBoxes)
target_image = message_filters.Subscriber('/darknet_ros/detection_image', Image)
joy_sub = message_filters.Subscriber('/joy', Joy)
# Synchronize the topic and throw into the callback
ts = message_filters.ApproximateTimeSynchronizer([depth_image_sub, bb_sub, target_image, joy_sub], 1, 100)
ts.registerCallback(callback)
rospy.spin()
# Write out the Json file
with open("result.json", 'w') as output:
json.dump(result, output)
def __init__(self):
super(ProjectionNode, self).__init__()
# init the node
rospy.init_node('projection_node', anonymous=False)
self._bridge = CvBridge()
(depth_topic, face_topic, output_topic, f, cx, cy) = \
self.get_parameters()
# Subscribe to the face positions
sub_obj = message_filters.Subscriber(face_topic,\
DetectionArray)
sub_depth = message_filters.Subscriber(depth_topic,\
Image)
# Advertise the result of Face Depths
self.pub = rospy.Publisher(output_topic, \
DetectionArray, queue_size=1)
# Create the message filter
ts = message_filters.ApproximateTimeSynchronizer(\
[sub_obj, sub_depth], \
2, \
0.9)
ts.registerCallback(self.detection_callback)
self.f = f
self.cx = cx
self.cy = cy
# spin
rospy.spin()
def listener():
# In ROS, nodes are uniquely named. If two nodes with the same
# name are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'listener' node so that multiple listeners can
# run simultaneously.
rospy.init_node('listener', anonymous=True)
symbol = message_filters.Subscriber('/scan', LaserScan)
odom = message_filters.Subscriber('/odom', Odometry)
#odom = message_filters.Subscriber('/hsrb/odom',Odometry)
#pose = message_filters.Subscriber('/navigation/localization/amcl_pose',PoseWithCovarianceStamped)#TAKESHI REAL
#odom= message_filters.Subscriber("/hsrb/wheel_odom",Odometry)
#ats= message_filters.ApproximateTimeSynchronizer([symbol,odom,twist],queue_size=5,slop=.1,allow_headerless=True)
ats = message_filters.ApproximateTimeSynchronizer([symbol, odom],
queue_size=5,
slop=.1,
allow_headerless=True)
ats.registerCallback(callback)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
def on_start(self, args, time):
print("Start estimator for {}".format(self._camera))
topic = "/{}".format(self._camera)
self._pubImage = rospy.Publisher(topic + "/marker_image",
ImageMsg,
queue_size=1)
self._pubMarker = rospy.Publisher(topic + "/marker_pose",
PoseMsg,
queue_size=1)
sub1 = mf.Subscriber(topic + "/rgb/camera_info",
CameraInfoMsg,
queue_size=3)
sub2 = mf.Subscriber(topic + "/rgb/image_raw", ImageMsg, queue_size=3)
sub3 = mf.Subscriber(topic + "/depth_registered/image_raw",
ImageMsg,
queue_size=3)
sub4 = mf.Subscriber(topic + "/depth_registered/points",
PointCloud2Msg,
queue_size=3)
self._subCamera = [sub1, sub2, sub3, sub4]
self._sync = mf.ApproximateTimeSynchronizer(self._subCamera, 3, 0.01)
self._sync.registerCallback(self.__callback)
def __init__(self):
#rospy.loginfo("[%s] Initializing " %(self.node_name))
self.bridge = CvBridge()
#msg = rospy.wait_for_message('/X1/rgbd_camera/rgb/camera_info', CameraInfo, timeout=None)
msg = rospy.wait_for_message('/camera/color/camera_info', CameraInfo, timeout=None)
self.fx = msg.P[0]
self.fy = msg.P[5]
self.cx = msg.P[2]
self.cy = msg.P[6]
#-------point cloud without color-------
#self.depth_sub = rospy.Subscriber("/camera/depth/image_rect_raw", Image, self.img_cb, queue_size = 1, buff_size = 2**24)
#self.depth_sub = rospy.Subscriber("/X1/rgbd_camera/depth/image_raw", Image, self.img_cb, queue_size = 1, buff_size = 2**24)
#------------------------------------
#-------point cloud with color-------
self.depth_sub = message_filters.Subscriber("/camera/aligned_depth_to_color/image_raw", Image)
self.image_sub = message_filters.Subscriber("/camera/color/image_raw", Image)
self.ts = message_filters.ApproximateTimeSynchronizer([self.image_sub, self.depth_sub], 1, 0.3)
self.ts.registerCallback(self.img_cb)
#------------------------------------
self.pc_pub = rospy.Publisher("/pointcloud2_transformed", PointCloud2, queue_size=1)
self.points = []
def __init__(self, node):
self._logger = node.get_logger()
self._node = node
self._logger.info("Init ObjectFilter()")
self._pub_object_topic = self._node.create_publisher(
Objects, '/object_map/filtered_object')
sub_detection = message_filters.Subscriber(
self._node, ObjectsInBoxes,
'/movidius_ncs_stream/detected_objects')
sub_tracking = message_filters.Subscriber(
self._node, TrackedObjects, '/object_analytics/tracking')
sub_localization = message_filters.Subscriber(
self._node, ObjectsInBoxes3D, '/object_analytics/localization')
object_filter = message_filters.ApproximateTimeSynchronizer(
[sub_detection, sub_tracking, sub_localization],
10,
0.01,
allow_headerless=False)
object_filter.registerCallback(self._callback)
def __init__(self):
rospy.init_node('object_subscriber', anonymous=True)
self.object_sub1 = message_filters.Subscriber("/object_coord1",
Float64MultiArray)
self.object_sub2 = message_filters.Subscriber("/object_coord2",
Float64MultiArray)
self.object_true_x = message_filters.Subscriber(
"/target2/x2_position_controller/command", Float64)
self.object_true_y = message_filters.Subscriber(
"/target2/y2_position_controller/command", Float64)
self.object_true_z = message_filters.Subscriber(
"/target2/z2_position_controller/command", Float64)
self.object_x = rospy.Publisher(
"/target2/x_mea_position_controller/command",
Float64,
queue_size=10)
self.object_y = rospy.Publisher(
"/target2/y_mea_position_controller/command",
Float64,
queue_size=10)
self.object_z = rospy.Publisher(
"/target2/z_mea_position_controller/command",
Float64,
queue_size=10)
self.object_ts = message_filters.ApproximateTimeSynchronizer(
[
self.object_sub1, self.object_sub2, self.object_true_x,
self.object_true_y, self.object_true_z
],
10,
slop=0.1,
allow_headerless=True)
self.object_ts.registerCallback(self.object_callback)
def __init__(self, folder):
if (folder != None):
self.folder = folder
self.bridge = CvBridge()
self.string_pub_ = rospy.Publisher('sync_msg', String, queue_size=2)
# command subscriber
if rospy.has_param('twist_topic'):
self.twist_topic = rospy.get_param('twist_topic')
rospy.loginfo('Parameter %s has value %s',
rospy.resolve_name('twist_topic'), self.twist_topic)
self.twist_sub_ = message_filters.Subscriber(self.twist_topic,
TwistStamped)
# image subscriber
if rospy.has_param('/robot/img_topic'):
self.img_topic = rospy.get_param('/robot/img_topic')
rospy.loginfo('Parameter %s has value %s',
rospy.resolve_name('img_topic'), self.img_topic)
self.img_sub_ = message_filters.Subscriber(self.img_topic,
CompressedImage)
# sync topics
self.ts = message_filters.ApproximateTimeSynchronizer(
[self.img_sub_, self.twist_sub_], 5, 0.5)
print('registering callback...')
self.ts.registerCallback(self.callback)
#self.grayscale = rospy.get_param("grayscale")
print('opening file for targets...')
# init table for targets and ids
with open(self.target_file, 'w') as f:
writer = csv.writer(f)
writer.writerow(['id', 'linear', 'angular', 'target'])
print('topics: ', self.img_topic, ' -- ', self.twist_topic)
rospy.loginfo('Calculated z %f, phi %f and theta %f', u.thrust,
u.orientation.x, u.orientation.y)
if __name__ == '__main__':
rospy.init_node('altitude_pid', anonymous=True)
rate = rospy.Rate(20)
ref = rospy.get_param('~alt_ref')
name = rospy.get_param('~name')
pub = rospy.Publisher('/mambo_con/' + name + '/zd_input',
AttitudeTarget,
queue_size=10)
posesub = message_filters.Subscriber('/vrpn_client_node/' + name + '/pose',
PoseStamped)
twistsub = message_filters.Subscriber(
'/vrpn_client_node/' + name + '/twist', TwistStamped)
# dsub = message_filters.Subscriber('/vrpn_client_node/' + name + '/accel', TwistStamped)
ats = message_filters.ApproximateTimeSynchronizer([posesub, twistsub],
queue_size=10,
slop=0.1)
ats.registerCallback(mocapcb)
rospy.spin()
rospy.loginfo('Tracking altitude: %f' % ref)
# while not rospy.is_shutdown():
# ## Add the computation of u.
# rospy.loginfo('Computation at %s' % rospy.get_time())
# rospy.spin()
def __init__(self):
# Subscriber
# self.pc_sub = rospy.Subscriber("/kitti/oxts/gps/vel",TwistStamped, self.IMUcallback, queue_size=1)
# self.imu_sub = rospy.Subscriber("/kitti/oxts/gps/vel",TwistStamped, self.IMUcallback, queue_size=1)
self.imu_sub = message_filters.Subscriber("/kitti/oxts/gps/vel",
TwistStamped)
self.det_front_sub = message_filters.Subscriber(
"/detection/lidar_detector/objects_markers_front", MarkerArray)
self.det_back_sub = message_filters.Subscriber(
"/detection/lidar_detector/objects_markers", MarkerArray)
# Publisher
self.pub_det_markerarray = rospy.Publisher('pillar_marker_array',
BoundingBoxArray,
queue_size=1)
self.pub_frame_seq = rospy.Publisher('kitti_frame_seq',
OverlayText,
queue_size=1)
self.pub_boxes = rospy.Publisher('kitti_box_track',
BoundingBoxArray,
queue_size=1)
self.pub_pictograms = rospy.Publisher('kitti_box_pictogram_track',
PictogramArray,
queue_size=1)
self.pub_selfvelo_text = rospy.Publisher('kitti_selfvelo_text_track',
Marker,
queue_size=1)
self.pub_selfveloDirection = rospy.Publisher(
'kitti_selfvelo_direction_track', Marker, queue_size=1)
self.pub_objs_ori = rospy.Publisher('kitti_objs_ori_track',
MarkerArray,
queue_size=3)
self.pub_objs_velo = rospy.Publisher('kitti_objs_velo_track',
MarkerArray,
queue_size=1)
self.pub_path = rospy.Publisher('kitti_objs_path_track',
MarkerArray,
queue_size=1)
self.pub_warning_lines = rospy.Publisher('kitti_warning_lines_track',
MarkerArray,
queue_size=1)
self.pub_ego_outCircle = rospy.Publisher('kitti_ego_outCircle_track',
Marker,
queue_size=1)
self.pub_ego_innerCircle = rospy.Publisher(
'kitti_ego_innerCircle_track', Marker, queue_size=1)
self.pub_ego_car = rospy.Publisher('kitti_ego_car_track',
Marker,
queue_size=1)
# Publisher & Subscriber Wrapper
pub_list = [self.det_front_sub, self.det_back_sub, self.imu_sub]
# pub_list = [self.det_front_sub, self.det_back_sub]
# pub_list = [self.det_front_sub]
self.ts = message_filters.ApproximateTimeSynchronizer(
pub_list, 1, 0.1, allow_headerless=True)
# self.ts = message_filters.TimeSynchronizer(pub_list, 10)
# self.ts = message_filters.Cache(self.det_front_sub, 10, allow_headerless=False)
# self.ts = message_filters.Cache(self.det_back_sub, 10, allow_headerless=False)
self.ts.registerCallback(self.callback)
self.tf2 = TransformListener() # for callback function
# Multi-Objects tracking instance
self.mot_tracker = AB3DMOT()
with open(config_file, 'r') as f:
f.readline()
config = yaml.load(f)
lens = config['lens']
fx = float(config['fx'])
fy = float(config['fy'])
cx = float(config['cx'])
cy = float(config['cy'])
k1 = float(config['k1'])
k2 = float(config['k2'])
p1 = float(config['p1/k3'])
p2 = float(config['p2/k4'])
K = np.matrix([[fx, 0.0, cx],
[0.0, fy, cy],
[0.0, 0.0, 1.0]])
D = np.array([k1, k2, p1, p2])
print("Camera parameters")
print("Lens = %s" % lens)
print("K =")
print(K)
print("D =")
print(D)
pub = rospy.Publisher("/reprojection", Image, queue_size=1)
scan_sub = message_filters.Subscriber(scan_topic, LaserScan, queue_size=1)
image_sub = message_filters.Subscriber(image_topic, Image, queue_size=1)
ts = message_filters.ApproximateTimeSynchronizer([scan_sub, image_sub], 10, time_diff)
ts.registerCallback(callback)
rospy.spin()
route.reverse()
waypoint = WayPoint()
waypointlist = WayPointList()
print "------------------------Start--------------------------"
for i in route:
print "x = ", i[0], ", y = ", i[1]
waypoint = WayPoint()
waypoint.x = i[0]
waypoint.y = i[1]
waypointlist.list.append(waypoint)
print "-----------------------Arrive--------------------------"
pub_waypoint.publish(waypointlist)
start = None
nodes = []
# if python says run, then we should run
if __name__ == '__main__':
rospy.init_node('rrt', anonymous=False)
pub_waypoint = rospy.Publisher('/waypoint_list',
WayPointList,
queue_size=1)
obs_sub = message_filters.Subscriber("/obstacle_list", ObstaclePoseList)
pos_sub = message_filters.Subscriber("/pose", PoseStamped)
ats = message_filters.ApproximateTimeSynchronizer([obs_sub, pos_sub],
queue_size=1,
slop=1)
ats.registerCallback(call_back)
print "f**k"
rospy.spin()
print(im.dtype, im[0,0,0])
Im_to_ros = Image()
try:
Im_to_ros = bridge.cv2_to_imgmsg(im, "bgr8")
except CvBridgeError as e:
print (e)
Im_to_ros.header.stamp = rospy.Time.now()
Im_to_ros.header.frame_id = 'camera_link'
pub_dr.publish(Im_to_ros)
print ('Image detection output saved to {}'.format(out_file_name))
rospy.init_node('rgb_detection')
DetectInit()
rgb_sub = message_filters.Subscriber('camera/color/image_raw', Image)
#rgb_sub = message_filters.Subscriber('camera/infra1/image_rect_raw', Image)
pc_sub = message_filters.Subscriber('/camera/depth_registered/points', PointCloud2)
#depth_sub = message_filters.Subscriber('camera/depth/image_rect_raw', Image)
pub = rospy.Publisher('rgb_detection/enemy_position', ObjectList, queue_size=1)
pub_dr = rospy.Publisher('rgb_detection/detection_result', Image, queue_size=1)
#TODO 在后面的试验中调调整slop
TsDet = message_filters.ApproximateTimeSynchronizer([rgb_sub,pc_sub],queue_size=5, slop=0.1, allow_headerless=False)
TsDet.registerCallback(TsDet_callback)
rospy.spin()
if mc.DRAW_Video:
video.release()
cv2.destroyAllWindows()
def __init__(self,
image_namespace,
depth_namespace=None,
score_thresh=0.5,
service_name=default_service_name,
vis=False):
self._service_name = service_name
stem = image_namespace.split('/')[-1]
self.namespace = "rasberry_perception/" + stem + "/"
self.score_thresh = score_thresh
# Wait for connection to detection service
self.detector = Client()
self.vis = vis
if self.vis:
# Discard frame if worker busy
self.publisher_tasks = WorkerTaskQueue(num_workers=1,
max_size=1,
discard=True)
rospy.on_shutdown(self.on_shutdown)
# Initialise publishers
self.detections_pub = rospy.Publisher(self.namespace + "detections",
Detections,
queue_size=1)
self.image_pub = rospy.Publisher(self.namespace + "colour/image_raw",
Image,
queue_size=1)
self.image_info_pub = rospy.Publisher(self.namespace +
"colour/camera_info",
CameraInfo,
queue_size=1)
self.detections_vis_pub = rospy.Publisher(self.namespace +
"colour/vis_raw",
Image,
queue_size=1)
# Initialise subscribers
subscribers = [
message_filters.Subscriber(image_namespace + "/image_raw", Image),
message_filters.Subscriber(image_namespace + "/camera_info",
CameraInfo),
]
self.depth_enabled = depth_namespace is not None
if self.depth_enabled:
self.depth_pub = rospy.Publisher(self.namespace +
"depth/image_raw",
Image,
queue_size=1)
self.depth_info_pub = rospy.Publisher(self.namespace +
"depth/camera_info",
CameraInfo,
queue_size=1)
self.depth_bbox_detections_pub = rospy.Publisher(self.namespace +
"bbox_detections",
PoseArray,
queue_size=1)
self.depth_segm_detections_pub = rospy.Publisher(self.namespace +
"segm_detections",
PoseArray,
queue_size=1)
subscribers.extend([
message_filters.Subscriber(depth_namespace + "/image_raw",
Image),
message_filters.Subscriber(depth_namespace + "/camera_info",
CameraInfo),
])
# Start subscription
sync_queue, sync_thresh = 1, 0.1
rospy.loginfo(
"Waiting for topics with time synchroniser (queue {}, {}s tolerance) on '{}'"
.format(sync_queue, sync_thresh,
', '.join([s.topic for s in subscribers])))
self.ts = message_filters.ApproximateTimeSynchronizer(
subscribers, 1, 0.1)
self.ts.registerCallback(self.run_detector)
def main():
global _ultrasound_image, _robot_pose, _new_pose_available
rospy.init_node('z_wire_to_pose', anonymous=True)
marker_pub = rospy.Publisher('z_wire_pose', Marker, queue_size = 1)
marker_pub_phantom = rospy.Publisher('z_wire_phantom', Marker, queue_size = 1)
robo_sub = message_filters.Subscriber('/pose_ee', TransformStamped)
ultrasound_sub = message_filters.Subscriber('/camera/image_raw', Image)
ts = message_filters.ApproximateTimeSynchronizer([robo_sub, ultrasound_sub], 10, 1/15)
ts.registerCallback(_sync_cb)
last_im_points = np.zeros((9,2))
# last_im_points = [[521, 261],
# [722, 261],
# [797, 262],
# [523, 366],
# [603, 365],
# [797, 363],
# [592, 471],
# [727, 468],
# [794, 469]]
points_tracked = [False] * 9
rate = rospy.Rate(30)
centroids = []
def on_click(event, x, y, p1, p2):
n_tracked_points = np.sum(np.equal(points_tracked, True))
no_free_centroids = len(centroids) <= n_tracked_points
all_points_tracked = n_tracked_points == len(points_tracked)
if event != cv2.EVENT_LBUTTONDBLCLK or no_free_centroids or all_points_tracked:
return
print("Clicked %d, %d" % (x,y))
try:
idx = points_tracked.index(False)
except ValueError:
return
points_tracked[idx] = True
last_im_points[idx] = [x,y]
print("Starting")
while not rospy.is_shutdown():
marker_pub_phantom.publish(make_wire_marker(np.eye(4), 10))
rate.sleep()
if not _new_pose_available:
continue
centroids = find_centroids(_ultrasound_image)
for pt in centroids:
cv2.circle(_ultrasound_image, tuple(pt), 3, color=(255,0,0), thickness=1)
for idx, pt in enumerate(last_im_points):
if points_tracked[idx]:
new_pt = find_closest_point(centroids, pt)
if np.linalg.norm(new_pt - pt) < 10:
last_im_points[idx] = new_pt
else:
points_tracked[idx] = False
if np.sum(np.equal(points_tracked, True)) == len(points_tracked):
im_points = centralize_and_scale(np.array(last_im_points), np.array([388,134]))
coords_3d = img_3d_coords_zFrame(im_points)
print(last_im_points, im_points, coords_3d)
if coords_3d is not None:
pos = find_img_frame_position(im_points, coords_3d)
rot = find_img_frame_axis(im_points, coords_3d)
T = np.eye(4)
T[0:3,0:3] = rot
T[0:3, 3] = pos
marker_pub.publish(make_pose_marker(T, 10))
colors = [np.random.rand(3,)*255 for x in last_im_points]
for pt, tracked in zip(last_im_points, points_tracked):
if tracked:
color = (0,255,0)
else:
color = (0,0,255)
cv2.circle(_ultrasound_image, tuple([int(x) for x in pt]), 5, color=color,thickness=2)
cv2.imshow('Ultrasound Image',_ultrasound_image)
cv2.setMouseCallback('Ultrasound Image', on_click)
k = cv2.waitKey(1) & 0xFF
if k == 27:
break
_new_pose_available = False
# Finding the grid coordinates corresponding to world coordinate
for key, value in grid_world_coor.items():
if value['world_coor'] == bot_goal[:2]:
grid_goal= key
if value['world_coor'] == bot_init[:2]:
grid_init= key
# Initiating the A* algorithm
a_star(grid= grid, init= grid_init[::-1], goal= grid_goal[::-1])
if len(grid_path)>0:
# Printing the calculated path
for each in grid_path:
grid[each[0]][each[1]] = '_'
print(np.array(grid))
# Converting grid coordinates into world coordinates
world_path= [grid_world_coor[(y, x)]['world_coor'] if (grid_world_coor[(y, x)]['world_coor'] != (-1, -4)) else (-0.5,-4) for (x, y) in grid_path[::-1]]
# Initiating subscribers for topics /base_scan & /odom
scan_sub= message_filters.Subscriber('/base_scan', LaserScan)
odom_sub= message_filters.Subscriber('/odom', Odometry)
# Initiating a synchronizer to get the messages in sync
sub_sync= message_filters.ApproximateTimeSynchronizer([scan_sub, odom_sub], 10, 0.1, True)
sub_sync.registerCallback(sync_callback)
# Runs the session infinitely
rospy.spin()
def __init__(self):
super().__init__("trajectory_visualizer")
self.fig = plt.figure()
self.max_vel = 0.0
self.min_vel = 0.0
self.min_accel = 0.0
self.max_accel = 0.0
self.min_jerk = 0.0
self.max_jerk = 0.0
# update flag
self.update_ex_vel_lim = False
self.update_lat_acc_fil = False
self.update_traj_raw = False
self.update_traj_resample = False
self.update_traj_final = False
self.update_lanechange_path = False
self.update_behavior_path = False
self.update_traj_ob_avoid = False
self.update_traj_ob_stop = False
self.tf_buffer = Buffer(node=self)
self.tf_listener = TransformListener(self.tf_buffer,
self,
spin_thread=True)
self.self_pose = Pose()
self.self_pose_received = False
self.localization_vx = 0.0
self.vehicle_vx = 0.0
self.trajectory_external_velocity_limited = Trajectory()
self.trajectory_lateral_acc_filtered = Trajectory()
self.trajectory_raw = Trajectory()
self.trajectory_time_resampled = Trajectory()
self.trajectory_final = Trajectory()
self.lane_change_path = PathWithLaneId()
self.behavior_path = Path()
self.obstacle_avoid_traj = Trajectory()
self.obstacle_stop_traj = Trajectory()
self.plotted = [False] * 9
self.sub_localization_twist = self.create_subscription(
Odometry, "/localization/kinematic_state",
self.CallbackLocalizationTwist, 1)
self.sub_vehicle_twist = self.create_subscription(
VelocityReport, "/vehicle/status/velocity_status",
self.CallbackVehicleTwist, 1)
# BUFFER_SIZE = 65536*100
optimizer_debug = "/planning/scenario_planning/motion_velocity_smoother/debug/"
self.sub1 = message_filters.Subscriber(
self, Trajectory,
optimizer_debug + "trajectory_external_velocity_limited")
self.sub2 = message_filters.Subscriber(
self, Trajectory,
optimizer_debug + "trajectory_lateral_acc_filtered")
self.sub3 = message_filters.Subscriber(
self, Trajectory, optimizer_debug + "trajectory_raw")
self.sub4 = message_filters.Subscriber(
self, Trajectory, optimizer_debug + "trajectory_time_resampled")
self.sub5 = message_filters.Subscriber(
self, Trajectory, "/planning/scenario_planning/trajectory")
lane_driving = "/planning/scenario_planning/lane_driving"
self.sub6 = message_filters.Subscriber(
self, PathWithLaneId,
lane_driving + "/behavior_planning/path_with_lane_id")
self.sub7 = message_filters.Subscriber(
self, Path, lane_driving + "/behavior_planning/path")
self.sub8 = message_filters.Subscriber(
self,
Trajectory,
lane_driving +
"/motion_planning/obstacle_avoidance_planner/trajectory",
)
self.sub9 = message_filters.Subscriber(
self, Trajectory, "/planning/scenario_planning/trajectory")
self.ts1 = message_filters.ApproximateTimeSynchronizer(
[self.sub1, self.sub2, self.sub3, self.sub4, self.sub5], 30, 0.5)
self.ts1.registerCallback(self.CallbackMotionVelOptTraj)
self.ts2 = message_filters.ApproximateTimeSynchronizer(
[self.sub6, self.sub7, self.sub8, self.sub9], 30, 1, 0)
self.ts2.registerCallback(self.CallBackLaneDrivingTraj)
# main process
if PLOT_TYPE == "VEL_ACC_JERK":
self.ani = animation.FuncAnimation(self.fig,
self.plotTrajectory,
interval=100,
blit=True)
self.setPlotTrajectory()
else:
self.ani = animation.FuncAnimation(self.fig,
self.plotTrajectoryVelocity,
interval=100,
blit=True)
self.setPlotTrajectoryVelocity()
plt.show()
return
def calibrate_triple_azure(self, msg):
self.is_finish = False
rospy.loginfo("Initial calibration between azures")
calibrate_1 = rospy.ServiceProxy('/azure1/extrinsic_calibration',
ExtrinsicCalibrate)
calibrate_2 = rospy.ServiceProxy('/azure2/extrinsic_calibration',
ExtrinsicCalibrate)
calibrate_3 = rospy.ServiceProxy('/azure3/extrinsic_calibration',
ExtrinsicCalibrate)
# initial calibration
all_sucess = False
while not all_sucess:
is_sucess_1 = calibrate_1(self.fiducial_ids)
is_sucess_2 = calibrate_2(self.fiducial_ids)
is_sucess_3 = calibrate_3(self.fiducial_ids)
all_sucess = is_sucess_1 or is_sucess_2 or is_sucess_3
rospy.sleep(1.0)
# refine the calibration using ICP b/w markers
rospy.loginfo("ICP b/w markers for calibration refinement ")
points_sub_1 = message_filters.Subscriber('/azure1/points2',
PointCloud2,
buff_size=1280 * 720 * 3)
points_sub_2 = message_filters.Subscriber('/azure2/points2',
PointCloud2,
buff_size=1280 * 720 * 3)
points_sub_3 = message_filters.Subscriber('/azure3/points2',
PointCloud2,
buff_size=1280 * 720 * 3)
fidvertices_sub_1 = message_filters.Subscriber(
'/azure1/fiducial_vertices', FiducialArray)
fidvertices_sub_2 = message_filters.Subscriber(
'/azure2/fiducial_vertices', FiducialArray)
fidvertices_sub_3 = message_filters.Subscriber(
'/azure3/fiducial_vertices', FiducialArray)
self.icp_subs = [
points_sub_1, points_sub_2, points_sub_3, fidvertices_sub_1,
fidvertices_sub_2, fidvertices_sub_3
]
self.ts = message_filters.ApproximateTimeSynchronizer([
points_sub_1, points_sub_2, points_sub_3, fidvertices_sub_1,
fidvertices_sub_2, fidvertices_sub_3
],
queue_size=1,
slop=2)
self.ts.registerCallback(self.icp_with_markers)
# wait for ICP results
while not self.is_finish:
pass
rospy.loginfo("Extrinisic calibration finished ")
if rospy.get_param('~publish_cloud'):
# merge pointclouds and publish it
rospy.loginfo("Merging pointclouds according to the ICP results")
points_sub_1 = message_filters.Subscriber('/azure1/points2',
PointCloud2,
buff_size=1280 * 720 * 3)
points_sub_2 = message_filters.Subscriber('/azure2/points2',
PointCloud2,
buff_size=1280 * 720 * 3)
points_sub_3 = message_filters.Subscriber('/azure3/points2',
PointCloud2,
buff_size=1280 * 720 * 3)
self.merge_subscribers = [points_sub_1, points_sub_2, points_sub_3]
self.ts = message_filters.ApproximateTimeSynchronizer(
[points_sub_1, points_sub_2, points_sub_3],
queue_size=1,
slop=1)
self.ts.registerCallback(self.merge_pointcloud)
return self.is_finish
a = [1]
return a
sum = reduce(lambda x, y: x + y, a)
for i in np.arange(np.shape(a)[0]):
a[i] = a[i] / sum
return a
def normalize2(self, a):
if min(a) < 0:
for i in np.arange(len(a)):
a[i] = (a[i] - min(a)) / (max(a) - min(a))
return a
if __name__ == "__main__":
MulTraj_ge = MulTraj_Gene()
rospy.loginfo("The class has been initialized!")
ts = message_filters.ApproximateTimeSynchronizer(
[MulTraj_ge.info_sub, MulTraj_ge.odom_sub], 10,
0.1) #,allow_headerless=True)
rospy.loginfo("Approximate Time!")
ts.registerCallback(MulTraj_ge.info_odom_callback)
rospy.loginfo("Callback function!")
#rospy.logerr(MulTraj_ge.pose.header.frame_id)
#MulTraj_ge.listener.waitForTransform("map","thermal_camera",rospy.Time(),rospy.Duration(2.0))
# while not rospy.is_shutdown():
# while not rospy.is_shutdown():
rospy.spin()
def image_callback(img, pose):
#rospy.loginfo(img.header)
rospy.loginfo(pose.pose.position)
try:
cv_image = bridge.imgmsg_to_cv2(img, "passthrough")
except CvBridgeError, e:
rospy.logerr("CvBridge Error: {0}".format(e))
#Pose_estimate(cv_image)
RGB_img = cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB)
Pose_estimate(RGB_img)
#cv2.waitKey(1)
#def pose_callback(pose):
# rospy.loginfo(pose.pose.position.x)
#sub_image=rospy.Subscriber("/downward_cam/camera/image",Image,image_callback)
#sub_pose=rospy.Subscriber("/ground_truth_to_tf/pose",PoseStamped,pose_callback)
#createWindow()
sub_image = message_filters.Subscriber("/downward_cam/camera/image", Image)
sub_pose = message_filters.Subscriber("/ground_truth_to_tf/pose", PoseStamped)
sync = message_filters.ApproximateTimeSynchronizer([sub_image, sub_pose], 10,
3)
sync.registerCallback(image_callback)
while not rospy.is_shutdown():
rospy.init_node('info')
rospy.spin()
def __init__(self, sim: Optional[simulation] = None):
self.observation_space = spaces.Dict({
# Non-priviliged Space
'target_dir':
spaces.Box(low=-np.inf * np.ones((2, )),
high=np.inf * np.ones((2, )),
dtype=np.float32),
'turn_dir':
spaces.Discrete(3),
'gravity_vector':
spaces.Box(low=-np.inf * np.ones((3, )),
high=np.inf * np.ones((3, )),
dtype=np.float32),
'angular_vel':
spaces.Box(low=-np.inf * np.ones((3, )),
high=np.inf * np.ones((3, )),
dtype=np.float32),
'linear_vel':
spaces.Box(low=-np.inf * np.ones((3, )),
high=np.inf * np.ones((3, )),
dtype=np.float32),
'joint_angles':
spaces.Box(low=np.zeros((12, )),
high=2 * np.pi * np.ones((12, )),
dtype=np.float32),
'joint_vels':
spaces.Box(low=-np.inf * np.ones((12, )),
high=np.inf * np.ones((12, )),
dtype=np.float32),
'ftg_phases':
spaces.Box(low=-np.ones((8, )),
high=np.ones((8, )),
dtype=np.float32),
'ftg_freqs':
spaces.Box(low=-np.inf * np.ones((4, )),
high=np.inf * np.ones((4, )),
dtype=np.float32),
'base_freq':
spaces.Box(low=-np.inf, high=np.inf, shape=(1, ), dtype=np.float),
'joint_err_hist':
spaces.Box(low=np.zeros((self.JOINT_ERR_HISTORY_LEN, 12)),
high=2 * np.pi * np.ones(
(self.JOINT_ERR_HISTORY_LEN, 12)),
dtype=np.float),
'joint_vel_hist':
spaces.Box(low=-np.inf * np.ones((self.JOINT_VEL_HISTORY_LEN, 12)),
high=np.inf * np.ones((self.JOINT_VEL_HISTORY_LEN, 12)),
dtype=np.float),
'foot_target_hist':
spaces.Box(low=-np.inf * np.ones((self.FOOT_HISTORY_LEN, 4, 3)),
high=np.inf * np.ones((self.FOOT_HISTORY_LEN, 4, 3)),
dtype=np.float),
'toes_contact':
spaces.MultiBinary(4),
'thighs_contact':
spaces.MultiBinary(4),
'shanks_contact':
spaces.MultiBinary(4),
# Priviliged Space
'normal_foot':
spaces.Box(low=-np.inf * np.ones((4, 3)),
high=np.inf * np.ones((4, 3)),
dtype=np.float32),
'height_scan':
spaces.Box(low=-np.inf * np.ones((4, 9)),
high=np.inf * np.ones((4, 9)),
dtype=np.float32),
'foot_forces':
spaces.Box(low=-np.inf * np.ones((4, )),
high=np.inf * np.ones((4, )),
dtype=np.float32),
'foot_friction':
spaces.Box(low=-np.inf * np.ones((4, )),
high=np.inf * np.ones((4, )),
dtype=np.float32),
'foot_friction':
spaces.Box(low=-np.inf * np.ones((3, )),
high=np.inf * np.ones((3, )),
dtype=np.float32)
})
self.action_space = spaces.Box(low=-float('inf') * np.ones((16, )),
high=float('inf') * np.ones((16, )),
dtype=np.float32)
self.H = np.zeros(
(HISTORY_LEN, controller_neural_network.NORMAL_DATA_SHAPE))
self.foot_target_hist = np.zeros((self.FOOT_HISTORY_LEN, 4, 3))
self.joint_vel_hist = np.zeros((self.JOINT_VEL_HISTORY_LEN, 12))
self.joint_err_hist = np.zeros((self.JOINT_ERR_HISTORY_LEN, 12))
self.joint_velocities = np.zeros((12, ))
self.transf_matrix = np.zeros((4, 4, 4))
self.external_force = np.zeros((3, ))
self.ftg_phases = np.zeros((8, ))
self.ftg_freqs = np.zeros((4, ))
self.base_freq = BASE_FREQ
self.gravity_vector = GRAVITY_VECTOR
self.target_dir = np.zeros((2, ))
self.timestep = 0
self.E_v = []
self.sim = sim
if self.sim == None:
# ROS publisher node that update the spot mini joints
self.reset_pub = rospy.Publisher('reset_simulation',
text,
queue_size=QUEUE_SIZE)
# ROS publisher node that update the spot mini joints
self.joints_pub = rospy.Publisher('spot_joints',
joint_angles,
queue_size=QUEUE_SIZE)
normal_data_sub = message_filters.Subscriber(
'normal_data', normal_data)
priviliged_data_sub = message_filters.Subscriber(
'priviliged_data', priviliged_data)
timestep_sub = message_filters.Subscriber('timestep', timestep)
ts = message_filters.ApproximateTimeSynchronizer(
[timestep_sub, normal_data_sub, priviliged_data_sub],
queue_size=QUEUE_SIZE,
slop=0.1,
allow_headerless=True)
ts.registerCallback(self.__update_obs_ros)
else:
self.count = 0
self.begin_time = time()
self.__update_obs_sim()
import os
import sys
rospy.init_node('sync_odom_with_clock')
if len(sys.argv) < 3:
print("usage: sync_odom_with_clock.py filename_out topic")
exit(1)
filename_out = os.path.join(sys.path[0], sys.argv[1])
topic = sys.argv[2]
rospy.loginfo("filename_out : %s", filename_out)
rospy.loginfo("topic : %s", topic)
outbag = rosbag.Bag(filename_out, 'w')
def callback(clk_msg, odom_msg):
odom_msg.header.stamp = clk_msg.header.stamp
outbag.write(topic, odom_msg, clk_msg.header.stamp)
queue_size = 1000 # because of 200 Hz Vicon
slop = 1.
msg_clock_sub = message_filters.Subscriber("/stamp", Odometry, queue_size = 1000)
msg_odom_sub = message_filters.Subscriber(topic, Odometry, queue_size = 1000)
ts = message_filters.ApproximateTimeSynchronizer([msg_clock_sub, msg_odom_sub], queue_size, slop)
ts.registerCallback(callback)
rospy.loginfo("Start spining")
rospy.spin()
outbag.close()
exit(0)
def __init__(self):
self._cv_bridge = CvBridge()
self._captured_img_l = None
self._captured_img_r = None
self._captured_img_width = 1
self._captured_img_height = 1
self._left_camera_image_topic = "/camera_l/image_rect_color"
self._right_camera_image_topic = "/camera_r/image_rect_color"
sub_img_l = message_filters.Subscriber(self._left_camera_image_topic,
Image)
sub_img_r = message_filters.Subscriber(self._right_camera_image_topic,
Image)
self.mf = message_filters.ApproximateTimeSynchronizer(
[sub_img_l, sub_img_r], 100, 10.0)
self.mf.registerCallback(self._img_callback)
self._pub_depth_img = rospy.Publisher("/depth/image_rect",
Image,
queue_size=1)
self._is_debug = rospy.get_param("~debug")
self.img_scale = 0.5
rospy.loginfo("waiting for left camera image")
rospy.wait_for_message(self._left_camera_image_topic, Image)
rospy.loginfo("waiting for right camera image")
rospy.wait_for_message(self._right_camera_image_topic, Image)
rospy.loginfo("camera image topic received")
rospy.loginfo("loading camera parameter")
camera_param = np.load('{}/config/camera_param_fisheye.npz'.format(
rospkg.RosPack().get_path('jnmouse_ros_examples')))
mtx_l, dist_l, mtx_r, dist_r, R, T = [
camera_param[i]
for i in ["mtx_l", "dist_l", "mtx_r", "dist_r", "R", "T"]
]
newmtx_r = cv2.fisheye.estimateNewCameraMatrixForUndistortRectify(
mtx_r,
dist_r, (self._captured_img_width, self._captured_img_height),
None,
balance=1.0)
newmtx_l = cv2.fisheye.estimateNewCameraMatrixForUndistortRectify(
mtx_l,
dist_l, (self._captured_img_width, self._captured_img_height),
None,
balance=1.0)
Rp_l, Rp_r, Pp_l, Pp_r, self.Q = cv2.fisheye.stereoRectify(
newmtx_l, dist_l, newmtx_r, dist_r,
(self._captured_img_width, self._captured_img_height), R, T, 0)
min_disp = 16
window_size = 9
self.stereo = cv2.StereoSGBM_create(
minDisparity=min_disp,
numDisparities=16 * 3,
blockSize=window_size,
P1=8 * 1 * window_size**2,
P2=32 * 1 * window_size**2,
disp12MaxDiff=1,
uniquenessRatio=5,
speckleWindowSize=50,
speckleRange=2,
mode=cv2.STEREO_SGBM_MODE_SGBM_3WAY)
def __init__(self,
left_image_topic,
right_image_topic,
callback=None,
slop=None,
encoding="bgr8",
queue_size=10):
'''
Contruct a StereoImageSubscriber
@param left_image_topic: ROS topic to subscribe for the left camera
ex: /camera/front/left/image_rect_color
@param right_image_topic: ROS topic to subscribe to for the right camera
ex: /camera/front/right/image_rect_color
@param callback: Function with signature foo(left_img, right_img) to call when a synchronized pair is ready.
If left as None, the latest synced images are stored as self.last_image_left and self.last_image_right
@param slop: Maximum time in seconds between left and right images to be considered synced.
If left as None, will only consider synced if left and right images have exact same header time.
@param encoding: String to pass to CvBridge to encode ROS image message to numpy array
@param queue_size: Integer, the number of images to store in a buffer for each camera to find synced images
'''
if callback is None: # Set default callback to just set image_left and image_right
def callback(image_left, image_right):
setattr(self, 'last_image_left', image_left)
setattr(self, 'last_image_right', image_right)
self.bridge = CvBridge()
self.encoding = encoding
self.callback = callback
self.camera_info_left = None
self.camera_info_right = None
self.last_image_left = None
self.last_image_time_left = None
self.last_image_right = None
self.last_image_time_right = None
# Subscribe to image and camera info topics for both cameras
root_topic_left, image_subtopic_left = path.split(left_image_topic)
self._info_sub_left = rospy.Subscriber(
root_topic_left + '/camera_info',
CameraInfo,
lambda info: setattr(self, 'camera_info_left', info),
queue_size=queue_size)
image_sub_left = message_filters.Subscriber(left_image_topic, Image)
root_topic_right, image_subtopic_right = path.split(right_image_topic)
self._info_sub_right = rospy.Subscriber(
root_topic_right + '/camera_info',
CameraInfo,
lambda info: setattr(self, 'camera_info_right', info),
queue_size=queue_size)
image_sub_right = message_filters.Subscriber(right_image_topic, Image)
# Use message_filters library to set up synchronized subscriber to both image topics
if slop is None:
self._image_sub = message_filters.TimeSynchronizer(
[image_sub_left, image_sub_right], queue_size)
else:
self._image_sub = message_filters.ApproximateTimeSynchronizer(
[image_sub_left, image_sub_right], queue_size, slop)
self._image_sub.registerCallback(self._image_callback)
"Median Y Error:",
np.median(print_array[:, 1]),
"Median Theta Error:",
np.median(print_array[:, 2]),
)
true_array.append([true_pose.pose.position.x, true_pose.pose.position.y])
pf_array.append([pf_pose.pose.position.x, pf_pose.pose.position.y])
# Plot trajectories
if plot and time.time() - start_time > 40 and not plotted:
plt.xlabel("x")
plt.ylabel("y")
plt.plot(np.array(true_array)[:, 0], np.array(true_array)[:, 1], c="r")
plt.plot(np.array(pf_array)[:, 0], np.array(pf_array)[:, 1], c="g")
plt.show()
plotted = True
if __name__ == "__main__":
rospy.init_node("testpf", anonymous=True)
gt_topic = str(rospy.get_param("~gt_topic", "/pf/ta/viz/inferred_pose"))
plot = bool(rospy.get_param("~plot", False))
# Time Synchronizer
pose_sub = message_filters.Subscriber("/pf/viz/inferred_pose", PoseStamped)
pf_sub = message_filters.Subscriber(gt_topic, PoseStamped)
ts = message_filters.ApproximateTimeSynchronizer([pose_sub, pf_sub], 10, 0.1)
ts.registerCallback(callback)
rospy.spin()
print ("threshould larm set {}".format(threshould_l))
print ("threshould rarm set {}".format(threshould_r))
print ("threshould larm cover set {}".format(threshould_cover_l))
print ("threshould rarm cover set {}".format(threshould_cover_r))
if __name__ == '__main__':
rospy.init_node('count_points')
pub_num_l = rospy.Publisher('/larm/num', Int32, queue_size=1)
pub_num_r = rospy.Publisher('/rarm/num', Int32, queue_size=1)
# pub_drop_flag_l = rospy.Publisher('/drop_flag_larm', Bool, queue_size=1)
# pub_drop_flag_r = rospy.Publisher('/drop_flag_rarm', Bool, queue_size=1)
pub_drop_flag = rospy.Publisher('/drop_flag_list', Int32MultiArray, queue_size=1)
sub1 = message_filters.Subscriber("/in_hand_point/attention_clipper_larm/output/point_indices",PointIndices)
sub2 = message_filters.Subscriber("/in_hand_point/attention_clipper_rarm/output/point_indices",PointIndices)
sub3 = message_filters.Subscriber("/in_hand_point/attention_clipper_cover_larm/output/point_indices",PointIndices)
sub4 = message_filters.Subscriber("/in_hand_point/attention_clipper_cover_rarm/output/point_indices",PointIndices)
rospy.Subscriber("/robot_to_mask_image_rarm/output",Image,count_rarm)
# rospy.Subscriber("/mask_image_to_label/output",Image,count_rarm)
rospy.Subscriber("/robot_to_mask_image_larm/output",Image,count_larm)
fps=0.1
delay=1 / fps *0.5
s = rospy.Service('count_points_threshould',CPthre , get_trigger)
mf= message_filters.ApproximateTimeSynchronizer([sub1,sub2,sub3,sub4],2,delay)
mf.registerCallback(timer_cb)
rospy.spin()
#Get output link
output_link = agent.GetOutputLink()
#-- ROS SOAR NODE INITIALIZATION-----------------------------------
print("Initializing ROS SOAR node")
rospy.init_node("soar_ros_node", anonymous=True) #Always first
# Creates a subscriber object for each topic
# The messages to be synced must have the 'header' field or
# use the 'allow_headerless=True' in the TimeSynchronizer() function
# if this field is not present
odom_sub = message_filters.Subscriber('/turtlebot2i/odom', Odometry)
scan_sub = message_filters.Subscriber('/turtlebot2i/lidar/scan', LaserScan)
# Make the topics sync through ApproximateTimeSynchronizer with 0.1s of tolerance
ts = message_filters.ApproximateTimeSynchronizer([odom_sub, scan_sub], 10,
0.1)
# Associate the synchronizer with a callback
ts.registerCallback(topic_callback)
pub = rospy.Publisher("soar_pub_topic", String, queue_size=10)
dummy_pub = rospy.Publisher(
"soar_sub_topic", String,
queue_size=10) #used for inputing debug data to soar_sub_topic
#tasks_list_topic=rospy.Subscriber("soar_tasks_list_topic", String, topic_callback)
dummy_tasks_list_topic = rospy.Publisher(
"soar_tasks_list_topic", String,
queue_size=10) #used for inputing debug data to soar_tasks_list_topic
'phone', 'world')
#################code_update_for Gyro incorporation#########################
if __name__ == "__main__":
try:
mag_sub = message_filters.Subscriber("/Magneto_topic_stamped",
SensorMsgStamped)
accel_sub = message_filters.Subscriber("/Accel_topic_stamped",
SensorMsgStamped)
#############added__code###########################################################
gyro_sub = message_filters.Subscriber("/Gyro_topic_stamped",
SensorMsgStamped)
#############added__code###########################################################
combined_sub = message_filters.ApproximateTimeSynchronizer(
[accel_sub, mag_sub], queue_size=5, slop=0.1)
combined_sub.registerCallback(got_accel_mag)
rospy.spin()
except rospy.ROSInterruptException:
pass
########################
# TO UNDERSTAND FUSION BETWEEN ACCELEROMETER AND MAGNETOMETER
# Author : Kartik Prakash
# Date : 7/Mar/2020
# STEPS:
# 1. Get the raw accel values and the magnetometer values
# 2. Try combining resultant accel and magnetometer to figure out RPY
#
#
########################
#!/usr/bin/env python
import rospy
import message_filters
from geometry_msgs.msg import PoseStamped
from sensor_msgs.msg import CompressedImage
def callback(msg_in_pose, msg_in_image):
rospy.loginfo("Synced Measurements:")
rospy.loginfo(msg_in_pose)
rospy.loginfo(msg_in_image.format)
if __name__ == '__main__':
# Initialize
rospy.init_node('synchronizer_py')
sub_pose = message_filters.Subscriber('/emulated_uav/pose', PoseStamped, queue_size=10)
sub_image = message_filters.Subscriber('/emulated_uav/image/compressed', CompressedImage, queue_size=10)
ts = message_filters.ApproximateTimeSynchronizer([sub_pose, sub_image], 10, 0.1, allow_headerless=True)
ts.registerCallback(callback)
# Loop here until quit
try:
rospy.loginfo("Started synchronizer node...")
rospy.spin()
except rospy.ROSInterruptException:
# Shutdown
rospy.loginfo("Shutting down synchronizer!")
sp.shutdown()
def reset_time(self, msg):
print 'reset time'
self.ts = message_filters.ApproximateTimeSynchronizer(
self.subscribers, 5, 0.001)
self.ts.registerCallback(self.callback)
