def write_estimated_pose_timer_callback(self): try: transform_msg = self.tf_buffer.lookup_transform( 'map', 'base_link', Time()) orientation = transform_msg.transform.rotation theta, _, _ = pyquaternion.Quaternion( x=orientation.x, y=orientation.y, z=orientation.z, w=orientation.w).yaw_pitch_roll self.estimated_poses_df = self.estimated_poses_df.append( { 't': nanoseconds_to_seconds( Time.from_msg(transform_msg.header.stamp).nanoseconds), 'x': transform_msg.transform.translation.x, 'y': transform_msg.transform.translation.y, 'theta': theta }, ignore_index=True) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): pass
def test_headerless(self): sub = Subscriber(self.node, String, "/empty") cache = Cache(sub, 5, allow_headerless=False) msg = String() cache.add(msg) self.assertIsNone( cache.getElemAfterTime(Time(clock_type=ClockType.ROS_TIME)), "Headerless message invalidly added.") cache = Cache(sub, 5, allow_headerless=True) cache.add(msg) s = cache.getElemAfterTime(Time(clock_type=ClockType.ROS_TIME)) self.assertEqual(s, msg, "invalid msg returned in headerless scenario") currentRosTime = ROSClock().now() s = cache.getElemAfterTime(currentRosTime) self.assertIsNone(s, "invalid msg returned in headerless scenario") cache.add(msg) s = cache.getInterval(Time(clock_type=ClockType.ROS_TIME), currentRosTime) self.assertEqual(s, [msg], "invalid msg returned in headerless scenario") s = cache.getInterval(Time(clock_type=ClockType.ROS_TIME), (currentRosTime + Duration(seconds=2))) self.assertEqual(s, [msg, msg], "invalid msg returned in headerless scenario")
def lookup_transform_full( self, target_frame: str, target_time: Time, source_frame: str, source_time: Time, fixed_frame: str, timeout: Duration = Duration() ) -> TransformStamped: """ Get the transform from the source frame to the target frame using the advanced API. :param target_frame: Name of the frame to transform into. :param target_time: The time to transform to. (0 will get the latest) :param source_frame: Name of the input frame. :param source_time: The time at which source_frame will be evaluated. (0 will get the latest) :param fixed_frame: Name of the frame to consider constant in time. :param timeout: Time to wait for the target frame to become available. :return: The transform between the frames. """ goal = LookupTransform.Goal() goal.target_frame = target_frame goal.source_frame = source_frame goal.source_time = source_time.to_msg() goal.timeout = timeout.to_msg() goal.target_time = target_time.to_msg() goal.fixed_frame = fixed_frame goal.advanced = True return self.__process_goal(goal)
def add(self, msg, my_queue, my_queue_index=None): if not hasattr(msg, 'header') or not hasattr(msg.header, 'stamp'): if not self.allow_headerless: msg_filters_logger = rclpy.logging.get_logger( 'message_filters_approx') msg_filters_logger.set_level(LoggingSeverity.INFO) msg_filters_logger.warn( "can not use message filters for " "messages without timestamp infomation when " "'allow_headerless' is disabled. auto assign " "ROSTIME to headerless messages once enabling " "constructor option of 'allow_headerless'.") return stamp = ROSClock().now() else: stamp = msg.header.stamp if not hasattr(stamp, 'nanoseconds'): stamp = Time.from_msg(stamp) # print(stamp) self.lock.acquire() my_queue[stamp.nanoseconds] = msg while len(my_queue) > self.queue_size: del my_queue[min(my_queue)] # self.queues = [topic_0 {stamp: msg}, topic_1 {stamp: msg}, ...] if my_queue_index is None: search_queues = self.queues else: search_queues = self.queues[:my_queue_index] + \ self.queues[my_queue_index+1:] # sort and leave only reasonable stamps for synchronization stamps = [] for queue in search_queues: topic_stamps = [] for s in queue: stamp_delta = Duration(nanoseconds=abs(s - stamp.nanoseconds)) if stamp_delta > self.slop: continue # far over the slop topic_stamps.append( ((Time(nanoseconds=s, clock_type=stamp.clock_type)), stamp_delta)) if not topic_stamps: self.lock.release() return topic_stamps = sorted(topic_stamps, key=lambda x: x[1]) stamps.append(topic_stamps) for vv in itertools.product(*[list(zip(*s))[0] for s in stamps]): vv = list(vv) # insert the new message if my_queue_index is not None: vv.insert(my_queue_index, stamp) qt = list(zip(self.queues, vv)) if (((max(vv) - min(vv)) < self.slop) and (len([1 for q, t in qt if t.nanoseconds not in q]) == 0)): msgs = [q[t.nanoseconds] for q, t in qt] self.signalMessage(*msgs) for q, t in qt: del q[t.nanoseconds] break # fast finish after the synchronization self.lock.release()
def _get_earlist_index(self, index_list): is_last = False earlist_index = -1 index = 0 queue_list = [] for queue in self.fusion_operator.queues_for_input_ports.values(): queue_list.append(queue) index = index + 1 index = 0 for queue in queue_list: if index_list[index] != None: if earlist_index < 0: earlist_index = index elif len(queue) > 0: new = queue[index_list[index]] new_stamp = new.header.stamp new_stamp = Time.from_msg(new_stamp) cur_queue = queue_list[earlist_index] cur = cur_queue[index_list[earlist_index]] cur_stamp = cur.header.stamp cur_stamp = Time.from_msg(cur_stamp) if (new_stamp < cur_stamp): earlist_index = index index = index + 1 if len(queue_list[earlist_index]) == index_list[earlist_index] + 1: is_last = True return earlist_index, is_last
def fusion_callback(self, channel, msg): try: start = time.time() self.fusion_operator.queues_for_input_ports[channel].append(msg) valid_input_data = self._find_valid_input_data() oldest_birthmark = None if valid_input_data: for c, data in valid_input_data.items(): if data == None: continue if oldest_birthmark == None or Time.from_msg( data.header.stamp) < Time.from_msg( oldest_birthmark): oldest_birthmark = data.header.stamp freshness_constraint = data.freshness_constraint valid_input_data[c] = pickle.loads(data.body) splash_message = SplashMessage() header = Header() header.stamp = oldest_birthmark splash_message.header = header splash_message.body = array('B', pickle.dumps(valid_input_data)) splash_message.freshness_constraint = freshness_constraint self.fusion_operator.stream_output_ports[ self.fusion_operator.output_channel].write(splash_message) else: pass print(f"time: {(time.time() - start) * 1000:.2f}ms") except Exception as e: print(traceback.format_exc()) print(e)
def _check_msg_time(self, foot_location: PointStamped) -> bool: """Checks if the foot_location given by CoViD is not older than FOOT_LOCATION_TIME_OUT.""" msg_time = Time( seconds=foot_location.header.stamp.sec, nanoseconds=foot_location.header.stamp.nanosec, ) current_time = Time( seconds=self.gait_selection.get_clock().now().seconds_nanoseconds( )[0], nanoseconds=self.gait_selection.get_clock().now(). seconds_nanoseconds()[1], ) time_difference = current_time - msg_time self.logger.debug( "Time difference between CoViD foot location and current time: " f"{time_difference}.", ) if time_difference > FOOT_LOCATION_TIME_OUT: self.logger.info( "Foot location is more than 0.5 seconds old, time difference is " f"{time_difference}. Stopping gait.", ) self._end = True return True return False
def test_time_source_using_sim_time(self): time_source = TimeSource(node=self.node) clock = ROSClock() time_source.attach_clock(clock) # Setting ROS time active on a time source should also cause attached clocks' use of ROS # time to be set to active. self.assertFalse(time_source.ros_time_is_active) self.assertFalse(clock.ros_time_is_active) time_source.ros_time_is_active = True self.assertTrue(time_source.ros_time_is_active) self.assertTrue(clock.ros_time_is_active) # A subscriber should have been created assert time_source._clock_sub is not None # When using sim time, ROS time should look like the messages received on /clock self.publish_clock_messages() assert clock.now() > Time(seconds=0, clock_type=ClockType.ROS_TIME) assert clock.now() <= Time(seconds=5, clock_type=ClockType.ROS_TIME) # Check that attached clocks get the cached message clock2 = Clock(clock_type=ClockType.ROS_TIME) time_source.attach_clock(clock2) assert clock2.now() > Time(seconds=0, clock_type=ClockType.ROS_TIME) assert clock2.now() <= Time(seconds=5, clock_type=ClockType.ROS_TIME) # Check detaching the node time_source.detach_node() node2 = rclpy.create_node('TestTimeSource2', namespace='/rclpy') time_source.attach_node(node2) node2.destroy_node() assert time_source._node == node2 assert time_source._clock_sub is not None
def alignTimer(clock, timer): # This function is a simple approximate alignment of a timer, it can be made smarter by compensating ns1 # for the time it takes to reset the timer. This would have to be done by running this # algorithm a couple of times and take the avarage of the error and subtract this from ns1 dt = timer.timer_period_ns time1 = clock.now() ns = time1.nanoseconds N, mod = divmod(ns, dt) ns1 = (N + 1) * dt # time for which the timer should be reset if ns == 0: # when sim_time_true, clock is always zero in the constructor... https://answers.ros.org/question/356733/ros2-why-rclcpp-node-now-function-returns-0-in-node-construtor/ t0 = Time(nanoseconds=0) else: while True: # wait until one dt has passed... time2 = clock.now() ns2 = time2.nanoseconds if ns2 >= ns1: # reset timer timer.reset() break t0 = Time( nanoseconds=ns1 ) # return the time for when the timer was supposed to be reset return t0
def save_camera_data(bag, kitti_type, kitti, util, bridge, camera, camera_frame_id, topic_name, initial_time): print(f"Exporting camera {camera}") if kitti_type.find("raw") != -1: camera_pad = '{0:02d}'.format(camera) image_dir = os.path.join(kitti.data_path, 'image_{}'.format(camera_pad)) image_path = os.path.join(image_dir, 'data') image_filenames = sorted(os.listdir(image_path)) with open(os.path.join(image_dir, 'timestamps.txt')) as f: image_datetimes = map(lambda x: datetime.strptime(x[:-4], '%Y-%m-%d %H:%M:%S.%f'), f.readlines()) calib = CameraInfo() calib.header.frame_id = camera_frame_id calib.width, calib.height = tuple([int(x) for x in util[f'S_rect_{camera_pad}']]) calib.distortion_model = 'plumb_bob' calib.k = util[f'K_{camera_pad}'] calib.r = util[f'R_rect_{camera_pad}'] calib.d = [float(x) for x in util[f'D_{camera_pad}']] calib.p = util[f'P_rect_{camera_pad}'] elif kitti_type.find("odom") != -1: camera_pad = '{0:01d}'.format(camera) image_path = os.path.join(kitti.sequence_path, f'image_{camera_pad}') image_filenames = sorted(os.listdir(image_path)) image_datetimes = map(lambda x: initial_time + x.total_seconds(), kitti.timestamps) calib = CameraInfo() calib.header.frame_id = camera_frame_id calib.p = util[f'P{camera_pad}'] iterable = zip(image_datetimes, image_filenames) bar = progressbar.ProgressBar() for dt, filename in bar(iterable): image_filename = os.path.join(image_path, filename) cv_image = cv2.imread(image_filename) calib.height, calib.width = cv_image.shape[:2] if camera in (0, 1): cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY) encoding = "mono8" if camera in (0, 1) else "bgr8" image_message = bridge.cv2_to_imgmsg(cv_image, encoding=encoding) image_message.header.frame_id = camera_frame_id if kitti_type.find("raw") != -1: image_message.header.stamp = Time(nanoseconds=int(float(datetime.strftime(dt, "%s.%f")) * 1e+9)).to_msg() topic_ext = "/image_raw" elif kitti_type.find("odom") != -1: image_message.header.stamp = Time(seconds=dt).to_msg() topic_ext = "/image_rect" calib.header.stamp = image_message.header.stamp camera_info_topic = create_topic(topic_name + '/camera_info', "sensor_msgs/msg/CameraInfo", "cdr") bag.create_topic(camera_info_topic) camera_topic = create_topic(topic_name + topic_ext, "sensor_msgs/msg/Image", "cdr") bag.create_topic(camera_topic) bag.write((topic_name + topic_ext, serialize_message(image_message), timestamp_to_nanoseconds_from_epoch(image_message.header.stamp))) bag.write((topic_name + '/camera_info', serialize_message(calib), timestamp_to_nanoseconds_from_epoch(calib.header.stamp)))
def _heartbeat_callback(self, msg): self.get_logger().debug('Heartbeat received {}'.format(msg.stamp)) current = Time.from_msg(self._clock.now().to_msg()) if (current - Time.from_msg(msg.stamp)) > self._timeout_duration: return self._set_connected(True) self._timer.reset()
def test_time_message_conversions_big_nanoseconds(self): time1 = Time(nanoseconds=1553575413247045598, clock_type=ClockType.ROS_TIME) builtins_msg = Builtins() builtins_msg.time_value = time1.to_msg() # Default clock type resulting from from_msg will be ROS time time2 = Time.from_msg(builtins_msg.time_value) assert isinstance(time2, Time) assert time1 == time2
def on_message(self, message): cls = self.__class__ # check if we need to authenticate if cls.authenticate and not self.authenticated: try: if cls.bson_only_mode: msg = bson.BSON(message).decode() else: msg = json.loads(message) if msg['op'] == 'auth': # check the authorization information auth_srv_client = cls.node_handle.create_client( Authentication, 'authenticate') auth_srv_req = Authentication.Request() auth_srv_req.mac = msg['mac'] auth_srv_req.client = msg['client'] auth_srv_req.dest = msg['dest'] auth_srv_req.rand = msg['rand'] auth_srv_req.t = Time(seconds=msg['t']).to_msg() auth_srv_req.level = msg['level'] auth_srv_req.end = Time(seconds=msg['end']).to_msg() while not auth_srv_client.wait_for_service( timeout_sec=1.0): cls.node_handle.get_logger().info( 'Authenticate service not available, waiting again...' ) future = auth_srv_client.call_async(auth_srv_req) rclpy.spin_until_future_complete(cls.node_handle, future) # Log error if service could not be called. if future.result() is not None: self.authenticated = future.result().authenticated else: self.authenticated = False cls.node_handle.get_logger.error( 'Authenticate service call failed') if self.authenticated: cls.node_handle.get_logger().info( "Client {} has authenticated.".format( self.protocol.client_id)) return # if we are here, no valid authentication was given cls.node_handle.get_logger().warn( "Client {} did not authenticate. Closing connection.". format(self.protocol.client_id)) self.close() except: # proper error will be handled in the protocol class self.protocol.incoming(message) else: # no authentication required self.protocol.incoming(message)
def test_update_subgait_start_delayed_did_start(self): self.gait.start(Time(seconds=0), Duration(seconds=3)) gait_update = self.gait.update(Time(seconds=3.5)) self.assertEqual(self.gait.subgait_name, "right_open") self.assertEqual(self.gait._start_time, Time(seconds=3)) self.assertEqual(self.gait._end_time, Time(seconds=4.5)) self.assertFalse(self.gait._start_is_delayed) self.assertEqual(gait_update, GaitUpdate.subgait_updated())
def test_update_subgait_not_done(self): self.gait.start(Time(seconds=0)) gait_update = self.gait.update(Time(seconds=0.6)) self.assertEqual(self.gait.subgait_name, "right_open") self.assertEqual(self.gait._start_time, Time(seconds=0)) self.assertEqual(self.gait._end_time, Time(seconds=1.5)) self.assertFalse(self.gait._scheduled_early) self.assertEqual(gait_update, GaitUpdate.empty())
def test_triggered_time_jump_callbacks(self): one_second = Duration(seconds=1) half_second = Duration(seconds=0.5) negative_half_second = Duration(seconds=-0.5) negative_one_second = Duration(seconds=-1) threshold1 = JumpThreshold(min_forward=one_second, min_backward=negative_half_second, on_clock_change=False) threshold2 = JumpThreshold(min_forward=half_second, min_backward=negative_one_second, on_clock_change=False) pre_callback1 = Mock() post_callback1 = Mock() pre_callback2 = Mock() post_callback2 = Mock() clock = ROSClock() handler1 = clock.create_jump_callback(threshold1, pre_callback=pre_callback1, post_callback=post_callback1) handler2 = clock.create_jump_callback(threshold2, pre_callback=pre_callback2, post_callback=post_callback2) clock.set_ros_time_override(Time(seconds=1)) clock._set_ros_time_is_active(True) pre_callback1.assert_not_called() post_callback1.assert_not_called() pre_callback2.assert_not_called() post_callback2.assert_not_called() # forward jump clock.set_ros_time_override(Time(seconds=1.75)) pre_callback1.assert_not_called() post_callback1.assert_not_called() pre_callback2.assert_called() post_callback2.assert_called() pre_callback1.reset_mock() post_callback1.reset_mock() pre_callback2.reset_mock() post_callback2.reset_mock() # backwards jump clock.set_ros_time_override(Time(seconds=1)) pre_callback1.assert_called() post_callback1.assert_called() pre_callback2.assert_not_called() post_callback2.assert_not_called() handler1.unregister() handler2.unregister()
def test_time_message_conversions(self): time1 = Time(nanoseconds=1, clock_type=ClockType.ROS_TIME) builtins_msg = Builtins() builtins_msg.time_value = time1.to_msg() # Default clock type resulting from from_msg will be ROS time time2 = Time.from_msg(builtins_msg.time_value) assert isinstance(time2, Time) assert time1 == time2 # Clock type can be specified if appropriate time3 = Time.from_msg(builtins_msg.time_value, clock_type=ClockType.SYSTEM_TIME) assert time3.clock_type == ClockType.SYSTEM_TIME
def main() -> int: args = get_args() infile = args.infile if not os.path.exists(infile): raise OSError("The input file does not exist: {}".format(infile)) outfile = args.outfile if outfile is None: outfile = infile.rstrip(".bag") outfile += "-filtered.bag" t0 = None t1 = None if args.start_time is not None: t0 = Time(nanoseconds=args.start_time) if args.end_time is not None: t1 = Time(nanoseconds=args.end_time) topic_list = None if args.topics is not None: topic_list = args.topics topic_lut = {} if args.map is not None: for remapping in args.map: from_to = remapping.split(":") if len(from_to) == 2: topic_lut[str(from_to[0])] = str(from_to[1]) with BagReader(infile) as inbag: compress_opts = None if ((not args.compress) and (not args.uncompress)): compress_opts = inbag.comp_opt_ with BagWriter(outfile, compress=args.compress, storage_opts=inbag.s_opt_, converter_opts=inbag.c_opt_, compression_opts=compress_opts) as outbag: for topic, msg, t, in inbag.read_messages(topics=topic_list, start_time=t0, end_time=t1): out_topic = topic if ((args.map is not None) and (topic in topic_lut)): out_topic = topic_lut[topic] outbag.write(out_topic, msg, t) return 0
def test_start_delayed(self): gait_update = self.gait.start(Time(seconds=0), Duration(seconds=1)) self.assertEqual(self.gait.subgait_name, "right_open") self.assertEqual(self.gait._start_time, Time(seconds=1)) self.assertEqual(self.gait._end_time, Time(seconds=2.5)) self.assertTrue(self.gait._start_is_delayed) self.assertEqual( gait_update, GaitUpdate.should_schedule_early( self.gait._command_from_current_subgait()), )
def test_start(self): gait_update = self.gait.start(Time(seconds=0)) self.assertEqual(self.gait.subgait_name, "right_open") self.assertEqual(self.gait._start_time, Time(seconds=0)) self.assertEqual(self.gait._end_time, Time(seconds=1.5)) self.assertFalse(self.gait._start_is_delayed) self.assertEqual( gait_update, GaitUpdate.should_schedule( self.gait._command_from_current_subgait()), )
def test_update_subgait_done(self): self.gait.start(Time(seconds=0)) gait_update = self.gait.update(Time(seconds=1.8)) self.assertEqual(self.gait.subgait_name, "left_swing") self.assertEqual(self.gait._start_time, Time(seconds=1.8)) self.assertEqual(self.gait._end_time, Time(seconds=2.9)) self.assertFalse(self.gait._scheduled_early) self.assertEqual( gait_update, GaitUpdate.should_schedule( self.gait._command_from_current_subgait()), )
def read_data_frames(bag_view: BagView, field_dict: Dict[str, List[str]], auto_stamp=True): data = {} field_types = {} topics = dict(bag_view.topics()) for (topic, fields) in field_dict.items(): data[topic] = {} field_types[topic] = {} msg_type = topics[topic] fields_and_types = msg_type.get_fields_and_field_types() if ('header.stamp' not in fields and auto_stamp and 'header' in fields_and_types and fields_and_types['header'] == 'std_msgs/Header'): fields = fields.copy() # ensure not to mutate the input fields.append('header.stamp') for field in fields: field_types[topic][field] = _field_type(msg_type, field) data[topic][field] = [] for (topic, msg, _) in bag_view: for field, field_type in field_types[topic].items(): value = _rgetattr(msg, field) if field_type == 'builtin_interfaces/Time': ns = Time.from_msg(value).nanoseconds value = pd.to_datetime(ns, origin='unix', unit='ns') data[topic][field].append(value) return { topic: pd.DataFrame(data_dict) for topic, data_dict in data.items() }
def _odom_callback(self, msg): try: # Get latest transform tf = self._tf_buffer.lookup_transform( "map", "base_link", Time(), timeout=Duration(seconds=1.0)) self._odom_pose_stamped.header = msg.header self._odom_pose_stamped.pose = msg.pose.pose tf_pose = tf2_geometry_msgs.do_transform_pose( self._odom_pose_stamped, tf) except LookupException: self.get_logger().warn( "Failed to lookup_transform from map to odom") return except ConnectivityException: self.get_logger().warn( "ConnectivityException, 'map' and 'base_link' are not part of the same tree" ) return self._position = (tf_pose.pose.position.x, tf_pose.pose.position.y) self._orientation = self.quaternion_to_euler( tf_pose.pose.orientation.x, tf_pose.pose.orientation.y, tf_pose.pose.orientation.z, tf_pose.pose.orientation.w, )
def callback_delay(self, msg): """ Calculate delay time. :param msg: Message instance """ if not hasattr(msg, 'header'): raise RuntimeError('msg does not have header') with self.lock: curr_rostime = self._clock.now() # time reset if curr_rostime.nanoseconds == 0: if len(self.delays) > 0: print('time has reset, resetting counters') self.delays = [] return curr = curr_rostime.nanoseconds if self.msg_t0 < 0 or self.msg_t0 > curr: self.msg_t0 = curr self.msg_tn = curr self.delays = [] else: # store the duration nanoseconds in self.delays duration = (curr_rostime - Time.from_msg(msg.header.stamp)) self.delays.append(duration.nanoseconds) self.msg_tn = curr if len(self.delays) > self.window_size: self.delays.pop(0)
def _task(self): if len(self._queue) > 0: # print("*"*5, len(self._queue)) msg = self._queue.pop(0) # self._count_queue.append(msg) self.count += 1 self.count_rate += 1 time_diff_ms = ( self._stream_output_port.parent.get_clock().now().nanoseconds - Time.from_msg(msg.header.stamp).nanoseconds) / 1000000 if msg.freshness > 0 and msg.freshness < time_diff_ms: self.count_fresh += 1 # raise self.exception('{}ms exceeded(constraint: {}ms, cur: {}ms'.format(time_diff_ms - msg.freshness, msg.freshness, time_diff_ms)) self._task() return tlqkf1 = time.time() self.count_publish += 1 # print("rate publish: ", tlqkf1 - self.tlqkf1_prev) self._publisher.publish(msg) self.tlqkf1_prev = tlqkf1 elif self._last_sent_item: self.count += 1 msg = self._last_sent_item else: return self._last_sent_item = msg
def add(self, msg): if not hasattr(msg, 'header') or not hasattr(msg.header, 'stamp'): if not self.allow_headerless: msg_filters_logger = rclpy.logging.get_logger( 'message_filters_cache') msg_filters_logger.set_level(LoggingSeverity.INFO) msg_filters_logger.warn("can not use message filters messages " "without timestamp infomation when " "'allow_headerless' is disabled. " "auto assign ROSTIME to headerless " "messages once enabling constructor " "option of 'allow_headerless'.") return stamp = ROSClock().now() else: stamp = msg.header.stamp if not hasattr(stamp, 'nanoseconds'): stamp = Time.from_msg(stamp) # Insert sorted self.cache_times.append(stamp) self.cache_msgs.append(msg) # Implement a ring buffer, discard older if oversized if (len(self.cache_msgs) > self.cache_size): del self.cache_msgs[0] del self.cache_times[0] # Signal new input self.signalMessage(msg)
def update_state(self, msg: AsebaEvent) -> None: if self.state == CalibrationState.waiting_button: return if self.state == CalibrationState.waiting_ground: self.ths = [d - self.gap for d in msg.data] self.state = CalibrationState.running return if self.motor_speed == 0: return line = self.line if self.motor == 'right': v = msg.data[0] th = self.ths[0] else: v = msg.data[1] th = self.ths[1] self.get_logger().info(f'update_state {line.state} {v} {th} -> ?') stamp = Time.from_msg(msg.stamp).nanoseconds * 1e-9 if v > th and line.state == LineState.UNKNOWN: line.state = LineState.OFF if v < th and line.state == LineState.OFF: line.state = LineState.IN line.in_at_time = stamp if v > th and line.state == LineState.IN: line.state = LineState.OFF pick = (stamp + line.in_at_time) / 2 self.get_logger().info(f"Line after {pick} s") self.add_pick(pick)
def test_duration_comparators(self): duration1 = Duration(nanoseconds=1) duration2 = Duration(nanoseconds=2) self.assertFalse(duration1 == duration2) self.assertTrue(duration1 != duration2) self.assertFalse(duration1 > duration2) self.assertFalse(duration1 >= duration2) self.assertTrue(duration1 < duration2) self.assertTrue(duration1 <= duration2) duration1 = Duration(nanoseconds=5e9) duration2 = Duration(seconds=5) self.assertTrue(duration1 == duration2) # Invalid combinations duration1 = Duration(nanoseconds=1) with self.assertRaises(TypeError): duration1 == 1 time = Time() with self.assertRaises(TypeError): duration1 == time with self.assertRaises(TypeError): duration1 != time with self.assertRaises(TypeError): duration1 > time with self.assertRaises(TypeError): duration1 >= time with self.assertRaises(TypeError): duration1 < time with self.assertRaises(TypeError): duration1 <= time
def lookup_transform( self, target_frame: str, source_frame: str, time: Time, timeout: Duration = Duration() ) -> TransformStamped: """ Get the transform from the source frame to the target frame. :param target_frame: Name of the frame to transform into. :param source_frame: Name of the input frame. :param time: The time at which to get the transform (0 will get the latest). :param timeout: Time to wait for the target frame to become available. :return: The transform between the frames. """ if isinstance(time, builtin_interfaces.msg.Time): source_time = Time.from_msg(time) warnings.warn( 'Passing a builtin_interfaces.msg.Time argument is deprecated, and will be removed in the near future. ' 'Use rclpy.time.Time instead.') elif isinstance(time, Time): source_time = time else: raise TypeError('Must pass a rclpy.time.Time object.') goal = LookupTransform.Goal() goal.target_frame = target_frame goal.source_frame = source_frame goal.source_time = source_time.to_msg() goal.timeout = timeout.to_msg() goal.advanced = False return self.__process_goal(goal)
def now(self): from rclpy.time import Time time_handle = _rclpy.rclpy_clock_get_now(self._clock_handle) # TODO(dhood): Return a python object from the C extension return Time( nanoseconds=_rclpy.rclpy_time_point_get_nanoseconds(time_handle), clock_type=self.clock_type)