diff --git a/.rosinstall b/.rosinstall index d09ee0533..dc3f3289d 100755 --- a/.rosinstall +++ b/.rosinstall @@ -18,10 +18,10 @@ uri: https://github.com/ignc-research/stable-baselines3 version: master -- git: - local-name: ../forks/navigation/local_planner/teb - uri: https://github.com/rst-tu-dortmund/teb_local_planner - version: melodic-devel +#- git: +# local-name: ../forks/navigation/local_planner/teb +# uri: https://github.com/rst-tu-dortmund/teb_local_planner +# version: melodic-devel - git: local-name: ../forks/navigation/local_planner/mpc uri: https://github.com/rst-tu-dortmund/mpc_local_planner @@ -30,3 +30,7 @@ local-name: ../forks/navigation/msgs/ford uri: https://bitbucket.org/acl-swarm/ford_msgs/src/master/ version: master +- git: + local-name: ../forks/stable-baselines3 + uri: https://github.com/tuananhroman/stable-baselines3 + version: master diff --git a/Docker/Dockerfile b/Docker/Dockerfile new file mode 100644 index 000000000..477644770 --- /dev/null +++ b/Docker/Dockerfile @@ -0,0 +1,79 @@ +################################################################# +# Dockerfile for Arena-Rosnav + +# URL: https://github.com/ignc-research/arena-rosnav + +# Based on Ubuntu 18.04 + ROS-Melodic-Desktop-Full Version + +# Author: Yi Sun + +################################################################# + +# This image includes additional meta-packages such for desktop installations than offical image +FROM osrf/ros:melodic-desktop-full + +# Change the default shell from /bin/sh to /bin/bash +SHELL ["/bin/bash","-c"] + +# 1. Install additinal pkgs +RUN apt-get -y update && apt-get install -y \ + libqt4-dev \ + libopencv-dev \ + liblua5.2-dev \ + screen \ + python3.6 \ + python3.6-dev \ + libpython3.6-dev \ + python3-catkin-pkg-modules \ + python3-rospkg-modules \ + python3-empy \ + python3-setuptools \ + python3-pip \ + ros-melodic-navigation \ + ros-melodic-teb-local-planner \ + ros-melodic-mpc-local-planner \ +&& echo $'\n\ +source /opt/ros/melodic/setup.sh' >> /root/.bashrc + +# 2. Prepare virtual environment +RUN pip3 install --upgrade pip \ +&& pip3 install virtualenv virtualenvwrapper\ +&& cd /root \ +&& mkdir .python_env \ +&& echo $'\n\ +export WORKON_HOME=/root/.python_env \n\ +export VIRTUALENVWRAPPER_PYTHON=/usr/bin/python3 \n\ +export VIRTUALENVWRAPPER_VIRTUALENV=/usr/local/bin/virtualenv \n\ +source /usr/local/bin/virtualenvwrapper.sh' >> /root/.bashrc + +ENV WORKON_HOME=/root/.python_env \ + VIRTUALENVWRAPPER_PYTHON=/usr/bin/python3 \ + VIRTUALENVWRAPPER_VIRTUALENV=/usr/local/bin/virtualenv + +# 3. Create python virtualenv and install dependencies +RUN . /usr/local/bin/virtualenvwrapper.sh \ +&& mkvirtualenv --python=python3.6 rosnav \ +&& workon rosnav \ +&& pip install --extra-index-url https://rospypi.github.io/simple/ rospy rosbag tf tf2_ros --ignore-installed \ +&& pip install pyyaml catkin_pkg netifaces pathlib \ +&& pip --no-cache-dir install stable-baselines3 + +# 4. Install Arena-Rosnav repo and compile +RUN git clone --depth 1 https://github.com/ignc-research/arena-rosnav /root/catkin_ws/src/arena-rosnav \ +&& . /usr/local/bin/virtualenvwrapper.sh \ +&& . /opt/ros/melodic/setup.sh \ +&& workon rosnav \ +&& pip install -r /root/catkin_ws/src/arena-rosnav/arena_navigation/arena_local_planner/model_based/cadrl_ros/requirements_cadrl.txt \ +&& pip install gym \ +&& cd /root/catkin_ws/src/arena-rosnav \ +&& rosws update \ +&& . /opt/ros/melodic/setup.sh \ +&& cd /root/catkin_ws \ +&& catkin_make -DPYTHON_EXECUTABLE=/usr/bin/python3 \ +&& echo $'source /root/catkin_ws/devel/setup.bash' >> /root/.bashrc + +# 5. Install geometry2 pkg +RUN . /root/.bashrc \ +&& . /opt/ros/melodic/setup.sh \ +&& cd /root/catkin_ws/src/arena-rosnav/ \ +&& . geometry2_install.sh \ No newline at end of file diff --git a/Docker/README.md b/Docker/README.md new file mode 100644 index 000000000..6e732a596 --- /dev/null +++ b/Docker/README.md @@ -0,0 +1,71 @@ +## Install docker +Ubuntu: + https://cnvrg.io/how-to-setup-docker-and-nvidia-docker-2-0-on-ubuntu-18-04/ + + Mac: +https://docs.docker.com/docker-for-mac/install/ + + Windows: + https://docs.docker.com/docker-for-windows/install/ + +### Install docker-compose +https://docs.docker.com/compose/install/ + +## Build image and setup container +1. clone the whole repositary +``` +git clone https://github.com/ignc-research/arena-rosnav +``` +2. Open local terminal under the path where you just clone the repo +3. Compose the docker +``` +docker-compose up -d +``` + +## Start /Enter /Stop the container +1. Check if the arena-rosnav and novnc container is already started +``` +docker ps +``` +2. Start container (under the path of arena-rosnav) +``` +docker-compose start +``` +3. Enter the ros contaier +``` +docker exec -it arena-rosnav bash +``` +4. Stop the container (under the path of arena-rosnav) +``` +docker-compose stop +``` + +## Show the GUI app in ROS by browser +1. Open 'http://localhost:8080/vnc.html' +2. You can adjust the window size to automatic local scale or set fullscreen though the side bar on the left + +## Test +1. After entering the container by `docker exec -it arena-rosnav bash` activate the venv by +``` +workon rosnav +``` +2. Roslaunch in venv +``` +roslaunch arena_bringup start_arena_flatland.launch train_mode:=false use_viz:=true local_planner:=dwa map_file:=map1 obs_vel:=0.3 + +``` +3. Open http://localhost:8080/vnc.html you will see the rviz window in browser +3. By setting a 2D Nav Goal manually on white goal point you will see the robot move to the goal automatically + +## Develop in VS Code +1. Install plugin Remote - Containers in VS Code +2. Open the arena-rosnav folder in VS Code +3. Click the green icon in the lower left corner (or type `F1`) +4. Select `Remote-Containers: Reopen in Container` +5. Select `From docker-compose.yml` +6. Select `ros` +7. Modify the files which are automatically created by VS Code + * devcontainer.json in .devcontainer folder + You can change the workspaceFolder to "workspaceFolder": "/root/catkin_ws/src/arena-rosnav" + * docker-compose.yml in .devcontainer folder + You can comment the "volumes:- .:/workspace:cached", which is redundant diff --git a/Docker/novnc.env b/Docker/novnc.env new file mode 100644 index 000000000..81e958c88 --- /dev/null +++ b/Docker/novnc.env @@ -0,0 +1,3 @@ +DISPLAY_WIDTH=1920 +DISPLAY_HEIGHT=1080 +RUN_XTERM=no \ No newline at end of file diff --git a/Docker/ros.env b/Docker/ros.env new file mode 100644 index 000000000..3e841ee9e --- /dev/null +++ b/Docker/ros.env @@ -0,0 +1,4 @@ +# ROS_MASTER_URI=[IP or HOSTNAME]:11311 +# ROS_HOSTNAME=ros +# TB3_MODEL=waffle_pi +# TURTLEBOT3_MODEL=waffle_pi \ No newline at end of file diff --git a/arena_bringup/launch/plan_fsm_param.yaml b/arena_bringup/launch/plan_fsm_param.yaml old mode 100755 new mode 100644 diff --git a/arena_bringup/launch/start_arena_flatland.launch b/arena_bringup/launch/start_arena_flatland.launch old mode 100755 new mode 100644 diff --git a/arena_bringup/launch/sublaunch/amcl.launch b/arena_bringup/launch/sublaunch/amcl.launch new file mode 100644 index 000000000..5535257dd --- /dev/null +++ b/arena_bringup/launch/sublaunch/amcl.launch @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/arena_bringup/launch/sublaunch/fake_localization.launch b/arena_bringup/launch/sublaunch/fake_localization.launch new file mode 100644 index 000000000..1c0165efc --- /dev/null +++ b/arena_bringup/launch/sublaunch/fake_localization.launch @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + \ No newline at end of file diff --git a/arena_bringup/launch/sublaunch/flatland_simulator.launch b/arena_bringup/launch/sublaunch/flatland_simulator.launch new file mode 100644 index 000000000..595ef0492 --- /dev/null +++ b/arena_bringup/launch/sublaunch/flatland_simulator.launch @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/arena_bringup/launch/sublaunch/move_base.launch b/arena_bringup/launch/sublaunch/move_base.launch new file mode 100644 index 000000000..b0fd65180 --- /dev/null +++ b/arena_bringup/launch/sublaunch/move_base.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/arena_bringup/launch/sublaunch/move_base/move_base_cadrl.launch b/arena_bringup/launch/sublaunch/move_base/move_base_cadrl.launch new file mode 100644 index 000000000..6177c3f4e --- /dev/null +++ b/arena_bringup/launch/sublaunch/move_base/move_base_cadrl.launch @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/arena_bringup/launch/sublaunch/move_base/move_base_dwa.launch b/arena_bringup/launch/sublaunch/move_base/move_base_dwa.launch new file mode 100644 index 000000000..092893529 --- /dev/null +++ b/arena_bringup/launch/sublaunch/move_base/move_base_dwa.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/arena_bringup/launch/sublaunch/move_base/move_base_mpc.launch b/arena_bringup/launch/sublaunch/move_base/move_base_mpc.launch new file mode 100644 index 000000000..bc809000b --- /dev/null +++ b/arena_bringup/launch/sublaunch/move_base/move_base_mpc.launch @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/arena_bringup/launch/sublaunch/move_base/move_base_teb.launch b/arena_bringup/launch/sublaunch/move_base/move_base_teb.launch new file mode 100644 index 000000000..27c03fbc5 --- /dev/null +++ b/arena_bringup/launch/sublaunch/move_base/move_base_teb.launch @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/arena_bringup/launch/sublaunch/plan_manager.launch b/arena_bringup/launch/sublaunch/plan_manager.launch new file mode 100644 index 000000000..0a4d057ae --- /dev/null +++ b/arena_bringup/launch/sublaunch/plan_manager.launch @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/arena_bringup/launch/sublaunch/task_generator.launch b/arena_bringup/launch/sublaunch/task_generator.launch new file mode 100644 index 000000000..0b5a26616 --- /dev/null +++ b/arena_bringup/launch/sublaunch/task_generator.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/arena_bringup/launch/sublaunch_testing/fake_localization.launch b/arena_bringup/launch/sublaunch_testing/fake_localization.launch old mode 100755 new mode 100644 diff --git a/arena_bringup/launch/sublaunch_testing/flatland_simulator.launch b/arena_bringup/launch/sublaunch_testing/flatland_simulator.launch old mode 100755 new mode 100644 diff --git a/arena_bringup/launch/sublaunch_testing/task_generator.launch b/arena_bringup/launch/sublaunch_testing/task_generator.launch old mode 100755 new mode 100644 index e7f9a6421..964906976 --- a/arena_bringup/launch/sublaunch_testing/task_generator.launch +++ b/arena_bringup/launch/sublaunch_testing/task_generator.launch @@ -1,7 +1,7 @@ - + diff --git a/arena_bringup/launch/sublaunch_testing/timed_space_planner_fsm.launch b/arena_bringup/launch/sublaunch_testing/timed_space_planner_fsm.launch old mode 100755 new mode 100644 diff --git a/arena_bringup/launch/sublaunch_training/fake_localization.launch b/arena_bringup/launch/sublaunch_training/fake_localization.launch old mode 100755 new mode 100644 diff --git a/arena_bringup/launch/sublaunch_training/flatland_simulator.launch b/arena_bringup/launch/sublaunch_training/flatland_simulator.launch old mode 100755 new mode 100644 diff --git a/arena_bringup/launch/sublaunch_training/timed_space_planner_fsm.launch b/arena_bringup/launch/sublaunch_training/timed_space_planner_fsm.launch old mode 100755 new mode 100644 diff --git a/arena_bringup/launch/visualization_training.launch b/arena_bringup/launch/visualization_training.launch old mode 100755 new mode 100644 diff --git a/arena_navigation/arena_local_planner/evaluation/empty_20_vel_02.pdf b/arena_navigation/arena_local_planner/evaluation/empty_20_vel_02.pdf new file mode 100644 index 000000000..73e3dca78 Binary files /dev/null and b/arena_navigation/arena_local_planner/evaluation/empty_20_vel_02.pdf differ diff --git a/arena_navigation/arena_local_planner/evaluation/scenario_eval_linh.py b/arena_navigation/arena_local_planner/evaluation/scenario_eval_linh.py new file mode 100644 index 000000000..35e26f24b --- /dev/null +++ b/arena_navigation/arena_local_planner/evaluation/scenario_eval_linh.py @@ -0,0 +1,325 @@ +# +import rosbag +import bagpy +from bagpy import bagreader +import pandas as pd +import matplotlib.pyplot as plt +from matplotlib.lines import Line2D +from matplotlib.pyplot import figure +from matplotlib.patches import Polygon +import matplotlib.cm as cm +import numpy as np +import math +import seaborn as sb +import rospy +from visualization_msgs.msg import Marker, MarkerArray +import pathlib +import os + +# +class newBag(): + def __init__(self, planner,file_name, bag_name): + + odom_topic="/sensorsim/police/odom" + collision_topic="/sensorsim/police/collision" + self.bag = bagreader(bag_name) + eps = self.split_runs(odom_topic, collision_topic) + self.evalPath(planner,file_name,eps) + self.nc_total = 0 + # self.make_heat_map(1) + + + + + def make_txt(self,file,msg,ron="a"): + # f = open(file, ron) + # f.write(msg) + # f.close() + return + + def make_heat_map(self,xy): + # fig, ax = plt.subplots(figsize=(6, 7)) + # data = np.zeros((33, 25)) + + # for xya in xy: + # for pos in xya: + # y = -int(round(pos[0], 0)) + # x = int(round(pos[1], 0)) + # data[x,y] += 1 + # # # print(data[x,y]) + # # heat_map = sb.heatmap(data, cmap="YlGnBu") + # # heat_map.invert_yaxis() + # # plt.show() + + # # delta = 0.025 + # # x = y = np.arange(-3.0, 3.0, delta) + # # X, Y = np.meshgrid(x, y) + # # Z1 = np.exp(-X**2 - Y**2) + # # Z2 = np.exp(-(X - 1)**2 - (Y - 1)**2) + # # Z = (Z1 - Z2) * 2 + + # im = ax.imshow(data, interpolation='bilinear', cmap=cm.RdYlGn, + # origin='lower', extent=[-3, 3, -3, 3] + # ,vmax=abs(data).max(), vmin=-abs(data).max()) + # plt.show() + return + + def split_runs(self,odom_topic,collision_topic): + # get odometry + + odom_csv = self.bag.message_by_topic(odom_topic) + df_odom = pd.read_csv(odom_csv, error_bad_lines=False) + + # get collision time + try: + # check if collision was published + collision_csv = self.bag.message_by_topic(collision_topic) + df_collision = pd.read_csv(collision_csv, error_bad_lines=False) + except Exception as e: + # otherwise run had zero collisions + df_collision = [] + t_col = [] + + for i in range(len(df_collision)): + t_col.append(df_collision.loc[i, "Time"]) + self.nc_total = len(t_col) + # get reset time + reset_csv = self.bag.message_by_topic("/scenario_reset") + df_reset = pd.read_csv(reset_csv, error_bad_lines=False) + t_reset = [] + for i in range(len(df_reset)): + t_reset.append(df_reset.loc[i, "Time"]) + # print(t_reset) + + pose_x = [] + pose_y = [] + t = [] + bags = {} + # run idx + n = 0 + # collsion pos + col_xy = [] + nc = 0 + + + for i in range(len(df_odom)): + current_time = df_odom.loc[i, "Time"] + x = df_odom.loc[i, "pose.pose.position.x"] + y = df_odom.loc[i, "pose.pose.position.y"] + reset = t_reset[n] + # check if respawned + # if current_time > reset-6 and n < len(t_reset)-1 and x<0: + global start_x + if current_time > reset-6 and n < len(t_reset)-1 and x < start_x: + n += 1 + # store the run + bags["run_"+str(n)] = [pose_x,pose_y,t,col_xy] + + # reset + pose_x = [] + pose_y = [] + t = [] + col_xy = [] + + if len(pose_x) > 0: + pose_x.append(x) + pose_y.append(y) + elif x < start_x: + # print("run_"+str(n)) + # print("x:",x,"y",y) + pose_x.append(x) + pose_y.append(y) + + t.append(current_time) + # get trajectory + + # check for col + if len(t_col) > nc: + if current_time >= t_col[nc]: + col_xy.append([x,y]) + nc += 1 + + if "run_1" in bags: + bags.pop("run_1") + return bags + + def average(self,lst): + if len(lst)>0: + return sum(lst) / len(lst) + else: + return 0 + + def print_patches(self,xya,clr): + global ax, lgnd + + for run_a in xya: + for col_xy in run_a: + circle = plt.Circle((-col_xy[1], col_xy[0]), 0.3, color=clr, fill = True) + ax.add_patch(circle) + + # a = 0.5 + # h = a*math.sqrt(3)/2 + # patches = [] + # polyg_pts = [] + # polyg_pts.append([a/2, -h/2]) + # polyg_pts.append([0, h/2]) + # polyg_pts.append([-a/2, -h/2]) + + # polygon = Polygon(polyg_pts, True) + # plt.polygon + # patches.append(polygon) + + + # plt.Polygon([pt1,pt2], closed=None, fill=None, edgecolor='r') + + # ax.add_patch(polygon) + + def evalPath(self, planner, file_name, bags): + col_xy = [] + global ax, lgnd + + durations = [] + trajs = [] + vels = [] + + self.make_txt(file_name, "\n"+"Evaluation of "+planner+":") + + for run in bags: + if run != "nrun_30": + pose_x = bags[run][0] + pose_y = bags[run][1] + + x = np.array(pose_x) + y = -np.array(pose_y) + + t = bags[run][2] + + dist_array = (x[:-1]-x[1:])**2 + (y[:-1]-y[1:])**2 + path_length = np.sum(np.sqrt(dist_array)) + # for av + trajs.append(path_length) + if path_length > 0: + ax.plot(y, x, lgnd[planner], alpha=0.2) + + duration = t[len(t)-1] - t[0] + # for av + durations.append(duration) + av_vel = path_length/duration + # for av + vels.append(av_vel) + + n_col = len(bags[run][3]) + + duration = round(duration,3) + path_length = round(path_length,3) + av_vel = round(av_vel,3) + + cr = run+": "+str([duration, path_length, av_vel, n_col]) + print(cr) + self.make_txt(file_name, "\n"+cr) + + col_xy.append(bags[run][3]) + + + msg_planner = "\n---------------------- "+planner+" summary: ----------------------" + msg_at = "\naverage time: "+str(round(self.average(durations),3))+" s" + msg_ap = "\naverage path length: "+str(round(self.average(trajs),3))+" m" + msg_av = "\naverage velocity: "+str(round(self.average(vels),3))+" m/s" + msg_col = "\ntotal number of collisions: "+str(self.nc_total)+"\n" + + print("---------------------- "+planner+" ----------------------") + print("average time: ", round(self.average(durations),3), "s") + print("average path length: ", round(self.average(trajs),3), "m") + print("average velocity: ", round(self.average(vels),3), " m/s") + print("total collisions: ", str(self.nc_total)) + + self.make_txt(file_name,msg_planner) + self.make_txt(file_name,msg_at) + self.make_txt(file_name,msg_ap) + self.make_txt(file_name,msg_av) + self.make_txt(file_name,msg_col) + + self.print_patches(col_xy,lgnd[planner]) + self.make_heat_map(col_xy) + +def eval_all(a,map,ob,vel): + global ax, sm, lgnd + fig, ax = plt.subplots(figsize=(6, 7)) + + + mode = map + "_" + ob + "_" + vel + fig.suptitle(mode, fontsize=16) + if not "empty" in map: + img = plt.imread("map_small.png") + ax.imshow(img, extent=[-19.7, 6, -6, 27]) + # plt.scatter(sm[1], sm[0]) + + + cur_path = str(pathlib.Path().absolute()) + # print(cur_path) + for planner in a: + for file in os.listdir(cur_path+"/bags/scenarios/"+planner): + if file.endswith(".bag") and map in file and ob in file and vel in file: + print("bags/scenarios/"+planner+"/"+file) + fn = planner + mode + + newBag(planner, fn, "bags/scenarios/"+planner+"/"+file) + + + legend_elements = [] + for l in lgnd: + el = Line2D([0], [0], color=lgnd[l], lw=4, label=l) + legend_elements.append(el) + + ax.legend(handles=legend_elements, loc=0) + plt.savefig(mode+'.pdf') + +def getMap(msg): + global ax, sm + points_x = [] + points_y = [] + # print(msg.markers[0]) + for p in msg.markers[0].points: + points_x.append(p.x-6) + points_y.append(-p.y+6) + # plt.scatter(points_y, points_x) + sm = [points_x, points_y] + # plt.show() + +def run(): + global ax, start_x, sm, lgnd + + lgnd = {} + lgnd["arena"] = "tab:purple" + lgnd["cadrl"] = "tab:red" + lgnd["dwa"] = "tab:blue" + lgnd["mpc"] = "tab:green" + lgnd["teb"] = "tab:orange" + # static map + # rospy.init_node("eval", anonymous=False) + # rospy.Subscriber('/flatland_server/debug/layer/static',MarkerArray, getMap) + + + start_x = 0.5 + + # 20 01 + eval_all(["mpc"],"empty","20","vel_02") + + + # # empty map + # # 5 01 + # eval_all(["arena","cadrl","dwa","mpc","teb"],"empty","5","vel_01.") + # # 10 01 + # eval_all(["arena","cadrl","dwa","mpc","teb"],"empty","10","vel_01") + # # 20 01 + # eval_all(["arena","cadrl","dwa","mpc","teb"],"empty","20","vel_01") + + + + plt.show() + # rospy.spin() + + +if __name__=="__main__": + run() + diff --git a/arena_navigation/arena_local_planner/learning_based/arena_local_planner_drl/rl_agent/envs/wp3_env.py b/arena_navigation/arena_local_planner/learning_based/arena_local_planner_drl/rl_agent/envs/wp3_env.py new file mode 100644 index 000000000..d7901fb66 --- /dev/null +++ b/arena_navigation/arena_local_planner/learning_based/arena_local_planner_drl/rl_agent/envs/wp3_env.py @@ -0,0 +1,251 @@ +#! /usr/bin/env python +from operator import is_ +from random import randint +import gym +from gym import spaces +from gym.spaces import space +from typing import Union +from stable_baselines3.common.env_checker import check_env +import yaml +from rl_agent.utils.observation_collector import ObservationCollector +from rl_agent.utils.reward import RewardCalculator +from rl_agent.utils.debug import timeit +from task_generator.tasks import ABSTask +import numpy as np +import rospy +from geometry_msgs.msg import Twist, PoseStamped, Pose2D +from flatland_msgs.srv import StepWorld, StepWorldRequest +from nav_msgs.msg import Odometry, Path +import time +import math +# for transformations +from tf.transformations import * + +from arena_plan_msgs.msg import RobotState, RobotStateStamped + +class wp3Env(gym.Env): + """Custom Environment that follows gym interface""" + + def __init__(self, task: ABSTask, robot_yaml_path: str, settings_yaml_path: str, is_action_space_discrete, safe_dist: float = None, goal_radius: float = 0.1, max_steps_per_episode=100): + """Default env + Flatland yaml node check the entries in the yaml file, therefore other robot related parameters cound only be saved in an other file. + TODO : write an uniform yaml paser node to handel with multiple yaml files. + + + + Args: + task (ABSTask): [description] + robot_yaml_path (str): [description] + setting_yaml_path ([type]): [description] + reward_fnc (str): [description] + is_action_space_discrete (bool): [description] + safe_dist (float, optional): [description]. Defaults to None. + goal_radius (float, optional): [description]. Defaults to 0.1. + """ + super(wp3Env, self).__init__() + # Define action and observation space + # They must be gym.spaces objects + + self._is_action_space_discrete = is_action_space_discrete + self.setup_by_configuration(robot_yaml_path, settings_yaml_path) + # observation collector + self.observation_collector = ObservationCollector( + self._laser_num_beams, self._laser_max_range) + self.observation_space = self.observation_collector.get_observation_space() + + # reward calculator + if safe_dist is None: + safe_dist = 1.5*self._robot_radius + + self.reward_calculator = RewardCalculator( + safe_dist=1.1*self._robot_radius, goal_radius=goal_radius) + + #subscriber to infer callback and out of sleep loop + #sub robot position and sub global goal + self._robot_state_sub = rospy.Subscriber('/odom', Odometry, self.cbRobotPosition) + + self._wp4train_sub = rospy.Subscriber('plan_manager/wp4train', PoseStamped, self.cbwp4train) + self._globalPlan_sub = rospy.Subscriber('plan_manager/globalPlan', Path, self.cbglobalPlan) + self._wp4train_reached =False + # action agent publisher + self.agent_action_pub = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size=1) + + # service clients + self._is_train_mode = rospy.get_param("train_mode") + if self._is_train_mode: + self._service_name_step = '/step_world' + self._sim_step_client = rospy.ServiceProxy( + self._service_name_step, StepWorld) + self.task = task + self.range_circle = 1.5 + self._steps_curr_episode = 0 + self._max_steps_per_episode = max_steps_per_episode + + #global variables for subscriber callbacks + self._robot_pose = Pose2D() + self._globalPlan = Path() + self._wp4train = PoseStamped() + + self._previous_time = 0 + self._step_counter = 0 + # # get observation + # obs=self.observation_collector.get_observations() + + def cbRobotPosition(self,msg): + self._robot_pose.x = msg.pose.pose.position.x + self._robot_pose.y = msg.pose.pose.position.y + + def cbglobalPlan(self,msg): + self._globalPlan = msg + + def cbwp4train(self,msg): + self._wp4train = msg + + def setup_by_configuration(self, robot_yaml_path: str, settings_yaml_path: str): + """get the configuration from the yaml file, including robot radius, discrete action space and continuous action space. + + Args: + robot_yaml_path (str): [description] + """ + with open(robot_yaml_path, 'r') as fd: + robot_data = yaml.safe_load(fd) + # get robot radius + for body in robot_data['bodies']: + if body['name'] == "base_footprint": + for footprint in body['footprints']: + if footprint['type'] == 'circle': + self._robot_radius = footprint.setdefault( + 'radius', 0.3)*1.04 + if footprint['radius']: + self._robot_radius = footprint['radius']*1.04 + # get laser related information + for plugin in robot_data['plugins']: + if plugin['type'] == 'Laser': + laser_angle_min = plugin['angle']['min'] + laser_angle_max = plugin['angle']['max'] + laser_angle_increment = plugin['angle']['increment'] + self._laser_num_beams = int( + round((laser_angle_max-laser_angle_min)/laser_angle_increment)+1) + self._laser_max_range = plugin['range'] + + with open(settings_yaml_path, 'r') as fd: + setting_data = yaml.safe_load(fd) + if self._is_action_space_discrete: + # self._discrete_actions is a list, each element is a dict with the keys ["name", 'linear','angular'] + self._discrete_acitons = setting_data['robot']['discrete_actions'] + self.action_space = spaces.Discrete( + len(self._discrete_acitons)) + else: + angular_range = setting_data['robot']['continuous_actions']['angular_range'] + self.action_space = spaces.Box(low=np.array([angular_range[0]]), + high=np.array([angular_range[1]]), dtype=np.float) + + def _pub_action(self, action): + + #todo instead of counter, wait for robot pose = wp4train pose + if self._step_counter - self._previous_time > 80: + self._previous_time = self._step_counter + # _, obs_dict = self.observation_collector.get_observations() + # robot_position = obs_dict['robot_position'] + action_msg = PoseStamped() + #todo consider the distance to global path when choosing next optimal waypoint + #caluclate range with current robot position and transform into posestamped message + # robot_position+(angle*range) + #send a goal message as action, remeber to normalize the quaternions (put orientationw as 1) and set the frame id of the goal! + angle_grad = math.degrees(action[0]) # e.g. 90 degrees + action_msg.pose.position.x = self._wp4train.pose.position.x + (self.range_circle*math.cos(angle_grad)) + action_msg.pose.position.y = self._wp4train.pose.position.y + (self.range_circle*math.sin(angle_grad)) + dist_robot_goal = np.array([(self.range_circle*math.cos(angle_grad)) ,(self.range_circle*math.sin(angle_grad))]) + dist_norm = np.linalg.norm(dist_robot_goal) + action_msg.pose.orientation.w = 1 + action_msg.header.frame_id ="map" + self.agent_action_pub.publish(action_msg) + print(angle_grad) + #rospy.sleep(1) + #print("chosen action: {0}, deegrees: {1}, sum: {2}, cos(): {3}, robot_position: {4}".format(action[0], math.degrees(action[0]), (self.range_circle*np.cos(math.degrees(action[0]))),np.cos(math.degrees(action[0])), robot_position )) + + # while (math.isclose(self._robot_pose.x, action_msg.pose.position.x, rel_tol=0.2) and math.isclose(self._robot_pose.y, action_msg.pose.position.y, rel_tol=0.2)) == False : + # #log laserscan and write into history buffer + # print("positions not the same") + # print(self._robot_pose.x) + # print(action_msg.pose.position.x) + # print(self._robot_pose.y) + # print(action_msg.pose.position.y) + # print((math.isclose(self._robot_pose.x, action_msg.pose.position.x, rel_tol=0.2) and math.isclose(self._robot_pose.y, action_msg.pose.position.y, rel_tol=0.5))) + # #time.sleep(1) + def step(self, action): + + """ + done_reasons: 0 - exceeded max steps + 1 - collision with obstacle + 2 - goal reached + """ + self._step_counter += 1 + # wait for about 10 steps until do next step, todo wait until agent reaches the goal (robot pose = goal pose from move base simple goal) + + self._pub_action(action) + ##todo: wait for robot_pos = goal pos + + + # wait for new observations + s = time.time() + merged_obs, obs_dict = self.observation_collector.get_observations() + # print("get observation: {}".format(time.time()-s)) + + # calculate reward + reward, reward_info = self.reward_calculator.get_reward( + obs_dict['laser_scan'], obs_dict['goal_in_robot_frame']) + done = reward_info['is_done'] + + print("reward: {}".format(reward)) + + # info + info = {} + if done: + info['done_reason'] = reward_info['done_reason'] + else: + if self._steps_curr_episode == self._max_steps_per_episode: + done = True + info['done_reason'] = 0 + + return merged_obs, reward, done, info + + def reset(self): + + self._previous_time = -100 # some negative number to infer first run + self._step_counter = 0 + # set task + # regenerate start position end goal position of the robot and change the obstacles accordingly + self.agent_action_pub.publish(PoseStamped()) + if self._is_train_mode: + self._sim_step_client() + self.task.reset() + self.reward_calculator.reset() + self._steps_curr_episode = 0 + obs, _ = self.observation_collector.get_observations() + return obs # reward, done, info can't be included + + def close(self): + pass + + +if __name__ == '__main__': + + rospy.init_node('wp3_gym_env', anonymous=True) + print("start") + + wp3_env = wp3Env() + check_env(wp3_env, warn=True) + + # init env + obs = wp3_env.reset() + + # run model + n_steps = 200 + for step in range(n_steps): + # action, _states = model.predict(obs) + action = wp3_env.action_space.sample() + + obs, rewards, done, info = wp3_env.step(action) + + time.sleep(0.1) diff --git a/arena_navigation/arena_local_planner/learning_based/arena_local_planner_drl/scripts/training/wp3_train.py b/arena_navigation/arena_local_planner/learning_based/arena_local_planner_drl/scripts/training/wp3_train.py new file mode 100644 index 000000000..b93cbc9ab --- /dev/null +++ b/arena_navigation/arena_local_planner/learning_based/arena_local_planner_drl/scripts/training/wp3_train.py @@ -0,0 +1,218 @@ +import os +import rospy + +from datetime import datetime as dt + +from stable_baselines3 import PPO +from stable_baselines3.common.vec_env import SubprocVecEnv, DummyVecEnv +from stable_baselines3.common.monitor import Monitor +from stable_baselines3.common.callbacks import EvalCallback + +from task_generator.task_generator.tasks import get_predefined_task +from arena_navigation.arena_local_planner.learning_based.arena_local_planner_drl.scripts.custom_policy import * +from arena_navigation.arena_local_planner.learning_based.arena_local_planner_drl.rl_agent.envs.wp3_env import wp3Env +from arena_navigation.arena_local_planner.learning_based.arena_local_planner_drl.tools.argsparser import parse_training_args +from arena_navigation.arena_local_planner.learning_based.arena_local_planner_drl.tools.train_agent_utils import * +from arena_navigation.arena_local_planner.learning_based.arena_local_planner_drl.tools.custom_mlp_utils import * + + +###HYPERPARAMETER### +robot = "myrobot" +gamma = 0.99 +n_steps = 500 +ent_coef = 0.01 +learning_rate = 2.5e-4 +vf_coef = 0.5 +max_grad_norm = 0.5 +gae_lambda = 0.95 +batch_size = 64 +n_epochs = 4 +clip_range = 0.2 +reward_fnc = "00" +discrete_action_space = False +#################### + + +class agent_hyperparams(object): + """ Class containing agent specific hyperparameters (for documentation purposes) + + :param agent_name: Precise agent name (as generated by get_agent_name()) + :param robot: Robot name to load robot specific .yaml file containing settings + :param gamma: Discount factor + :param n_steps: The number of steps to run for each environment per update + :param ent_coef: Entropy coefficient for the loss calculation + :param learning_rate: The learning rate, it can be a function + of the current progress remaining (from 1 to 0) + (i.e. batch size is n_steps * n_env where n_env is number of environment copies running in parallel) + :param vf_coef: Value function coefficient for the loss calculation + :param max_grad_norm: The maximum value for the gradient clipping + :param gae_lambda: Factor for trade-off of bias vs variance for Generalized Advantage Estimator + :param batch_size: Minibatch size + :param n_epochs: Number of epoch when optimizing the surrogate loss + :param clip_range: Clipping parameter, it can be a function of the current progress + remaining (from 1 to 0). + :param reward_fnc: Number of the reward function (defined in ../rl_agent/utils/reward.py) + :param discrete_action_space: If robot uses discrete action space + :param n_timesteps: The number of timesteps trained on in total. + """ + def __init__(self, agent_name: str, robot: str, gamma: float, n_steps: int, ent_coef: float, learning_rate: float, vf_coef: float, max_grad_norm: float, gae_lambda: float, + batch_size: int, n_epochs: int, clip_range: float, reward_fnc, discrete_action_space: bool, n_timesteps: int = 0): + self.agent_name = agent_name + self.robot = robot + self.gamma = gamma + self.n_steps = n_steps + self.ent_coef = ent_coef + self.learning_rate = learning_rate + self.vf_coef = vf_coef + self.max_grad_norm = max_grad_norm + self.gae_lambda = gae_lambda + self.batch_size = batch_size + self.n_epochs = n_epochs + self.clip_range = clip_range + self.reward_fnc = reward_fnc + self.discrete_action_space = discrete_action_space + self.n_timesteps = n_timesteps + + +def get_agent_name(args): + """ Function to get agent name to save to/load from file system + + Example names: + "MLP_B_64-64_P_32-32_V_32-32_relu_2021_01_07__10_32" + "DRL_LOCAL_PLANNER_2021_01_08__7_14" + + :param args (argparse.Namespace): Object containing the program arguments + """ + START_TIME = dt.now().strftime("%Y_%m_%d__%H_%M") + + if args.custom_mlp: + return "MLP_B_" + args.body + "_P_" + args.pi + "_V_" + args.vf + "_" + args.act_fn + "_" + START_TIME + if args.load is None: + return args.agent + "_" + START_TIME + return args.load + + +def get_paths(agent_name: str, args): + """ Function to generate agent specific paths + + :param agent_name: Precise agent name (as generated by get_agent_name()) + :param args (argparse.Namespace): Object containing the program arguments + """ + dir = rospkg.RosPack().get_path('arena_local_planner_drl') + + PATHS = { + 'model' : os.path.join(dir, 'agents', agent_name), + 'tb' : os.path.join(dir, 'training_logs', 'tensorboard', agent_name), + 'eval' : os.path.join(dir, 'training_logs', 'train_eval_log', agent_name), + 'robot_setting' : os.path.join(rospkg.RosPack().get_path('simulator_setup'), 'robot', robot + '.model.yaml'), + 'robot_as' : os.path.join(rospkg.RosPack().get_path('arena_local_planner_drl'), 'configs', 'default_settings.yaml'), + } + + hyperparams = agent_hyperparams(agent_name, robot, gamma, n_steps, ent_coef, learning_rate, vf_coef,max_grad_norm, gae_lambda, batch_size, + n_epochs, clip_range, reward_fnc, discrete_action_space) + + # check for mode + if args.load is None: + os.makedirs(PATHS.get('model')) + write_hyperparameters_json(hyperparams, PATHS) + else: + if not os.path.isfile(os.path.join(PATHS.get('model'), AGENT_NAME + ".zip")) and not os.path.isfile(os.path.join(PATHS.get('model'), "best_model.zip")): + raise FileNotFoundError("Couldn't find model named %s.zip' or 'best_model.zip' in '%s'" % (AGENT_NAME, PATHS.get('model'))) + + #print_hyperparameters_json(hyperparams, PATHS) + + # evaluation log enabled + if args.eval_log: + if not os.path.exists(PATHS.get('eval')): + os.makedirs(PATHS.get('eval')) + else: + PATHS['eval'] = None + # tensorboard log enabled + if args.tb: + if not os.path.exists(PATHS.get('tb')): + os.makedirs(PATHS.get('tb')) + else: + PATHS['tb'] = None + + return PATHS + + +if __name__ == "__main__": + args, _ = parse_training_args() + + rospy.init_node("test", disable_signals=True) + + # generate agent name and model specific paths + AGENT_NAME = get_agent_name(args) + print("________ STARTING TRAINING WITH: %s ________\n" % AGENT_NAME) + PATHS = get_paths(AGENT_NAME, args) + + # set num of timesteps to be generated + if args.n is None: + n_timesteps = 60000000 + else: + n_timesteps = args.n + + # instantiate gym environment + n_envs = 1 + task = get_predefined_task("random") + env = DummyVecEnv([lambda: wp3Env(task, PATHS.get('robot_setting'), PATHS.get('robot_as'), discrete_action_space, goal_radius=1.25, max_steps_per_episode=550)] * n_envs) + + # instantiate eval environment + eval_env = Monitor(wp3Env(task, PATHS.get('robot_setting'), PATHS.get('robot_as'), discrete_action_space, goal_radius=1.25, max_steps_per_episode=550), PATHS.get('eval'), info_keywords=("done_reason",)) + eval_cb = EvalCallback(eval_env, n_eval_episodes=10, eval_freq=5000, log_path=PATHS.get('eval'), best_model_save_path=PATHS.get('model'), deterministic=True) + + # determine mode + if args.custom_mlp: + # custom mlp flag + model = PPO("MlpPolicy", env, policy_kwargs = dict(net_arch = args.net_arch, activation_fn = get_act_fn(args.act_fn)), + gamma = gamma, n_steps = n_steps, ent_coef = ent_coef, learning_rate = learning_rate, vf_coef = vf_coef, + max_grad_norm = max_grad_norm, gae_lambda = gae_lambda, batch_size = batch_size, n_epochs = n_epochs, clip_range = clip_range, + tensorboard_log = PATHS.get('tb'), verbose = 1) + + elif args.agent is not None: + # predefined agent flag + if args.agent == "MLP_ARENA2D": + model = PPO(MLP_ARENA2D_POLICY, env, gamma = gamma, n_steps = n_steps, ent_coef = ent_coef, + learning_rate = learning_rate, vf_coef = vf_coef, max_grad_norm = max_grad_norm, gae_lambda = gae_lambda, + batch_size = batch_size, n_epochs = n_epochs, clip_range = clip_range, tensorboard_log = PATHS.get('tb'), verbose = 1) + + elif args.agent == "DRL_LOCAL_PLANNER" or args.agent == "CNN_NAVREP": + if args.agent == "DRL_LOCAL_PLANNER": + policy_kwargs = policy_kwargs_drl_local_planner + else: + policy_kwargs = policy_kwargs_navrep + + model = PPO("CnnPolicy", env, policy_kwargs = policy_kwargs, + gamma = gamma, n_steps = n_steps, ent_coef = ent_coef, learning_rate = learning_rate, vf_coef = vf_coef, + max_grad_norm = max_grad_norm, gae_lambda = gae_lambda, batch_size = batch_size, n_epochs = n_epochs, + clip_range = clip_range, tensorboard_log = PATHS.get('tb'), verbose = 1) + + else: + # load flag + if os.path.isfile(os.path.join(PATHS.get('model'), AGENT_NAME + ".zip")): + model = PPO.load(os.path.join(PATHS.get('model'), AGENT_NAME), env) + elif os.path.isfile(os.path.join(PATHS.get('model'), "best_model.zip")): + model = PPO.load(os.path.join(PATHS.get('model'), "best_model"), env) + + # start training + model.learn(total_timesteps = n_timesteps, callback=eval_cb, reset_num_timesteps = False) + #model.save(os.path.join(PATHS.get('model'), AGENT_NAME)) + + # update the timesteps the model has trained in total + update_total_timesteps_json(n_timesteps, PATHS) + + print("training done and model saved!") + +""" + s = time.time() + model.learn(total_timesteps = 3000) + print("steps per second: {}".format(1000 / (time.time() - s))) + # obs = env.reset() + # for i in range(1000): + # action, _state = model.predict(obs, deterministic = True) + # obs, reward, done, info = env.step(action) + # env.render() + # if done: + # obs = env.reset() +""" \ No newline at end of file diff --git a/arena_navigation/arena_local_planner/learning_based/arena_local_planner_drl/tools/__init__.py b/arena_navigation/arena_local_planner/learning_based/arena_local_planner_drl/tools/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/arena_navigation/arena_plan_manager/src/plan_manager copy.cpp b/arena_navigation/arena_plan_manager/src/plan_manager copy.cpp new file mode 100755 index 000000000..e56a566d2 --- /dev/null +++ b/arena_navigation/arena_plan_manager/src/plan_manager copy.cpp @@ -0,0 +1,283 @@ +#include + + + + + +using namespace std; + +void PlanManager::init(ros::NodeHandle& nh) { + + /* plan param */ + bool train_mode; + ros::NodeHandle n; + n.param("train_mode", train_mode, false); + mode_=train_mode?TRAIN:TEST; + + nh.param("look_ahead_distance", look_ahead_distance_, 1.5); + nh.param("tolerance_approach", tolerance_approach_, 0.5); + + nh.param("timeout_goal", timeout_goal_, 300.); //sec + nh.param("timeout_subgoal", timeout_subgoal_, 60.); //sec + + + + + + + + /* initialize main modules */ + planner_collector_.reset(new PlanCollector); + planner_collector_->initPlanModules(nh); + visualization_.reset(new PlanningVisualization(nh)); + + /* init variables */ + exec_state_ = FSM_EXEC_STATE::INIT; + have_goal_ = false; + have_odom_ = false; + cur_state_=new RobotState(Eigen::Vector2d::Zero(),0.0,Eigen::Vector2d::Zero(),0.0); + + /* callback */ + exec_timer_ = nh.createTimer(ros::Duration(0.01), &PlanManager::execFSMCallback, this); + //safety_timer_ = nh.createTimer(ros::Duration(0.05), &PlanManager::checkCollisionCallback, this); + + // subscriber + goal_sub_ =nh.subscribe("/goal", 1, &PlanManager::goalCallback, this); + odom_sub_ = nh.subscribe("/odometry/ground_truth", 1, &PlanManager::odometryCallback, this); // odom //odometry/ground_truth + + // publisher + subgoal_pub_ = nh.advertise("subgoal",10);// relative name:/ns/node_name/subgoal + robot_state_pub_ = nh.advertise("robot_state",10); + /* test purpose*/ + +} + +void PlanManager::goalCallback(const geometry_msgs::PoseStampedPtr& msg) { + // position z must be zero(2D motion plan) + if(msg->pose.position.z !=0) return; + + cout << " Task Triggered!" << endl; + + // set end_state + end_state_=new RobotState(msg->pose); + end_state_->vel2d.setZero(); + + // change state: to GEN_NEW_GLOBAL + if (exec_state_ == WAIT_GOAL){ + changeFSMExecState(GEN_NEW_GLOBAL, "TRIG"); + } + else if (exec_state_ == EXEC_LOCAL){ + changeFSMExecState(GEN_NEW_GLOBAL, "TRIG"); + } + else if (exec_state_ == REPLAN_MID){ + changeFSMExecState(GEN_NEW_GLOBAL, "TRIG"); + } + else if (exec_state_ == INIT){ + changeFSMExecState(GEN_NEW_GLOBAL, "TRIG"); + } + + // set have_goal + cout << "Goal set!" << endl; + have_goal_ = true; + visualization_->drawGoal(end_state_->to_PoseStampted(), 0.5, Eigen::Vector4d(1, 1, 1, 1.0)); + + // init start_time for this task + start_time_=ros::Time::now(); +} + +void PlanManager::odometryCallback(const nav_msgs::OdometryConstPtr& msg){ + // get robot state according to odometry msg + cur_state_= new RobotState(*msg); + + // publish robot state + robot_state_pub_.publish(cur_state_->toRobotStateStamped()); + + // set have_odom(means localization system is ready) + have_odom_ = true; +} + +void PlanManager::execFSMCallback(const ros::TimerEvent& e) { + // print state with a fixed rate + static int fsm_num = 0; + fsm_num++; + if (fsm_num == 100) { + printFSMExecState(); + if (!have_odom_) cout << "no odom." << endl; + if (!have_goal_) cout << "wait for goal." << endl; + fsm_num = 0; + } + + // FSM state-> action + switch (exec_state_) { + case INIT: { + if (!have_odom_) { + return; + } + if (!have_goal_) { + return; + } + changeFSMExecState(WAIT_GOAL, "FSM"); + break; + } + + case WAIT_GOAL: { + if (!have_goal_) + return; + else { + // go to GEN_NEW_GLOBAL state, going to do global (re)plan + changeFSMExecState(GEN_NEW_GLOBAL, "FSM"); + } + break; + } + + + case GEN_NEW_GLOBAL: { + if(mode_==TRAIN){ + changeFSMExecState(REPLAN_MID, "FSM"); + return; + } + // set robot start state + start_state_=new RobotState(cur_state_->pose2d,cur_state_->theta,cur_state_->vel2d,cur_state_->w); + + // execute global planning + bool global_plan_success=planner_collector_->generate_global_plan(*start_state_,*end_state_); + + if(global_plan_success){ + // success:go to REPLAN_MID state, going to do mid horizon replan(subgoal) + visualization_->drawGlobalPath(planner_collector_->global_path_,0.03, Eigen::Vector4d(0.5, 0.5, 0.5, 0.6)); + changeFSMExecState(REPLAN_MID, "FSM"); + }else{ + // failed: go to GEN_NEW_GLOBAL state, going to do global (re)plan + changeFSMExecState(GEN_NEW_GLOBAL, "FSM"); + } + break; + } + + case EXEC_LOCAL: { + if(mode_==TRAIN){ + //cout<<"EXEC_LOCAL"<<"Train mode"< rand_obstacle; + //uniform_real_distribution rand_goal; + //rand_obstacle = uniform_real_distribution(0.0, 3.0 ); + //default_random_engine eng(rd()); + //dist_to_obstacle=rand_obstacle(eng); + + // calculate: distance to (global) goal + double dist_to_goal; + dist_to_goal=(cur_state_->pose2d-end_state_->pose2d).norm(); + + // calculate: distance to (mid horizon) subgoal + double dist_to_subgoal; + dist_to_subgoal=(cur_state_->pose2d-planner_collector_->subgoal_state_->pose2d).norm(); + + // calculate: timecot to avoid task takes too much time + double time_cost_goal=ros::Time::now().toSec()-start_time_.toSec(); + double time_cost_subgoal=ros::Time::now().toSec()-subgoal_start_time_.toSec(); + + + /* check state_transfer: Goal Criterion */ + // check if reached goal + if(dist_to_goaltimeout_goal_){ + have_goal_=false; + cout<<"failed to goal"<timeout_subgoal_){ + cout<<"subgoal has been published for"<look_ahead_distance_){ + // if the robot stopped at a pos far from subgoal, then won't replan mid, but wait for timeout and global replan + bool robot_stopped=cur_state_->vel2d.norm()<0.1; + if(!robot_stopped){ + changeFSMExecState(REPLAN_MID, "FSM"); + } + return; + } + + // check if subgoal is reached. If reached then replan + if(dist_to_subgoalto_PoseStampted()); + visualization_->drawSubgoal(end_state_->to_PoseStampted(), 0.3, Eigen::Vector4d(0, 0, 0, 1.0)); + cout<<"MID_REPLAN Success"<pose2d,cur_state_->theta,cur_state_->vel2d,cur_state_->w); + double obstacle_info=1.0; + double sensor_info=1.0; + + /* new waypoint generation*/ + bool get_subgoal_success = planner_collector_->generate_subgoal(cur_state_,end_state_, planner_collector_->global_path_,obstacle_info,sensor_info); + + if (get_subgoal_success) { + // success: publish new subgoal & going to state EXEC_LOCAL + subgoal_pub_.publish(planner_collector_->subgoal_); + visualization_->drawSubgoal(planner_collector_->subgoal_, 0.3, Eigen::Vector4d(0, 0, 0, 1.0)); + // reset subgoal start time(be used for timeout criterion) + subgoal_start_time_=ros::Time::now(); + cout<<"MID_REPLAN Success"<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 \ No newline at end of file diff --git a/gui/README.md b/gui/README.md index f070349bf..cfad4ac99 100644 --- a/gui/README.md +++ b/gui/README.md @@ -31,8 +31,8 @@ The GUI represents an user interface for creating scenarios more intuitive, a lo 4. Execute the `paint.py` file.
`$ python paint.py` 5. Generate a new scenario in the paint app. Look at the rules! When ready, the next script `parser.py` will automatically start.
-6. The generated json file will be saved under the name `new_scenario.json` in the `output` folder as well as in the folder `arena-rosnav/simulator_setup/scenerios/`, where all scenarios are stored. -7. Modify the `arena-rosnav/arena_bringup/launch/sublaunch/task_generator.launch` file, changing the name of the expected json file to `new_scenario.json`.
+6. The generated json file will be saved under the name `new_scenario.json` in the `output` folder as well as in the folder `arena-rosnav/simulator_setup/scenarios/`, where all scenarios are stored. +7. Modify the `arena-rosnav/arena_bringup/launch/sublaunch_testing/task_generator.launch` file, changing the name of the expected json file to `new_scenario.json`.
8. Activate rosnav.
`$ workon rosnav`
**Attention:** Rosnav and kivy_env might not be able to stay active at the same time, so activate kivy_env for the gui and rosnav for launching. diff --git a/simulator_setup/maps/complex_map_1/complex_map_1.png b/simulator_setup/maps/complex_map_1/complex_map_1.png new file mode 100644 index 000000000..d17cf03ca Binary files /dev/null and b/simulator_setup/maps/complex_map_1/complex_map_1.png differ diff --git a/simulator_setup/maps/complex_map_1/map.world.yaml b/simulator_setup/maps/complex_map_1/map.world.yaml new file mode 100644 index 000000000..c6637ed6e --- /dev/null +++ b/simulator_setup/maps/complex_map_1/map.world.yaml @@ -0,0 +1,23 @@ +properties: + # optional, defaults to 10, number of velocity iterations for the Box2D + # physics solver + velocity_iterations: 10 + + # optional, defaults to 10, number of position iterations for the Box2D + # physics solver + position_iterations: 10 +layers: # Support for arbitrary number of layers + # - name: "robot" # So that sensors can trigger on just the robot + - name: "static" + map: "map.yaml" # leading / denotes absolute file path, otherwise relative + color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary + - name: "ped" + # map: "../no_map/map.yaml" # leading / denotes absolute file path, otherwise relative + color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary + # - name: "2d" # layer 0 named "2d" + # map: "map.yaml" # leading / denotes absolute file path, otherwise relative + # color: [0, 1, 0, 1] # List of floats [r,g,b,a] to color debug boundary + # - name: "3d" + # map: "map.yaml" # leading / denotes absolute file path, otherwise relative + # color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary + diff --git a/simulator_setup/maps/complex_map_1/map.yaml b/simulator_setup/maps/complex_map_1/map.yaml new file mode 100644 index 000000000..ea6cb98ac --- /dev/null +++ b/simulator_setup/maps/complex_map_1/map.yaml @@ -0,0 +1,9 @@ +image: complex_map_1.png +resolution: 0.05 +# origin: [-1.1, -9.05, 0.0] +# origin: [-0.5, -1.45, 0.0] +origin: [0.0, 0.0, 0.0] +occupied_thresh: 0.65 +free_thresh: 0.196 +negate: 0 +#mode: scale diff --git a/simulator_setup/maps/complex_map_1/pedsim_scenario.xml b/simulator_setup/maps/complex_map_1/pedsim_scenario.xml new file mode 100644 index 000000000..ace8d3176 --- /dev/null +++ b/simulator_setup/maps/complex_map_1/pedsim_scenario.xml @@ -0,0 +1,13 @@ + + + + --> + + + + + + + + + \ No newline at end of file diff --git a/simulator_setup/maps/complex_map_2/complex_map_2.png b/simulator_setup/maps/complex_map_2/complex_map_2.png new file mode 100644 index 000000000..4449fd9c5 Binary files /dev/null and b/simulator_setup/maps/complex_map_2/complex_map_2.png differ diff --git a/simulator_setup/maps/complex_map_2/map.world.yaml b/simulator_setup/maps/complex_map_2/map.world.yaml new file mode 100644 index 000000000..c6637ed6e --- /dev/null +++ b/simulator_setup/maps/complex_map_2/map.world.yaml @@ -0,0 +1,23 @@ +properties: + # optional, defaults to 10, number of velocity iterations for the Box2D + # physics solver + velocity_iterations: 10 + + # optional, defaults to 10, number of position iterations for the Box2D + # physics solver + position_iterations: 10 +layers: # Support for arbitrary number of layers + # - name: "robot" # So that sensors can trigger on just the robot + - name: "static" + map: "map.yaml" # leading / denotes absolute file path, otherwise relative + color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary + - name: "ped" + # map: "../no_map/map.yaml" # leading / denotes absolute file path, otherwise relative + color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary + # - name: "2d" # layer 0 named "2d" + # map: "map.yaml" # leading / denotes absolute file path, otherwise relative + # color: [0, 1, 0, 1] # List of floats [r,g,b,a] to color debug boundary + # - name: "3d" + # map: "map.yaml" # leading / denotes absolute file path, otherwise relative + # color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary + diff --git a/simulator_setup/maps/complex_map_2/map.yaml b/simulator_setup/maps/complex_map_2/map.yaml new file mode 100644 index 000000000..9f56b5035 --- /dev/null +++ b/simulator_setup/maps/complex_map_2/map.yaml @@ -0,0 +1,9 @@ +image: complex_map_2.png +resolution: 0.05 +# origin: [-1.1, -9.05, 0.0] +# origin: [-0.5, -1.45, 0.0] +origin: [0.0, 0.0, 0.0] +occupied_thresh: 0.65 +free_thresh: 0.196 +negate: 0 +#mode: scale diff --git a/simulator_setup/maps/complex_map_2/pedsim_scenario.xml b/simulator_setup/maps/complex_map_2/pedsim_scenario.xml new file mode 100644 index 000000000..ace8d3176 --- /dev/null +++ b/simulator_setup/maps/complex_map_2/pedsim_scenario.xml @@ -0,0 +1,13 @@ + + + + --> + + + + + + + + + \ No newline at end of file diff --git a/simulator_setup/maps/complex_map_3/complex_map_3.png b/simulator_setup/maps/complex_map_3/complex_map_3.png new file mode 100644 index 000000000..4fc4daeff Binary files /dev/null and b/simulator_setup/maps/complex_map_3/complex_map_3.png differ diff --git a/simulator_setup/maps/complex_map_3/map.world.yaml b/simulator_setup/maps/complex_map_3/map.world.yaml new file mode 100644 index 000000000..c6637ed6e --- /dev/null +++ b/simulator_setup/maps/complex_map_3/map.world.yaml @@ -0,0 +1,23 @@ +properties: + # optional, defaults to 10, number of velocity iterations for the Box2D + # physics solver + velocity_iterations: 10 + + # optional, defaults to 10, number of position iterations for the Box2D + # physics solver + position_iterations: 10 +layers: # Support for arbitrary number of layers + # - name: "robot" # So that sensors can trigger on just the robot + - name: "static" + map: "map.yaml" # leading / denotes absolute file path, otherwise relative + color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary + - name: "ped" + # map: "../no_map/map.yaml" # leading / denotes absolute file path, otherwise relative + color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary + # - name: "2d" # layer 0 named "2d" + # map: "map.yaml" # leading / denotes absolute file path, otherwise relative + # color: [0, 1, 0, 1] # List of floats [r,g,b,a] to color debug boundary + # - name: "3d" + # map: "map.yaml" # leading / denotes absolute file path, otherwise relative + # color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary + diff --git a/simulator_setup/maps/complex_map_3/map.yaml b/simulator_setup/maps/complex_map_3/map.yaml new file mode 100644 index 000000000..af6fe70e1 --- /dev/null +++ b/simulator_setup/maps/complex_map_3/map.yaml @@ -0,0 +1,9 @@ +image: complex_map_3.png +resolution: 0.05 +# origin: [-1.1, -9.05, 0.0] +# origin: [-0.5, -1.45, 0.0] +origin: [0.0, 0.0, 0.0] +occupied_thresh: 0.65 +free_thresh: 0.196 +negate: 0 +#mode: scale diff --git a/simulator_setup/maps/complex_map_3/pedsim_scenario.xml b/simulator_setup/maps/complex_map_3/pedsim_scenario.xml new file mode 100644 index 000000000..ace8d3176 --- /dev/null +++ b/simulator_setup/maps/complex_map_3/pedsim_scenario.xml @@ -0,0 +1,13 @@ + + + + --> + + + + + + + + + \ No newline at end of file diff --git a/simulator_setup/maps/corridor_1/corridor_1.png b/simulator_setup/maps/corridor_1/corridor_1.png new file mode 100644 index 000000000..118a38ff8 Binary files /dev/null and b/simulator_setup/maps/corridor_1/corridor_1.png differ diff --git a/simulator_setup/maps/corridor_1/map.world.yaml b/simulator_setup/maps/corridor_1/map.world.yaml new file mode 100644 index 000000000..c6637ed6e --- /dev/null +++ b/simulator_setup/maps/corridor_1/map.world.yaml @@ -0,0 +1,23 @@ +properties: + # optional, defaults to 10, number of velocity iterations for the Box2D + # physics solver + velocity_iterations: 10 + + # optional, defaults to 10, number of position iterations for the Box2D + # physics solver + position_iterations: 10 +layers: # Support for arbitrary number of layers + # - name: "robot" # So that sensors can trigger on just the robot + - name: "static" + map: "map.yaml" # leading / denotes absolute file path, otherwise relative + color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary + - name: "ped" + # map: "../no_map/map.yaml" # leading / denotes absolute file path, otherwise relative + color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary + # - name: "2d" # layer 0 named "2d" + # map: "map.yaml" # leading / denotes absolute file path, otherwise relative + # color: [0, 1, 0, 1] # List of floats [r,g,b,a] to color debug boundary + # - name: "3d" + # map: "map.yaml" # leading / denotes absolute file path, otherwise relative + # color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary + diff --git a/simulator_setup/maps/corridor_1/map.yaml b/simulator_setup/maps/corridor_1/map.yaml new file mode 100644 index 000000000..8e795e3b7 --- /dev/null +++ b/simulator_setup/maps/corridor_1/map.yaml @@ -0,0 +1,9 @@ +image: corridor_1.png +resolution: 0.05 +# origin: [-1.1, -9.05, 0.0] +# origin: [-0.5, -1.45, 0.0] +origin: [0.0, 0.0, 0.0] +occupied_thresh: 0.65 +free_thresh: 0.196 +negate: 0 +#mode: scale diff --git a/simulator_setup/maps/corridor_1/pedsim_scenario.xml b/simulator_setup/maps/corridor_1/pedsim_scenario.xml new file mode 100644 index 000000000..ace8d3176 --- /dev/null +++ b/simulator_setup/maps/corridor_1/pedsim_scenario.xml @@ -0,0 +1,13 @@ + + + + --> + + + + + + + + + \ No newline at end of file diff --git a/simulator_setup/maps/corridor_2/corridor_2.png b/simulator_setup/maps/corridor_2/corridor_2.png new file mode 100644 index 000000000..40aa6b5e1 Binary files /dev/null and b/simulator_setup/maps/corridor_2/corridor_2.png differ diff --git a/simulator_setup/maps/corridor_2/map.world.yaml b/simulator_setup/maps/corridor_2/map.world.yaml new file mode 100644 index 000000000..c6637ed6e --- /dev/null +++ b/simulator_setup/maps/corridor_2/map.world.yaml @@ -0,0 +1,23 @@ +properties: + # optional, defaults to 10, number of velocity iterations for the Box2D + # physics solver + velocity_iterations: 10 + + # optional, defaults to 10, number of position iterations for the Box2D + # physics solver + position_iterations: 10 +layers: # Support for arbitrary number of layers + # - name: "robot" # So that sensors can trigger on just the robot + - name: "static" + map: "map.yaml" # leading / denotes absolute file path, otherwise relative + color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary + - name: "ped" + # map: "../no_map/map.yaml" # leading / denotes absolute file path, otherwise relative + color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary + # - name: "2d" # layer 0 named "2d" + # map: "map.yaml" # leading / denotes absolute file path, otherwise relative + # color: [0, 1, 0, 1] # List of floats [r,g,b,a] to color debug boundary + # - name: "3d" + # map: "map.yaml" # leading / denotes absolute file path, otherwise relative + # color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary + diff --git a/simulator_setup/maps/corridor_2/map.yaml b/simulator_setup/maps/corridor_2/map.yaml new file mode 100644 index 000000000..b6b56b370 --- /dev/null +++ b/simulator_setup/maps/corridor_2/map.yaml @@ -0,0 +1,9 @@ +image: corridor_2.png +resolution: 0.05 +# origin: [-1.1, -9.05, 0.0] +# origin: [-0.5, -1.45, 0.0] +origin: [0.0, 0.0, 0.0] +occupied_thresh: 0.65 +free_thresh: 0.196 +negate: 0 +#mode: scale diff --git a/simulator_setup/maps/corridor_2/pedsim_scenario.xml b/simulator_setup/maps/corridor_2/pedsim_scenario.xml new file mode 100644 index 000000000..ace8d3176 --- /dev/null +++ b/simulator_setup/maps/corridor_2/pedsim_scenario.xml @@ -0,0 +1,13 @@ + + + + --> + + + + + + + + + \ No newline at end of file diff --git a/simulator_setup/maps/corridor_3/corridor_3.png b/simulator_setup/maps/corridor_3/corridor_3.png new file mode 100644 index 000000000..cec8fa652 Binary files /dev/null and b/simulator_setup/maps/corridor_3/corridor_3.png differ diff --git a/simulator_setup/maps/corridor_3/map.world.yaml b/simulator_setup/maps/corridor_3/map.world.yaml new file mode 100644 index 000000000..c6637ed6e --- /dev/null +++ b/simulator_setup/maps/corridor_3/map.world.yaml @@ -0,0 +1,23 @@ +properties: + # optional, defaults to 10, number of velocity iterations for the Box2D + # physics solver + velocity_iterations: 10 + + # optional, defaults to 10, number of position iterations for the Box2D + # physics solver + position_iterations: 10 +layers: # Support for arbitrary number of layers + # - name: "robot" # So that sensors can trigger on just the robot + - name: "static" + map: "map.yaml" # leading / denotes absolute file path, otherwise relative + color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary + - name: "ped" + # map: "../no_map/map.yaml" # leading / denotes absolute file path, otherwise relative + color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary + # - name: "2d" # layer 0 named "2d" + # map: "map.yaml" # leading / denotes absolute file path, otherwise relative + # color: [0, 1, 0, 1] # List of floats [r,g,b,a] to color debug boundary + # - name: "3d" + # map: "map.yaml" # leading / denotes absolute file path, otherwise relative + # color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary + diff --git a/simulator_setup/maps/corridor_3/map.yaml b/simulator_setup/maps/corridor_3/map.yaml new file mode 100644 index 000000000..6f626ab8e --- /dev/null +++ b/simulator_setup/maps/corridor_3/map.yaml @@ -0,0 +1,9 @@ +image: corridor_3.png +resolution: 0.05 +# origin: [-1.1, -9.05, 0.0] +# origin: [-0.5, -1.45, 0.0] +origin: [0.0, 0.0, 0.0] +occupied_thresh: 0.65 +free_thresh: 0.196 +negate: 0 +#mode: scale diff --git a/simulator_setup/maps/corridor_3/pedsim_scenario.xml b/simulator_setup/maps/corridor_3/pedsim_scenario.xml new file mode 100644 index 000000000..ace8d3176 --- /dev/null +++ b/simulator_setup/maps/corridor_3/pedsim_scenario.xml @@ -0,0 +1,13 @@ + + + + --> + + + + + + + + + \ No newline at end of file diff --git a/simulator_setup/maps/map_empty_small/map.world.yaml b/simulator_setup/maps/map_empty_small/map.world.yaml new file mode 100644 index 000000000..1c779955c --- /dev/null +++ b/simulator_setup/maps/map_empty_small/map.world.yaml @@ -0,0 +1,12 @@ +properties: + + velocity_iterations: 10 + + position_iterations: 10 +layers: # Support for arbitrary number of layers + - name: "static" + map: "map.yaml" # leading / denotes absolute file path, otherwise relative + color: [0, 1, 0, 1] # List of floats [r,g,b,a] to color debug boundary + + + diff --git a/simulator_setup/maps/map_empty_small/map.yaml b/simulator_setup/maps/map_empty_small/map.yaml new file mode 100644 index 000000000..8af8a0329 --- /dev/null +++ b/simulator_setup/maps/map_empty_small/map.yaml @@ -0,0 +1,7 @@ +image: map_small.png +resolution: 0.05 +origin: [-6.0, -6.0, 0.0] +occupied_thresh: 0.65 +free_thresh: 0.196 +negate: 0 + diff --git a/simulator_setup/maps/map_empty_small/map_small.png b/simulator_setup/maps/map_empty_small/map_small.png new file mode 100644 index 000000000..63cbb567b Binary files /dev/null and b/simulator_setup/maps/map_empty_small/map_small.png differ diff --git a/simulator_setup/maps/map_middle_complexity/map.world.yaml b/simulator_setup/maps/map_middle_complexity/map.world.yaml new file mode 100644 index 000000000..7b774db17 --- /dev/null +++ b/simulator_setup/maps/map_middle_complexity/map.world.yaml @@ -0,0 +1,15 @@ +properties: + # optional, defaults to 10, number of velocity iterations for the Box2D + # physics solver + velocity_iterations: 10 + + # optional, defaults to 10, number of position iterations for the Box2D + # physics solver + position_iterations: 10 +layers: # Support for arbitrary number of layers + - name: "ped" + color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary + - name: "static" + map: "map.yaml" # leading / denotes absolute file path, otherwise relative + color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary + \ No newline at end of file diff --git a/simulator_setup/maps/map_middle_complexity/map.yaml b/simulator_setup/maps/map_middle_complexity/map.yaml new file mode 100644 index 000000000..2c56dfbff --- /dev/null +++ b/simulator_setup/maps/map_middle_complexity/map.yaml @@ -0,0 +1,7 @@ +image: map_middle_complexity.png +resolution: 0.05 +origin: [0.0, 0.0, 0.0] +occupied_thresh: 0.65 +free_thresh: 0.196 +negate: 0 + diff --git a/simulator_setup/maps/map_middle_complexity/map_middle_complexity.png b/simulator_setup/maps/map_middle_complexity/map_middle_complexity.png new file mode 100644 index 000000000..b2d0d6910 Binary files /dev/null and b/simulator_setup/maps/map_middle_complexity/map_middle_complexity.png differ diff --git a/simulator_setup/maps/map_middle_complexity/pedsim_scenario.xml b/simulator_setup/maps/map_middle_complexity/pedsim_scenario.xml new file mode 100644 index 000000000..595040d24 --- /dev/null +++ b/simulator_setup/maps/map_middle_complexity/pedsim_scenario.xml @@ -0,0 +1,67 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + --> + + + + + + + + + \ No newline at end of file diff --git a/simulator_setup/maps/open_field_1/map.world.yaml b/simulator_setup/maps/open_field_1/map.world.yaml new file mode 100644 index 000000000..c6637ed6e --- /dev/null +++ b/simulator_setup/maps/open_field_1/map.world.yaml @@ -0,0 +1,23 @@ +properties: + # optional, defaults to 10, number of velocity iterations for the Box2D + # physics solver + velocity_iterations: 10 + + # optional, defaults to 10, number of position iterations for the Box2D + # physics solver + position_iterations: 10 +layers: # Support for arbitrary number of layers + # - name: "robot" # So that sensors can trigger on just the robot + - name: "static" + map: "map.yaml" # leading / denotes absolute file path, otherwise relative + color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary + - name: "ped" + # map: "../no_map/map.yaml" # leading / denotes absolute file path, otherwise relative + color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary + # - name: "2d" # layer 0 named "2d" + # map: "map.yaml" # leading / denotes absolute file path, otherwise relative + # color: [0, 1, 0, 1] # List of floats [r,g,b,a] to color debug boundary + # - name: "3d" + # map: "map.yaml" # leading / denotes absolute file path, otherwise relative + # color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary + diff --git a/simulator_setup/maps/open_field_1/map.yaml b/simulator_setup/maps/open_field_1/map.yaml new file mode 100644 index 000000000..44329be4a --- /dev/null +++ b/simulator_setup/maps/open_field_1/map.yaml @@ -0,0 +1,9 @@ +image: open_field_1.png +resolution: 0.05 +# origin: [-1.1, -9.05, 0.0] +# origin: [-0.5, -1.45, 0.0] +origin: [0.0, 0.0, 0.0] +occupied_thresh: 0.65 +free_thresh: 0.196 +negate: 0 +#mode: scale diff --git a/simulator_setup/maps/open_field_1/open_field_1.png b/simulator_setup/maps/open_field_1/open_field_1.png new file mode 100644 index 000000000..78e4f2e0e Binary files /dev/null and b/simulator_setup/maps/open_field_1/open_field_1.png differ diff --git a/simulator_setup/maps/open_field_1/pedsim_scenario.xml b/simulator_setup/maps/open_field_1/pedsim_scenario.xml new file mode 100644 index 000000000..ace8d3176 --- /dev/null +++ b/simulator_setup/maps/open_field_1/pedsim_scenario.xml @@ -0,0 +1,13 @@ + + + + --> + + + + + + + + + \ No newline at end of file diff --git a/simulator_setup/maps/open_field_2/map.world.yaml b/simulator_setup/maps/open_field_2/map.world.yaml new file mode 100644 index 000000000..c6637ed6e --- /dev/null +++ b/simulator_setup/maps/open_field_2/map.world.yaml @@ -0,0 +1,23 @@ +properties: + # optional, defaults to 10, number of velocity iterations for the Box2D + # physics solver + velocity_iterations: 10 + + # optional, defaults to 10, number of position iterations for the Box2D + # physics solver + position_iterations: 10 +layers: # Support for arbitrary number of layers + # - name: "robot" # So that sensors can trigger on just the robot + - name: "static" + map: "map.yaml" # leading / denotes absolute file path, otherwise relative + color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary + - name: "ped" + # map: "../no_map/map.yaml" # leading / denotes absolute file path, otherwise relative + color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary + # - name: "2d" # layer 0 named "2d" + # map: "map.yaml" # leading / denotes absolute file path, otherwise relative + # color: [0, 1, 0, 1] # List of floats [r,g,b,a] to color debug boundary + # - name: "3d" + # map: "map.yaml" # leading / denotes absolute file path, otherwise relative + # color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary + diff --git a/simulator_setup/maps/open_field_2/map.yaml b/simulator_setup/maps/open_field_2/map.yaml new file mode 100644 index 000000000..8edb90df0 --- /dev/null +++ b/simulator_setup/maps/open_field_2/map.yaml @@ -0,0 +1,9 @@ +image: open_field_2.png +resolution: 0.05 +# origin: [-1.1, -9.05, 0.0] +# origin: [-0.5, -1.45, 0.0] +origin: [0.0, 0.0, 0.0] +occupied_thresh: 0.65 +free_thresh: 0.196 +negate: 0 +#mode: scale diff --git a/simulator_setup/maps/open_field_2/open_field_2.png b/simulator_setup/maps/open_field_2/open_field_2.png new file mode 100644 index 000000000..8e3508737 Binary files /dev/null and b/simulator_setup/maps/open_field_2/open_field_2.png differ diff --git a/simulator_setup/maps/open_field_2/pedsim_scenario.xml b/simulator_setup/maps/open_field_2/pedsim_scenario.xml new file mode 100644 index 000000000..ace8d3176 --- /dev/null +++ b/simulator_setup/maps/open_field_2/pedsim_scenario.xml @@ -0,0 +1,13 @@ + + + + --> + + + + + + + + + \ No newline at end of file diff --git a/simulator_setup/maps/open_field_3/agent_scenario.xml b/simulator_setup/maps/open_field_3/agent_scenario.xml new file mode 100644 index 000000000..7a9b417cb --- /dev/null +++ b/simulator_setup/maps/open_field_3/agent_scenario.xml @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/simulator_setup/maps/open_field_3/map.world.yaml b/simulator_setup/maps/open_field_3/map.world.yaml new file mode 100644 index 000000000..c6637ed6e --- /dev/null +++ b/simulator_setup/maps/open_field_3/map.world.yaml @@ -0,0 +1,23 @@ +properties: + # optional, defaults to 10, number of velocity iterations for the Box2D + # physics solver + velocity_iterations: 10 + + # optional, defaults to 10, number of position iterations for the Box2D + # physics solver + position_iterations: 10 +layers: # Support for arbitrary number of layers + # - name: "robot" # So that sensors can trigger on just the robot + - name: "static" + map: "map.yaml" # leading / denotes absolute file path, otherwise relative + color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary + - name: "ped" + # map: "../no_map/map.yaml" # leading / denotes absolute file path, otherwise relative + color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary + # - name: "2d" # layer 0 named "2d" + # map: "map.yaml" # leading / denotes absolute file path, otherwise relative + # color: [0, 1, 0, 1] # List of floats [r,g,b,a] to color debug boundary + # - name: "3d" + # map: "map.yaml" # leading / denotes absolute file path, otherwise relative + # color: [0, 1, 0, 0] # List of floats [r,g,b,a] to color debug boundary + diff --git a/simulator_setup/maps/open_field_3/map.yaml b/simulator_setup/maps/open_field_3/map.yaml new file mode 100644 index 000000000..64ac0e342 --- /dev/null +++ b/simulator_setup/maps/open_field_3/map.yaml @@ -0,0 +1,9 @@ +image: open_field_3.png +resolution: 0.05 +# origin: [-1.1, -9.05, 0.0] +# origin: [-0.5, -1.45, 0.0] +origin: [0.0, 0.0, 0.0] +occupied_thresh: 0.65 +free_thresh: 0.196 +negate: 0 +#mode: scale diff --git a/simulator_setup/maps/open_field_3/open_field_3.png b/simulator_setup/maps/open_field_3/open_field_3.png new file mode 100644 index 000000000..df429c18a Binary files /dev/null and b/simulator_setup/maps/open_field_3/open_field_3.png differ diff --git a/simulator_setup/maps/open_field_3/pedsim_scenario.xml b/simulator_setup/maps/open_field_3/pedsim_scenario.xml new file mode 100644 index 000000000..ace8d3176 --- /dev/null +++ b/simulator_setup/maps/open_field_3/pedsim_scenario.xml @@ -0,0 +1,13 @@ + + + + --> + + + + + + + + + \ No newline at end of file diff --git a/simulator_setup/scenarios/new_scenario.json b/simulator_setup/scenarios/new_scenario.json new file mode 100644 index 000000000..5d0020257 --- /dev/null +++ b/simulator_setup/scenarios/new_scenario.json @@ -0,0 +1,114 @@ +{ + "scenarios": [ + { + "scene_name": "new_scenario", + "repeats": 5, + "dynamic_obstacles": { + "dynamic_obs_0": { + "obstacle_radius": 0.666, + "linear_velocity": 0.3, + "type": "circle", + "start_pos": [ + 6.2544, + 0.3650000000000002, + 0 + ], + "waypoints": [ + [ + 2.5974000000000004, + 0.7326, + 0 + ] + ], + "is_waypoint_relative": true, + "mode": "yoyo", + "triggers": [ + "watcher_0" + ] + }, + "dynamic_obs_1": { + "obstacle_radius": 0.666, + "linear_velocity": 0.3, + "type": "circle", + "start_pos": [ + 9.784200000000002, + 3.8282000000000007, + 0 + ], + "waypoints": [ + [ + -2.2644, + -0.0666, + 0 + ] + ], + "is_waypoint_relative": true, + "mode": "yoyo", + "triggers": [ + "watcher_1" + ] + }, + "dynamic_obs_2": { + "obstacle_radius": 0.666, + "linear_velocity": 0.3, + "type": "circle", + "start_pos": [ + 7.253400000000001, + 6.825200000000001, + 0 + ], + "waypoints": [ + [ + 2.5974000000000004, + 0.2664, + 0 + ] + ], + "is_waypoint_relative": true, + "mode": "yoyo", + "triggers": [ + "watcher_2" + ] + } + }, + "static_obstacles": { + + }, + "robot": { + "start_pos": [ + 5.988000000000001, + -2.5654000000000017, + 0.0 + ], + "goal_pos": [ + 8.452200000000001, + 10.221799999999998, + 0.0 + ] + }, + "watchers": { + "watcher_0": { + "pos": [ + 6.587400000000001, + 0.6314000000000002 + ], + "range": 1.6650000000000003 + }, + "watcher_1": { + "pos": [ + 9.318000000000001, + 2.9624000000000006 + ], + "range": 1.6650000000000003 + }, + "watcher_2": { + "pos": [ + 6.920400000000001, + 6.425600000000001 + ], + "range": 1.6650000000000003 + } + } + } + ] +} \ No newline at end of file