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軸はモータ２（左指近位関節）を動かす
# #曲げる方向を正とした
# 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軸はモータ１（左指遠位関節）を動かす
# 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()
