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: """