def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--finger-type",
choices=finger_types_data.get_valid_finger_types(),
required=True,
help="""Pass a valid finger type.""",
)
parser.add_argument(
"--real-time-mode",
"-r",
action="store_true",
help="""Run simulation in real time. If not set, the simulation runs
as fast as possible.
""",
)
parser.add_argument(
"--first-action-timeout",
"-t",
type=float,
default=math.inf,
help="""Timeout (in seconds) for reception of first action after
starting the backend. If not set, the timeout is disabled.
""",
)
parser.add_argument(
"--max-number-of-actions",
"-a",
type=int,
default=0,
help="""Maximum numbers of actions that are processed. After this the
backend shuts down automatically.
""",
)
parser.add_argument(
"--robot-logfile",
"-l",
type=str,
help="""Path to a file to which the robot data log is written. If not
specified, no log is generated.
""",
)
parser.add_argument(
"--camera-logfile",
type=str,
help="""Path to a file to which the camera data log is written. If not
specified, no log is generated.
""",
)
parser.add_argument(
"--ready-indicator",
type=str,
metavar="READY_INDICATOR_FILE",
help="""Path to a file that will be created once the backend is ready
and will be deleted again when it stops (before storing the logs).
""",
)
parser.add_argument(
"--add-cube",
action="store_true",
help="""Spawn a cube and run the object tracker backend.""",
)
parser.add_argument(
"--cameras",
action="store_true",
help="""Run camera backend using rendered images.""",
)
parser.add_argument(
"--visualize",
"-v",
action="store_true",
help="Run pyBullet's GUI for visualization.",
)
args = parser.parse_args()
# configure the logging module
log_handler = logging.StreamHandler(sys.stdout)
logging.basicConfig(
format="[SIM_TRIFINGER_BACKEND %(levelname)s %(asctime)s] %(message)s",
level=logging.DEBUG,
handlers=[log_handler],
)
# select the correct types/functions based on which robot is used
num_fingers = finger_types_data.get_number_of_fingers(args.finger_type)
if num_fingers == 1:
shared_memory_id = "finger"
finger_types = robot_interfaces.finger
create_backend = drivers.create_single_finger_backend
elif num_fingers == 3:
shared_memory_id = "trifinger"
finger_types = robot_interfaces.trifinger
create_backend = drivers.create_trifinger_backend
# If max_number_of_actions is set, choose the history size of the time
# series such that the whole episode fits in (+1 for the status message
# containing the "limit exceeded" error).
if args.max_number_of_actions:
history_size = args.max_number_of_actions + 1
else:
history_size = 1000
robot_data = finger_types.MultiProcessData(
shared_memory_id, True, history_size=history_size
)
robot_logger = finger_types.Logger(robot_data)
backend = create_backend(
robot_data,
args.real_time_mode,
args.visualize,
args.first_action_timeout,
args.max_number_of_actions,
)
backend.initialize()
#
# Camera and Object Tracker Interface
# Important: These objects need to be created _after_ the simulation is
# initialized (i.e. after the SimFinger instance is created).
#
if args.cameras and not args.add_cube:
# If cameras are enabled but not the object, use the normal
# PyBulletTriCameraDriver.
from trifinger_cameras import tricamera
camera_data = tricamera.MultiProcessData("tricamera", True, 10)
camera_driver = tricamera.PyBulletTriCameraDriver()
camera_backend = tricamera.Backend(camera_driver, camera_data) # noqa
elif args.add_cube:
# If the cube is enabled, use the PyBulletTriCameraObjectTrackerDriver.
# In case the cameras are not requested, disable rendering of the
# images to save time.
import trifinger_object_tracking.py_tricamera_types as tricamera
# spawn a cube in the centre of the arena
cube = collision_objects.Block(position=[0.0, 0.0, 0.035])
render_images = args.cameras
camera_data = tricamera.MultiProcessData("tricamera", True, 10)
camera_driver = tricamera.PyBulletTriCameraObjectTrackerDriver(
cube, robot_data, render_images
)
camera_backend = tricamera.Backend(camera_driver, camera_data) # noqa
#
# Camera/Object Logger
#
camera_logger = None
if args.camera_logfile:
try:
camera_data
except NameError:
logging.critical("Cannot create camera log camera is not running.")
return
# TODO
camera_fps = 10
robot_rate_hz = 1000
buffer_length_factor = 1.5
episode_length_s = args.max_number_of_actions / robot_rate_hz
# Compute camera log size based on number of robot actions plus some
# safety buffer
log_size = int(camera_fps * episode_length_s * buffer_length_factor)
logging.info("Initialize camera logger with buffer size %d", log_size)
camera_logger = tricamera.Logger(camera_data, log_size)
# if specified, create the "ready indicator" file to indicate that the
# backend is ready
if args.ready_indicator:
pathlib.Path(args.ready_indicator).touch()
backend.wait_until_first_action()
if camera_logger:
camera_logger.start()
logging.info("Start camera logging")
backend.wait_until_terminated()
# delete the ready indicator file to indicate that the backend has shut
# down
if args.ready_indicator:
pathlib.Path(args.ready_indicator).unlink()
if camera_logger:
logging.info(
"Save recorded camera data to file %s", args.camera_logfile
)
camera_logger.stop_and_save(args.camera_logfile)
if camera_data:
# stop the camera backend
logging.info("Stop camera backend")
camera_backend.shutdown()
if args.robot_logfile:
logging.info("Save robot data to file %s", args.robot_logfile)
if args.max_number_of_actions:
end_index = args.max_number_of_actions
else:
end_index = -1
robot_logger.write_current_buffer_binary(
args.robot_logfile, start_index=0, end_index=end_index
)
# cleanup stuff before the simulation (backend) is terminated
if args.add_cube:
del cube
logging.info("Done.")
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--max-number-of-actions",
"-a",
type=int,
required=True,
help="""Maximum numbers of actions that are processed. After this the
backend shuts down automatically.
""",
)
parser.add_argument(
"--fail-on-incomplete-run",
action="store_true",
help="""Exit with non-zero return code if the backend is terminated
before reaching `--max-number-of-actions`.
""",
)
parser.add_argument(
"--first-action-timeout",
"-t",
type=float,
default=math.inf,
help="""Timeout (in seconds) for reception of first action after
starting the backend. If not set, the timeout is disabled.
""",
)
camera_group = parser.add_mutually_exclusive_group()
camera_group.add_argument(
"--cameras",
"-c",
action="store_true",
help="Run camera backend.",
)
camera_group.add_argument(
"--cameras-with-tracker",
action="store_true",
help="Run camera backend with integrated object tracker.",
)
args = parser.parse_args()
rclpy.init()
node = NotificationNode("trifinger_backend")
logger = node.get_logger()
cameras_enabled = False
if args.cameras:
cameras_enabled = True
from trifinger_cameras import tricamera
CameraDriver = tricamera.TriCameraDriver
elif args.cameras_with_tracker:
cameras_enabled = True
import trifinger_object_tracking.py_tricamera_types as tricamera
CameraDriver = tricamera.TriCameraObjectTrackerDriver
if cameras_enabled:
logger.info("Start camera backend")
camera_data = tricamera.MultiProcessData("tricamera", False)
camera_driver = CameraDriver("camera60", "camera180", "camera300")
camera_backend = tricamera.Backend(camera_driver, camera_data)
logger.info("Camera backend ready.")
logger.info("Start robot backend")
# Use robot-dependent config file
config_file_path = "/etc/trifingerpro/trifingerpro.yml"
# Storage for all observations, actions, etc.
robot_data = robot_interfaces.trifinger.MultiProcessData(
"trifinger", False)
# The backend sends actions from the data to the robot and writes
# observations from the robot to the data.
backend = robot_fingers.create_trifinger_backend(
robot_data,
config_file_path,
first_action_timeout=args.first_action_timeout,
max_number_of_actions=args.max_number_of_actions,
)
# Initializes the robot (e.g. performs homing).
backend.initialize()
logger.info("Robot backend is ready")
# send ready signal
node.publish_status("READY")
# wait until backend terminates or shutdown request is received
while backend.is_running():
rclpy.spin_once(node, timeout_sec=1)
if node.shutdown_requested:
backend.request_shutdown()
backend.wait_until_terminated()
break
if cameras_enabled:
camera_backend.shutdown()
termination_reason = backend.get_termination_reason()
logger.debug("Backend termination reason: %d" % termination_reason)
rclpy.shutdown()
TermReason = robot_interfaces.RobotBackendTerminationReason
if (args.fail_on_incomplete_run and termination_reason !=
TermReason.MAXIMUM_NUMBER_OF_ACTIONS_REACHED):
# if --fail-on-incomplete-run is set any reason other than having
# reached the action limit is considered a failure.
logger.fatal("Expected termination reason %d (%s) but got %d" % (
TermReason.MAXIMUM_NUMBER_OF_ACTIONS_REACHED,
TermReason.MAXIMUM_NUMBER_OF_ACTIONS_REACHED.name,
termination_reason,
))
return 20
elif termination_reason < 0:
# negative termination reason means there was an error
# negate code as exit codes should be positive
return -termination_reason
else:
return 0
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--finger-type",
choices=finger_types_data.get_valid_finger_types(),
required=True,
help="""Pass a valid finger type.""",
)
parser.add_argument(
"--real-time-mode",
"-r",
action="store_true",
help="""Run simulation in real time. If not set, the simulation runs
as fast as possible.
""",
)
parser.add_argument(
"--first-action-timeout",
"-t",
type=float,
default=math.inf,
help="""Timeout (in seconds) for reception of first action after
starting the backend. If not set, the timeout is disabled.
""",
)
parser.add_argument(
"--max-number-of-actions",
"-a",
type=int,
default=0,
help="""Maximum numbers of actions that are processed. After this the
backend shuts down automatically.
""",
)
parser.add_argument(
"--logfile",
"-l",
type=str,
help="""Path to a file to which the data log is written. If not
specified, no log is generated.
""",
)
parser.add_argument(
"--add-cube",
action="store_true",
help="""Spawn a cube and run the object tracker backend.""",
)
parser.add_argument(
"--cameras",
action="store_true",
help="""Run camera backend using rendered images.""",
)
parser.add_argument(
"--visualize",
"-v",
action="store_true",
help="Run pyBullet's GUI for visualization.",
)
args = parser.parse_args()
# select the correct types/functions based on which robot is used
num_fingers = finger_types_data.get_number_of_fingers(args.finger_type)
if num_fingers == 1:
shared_memory_id = "finger"
finger_types = robot_interfaces.finger
create_backend = drivers.create_single_finger_backend
elif num_fingers == 3:
shared_memory_id = "trifinger"
finger_types = robot_interfaces.trifinger
create_backend = drivers.create_trifinger_backend
# If max_number_of_actions is set, choose the history size of the time
# series such that the whole episode fits in (+1 for the status message
# containing the "limit exceeded" error).
if args.max_number_of_actions:
history_size = args.max_number_of_actions + 1
else:
history_size = 1000
robot_data = finger_types.MultiProcessData(shared_memory_id,
True,
history_size=history_size)
logger = finger_types.Logger(robot_data)
backend = create_backend(
robot_data,
args.real_time_mode,
args.visualize,
args.first_action_timeout,
args.max_number_of_actions,
)
backend.initialize()
# Object and Object Tracker Interface
# Important: These objects need to be created _after_ the simulation is
# initialized (i.e. after the SimFinger instance is created).
if args.add_cube:
# only import when really needed
import trifinger_object_tracking.py_object_tracker as object_tracker
# spawn a cube in the arena
cube = collision_objects.Block()
# initialize the object tracker interface
object_tracker_data = object_tracker.Data("object_tracker", True)
object_tracker_backend = object_tracker.SimulationBackend( # noqa
object_tracker_data, cube, args.real_time_mode)
if args.cameras:
from trifinger_cameras import tricamera
camera_data = tricamera.MultiProcessData("tricamera", True, 10)
camera_driver = tricamera.PyBulletTriCameraDriver()
camera_backend = tricamera.Backend(camera_driver, camera_data) # noqa
backend.wait_until_terminated()
if args.logfile:
if args.max_number_of_actions:
end_index = args.max_number_of_actions
else:
end_index = -1
logger.write_current_buffer(args.logfile,
start_index=0,
end_index=end_index)
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--max-number-of-actions",
"-a",
type=int,
required=True,
help="""Maximum numbers of actions that are processed. After this the
backend shuts down automatically.
""",
)
parser.add_argument(
"--first-action-timeout",
"-t",
type=float,
default=math.inf,
help="""Timeout (in seconds) for reception of first action after
starting the backend. If not set, the timeout is disabled.
""",
)
camera_group = parser.add_mutually_exclusive_group()
camera_group.add_argument(
"--cameras",
"-c",
action="store_true",
help="Run camera backend.",
)
camera_group.add_argument(
"--cameras-with-tracker",
action="store_true",
help="Run camera backend with integrated object tracker.",
)
parser.add_argument(
"--robot-logfile",
type=str,
help="""Path to a file to which the robot data log is written. If not
specified, no log is generated.
""",
)
parser.add_argument(
"--camera-logfile",
type=str,
help="""Path to a file to which the camera data is written. If not
specified, no log is generated.
""",
)
parser.add_argument(
"--ready-indicator",
type=str,
metavar="READY_INDICATOR_FILE",
help="""Path to a file that will be created once the backend is ready
and will be deleted again when it stops (before storing the logs).
""",
)
args = parser.parse_args()
log_handler = logging.StreamHandler(sys.stdout)
logging.basicConfig(
format="[TRIFINGER_BACKEND %(levelname)s %(asctime)s] %(message)s",
level=logging.DEBUG,
handlers=[log_handler],
)
cameras_enabled = False
if args.cameras:
cameras_enabled = True
from trifinger_cameras import tricamera
CameraDriver = tricamera.TriCameraDriver
elif args.cameras_with_tracker:
cameras_enabled = True
import trifinger_object_tracking.py_tricamera_types as tricamera
CameraDriver = tricamera.TriCameraObjectTrackerDriver
if cameras_enabled:
logging.info("Start camera backend")
# make sure camera time series covers at least one second
CAMERA_TIME_SERIES_LENGTH = 15
camera_data = tricamera.MultiProcessData("tricamera", True,
CAMERA_TIME_SERIES_LENGTH)
camera_driver = CameraDriver("camera60", "camera180", "camera300")
camera_backend = tricamera.Backend(camera_driver, camera_data) # noqa
logging.info("Camera backend ready.")
logging.info("Start robot backend")
# Use robot-dependent config file
config_file_path = "/etc/trifingerpro/trifingerpro.yml"
# Storage for all observations, actions, etc.
history_size = args.max_number_of_actions + 1
robot_data = robot_interfaces.trifinger.MultiProcessData(
"trifinger", True, history_size=history_size)
if args.robot_logfile:
robot_logger = robot_interfaces.trifinger.Logger(robot_data)
# The backend sends actions from the data to the robot and writes
# observations from the robot to the data.
backend = robot_fingers.create_trifinger_backend(
robot_data,
config_file_path,
first_action_timeout=args.first_action_timeout,
max_number_of_actions=args.max_number_of_actions,
)
# Initializes the robot (e.g. performs homing).
backend.initialize()
logging.info("Robot backend is ready")
if cameras_enabled and args.camera_logfile:
camera_fps = 10
robot_rate_hz = 1000
# make the logger buffer a bit bigger as needed to be on the safe side
buffer_length_factor = 1.5
episode_length_s = args.max_number_of_actions / robot_rate_hz
# Compute camera log size based on number of robot actions plus a
# 10% buffer
log_size = int(camera_fps * episode_length_s * buffer_length_factor)
logging.info("Initialize camera logger with buffer size %d", log_size)
camera_logger = tricamera.Logger(camera_data, log_size)
# if specified, create the "ready indicator" file to indicate that the
# backend is ready
if args.ready_indicator:
pathlib.Path(args.ready_indicator).touch()
if cameras_enabled and args.camera_logfile:
backend.wait_until_first_action()
camera_logger.start()
logging.info("Start camera logging")
termination_reason = backend.wait_until_terminated()
logging.debug("Backend termination reason: %d", termination_reason)
# delete the ready indicator file to indicate that the backend has shut
# down
if args.ready_indicator:
pathlib.Path(args.ready_indicator).unlink()
if cameras_enabled and args.camera_logfile:
logging.info("Save recorded camera data to file %s",
args.camera_logfile)
camera_logger.stop_and_save(args.camera_logfile)
if args.robot_logfile:
logging.info("Save robot data to file %s", args.robot_logfile)
if args.max_number_of_actions:
end_index = args.max_number_of_actions
else:
end_index = -1
robot_logger.write_current_buffer_binary(args.robot_logfile,
start_index=0,
end_index=end_index)
if termination_reason < 0:
# negate code as exit codes should be positive
return -termination_reason
else:
return 0
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--max-number-of-actions",
"-a",
type=int,
required=True,
help="""Maximum numbers of actions that are processed. After this the
backend shuts down automatically.
""",
)
camera_group = parser.add_mutually_exclusive_group()
camera_group.add_argument(
"--cameras",
"-c",
action="store_true",
help="Run camera backend.",
)
camera_group.add_argument(
"--cameras-with-tracker",
action="store_true",
help="Run camera backend with integrated object tracker.",
)
parser.add_argument(
"--robot-logfile",
type=str,
help="""Path to a file to which the robot data log is written. If not
specified, no log is generated.
""",
)
parser.add_argument(
"--camera-logfile",
type=str,
help="""Path to a file to which the camera data is written. If not
specified, no log is generated.
""",
)
args = parser.parse_args()
rclpy.init()
node = NotificationNode("trifinger_data")
logger = node.get_logger()
cameras_enabled = False
if args.cameras:
cameras_enabled = True
from trifinger_cameras import tricamera
elif args.cameras_with_tracker:
cameras_enabled = True
import trifinger_object_tracking.py_tricamera_types as tricamera
if cameras_enabled:
logger.info("Start camera data")
camera_data = tricamera.MultiProcessData("tricamera", True,
CAMERA_TIME_SERIES_LENGTH)
logger.info("Start robot data")
# Storage for all observations, actions, etc.
robot_data = robot_interfaces.trifinger.MultiProcessData(
"trifinger", True, history_size=ROBOT_TIME_SERIES_LENGTH)
if args.robot_logfile:
robot_logger = robot_interfaces.trifinger.Logger(
robot_data, buffer_limit=args.max_number_of_actions)
robot_logger.start()
if cameras_enabled and args.camera_logfile:
camera_fps = 10
robot_rate_hz = 1000
# make the logger buffer a bit bigger as needed to be on the safe side
buffer_length_factor = 1.5
episode_length_s = args.max_number_of_actions / robot_rate_hz
# Compute camera log size based on number of robot actions plus some
# buffer
log_size = int(camera_fps * episode_length_s * buffer_length_factor)
logger.info("Initialize camera logger with buffer size %d" % log_size)
camera_logger = tricamera.Logger(camera_data, log_size)
logger.info("Data backend is ready")
# send ready signal
node.publish_status("READY")
if cameras_enabled and args.camera_logfile:
# wait for first action to be sent by the user (but make sure to not
# block when shutdown is requested)
while (not robot_data.desired_action.wait_for_timeindex(
0, max_duration_s=1) and not node.shutdown_requested):
rclpy.spin_once(node, timeout_sec=0)
camera_logger.start()
logger.info("Start camera logging")
while not node.shutdown_requested:
rclpy.spin_once(node)
logger.debug("Received shutdown signal")
if cameras_enabled and args.camera_logfile:
logger.info("Save recorded camera data to file %s" %
args.camera_logfile)
camera_logger.stop_and_save(args.camera_logfile)
if args.robot_logfile:
logger.info("Save robot data to file %s" % args.robot_logfile)
robot_logger.stop_and_save(
args.robot_logfile,
robot_interfaces.trifinger.Logger.Format.BINARY)
rclpy.shutdown()
return 0
def main():
argparser = argparse.ArgumentParser(description=__doc__)
argparser.add_argument(
"x",
type=float,
help="Target x-position of the cube in world frame.",
)
argparser.add_argument(
"y",
type=float,
help="Target y-position of the cube in world frame.",
)
argparser.add_argument(
"z",
type=float,
help="Target z-position of the cube in world frame.",
)
argparser.add_argument(
"--multi-process",
action="store_true",
help="""If set, use multiprocess camera data to access backend in other
process. Otherwise run the backend locally.
""",
)
args = argparser.parse_args()
camera_names = ["camera60", "camera180", "camera300"]
cube_position = np.array([args.x, args.y, args.z])
cube_pose = trifinger_object_tracking.py_object_tracker.ObjectPose()
cube_pose.position = cube_position
cube_pose.orientation = np.array([0, 0, 0, 1])
calib_files = []
for name in camera_names:
camera_config_dir = pathlib.Path(
get_package_share_directory("trifinger_cameras"), "config"
)
calib_file = (
camera_config_dir
/ "camera_calibration_trifingerone"
/ ("%s_cropped.yml" % name)
)
if calib_file.exists():
calib_files.append(str(calib_file))
else:
print("{} does not exist.".format(calib_file))
sys.exit(1)
cube_visualizer = CubeVisualizer(calib_files)
if args.multi_process:
camera_data = tricamera.MultiProcessData("tricamera", False)
else:
camera_data = tricamera.SingleProcessData()
camera_driver = tricamera.TriCameraDriver(
*camera_names, downsample_images=False
)
camera_backend = tricamera.Backend(camera_driver, camera_data) # noqa
camera_frontend = tricamera.Frontend(camera_data)
while True:
observation = camera_frontend.get_latest_observation()
images = [
convert_image(camera.image) for camera in observation.cameras
]
images = cube_visualizer.draw_cube(images, cube_pose, False)
# show images of all three cameras next to each other
stacked_image = np.hstack(images)
cv2.imshow(" | ".join(camera_names), stacked_image)
# for name, image in zip(camera_names, images):
# cv2.imshow(name, image)
# stop if either "q" or ESC is pressed
if cv2.waitKey(100) in [ord("q"), 27]: # 27 = ESC
break