diff --git a/.gitignore b/.gitignore index 76e3b9355..9cb37a23f 100644 --- a/.gitignore +++ b/.gitignore @@ -2,7 +2,7 @@ __pycache__/ *.py[cod] *$py.class - +rktl_sim/urdf/goal_b.urdf # C extensions *.so @@ -142,3 +142,12 @@ dmypy.json # MATLAB *.asv + +#urdf removal: +rktl_sim/urdf/walls.urdf +rktl_sim/urdf/goal_a.urdf +rktl_sim/urdf/goal_b.urdf +rktl_sim/urdf/walls.urdf + +# macOS +.DS_Store diff --git a/docker/Dockerfile b/docker/Dockerfile index 4056b3bd3..8fdf6e0e2 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -10,7 +10,7 @@ RUN apt update && \ pylint clang-format jq swig zsh libeigen3-dev python3-catkin-tools && \ # pip pip3 install --upgrade pip && \ - pip3 install scipy numpy matplotlib \ + pip3 install scipy numpy matplotlib pfilter \ pandas jupyterlab autopep8 pyflakes && \ # Clean Up to reduce image size rm -rf /var/lib/apt/lists/* /root/.cache/pip/ diff --git a/docker/docker-server-run.sh b/docker/docker-server-run.sh index 124b12df5..a5d59f361 100755 --- a/docker/docker-server-run.sh +++ b/docker/docker-server-run.sh @@ -17,7 +17,7 @@ then exit fi -$WS_DIR/src/rocket_league/docker/docker-build.sh +$WS_DIR/src/rocket_league/docker/docker-build.sh --pull #XAUTH VOODOO rm -rf /home/$USER/.docker.tmp diff --git a/rktl_autonomy/nodes/rocket_league_agent b/rktl_autonomy/nodes/rocket_league_agent index 52e9a4ba2..bfd25c99a 100755 --- a/rktl_autonomy/nodes/rocket_league_agent +++ b/rktl_autonomy/nodes/rocket_league_agent @@ -11,18 +11,20 @@ from stable_baselines3 import PPO from os.path import expanduser import rospy -# create interface (and init ROS) +# Create interface (and init ROS). env = RocketLeagueInterface(eval=True) -# load the model +# Load the model. weights = expanduser(rospy.get_param('~weights')) model = PPO.load(weights) -# evaluate in real-time +# Evaluate in real-time. obs = env.reset() while True: + # Predict the future action for the sim. action, __ = model.predict(obs) try: + # step the sim with the action from the model. obs, __, __, __ = env.step(action) except rospy.ROSInterruptException: exit() diff --git a/rktl_autonomy/scripts/train_rocket_league.py b/rktl_autonomy/scripts/train_rocket_league.py index dcb356fc8..349c3a5c6 100755 --- a/rktl_autonomy/scripts/train_rocket_league.py +++ b/rktl_autonomy/scripts/train_rocket_league.py @@ -4,6 +4,7 @@ BSD 3-Clause License Copyright (c) 2021, Autonomous Robotics Club of Purdue (Purdue ARC) All rights reserved. +stable_baselines3 resource: https://stable-baselines3.readthedocs.io/_/downloads/en/master/pdf/ """ from rktl_autonomy import RocketLeagueInterface @@ -15,32 +16,34 @@ from os.path import expanduser import uuid -if __name__ == '__main__': # this is required due to forking processes - run_id = str(uuid.uuid4()) # ALL running environments must share this +if __name__ == '__main__': + # This is required due to forking processes. + # ALL running environments must share this id. + run_id = str(uuid.uuid4()) print(f"RUN ID: {run_id}") - # to pass launch args, add to env_kwargs: 'launch_args': ['render:=false', 'plot_log:=true'] - env = make_vec_env(RocketLeagueInterface, env_kwargs={'run_id':run_id}, - n_envs=24, vec_env_cls=SubprocVecEnv) + # Pass launch args by adding to env_kwargs: 'launch_args': ['render:=false', 'plot_log:=true']. + env = make_vec_env(RocketLeagueInterface, env_kwargs={'run_id': run_id}, + n_envs=24, vec_env_cls=SubprocVecEnv) model = PPO("MlpPolicy", env) - # log training progress as CSV + # Log training progress as CSV. log_dir = expanduser(f'~/catkin_ws/data/rocket_league/{run_id}') logger = configure(log_dir, ["stdout", "csv", "log"]) model.set_logger(logger) - # log model weights - freq = 20833 # save 20 times + # Log model weights. + freq = 20833 # save 20 times # freq = steps / (n_saves * n_envs) callback = CheckpointCallback(save_freq=freq, save_path=log_dir) - # run training - steps = 240000000 # 240M (10M sequential) + # Run training. + steps = 240000000 # 240M (10M sequential) print(f"training on {steps} steps") model.learn(total_timesteps=steps, callback=callback) - # save final weights + # Save final weights. print("done training") model.save(log_dir + "/final_weights") - env.close() # this must be done to clean up other processes + env.close() # This must be done to clean up other processes diff --git a/rktl_autonomy/src/rktl_autonomy/_ros_interface.py b/rktl_autonomy/src/rktl_autonomy/_ros_interface.py index 5e6b27784..04bb9c545 100644 --- a/rktl_autonomy/src/rktl_autonomy/_ros_interface.py +++ b/rktl_autonomy/src/rktl_autonomy/_ros_interface.py @@ -15,38 +15,38 @@ from rosgraph_msgs.msg import Clock from diagnostic_msgs.msg import DiagnosticStatus, KeyValue + class SimTimeException(Exception): """For when advancing sim time does not go as planned.""" pass -class ROSInterface(Env): - """Extension of the Gym environment class for all specific interfaces - to extend. This class handles logic regarding timesteps in ROS, and - allows users to treat any ROS system as a Gym environment once the - interface is created. +class ROSInterface(Env): + """Extension of the Gym environment class for all specific interfaces to extend. + This class handles logic regarding timesteps in ROS, and + allows users to treat any ROS system as a Gym environment once the interface is created. All classes extending this for a particular environment must do the following: - - implement all abstract properties: + - Implement all abstract properties: - action_space - observation_space - - implement all abstract methods: + - Implement all abstract methods: - _reset_env() - _reset_self() - _has_state() - _clear_state() - _get_state() - _publish_action() - - notify _cond when _has_state() may have turned true + - Notify _cond when _has_state() may have turned true. """ def __init__(self, node_name='gym_interface', eval=False, launch_file=None, launch_args=[], run_id=None): - """init function - Params: - node_name: desired name of this node in the ROS network - eval: set true if evaluating an agent in an existing ROS env, set false if training an agent - launch_file: if training, launch file to be used (ex: ['rktl_autonomy', 'rocket_league_train.launch']) - launch_args: if training, arguments to be passed to roslaunch (ex: ['render:=true', rate:=10]) - run_id: if training, used to prevent deadlocks. if logging, run_id describes where to save files. Default is randomly generated + """ + Initializes the rospy interface. + @param node_name: Desired name of this node in the ROS network. + @param eval: Set true if evaluating an agent in an existing ROS env, set false if training an agent. + @param launch_file: If training, launch file to be used (ex: ['rktl_autonomy', 'rocket_league_train.launch']). + @param launch_args: If training, arguments to be passed to roslaunch (ex: ['render:=true', rate:=10]). + @param run_id: If training, used to prevent deadlocks. if logging, run_id describes where to save files. """ super().__init__() self.__EVAL_MODE = eval @@ -66,12 +66,11 @@ def __init__(self, node_name='gym_interface', eval=False, launch_file=None, laun port = 11311 with socket.socket() as sock: try: - # see if default port is available sock.bind(('localhost', port)) except socket.error: - # find a random open one sock.bind(('localhost', 0)) port = sock.getsockname()[1] + # launch the training ROS network ros_id = roslaunch.rlutil.get_or_generate_uuid(None, False) roslaunch.configure_logging(ros_id) @@ -79,8 +78,8 @@ def __init__(self, node_name='gym_interface', eval=False, launch_file=None, laun launch_args = [f'render:={port==11311}', f'plot_log:={port==11311}'] + launch_args + [f'agent_name:={node_name}'] launch = roslaunch.parent.ROSLaunchParent(ros_id, [(launch_file, launch_args)], port=port) launch.start() - self.close = lambda : launch.shutdown() - # initialize self + self.close = lambda: launch.shutdown() + os.environ['ROS_MASTER_URI'] = f'http://localhost:{port}' rospy.init_node(node_name) # let someone else take a turn @@ -92,7 +91,6 @@ def __init__(self, node_name='gym_interface', eval=False, launch_file=None, laun # private variables self._cond = Condition() - # additional set up for training if not self.__EVAL_MODE: self.__DELTA_T = rospy.Duration.from_sec(1.0 / rospy.get_param('~rate', 30.0)) self.__clock_pub = rospy.Publisher('/clock', Clock, queue_size=1, latch=True) @@ -114,48 +112,43 @@ def __init__(self, node_name='gym_interface', eval=False, launch_file=None, laun def step(self, action): """ - Implementation of gym.Env.step. This function will intentionally block - if the ROS environment is not ready. - - Run one timestep of the environment's dynamics. When end of - episode is reached, you are responsible for calling `reset()` - to reset this environment's state. - Accepts an action and returns a tuple (observation, reward, done, info). - Args: - action (object): an action provided by the agent - Returns: - observation (object): agent's observation of the current environment - reward (float) : amount of reward returned after previous action - done (bool): whether the episode has ended, in which case further step() calls will return undefined results - info (dict): contains auxiliary diagnostic information (helpful for debugging, and sometimes learning) + Implementation of gym.Env.step. + This function will intentionally block if the ROS environment is not ready. + Run one timestep of the environment's dynamics. + When end of episode is reached, you are responsible for calling `reset()` to reset this environment's state. + @param action: An action provided by the agent. + @return observation: A tuple of the following: + (object): Agent's observation of the current environment. + reward (float) : Amount of reward returned after previous action. + done (bool): Whether the episode has ended, in which case further step() calls will return undefined results. + info (dict): Contains auxiliary diagnostic information (helpful for debugging, and sometimes learning). """ + self._clear_state() self._publish_action(action) self.__step_time_and_wait_for_state() state = self._get_state() - self.__net_reward += state[1] # logging + self.__net_reward += state[1] # logging return state def reset(self): - """Resets the environment to an initial state and returns an initial observation. - - Note that this function should not reset the environment's random - number generator(s); random variables in the environment's state should - be sampled independently between multiple calls to `reset()`. In other - words, each call of `reset()` should yield an environment suitable for - a new episode, independent of previous episodes. - Returns: - observation (object): the initial observation. """ + Resets the environment to an initial state and returns an initial observation. + Note that this function should not reset the environment's random number generator(s). + Random variables in the environment's state should be sampled independently between multiple calls to reset(). + @return: the initial observation. + """ + if self._has_state(): - # generate log + info = { - 'episode' : self.__episode, - 'net_reward' : self.__net_reward, - 'duration' : (rospy.Time.now() - self.__start_time).to_sec() + 'episode': self.__episode, + 'net_reward': self.__net_reward, + 'duration': (rospy.Time.now() - self.__start_time).to_sec() } + + # update the message log with these parameters by publishing it info.update(self._get_state()[3]) - # send message msg = DiagnosticStatus() msg.level = DiagnosticStatus.OK msg.name = 'ROS-Gym Interface' @@ -163,25 +156,28 @@ def reset(self): msg.hardware_id = self.__LOG_ID msg.values = [KeyValue(key=key, value=str(value)) for key, value in info.items()] self.__log_pub.publish(msg) - # update variables (update time after reset) self.__episode += 1 self.__net_reward = 0 - # reset if not self.__EVAL_MODE: self._reset_env() self._reset_self() self.__step_time_and_wait_for_state(5) - self.__start_time = rospy.Time.now() # logging + self.__start_time = rospy.Time.now() # logging return self._get_state()[0] def __step_time_and_wait_for_state(self, max_retries=1): - """Step time until a state is known.""" + """ + Increment the time and clock. + Try to publish the next simulation step in the number of tries. + @param max_retries: Number of time steps until state is known. + """ if not self.__EVAL_MODE: self.__time += self.__DELTA_T self.__clock_pub.publish(self.__time) retries = 0 while not self.__wait_once_for_state(): + if retries >= max_retries: rospy.logerr("Failed to get new state.") raise SimTimeException @@ -190,56 +186,57 @@ def __step_time_and_wait_for_state(self, max_retries=1): self.__clock_pub.publish(self.__time) retries += 1 else: + # call for the provided number of retries while not self.__wait_once_for_state(): - pass # idle wait + pass # idle wait def __wait_once_for_state(self): - """Wait and allow other threads to run.""" + """ Wait and allow other threads to run.""" with self._cond: has_state = self._cond.wait_for(self._has_state, 0.25) if rospy.is_shutdown(): raise rospy.ROSInterruptException() return has_state - # All the below abstract methods / properties must be implemented by subclasses @property @abstractmethod def action_space(self): - """The Space object corresponding to valid actions.""" + """ The Space object corresponding to valid actions.""" raise NotImplementedError @property @abstractmethod def observation_space(self): - """The Space object corresponding to valid observations.""" + """ The Space object corresponding to valid observations.""" raise NotImplementedError @abstractmethod def _reset_env(self): - """Reset environment for a new episode.""" + """ Reset environment for a new episode.""" raise NotImplementedError @abstractmethod def _reset_self(self): - """Reset internally for a new episode.""" + """ Reset internally for a new episode.""" raise NotImplementedError @abstractmethod def _has_state(self): - """Determine if the new state is ready.""" + """ Determine if the new state is ready.""" + raise NotImplementedError @abstractmethod def _clear_state(self): - """Clear state variables / flags in preparation for new ones.""" + """ Clear state variables / flags in preparation for new ones.""" raise NotImplementedError @abstractmethod def _get_state(self): - """Get state tuple (observation, reward, done, info).""" + """ Get state tuple (observation, reward, done, info).""" raise NotImplementedError @abstractmethod def _publish_action(self, action): - """Publish an action to the ROS network.""" + """ Publish an action to the ROS network.""" raise NotImplementedError diff --git a/rktl_autonomy/src/rktl_autonomy/rocket_league_interface.py b/rktl_autonomy/src/rktl_autonomy/rocket_league_interface.py index ad94d5a41..dfbb701e5 100755 --- a/rktl_autonomy/src/rktl_autonomy/rocket_league_interface.py +++ b/rktl_autonomy/src/rktl_autonomy/rocket_league_interface.py @@ -21,9 +21,13 @@ from enum import IntEnum, unique, auto from math import pi, tan + @unique class CarActions(IntEnum): - """Possible actions for car.""" + """ + Possible actions for car. + Currently using discrete action space. + """ STOP = 0 FWD_LEFT = auto() FWD_RIGHT = auto() @@ -33,19 +37,30 @@ class CarActions(IntEnum): REV = auto() SIZE = auto() + class RocketLeagueInterface(ROSInterface): - """ROS interface for the Rocket League.""" - def __init__(self, eval=False, launch_file=['rktl_autonomy', 'rocket_league_train.launch'], launch_args=[], run_id=None): - super().__init__(node_name='rocket_league_agent', eval=eval, launch_file=launch_file, launch_args=launch_args, run_id=run_id) - ## Constants - # Actions + def __init__(self, eval=False, launch_file=['rktl_autonomy', 'rocket_league_train.launch'], launch_args=[], + run_id=None): + """ + ROS interface for the Rocket League. + Set parameters for game elements (car/bar) and field object properties (goal,walls,planes). + Look at _ros_interface.py init function for the parameters. + @param eval: Specify whether you want to train data or not. + @param launch_file: Used in _ros_interface.py to configure the training for roslaunch. + @param launch_args: Specify the files to be used in launching. + @param run_id: The id for this specific training run. + """ + super().__init__(node_name='rocket_league_agent', eval=eval, launch_file=launch_file, launch_args=launch_args, + run_id=run_id) + + # car action constants self._MIN_VELOCITY = -rospy.get_param('/cars/throttle/max_speed') - self._MAX_VELOCITY = rospy.get_param('/cars/throttle/max_speed') + self._MAX_VELOCITY = rospy.get_param('/cars/throttle/max_speed') self._MIN_CURVATURE = -tan(rospy.get_param('/cars/steering/max_throw')) / rospy.get_param('cars/length') - self._MAX_CURVATURE = tan(rospy.get_param('/cars/steering/max_throw')) / rospy.get_param('cars/length') + self._MAX_CURVATURE = tan(rospy.get_param('/cars/steering/max_throw')) / rospy.get_param('cars/length') - # Action space overrides + # car action space overrides if rospy.has_param('~action_space/velocity/min'): min_velocity = rospy.get_param('~action_space/velocity/min') assert min_velocity > self._MIN_VELOCITY @@ -63,14 +78,14 @@ def __init__(self, eval=False, launch_file=['rktl_autonomy', 'rocket_league_trai assert max_curvature < self._MAX_CURVATURE self._MAX_CURVATURE = max_curvature - # Observations + # observations self._FIELD_WIDTH = rospy.get_param('/field/width') self._FIELD_LENGTH = rospy.get_param('/field/length') self._GOAL_DEPTH = rospy.get_param('~observation/goal_depth', 0.075) self._MAX_OBS_VEL = rospy.get_param('~observation/velocity/max_abs', 3.0) - self._MAX_OBS_ANG_VEL = rospy.get_param('~observation/angular_velocity/max_abs', 2*pi) + self._MAX_OBS_ANG_VEL = rospy.get_param('~observation/angular_velocity/max_abs', 2 * pi) - # Learning + # learning self._MAX_TIME = rospy.get_param('~max_episode_time', 30.0) self._CONSTANT_REWARD = rospy.get_param('~reward/constant', 0.0) self._BALL_DISTANCE_REWARD = rospy.get_param('~reward/ball_dist_sq', 0.0) @@ -81,22 +96,22 @@ def __init__(self, eval=False, launch_file=['rktl_autonomy', 'rocket_league_trai self._WALL_REWARD = rospy.get_param('~reward/walls/value', 0.0) self._WALL_THRESHOLD = rospy.get_param('~reward/walls/threshold', 0.0) - # Publishers + # publishers self._command_pub = rospy.Publisher('cars/car0/command', ControlCommand, queue_size=1) self._reset_srv = rospy.ServiceProxy('sim_reset', Empty) - # State variables + # state variables self._car_odom = None self._ball_odom = None self._score = None self._start_time = None - # Subscribers + # subscribers rospy.Subscriber('cars/car0/odom', Odometry, self._car_odom_cb) rospy.Subscriber('ball/odom', Odometry, self._ball_odom_cb) rospy.Subscriber('match_status', MatchStatus, self._score_cb) - # block until environment is ready + # block until environment in ready if not eval: rospy.wait_for_service('sim_reset') @@ -107,24 +122,27 @@ def action_space(self): @property def observation_space(self): - """The Space object corresponding to valid observations.""" + """ + The Space object corresponding to valid observations. + @return: The lower and upper bound of the field. + """ return Box( # x, y, theta, v, omega (car) # x, y, vx, vy (ball) low=np.array([ - -(self._FIELD_LENGTH/2) - self._GOAL_DEPTH, - -self._FIELD_WIDTH/2, -pi, + -(self._FIELD_LENGTH / 2) - self._GOAL_DEPTH, + -self._FIELD_WIDTH / 2, -pi, -self._MAX_OBS_VEL, -self._MAX_OBS_ANG_VEL, - -(self._FIELD_LENGTH/2) - self._GOAL_DEPTH, - -self._FIELD_WIDTH/2, + -(self._FIELD_LENGTH / 2) - self._GOAL_DEPTH, + -self._FIELD_WIDTH / 2, -self._MAX_OBS_VEL, -self._MAX_OBS_VEL], dtype=np.float32), high=np.array([ - (self._FIELD_LENGTH/2) + self._GOAL_DEPTH, - self._FIELD_WIDTH/2, pi, + (self._FIELD_LENGTH / 2) + self._GOAL_DEPTH, + self._FIELD_WIDTH / 2, pi, self._MAX_OBS_VEL, self._MAX_OBS_ANG_VEL, - (self._FIELD_LENGTH/2) + self._GOAL_DEPTH, - self._FIELD_WIDTH/2, + (self._FIELD_LENGTH / 2) + self._GOAL_DEPTH, + self._FIELD_WIDTH / 2, self._MAX_OBS_VEL, self._MAX_OBS_VEL], dtype=np.float32)) @@ -140,9 +158,9 @@ def _reset_self(self): def _has_state(self): """Determine if the new state is ready.""" return ( - self._car_odom is not None and - self._ball_odom is not None and - self._score is not None) + self._car_odom is not None and + self._ball_odom is not None and + self._score is not None) def _clear_state(self): """Clear state variables / flags in preparation for new ones.""" @@ -151,15 +169,18 @@ def _clear_state(self): self._score = None def _get_state(self): - """Get state tuple (observation, reward, done, info).""" + """ + Checks if the ball and car are in the field limits, steps the time, and checks if there are rewards. + @return: state tuple (observation, reward, done, info) + """ assert self._has_state() - # combine car / ball odoms for observation + # combine the car and ball odoms for observation car = np.asarray(self._car_odom, dtype=np.float32) ball = np.asarray(self._ball_odom, dtype=np.float32) observation = np.concatenate((car, ball)) - # ensure it fits within the observation limits + # ensure the observation fits within the the limits if not self.observation_space.contains(observation): rospy.logwarn_throttle(5, "Coercing observation into valid bounds") np.clip( @@ -168,20 +189,20 @@ def _get_state(self): self.observation_space.high, out=observation) - # check if time exceeded + # check if time has exceeded if self._start_time is None: self._start_time = rospy.Time.now() done = (rospy.Time.now() - self._start_time).to_sec() >= self._MAX_TIME - # Determine reward + # determine the reward reward = self._CONSTANT_REWARD ball_dist_sq = np.sum(np.square(ball[0:2] - car[0:2])) reward += self._BALL_DISTANCE_REWARD * ball_dist_sq - goal_dist_sq = np.sum(np.square(ball[0:2] - np.array([self._FIELD_LENGTH/2, 0]))) + goal_dist_sq = np.sum(np.square(ball[0:2] - np.array([self._FIELD_LENGTH / 2, 0]))) reward += self._GOAL_DISTANCE_REWARD * goal_dist_sq - + # check if someone scored if self._score != 0: done = True if self._score > 0: @@ -192,38 +213,41 @@ def _get_state(self): x, y, __, v, __ = self._car_odom if v < 0: reward += self._REVERSE_REWARD - if (abs(x) > self._FIELD_LENGTH/2 - self._WALL_THRESHOLD or - abs(y) > self._FIELD_WIDTH/2 - self._WALL_THRESHOLD): + if (abs(x) > self._FIELD_LENGTH / 2 - self._WALL_THRESHOLD or + abs(y) > self._FIELD_WIDTH / 2 - self._WALL_THRESHOLD): reward += self._WALL_REWARD - # info dict - info = {'goals' : self._score} + info = {'goals': self._score} return (observation, reward, done, info) def _publish_action(self, action): - """Publish an action to the ROS network.""" + """ + Publish an action to the ROS network (using a message of action and curvature). + @param action: The desired action. + @return: Translated command in curvature and velocity. + """ assert self.action_space.contains(action) msg = ControlCommand() msg.header.stamp = rospy.Time.now() - - if ( action == CarActions.FWD or + # set msg velocity to max for Forward, Forward-Right, and Forward-Left movement + if (action == CarActions.FWD or action == CarActions.FWD_RIGHT or action == CarActions.FWD_LEFT): msg.velocity = self._MAX_VELOCITY - elif ( action == CarActions.REV or - action == CarActions.REV_RIGHT or - action == CarActions.REV_LEFT): + elif (action == CarActions.REV or + action == CarActions.REV_RIGHT or + action == CarActions.REV_LEFT): msg.velocity = self._MIN_VELOCITY else: msg.velocity = 0.0 - - if ( action == CarActions.FWD_LEFT or + # set msg velocity to min for Back, Back-Right, and Back-Left movement + if (action == CarActions.FWD_LEFT or action == CarActions.REV_LEFT): msg.curvature = self._MAX_CURVATURE - elif ( action == CarActions.FWD_RIGHT or - action == CarActions.REV_RIGHT): + elif (action == CarActions.FWD_RIGHT or + action == CarActions.REV_RIGHT): msg.curvature = self._MIN_CURVATURE else: msg.curvature = 0.0 @@ -249,6 +273,7 @@ def _car_odom_cb(self, odom_msg): def _ball_odom_cb(self, odom_msg): """Callback for odometry of ball.""" + x = odom_msg.pose.pose.position.x y = odom_msg.pose.pose.position.y vx = odom_msg.twist.twist.linear.x diff --git a/rktl_launch/config/global_params.yaml b/rktl_launch/config/global_params.yaml index ff7a0524a..146450eb3 100644 --- a/rktl_launch/config/global_params.yaml +++ b/rktl_launch/config/global_params.yaml @@ -15,7 +15,9 @@ ball: # car dimensions and physical constants cars: width: 0.0762 # (3 in) - length: 0.12 # (8 in) + length: 0.12 + # max: 0.12 # (8 in) + # min: 0.6 steering: max_throw: 0.1826 # 15 degrees rate: 0.9128 # 30 degrees/s diff --git a/rktl_planner/nodes/path_planner b/rktl_planner/nodes/path_planner index 07cd6b9dc..977cf331e 100755 --- a/rktl_planner/nodes/path_planner +++ b/rktl_planner/nodes/path_planner @@ -11,7 +11,15 @@ from rktl_msgs.msg import BezierPathList, Path from std_srvs.srv import Empty, EmptyRequest, EmptyResponse from rktl_planner.srv import CreateBezierPath, CreateBezierPathRequest + def create_simple_path_req(car_odom, ball_odom, goal_pos): + """ + All poses lead to forward motion of the car. + @return: A list of future car poses depending on a desired action. + pose0: The pose & duration if the car continues moving as is. + pose1: The pose & duration if the car attempts to move forward to the ball. + pose2: the pose if the car attempts a full stop moving forward. + """ req = CreateBezierPathRequest() req.velocity = 0.5 @@ -54,7 +62,14 @@ def create_simple_path_req(car_odom, ball_odom, goal_pos): return req + def create_backup_path_req(car_odom, ball_odom, goal_pos): + """ + All poses lead to backward motion of the car. + @return: A list of future car poses depending on a desired action. + pose0: The pose & duration if the car continues to move as is. + pose1: The pose & duration if the car attempts to move backward to the ball. + """ req = CreateBezierPathRequest() req.velocity = -0.5 req.bezier_segment_duration.data = rospy.Duration(0.5) @@ -86,25 +101,32 @@ def create_backup_path_req(car_odom, ball_odom, goal_pos): return req + def create_complex_path_req(car_odom, ball_odom, goal_pos): + """Returns either a backup or forward req depending on the car's position to the ball.""" car_orient = car_odom.pose.pose.orientation car_yaw = euler_from_quaternion( np.array([car_orient.x, car_orient.y, car_orient.z, car_orient.w]))[2] car_x = car_odom.pose.pose.position.x ball_x = ball_odom.pose.pose.position.x - if (car_yaw < math.pi/2 or car_yaw > 3*math.pi/2) and (car_x > ball_x): + if (car_yaw < math.pi / 2 or car_yaw > 3 * math.pi / 2) and (car_x > ball_x): return create_backup_path_req(car_odom, ball_odom, goal_pos) else: return create_simple_path_req(car_odom, ball_odom, goal_pos) + class PathPlanner(object): + """Handles a car's future pose and duration for each valid action.""" + def __init__(self): rospy.init_node('path_planner') planner_type = rospy.get_param('~planner_type', 'simple') if planner_type == 'simple': + # enables only forward motion self.path_req = create_simple_path_req elif planner_type == 'complex': + # enables both forward and backward motion self.path_req = create_complex_path_req else: raise NotImplementedError(f'unrecognized planner type: {rospy.get_param("~planner_type")}') @@ -119,10 +141,8 @@ class PathPlanner(object): self.path_client = rospy.ServiceProxy('create_bezier_path', CreateBezierPath) # Publishers - self.linear_path_pub = rospy.Publisher('linear_path', - Path, queue_size=1, latch=True) - self.bezier_path_pub = rospy.Publisher('bezier_path', - BezierPathList, queue_size=1, latch=True) + self.linear_path_pub = rospy.Publisher('linear_path', Path, queue_size=1, latch=True) + self.bezier_path_pub = rospy.Publisher('bezier_path', BezierPathList, queue_size=1, latch=True) self.car_odom = Odometry() self.ball_odom = Odometry() @@ -148,5 +168,6 @@ class PathPlanner(object): self.bezier_path_pub.publish(bezier_path_list) return EmptyResponse() + if __name__ == '__main__': PathPlanner() diff --git a/rktl_sim/nodes/simulator b/rktl_sim/nodes/simulator index 3b5bb96cb..518082fc8 100755 --- a/rktl_sim/nodes/simulator +++ b/rktl_sim/nodes/simulator @@ -7,6 +7,7 @@ License: """ # 3rd party modules +import random from geometry_msgs.msg import PoseWithCovarianceStamped from nav_msgs.msg import Odometry import os @@ -15,22 +16,48 @@ from std_srvs.srv import Empty, EmptyResponse from threading import Lock from enum import Enum -# Local library +# local libraries import simulator from rktl_sim.srv import CreateCar, CreateCarResponse, DeleteCar, DeleteCarResponse from rktl_msgs.msg import MatchStatus, ControlCommand, ControlEffort + class SimulatorMode(Enum): - IDEAL = 1 - REALISTIC = 2 + IDEAL = 1 # no sensor noise, publish car and ball odom & pose early + REALISTIC = 2 # sensor noise for pose & orient of ball and car, publish with a delay + class Simulator(object): - """ROS wrapper for the simulator.""" + """Serves as an interface between params and services to the Sim class. + Handles simulator configuration parameters: + sim rate(default=30), frame id(default=map), timeout(default=10) + sensor noise, urdf paths, car list, car properties + + Two types of parameters: configuration-based and instance-based. + Configuration-based parameters determine the kinematics and dynamics of the sim. + A few examples: car size, sim timestep, friction, etc. + Instance-based parameters determine the initial state of a new run. + A few examples: car pose, ball pose, etc. + On startup, we load both configuration and instance parameters and create an initial setup. + + Simulator controls Configuration-based parameters. + Sim.py handles instance-based parameters. + """ def __init__(self): + """Initialize the simulator environment, field and objects properties, and car and ball objects.""" + self.props = None + self.spawn_bounds = None + self.sensor_noise = None + self.ball_noise = None + self.car_noise = None + self.ball_init_pose = None + self.ball_init_speed = None + self.car_properties = None + + # setting config parameters (stay constant for the whole simulator run) rospy.init_node('simulator') - - mode = rospy.get_param('~mode') + mode = self.get_sim_param('~mode') if mode == 'ideal': self.mode = SimulatorMode.IDEAL elif mode == 'realistic': @@ -38,120 +65,73 @@ class Simulator(object): else: rospy.signal_shutdown('unknown sim mode set "{}"'.format(mode)) - render_enabled = rospy.get_param('~render', False) - rate = rospy.Rate(rospy.get_param('~rate', 30)) - self.frame_id = rospy.get_param('~frame_id', 'map') - self.timeout = rospy.get_param('~timeout', 10) - - # Setting up field - fw = rospy.get_param('/field/width') - fl = rospy.get_param('/field/length') - wt = rospy.get_param('/field/wall_thickness') - spawn_height = rospy.get_param('~spawn_height', 0.06) - - # Setup bounds for spawning car and ball - spawn_bounds = [[-(fl/2) + (2 * wt), (fl/2) - (2 * wt)], - [-(fw/2) + (2 * wt), (fw/2) - (2 * wt)], - [spawn_height, spawn_height]] + render_enabled = self.get_sim_param('~render', secondParam=False) + rate = rospy.Rate(self.get_sim_param('~rate', secondParam=30)) + self.frame_id = self.get_sim_param('~frame_id', secondParam='map') + self.timeout = self.get_sim_param('~timeout', secondParam=10) - urdf_paths = rospy.get_param('~urdf') - for path in urdf_paths.values(): + # setup urdf file paths: a universal way to describe kinematics and dynamics of robots + self.urdf_paths = self.get_sim_param('~urdf') + for path in self.urdf_paths.values(): self.check_urdf(path) - # Setup simulator properties - props = { - 'engine': rospy.get_param('~engine', None), - 'dynamics': rospy.get_param('~dynamics', None), + self.props = { + 'engine': self.get_sim_param('~engine', secondParam=None), + 'dynamics': self.get_sim_param('~dynamics', secondParam=None), } - # Creating physics simulator - self.sim = simulator.Sim(props, urdf_paths, spawn_bounds, render_enabled) + # prep the simulator for a new run, setting all instance parameters for the sim - # Setting sensor noise - sensor_noise = rospy.get_param('~sensor_noise', None) - ball_noise = None - car_noise = None - if sensor_noise is not None and self.mode == SimulatorMode.REALISTIC: - ball_noise = sensor_noise.get('ball', None) - car_noise = sensor_noise.get('car', None) - self.car_noise = car_noise - - # Creating the ball - ball_init_pose = rospy.get_param('~ball/init_pose', None) - ball_init_speed = rospy.get_param('~ball/init_speed', None) - - self.sim.create_ball('ball', init_pose=ball_init_pose, - init_speed=ball_init_speed, noise=ball_noise) - - if self.mode == SimulatorMode.REALISTIC: - self.ball_pose_pub = rospy.Publisher('/ball/pose_sync_early', - PoseWithCovarianceStamped, queue_size=1) - self.ball_odom_pub = rospy.Publisher('/ball/odom_truth', - Odometry, queue_size=1) - elif self.mode == SimulatorMode.IDEAL: - self.ball_odom_pub = rospy.Publisher('/ball/odom', - Odometry, queue_size=1) - - # Creating cars - car_properties = {} - car_properties['length'] = rospy.get_param('/cars/length') - car_properties['max_speed'] = rospy.get_param('/cars/throttle/max_speed') - car_properties['throttle_tau'] = rospy.get_param('/cars/throttle/tau') - car_properties['steering_throw'] = rospy.get_param('/cars/steering/max_throw') - car_properties['steering_rate'] = rospy.get_param('/cars/steering/rate') - car_properties['simulate_effort'] = (self.mode == SimulatorMode.REALISTIC) - self.car_properties = car_properties - + self.spawn_bounds = self.get_spawn_bounds() self.car_ids = {} self.car_pose_pubs = {} self.car_odom_pubs = {} self.car_effort_subs = {} self.car_cmd_subs = {} - car_configs = rospy.get_param('~cars', []) - for car_config in car_configs: - init_pose = None - if 'init_pose' in car_config: - init_pose = car_config['init_pose'] - - if 'name' not in car_config: - rospy.signal_shutdown('no "name" set for car config in sim') - car_name = car_config['name'] - - self.car_ids[car_name] = self.sim.create_car( - 'car', init_pose=init_pose, noise=car_noise, car_props=car_properties) - - car_id = self.car_ids[car_name] - self.car_effort_subs[car_name] = rospy.Subscriber( - f'/cars/{car_name}/effort', ControlEffort, - self.effort_cb, callback_args=car_id) - self.car_cmd_subs[car_name] = rospy.Subscriber( - f'/cars/{car_name}/command', ControlCommand, - self.cmd_cb, callback_args=car_id) - - if self.mode == SimulatorMode.REALISTIC: - self.car_pose_pubs[car_name] = rospy.Publisher( - f'/cars/{car_name}/pose_sync_early', - PoseWithCovarianceStamped, queue_size=1) - self.car_odom_pubs[car_name] = rospy.Publisher( - f'/cars/{car_name}/odom_truth', Odometry, queue_size=1) - elif self.mode == SimulatorMode.IDEAL: - self.car_odom_pubs[car_name] = rospy.Publisher( - f'/cars/{car_name}/odom', Odometry, queue_size=1) - - # Node data + # TODO: find a better way to not have duplicate code segment + self.car_properties = {'length': self.get_sim_param('/cars/length'), + 'max_speed': self.get_sim_param("/cars/throttle/max_speed"), + 'steering_throw': self.get_sim_param("/cars/steering/max_throw"), + 'throttle_tau': self.get_sim_param("/cars/throttle/tau"), + 'steering_rate': self.get_sim_param("/cars/steering/rate"), + 'simulate_effort': (self.mode == SimulatorMode.REALISTIC)} + + self.sim = simulator.Sim(self.props, self.urdf_paths, self.spawn_bounds, render_enabled) + print("done creating sim!") + self.sim.create_ball('ball', init_pose=self.ball_init_pose, + init_speed=self.ball_init_speed, noise=self.ball_noise) + print("done creating ball!") + + self.update_all_cars() + print("done creating cars!") self.cmd_lock = Lock() self.reset_lock = Lock() self.last_time = None + print("calling reset cb!") + self.reset_cb(None) # janky reset call with mandatory none parameter # Publishers self.status_pub = rospy.Publisher( 'match_status', MatchStatus, queue_size=1) - + self.ball_pose_pub, self.ball_odom_pub = None, None + if self.mode == SimulatorMode.REALISTIC: + self.ball_pose_pub = rospy.Publisher('/ball/pose_sync_early', + PoseWithCovarianceStamped, queue_size=1) + self.ball_odom_pub = rospy.Publisher('/ball/odom_truth', + Odometry, queue_size=1) + elif self.mode == SimulatorMode.IDEAL: + self.ball_odom_pub = rospy.Publisher('/ball/odom', + Odometry, queue_size=1) + print("done creating publishers!") # Services rospy.Service('sim_reset', Empty, self.reset_cb) + rospy.Service('sim_reset_ball', Empty, self.reset_ball_cb) rospy.Service('sim_create_car', CreateCar, self.create_car_cb) rospy.Service('sim_delete_car', DeleteCar, self.delete_car_cb) + rospy.Service('sim_create_all_cars', Empty, self.create_all_cars_cb) + rospy.Service('sim_delete_all_cars', Empty, self.delete_all_cars_cb) + print("done creating services!") while not rospy.is_shutdown(): self.loop_once() try: @@ -159,80 +139,96 @@ class Simulator(object): except rospy.ROSInterruptException: pass - def check_urdf(self, param): - """Validates that a URDF exists, then returns path""" - if param is None: - rospy.signal_shutdown('no urdf path set for "{}"'.format(param)) + def check_urdf(self, urdf_path): + """Validates that a URDF exists, then returns its file.""" + + if urdf_path is None: + rospy.signal_shutdown('no urdf path set for "{}"'.format(urdf_path)) i = 0 - while not os.path.isfile(param) and i < 5: - rospy.sleep(0.1) # Wait for xacro build + while not os.path.isfile(urdf_path) and i < 5: + rospy.sleep(0.1) # wait for xacro build i += 1 - - if not os.path.isfile(param): + + if not os.path.isfile(urdf_path): rospy.signal_shutdown( - 'no urdf file exists at path {}'.format(param)) + 'no urdf file exists at path {}'.format(urdf_path)) def effort_cb(self, effort_msg, car_id): - """Sets a car's effort (overwrites commands)""" + """Sets a car's effort with throttle and steering (overwrites commands).""" self.cmd_lock.acquire() self.sim.set_car_command(car_id, - (effort_msg.throttle, effort_msg.steering)) + (effort_msg.throttle, effort_msg.steering)) self.cmd_lock.release() def cmd_cb(self, cmd_msg, car_id): - """Sets a car's command (overwrites efforts)""" + """Sets a car's command with the velocity and curvature. (overwrites effort).""" self.cmd_lock.acquire() self.sim.set_car_command(car_id, - (cmd_msg.velocity, cmd_msg.curvature)) + (cmd_msg.velocity, cmd_msg.curvature)) self.cmd_lock.release() def create_car_cb(self, req): - """Adds a new car to the simulator""" self.reset_lock.acquire() if req.name in self.car_ids: + self.reset_lock.release() return CreateCarResponse(False) - init_pose = None + # sim.py class handles the randomization + init_pose = { + "pos": None, + "orient": None + } if req.reset_to_random_pose is False: init_pose = { "pos": [req.init_pos.x, - req.init_pos.y, + req.init_pos.y, req.init_pos.z], "orient": [0.0, 0.0, req.init_yaw], } - - self.car_ids[req.name] = self.sim.create_car( - 'car', init_pose, noise=self.car_noise, props=self.car_properties) - - car_id = self.car_ids[req.name] - self.car_effort_subs[req.name] = rospy.Subscriber( - f'/cars/{req.name}/effort', ControlEffort, - self.effort_cb, callback_args=car_id) - self.car_cmd_subs[req.name] = rospy.Subscriber( - f'/cars/{req.name}/command', ControlCommand, - self.cmd_cb, callback_args=car_id) - - if self.mode == SimulatorMode.REALISTIC: - self.car_pose_pubs[req.name] = rospy.Publisher( - f'/cars/{req.name}/pose_sync_early', - PoseWithCovarianceStamped, queue_size=1) - self.car_odom_pubs[req.name] = rospy.Publisher( - f'/cars/{req.name}/odom_truth', Odometry, queue_size=1) - elif self.mode == SimulatorMode.IDEAL: - self.car_odom_pubs[req.name] = rospy.Publisher( - f'/cars/{req.name}/odom', Odometry, queue_size=1) - + car_param = [ + { + "name": req.name, + "init_pose": init_pose + } + ] + rospy.set_param('/cars/', car_param) + self.update_all_cars() + + # self.car_ids[req.name] = self.sim.create_car( + # 'car', init_pose, noise=self.car_noise, props=self.car_properties) + # + # # Subscribers + # car_id = self.car_ids[req.name] + # self.car_effort_subs[req.name] = rospy.Subscriber( + # f'/cars/{req.name}/effort', ControlEffort, + # self.effort_cb, callback_args=car_id) + # self.car_cmd_subs[req.name] = rospy.Subscriber( + # f'/cars/{req.name}/command', ControlCommand, + # self.cmd_cb, callback_args=car_id) + # + # if self.mode == SimulatorMode.REALISTIC: + # # Publishers + # self.car_pose_pubs[req.name] = rospy.Publisher( + # f'/cars/{req.name}/pose_sync_early', + # PoseWithCovarianceStamped, queue_size=1) + # self.car_odom_pubs[req.name] = rospy.Publisher( + # f'/cars/{req.name}/odom_truth', Odometry, queue_size=1) + # elif self.mode == SimulatorMode.IDEAL: + # self.car_odom_pubs[req.name] = rospy.Publisher( + # f'/cars/{req.name}/odom', Odometry, queue_size=1) + + self.reset_cb(None) # janky reset call with mandatory none parameter self.reset_lock.release() return CreateCarResponse(True) def delete_car_cb(self, req): - """Removes a car from the simulator""" self.reset_lock.acquire() if req.name not in self.car_ids: + self.reset_lock.release() return DeleteCarResponse(False) - # Delete all related pubs and subs + # unsubscribe from the subscribers and publishers if req.name in self.car_effort_subs: self.car_effort_subs[req.name].unregister() del self.car_effort_subs[req.name] @@ -245,33 +241,70 @@ class Simulator(object): if req.name in self.car_pose_pubs: self.car_pose_pubs[req.name].unregister() del self.car_pose_pubs[req.name] - - # Delete the car's body from pybullet + + # delete cars body from pybullet res = self.sim.delete_car(self.car_ids[req.name]) del self.car_ids[req.name] - + + # delete car from rospy to prevent regeneration + rospy.delete_param(f'/cars/{req.name}') + self.reset_lock.release() + return DeleteCarResponse(res) def reset_cb(self, _): - """Resets simulator""" + print("inside reset CB!") self.reset_lock.acquire() - self.sim.reset() - self.car_cmd = (0.0, 0.0) + # setting sim parameters (can be modified by the user) + print("got spawn bounds!") + self.spawn_bounds = self.get_spawn_bounds() + + self.sensor_noise = self.get_sim_param('~sensor_noise', secondParam=None) + print("got sensor noise!") + self.car_noise = None + if self.sensor_noise is not None and self.mode == SimulatorMode.REALISTIC: + self.car_noise = self.sensor_noise.get('car', None) + print("set car sensor noise!") + self.reset_ball_cb(None) + print("reset the ball!!") + self.car_properties = {'length': self.get_sim_param('/cars/length'), + 'max_speed': self.get_sim_param("/cars/throttle/max_speed"), + 'steering_throw': self.get_sim_param("/cars/steering/max_throw"), + 'throttle_tau': self.get_sim_param("/cars/throttle/tau"), + 'steering_rate': self.get_sim_param("/cars/steering/rate"), + 'simulate_effort': (self.mode == SimulatorMode.REALISTIC)} + self.sim.reset(self.spawn_bounds, self.car_properties, self.ball_init_pose, self.ball_init_speed) + print("reset the sim!!!") + self.last_time = None self.reset_lock.release() return EmptyResponse() + def reset_ball_cb(self, _): + """Resets the ball sensor noise and pose WITHOUT resetting the whole sim.""" + self.ball_noise = None + if self.sensor_noise is not None and self.mode == SimulatorMode.REALISTIC: + self.ball_noise = self.sensor_noise.get('ball', None) + + self.ball_init_pose = self.get_sim_param('~ball/init_pose') + self.ball_init_speed = self.get_sim_param('/ball/init_speed') + + # print(dir(self.sim)) + + self.sim.reset_ball() + return EmptyResponse() + def loop_once(self): - """Main loop""" + """Step the simulation once step, updating match status, moving and publishing new car and ball positions.""" self.reset_lock.acquire() + print("looping now!") now = rospy.Time.now() if self.last_time is not None and self.last_time != now: - # Iterate sim one step delta_t = (now - self.last_time).to_sec() self.sim.step(delta_t) - # Publish game status + # publish game status status = MatchStatus() if self.sim.scored: if self.sim.winner == "A": @@ -282,7 +315,7 @@ class Simulator(object): status.status = MatchStatus.ONGOING self.status_pub.publish(status) - # Publish pose and odometry data + # publish pose and odom data if self.mode == SimulatorMode.REALISTIC: ball_msg = PoseWithCovarianceStamped() ball_msg.header.stamp = now @@ -301,8 +334,7 @@ class Simulator(object): car_msg = PoseWithCovarianceStamped() car_msg.header.stamp = now car_msg.header.frame_id = self.frame_id - car_pos, car_quat = self.sim.get_car_pose( - self.car_ids[car_name], add_noise=True) + car_pos, car_quat = self.sim.get_car_pose(self.car_ids[car_name], add_noise=True) car_msg.pose.pose.position.x = car_pos[0] car_msg.pose.pose.position.y = car_pos[1] car_msg.pose.pose.position.z = car_pos[2] @@ -311,7 +343,6 @@ class Simulator(object): car_msg.pose.pose.orientation.z = car_quat[2] car_msg.pose.pose.orientation.w = car_quat[3] self.car_pose_pubs[car_name].publish(car_msg) - ball_msg = Odometry() ball_msg.header.stamp = now ball_msg.header.frame_id = self.frame_id @@ -336,8 +367,7 @@ class Simulator(object): car_msg = Odometry() car_msg.header.stamp = now car_msg.header.frame_id = self.frame_id - car_pos, car_quat = self.sim.get_car_pose( - self.car_ids[car_name]) + car_pos, car_quat = self.sim.get_car_pose(self.car_ids[car_name]) car_msg.pose.pose.position.x = car_pos[0] car_msg.pose.pose.position.y = car_pos[1] car_msg.pose.pose.position.z = car_pos[2] @@ -345,8 +375,7 @@ class Simulator(object): car_msg.pose.pose.orientation.y = car_quat[1] car_msg.pose.pose.orientation.z = car_quat[2] car_msg.pose.pose.orientation.w = car_quat[3] - car_linear, car_angular = self.sim.get_car_velocity( - self.car_ids[car_name]) + car_linear, car_angular = self.sim.get_car_velocity(self.car_ids[car_name]) car_msg.twist.twist.linear.x = car_linear[0] car_msg.twist.twist.linear.y = car_linear[1] car_msg.twist.twist.linear.z = car_linear[2] @@ -358,5 +387,172 @@ class Simulator(object): self.last_time = now self.reset_lock.release() + def get_sim_param(self, path, returnValue=False, secondParam=None): + """ + @param secondParam: Specify if you want to pass in a second parameter to rospy. + @param path: A direct path to the variable. + @param returnValue: + True: None is returned if variable does not exist. + False: An error is thrown if variable does not exist. + @return: None or Exception. + """ + rospy_param = rospy.get_param(path,secondParam) + if not rospy_param: + if returnValue: + rospy.logfatal(f'invalid file path: {path}') + return None + else: + if '~' in path: + if secondParam is not None: + print(rospy.get_param(f'{path}',secondParam)) + return rospy.get_param(f'{path}',secondParam) + else: + print(rospy.get_param(f'{path}')) + return rospy.get_param(f'{path}') + + type_rospy = type(rospy_param) + + if type_rospy == dict: + if secondParam is None: + + min_param = (float)(rospy.get_param(f'{path}/min')) + max_param = (float)(rospy.get_param(f'{path}/max')) + else: + min_param = (float)(rospy.get_param(f'{path}/min', secondParam)) + max_param = (float)(rospy.get_param(f'{path}/max', secondParam)) + + if not max_param: + if returnValue: + rospy.logfatal(f'invalid file path: {path}/max') + return None + if not min_param: + if returnValue: + rospy.logfatal(f'invalid file path: {path}/min') + return None + # accounting for bugs in yaml file + if min_param > max_param: + return (float)(random.uniform(max_param, min_param)) + else: + return (float)(random.uniform(min_param, max_param)) + + elif type_rospy == float or type_rospy == int: + if secondParam is not None: + return rospy.get_param(path, secondParam) + else: + return rospy.get_param(path) + if returnValue: + rospy.logfatal(f'invalid file path: {path}') + return None + + + + def get_spawn_bounds(self): + fw = self.get_sim_param("/field/width") + fl = self.get_sim_param("/field/length") + spawn_height = self.get_sim_param('~spawn_height', secondParam=0.06) + wt = self.get_sim_param('/field/wall_thickness') + + spawn_bounds = [[-(fl / 2) + (2 * wt), (fl / 2) - (2 * wt)], + [-(fw / 2) + (2 * wt), (fw / 2) - (2 * wt)], + [spawn_height, spawn_height]] + return spawn_bounds + + def delete_all_cars_cb(self,_=None): + self.reset_lock.acquire() + self.delete_all_cars() + self.reset_cb() + self.reset_lock.release() + return DeleteCarResponse(True) + # no point in having it + def create_all_cars_cb(self,_=None): + self.reset_lock.acquire() + self.update_all_cars() + self.reset_cb() + self.reset_lock.release() + return CreateCarResponse(True) + + def delete_all_cars(self): + """Removes a car's publishers and subscribers and instance-parameters""" + car_configs = self.get_sim_param('~cars', secondParam=[]) + self.reset_lock.acquire() + for car_config in car_configs: + # shutdown if the car does not exit + if 'name' not in car_config: + rospy.signal_shutdown('no "name" set for car config in sim') + car_name = car_config['name'] + if 'randomize_pose' in car_config: + init_pose = None + # delete the car from the sim side + self.sim.delete_car(self.car_ids[car_name]) + + # delete car's subscribers + if car_name in self.car_effort_subs: + self.car_effort_subs[car_name].unregister() + del self.car_effort_subs[car_name] + if car_name in self.car_cmd_subs: + self.car_cmd_subs[car_name].unregister() + del self.car_cmd_subs[car_name] + # delete cars publishers + if car_name in self.car_odom_pubs: + self.car_odom_pubs[car_name].unregister() + del self.car_odom_pubs[car_name] + if car_name in self.car_pose_pubs: + self.car_pose_pubs[car_name].unregister() + del self.car_pose_pubs[car_name] + # reset all lists to be safe + self.car_ids = {} + self.car_pose_pubs = {} + self.car_odom_pubs = {} + self.car_effort_subs = {} + self.car_cmd_subs = {} + self.reset_lock.release() + + return DeleteCarResponse(True) + + def update_all_cars(self): + """Generates instance-parameters, Subscribers, Publishers for each car.""" + car_configs = self.get_sim_param('~cars', secondParam=[]) + print("=-=-=-=-=-=-=-=-=-=-=-=-=-=-=") + print(car_configs) + + for car_config in car_configs: + init_pose = self.get_sim_param('~cars/init_pose') + + if 'name' not in car_config: + rospy.signal_shutdown('no "name" set for car config in sim') + car_name = car_config['name'] + if 'randomize_pose' in car_config: + init_pose = None + + # means the car is already there and we only need to reset it + if car_name not in self.car_ids: + # the car does not exist so we will create it + # otherwise, we are only reseting the car's parameters which will happen in the sim.reset call + print("============sub and pubs===================") + print(self.car_properties) + self.car_ids[car_name] = self.sim.create_car( + 'car', init_pose=init_pose, noise=self.car_noise, car_props=self.car_properties) + car_id = self.car_ids[car_name] + # create the car's Subscribers + self.car_effort_subs[car_name] = rospy.Subscriber( + f'/cars/{car_name}/effort', ControlEffort, + self.effort_cb, callback_args=car_id) + self.car_cmd_subs[car_name] = rospy.Subscriber( + f'/cars/{car_name}/command', ControlCommand, + self.cmd_cb, callback_args=car_id) + # create the car's Publishers + if self.mode == SimulatorMode.REALISTIC: + self.car_pose_pubs[car_name] = rospy.Publisher( + f'/cars/{car_name}/pose_sync_early', + PoseWithCovarianceStamped, queue_size=1) + self.car_odom_pubs[car_name] = rospy.Publisher( + f'/cars/{car_name}/odom_truth', Odometry, queue_size=1) + elif self.mode == SimulatorMode.IDEAL: + self.car_odom_pubs[car_name] = rospy.Publisher( + f'/cars/{car_name}/odom', Odometry, queue_size=1) + + return CreateCarResponse(True) + + if __name__ == "__main__": Simulator() diff --git a/rktl_sim/src/simulator/car.py b/rktl_sim/src/simulator/car.py index 378bc9742..6bcf5bc3b 100644 --- a/rktl_sim/src/simulator/car.py +++ b/rktl_sim/src/simulator/car.py @@ -10,12 +10,37 @@ import math import numpy as np +# locations used for accessing car position and orientation +JOINT_IDS = (1, 0, 2) # X, Y, W +BASE_QUATERNION = [0., 0., 0.] + + class Car(object): + """Handles Car actions and instance based parameters.""" + def __init__(self, car_id, pos, orient, car_properties): + """Sets instance-based properties for a car and generates instance-based properties for a sim run.""" + self._MAX_CURVATURE = None + self._STEERING_RATE = None + self._THROTTLE_TAU = None + self._STEERING_THROW = None + self._LENGTH = None + self._MAX_SPEED = None + self._psi = None + self.cmd = None + self.joint_ids = None + self._v_rear = None self.id = car_id + self.init_pos = None + self.orient = None self.simulate_effort = car_properties['simulate_effort'] + self.set_properties(car_properties) - # physical constants + self.body_link_id = 1 # urdf configuration + + self.reset(pos, orient) + + def set_properties(self, car_properties): self._LENGTH = car_properties['length'] self._MAX_SPEED = car_properties['max_speed'] self._THROTTLE_TAU = car_properties['throttle_tau'] @@ -23,42 +48,39 @@ def __init__(self, car_id, pos, orient, car_properties): self._STEERING_RATE = car_properties['steering_rate'] self._MAX_CURVATURE = math.tan(self._STEERING_THROW) / self._LENGTH - # urdf configuration - self.body_link_id = 1 - - # system state - self._v_rear = 0.0 - self._psi = 0.0 - - # model configuration - p.resetBasePositionAndOrientation( - self.id, [0., 0., pos[2]], p.getQuaternionFromEuler([0., 0., 0.])) - - self.joint_ids = (1, 0, 2) # X, Y, W - p.resetJointState(self.id, self.joint_ids[0], targetValue=pos[0]) - p.resetJointState(self.id, self.joint_ids[1], targetValue=pos[1]) - p.resetJointState(self.id, self.joint_ids[2], targetValue=orient[2]) - - self.cmd = None - def setCmd(self, cmd): self.cmd = cmd def step(self, dt): + """ + Runs a simulation step of the car, moving it within the time step. + @param dt: The duration of the car time step. + """ if self.cmd is None: - return + print("command is none") + self.cmd = (1.0, 1.0) + + + """ + ==================================================== + ==================================================== + ==================================================== + ==================================================== + TODO: bug descritpion: the self.cmd is none. + this means that we cannot use any of the calculations, and therefore cannot update hte car + if the command is set, everything works + """ - # get current yaw angle _, orient = self.get_pose() theta = p.getEulerFromQuaternion(orient)[2] if self.simulate_effort: - # transfrom control input to reference angles and velocities + # transform control input to reference angles and velocities v_rear_ref = self.cmd[0] * self._MAX_SPEED psi_ref = self.cmd[1] * self._STEERING_THROW # update rear wheel velocity using 1st order model - self._v_rear = (self._v_rear - v_rear_ref) * math.exp(-dt/self._THROTTLE_TAU) + v_rear_ref + self._v_rear = (self._v_rear - v_rear_ref) * math.exp(-dt / self._THROTTLE_TAU) + v_rear_ref # update steering angle using massless acceleration to a fixed rate if abs(psi_ref - self._psi) < self._STEERING_RATE * dt: @@ -69,31 +91,36 @@ def step(self, dt): else: self._psi -= self._STEERING_RATE * dt - # using bicycle model, extrapolate future state + # extrapolate future state sing bicycle model x_dot = self._v_rear * math.cos(theta + math.atan(math.tan(self._psi) / 2.0)) * \ - math.sqrt(math.pow(math.tan(self._psi), 2.0) / 4.0 + 1.0) + math.sqrt(math.pow(math.tan(self._psi), 2.0) / 4.0 + 1.0) y_dot = self._v_rear * math.sin(theta + math.atan(math.tan(self._psi) / 2.0)) * \ - math.sqrt(math.pow(math.tan(self._psi), 2.0) / 4.0 + 1.0) + math.sqrt(math.pow(math.tan(self._psi), 2.0) / 4.0 + 1.0) omega = self._v_rear * math.tan(self._psi) / self._LENGTH else: body_vel = self.cmd[0] if abs(body_vel) > self._MAX_SPEED: body_vel = math.copysign(self._MAX_SPEED, body_vel) - body_curv = self.cmd[1] - if abs(body_curv) > self._MAX_CURVATURE: - body_curv = math.copysign(self._MAX_CURVATURE, body_curv) - + body_curve = self.cmd[1] + if abs(body_curve) > self._MAX_CURVATURE: + body_curve = math.copysign(self._MAX_CURVATURE, body_curve) + x_dot = body_vel * math.cos(theta) y_dot = body_vel * math.sin(theta) - omega = body_vel * body_curv + omega = body_vel * body_curve p.setJointMotorControlArray(self.id, self.joint_ids, - targetVelocities=(x_dot, y_dot, omega), - controlMode=p.VELOCITY_CONTROL, - forces=(5000, 5000, 5000)) + targetVelocities=(x_dot, y_dot, omega), + controlMode=p.VELOCITY_CONTROL, + forces=(5000, 5000, 5000)) def get_pose(self, noise=None): + """ + Randomizes and sets a new position for the car. + @param noise: The sensor noise and if it is present (None=no noise). + @return: The position and orientation of the car. + """ pos = p.getLinkState(self.id, self.body_link_id)[0] heading = p.getJointState(self.id, self.joint_ids[2])[0] orient = (0.0, 0.0, heading) @@ -103,24 +130,49 @@ def get_pose(self, noise=None): else: pos = np.random.normal(pos, noise['pos']) orient = np.random.normal(orient, noise['orient']) - - return (pos, p.getQuaternionFromEuler(orient)) + print("position: pos:",pos, "quaternion",p.getQuaternionFromEuler(orient)) + return pos, p.getQuaternionFromEuler(orient) def get_velocity(self): + """Returns the linear and angular velocity of the car.""" + link_state = p.getLinkState(self.id, self.body_link_id, computeLinkVelocity=1) orient = link_state[1] linear, angular = link_state[6:8] heading = p.getEulerFromQuaternion(orient)[2] r_inv = np.array([[math.cos(heading), -math.sin(heading), 0.], [math.sin(heading), math.cos(heading), 0.], - [0., 0., 1.]], dtype=np.float) + [0., 0., 1.]], dtype=np.float) linear = r_inv @ linear - return (linear, angular) + print("velocity: linear:",linear, "angular",angular) + return linear, angular def reset(self, pos, orient): + """Resets the car state with the new pose and orient.""" + self.init_pos = pos + self.orient = orient + self._v_rear = 0.0 self._psi = 0.0 + p.resetBasePositionAndOrientation(self.id, [0., 0., pos[2]], p.getQuaternionFromEuler(BASE_QUATERNION)) + print("reseting car to new pose and orient") + self.joint_ids = JOINT_IDS # X, Y, W p.resetJointState(self.id, self.joint_ids[0], targetValue=pos[0]) p.resetJointState(self.id, self.joint_ids[1], targetValue=pos[1]) p.resetJointState(self.id, self.joint_ids[2], targetValue=orient[2]) + + self.cmd = None + + def check_overlap(self, pos): + """ + Returns whether the position will overlap with the current car. + @param pos: The position of the other object. + @return: Boolean if the positions overlap (true = overlap). + """ + print("x",pos[0],"y",pos[1]) + print("x1",self.init_pos[0],"y1",self.init_pos[1]) + val =((pos[0] - self.init_pos[0]) * (pos[0] - self.init_pos[0])) + ((pos[1] - self.init_pos[1]) * (pos[1] - self.init_pos[1])) + print(val) + dist = math.sqrt(val) + return dist < self._LENGTH diff --git a/rktl_sim/src/simulator/sim.py b/rktl_sim/src/simulator/sim.py index 972054cd7..cbc473b87 100644 --- a/rktl_sim/src/simulator/sim.py +++ b/rktl_sim/src/simulator/sim.py @@ -14,23 +14,41 @@ # Local modules from simulator.car import Car + class Sim(object): - """Oversees components of the simulator""" + """ + Oversees instance-based parameters and objects of the simulator. + Cars, ball objects, goal position, etc. + """ class NoURDFError(Exception): - """Exception for when a URDF isn't provided""" pass def __init__(self, props, urdf_paths, spawn_bounds, render_enabled): + """ + Initializes the playing field, field properties, and field elements. + @param props: Connect the pybullet object based on the gui and direct. + @param urdf_paths: Configure: filed type, walls, floor, goal a and b. + @param spawn_bounds: Initialize cars list and other data related to them. + @param render_enabled: Use the loadURDF via p.loadURDF (loads the specific instruction). + """ + self.touchedLast = None + self.ball_noise = None + self._speed_bound = None + self.init_ball_pos = None + # determine if you want to display the simulation if render_enabled: self._client = p.connect(p.GUI) else: self._client = p.connect(p.DIRECT) self.props = props + + # urdf is used to encode kinetics and location of the object + # use it for setting the field type, walls, floor and the different goals self.urdf_paths = urdf_paths self.spawn_bounds = spawn_bounds - + # set the floor for the simulation zero_pos = [0.0, 0.0, 0.0] zero_orient = p.getQuaternionFromEuler([0.0, 0.0, 0.0]) self._plane_id = None @@ -42,7 +60,7 @@ def __init__(self, props, urdf_paths, spawn_bounds, render_enabled): self.configure_dynamics(self._plane_id, "floor") else: raise self.NoURDFError() - + # set the walls for the simulation if "walls" in urdf_paths: self._walls_id = p.loadURDF( urdf_paths["walls"], zero_pos, zero_orient, useFixedBase=1 @@ -50,7 +68,7 @@ def __init__(self, props, urdf_paths, spawn_bounds, render_enabled): self.configure_dynamics(self._walls_id, "walls") else: raise self.NoURDFError() - + # set the goals for the simulation self._goal_a_id = None self._goal_b_id = None if "goal_a" in urdf_paths and "goal_b" in urdf_paths: @@ -62,8 +80,7 @@ def __init__(self, props, urdf_paths, spawn_bounds, render_enabled): urdf_paths["goal_b"], zero_pos, zero_orient, useFixedBase=1 ) else: - return self.NoURDFError() - + raise self.NoURDFError() self._cars = {} self._car_data = {} self._ball_id = None @@ -77,9 +94,15 @@ def __init__(self, props, urdf_paths, spawn_bounds, render_enabled): p.setGravity(0, 0, -10) def configure_dynamics(self, body_id, body_type): + """ + Set the car's curvature and general car behavior. + @param body_id: The id of the object to be configured. + @param body_type: The specific type of object (ie ball,car,goal,etc). + @return: Error if not initialized. + """ if 'dynamics' not in self.props or \ - self.props['dynamics'] is None or \ - body_type not in self.props['dynamics']: + self.props['dynamics'] is None or \ + body_type not in self.props['dynamics']: return num_links = p.getNumJoints(body_id) @@ -87,7 +110,15 @@ def configure_dynamics(self, body_id, body_type): p.changeDynamics(body_id, i, **self.props['dynamics'][body_type]) def create_ball(self, urdf_name, init_pose=None, init_speed=None, - noise=None, init_vel=None): + noise=None, init_vel=None): + """ + @param urdf_name: The id for the specific pybullet object. + @param init_pose: The initial position of the ball (override randomization). + @param init_speed: The max speed of the ball (override known speed parameter). + @param noise: The noise and if it should be present in the location of the object. + @param init_vel: The initial velocity of the ball (override randomization). + @return: The ball id if the creation was successful. + """ if urdf_name in self.urdf_paths: zero_orient = p.getQuaternionFromEuler([0.0, 0.0, 0.0]) if init_pose: @@ -104,7 +135,7 @@ def create_ball(self, urdf_name, init_pose=None, init_speed=None, self.urdf_paths[urdf_name], ball_pos, zero_orient) self.configure_dynamics(self._ball_id, "ball") - # initize ball with some speed + # initialize the ball with some speed if init_vel: ball_vel = init_vel else: @@ -117,6 +148,7 @@ def create_ball(self, urdf_name, init_pose=None, init_speed=None, random.uniform(-self._speed_bound, self._speed_bound), 0.0, ] + print("\n\n\n\n\n\n\n\n\n\n==========here7================") p.resetBaseVelocity(self._ball_id, ball_vel, zero_orient) self.ball_noise = noise return self._ball_id @@ -124,11 +156,21 @@ def create_ball(self, urdf_name, init_pose=None, init_speed=None, return None def create_car(self, urdf_name, init_pose=None, noise=None, car_props=None): + """ + Creates instance based car properties(pose,vel,orient) and configures car dynamics. + @param urdf_name: The id for the specific pybullet object. + @param init_pose: The initial position of the ball (override randomization). + @param noise: The noise and if it should be present in the location of the object. + @param car_props: Configuration based car properties. + @return: The car id if the creation was successful. + """ + + # set the spawn location for the car if urdf_name in self.urdf_paths: zero_pos = [0.0, 0.0, 0.0] zero_orient = [0.0, 0.0, 0.0] - car_id = p.loadURDF(self.urdf_paths[urdf_name], zero_pos, - p.getQuaternionFromEuler(zero_orient)) + car_id = p.loadURDF(self.urdf_paths[urdf_name], zero_pos, + p.getQuaternionFromEuler(zero_orient)) if init_pose: if "pos" in init_pose: car_pos = init_pose["pos"] @@ -158,6 +200,8 @@ def create_car(self, urdf_name, init_pose=None, noise=None, car_props=None): car_orient, car_props ) + + # configures dynamics of the car self.configure_dynamics(car_id, "car") self._car_data[car_id] = { @@ -170,6 +214,11 @@ def create_car(self, urdf_name, init_pose=None, noise=None, car_props=None): return None def delete_car(self, car_id): + """ + Removes a car from being tracked in the _cars and _car_data lists. + @param car_id: The id of the car in the simulator class. + @return: Whether the deletion was successful. + """ if car_id not in self._cars: return False @@ -179,7 +228,10 @@ def delete_car(self, car_id): return True def step(self, dt): - """Advance one time-step in the sim.""" + """ + Moves the sim forward one timestep, checking if a goal is score to end the sim round. + @param dt: The change in time (delta-t) for this sim step. + """ if self._ball_id is not None: ball_contacts = p.getContactPoints(bodyA=self._ball_id) for contact in ball_contacts: @@ -195,12 +247,13 @@ def step(self, dt): # PyBullet steps at 240hz p_dt = 1.0 / 240.0 for _ in range(round(dt / p_dt)): - # Step kinematic objects independently, at max possible rate + # step kinematic objects independently, at max possible rate for car in self._cars.values(): car.step(p_dt) p.stepSimulation() def get_car_pose(self, id, add_noise=False): + if id not in self._cars: return None @@ -211,6 +264,7 @@ def get_car_pose(self, id, add_noise=False): return self._cars[id].get_pose(noise=None) def get_car_velocity(self, id): + """Returns a tuple of linear and angular velocity for the car.""" if id not in self._cars: return None @@ -223,6 +277,7 @@ def set_car_command(self, id, cmd): return self._cars[id].setCmd(cmd) def get_ball_pose(self, add_noise=False): + """@param add_noise: State whether you want noise to get the ball position (default=False).""" if self._ball_id is None: return None pos, _ = p.getBasePositionAndOrientation(self._ball_id) @@ -240,11 +295,70 @@ def get_ball_velocity(self): return None return p.getBaseVelocity(self._ball_id) - def reset(self): + def reset(self, spawn_bounds, car_properties, ball_init_pose, ball_init_speed): + """ + Resets the ball, score, winner, spawn bounds, cars and ball. + @param spawn_bounds: The new spawn bounds. + @param car_properties: The new car properties. + """ self.scored = False self.winner = None self.touched_last = None + if ball_init_pose is not None: self.init_ball_pos = ball_init_pose + if ball_init_speed is not None: self._speed_bound = ball_init_speed + self.spawn_bounds = spawn_bounds + self.reset_ball() + for car in self._cars.values(): + self.reset_car(car, car_properties) + + + def reset_car(self, car, car_properties): + """ + Loops over the cars and generates new initial positions (if they were not specified). + @param car_properties: The new car config properties. + """ + # reset the car properties in advance + car.set_properties(car_properties) + car_pos = self._car_data[car.id]["init_pos"] + car_orient = self._car_data[car.id]["init_orient"] + + if car_pos is None: + car_pos = self.generate_new_car_pos() + + while self.check_if_pos_overlap(car_pos): + print("\n\n\n\n\n\n\n\n\noverlap generation") + car_pos = self.generate_new_car_pos() + + if car_orient is None: + car_orient = [0, 0, random.uniform(0, 2 * math.pi)] + car.reset(car_pos, car_orient) + + + def check_if_pos_overlap(self, car_pos): + """ + Checks if two cars spawn bounds overlap with each other. + @param car_pos: The position of the car. + @return: Whether overlap happens (true = need to generate new bounds). + """ + for car in self._cars.values(): + overlap = car.check_overlap(car_pos) + if overlap: + return True + + return False + + + def generate_new_car_pos(self): + car_pos = [random.uniform(self.spawn_bounds[0][0], self.spawn_bounds[0][1]), + random.uniform( + self.spawn_bounds[1][0], self.spawn_bounds[1][1]), + random.uniform(self.spawn_bounds[2][0], self.spawn_bounds[2][1])] + return car_pos + + + def reset_ball(self): + if self._ball_id is not None: ball_pos = self.init_ball_pos if ball_pos is None: @@ -256,25 +370,9 @@ def reset(self): p.resetBasePositionAndOrientation( self._ball_id, ball_pos, p.getQuaternionFromEuler([0, 0, 0]) ) - ball_vel = [ random.uniform(-self._speed_bound, self._speed_bound), random.uniform(-self._speed_bound, self._speed_bound), 0.0, ] p.resetBaseVelocity(self._ball_id, ball_vel, [0, 0, 0]) - - for car in self._cars.values(): - car_pos = self._car_data[car.id]["init_pos"] - car_orient = self._car_data[car.id]["init_orient"] - - if car_pos is None: - car_pos = [random.uniform(self.spawn_bounds[0][0], self.spawn_bounds[0][1]), - random.uniform( - self.spawn_bounds[1][0], self.spawn_bounds[1][1]), - random.uniform(self.spawn_bounds[2][0], self.spawn_bounds[2][1])] - - if car_orient is None: - car_orient = [0, 0, random.uniform(0, 2 * math.pi)] - - car.reset(car_pos, car_orient) diff --git a/rktl_sim/urdf/walls.urdf b/rktl_sim/urdf/walls.urdf deleted file mode 100644 index df8e676a8..000000000 --- a/rktl_sim/urdf/walls.urdf +++ /dev/null @@ -1,126 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -