def start_monitor(): threads = [] # 初始化监测ros节点 rospy.init_node("topic_monitor_node", log_level=rospy.DEBUG) # 报警配置 config_parser = ConfigParser.ConfigParser() config_parser.read(config.CONFIG_INI_PATH) # 协程运行监控camera、ublox等的状态 others_topic = dict(config_parser.items("topic_others")) for topic_key, topic in others_topic.items(): topic_object = rostopic.ROSTopicHz(-1) rospy.Subscriber(topic, rospy.AnyMsg, topic_object.callback_hz) threads.append(threading.Thread(target=topic_monitor_threads, args=(topic_key, topic_object, topic,))) # 协程运行监控car_info各个传感器的状态 car_topic = dict(config_parser.items("topic_radar")) for topic_key, topic in car_topic.items(): topic_object = rostopic.ROSTopicHz(-1) rospy.Subscriber(topic, rospy.AnyMsg, topic_object.callback_hz) threads.append(threading.Thread(target=radar_monitor_threads, args=(topic_key, topic_object, topic,))) # 协程运行监控car_info各个传感器的状态 radar_topic = dict(config_parser.items("topic_car_info")) for topic_key, topic in radar_topic.items(): topic_object = rostopic.ROSTopicHz(-1) rospy.Subscriber(topic, rospy.AnyMsg, topic_object.callback_hz) threads.append(threading.Thread(target=car_info_monitor_threads, args=(topic_key, topic_object, topic,))) # 创建监控nvme磁盘的线程 threads.append(threading.Thread(target=nvme_monitor_threads)) # 创建监控cpu的线程 threads.append(threading.Thread(target=cpu_monitor_threads)) # 创建监控内存的线程 threads.append(threading.Thread(target=memory_monitor_threads)) # 创建监控disk的线程 threads.append(threading.Thread(target=disk_monitor_threads)) # 开启线程任务 for t in threads: t.setDaemon(True) t.start() for t in threads: t.join()
def run(self): # wait for first message while len(self.missing_topics) != 0 and not rospy.is_shutdown(): for topic in self.topics: msg_class, real_topic, msg_eval = rostopic.get_topic_class( topic, blocking=False) #pause hz until topic is published if real_topic: if real_topic in self.missing_topics: self.missing_topics.remove(real_topic) try: rospy.logdebug( "hz monitor is waiting for type of topics {}.".format( self.missing_topics)) rospy.sleep(1.0) except rospy.exceptions.ROSInterruptException: pass # call rostopic hz self.rt_HZ_store = dict() for topic in self.topics: rt = rostopic.ROSTopicHz(self.window_size) rospy.Subscriber(topic, rospy.AnyMsg, rt.callback_hz) self.rt_HZ_store[topic] = rt rospy.loginfo("subscribed to [{}]".format(topic))
def __init__(self, manager_name, author_name): self._skill_mgr_name = manager_name self._author = author_name self._active_tasks = set() self._module_list = dict() self._skill_list = dict() self._msg_lock = Lock() self._msg_rec = Event() rospy.wait_for_service(self._skill_mgr_name + '/get_skills') self._skill_exe_client = rospy.ServiceProxy( self._skill_mgr_name + '/command', srvs.SkillCommand) self._get_skills = rospy.ServiceProxy( self._skill_mgr_name + '/get_skills', srvs.ResourceGetDescriptions) self._monitor_sub = rospy.Subscriber(self._skill_mgr_name + '/monitor', msgs.SkillProgress, self._progress_cb) self._tick_rate = rostopic.ROSTopicHz(50) self._tick_rate_sub = rospy.Subscriber( self._skill_mgr_name + '/tick_rate', Empty, self._tick_rate.callback_hz) self._set_debug = rospy.Publisher(self._skill_mgr_name + "/set_debug", Bool, queue_size=1, latch=True) self._monitor_cb = None self.get_skill_list(True)
def __init__(self, topic, msg_type, name=None): super(TopicVisualize, self).__init__() # Attributes self.topic = topic self.msg_type = msg_type if name is None: name = topic self.max_delay = 0.0 self.total_delay = 0.0 self.total_msg_count = 0 self.createLayout(name) # Subscribers self.rate = rostopic.ROSTopicHz(250) rospy.Subscriber(self.topic, self.msg_type, self.rate.callback_hz, callback_args=self.topic) rospy.Subscriber(self.topic, self.msg_type, self.callback) # Signal Connections self.publish_rate_signal.connect(self.showMessageRate) self.delay_signal.connect(self.showMessageDelay) self.max_delay_signal.connect(self.showMaxDelay) self.average_delay_signal.connect(self.showAverageDelay) # self.value_out_of_range_signal.connect(self.showValueRange) # Start Class timer self.classTimer = self.ClassTimer(self.timerCallback) self.classTimer.start()
def registerLidar(self): self.rl = rostopic.ROSTopicHz(window_size=20) self.l0 = rospy.Subscriber( "/left_os1/os1_cloud_node/points", senmsg.PointCloud2, self.rl.callback_hz, callback_args="/left_os1/os1_cloud_node/points") self.l1 = rospy.Subscriber( "/right_os1/os1_cloud_node/points", senmsg.PointCloud2, self.rl.callback_hz, callback_args="/right_os1/os1_cloud_node/points") self.l2 = rospy.Subscriber("/cloud", senmsg.PointCloud2, self.rl.callback_hz, callback_args="/cloud") self.l3 = rospy.Subscriber( "/velodyne_left/velodyne_points", senmsg.PointCloud2, self.rl.callback_hz, callback_args="/velodyne_left/velodyne_points") self.l4 = rospy.Subscriber( "/velodyne_right/velodyne_points", senmsg.PointCloud2, self.rl.callback_hz, callback_args="/velodyne_right/velodyne_points")
def __init__(self, topics, max_publish_rate=1): # this will listen to publishing rate self.topic_hz = dict() # subscriber per topic self.subscribers = dict() for topic_name in topics: # poll self.topic_hz[topic_name] = rostopic.ROSTopicHz(-1) # subscribe self.subscribers[topic_name] = rospy.Subscriber( topic_name, rospy.AnyMsg, self.topic_hz[topic_name].callback_hz, callback_args=topic_name, queue_size=1) # a publishing rate higher than this will cause an error report self.min_publish_rate = max_publish_rate # publisher for bit self.bit_pub = rospy.Publisher('/wizzy/bit_state', std_msgs.msg.String, queue_size=10)
def __init__(self, layout, footIMU_topic, odometry_topic, odometry_service, fusion_service): super(Human_Diagnostic, self).__init__() # Attributes self.footIMU_topic = footIMU_topic self.odometry_topic = odometry_topic self.odometry_started = False self.fusion_started = False self.layout = layout self.createLayout(self.layout) # Subscribers self.footIMU_rate = rostopic.ROSTopicHz(1000) self.odometry_rate = rostopic.ROSTopicHz(1000) rospy.Subscriber(self.footIMU_topic, Imu, self.footIMU_rate.callback_hz, callback_args=self.footIMU_topic) rospy.Subscriber(self.odometry_topic, Odometry, self.odometry_rate.callback_hz, callback_args=self.odometry_topic) # Services self.odometry_service = None try: rospy.wait_for_service(odometry_service, timeout=3) self.odometry_service = rospy.ServiceProxy(odometry_service, SetBool) except Exception as e: self.start_odometry_button.setStyleSheet("QPushButton { background: red }") self.start_odometry_button.setText("Odometry: Error") rospy.logwarn ("Human Diagnostics : Exception:" + str(e)) self.fusion_service = None try: rospy.wait_for_service(fusion_service, timeout=3) self.fusion_service = rospy.ServiceProxy(fusion_service, SetBool) except Exception as e: self.start_fusion_button.setStyleSheet("QPushButton { background: red }") self.start_fusion_button.setText("Fusion: Error") rospy.logwarn ("Human Diagnostics : Exception:" + str(e)) # Signal Connections self.footIMU_rate_signal.connect(self.showFootImuMessageRate) self.Odometry_rate_signal.connect(self.showOdometryMessageRate) self.start_odometry_button.clicked.connect(self.start_odometry) self.start_fusion_button.clicked.connect(self.start_fusion) # Start Class timer self.classTimer = self.ClassTimer(self.timerCallback) self.classTimer.start()
def registerCamera(self): self.rc = rostopic.ROSTopicHz(window_size=20) self.c0 = rospy.Subscriber( "/zed_node/stereo/image_rect_color", senmsg.Image, self.rc.callback_hz, callback_args="/zed_node/stereo/image_rect_color") self.c1 = rospy.Subscriber("/zed_node/stereo/image_rect_color", senmsg.Image, self.zedCallBack)
def main(self): rospy.init_node('rviz_visualizer_node') self.r = rostopic.ROSTopicHz(-1) rospy.Subscriber('/reactive_cones', numpy_msg(Floats), self.r.callback_hz, callback_args='/global_map_markers') rospy.Subscriber('/reactive_cones', numpy_msg(Floats), self.callback_reactive) rospy.Subscriber('/global_map_markers', numpy_msg(Floats), self.callback_global) rospy.Subscriber('/odometry/filtered', Odometry, self.callback_odom) rospy.Timer(rospy.Duration(0.1), self.publish_FOV) rospy.Timer(rospy.Duration(0.1), self.lifetime_global) rospy.spin()
def hzFunk(self): a = rostopic.ROSTopicHz(-1) b = rostopic.ROSTopicHz(-1) c = rostopic.ROSTopicHz(-1) rospy.Subscriber("/bebop/cmd_vel", Twist, a.callback_hz) rospy.Subscriber("/bebop/odom", Odometry, b.callback_hz) rospy.Subscriber("/bebop/image_raw", Image, c.callback_hz) time.sleep(1.0) list = [] try: list.append(a.get_hz()[0]) except: list.append("No message") try: list.append(b.get_hz()[0]) except: list.append("No message") try: list.append(c.get_hz()[0]) except: list.append("No message") self.checkHz.emit(list)
def getting_rates(): r = rostopic.ROSTopicHz(-1) s = rospy.Subscriber('/Lidar_Prediction', Float32, r.callback_hz, callback_args='/Lidar_Prediction') l = rospy.Subscriber('/Camera_Prediction', Float32, r.callback_hz, callback_args='/Camera_Prediction') rospy.sleep(1) r.print_hz(['/Lidar_Prediction']) r.print_hz(['/Camera_Prediction'])
def start(self): self.set_physics = rospy.ServiceProxy(self.param.topics.set_physics, SetPhysicsProperties) self.get_physics = rospy.ServiceProxy(self.param.topics.get_physics, GetPhysicsProperties) self.rater = rostopic.ROSTopicHz(10) self.subscriber = rospy.Subscriber( self.param.topics.target, rospy.AnyMsg, self.rater.callback_hz, callback_args=self.param.topics.target, ) super().start()
def get_rostopic_hz(topic, window_size=-1, filter_expr=None, use_wtime=False, avg=2): """ Periodically print the publishing rate of a topic to console until shutdown :param topic: topic names, ``list`` of ``str`` :param window_size: number of messages to average over, -1 for infinite, ``int`` :param filter_expr: Python filter expression that is called with m, the message instance """ rostopic._check_master() if rospy.is_shutdown(): return rt = rostopic.ROSTopicHz(window_size, filter_expr=filter_expr, use_wtime=use_wtime) msg_class, real_topic, _ = rostopic.get_topic_class(topic, blocking=False) if real_topic is None: return 0.0 # pause hz until topic is published # we use a large buffer size as we don't know what sort of messages we're dealing with. # may parameterize this in the future if filter_expr is not None: # have to subscribe with topic_type rospy.Subscriber(real_topic, msg_class, rt.callback_hz, callback_args=topic) else: rospy.Subscriber(real_topic, rospy.AnyMsg, rt.callback_hz, callback_args=topic) # if rospy.get_param('use_sim_time', False): # print("WARNING: may be using simulated time",file=sys.stderr) rostopic._sleep(1.0) hz_avg = 0.0 for i in range(0, avg): hz_cur = rt.get_hz(topic) if hz_cur: hz_avg = hz_avg + hz_cur[0] rostopic._sleep(0.5) return hz_avg / avg
def runTest(self): if not rospy.is_shutdown(): msg_class, real_topic, _ = rostopic.get_topic_class( self.topic_name) self.assertIsNotNone(real_topic) if real_topic: rt = rostopic.ROSTopicHz(-1, filter_expr=None, use_wtime=False) sub = rospy.Subscriber(real_topic, msg_class, rt.callback_hz) if not rospy.is_shutdown(): rospy.sleep(1.0) hz = self.getHz(rt) self.assertNotEqual(max(0.0, hz - self.min_freq), 0.0)
def start(self): rospy.wait_for_service(self.param.topics.get_physics, timeout=5) self.set_physics = rospy.ServiceProxy(self.param.topics.set_physics, SetPhysicsProperties, persistent=True) self.get_physics = rospy.ServiceProxy(self.param.topics.get_physics, GetPhysicsProperties, persistent=True) if self.param.use_sync: self.published_subscriber = rospy.Subscriber( self.param.sync.source_topic, rospy.AnyMsg, callback=self.receive_sync_source, queue_size=1, ) self.pause_physics_proxy = rospy.ServiceProxy( self.param.topics.pause_gazebo, EmptySrv) self.unpause_physics_proxy = rospy.ServiceProxy( self.param.topics.unpause_gazebo, EmptySrv) self.rater = rostopic.ROSTopicHz(5) # Start in very slow mode to ensure that everything is started before speeding up self._update_properties(update_rate=self.param.update_rate.min) self.subscribers = {} for target in self.param.targets: topic = target["topic"] self.subscribers[topic] = rospy.Subscriber( topic, rospy.AnyMsg, self.rater.callback_hz, callback_args=topic, ) # Wait for atleast one message on every target topic # Continue of none is received. The topic will then be disregarded (for now). with contextlib.suppress(rospy.ROSException): rospy.wait_for_message(topic, rospy.AnyMsg, timeout=0.1) super().start()
def registerPose(self): self.rp = rostopic.ROSTopicHz(window_size=20) self.p0 = rospy.Subscriber("/gps/nova/current_pose", geomsg.PoseStamped, self.rp.callback_hz, callback_args="/gps/nova/current_pose") self.p1 = rospy.Subscriber("/gps/duro/current_pose", geomsg.PoseStamped, self.rp.callback_hz, callback_args="/gps/duro/current_pose") self.p2 = rospy.Subscriber("/gps/duro/current_pose", geomsg.PoseStamped, self.duroPoseCallBack) self.p3 = rospy.Subscriber("/gps/nova/current_pose", geomsg.PoseStamped, self.novaPoseCallBack) self.p4 = rospy.Subscriber("/gps/duro/status_string", rosmsg.String, self.duroRtkStatusCallBack) self.p5 = rospy.Subscriber("/gps/nova/bestvel", novamsg.NovatelVelocity, self.novaRtkStatusCallback) self.p6 = rospy.Subscriber("/vehicle_status", autowmsgs.VehicleStatus, self.vehicleStatusCallback)
def run(self): r = rospy.Rate(self.sampling_rate) # wait for first message while not rospy.is_shutdown(): for topic in self.topics: msg_class, real_topic, _ = rostopic.get_topic_class( topic, blocking=False) #pause hz until topic is published if real_topic: if real_topic in self.missing_topics: self.missing_topics.remove(real_topic) if len(self.missing_topics) == 0: break rospy.loginfo("hz monitor is waiting for type of topics %s." % str(self.missing_topics)) self.publish_diagnostics() try: r.sleep() except rospy.exceptions.ROSInterruptException as e: pass # call rostopic hz #rt = rostopic.ROSTopicHz(self.window_size) rt_HZ_store = [] for topic in self.topics: rt = rostopic.ROSTopicHz(self.window_size) rospy.Subscriber(topic, rospy.AnyMsg, rt.callback_hz) rt_HZ_store.append(rt) rospy.loginfo("subscribed to [%s]" % topic) # publish diagnostics continuously while not rospy.is_shutdown(): #rt.print_hz() # taken from 'rostopic hz' (/opt/ros/indigo/lib/python2.7/dist-packages/rostopic/__init__.py) self.publish_diagnostics(rt_HZ_store) try: r.sleep() except rospy.exceptions.ROSInterruptException as e: pass
def clear(self): self.rate_label.setText('0') self.delay_label.setText('0') self.max_delay_label.setText('0') self.average_delay_label.setText('0') # self.value_range_label.setText('OK') self.max_delay = 0.0 self.total_delay = 0.0 self.total_msg_count = 0 # Create new message rate counter self.rate = rostopic.ROSTopicHz(250) rospy.Subscriber(self.topic, self.msg_type, self.rate.callback_hz, callback_args=self.topic) self.rate_label.setStyleSheet("") self.delay_label.setStyleSheet("") self.max_delay_label.setStyleSheet("") self.average_delay_label.setStyleSheet("")
def __init__(self, widget, topic, uav_name): super(UAV_Diagnostic, self).__init__() # Attributes self.topic = topic self.widget = widget self.max_delay = 0.0 self.average_delay = 0.0 self.delay_std_deviation = 0.0 self.total_delay = 0.0 self.total_squared_delay = 0.0 self.total_msg_count = 0 self.record = False self.auto_follow = False self.uav_name = uav_name # Retrieve GUI Components self.publish_rate_label = self.widget.findChild( qt.QtWidgets.QLabel, uav_name + '_publish_rate_label') self.delay_label = self.widget.findChild(qt.QtWidgets.QLabel, uav_name + '_delay_label') self.max_delay_label = self.widget.findChild( qt.QtWidgets.QLabel, uav_name + '_max_delay_label') self.average_delay_label = self.widget.findChild( qt.QtWidgets.QLabel, uav_name + '_average_delay_label') self.delay_std_deviation_label = self.widget.findChild( qt.QtWidgets.QLabel, uav_name + '_delay_std_deviation_label') self.port_status_label = self.widget.findChild( qt.QtWidgets.QLabel, uav_name + '_port_status_label') self.ping_status_label = self.widget.findChild( qt.QtWidgets.QLabel, uav_name + '_ping_status_label') self.value_range_label = self.widget.findChild( qt.QtWidgets.QLabel, uav_name + '_value_range_label') self.record_button = self.widget.findChild( qt.QtWidgets.QPushButton, uav_name + '_record_pushButton') self.autonomous_button = self.widget.findChild( qt.QtWidgets.QPushButton, uav_name + '_autonomous_pushButton') # Subscribers self.rate = rostopic.ROSTopicHz(-1) rospy.Subscriber(topic, UUBmsg, self.rate.callback_hz, callback_args=topic) rospy.Subscriber(topic, UUBmsg, self.UUBCallback) # TODO Subscribe to Diagnostic Topic #### Services #### # Bag Recorder Service service_name = uav_name + '_bag_recorder/record_bag' self.record_service = None try: rospy.wait_for_service(service_name, timeout=3) self.record_service = rospy.ServiceProxy(service_name, SetBool) except Exception as e: self.record_button.setStyleSheet("QPushButton { background: red }") self.record_button.setText("Error") rospy.logwarn("UAV Diagnostics :" + self.uav_name + " Exception:" + str(e)) # Autonomous Mode Service autonomous_mode_service_name = uav_name + '/follow_me/enable' self.autonomous_mode_service = None try: rospy.wait_for_service(autonomous_mode_service_name, timeout=3) self.autonomous_mode_service = rospy.ServiceProxy( autonomous_mode_service_name, SetBool) except Exception as e: self.autonomous_button.setStyleSheet( "QPushButton { background: red }") self.autonomous_button.setText("Error") rospy.logwarn("UAV Diagnostics :" + self.uav_name + " Exception:" + str(e)) # Signal Connections self.publish_rate_signal.connect(self.showMessageRate) self.delay_signal.connect(self.showMessageDelay) self.max_delay_signal.connect(self.showMaxDelay) self.average_delay_signal.connect(self.showAverageDelay) self.delay_std_deviation_signal.connect(self.showDelayStdDeviation) self.value_out_of_range_signal.connect(self.showValueRange) self.record_button.clicked.connect(self.recordBag) self.autonomous_button.clicked.connect(self.autonomousFollow) # Start Class timer self.classTimer = self.ClassTimer(self.timerCallback) self.classTimer.start()
def __init__(self, widget, footIMU_topic, waistIMU_topic, odometry_topic): super(Human_Diagnostic, self).__init__() # Attributes self.footIMU_topic = footIMU_topic self.waistIMU_topic = waistIMU_topic self.odometry_topic = odometry_topic self.record = False self.widget = widget # Retrieve GUI Components self.footIMU_label = self.widget.findChild(qt.QtWidgets.QLabel, 'human_footIMU_rate_label') self.waistIMU_label = self.widget.findChild( qt.QtWidgets.QLabel, 'human_waistIMU_rate_label') self.odometry_label = self.widget.findChild( qt.QtWidgets.QLabel, 'human_odometry_rate_label') self.cpu_usage_label = self.widget.findChild(qt.QtWidgets.QLabel, 'human_CPU_usage_label') self.ram_usage_label = self.widget.findChild(qt.QtWidgets.QLabel, 'human_RAM_usage_label') self.record_button = self.widget.findChild(qt.QtWidgets.QPushButton, 'human_record_pushButton') # Subscribers self.footIMU_rate = rostopic.ROSTopicHz(-1) self.waistIMU_rate = rostopic.ROSTopicHz(-1) self.odometry_rate = rostopic.ROSTopicHz(-1) rospy.Subscriber(self.footIMU_topic, Imu, self.footIMU_rate.callback_hz, callback_args=self.footIMU_topic) rospy.Subscriber(self.waistIMU_topic, Imu, self.waistIMU_rate.callback_hz, callback_args=self.waistIMU_topic) rospy.Subscriber(self.odometry_topic, Odometry, self.odometry_rate.callback_hz, callback_args=self.odometry_topic) # Services service_name = 'human_bag_recorder/record_bag' self.record_service = None try: rospy.wait_for_service(service_name, timeout=3) self.record_service = rospy.ServiceProxy(service_name, SetBool) except Exception as e: self.record_button.setStyleSheet("QPushButton { background: red }") self.record_button.setText("Error") rospy.logwarn("Human Diagnostics : Exception:" + str(e)) # Signal Connections self.footIMU_rate_signal.connect(self.showFootImuMessageRate) self.waistIMU_rate_signal.connect(self.showWaistImuMessageRate) self.Odometry_rate_signal.connect(self.showOdometryMessageRate) self.cpu_usage_signal.connect(self.showCpuUsage) self.ram_usage_signal.connect(self.showRamUsage) self.record_button.clicked.connect(self.recordBag) # Start Class timer self.classTimer = self.ClassTimer(self.timerCallback) self.classTimer.start()
def Loop(self): sign = lambda x: 1 if x > 0 else -1 if x < 0 else 0 #擦り動作用アレイ thick_array = self.Gen_thick_array() rate = rospy.Rate(self.ctrl_rate) # topicの配信頻度を取得用 # dynamixelの制御Hzをとるのじゃ! # rate, min_delta, max_delta, standard deviation, window number = self.topic_hz.get_hz([/dynamixel_data]) self.topic_hz = rostopic.ROSTopicHz(600) s = rospy.Subscriber('/dynamixel_data', dynamixel_msg, self.topic_hz.callback_hz, callback_args='/dynamixel_data') #Virtual offset: self.trg_offset = 0.0 while self.is_running: start = time.time() #モータの情報取得 # pos = [dxl[id].Position() for id in range(len(dxl))] pos, vel, pwm, cur = self.observer() time_get = time.time() - start for i in range(4): self.trg_pos[i] = pos[i] #取得した情報をパブリッシュ dy_data = dynamixel_msg() dy_data.header.stamp = rospy.Time.now() dy_data.Pos = pos dy_data.Vel = vel dy_data.Pwm = pwm dy_data.Cur = cur data_pub.publish(dy_data) # #スティックX軸はモータ2(左指近位関節)を動かす # #曲げる方向を正とした # if self.DIRECTIONS[0] == 1: # self.trg_pos[1] = dxl[1].Position()-self.p # elif self.DIRECTIONS[0] == -1: # self.trg_pos[1] = dxl[1].Position()+self.p # #スティックY軸はモータ1(左指遠位関節)を動かす # if self.DIRECTIONS[1] == 1: # self.trg_pos[0] = dxl[0].Position()-self.p # elif self.DIRECTIONS[1] == -1: # self.trg_pos[0] = dxl[0].Position()+self.p #スティックX軸は擦り動作 if self.DIRECTIONS[0] == 1: rubbing.surface_pos = rubbing.surface_pos + self.v * 0.01 elif self.DIRECTIONS[0] == -1: rubbing.surface_pos = rubbing.surface_pos - self.v * 0.01 #スティックY軸は指缶距離調整 if self.DIRECTIONS[1] == 1: rubbing.Set_interval(rubbing.interval - self.p * 0.1) elif self.DIRECTIONS[1] == -1: rubbing.Set_interval(rubbing.interval + self.p * 0.1) # #ボタン左右は仮想面の傾き # if self.DIRECTIONS[3] == 1: # rubbing.degree_of_surface += self.p*0.1 # elif self.DIRECTIONS[3] == -1: # rubbing.degree_of_surface -= self.p*0.1 # #ボタン上下は指の傾き # if self.DIRECTIONS[2] == 1: # rubbing.degree_of_finger += self.p*0.1 # elif self.DIRECTIONS[2] == -1: # rubbing.degree_of_finger -= self.p*0.1 rubbing.control_f = any(self.FLAG_MOVES[:2]) # if self.DIRECTIONS[4] == 1: # self.trg_vel[0] = self.v # self.trg_vel[2] = self.v # elif self.DIRECTIONS[4] == -1: # self.trg_vel[0] = -self.v # self.trg_vel[2] = -self.v #自動擦り動作用 if self.thick_f: if self.thick_i < len(thick_array): auto_surface_pos = thick_array[self.thick_i] self.thick_i += 1 rubbing.surface_pos = auto_surface_pos else: self.thick_i = 0 self.thick_f = 0 #目標位置の計算 if rubbing.running: rubbing.Update() a, b = rubbing.calculation_degree() pos_r, pos_l = rubbing.deg2pos(a, b) pos_r_dist, pos_l_dist = rubbing.calculation_pos_dist() self.trg_pos[2], self.trg_pos[0] = int(pos_r), int(pos_l) self.trg_pos[3], self.trg_pos[1] = int(pos_r_dist), int( pos_l_dist) # 指先を平行に保つの開始 if self.offset_f: rubbing.surface_pos = 0 rubbing.degree_of_surface = 0 rubbing.running = 1 if self.test_button_f: if self.first_f: rubbing.Pulse_deformed(0.01, 1.5, 5, 10, 0.3) self.first_f = False self.test_button_f = False # キャリブレーション if self.calibration_f: # self.Calibration_initial_position() self.Manual_Calibration_initial_position() self.calibration_f = 0 # 目標位置をdynamixelへ送信 # print(self.trg_pos) self.controller(self.trg_pos) rate.sleep() update_fps = 1 / (time.time() - start) #各種パラメータをパブリッシュ #------------------------------ dy_param = dynamixel_param_msg() dy_param.header.stamp = rospy.Time.now() dy_param.surface_pos = rubbing.surface_pos dy_param.interval = rubbing.interval dy_param.fps = update_fps dy_param.trg_pos = self.trg_pos dy_param.degree_of_finger = rubbing.degree_of_finger # print(dy_param) param_pub.publish(dy_param) #------------------------------- time_all = time.time() - start
if (i == 0): rospy.INFO( "Firstly have to run pozyx_simulation package uwb_simulation.py file" ) break return sensor_pos if __name__ == "__main__": global odom_hz odom_topic_name = "odom" #get uwb anchors postion sensor_pos = get_anchors_pos() #get odom hz r = rostopic.ROSTopicHz(-1) s = rospy.Subscriber('/' + odom_topic_name, Odometry, r.callback_hz, callback_args='/odom') rospy.sleep(1) odom_hz = int(r.get_hz('/' + odom_topic_name)[0]) s.unregister() rospy.Subscriber("odom", Odometry, subscribe_odom_data) rospy.Subscriber("uwb_data_topic", uwb_data, subscribe_uwb_data) rospy.spin()