def __init__(self):
rospy.init_node("uw_teleop")
self.vel_pub1 = rospy.Publisher('/dataNavigator1',
Odometry,
queue_size=10)
self.vel_pub2 = rospy.Publisher('/dataNavigator2',
Odometry,
queue_size=10)
self.lidar_pub = rospy.Publisher('/dataNavigatorlidar',
Odometry,
queue_size=10)
self.rate = rospy.Rate(20)
self.joy_sub = rospy.Subscriber("/joy", Joy, self.joy_msg_callback)
self.bf1_odom_sub = rospy.Subscriber("/uwsim/BlueFox1Odom", Odometry,
self.pose1_callback)
self.bf2_odom_sub = rospy.Subscriber("/uwsim/BlueFox2Odom", Odometry,
self.pose2_callback)
self.bf1_laser_sub = rospy.Subscriber("/BlueFox1/multibeam11",
LaserScan, self.laser1_callback)
self.bf1_lidar_sub = rospy.Subscriber("/lidar/multibeam", LaserScan,
self.lidar_callback)
self.bf2_laser_sub = rospy.Subscriber("/BlueFox2/multibeam", LaserScan,
self.laser2_callback)
# messages required by /ndt_mapping
self.lidar_pc2_pub = rospy.Publisher('points_raw',
PointCloud2,
queue_size=10)
self.imu_pub = rospy.Publisher("/imu_raw", Imu, queue_size=10)
self.vehicle_odom_pub = rospy.Publisher('/odom_pose',
Odometry,
queue_size=10)
self.joy_data = Joy()
self.vel_cmd1 = Odometry()
self.vel_cmd2 = Odometry()
self.bf1_pose = None
self.bf2_pose = None
self.odom_hz = 10
self.bf1_vel = None
self.bf2_vel = None
self.bf1_laser = LaserScan()
self.bf2_laser = LaserScan()
self.joy_button = None
self.lidar_counter = 0 # this is a hack to make lidar spin
self.laser_projection = LaserProjection()
self.velodyne_mapped_laserscan = LaserScan()
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
def __init__(self, **kwargs):
self.robot_frame = rospy.get_param('~robot_frame', 'load/base_link')
self.laser_frame = rospy.get_param('~laser_frame', 'load/laser')
self.footprint = rospy.get_param(
'~footprint',
[[-0.33, 0.33], [0.33, 0.33], [0.33, -0.33], [-0.33, -0.33]])
# class variables
self.tf_listener = tf.TransformListener()
self.laser_proj = LaserProjection()
self.only_use_half = kwargs.get('only_use_half', False)
self.use_front_half = True
footprint_padding = kwargs.get('footprint_padding', 0.1)
self.set_footprint_padding(footprint_padding)
self.cloud = None
self.debug = kwargs.get('debug', False)
# publisher
self.pc_pub = rospy.Publisher('~pc_debug', PointCloud2, queue_size=1)
self.footprint_pub = rospy.Publisher('~footprint_debug',
PolygonStamped,
queue_size=1)
# subscribers
laser_sub = rospy.Subscriber('~laser', LaserScan, self.laser_cb)
rospy.sleep(0.5)
if self.debug:
self.pub_debug_footprint()
rospy.sleep(0.2)
def __init__(self):
rospy.init_node('map_to_pointcloud')
self.br = tf.TransformBroadcaster()
self.transform = TransformStamped()
self.laser_footprint_transform = TransformStamped()
self.tf_listener = tf.TransformListener()
self.ts_old = rospy.Time.now()
self.index_old = 0
self.tf_listener.waitForTransform("/pr1/base_footprint", "pr1/left_laser_link", rospy.Time(0), rospy.Duration(1))
(self.trans, self.rot) = self.tf_listener.lookupTransform("/pr1/base_footprint", "pr1/left_laser_link",rospy.Time(0))
self.laser_footprint_transform.transform.translation.x = self.trans[0]
self.laser_footprint_transform.transform.translation.y = self.trans[1]
self.laser_footprint_transform.transform.rotation.x = self.rot[0]
self.laser_footprint_transform.transform.rotation.y = self.rot[1]
self.laser_footprint_transform.transform.rotation.z = self.rot[2]
self.laser_footprint_transform.transform.rotation.w = self.rot[3]
#rospy.Subscriber("/pr1/amcl_pose", PoseWithCovarianceStamped, self.robot_pose_cb)
rospy.Subscriber("map", OccupancyGrid, self.map_cb)
rospy.sleep(1)
self.laser_projection = LaserProjection()
rospy.Subscriber("/pr1/l_scan", LaserScan, self.laser_cb)
self.pc2_pub = rospy.Publisher("pointcloud", pc2, queue_size=1)
rospy.spin()
def __init__(self):
self.laserProj = LaserProjection()
self.pcPub = rospy.Publisher("/laserPointCloud", pc, queue_size=1)
self.laserSub = rospy.Subscriber("/scan",
LaserScan,
self.laserCallback,
queue_size=1)
def __init__(self):
self.laserProj = LaserProjection()
self.pcPub = rospy.Publisher('Pointy_Cloud',
PointCloud2,
queue_size=10)
self.laserSub = rospy.Subscriber("lidar", LaserScan,
self.laserCallback)
def __init__(self):
self.global_x = 0
self.global_y = 0
self.global_yaw = 0
self.global_ang_z = 0
self.RESOLUTION = 0.05
self.width = int(20 / self.RESOLUTION)
self.height = int(20 / self.RESOLUTION)
self.robot_in_grid = [0, 0]
self.full_scan = []
self.full_scan.append((0, 0, 0, 0, 0))
self.scanned_map = np.zeros(
(self.width, self.height)
) # initialize map to zero. Each cell represent a RESOLUTION squared area
self.occupancy_grid = np.ones(
(self.width, self.height), dtype=np.int) * -1
self.laserProj = LaserProjection()
self.pcPub = rospy.Publisher("/laserPointCloud",
PointCloud2,
queue_size=1)
self.laserSub = rospy.Subscriber("/scan", LaserScan,
self.laserCallback)
self.laserSub = rospy.Subscriber("/odom", Odometry,
self.positionCallback)
self.occup_grid_pub = rospy.Publisher("/my_map",
OccupancyGrid,
queue_size=1)
def __init__(self):
self.laserProj = LaserProjection()
self.pcPub = rospy.Publisher("/prometheus/sensors/pcl2",
pc2,
queue_size=1)
self.laserSub = rospy.Subscriber("/prometheus/sensors/2Dlidar_scan",
LaserScan, self.laserCallback)
def __init__(self):
# Variables
self.laserProj = LaserProjection()
self.isWater = False
# Publishers
self.pub = rospy.Publisher("/sonar_lidar_state",
LaserScanFiltered,
queue_size=10)
self.pcWaterPub = rospy.Publisher("/water_pc", pc2, queue_size=1)
# Subscribers
rospy.Subscriber("/scan", LaserScan, self.callback_get_lidar_scan)
rospy.Subscriber("/sonar", Range, self.callback_get_sonar_scan)
# Services
self.service_set_isWater = rospy.Service(
'is_water', SetBool, self.callback_isWater
) # Toggle for isWater when water is classified
# Defining variables:
self.scanFiltered = LaserScanFiltered()
self.scanFiltered.laser_range = 0.0
self.scanFiltered.laser_intensity = 0.0
self.scanFiltered.sonar_range = 0.0
self.sensor_offset = 13.5
rospy.spin()
def __init__(self,state):
rospy.loginfo("Scan done")
self.state = state
self.done = False
self.laser_msg = rospy.Subscriber("/scan", LaserScan, self.update_laser)
self.lp = LaserProjection()
pc = self.lp.projectLaser(scan)
pc.rangemin = targetdistance
def __init__(self, verbose=False):
self.rangeData = LaserScan()
self.scan_sub = rospy.Subscriber("/base_scan", LaserScan,
self.callback)
self.listener = tf.TransformListener()
self.laser_projector = LaserProjection()
self.pc = []
self.leg1 = []
self.leg2 = []
self.br = tf.TransformBroadcaster()
self.rate = rospy.Rate(20.0)
self.calibrated = False
self.reCalibration = False
self.priorOri_in_base_laser_link = [] # in base_laser_link frame
self.priorLeft_in_base_laser_link = [] # in base_laser_link frame
self.priorRight_in_base_laser_link = [] # in base_laser_link frame
self.odomL = [] # in odom_combined frame
self.odomR = [] # in odom_combined frame
self.priorAvailable = False
self.newObsWeight = 0.2
self.offsetXY = [-0.044, 0]
self.binOffset = 0.02
self.pubShelfSep = rospy.Publisher('pubShelfSep', PoseStamped)
self.reCalibrationCount = 4
self.tolerance = 0.1
self.updateRounds = 100
self.asyncRate = 20
self.limitInitX = True
self.xLimit = 0.1
self.emergencyThreshold = 30 * 20
self.emergencyTimeWindow = 60
while not rospy.is_shutdown():
try:
self.offsetXY = rospy.get_param('/base_scan/offset')
break
except:
rospy.loginfo(
'[shelf publisher]: waiting for base scan offset param')
rospy.sleep(random.uniform(0, 1))
continue
self.start_srv = rospy.Service(rospy.get_name() + '/start', Empty,
self.start_srv_callback)
self._start = False
# backup human supervised info for emergency
rp = rospkg.RosPack()
try:
dict_fp = rp.get_path('amazon_challenge_grasping')
except:
rospy.logerr('[shelf_publisher]: emergency file not found!')
sys.exit(1)
self.fileName = dict_fp + '/config/shelf_emergency.dat'
self.emergency = {}
def __init__(self):
self.laserProj = LaserProjection()
# self.inMsg = rospy.get_param('~inMsg')
# self.outMsg = rospy.get_param('~outMsg')
self.inMsg = '/scan'
self.outMsg = '/converted_pc'
self.pcPub = rospy.Publisher(self.outMsg, pc2, queue_size=1)
self.laserSub = rospy.Subscriber(self.inMsg, LaserScan,
self.laserCallback)
self.count = 0
def __init__(self):
self.laserProj = LaserProjection()
#self.pub = rospy.Publisher("/own/true/sonar_PC2",PointCloud2, queue_size=1)
#self.pub1 = rospy.Publisher("/own/simulated/sonar_PC2",PointCloud2, queue_size=1)
self.pub2 = rospy.Publisher("/sonar/PC2", PointCloud2, queue_size=1)
#self.sub = rospy.Subscriber("/desistek_saga/sonar",LaserScan, self.callback)
#self.sub1 = rospy.Subscriber("/own/simulated/sonar_LS",LaserScan, self.callback1)
self.sub2 = rospy.Subscriber("/sonar/LS", LaserScan, self.callback2)
def __init__(self, verbose=False):
self.rangeData = LaserScan()
self.scan_sub = rospy.Subscriber("/scan", LaserScan,
self.callback_laser)
rospy.sleep(1)
self.listener = tf.TransformListener()
self.laser_projector = LaserProjection()
self.pc = []
self.leg1 = []
self.leg2 = []
self.br = tf.TransformBroadcaster()
self.rate = rospy.Rate(10.0)
self.rate_measurements = rospy.Rate(0.5)
self.priorOri_in_base = []
self.priorLeft_in_base = []
self.priorRight_in_base = []
self.odomL = []
self.odomR = []
self.priorAvailable = False
self.newObsWeight = 0.2
self.offsetXY = [0.0, 0.0]
self.binOffset = 0.02
#self.pubShelfSep = rospy.Publisher('pubShelfSep', PoseStamped, queue_size=10)
self.reCalibrationCount = 4
self.tolerance = 0.1
self.updateRounds = 100
self.asyncRate = 20
self.limitInitX = True
self.xLimit = 0.1
self.rp = rospkg.RosPack()
self.writeFile = False
self.calculateTF = False
self.sendTF = False
self._ready = False
self.radius_th_leg = 0.1
self.n_samples = 5
self.sleep_between_scans = 2
while not rospy.is_shutdown():
try:
self._shelfHeight = rospy.get_param(
"/apc/shelf_calibration/height")
self._shelfRoll = rospy.get_param(
"/apc/shelf_calibration/roll")
self._shelfPitch = rospy.get_param(
"/apc/shelf_calibration/pitch")
break
except:
rospy.sleep(1.0)
continue
self.start_srv = rospy.Service(rospy.get_name() + '/calibrate', Empty,
self.start_srv_callback)
def readscan(self):
laser_projector = LaserProjection()
for topic, msg, time_stamp in self.bag.read_messages(
topics=[self.scan_topic]):
cloud = laser_projector.projectLaser(msg)
frame_points = np.zeros([0, 2])
for p in pc2.read_points(cloud,
field_names=("x", "y", "z"),
skip_nans=True):
p2d = np.array([p[0], p[1]])
frame_points = np.vstack([frame_points, p2d])
self.points.append([time_stamp, frame_points])
def __init__(self):
self.tfr = TransformerROS()
self.projector = LaserProjection()
self.outputTopic = rospy.get_param('output_topic', 'cloud')
self.inputTopic = rospy.get_param('input_topic', 'scan')
self.outputFrame = rospy.get_param('output_frame', 'base_link')
self.publisher = rospy.Publisher(self.outputTopic,
PointCloud2,
queue_size=10)
self.subscriber = rospy.Subscriber(self.inputTopic, LaserScan,
self.laser_callback)
def __init__(self,
slice_size=SLICE_SIZE_DEFAULT,
slice_offset=SLICE_OFFSET_DEFAULT,
max_dist_mult=MAX_DIST_MULT_DEFAULT,
publish_rate=PUBLISH_RATE_DEFAULT,
publish_pc=PUBLISH_PC_DEFAULT,
scan_topic=SCAN_TOPIC_DEFAULT,
contour_topic=CONTOUR_TOPIC_DEFAULT,
centroid_topic=CENTROID_TOPIC_DEFAULT,
pc_topic=PC_TOPIC_DEFAULT):
self.__slice_size = slice_size
self.__slice_offset = slice_offset
self.__max_dist_mult = max_dist_mult
self.__publish_point_cloud = publish_pc
self.__rate = rospy.Rate(publish_rate)
self.__laser_projector = LaserProjection()
self.__vals_lock = Lock()
self.__curr_msg = None
self.__data_available = False
self.__stopped = False
# Create Slices once and reset them on each iteration
self.__slices = [
Slice(v, v + self.__slice_size)
for v in range(0, 180, self.__slice_size)
]
rospy.loginfo(
"Publishing Contour vals to topic {}".format(contour_topic))
self.__contour_pub = rospy.Publisher(contour_topic,
Contour,
queue_size=5)
rospy.loginfo(
"Publishing Point vals to topic {}".format(centroid_topic))
self.__centroid_pub = rospy.Publisher(centroid_topic,
Point,
queue_size=5)
if self.__publish_point_cloud:
rospy.loginfo(
"Publishing PointCloud2 vals to topic {}".format(pc_topic))
self.__pc_pub = rospy.Publisher(pc_topic,
PointCloud2,
queue_size=5)
rospy.loginfo("Subscribing to LaserScan topic {}".format(scan_topic))
self.__scan_sub = rospy.Subscriber(scan_topic, LaserScan, self.on_msg)
def callbac(self, msg):
lst = []
self.converter = LaserProjection()
cloud = self.converter.projectLaser(msg)
self.pub = rospy.Publisher("/pc", pc2, queue_size=1)
self.pub.publish(cloud)
for p in pc22.read_points(cloud,
field_names=("x", "y", "z"),
skip_nans=True):
lst.append((p[0], p[1], p[2]))
print(lst)
print("")
print("-------------------")
print("")
def __init__(self):
plt.ion()
plt.show()
self.proj = LaserProjection()
rospy.init_node('nav_test', anonymous=False)
self.bridge = CvBridge()
self.OPENNING_ANGLE = np.deg2rad(60)
self.has_box = False
self.box_x = 0
self.moving = False
rospy.Subscriber("/usb_cam/image_raw", Image, self.img_callback)
rospy.Subscriber('scan', LaserScan, self.scan_callback)
rospy.on_shutdown(self.shutdown)
self.state = state_start
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base",
MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
rospy.loginfo("Starting navigation test")
if not rospy.is_shutdown():
rospy.spin()
class Laser2PC():
#translate laser to pc and publish point cloud to /cloud_in
def __init__(self):
self._measurements = [float('inf')]
self.laserProj = LaserProjection()
self.pcPub = rospy.Publisher('/point_cloud2', pc2, queue_size=5)
self.laserSub = rospy.Subscriber('/scan', LaserScan,
self.laserCallBack)
def laserCallBack(self, data):
# rospy.loginfo(rospy.get_caller_id() + 'I heard %s', data)
cloud_out = self.laserProj.projectLaser(data)
cloud_out.header.seq = data.header.frame_id
cloud_out.header.stamp = data.header.stamp
cloud_out.header.frame_id = 'laser0_frame'
self._measurements = cloud_out
self.pcPub.publish(cloud_out)
@property
def ready(self):
return not np.isnan(self._measurements)
@property
def measurements(self):
return self._measurements
class Laser2PC():
def __init__(self):
self.laserProj = LaserProjection()
self.pcPub = rospy.Publisher("/prometheus/sensors/pcl2",
pc2,
queue_size=1)
self.laserSub = rospy.Subscriber("/prometheus/sensors/2Dlidar_scan",
LaserScan, self.laserCallback)
def laserCallback(self, data):
# cc = LaserScan()
# cc.header = data.header
# cc.angle_max = data.angle_max
# cc.angle_min = data.angle_min
# cc.angle_increment = data.angle_increment
# cc.time_increment = data.time_increment
# cc.scan_time = data.scan_time
# cc.range_max = data.range_max
# cc.range_min = data.range_min
# cc.ranges = data.ranges
# cc.intensities = data.intensities
# a = len(data.ranges)
# for i in range(0,a):
# if data.ranges[i] != float("inf"):
# if data.ranges[i] < 0.4:
# cc.ranges[i] = 0.41
cloud_out = self.laserProj.projectLaser(data)
self.pcPub.publish(cloud_out)
def push(self,data_LaserScan):
# rospy.loginfo('project data_LaserScan to PointCloud OK')
pcl_LaserScan = LaserProjection().projectLaser(data_LaserScan)
points_xyz = self.xyz_from_pcl(pcl_LaserScan) # points_xyz: [N,3] [:,1]=0
# print "scan point N = ",points_xyz.shape[0]," / ", pcl_LaserScan.width, " rangesN = ",len(data_LaserScan.ranges)
if self.status == 'start' or self.status == 'scanning':
if self.status == 'start':
self.status = 'scanning'
self.add_data( pcl_LaserScan )
self.scanN += 1
self.pcl_3d_pub.publish(self.pcl_3d)
self.pcl3d_bag.write( 'pcl_3d', self.pcl_3d )
elif self.status == 'stop':
self.status = 'waitting'
if self.separate_models:
self.pcl_n = self.pcl_n + 1
self.reset()
self.pcl3d_bag.close()
rospy.loginfo('stop recording, save this model: ' + self.res_bag_name )
if self.status == 'scanning' and self.theta > 181.0 * math.pi / 180:
self.stop()
return self.scanN, self.theta
class Laser2PC():
def __init__(self):
self.laserProj = LaserProjection()
self.pcPub = rospy.Publisher("/laserPointCloud",
PointCloud2,
queue_size=1)
self.laserSub = rospy.Subscriber("/scan",
LaserScan,
self.laserCallback,
queue_size=1)
def laserCallback(self, msg):
pc2_msg = self.laserProj.projectLaser(msg)
self.pcPub.publish(pc2_msg)
# Convert it to a generator of the individual points
point_generator = pc2.read_points(pc2_msg)
# Access a generator in a loop
sum_points = 0.0
num = 0
for point in point_generator:
if not np.isnan(point[2]):
sum_points += point[2]
num += 1
# Calculate the average z value for example
print("Average height: " + str(sum_points / num))
def __init__(self):
self.max_vel = 0.0
self._sub = rospy.Subscriber('/scan', LaserScan, self.callback_scan)
self._pub = rospy.Publisher('/max_speed', Float32, queue_size=1)
self.laser_projector = LaserProjection()
self.scancount = 0
self.backcount = 0
self.backing = 0
def __init__(self):
"""
Class constructor
"""
rospy.init_node('explore', anonymous=True)
self.laser = LaserScan()
self.laser_proj = LaserProjection()
self.pc_pub = rospy.Publisher('/laserPointCloud',
PointCloud2,
queue_size=1)
self.sacn_subscriber = rospy.Subscriber('/scan', LaserScan,
self.callback)
self.velocity_publisher = rospy.Publisher('/cmd_vel',
Twist,
queue_size=10)
self.front_thresh = 0.3
class Laser2PC():
def __init__(self):
self.laserProj = LaserProjection()
self.pcPub = rospy.Publisher("/laserPointCLoud",pc2, queue_size=1)
self.laserSub = rospy.Subscriber("scan",LaserScan,self.laserCallback)
def laserCallback(self,data):
cloud_out = self.laserProj.projectLaser(data)
self.pcPub.publish(cloud_out)
class LaserScanToPointCloud:
def __init__(self):
self.laserProj = LaserProjection()
self.pointCloudPublisher = rospy.Publisher("/points", PointCloud2, queue_size = 1)
self.laserScanSubscriber = rospy.Subscriber("/scan", LaserScan, self.laserScanCallback)
def laserScanCallback(self, data):
self.pointCloudPublisher.publish(self.laserProj.projectLaser(data))
def __init__(self):
# Variables
self.laserProj = LaserProjection()
# Publishers
self.pcWorldPub = rospy.Publisher("/world_pc", pc2, queue_size=1)
#self.pcWaterPub = rospy.Publisher("/water_pc", pc2, queue_size=1)
self.mkArrayPub = rospy.Publisher("/marker_array_water_recoloured",
MarkerArray,
queue_size=1)
# Subscribers
rospy.Subscriber("/scan", LaserScan, self.callback_lidar_scan)
rospy.Subscriber("/marker_array_water", MarkerArray,
self.callback_recolour)
rospy.spin()
def __init__(self):
# Defining variables:
self.laserProj = LaserProjection()
self.scanFiltered = LaserScanFiltered()
self.scanFiltered.laser_range = 0.0
self.scanFiltered.laser_intensity = 0.0
self.scanFiltered.sonar_range = 0.0
self.sensor_offset = 10.0
rospy.Subscriber("/scan", LaserScan, self.callback_get_lidar_scan)
rospy.Subscriber("/sensor_state", SensorState,
self.callback_get_sonar_scan)
self.pub = rospy.Publisher("/sonar_lidar_state",
LaserScanFiltered,
queue_size=10)
self.pcWaterPub = rospy.Publisher("/water_pc", pc2, queue_size=1)
rospy.spin()
def readscan(self):
laser_projector = LaserProjection()
for topic, msg, time_stamp in self.bag.read_messages(
topics=[self.scan_topic]):
cloud = laser_projector.projectLaser(msg)
frame_points = np.zeros([0, 2])
for p in pc2.read_points(cloud,
field_names=("x", "y", "z"),
skip_nans=True):
#theta = math.atan2(p[0] , p[1])
#laser_range = np.pi
#if theta > np.pi/2 + np.pi/3 + laser_range/2 or theta < np.pi/2 + np.pi/3 - laser_range/2:
# continue
#d = math.sqrt(p[0]*p[0] + p[1]*p[1])
#if d > 1.5 and theta > np.pi/2:
# continue
p2d = np.array([p[0], p[1]])
frame_points = np.vstack([frame_points, p2d])
self.points.append([time_stamp, frame_points])
class Laser2PC():
def __init__(self):
self.laserProj= LaserProjection()
self.pcPub = rospy.Publisher('Pointy_Cloud', PointCloud2, queue_size=10)
self.laserSub = rospy.Subscriber("lidar", LaserScan, self.laserCallback)
def laserCallback(self,msg):
point_cloud = self.laserProj.projectLaser(msg)
self.pcPub.publish(point_cloud)
def __init__(self, verbose=False):
self.rangeData = LaserScan()
self.scan_sub = rospy.Subscriber("/base_scan", LaserScan, self.callback)
self.listener = tf.TransformListener()
self.laser_projector = LaserProjection()
self.pc = []
self.leg1 = []
self.leg2 = []
self.br = tf.TransformBroadcaster()
self.rate = rospy.Rate(20.0)
self.calibrated = False
self.reCalibration = False
self.priorOri_in_base_laser_link = [] # in base_laser_link frame
self.priorLeft_in_base_laser_link = [] # in base_laser_link frame
self.priorRight_in_base_laser_link = [] # in base_laser_link frame
self.odomL = [] # in odom_combined frame
self.odomR = [] # in odom_combined frame
self.priorAvailable = False
self.newObsWeight = 0.2
self.offsetXY = [-0.044, 0]
self.binOffset = 0.02
self.pubShelfSep = rospy.Publisher('pubShelfSep', PoseStamped)
self.reCalibrationCount = 4
self.tolerance = 0.1
self.updateRounds = 100
self.asyncRate = 20
self.limitInitX = True
self.xLimit = 0.1
self.emergencyThreshold = 30*20
self.emergencyTimeWindow = 60
while not rospy.is_shutdown():
try:
self.offsetXY = rospy.get_param('/base_scan/offset')
break
except:
rospy.loginfo('[shelf publisher]: waiting for base scan offset param')
rospy.sleep(random.uniform(0,1))
continue
self.start_srv = rospy.Service(rospy.get_name() + '/start', Empty, self.start_srv_callback)
self._start = False
# backup human supervised info for emergency
rp = rospkg.RosPack()
try:
dict_fp = rp.get_path('amazon_challenge_grasping')
except:
rospy.logerr('[shelf_publisher]: emergency file not found!')
sys.exit(1)
self.fileName = dict_fp + '/config/shelf_emergency.dat'
self.emergency = {}
def __init__(self, verbose=False):
self.rangeData = LaserScan()
self.scan_sub = rospy.Subscriber("/scan", LaserScan, self.callback_laser)
rospy.sleep(1)
self.listener = tf.TransformListener()
self.laser_projector = LaserProjection()
self.pc = []
self.leg1 = []
self.leg2 = []
self.br = tf.TransformBroadcaster()
self.rate = rospy.Rate(10.0)
self.rate_measurements = rospy.Rate(0.5)
self.priorOri_in_base = []
self.priorLeft_in_base = []
self.priorRight_in_base = []
self.odomL = []
self.odomR = []
self.priorAvailable = False
self.newObsWeight = 0.2
self.offsetXY = [0.0, 0.0]
self.binOffset = 0.02
#self.pubShelfSep = rospy.Publisher('pubShelfSep', PoseStamped, queue_size=10)
self.reCalibrationCount = 4
self.tolerance = 0.1
self.updateRounds = 100
self.asyncRate = 20
self.limitInitX = True
self.xLimit = 0.0
self.rp = rospkg.RosPack()
self.writeFile = False
self.calculateTF = False
self.sendTF = False
self._ready = False
self.radius_th_leg = 0.1
self.n_samples = 5
self.sleep_between_scans = 2
while not rospy.is_shutdown():
try:
self._shelfHeight = rospy.get_param("/apc/shelf_calibration/height")
self._shelfRoll = rospy.get_param("/apc/shelf_calibration/roll")
self._shelfPitch = rospy.get_param("/apc/shelf_calibration/pitch")
break
except:
rospy.sleep(1.0)
continue
self.start_srv = rospy.Service(rospy.get_name() + '/calibrate', Empty, self.start_srv_callback)
class Lidar():
def __init__(self):
rospy.loginfo("Initializing Lidar class. Creating subscriber")
self.scan_sub = rospy.Subscriber('/base_scan', LaserScan, self.on_scan)
self.xyz_points = None
rospy.loginfo("Initializing Lidar class. Creating LaserProjection")
self.laser_projector = LaserProjection()
def on_scan(self, scan):
rospy.loginfo("Got scan, projecting")
cloud = self.laser_projector.projectLaser(scan)
gen = pc2.read_points(cloud, skip_nans=True, field_names=("x", "y", "z"))
self.xyz_points = np.array(list(gen))
rospy.sleep(1/Me.CMD_FREQ)
def show_point_plot(self):
x = self.xyz_points[:,0]
y = self.xyz_points[:,1]
plt.scatter(x, y)
plt.show()
def __init__(self, verbose=False):
self.rangeData = LaserScan()
self.scan_sub = rospy.Subscriber("/base_scan", LaserScan, self.callback)
self.listener = tf.TransformListener()
self.laser_projector = LaserProjection()
self.pc = []
self.leg1 = []
self.leg2 = []
self.br = tf.TransformBroadcaster()
self.rate = rospy.Rate(100.0)
self.calibrated = False
self.reCalibration = False
self.priorOri = []
self.priorLeft = []
self.priorRight = []
self.odomL = []
self.odomR = []
self.priorAvailable = False
self.newObsWeight = 0.1
self.offsetXY = [-0.044, 0]
def __init__(self):
rospy.loginfo("Initializing Lidar class. Creating subscriber")
self.scan_sub = rospy.Subscriber('/base_scan', LaserScan, self.on_scan)
self.xyz_points = None
rospy.loginfo("Initializing Lidar class. Creating LaserProjection")
self.laser_projector = LaserProjection()
def test_project_laser(self):
tolerance = 6 # decimal places
projector = LaserProjection()
ranges = [-1.0, 1.0, 2.0, 3.0, 4.0, 5.0, 100.0]
intensities = np.arange(1.0, 6.0).tolist()
min_angles = -np.pi / np.array([1.0, 1.5, 2.0, 4.0, 8.0])
max_angles = -min_angles
angle_increments = np.pi / np.array([180., 360., 720.])
scan_times = [rospy.Duration(1./i) for i in [40, 20]]
for range_val, intensity_val, \
angle_min, angle_max, angle_increment, scan_time in \
product(ranges, intensities,
min_angles, max_angles, angle_increments, scan_times):
try:
scan = build_constant_scan(
range_val, intensity_val,
angle_min, angle_max, angle_increment, scan_time)
except BuildScanException:
if (angle_max - angle_min)/angle_increment > 0:
self.fail()
cloud_out = projector.projectLaser(scan, -1.0,
LaserProjection.ChannelOption.INDEX)
self.assertEquals(len(cloud_out.fields), 4,
"PointCloud2 with channel INDEX: fields size != 4")
cloud_out = projector.projectLaser(scan, -1.0,
LaserProjection.ChannelOption.INTENSITY)
self.assertEquals(len(cloud_out.fields), 4,
"PointCloud2 with channel INDEX: fields size != 4")
cloud_out = projector.projectLaser(scan, -1.0)
self.assertEquals(len(cloud_out.fields), 5,
"PointCloud2 with channel INDEX: fields size != 5")
cloud_out = projector.projectLaser(scan, -1.0,
LaserProjection.ChannelOption.INTENSITY |
LaserProjection.ChannelOption.INDEX)
self.assertEquals(len(cloud_out.fields), 5,
"PointCloud2 with channel INDEX: fields size != 5")
cloud_out = projector.projectLaser(scan, -1.0,
LaserProjection.ChannelOption.INTENSITY |
LaserProjection.ChannelOption.INDEX |
LaserProjection.ChannelOption.DISTANCE)
self.assertEquals(len(cloud_out.fields), 6,
"PointCloud2 with channel INDEX: fields size != 6")
cloud_out = projector.projectLaser(scan, -1.0,
LaserProjection.ChannelOption.INTENSITY |
LaserProjection.ChannelOption.INDEX |
LaserProjection.ChannelOption.DISTANCE |
LaserProjection.ChannelOption.TIMESTAMP)
self.assertEquals(len(cloud_out.fields), 7,
"PointCloud2 with channel INDEX: fields size != 7")
valid_points = 0
for i in range(len(scan.ranges)):
ri = scan.ranges[i]
if (PROJECTION_TEST_RANGE_MIN <= ri and
ri <= PROJECTION_TEST_RANGE_MAX):
valid_points += 1
self.assertEqual(valid_points, cloud_out.width,
"Valid points != PointCloud2 width")
idx_x = idx_y = idx_z = 0
idx_intensity = idx_index = 0
idx_distance = idx_stamps = 0
i = 0
for f in cloud_out.fields:
if f.name == "x":
idx_x = i
elif f.name == "y":
idx_y = i
elif f.name == "z":
idx_z = i
elif f.name == "intensity":
idx_intensity = i
elif f.name == "index":
idx_index = i
elif f.name == "distances":
idx_distance = i
elif f.name == "stamps":
idx_stamps = i
i += 1
i = 0
for point in pc2.read_points(cloud_out):
ri = scan.ranges[i]
ai = scan.angle_min + i * scan.angle_increment
self.assertAlmostEqual(point[idx_x], ri * np.cos(ai),
tolerance, "x not equal")
self.assertAlmostEqual(point[idx_y], ri * np.sin(ai),
tolerance, "y not equal")
self.assertAlmostEqual(point[idx_z], 0,
tolerance, "z not equal")
self.assertAlmostEqual(point[idx_intensity],
scan.intensities[i],
tolerance, "Intensity not equal")
self.assertAlmostEqual(point[idx_index], i,
tolerance, "Index not equal")
self.assertAlmostEqual(point[idx_distance], ri,
tolerance, "Distance not equal")
self.assertAlmostEqual(point[idx_stamps],
i * scan.time_increment,
tolerance, "Timestamp not equal")
i += 1
class lineScan:
def __init__(self, verbose=False):
self.rangeData = LaserScan()
self.scan_sub = rospy.Subscriber("/base_scan", LaserScan, self.callback)
self.listener = tf.TransformListener()
self.laser_projector = LaserProjection()
self.pc = []
self.leg1 = []
self.leg2 = []
self.br = tf.TransformBroadcaster()
self.rate = rospy.Rate(100.0)
self.calibrated = False
self.reCalibration = False
self.priorOri = []
self.priorLeft = []
self.priorRight = []
self.odomL = []
self.odomR = []
self.priorAvailable = False
self.newObsWeight = 0.1
self.offsetXY = [-0.044, 0]
def callback(self,data):
self.rangeData = data
def refreshRangeData(self):
self.rangeData = LaserScan() # flush
while len(self.rangeData.ranges) == 0:
rospy.sleep(0.04)
def getStatus(self):
return self.calibrated
def getCloud(self):
self.refreshRangeData()
cloud2 = self.laser_projector.projectLaser(self.rangeData)
xyz = pc2.read_points(cloud2, skip_nans=True, field_names=("x", "y", "z"))
self.pc = []
while True:
try:
self.pc.append(xyz.next())
except:
break
return self.pc
def segmentLength(self, m, i1, i2):
v1 = m[i1]
v2 = m[i2]
return np.linalg.norm(v1-v2)
def cloud2Input(self):
while not rospy.is_shutdown():
plt.clf()
pc = self.getCloud()
m = np.asarray(pc)
segs = []
r = split_and_merge(m, 0.1)
for i in range(1, len(r)):
if r[i] - r[i-1] < 20:
continue
l = self.segmentLength(m, r[i], r[i-1])
if l > 0.8 and l < 0.9:
print 'found'
print l
segs.append([r[i], r[i-1]])
# Plot found lines
plt.plot(m[:, 0], m[:, 1], '.r')
plt.plot(m[r, 0], m[r, 1], '-')
plt.plot(m[r, 0], m[r, 1], '+')
for s in segs:
plt.plot(m[s, 0], m[s, 1], 'g-+', linewidth=5)
plt.axis('equal')
plt.draw()
plt.title("Laser scan")
plt.show(block=False)
class baseScan:
def __init__(self, verbose=False):
self.rangeData = LaserScan()
self.scan_sub = rospy.Subscriber("/base_scan", LaserScan, self.callback)
self.listener = tf.TransformListener()
self.laser_projector = LaserProjection()
self.pc = []
self.leg1 = []
self.leg2 = []
self.br = tf.TransformBroadcaster()
self.rate = rospy.Rate(20.0)
self.calibrated = False
self.reCalibration = False
self.priorOri_in_base_laser_link = [] # in base_laser_link frame
self.priorLeft_in_base_laser_link = [] # in base_laser_link frame
self.priorRight_in_base_laser_link = [] # in base_laser_link frame
self.odomL = [] # in odom_combined frame
self.odomR = [] # in odom_combined frame
self.priorAvailable = False
self.newObsWeight = 0.2
self.offsetXY = [-0.044, 0]
self.binOffset = 0.02
self.pubShelfSep = rospy.Publisher('pubShelfSep', PoseStamped)
self.reCalibrationCount = 4
self.tolerance = 0.1
self.updateRounds = 100
self.asyncRate = 20
self.limitInitX = True
self.xLimit = 0.1
self.emergencyThreshold = 30*20
self.emergencyTimeWindow = 60
while not rospy.is_shutdown():
try:
self.offsetXY = rospy.get_param('/base_scan/offset')
break
except:
rospy.loginfo('[shelf publisher]: waiting for base scan offset param')
rospy.sleep(random.uniform(0,1))
continue
self.start_srv = rospy.Service(rospy.get_name() + '/start', Empty, self.start_srv_callback)
self._start = False
# backup human supervised info for emergency
rp = rospkg.RosPack()
try:
dict_fp = rp.get_path('amazon_challenge_grasping')
except:
rospy.logerr('[shelf_publisher]: emergency file not found!')
sys.exit(1)
self.fileName = dict_fp + '/config/shelf_emergency.dat'
self.emergency = {}
def start_srv_callback(self, req):
rospy.loginfo('[shelf_publisher]: starting shelf publisher!')
if self._start:
rospy.logwarn('[shelf_publisher]: already started!!!!')
self._start = True
return EmptyResponse()
def raw_input_with_timeout(prompt, timeout=1.0):
print prompt,
timer = threading.Timer(timeout, thread.interrupt_main)
astring = None
try:
timer.start()
astring = raw_input(prompt)
except KeyboardInterrupt:
pass
timer.cancel()
return astring
def callback(self,data):
self.rangeData = data
def refreshRangeData(self):
self.rangeData = LaserScan() # flush
while len(self.rangeData.ranges) == 0 and not rospy.is_shutdown():
rospy.sleep(0.04)
def getStatus(self):
return self.calibrated
def getCloud(self):
# self.refreshRangeData()
cloud2 = self.laser_projector.projectLaser(self.rangeData)
xyz = pc2.read_points(cloud2, skip_nans=True, field_names=("x", "y", "z"))
self.pc = []
while not rospy.is_shutdown():
try:
self.pc.append(xyz.next())
except:
break
return self.pc
def findLegsOnce(self):
pc = self.getCloud()
x = []
y= []
for i in range(len(pc)):
x.append(pc[i][0])
y.append(pc[i][1])
radius = []
if self.calibrated or self.reCalibration: # use prior to find legs
for i in range(len(x)):
radius.append(math.sqrt((x[i]-self.priorLeft_in_base_laser_link[0])**2 + (y[i] - self.priorLeft_in_base_laser_link[1])**2))
n1 = radius.index(min(radius))
radius = []
for i in range(len(x)):
radius.append(math.sqrt((x[i]-self.priorRight_in_base_laser_link[0])**2 + (y[i] - self.priorRight_in_base_laser_link[1])**2))
n2 = radius.index(min(radius))
leg1 = [x[n1], y[n1]]
leg2 = [x[n2], y[n2]]
else:
# Assuming there is nothing between the robot and the shelf
if self.limitInitX:
y = [y[i] for i in range(len(y)) if x[i] >= self.xLimit]
x = [x[i] for i in range(len(x)) if x[i] >= self.xLimit]
for i in range(len(x)):
radius.append(math.sqrt(x[i]**2 + y[i]**2))
n = radius.index(min(radius))
x2 = [x[i] for i in range(len(x)) if math.sqrt( (x[i]-x[n])**2 + (y[i]-y[n])**2 ) > 0.7]
y2 = [y[i] for i in range(len(y)) if math.sqrt( (x[i]-x[n])**2 + (y[i]-y[n])**2 ) > 0.7]
radius2 = []
for i in range(len(x2)):
radius2.append(math.sqrt(x2[i]**2 + y2[i]**2))
n2 = radius2.index(min(radius2))
leg1 = [x[n], y[n]]
leg2 = [x2[n2], y2[n2]]
leg1 = [leg1[0] + self.offsetXY[0], leg1[1] + self.offsetXY[1]]
leg2 = [leg2[0] + self.offsetXY[0], leg2[1] + self.offsetXY[1]]
return [leg1, leg2] # left, right
def findLegs(self):
'''
accumulate new observations
'''
L1x = 0
L1y = 0
L2x = 0
L2y = 0
legs = self.findLegsOnce()
if legs[0][1] < legs[1][1]:
legs[0], legs[1] = legs[1], legs[0]
if self.calibrated:
self.leg1[0] = self.leg1[0] * (1-self.newObsWeight) + legs[0][0] * self.newObsWeight
self.leg1[1] = self.leg1[1] * (1-self.newObsWeight) + legs[0][1] * self.newObsWeight
self.leg2[0] = self.leg2[0] * (1-self.newObsWeight) + legs[1][0] * self.newObsWeight
self.leg2[1] = self.leg2[1] * (1-self.newObsWeight) + legs[1][1] * self.newObsWeight
else:
self.leg1 = legs[0]
self.leg2 = legs[1]
return [self.leg1, self.leg2] # left, right
def getShelfFrame(self):
# with respect to the frame of /base_scan
legs = self.findLegs()
ori_x = (legs[0][0] + legs[1][0]) / 2.
ori_y = (legs[0][1] + legs[1][1]) / 2.
left_leg = legs[0]
if legs[0][1] < legs[1][1]:
left_leg = legs[1]
rotAngle = atan2(ori_x - left_leg[0], left_leg[1] - ori_y)
return [ori_x, ori_y], rotAngle, legs
def tf2PoseStamped(self, xy, ori):
shelfPoseMsg = PoseStamped()
shelfPoseMsg.pose.position.x = xy[0]
shelfPoseMsg.pose.position.y = xy[1]
shelfPoseMsg.pose.position.z = 0.0
shelfPoseMsg.pose.orientation.x = ori[0]
shelfPoseMsg.pose.orientation.y = ori[1]
shelfPoseMsg.pose.orientation.z = ori[2]
shelfPoseMsg.pose.orientation.w = ori[3]
shelfPoseMsg.header.frame_id = 'base_laser_link'
shelfPoseMsg.header.stamp = rospy.Time.now()
return shelfPoseMsg
def saveEmergency(self):
with open(self.fileName, 'wb') as handle:
pickle.dump(self.emergency, handle)
def loadEmergency(self):
with open(self.fileName, 'rb') as handle:
self.emergency = pickle.load(handle)
def publish2TF(self):
answer = 'n'
ask = True
u = 0
shelfN = 0
recalibrateCount = 0
emergencyCount = 0
t_init = rospy.Time.now()
while not rospy.is_shutdown():
self.rate.sleep()
# check if human calibration is done
shelfOri, shelfRot, legs = self.getShelfFrame()
if self.reCalibration:
u = 0
if not self.calibrated:
self.br.sendTransform((shelfOri[0], shelfOri[1], 0),
tf.transformations.quaternion_from_euler(0, 0, shelfRot),
rospy.Time.now(),
"/shelf_frame", # child
"/base_laser_link" # parent
)
self.br.sendTransform((legs[0][0], legs[0][1], 0), \
tf.transformations.quaternion_from_euler(0, 0, shelfRot), \
rospy.Time.now(), \
"/left_leg", \
"/base_laser_link")
self.br.sendTransform((legs[1][0], legs[1][1], 0), \
tf.transformations.quaternion_from_euler(0, 0, shelfRot), \
rospy.Time.now(), \
"/right_leg", \
"/base_laser_link")
if self.priorAvailable:
while not rospy.is_shutdown():
try:
shelf_in_odom, shelf_rot_in_odom = self.listener.lookupTransform("/odom_combined", "/shelf_frame", rospy.Time(0))
self.priorLeft_in_base_laser_link, dummy = self.listener.lookupTransform("/base_laser_link", "/odomL", rospy.Time(0))
self.priorRight_in_base_laser_link, dummy = self.listener.lookupTransform("/base_laser_link", "/odomR", rospy.Time(0))
break
except:
continue
else:
try:
shelf_in_odom, shelf_rot_in_odom = self.listener.lookupTransform("/odom_combined", "/shelf_frame", rospy.Time(0))
except Exception, e:
# print e
continue
if self.reCalibration and math.sqrt((shelf_in_odom[0]-self.priorOri_in_odom[0]) **2 + (shelf_in_odom[1]-self.priorOri_in_odom[1]) **2) <= self.tolerance:
# rospy.sleep(2)
recalibrateCount += 1
if recalibrateCount == self.reCalibrationCount: # take recalibration only if it's stable
rospy.loginfo('reCalibrated!')
recalibrateCount = 0
u = 0
while not rospy.is_shutdown(): # make sure the odomL and odomR are updated
try:
######## self.priorOri_in_odom, self.priorRot_in_odom = self.listener.lookupTransform("/odom_combined", "/shelf_frame", rospy.Time(0))
######## self.odomL, self.odomL_rot = self.listener.lookupTransform("/odom_combined", "/left_leg", rospy.Time(0))
######## self.odomR, self.odomR_rot = self.listener.lookupTransform("/odom_combined", "/right_leg", rospy.Time(0))
self.calibrated = True
self.reCalibration = False
rospy.loginfo("Prior origin in odom_combined: X = %4f, Y = %4f" % (self.priorOri_in_odom[0], self.priorOri_in_odom[1]))
break
except:
continue
if not self.calibrated and ask:
emergencyCount += 1
if emergencyCount == self.emergencyThreshold:
self.loadEmergency()
self.odomL = self.emergency.odomL
self.odomL_rot = self.emergency.odomL_rot
self.odomR = self.emergency.odomR
self.odomR_rot = self.emergency.odomR_rot
emergency_timestamp = self.emergency.timestamp
if (rospy.Time.now() - emergency_timestamp).to_sec() > self.emergencyTimeWindow:
print ''
rospy.logwarn('***********************************************************************')
rospy.logwarn('[shelf_publisher] NOT ABLE TO RECOVER FROM CRASHING, GAME OVER')
rospy.logwarn('***********************************************************************')
continue
ask = False
self.calibrated = True
self.priorAvailable = True
self.priorOri_in_odom = shelf_in_odom
self.priorRot_in_odom = shelf_rot_in_odom
self.priorLeft_in_base_laser_link = legs[0]
self.priorRight_in_base_laser_link = legs[1]
print ''
rospy.logwarn('***********************************************************************')
rospy.logwarn('[shelf_publisher] EMERGENCY RECOVERY CALLED!!!!!!!!!!!!!!!!!!!!!!')
rospy.logwarn('***********************************************************************')
else:
sys.stdout.write("\r [ROS time: %s] Is the current shelf pose estimation good? (y/n)" % rospy.get_time() )
sys.stdout.flush()
if self._start:
answer = 'y'
if answer == 'y' or answer == 'yes':
self.calibrated = True
ask = False
self.priorAvailable = True
self.priorOri_in_odom = shelf_in_odom
self.priorRot_in_odom = shelf_rot_in_odom
self.priorLeft_in_base_laser_link = legs[0]
self.priorRight_in_base_laser_link = legs[1]
print ""
rospy.loginfo("Human calibration finished")
rospy.loginfo("Prior origin in /odom_combined: X = %4f, Y = %4f" % (self.priorOri_in_odom[0], self.priorOri_in_odom[1]))
while not rospy.is_shutdown():
try:
self.odomL, self.odomL_rot = self.listener.lookupTransform("/odom_combined", "/left_leg", rospy.Time(0))
self.odomR, self.odomR_rot = self.listener.lookupTransform("/odom_combined", "/right_leg", rospy.Time(0))
break
except:
pass
self.emergency = shelfInfo(self.odomL, self.odomL_rot, self.odomR, self.odomR_rot, rospy.Time.now())
self.saveEmergency()
else:
continue
# check in the odometry frame
if self.priorAvailable:
# print 'pub odom'
self.br.sendTransform(self.odomL, \
self.odomL_rot, \
rospy.Time.now(),\
"/odomL", \
"/odom_combined")
self.br.sendTransform(self.odomR, \
self.odomR_rot, \
rospy.Time.now(),\
"/odomR", \
"/odom_combined")
if self.calibrated:
u += 1
shelfN += 1
if math.sqrt((shelf_in_odom[0] - self.priorOri_in_odom[0]) **2 + (shelf_in_odom[1] - self.priorOri_in_odom[1]) **2) > self.tolerance:
rospy.logwarn('something is wrong with shelf pose estimation!!!!!!!!!! RECALIBRATING')
recalibrateCount = 0
u = 0
self.calibrated = False
self.reCalibration = True
continue
else:
self.br.sendTransform((shelfOri[0], shelfOri[1], 0),
tf.transformations.quaternion_from_euler(0, 0, shelfRot),
rospy.Time.now(),
"/shelf_frame", # child
"/base_laser_link" # parent
)
self.br.sendTransform((legs[0][0], legs[0][1], 0), \
tf.transformations.quaternion_from_euler(0, 0, shelfRot), \
rospy.Time.now(),\
"/left_leg", \
"/base_laser_link")
self.br.sendTransform((legs[1][0], legs[1][1], 0), \
tf.transformations.quaternion_from_euler(0, 0, shelfRot), \
rospy.Time.now(),\
"/right_leg", \
"/base_laser_link")
self.pubShelfSep.publish(self.tf2PoseStamped(shelfOri, tf.transformations.quaternion_from_euler(0, 0, shelfRot)))
if u == self.updateRounds:
while not rospy.is_shutdown(): # make sure the odomL and odomR are updated
try:
self.priorOri_in_odom, self.priorRot_in_odom = self.listener.lookupTransform("/odom_combined", "/shelf_frame", rospy.Time(0))
self.odomL, self.odomL_rot = self.listener.lookupTransform("/odom_combined", "/left_leg", rospy.Time(0))
self.odomR, self.odomR_rot = self.listener.lookupTransform("/odom_combined", "/right_leg", rospy.Time(0))
rospy.loginfo("Prior origin in /odom_combined: X = %4f, Y = %4f" % (self.priorOri_in_odom[0], self.priorOri_in_odom[1]))
u = 0
break
except:
continue
if (rospy.Time.now()-t_init).to_sec()>1.0:
t_init = rospy.Time.now()
self.emergency = shelfInfo(self.odomL, self.odomL_rot, self.odomR, self.odomR_rot, rospy.Time.now())
self.saveEmergency()
# print 'pub'
shelfN = 0
self.br.sendTransform((0, 0.1515 + self.binOffset, 1.21),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"/shelf_bin_A", # child
"/shelf_frame" # parent
)
self.br.sendTransform((0, - 0.1515 + self.binOffset, 1.21),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"/shelf_bin_B", # child
"/shelf_frame" # parent
)
self.br.sendTransform((0, - 0.4303 + self.binOffset, 1.21),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"/shelf_bin_C", # child
"/shelf_frame" # parent
)
self.br.sendTransform((0, 0.1515 + self.binOffset, 1),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"/shelf_bin_D", # child
"/shelf_frame" # parent
)
self.br.sendTransform((0, - 0.1515 + self.binOffset, 1),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"/shelf_bin_E", # child
"/shelf_frame" # parent
)
self.br.sendTransform((0, - 0.4303 + self.binOffset, 1),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"/shelf_bin_F", # child
"/shelf_frame" # parent
)
self.br.sendTransform((0, 0.1515 + self.binOffset, 0.78),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"/shelf_bin_G", # child
"/shelf_frame" # parent
)
self.br.sendTransform((0, - 0.1515 + self.binOffset, 0.78),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"/shelf_bin_H", # child
"/shelf_frame" # parent
)
self.br.sendTransform((0, - 0.4303 + self.binOffset, 0.78),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"/shelf_bin_I", # child
"/shelf_frame" # parent
)
self.br.sendTransform((0, 0.1515 + self.binOffset, 0.51),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"/shelf_bin_J", # child
"/shelf_frame" # parent
)
self.br.sendTransform((0, - 0.1515 + self.binOffset, 0.51),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"/shelf_bin_K", # child
"/shelf_frame" # parent
)
self.br.sendTransform((0, - 0.4303 + self.binOffset, 0.51),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(),
"/shelf_bin_L", # child
"/shelf_frame" # parent
)
class baseScan:
def __init__(self, verbose=False):
self.rangeData = LaserScan()
self.scan_sub = rospy.Subscriber("/scan", LaserScan, self.callback_laser)
rospy.sleep(1)
self.listener = tf.TransformListener()
self.laser_projector = LaserProjection()
self.pc = []
self.leg1 = []
self.leg2 = []
self.br = tf.TransformBroadcaster()
self.rate = rospy.Rate(10.0)
self.rate_measurements = rospy.Rate(0.5)
self.priorOri_in_base = []
self.priorLeft_in_base = []
self.priorRight_in_base = []
self.odomL = []
self.odomR = []
self.priorAvailable = False
self.newObsWeight = 0.2
self.offsetXY = [0.0, 0.0]
self.binOffset = 0.02
#self.pubShelfSep = rospy.Publisher('pubShelfSep', PoseStamped, queue_size=10)
self.reCalibrationCount = 4
self.tolerance = 0.1
self.updateRounds = 100
self.asyncRate = 20
self.limitInitX = True
self.xLimit = 0.0
self.rp = rospkg.RosPack()
self.writeFile = False
self.calculateTF = False
self.sendTF = False
self._ready = False
self.radius_th_leg = 0.1
self.n_samples = 5
self.sleep_between_scans = 2
while not rospy.is_shutdown():
try:
self._shelfHeight = rospy.get_param("/apc/shelf_calibration/height")
self._shelfRoll = rospy.get_param("/apc/shelf_calibration/roll")
self._shelfPitch = rospy.get_param("/apc/shelf_calibration/pitch")
break
except:
rospy.sleep(1.0)
continue
self.start_srv = rospy.Service(rospy.get_name() + '/calibrate', Empty, self.start_srv_callback)
def start_srv_callback(self, req):
while not self._ready:
rospy.logwarn('Shelf published waiting for scan callback')
rospy.sleep(1.0)
rospy.loginfo('[shelf_publisher]: starting shelf publisher')
self.calculateTF = True
return EmptyResponse()
def callback_laser(self, data):
self._ready = True
self.rangeData = data
def getCloud(self):
cloud2 = self.laser_projector.projectLaser(self.rangeData)
xyz = pc2.read_points(cloud2, skip_nans=True, field_names=("x", "y", "z"))
self.pc = []
while not rospy.is_shutdown():
try:
self.pc.append(xyz.next())
except:
break
return self.pc
def findLegsOnce(self):
pc = self.getCloud()
x = []
y = []
for i in range(len(pc)):
x.append(pc[i][0])
y.append(pc[i][1])
radius = []
if self.limitInitX:
y = [y[i] for i in range(len(y)) if x[i] >= self.xLimit]
x = [x[i] for i in range(len(x)) if x[i] >= self.xLimit]
for i in range(len(x)):
radius.append(math.sqrt(x[i]**2 + y[i]**2))
n = radius.index(min(radius))
x1 = [x[i] for i in range(len(x)) if y[i] > 0.0]
y1 = [y[i] for i in range(len(y)) if y[i] > 0.0]
radius2 = []
x2 = [x[i] for i in range(len(x)) if y[i] < 0.0]
y2 = [y[i] for i in range(len(y)) if y[i] < 0.0]
for i in range(len(x2)):
radius2.append(math.sqrt(x2[i]**2 + y2[i]**2))
n2 = radius2.index(min(radius2))
leg1_p = [x[n], y[n]]
leg2_p = [x2[n2], y2[n2]]
x1_avg = [x[i] for i in range(len(x)) if math.sqrt( (x[i]-x[n])**2 + (y[i]-y[n])**2 ) < self.radius_th_leg]
y1_avg = [y[i] for i in range(len(y)) if math.sqrt( (x[i]-x[n])**2 + (y[i]-y[n])**2 ) < self.radius_th_leg]
x2_avg = [x[i] for i in range(len(x)) if math.sqrt( (x[i]-x[n2])**2 + (y[i]-y[n2])**2 ) < self.radius_th_leg]
y2_avg = [y[i] for i in range(len(y)) if math.sqrt( (x[i]-x[n2])**2 + (y[i]-y[n2])**2 ) < self.radius_th_leg]
leg1 = [numpy.mean(x1) + self.offsetXY[0], numpy.mean(y1) + self.offsetXY[1]]
leg2 = [numpy.mean(x2) + self.offsetXY[0], numpy.mean(y2) + self.offsetXY[1]]
#leg1 = [leg1[0] + self.offsetXY[0], leg1[1] + self.offsetXY[1]]
#leg2 = [leg2[0] + self.offsetXY[0], leg2[1] + self.offsetXY[1]]
return [leg1, leg2] # left, right
def findLegs(self):
leg1_x = numpy.zeros(self.n_samples)
leg1_y = numpy.zeros(self.n_samples)
leg2_x = numpy.zeros(self.n_samples)
leg2_y = numpy.zeros(self.n_samples)
for i in range(0, self.n_samples):
rospy.loginfo('taking laser scan: ')
rospy.loginfo(i)
legs_sample = self.findLegsOnce()
leg1_x[i] = legs_sample[0][0]
leg1_y[i] = legs_sample[0][1]
leg2_x[i] = legs_sample[1][0]
leg2_y[i] = legs_sample[1][1]
time.sleep(self.sleep_between_scans)
#self.rate_measurements.sleep()
leg1_m = [numpy.mean(leg1_x), numpy.mean(leg1_y)]
leg2_m = [numpy.mean(leg2_x), numpy.mean(leg2_y)]
rospy.loginfo('done scan')
rospy.loginfo(leg1_m)
rospy.loginfo(leg2_m)
legs = [leg1_m, leg2_m]
if legs[0][1] < legs[1][1]:
legs[0], legs[1] = legs[1], legs[0]
self.leg1 = legs[0]
self.leg2 = legs[1]
return [self.leg1, self.leg2] # left, right
def getShelfFrame(self):
legs = self.findLegs()
ori_x = (legs[0][0] + legs[1][0]) / 2.
ori_y = (legs[0][1] + legs[1][1]) / 2.
left_leg = legs[0]
if legs[0][1] < legs[1][1]:
left_leg = legs[1]
rotAngle = atan2(ori_x - left_leg[0], left_leg[1] - ori_y)
return [ori_x, ori_y], rotAngle, legs
def tf2PoseStamped(self, xy, ori):
shelfPoseMsg = PoseStamped()
shelfPoseMsg.pose.position.x = xy[0]
shelfPoseMsg.pose.position.y = xy[1]
shelfPoseMsg.pose.position.z = 0.0
shelfPoseMsg.pose.orientation.x = ori[0]
shelfPoseMsg.pose.orientation.y = ori[1]
shelfPoseMsg.pose.orientation.z = ori[2]
shelfPoseMsg.pose.orientation.w = ori[3]
shelfPoseMsg.header.frame_id = 'base'
shelfPoseMsg.header.stamp = rospy.Time.now()
return shelfPoseMsg
def estimateShelfFrame(self):
while not rospy.is_shutdown():
self.rate.sleep()
t_init = rospy.Time.now()
if self.calculateTF:
try:
shelfOri, shelfRot, legs = self.getShelfFrame()
except:
rospy.logerr("Shelf calibration is not getting points")
continue
self.calculateTF = False
self.sendTF = True
self.writeFile = True
if self.sendTF:
# F1 = kdl.Frame(kdl.Rotation.RPY(0, 0, shelfRot),
# kdl.Vector(shelfOri[0], shelfOri[0], self._shelfHeight))
# F2 = kdl.Frame(kdl.Rotation.RPY(self._shelfRoll, self._shelfPitch, 0), kdl.Vector(0,0,0))
# F3 = F1*F2
# position_xyz3 = F3.p
# orientation_xyzw3 = F3.M.GetQuaternion()
# self.br.sendTransform((position_xyz3[0], position_xyz3[1], position_xyz3[2]),
# orientation_xyzw3,
# rospy.Time.now(),
# "/shelf_front", # child
# "/base" # parent
# )
self.br.sendTransform((shelfOri[0], shelfOri[1], self._shelfHeight),
tf.transformations.quaternion_from_euler(self._shelfRoll, self._shelfPitch, shelfRot),
rospy.Time.now(),
"/shelf_front", # child
"/base" # parent
)
self.br.sendTransform((legs[0][0], legs[0][1], self._shelfHeight), \
tf.transformations.quaternion_from_euler(self._shelfRoll, self._shelfPitch, shelfRot),
rospy.Time.now(),
"/left_leg", # child
"/base" # parent
)
self.br.sendTransform((legs[1][0], legs[1][1], self._shelfHeight), \
tf.transformations.quaternion_from_euler(self._shelfRoll, self._shelfPitch, shelfRot),
rospy.Time.now(),
"/right_leg", # child
"/base" # parent
)
#self.pubShelfSep.publish(self.tf2PoseStamped(shelfOri, tf.transformations.quaternion_from_euler(0, 0, shelfRot)))
if self.writeFile:
file_path = self.rp.get_path('calibration_data')
f = open(file_path + "/extrinsics/shelf_front.txt", 'w')
f.write(str(shelfOri[0])+'\t')
f.write(str(shelfOri[1])+'\t')
f.write(str(0.0)+'\t')
quaternion_shelf = tf.transformations.quaternion_from_euler(0, 0, shelfRot)
f.write(str(quaternion_shelf[0])+'\t')
f.write(str(quaternion_shelf[1])+'\t')
f.write(str(quaternion_shelf[2])+'\t')
f.write(str(quaternion_shelf[3])+'\n')
f.close()
self.writeFile = False