def listener():
# Initialization
rospy.init_node('motor_node')
base_sub = Subscriber("base_motors", JointState)
ats = ApproximateTimeSynchronizer([base_sub], queue_size=2, slop=0.1)
ats.registerCallback(send_to_motors)
rospy.spin()
def __init__(self):
# Init attributes
print("start")
self.pose = PoseStamped()
self.pose.header.frame_id = "base_link"
self.first_pose = PoseStamped()
self.euler = None
self.active = True
self.start = True
self.first = True
# Init subscribers and publishers
#tss = TimeSynchronizer(Subscriber("/imu/data", Imu),Subscriber("/fix", NavSatFix))
#tss.registerCallback(call_back)
imu_sub = Subscriber("imu/data", Imu)
gps_sub = Subscriber("/fix", NavSatFix)
ats = ApproximateTimeSynchronizer((imu_sub, gps_sub),
queue_size=1,
slop=0.1)
ats.registerCallback(self.call_back)
#self.sub_imu = rospy.Subscriber("/imu/data", Imu, self.imu_cb, queue_size=1)
#self.sub_gps = rospy.Subscriber("/gps/fix", NavSatFix, self.gps_cb, queue_size=1)
self.pub_gazebo = rospy.Publisher('/david/cmd_vel',
Twist,
queue_size=1)
self.pub_pose = rospy.Publisher('/pose', PoseStamped, queue_size=1)
def __init__(self):
print("Verifying Estimates")
self.syn_pub = rospy.Publisher('sync_estimates', SyncVerifyEstimates, queue_size=10)
self.error_pub = rospy.Publisher('estimate_error', SyncEstimateError, queue_size=10)
estimate_subs = Subscriber("xyz_debug_estimates", XYZDebugEstimate)
truth_subs = Subscriber("mocap/velocity/body_level_frame", TwistWithCovarianceStamped)
rgbd_subs = Subscriber("rgbd_velocity/body_level_frame", DeltaToVel)
mocap_pose_subs = Subscriber("pose_stamped", PoseStamped)
self.sync_msg = SyncVerifyEstimates()
self.estimate_error = SyncEstimateError()
self.multiplier = 1
self.sonar_offset = 0
use_sonar = False
self.multiplier = rospy.get_param("SD_Multiplied")
self.sonar_offset = rospy.get_param("Z_offset")
use_sonar = rospy.get_param("use_sonar")
print(use_sonar)
if use_sonar:
sonar_subs = Subscriber("sonar_ned", Range)
approx_sync_sonar = ApproximateTimeSynchronizer([estimate_subs, truth_subs, rgbd_subs, mocap_pose_subs,sonar_subs], queue_size=5, slop=0.1)
approx_sync_sonar.registerCallback(self.callbackSonar)
else:
approx_sync_wo_sonar = ApproximateTimeSynchronizer([estimate_subs, truth_subs, rgbd_subs, mocap_pose_subs], queue_size=5, slop=0.1)
approx_sync_wo_sonar.registerCallback(self.callbackNOSonar)
def run(self):
sync = ApproximateTimeSynchronizer([
Subscriber(self.map1, OccupancyGrid),
Subscriber(self.map2, OccupancyGrid)
], 10, 1)
sync.registerCallback(self.sync_callback)
rospy.spin()
def __init__(self):
print "Entered Initialize - range measure multiple"
self.sigma_rho = rospy.get_param("~sigma_rho")
self.rng_pub10 = rospy.Publisher('/ekf_bot/range_meas10',
sensor_rho_pairwise,
queue_size=1)
self.rng_pub20 = rospy.Publisher('/ekf_bot/range_meas20',
sensor_rho_pairwise,
queue_size=1)
self.rng_pub12 = rospy.Publisher('/ekf_bot/range_meas12',
sensor_rho_pairwise,
queue_size=1)
robot0_pose_sub = Subscriber("/ekf_bot/inertial_pose0", inert_pose)
robot1_pose_sub = Subscriber("/ekf_bot/inertial_pose1", inert_pose)
robot2_pose_sub = Subscriber("/ekf_bot/inertial_pose2", inert_pose)
self.rng10 = sensor_rho_pairwise()
self.rng20 = sensor_rho_pairwise()
self.rng12 = sensor_rho_pairwise()
ats = ApproximateTimeSynchronizer(
[robot0_pose_sub, robot1_pose_sub, robot2_pose_sub],
queue_size=1000,
slop=0.1,
allow_headerless=False)
ats.registerCallback(self.rng_pairwise)
self.node_no = 1
def __init__(self):
cv2.namedWindow("Live Image", 1)
cv2.namedWindow("Base Image", 1)
cv2.namedWindow("Original Image", 1)
cv2.startWindowThread()
self.bridge = CvBridge()
image_sub = Subscriber(
"/camera/rgb/image_color",
Image,
queue_size=1
)
person_sub = Subscriber(
"/upper_body_detector/detections",
UpperBodyDetector,
queue_size=1
)
depth_sub = Subscriber(
"/camera/depth/image_rect",
Image,
queue_size=1
)
# detections_image_sub = Subscriber(
# "/upper_body_detector/image",
# Image,
# queue_size=1
# )
ts = ApproximateTimeSynchronizer([image_sub, person_sub, depth_sub], 1, 0.1)
ts.registerCallback(self.image_callback)
class Decision_Manager:
def __init__(self, safety_topic, drive_topic, drive_pub):
# Initialize subscribers
self.scan_subscriber = Subscriber(safety_topic,
StampedBool,
queue_size=100)
self.odom_subscriber = Subscriber(drive_topic,
AckermannDriveStamped,
queue_size=100)
self.drive_publisher = rospy.Publisher(drive_pub,
AckermannDriveStamped,
queue_size=10)
#create the time synchronizer
self.sub = ApproximateTimeSynchronizer(
[self.scan_subscriber, self.odom_subscriber],
queue_size=100,
slop=0.020)
#register the callback to the synchronizer
self.sub.registerCallback(self.master_callback)
def master_callback(self, bool_topic, drive_topic):
msg = drive_topic
if (bool(bool_topic.data)):
msg.drive.speed = 0
self.drive_publisher.publish(msg)
def test_approx(self):
m0 = MockFilter()
m1 = MockFilter()
ts = ApproximateTimeSynchronizer([m0, m1], 1, 0.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(rospy.Time(t), random.random()) for t in range(N)]
seq1 = [MockMessage(rospy.Time(t), random.random()) for t in range(N)]
# random.shuffle(seq0)
ts = ApproximateTimeSynchronizer([m0, m1], N, 0.1)
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 execute(self, userdata):
# If prempted before even getting a chance, give up.
if self.preempt_requested():
self.service_preempt()
return 'aborted'
self._trigger_event.clear()
self._height_sub = Subscriber('height', Range)
self._image_sub = Subscriber('image', Image)
sync = ApproximateTimeSynchronizer([self._height_sub, self._image_sub],
queue_size=1,
slop=0.05)
finishdata = []
self._finished = False
sync.registerCallback(self.callback, userdata, finishdata)
# Wait until a candidate has been found.
self._trigger_event.wait()
del sync
if self.preempt_requested():
self.service_preempt()
return 'aborted'
assert (len(finishdata) == 1)
return finishdata[0]
def execute(self, userdata):
# If prempted before even getting a chance, give up.
if self.preempt_requested():
self.service_preempt()
return 'aborted'
self._trigger_event.clear()
self._height_sub = Subscriber('height', Range)
self._image_sub = Subscriber('image', Image)
sync = ApproximateTimeSynchronizer([self._height_sub, self._image_sub],
queue_size=1, slop=0.05)
finishdata = []
self._finished = False
sync.registerCallback(self.callback, userdata, finishdata)
# Wait until a candidate has been found.
self._trigger_event.wait()
del sync
if self.preempt_requested():
self.service_preempt()
return 'aborted'
assert(len(finishdata) == 1)
return finishdata[0]
class image_converter:
def __init__(self):
self.image_pub = rospy.Publisher("image_topic_2", Image, queue_size=10)
self.bridge = CvBridge()
image_sub = Subscriber("/front_camera/image_raw", Image)
detect_sub = Subscriber("/detectnet/detections", Detection2DArray)
self.ts = ApproximateTimeSynchronizer([image_sub, detect_sub],
queue_size=10,
slop=0.5)
self.ts.registerCallback(self.callback)
def callback(self, data, detection):
rospy.loginfo(len(detection.detections))
if len(detection.detections) >= 1:
rospy.loginfo(detection.detections[0].bbox.center.x)
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
(rows, cols, channels) = cv_image.shape
if cols > 60 and rows > 60:
cv2.rectangle(cv_image, (50, 50), (65, 65), (255, 0, 0), 2)
cv_image = print_rectangle(self, data, detection, cv_image)
#la partie a custom
#cv2.circle(cv_image, (50,50), 10, 255)
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
except CvBridgeError as e:
print(e)
def main(args):
rospy.init_node('bag_to_yaprofi')
global outdir, br, seq
outdir = args.outdir
br = cv_bridge.CvBridge()
seq = args.seq
os.makedirs(os.path.join(outdir, 'rgb_left'))
os.makedirs(os.path.join(outdir, 'rgb_right'))
os.makedirs(os.path.join(outdir, 'depth'))
with open(os.path.join(outdir, 'rgb_left.txt'), 'w') as f:
f.write('#\n' * 3)
with open(os.path.join(outdir, 'rgb_right.txt'), 'w') as f:
f.write('#\n' * 3)
with open(os.path.join(outdir, 'depth.txt'), 'w') as f:
f.write('#\n' * 3)
with open(os.path.join(outdir, 'gt_tum.txt'), 'w') as f:
pass
with open(os.path.join(outdir, 'gt_kitti.txt'), 'w') as f:
pass
sub_image_left = Subscriber('/zed_node/left/image_rect_color/compressed', CompressedImage)
sub_image_right = Subscriber('/zed_node/right/image_rect_color/compressed', CompressedImage)
sub_depth = Subscriber('/zed_node/depth/depth_registered', Image)
sub_odom = Subscriber('/groundtruth', Odometry)
ats = ApproximateTimeSynchronizer([sub_image_left, sub_image_right, sub_depth, sub_odom], queue_size=5, slop=0.05)
ats.registerCallback(callback)
rospy.spin()
class image_converter:
def __init__(self):
self.bridge = CvBridge()
self.tss = ApproximateTimeSynchronizer([Subscriber("/fusion/front_camera/image_raw",Image),Subscriber('/fusion/steering_report',SteeringReport),Subscriber('/fusion/throttle_report',ThrottleReport)],10, 0.2, allow_headerless=False)
self.tss.registerCallback(self.callback_img)
def callback_img(self,Image,SteeringReport,ThrottleReport):
i=0
try:
screen = self.bridge.imgmsg_to_cv2(Image, "bgr8")
screen = cv2.cvtColor(screen, cv2.COLOR_BGR2GRAY)
screen = cv2.resize(screen, (150,200))
steering=SteeringReport.steering_wheel_angle_cmd
throttle=ThrottleReport.pedal_cmd
except CvBridgeError as e:
print(e)
last_time = time.time()
output=[steering,throttle]
training_data.append([screen,output])
print len(training_data),steering,throttle
if len(training_data) % 100 == 0:
print(len(training_data))
np.save(file_name,training_data)
print 'saved'
def main():
global angle
global speed
global stop, y_max
angle = 0.0
speed = 0.0
rospy.init_node('listener', anonymous=True)
robo_angle_pub = rospy.Publisher('robo_angle', Float32, queue_size=10)
robo_speed_pub = rospy.Publisher('robo_speed', Float32, queue_size=10)
laser_sub = Subscriber("scan", sensor_msgs.msg.LaserScan)
camera_sub = Subscriber("/darknet_ros/bounding_boxes", BoundingBoxes)
ats = ApproximateTimeSynchronizer([laser_sub, camera_sub],
queue_size=5,
slop=0.1,
allow_headerless=True)
ats.registerCallback(avoid)
rospy.spin()
rate = rospy.Rate(1) # 10hz
while not rospy.is_shutdown():
msg_angle = angle
msg_speed = speed
print(
" s =",
speed,
"a =",
angle,
stop,
)
robo_angle_pub.publish(msg_angle)
robo_speed_pub.publish(msg_speed)
rate.sleep()
def listener(self):
# 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.
self.count = 0
#calib_file = "/home/robesafe/AB3DMOT/data/KITTI/resources/CARLA/calib/0102.txt"
#calib_file = "/media/robesafe/videos1.2/KITTI/KITTI_dataset_tracking/data_tracking_calib/training/calib/0001.txt"
calib_file = "t4ac_calib.txt"
self.calib = Calibration(calib_file) # load the calibration
self.bridge = CvBridge()
rospy.init_node('visualizer', anonymous=True)
subs_info = Subscriber("/AB3DMOT_kitti",
Object_kitti_list,
queue_size=5)
subs_img = Subscriber("/zed/zed_node/left/image_rect_color",
Image,
queue_size=5)
self.image_pub = rospy.Publisher("/AB3DMOT_image", Image)
#subs_img = Subscriber("/carla/ego_vehicle/camera/rgb/view/image_color", Image, queue_size = 5)
ats = ApproximateTimeSynchronizer([subs_info, subs_img],
queue_size=5,
slop=10)
ats.registerCallback(self.callback)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
def __init__(self):
# print "Entered Initialize - true relative pose"
self.true_pose_in0 = rospy.Publisher('/ekf_bot/inertial_pose0',
inert_pose,
queue_size=1)
self.true_pose_in1 = rospy.Publisher('/ekf_bot/inertial_pose1',
inert_pose,
queue_size=1)
self.true_pose_in2 = rospy.Publisher('/ekf_bot/inertial_pose2',
inert_pose,
queue_size=1)
robot0_pose_sub = Subscriber("/robot0/odom_truth", Odometry)
robot1_pose_sub = Subscriber("/robot1/odom_truth", Odometry)
robot2_pose_sub = Subscriber("/robot2/odom_truth", Odometry)
imu0 = Subscriber('/robot0/mobile_base/sensors/imu_data', Imu)
imu1 = Subscriber('/robot1/mobile_base/sensors/imu_data', Imu)
imu2 = Subscriber('/robot2/mobile_base/sensors/imu_data', Imu)
self.inert_pose0 = inert_pose()
self.inert_pose1 = inert_pose()
self.inert_pose2 = inert_pose()
ats = ApproximateTimeSynchronizer([
robot0_pose_sub, robot1_pose_sub, robot2_pose_sub, imu0, imu1, imu2
],
queue_size=1,
slop=0.1,
allow_headerless=False)
ats.registerCallback(self.true_inert_publish)
self.node_no = 1
def listener():
global control_pub, record_param_pub
# 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)
#rospy.Subscriber('/camera/imu', Imu, callback1)
#rospy.Subscriber('/dynamixel/XM3', XM2, callback2)
#rospy.Subsriber('/dynamixel/XM4', XM2, callback3)
#rospy.Subscriber('/vrpn_client_node/camera1/pose', PoseStamped, callback4)
ts = ApproximateTimeSynchronizer(
[Subscriber('/dynamixel/XM3', XM2),
Subscriber('/dynamixel/XM4', XM2)],
queue_size=100,
slop=0.1)
ts.registerCallback(callback)
control_pub = rospy.Publisher('Wheel', Float64MultiArray, queue_size=10)
record_param_pub = rospy.Publisher('record_inner_param',
Float64StampedMultiArray,
queue_size=10)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
def main():
global camera_info_topic, tf_listener
rospy.init_node('open3d_recorder', anonymous=True)
# Create TF listener
tf_listener = tf.TransformListener(rospy.Duration(500))
# Get parameters
depth_image_topic = rospy.get_param('~depth_image_topic')
color_image_topic = rospy.get_param('~color_image_topic')
camera_info_topic = rospy.get_param('~camera_info_topic')
cache_count = rospy.get_param('~cache_count', 10)
slop = rospy.get_param('~slop', 0.01) # The delay (in seconds) with which messages can be synchronized.
allow_headerless = False #allow storing headerless messages with current ROS time instead of timestamp
rospy.loginfo(rospy.get_caller_id() + ": depth_image_topic - " + depth_image_topic)
rospy.loginfo(rospy.get_caller_id() + ": color_image_topic - " + color_image_topic)
rospy.loginfo(rospy.get_caller_id() + ": camera_info_topic - " + camera_info_topic)
depth_sub = Subscriber(depth_image_topic, Image)
rgb_sub = Subscriber(color_image_topic, Image)
tss = ApproximateTimeSynchronizer([depth_sub, rgb_sub], cache_count, slop, allow_headerless)
tss.registerCallback(cameraCallback)
start_server = rospy.Service('start_recording', StartRecording, startRecordingCallback)
stop_server = rospy.Service('stop_recording', StopRecording, stopRecordingCallback)
rospy.spin()
def __init__(self):
self.node_name = rospy.get_name()
self.pose = Pose()
self.prior_pose = Pose()
self.prior_roll = 0
self.prior_pitch = 0
self.prior_yaw = 0
self.start = False
self.covariance = np.zeros((36,), dtype=float)
self.odometry = Odometry()
self.br = tf.TransformBroadcaster()
# param
self.imu_offset = 0
self.lat_orig = rospy.get_param('~latitude', 0.0)
self.long_orig = rospy.get_param('~longitude', 0.0)
self.utm_orig = fromLatLong(self.lat_orig, self.long_orig)
# Service
self.srv_imu_offset = rospy.Service('~imu_offset', SetValue, self.cb_srv_imu_offest)
# Publisher
self.pub_odm = rospy.Publisher("~odometry", Odometry, queue_size=1)
# Subscriber
sub_imu = message_filters.Subscriber("~imu/data", Imu)
sub_gps = message_filters.Subscriber("~fix", NavSatFix)
ats = ApproximateTimeSynchronizer((sub_imu, sub_gps), queue_size = 1, slop = 0.1)
ats.registerCallback(self.cb_gps_imu)
def __init__(self):
# variables
self.node_name = rospy.get_name()
self.odom = None
self.scan = None
self.vehicle_yaw = None
# param
self.cell_size = rospy.get_param('~cell_size', 0.5) # meter
self.map_length = rospy.get_param("~map_length", 35)
self.wait_time = rospy.get_param("~wait_time", 0.1)
self.vehicle_size = rospy.get_param("~vehicle_size", 0.1)
self.drift_x = rospy.get_param("~drift_x",
0.1) # case by odom and lidar position
self.drift_y = rospy.get_param("~drift_y", 0.1)
self.cell_length = int(self.map_length / self.cell_size * 2)
self.frame_id = rospy.get_param("~frame_id", "odom")
self.frame_period = rospy.get_param("~frame_period", 0.2)
# Publisher
self.pub_grid_map = rospy.Publisher("map", OccupancyGrid, queue_size=1)
rospy.Timer(rospy.Duration(self.frame_period), self.create_local_map)
# Subscriber
sub_odometry = Subscriber("odom", Odometry)
sub_laser_scan = Subscriber("scan", LaserScan)
ats = ApproximateTimeSynchronizer((sub_odometry, sub_laser_scan),
queue_size=1,
slop=self.wait_time)
ats.registerCallback(self.cb_sensor)
def __init__(self):
# Subscribers
# self.marker_array_subscribe = rospy.Subscriber("/apriltags_kinect2/detections", AprilTagDetections, self.detection_callback)
# self.rgb_image_subscribe = rospy.Subscriber("/head/kinect2/qhd/image_color_rect", Image, self.rgb_callback)
# self.depth_image_subscribe = rospy.Subscriber("/head/kinect2/qhd/image_depth_rect", Image, self.depth_callback)
self.camera_info = rospy.Subscriber("/head/kinect2/qhd/camera_info",
CameraInfo, self.camera_callback)
tss = ApproximateTimeSynchronizer([
Subscriber("/head/kinect2/qhd/image_color_rect", Image),
Subscriber("/head/kinect2/qhd/image_depth_rect", Image),
Subscriber("/apriltags_kinect2/detections", AprilTagDetections)
], 1, 0.5)
tss.registerCallback(self.processtag_callback)
# Vars
self.camera_intrinsics = None
self.rgb_image = None
self.depth_image = None
self.mark_array = None
# Converters
self.bridge = CvBridge()
# Publisher
self.marker_publish = rospy.Publisher(
"/apriltags_kinect2/marker_array_fused", MarkerArray)
self.detection_publish = rospy.Publisher(
"/apriltags_kinect2/detections_fused", AprilTagDetections)
class Safety(object):
"""
The class that handles emergency braking.
"""
def __init__(self):
"""
One publisher should publish to the /brake topic with a AckermannDriveStamped brake message.
One publisher should publish to the /brake_bool topic with a Bool message.
You should also subscribe to the /scan topic to get the LaserScan messages and
the /odom topic to get the current speed of the vehicle.
The subscribers should use the provided odom_callback and scan_callback as callback methods
NOTE that the x component of the linear velocity in odom is the speed
"""
self.brake_bool = rospy.Publisher('brake_bool',
StampedBool,
queue_size=100)
# Initialize subscribers
self.scan_subscriber = Subscriber('scan', LaserScan, queue_size=100)
self.odom_subscriber = Subscriber('odom', Odometry, queue_size=100)
#create the time synchronizer
self.sub = ApproximateTimeSynchronizer(
[self.scan_subscriber, self.odom_subscriber],
queue_size=100,
slop=0.020)
#register the callback to the synchronizer
self.sub.registerCallback(self.master_callback)
self.THRESHOLD = 0.5
def master_callback(self, scan_msg, odom_msg):
degrees_to_radians = np.pi / 180
linear_velocity = odom_msg.twist.twist.linear.x
angles_radian = (((np.arange(0, 1080) - 539) / 4)) * degrees_to_radians
projections = np.cos(angles_radian) * linear_velocity
projections = np.maximum(projections, 0.00000001)
# the velocity we get from odom is in the x direction
ranges = scan_msg.ranges
# compute the time to collision
ttc = ranges / projections
# minimum ttc
minimum_ttc = min(ttc)
msg = StampedBool()
msg.header.stamp = rospy.Time.now()
if minimum_ttc < self.THRESHOLD:
msg.data = True
rospy.loginfo("Engage AEB")
else:
msg.data = False
self.brake_bool.publish(msg)
class RedDepthNode(object):
""" This node reads from the Kinect color stream and
publishes an image topic with the most red pixel marked.
Subscribes:
/camera/rgb/image_color
Publishes:
red_marked_image
"""
def __init__(self):
""" Construct the red-pixel finder node."""
rospy.init_node('red_depth_node')
self.image_pub = rospy.Publisher('red_marked_image',
Image,
queue_size=10)
self.marker_pub = rospy.Publisher('red_marker', Marker, queue_size=10)
self.cv_bridge = CvBridge()
# Unfortunately the depth data and image data from the kinect aren't
# perfectly time-synchronized. The code below handles this issue.
img_sub = message_filters.Subscriber('/camera/rgb/image_color', Image)
cloud_sub = message_filters.Subscriber(\
'/camera/depth_registered/points',
PointCloud2)
self.kinect_synch = ApproximateTimeSynchronizer([img_sub, cloud_sub],
queue_size=10,
slop=.02)
self.kinect_synch.registerCallback(self.image_points_callback)
rospy.spin()
def image_points_callback(self, img, cloud):
""" Handle image/point_cloud callbacks. """
# Convert the image message to a cv image object
cv_img = self.cv_bridge.imgmsg_to_cv2(img, "bgr8")
# Do the image processing
red_pos = find_reddest_pixel_fast(cv_img)
cv2.circle(cv_img, red_pos, 5, (0, 255, 0), -1)
# Extract just the point we want from the point cloud message
# as an iterable of (x,y,z) tuples
points = point_cloud2.read_points(cloud,
skip_nans=False,
uvs=[red_pos])
# Iterate through the returned points (really just one)
for p in points:
if not np.isnan(p[0]):
# Publish a marker at the point.
marker = make_sphere_marker(p[0], p[1], p[2], .05,
cloud.header.frame_id,
cloud.header.stamp)
self.marker_pub.publish(marker)
# Convert the modified image back to a message.
img_msg_out = self.cv_bridge.cv2_to_imgmsg(cv_img, "bgr8")
self.image_pub.publish(img_msg_out)
def main3():
poseSub = Subscriber('RosAria/pose', Odometry)
imgSub = Subscriber('camera/color/image_raw', Image)
ats = ApproximateTimeSynchronizer([poseSub, imgSub],
queue_size=1,
slop=0.2)
ats.registerCallback(callPoseImg)
def main5():
poseSub = Subscriber('raw_odom', Odometry)
lblSub = Subscriber('semantic_label', SemLabel)
ats = ApproximateTimeSynchronizer([poseSub, lblSub],
queue_size=1,
slop=0.2)
ats.registerCallback(callPoseLbl)
def main6():
imgSub = Subscriber('camera/color/image_raw', Image)
depthSub = Subscriber('camera/aligned_depth_to_color/image_raw', Image)
ats = ApproximateTimeSynchronizer([imgSub, depthSub],
queue_size=1,
slop=0.2)
ats.registerCallback(callImgDepth)
def __init__(self):
rospy.init_node(NAME, anonymous=True)
# Image detection parameters
self.conf_thresh = 0.6
self.hier_thresh = 0.8
self.frame_height = 416
self.frame_width = 416
# Set up dynamic reconfigure
self.srv = Server(DetectorConfig, self.reconf_cb)
# Set up OpenCV Bridge
self.bridge = CvBridge()
# Load network
rp = rospkg.RosPack()
# Load object labels
classfile = os.path.join(rp.get_path(NAME), rospy.get_param('~label_file_path'))
self.classes = None
with open(classfile, 'rt') as f:
self.classes = f.read().rstrip('\n').split('\n')
# Load network
cfgfile = os.path.join(rp.get_path(NAME), rospy.get_param('~cfg_file_path'))
weightsfile = os.path.join(rp.get_path(NAME), rospy.get_param('~weights_file_path'))
self.net = cv.dnn.readNet(cfgfile, weightsfile)
self.net.setPreferableTarget(cv.dnn.DNN_TARGET_OPENCL)
# Set up detection image publisher
det_topic = rospy.get_param('~detection_image_topic_name')
self.det_pub = rospy.Publisher(det_topic, Image, queue_size=1, latch=True)
# Set up prediction publisher
pred_topic = rospy.get_param('~predictions_topic_name')
self.pred_pub = rospy.Publisher(pred_topic, Predictions, queue_size=1, latch=True)
# The first subscriber retrieves image data from the left image and passes it to the callback function
image_topic = rospy.get_param('~camera_topic_name')
image_sub = Subscriber(image_topic, Image, queue_size=1)
# The second subscriber retrieves depth data from the camera as 32 bit floats with values in meters and maps this onto an image structure, which is passed to callback2
if rospy.has_param('~depth_topic_name'):
depth_topic = rospy.get_param('~depth_topic_name')
depth_sub = Subscriber(depth_topic, Image, queue_size=1)
else:
depth_sub = None
# We want to receive both images in the same callback
if depth_sub:
rospy.loginfo("with depth sub")
ts = ApproximateTimeSynchronizer([image_sub, depth_sub], queue_size=1, slop=0.1)
else:
rospy.loginfo("without depth sub")
ts = ApproximateTimeSynchronizer([image_sub], queue_size=1, slop=0.1)
ts.registerCallback(self.image_cb)
def main7():
poseSub = Subscriber('rtabmap/odom', Odometry)
imgSub = Subscriber('camera/color/image_raw', Image)
depthSub = Subscriber('camera/aligned_depth_to_color/image_raw', Image)
ats = ApproximateTimeSynchronizer([poseSub, imgSub, depthSub],
queue_size=1,
slop=0.2)
ats.registerCallback(callPoseSE3ImgDepth)
def main():
rospy.init_node("main", anonymous=True)
occSub = Subscriber("/map", OccupancyGrid)
poseSub = Subscriber("/submap_list", SubmapList)
# tfSub = Subscriber("/tf", TFMessage)
ats = ApproximateTimeSynchronizer([occSub, poseSub], queue_size=5, slop=0.1)
ats.registerCallback(callback)
rospy.spin()
def main():
i = 0
rospy.init_node("syn")
rgb = Subscriber("/camera/rgb/image_rect_color", sensor_msgs.msg.Image)
depth = Subscriber("/camera/depth_registered/image_raw",
sensor_msgs.msg.Image)
ats = ApproximateTimeSynchronizer([rgb, depth], queue_size=5, slop=0.1)
ats.registerCallback(write)
rospy.spin()
rospy.on_shutdown(shut)
def __init__(self):
self.sigma_px = rospy.get_param(
"~sigma_px"
) # tilde is for a local variable accessible in that node only, for global variables no tilde required
self.sigma_py = rospy.get_param("~sigma_py")
self.sigma_del_psi = rospy.get_param("~sigma_del_psi")
self.sigma_v = rospy.get_param("~sigma_v")
self.sigma_w = rospy.get_param("~sigma_w")
self.num_iter = rospy.get_param("~num_iter")
self.dt = rospy.get_param("~time_step")
self.robot0_vel = rospy.get_param("~robot0_vel")
self.robot1_vel = rospy.get_param("~robot1_vel")
self.robot2_vel = rospy.get_param("~robot2_vel")
self.beta10 = rospy.get_param("~beta1")
self.beta20 = rospy.get_param("~beta2")
self.CRLFlag = rospy.get_param("~CRLFlag")
print("Entered Initialize - Multi vehicle EKF", self.sigma_px)
self.ekf_pb10 = rospy.Publisher('/ekf_bot/rel_est10',
rel_pose_est,
queue_size=1)
self.ekf_pb20 = rospy.Publisher('/ekf_bot/rel_est20',
rel_pose_est,
queue_size=1)
self.rl_pose_est10 = rel_pose_est()
self.rl_pose_est20 = rel_pose_est()
robot0_pose_sub = Subscriber("/ekf_bot/inertial_pose0", inert_pose)
robot1_pose_sub = Subscriber("/ekf_bot/inertial_pose1", inert_pose)
robot2_pose_sub = Subscriber("/ekf_bot/inertial_pose2", inert_pose)
imu0 = Subscriber('/robot0/mobile_base/sensors/imu_data', Imu)
imu1 = Subscriber('/robot1/mobile_base/sensors/imu_data', Imu)
imu2 = Subscriber('/robot2/mobile_base/sensors/imu_data', Imu)
true_rel_pose = Subscriber('/ekf_bot/rel_true_multiple',
rel_pose_multiple)
meas_node10 = Subscriber('/ekf_bot/range_meas10', sensor_rho_pairwise)
meas_node20 = Subscriber('/ekf_bot/range_meas20', sensor_rho_pairwise)
meas_node12 = Subscriber('/ekf_bot/range_meas12', sensor_rho_pairwise)
self.node_no = 1
self.Phat = np.identity(3)
self.xhat = np.identity(3)
self.Qu = np.matrix([[0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0]])
ats = ApproximateTimeSynchronizer([
robot0_pose_sub, robot1_pose_sub, robot2_pose_sub, imu0, imu1,
imu2, true_rel_pose, meas_node10, meas_node20, meas_node12
],
queue_size=1,
slop=0.1,
allow_headerless=False)
ats.registerCallback(self.ekf_func)
class RedDepthNode(object):
""" This node reads from the Kinect color stream and
publishes an image topic with the most red pixel marked.
Subscribes:
/camera/rgb/image_color
Publishes:
red_marked_image
"""
def __init__(self):
""" Construct the red-pixel finder node."""
rospy.init_node('red_depth_node')
self.image_pub = rospy.Publisher('red_marked_image',
Image,
queue_size=10)
self.marker_pub = rospy.Publisher('red_marker', Marker, queue_size=10)
self.cv_bridge = CvBridge()
# Unfortunately the depth data and image data from the kinect aren't
# perfectly time-synchronized. The code below handles this issue.
img_sub = message_filters.Subscriber('/camera/rgb/image_raw', Image)
dep_sub = message_filters.Subscriber(\
'/camera/depth/image_raw',
Image)
self.kinect_synch = ApproximateTimeSynchronizer([img_sub, dep_sub],
queue_size=10,
slop=.02)
self.kinect_synch.registerCallback(self.image_points_callback)
rospy.spin()
def image_points_callback(self, img, depth):
""" Handle image/point_cloud callbacks. """
# Convert the image message to a cv image object
cv_img = self.cv_bridge.imgmsg_to_cv2(img, "bgr8")
cv2.imwrite("./color_0.png", cv_img)
cv_depth = self.cv_bridge.imgmsg_to_cv2(depth, "passthrough")
cv2.imwrite("./dep_1.png", cv_depth)
# print cv_depth.max()
points_depth = np.uint8(cv_depth)
#
cv2.imwrite("./dep_2.png", points_depth)
# points_depth = np.float32([p/255.0 for p in points_depth])
# print np.array(points_depth).dtype.type,points_depth.max()
points_depth = np.float32([p / 1000.0 for p in cv_depth])
cv2.imwrite("./dep_3.png", points_depth)
print np.array(points_depth).dtype.type, points_depth.max()
np.save("./depth_0.npy", points_depth)
def main():
rospy.init_node("kinect_bno_fusion_node")
rospy.loginfo("kinect_bno_fusion_node started")
# create subscribers
XYZ_sub = Subscriber(vision_target_xyz_topic, PointStamped)
bno_quat_sub = Subscriber(bno_quat_topic, QuaternionStamped)
# try to sync datastreams
approxSync = ApproximateTimeSynchronizer([XYZ_sub, bno_quat_sub], queue_size=queue, slop=slop_time)
approxSync.registerCallback(gotStreams)
rospy.spin()
def main(args):
rospy.init_node('take_depthRGB_pics', anonymous=True)
#tss = ApproximateTimeSynchronizer([Subscriber("/camera/depth/image", Image),
# Subscriber("/camera/rgb/image_rect_color", Image)], queue_size=1, slop=0.1)
#tss.registerCallback(callback)
global stream_on
while (not rospy.is_shutdown()):
get_input = 'x'
get_input = raw_input("Press 's' to start stream, 'p' to pause: \n")
tss = ApproximateTimeSynchronizer([Subscriber("/camera/depth_registered/image_raw", Image),
Subscriber("/camera/rgb/image_raw", Image)], queue_size=1, slop=0.1)
if str(get_input) == 's':
stream_on = 1
tss.registerCallback(callback)
if str(get_input) == 'p':
stream_on = 0
tss.registerCallback(callback)
def __init__(self):
# Create new windows
cv2.namedWindow("Live Image", 1)
cv2.namedWindow("Base Image", 1)
cv2.namedWindow("Original Image", 1)
cv2.startWindowThread()
self.bridge = CvBridge()
# Commented code below used for testing only
# self.match_pub = rospy.Publisher("~match_image_out", Image, queue_size=1)
# self.base_pub = rospy.Publisher("~base_image_out", Image, queue_size=1)
# self.match_string = rospy.Publisher("~match_bool", String, queue_size=1)
# Subscribe to the required topics. Using /camera/ instead of /head_xiton/ when
# not running on Linda
image_sub = Subscriber(
"/head_xtion/rgb/image_color",
Image,
queue_size=1
)
person_sub = Subscriber(
"/upper_body_detector/detections",
UpperBodyDetector,
queue_size=1
)
depth_sub = Subscriber(
"/head_xtion/depth/image_rect",
Image,
queue_size=1
)
# Time syncronizer is implimented to make sure that all of the frames match up from all of the topics.
ts = ApproximateTimeSynchronizer([image_sub, person_sub, depth_sub], 1, 0.1)
ts.registerCallback(self.image_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