def keepRevolving(self, event):
"""Keep the micron join Revolving."""
"""
Revolves the joint on AUV (G500 and SPARUS-II) where the range
sensor is attached to.
"""
# r = rospy.Rate(self.config.angle_inc_rate)
# while not rospy.is_shutdown():
if self.joint_state_available_:
# ==================================================================
# Get last range
# ==================================================================
if len(self.accum_ranges_) == 0:
self.first_beam_time_ = rospy.Time.now()
if self.sonar_range_ < self.config.max_range:
self.accum_ranges_.append(self.sonar_range_)
rospy.logdebug("%s: Grabbed new beam", rospy.get_name())
else:
self.accum_ranges_.append(self.config.max_range)
rospy.logdebug("%s: Discarding beam > max_range",
rospy.get_name())
self.accum_angles_.append(self.last_angle_)
# ==================================================================
# Revolve simulated sonar
# ==================================================================
rospy.logdebug("%s: moving joint", rospy.get_name())
msg = JointState()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = 'seaking_profiler'
msg.name = [self.jointname]
msg.position = [cola2_lib.wrapAngle(self.angle_)]
msg.velocity = []
msg.effort = []
self.last_angle_ = self.angle_
self.joint_sonar_command_pub_.publish(msg)
rospy.logdebug("-------------------------------------------------")
if self.config.mode == 'sector':
rospy.logdebug("Sector")
else:
rospy.logdebug("Cont")
rospy.logdebug("Scan right = " + str(self.scan_right_))
rospy.logdebug("Angle: {0} ({1} - {2})".format(
math.degrees(self.angle_), math.degrees(self.config.llim),
math.degrees(self.config.rlim)))
# Update angle
# TODO what if sector goes through 0 and rlim>180 or llim<180?
change_direction = False
if self.config.mode == 'sector' and self.config.rlim > self.config.llim:
if self.scan_right_ and self.angle_ >= self.config.rlim:
self.scan_right_ = False
change_direction = True
elif not self.scan_right_ and self.angle_ <= self.config.llim:
self.scan_right_ = True
change_direction = True
elif self.config.mode == 'sector' and self.config.rlim < self.config.llim:
angle = cola2_lib.wrapAngle(self.angle_)
rlim = cola2_lib.wrapAngle(self.config.rlim)
llim = cola2_lib.wrapAngle(self.config.llim)
if self.scan_right_ and angle >= rlim:
self.scan_right_ = False
change_direction = True
elif not self.scan_right_ and angle <= llim:
self.scan_right_ = True
change_direction = True
elif self.config.mode != 'cont':
self.angle_ = self.config.rlim
if self.scan_right_:
ainc = self.config.angle_inc
else:
ainc = -self.config.angle_inc
self.angle_ = self.wrapAngle(self.angle_ + ainc)
# ==================================================================
# Checking if laser_scan is complete to be published
# ==================================================================
if len(self.accum_ranges_
) + 1 > self.config.publish_every or change_direction:
rospy.logdebug("len(self.accum_ranges_): " +
str(len(self.accum_ranges_)))
rospy.logdebug("self.accum_ranges_: " +
str(self.accum_ranges_))
rospy.logdebug("self.accum_angles_: " +
str(self.accum_angles_))
msg = LaserScan()
# timestamp in the header is the acquisition time of
# the first ray in the scan.
#
# in frame frame_id, angles are measured around
# the positive Z axis (counterclockwise, if Z is up)
# with zero angle being forward along the x axis
msg.header.stamp = self.first_beam_time_
msg.header.frame_id = self.config.frame_id
if self.scan_right_:
# angular distance between measurements [rad]
msg.angle_increment = self.config.angle_inc
# start angle of the scan [rad]
msg.angle_min = min(self.accum_angles_)
# end angle of the scan [rad]
msg.angle_max = max(self.accum_angles_)
else:
msg.angle_increment = -self.config.angle_inc
# start angle of the scan [rad]
msg.angle_min = max(self.accum_angles_)
# end angle of the scan [rad]
msg.angle_max = min(self.accum_angles_)
# time between measurements [seconds] - if your scanner
msg.time_increment = 1.0 / self.config.angle_inc_rate
# is moving, this will be used in interpolating position
# of 3d points
# time between scans [seconds]
msg.scan_time = msg.time_increment * (len(self.accum_ranges_) -
1)
# minimum range value [m]
msg.range_min = self.config.min_range
# max(self.accum_ranges) maximum range value [m]
msg.range_max = self.config.max_range
msg.ranges = self.accum_ranges_
msg.intensities = []
self.accum_ranges_ = []
self.accum_angles_ = []
self.tf_broadcaster_.sendTransform(
(self.translation[0], self.translation[1],
self.translation[2]),
tf.transformations.quaternion_from_euler(
self.rotation_rpy[0], self.rotation_rpy[1],
self.rotation_rpy[2]), msg.header.stamp,
self.config.frame_id, self.config.robot_frame_name)
self.sonar_slice_pub_.publish(msg)
else:
rospy.logwarn("%s: no joint state available", rospy.get_name())
