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())