def main(args):
"""
This is the main function for running the locomotion without crashing demo.
"""
if args.display_images == True:
from pyrobot.utils.util import try_cv2_import
cv2 = try_cv2_import()
bot = Robot("locobot", base_config={"base_planner": "none"})
bot.camera.reset()
print("Setting pan: {}, tilt: {}".format(*DEFAULT_PAN_TILT))
bot.camera.set_pan_tilt(*DEFAULT_PAN_TILT, wait=True)
model_path = os.path.join(args.save_dir, "crash_model.pth")
download_if_not_present(model_path, args.model_url)
crash_model = torch.load(model_path)
evaluator = Tester(crash_model)
control_start = time.time()
hist = "straight"
for _ in range(args.n_loops):
start_time = time.time()
rgb, _ = bot.camera.get_rgb_depth()
evals = evaluator.test(rgb)
print(evals)
if evals[3] > STRAIGHT_THRESHOLD:
hist = "straight"
go_straight(bot)
else:
if hist == "straight":
if evals[1] > evals[2]:
hist = "left"
turn_left(bot)
else:
hist = "right"
turn_right(bot)
elif hist == "left":
turn_left(bot)
elif hist == "right":
turn_right(bot)
stop_time = time.time()
time_elapsed = stop_time - start_time
if args.display_images == True:
cv2.imshow("image", evaluator.image)
cv2.waitKey(10)
if time.time() - control_start >= args.n_secs:
print("Time limit exceeded")
break
from abc import ABCMeta, abstractmethod
import numpy as np
import rospy
import tf
from geometry_msgs.msg import Twist, Pose, PoseStamped
from sensor_msgs.msg import JointState, CameraInfo, Image
import pyrobot.utils.util as prutil
from pyrobot.utils.move_group_interface import MoveGroupInterface as MoveGroup
from pyrobot_bridge.srv import *
from pyrobot.utils.util import try_cv2_import
cv2 = try_cv2_import()
from cv_bridge import CvBridge, CvBridgeError
import message_filters
import actionlib
from pyrobot_bridge.msg import (
MoveitAction,
MoveitGoal,
)
from actionlib_msgs.msg import GoalStatus
class Robot:
"""
