72 lines
1.7 KiB
Python
72 lines
1.7 KiB
Python
from datetime import datetime
|
|
from geometry_msgs.msg import PoseStamped
|
|
import pandas as pd
|
|
import rospy
|
|
import time
|
|
|
|
import active_grasp.msg
|
|
from robot_utils.ros.conversions import *
|
|
|
|
|
|
class CartesianPoseControllerClient:
|
|
def __init__(self, topic="/command"):
|
|
self.target_pub = rospy.Publisher(topic, PoseStamped, queue_size=10)
|
|
|
|
def send_target(self, pose):
|
|
msg = to_pose_stamped_msg(pose, "panda_link0")
|
|
self.target_pub.publish(msg)
|
|
|
|
|
|
class AABBox:
|
|
def __init__(self, bbox_min, bbox_max):
|
|
self.min = bbox_min
|
|
self.max = bbox_max
|
|
|
|
def is_inside(self, p):
|
|
return np.all(p > self.min) and np.all(p < self.max)
|
|
|
|
|
|
def from_bbox_msg(msg):
|
|
aabb_min = from_point_msg(msg.min)
|
|
aabb_max = from_point_msg(msg.max)
|
|
return AABBox(aabb_min, aabb_max)
|
|
|
|
|
|
def to_bbox_msg(bbox):
|
|
msg = active_grasp.msg.AABBox()
|
|
msg.min = to_point_msg(bbox.min)
|
|
msg.max = to_point_msg(bbox.max)
|
|
return msg
|
|
|
|
|
|
class Timer:
|
|
timers = dict()
|
|
|
|
def __init__(self, name):
|
|
self.name = name
|
|
|
|
def __enter__(self):
|
|
self.start()
|
|
return self
|
|
|
|
def __exit__(self, *exc_info):
|
|
self.stop()
|
|
|
|
def start(self):
|
|
self.tic = time.perf_counter()
|
|
|
|
def stop(self):
|
|
elapsed_time = time.perf_counter() - self.tic
|
|
self.timers[self.name] = elapsed_time
|
|
|
|
|
|
class Logger:
|
|
def __init__(self, logdir, policy, desc):
|
|
stamp = datetime.now().strftime("%y%m%d-%H%M%S")
|
|
name = "{}_policy={},{}".format(stamp, policy, desc).strip(",")
|
|
self.path = logdir / (name + ".csv")
|
|
|
|
def log_run(self, info):
|
|
df = pd.DataFrame.from_records([info])
|
|
df.to_csv(self.path, mode="a", header=not self.path.exists(), index=False)
|