Merge pull request #62 from osrf/M1chaelM/fix_run_trial

Fix automatic evaluation with `run_trial.bash`
osrf · Jun 26, 2023 · e339134 · e339134
2 parents 437f4cd + c29162c
commit e339134
Show file tree

Hide file tree

Showing 8 changed files with 73 additions and 95 deletions.
diff --git a/replay_trial.bash b/replay_trial.bash
@@ -20,7 +20,7 @@ NOCOLOR='\033[0m'
 # Define usage function.
 usage()
 {
-  echo "Usage: $0 [-n --nvidia] [--keep-docker] [--manual-play] [--keep-gz] <team_name> <task_name> <trial_num>"
+  echo "Usage: $0 [--keep-docker] [--manual-play] [--keep-gz] <team_name> <task_name> <trial_num>"
   echo "--keep-docker: Keep Gazebo window open and Docker container running after playback ends."
   echo "  By default, everything is terminated automatically."
   echo "--manual-play: Do not automatically start playback. Wait for user to click in GUI."
@@ -31,8 +31,6 @@ usage()
 }
 
 # Parse arguments
-nvidia_arg=""
-image_nvidia=""
 keep_docker=1
 
 # Args to pass to script internal to Docker container
@@ -45,12 +43,6 @@ do
   key="$1"
 
   case $key in
-    -n|--nvidia)
-      nvidia_arg="-n"
-      image_nvidia="-nvidia"
-      shift
-      ;;
-
     --keep-docker)
       keep_docker=1
       shift
@@ -166,8 +158,8 @@ y=$y" > ${HOST_GZ_GUI_CONFIG_DIR}/gui.ini
 
 # Run Gazebo simulation server container
 SERVER_CMD="/play_vrx_log.sh ${LOG_FILE} ${OUTPUT} ${manual_play} ${keep_gz}"
-SERVER_IMG="vrx-server-${ROS_DISTRO}${image_nvidia}:latest"
-${DIR}/vrx_server/run_container.bash $nvidia_arg ${SERVER_CONTAINER_NAME} $SERVER_IMG \
+SERVER_IMG="vrx-server-${ROS_DISTRO}:latest"
+${DIR}/vrx_server/run_container.bash ${SERVER_CONTAINER_NAME} $SERVER_IMG \
   "--net ${NETWORK} \
   --ip ${SERVER_ROS_IP} \
   -v ${HOST_LOG_DIR}:${LOG_DIR} \

diff --git a/run_trial.bash b/run_trial.bash
@@ -19,36 +19,10 @@ NOCOLOR='\033[0m'
 # Define usage function.
 usage()
 {
-  echo "Usage: $0 [-n --nvidia] <team_name> <task_name> <trial_num>"
+  echo "Usage: $0 <team_name> <task_name> <trial_num>"
   exit 1
 }
 
-# Parse arguments
-RUNTIME="runc"
-nvidia_arg=""
-image_nvidia=""
-
-POSITIONAL=()
-while [[ $# -gt 0 ]]
-do
-  key="$1"
-
-  case $key in
-      -n|--nvidia)
-      RUNTIME="nvidia"
-      nvidia_arg="-n"
-      image_nvidia="-nvidia"
-      shift
-      ;;
-      *)    # unknown option
-      POSITIONAL+=("$1")
-      shift
-      ;;
-  esac
-done
-
-set -- "${POSITIONAL[@]}"
-
 # Call usage() function if arguments not supplied.
 [[ $# -ne 3 ]] && usage
 
@@ -65,7 +39,9 @@ fi
 
 # Constants for containers
 SERVER_CONTAINER_NAME=vrx-server-system
-ROS_DISTRO=noetic
+SERVER_USER=developer
+ROS_DISTRO=humble
+SERVER_IMG="vrx-server-${ROS_DISTRO}:latest"
 LOG_DIR=/vrx/logs
 NETWORK=vrx-network
 NETWORK_SUBNET="172.16.0.10/16" # subnet mask allows communication between IP addresses with 172.16.xx.xx (xx = any)
@@ -97,7 +73,7 @@ chmod 777 ${HOST_LOG_DIR}
 echo -e "${GREEN}Done.${NOCOLOR}\n"
 
 # Find wamv urdf and task world files
-echo "Looking for generated files"
+echo "Looking for WAM-V urdf file (generated)"
 TEAM_GENERATED_DIR=${DIR}/generated/team_generated/${TEAM_NAME}
 if [ -f "${TEAM_GENERATED_DIR}/${TEAM_NAME}.urdf" ]; then
   echo "Successfully found: ${TEAM_GENERATED_DIR}/${TEAM_NAME}.urdf"
@@ -106,9 +82,11 @@ else
   echo -e "${RED}Err: ${TEAM_GENERATED_DIR}/${TEAM_NAME}.urdf not found."; exit 1;
 fi
 
+echo "Looking for task sdf file (generated/copied)"
 COMP_GENERATED_DIR=${DIR}/generated/task_generated/${TASK_NAME}
 if [ -f "${COMP_GENERATED_DIR}/worlds/${TASK_NAME}${TRIAL_NUM}.sdf" ]; then
   echo "Successfully found: ${COMP_GENERATED_DIR}/worlds/${TASK_NAME}${TRIAL_NUM}.sdf"
+  echo -e "${GREEN}Done.${NOCOLOR}\n"
 else
   echo -e "${RED}Err: ${COMP_GENERATED_DIR}/worlds/${TASK_NAME}${TRIAL_NUM}.sdf not found."; exit 1;
 fi
@@ -142,9 +120,8 @@ echo "---------------------------------"
 # simulation doesn't start too early, but may have issues if competitior
 # container waiting for ROS master and has error before server is created.
 # Run Gazebo simulation server container
-SERVER_CMD="/run_vrx_trial.sh /team_generated/${TEAM_NAME}.urdf /task_generated/worlds/${TASK_NAME}${TRIAL_NUM}.world ${LOG_DIR}"
-SERVER_IMG="vrx-server-${ROS_DISTRO}${image_nvidia}:latest"
-${DIR}/vrx_server/run_container.bash $nvidia_arg ${SERVER_CONTAINER_NAME} $SERVER_IMG \
+SERVER_CMD="/run_vrx_trial.sh /team_generated/${TEAM_NAME}.urdf /task_generated/worlds/${TASK_NAME}${TRIAL_NUM} ${LOG_DIR}"
+${DIR}/vrx_server/run_container.bash ${SERVER_CONTAINER_NAME} $SERVER_IMG \
   "--net ${NETWORK} \
   --ip ${SERVER_ROS_IP} \
   -v ${TEAM_GENERATED_DIR}:/team_generated \
@@ -172,8 +149,8 @@ docker run \
     --env ROS_MASTER_URI=${ROS_MASTER_URI} \
     --env ROS_IP=${COMPETITOR_ROS_IP} \
     --ip ${COMPETITOR_ROS_IP} \
+    --gpus all \
     --privileged \
-    --runtime=$RUNTIME \
     ${DOCKERHUB_IMAGE} &
 
 # Run competition until server is ended
@@ -183,10 +160,10 @@ echo "---------------------------------"
 
 # Copy the ROS log files from the server's container.
 echo "Copying ROS log files from server container..."
-docker cp --follow-link ${SERVER_CONTAINER_NAME}:/home/$USER/.ros/log/latest $HOST_LOG_DIR/ros-server-latest
-docker cp --follow-link ${SERVER_CONTAINER_NAME}:/home/$USER/.gazebo/ $HOST_LOG_DIR/gazebo-server
-docker cp --follow-link ${SERVER_CONTAINER_NAME}:/home/$USER/vrx_rostopics.bag $HOST_LOG_DIR/
-docker cp --follow-link ${SERVER_CONTAINER_NAME}:/home/$USER/verbose_output.txt $HOST_LOG_DIR/
+#docker cp --follow-link ${SERVER_CONTAINER_NAME}:/home/$SERVER_USER/.ros/log/latest $HOST_LOG_DIR/ros-server-latest
+docker cp --follow-link ${SERVER_CONTAINER_NAME}:/home/$SERVER_USER/.gz/ $HOST_LOG_DIR/gz-server
+docker cp --follow-link ${SERVER_CONTAINER_NAME}:/home/$SERVER_USER/vrx_rostopics.bag $HOST_LOG_DIR/
+docker cp --follow-link ${SERVER_CONTAINER_NAME}:/home/$SERVER_USER/verbose_output.txt $HOST_LOG_DIR/
 
 echo -e "${GREEN}OK${NOCOLOR}\n"
 

diff --git a/utils/get_trial_score.py b/utils/get_trial_score.py
@@ -1,11 +1,13 @@
-# get_trial_score.py: A Python script that gets the last message of a trial's rosbag file, 
+# get_trial_score.py: A Python script that gets the last message of a trial's ros2bag file, 
 #                     then records the final trial score in a text file
 #
 # eg. python get_trial_score.py <team_name> <task_name> <trial_number>
 
 import os
-import rosbag
+import rosbag2_py
 import sys
+from rclpy.serialization import deserialize_message
+from rosidl_runtime_py.utilities import get_message
 
 # Get arguments
 team_name = sys.argv[1]
@@ -17,25 +19,38 @@
 trial_directory = dir_path + '/../generated/logs/' + team_name + '/' + task_name + '/' + trial_number
 
 # Open ros bag
-bag_name = trial_directory + '/vrx_rostopics.bag'
-bag = rosbag.Bag(bag_name)
+bag_path = trial_directory + '/vrx_rostopics.bag/vrx_rostopics.bag_0.db3'
+storage_options = rosbag2_py.StorageOptions(uri=bag_path, storage_id='sqlite3')
+converter_options = rosbag2_py.ConverterOptions(input_serialization_format='cdr', 
+                                                output_serialization_format='cdr')
+
+reader = rosbag2_py.SequentialReader()
+reader.open(storage_options, converter_options)
+
+topic_types = reader.get_all_topics_and_types()
+
+type_map = {topic_types[i].name: topic_types[i].type for i in range(len(topic_types))}
+
+storage_filter = rosbag2_py.StorageFilter(topics=['/vrx/task/info'])
+reader.set_filter(storage_filter)
 
 # Get last message
 last_topic = None
-last_msg = None
+last_msg_data = None
 last_t = None
 
-for topic, msg, t in bag.read_messages(topics=['/vrx/task/info']):
-    last_topic = topic
-    last_msg = msg
-    last_t = t
+while reader.has_next():
+    (last_topic, last_msg_data, last_t) = reader.read_next() 
 
+msg_type = get_message(type_map[last_topic])
+last_msg = deserialize_message(last_msg_data, msg_type)
+score = None
+for field in last_msg.params:
+  if field.name == "score":
+    score = field.value.double_value
 # Write trial score to file
 trial_score_name = trial_directory + "/trial_score.txt"
-f = open(trial_score_name, 'w+')
-f.write("{}".format(last_msg.score))
+with open(trial_score_name, 'w+') as f:
+  f.write("{}".format(score))
 
 print("Successfully recorded trial score in {}".format(trial_score_name))
-f.close()
-bag.close()
-
diff --git a/vrx_server/build_image.bash b/vrx_server/build_image.bash
@@ -21,7 +21,7 @@ DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
 
 # Parse arguments
 local_base_name="vrx-local-base"
-image_name="vrx-server-jammy"
+image_name="vrx-server-humble"
 
 DOCKER_ARGS="--build-arg BASEIMG=$local_base_name"
 # DOCKER_ARGS="$DOCKER_ARGS --no-cache"

diff --git a/vrx_server/run_container.bash b/vrx_server/run_container.bash
@@ -22,32 +22,10 @@
 set -x
 
 # Parse arguments
-RUNTIME="runc"
-
-POSITIONAL=()
-while [[ $# -gt 0 ]]
-do
-  key="$1"
-
-  case $key in
-      -n|--nvidia)
-      RUNTIME="nvidia"
-      shift
-      ;;
-      *)    # unknown option
-      POSITIONAL+=("$1")
-[ruby $(which gz) sim-1] qt.qpa.plugin: Could not load the Qt platform plugin "xcb" in "" even though it was found.
-      shift
-      ;;
-  esac
-done
-
-set -- "${POSITIONAL[@]}"
-
 if [[ $# -lt 2 ]] 
 then
 [ruby $(which gz) sim-1] qt.qpa.plugin: Could not load the Qt platform plugin "xcb" in "" even though it was found.
-    echo "Usage: $0 [-n --nvidia] <container_name> <image_name> [<docker_extra_args> <command>]"
+    echo "Usage: $0 <container_name> <image_name> [<docker_extra_args> <command>]"
     exit 1
 fi
 

diff --git a/vrx_server/vrx-server/Dockerfile b/vrx_server/vrx-server/Dockerfile
@@ -19,6 +19,22 @@ RUN /bin/sh -c 'echo ". /opt/ros/${ROSDIST}/setup.bash" >> ~/.bashrc' \
  && /bin/sh -c 'echo ". ~/vrx_ws/install/setup.sh" >> ~/.bashrc'
 ## END OF SECTION BASED ON vrx/docker/Dockerfile
 
+# Cache fuel resources
+RUN /bin/bash -c 'gz fuel download -u https://fuel.gazebosim.org/1.0/openrobotics/models/sydney_regatta \
+  && gz fuel download -u https://fuel.gazebosim.org/1.0/openrobotics/models/post \
+  && gz fuel download -u https://fuel.gazebosim.org/1.0/openrobotics/models/ground_station \
+  && gz fuel download -u https://fuel.gazebosim.org/1.0/openrobotics/models/antenna \
+  && gz fuel download -u https://fuel.gazebosim.org/1.0/openrobotics/models/mb_marker_buoy_red \
+  && gz fuel download -u https://fuel.gazebosim.org/1.0/openrobotics/models/mb_marker_buoy_black \
+  && gz fuel download -u https://fuel.gazebosim.org/1.0/openrobotics/models/mb_marker_buoy_green \
+  && gz fuel download -u https://fuel.gazebosim.org/1.0/openrobotics/models/mb_marker_buoy_white \
+  && gz fuel download -u https://fuel.gazebosim.org/1.0/openrobotics/models/mb_round_buoy_orange \
+  && gz fuel download -u https://fuel.gazebosim.org/1.0/openrobotics/models/mb_round_buoy_black \
+  && gz fuel download -u https://fuel.gazebosim.org/1.0/openrobotics/models/platypus \
+  && gz fuel download -u https://fuel.gazebosim.org/1.0/openrobotics/models/crocodile \
+  && gz fuel download -u https://fuel.gazebosim.org/1.0/openrobotics/models/turtle \
+  && gz fuel download -u https://fuel.gazebosim.org/1.0/openrobotics/models/wam-v'
+
 # Expose port used to communiate with gzserver
 EXPOSE 11345
 

diff --git a/vrx_server/vrx-server/gz_utils.sh b/vrx_server/vrx-server/gz_utils.sh
@@ -4,7 +4,7 @@
 
 function is_gzserver_running()
 {
-  if pgrep gzserver > /dev/null; then
+  if pgrep -f "gz sim server" > /dev/null; then
     true
   else
     false
@@ -14,7 +14,7 @@ function is_gzserver_running()
 # Check if gzclient is running
 function is_gzclient_running()
 {
-  if pgrep gzclient > /dev/null; then
+  if pgrep "gz sim client" > /dev/null; then
     true
   else
     false

diff --git a/vrx_server/vrx-server/run_vrx_trial.sh b/vrx_server/vrx-server/run_vrx_trial.sh
@@ -40,29 +40,29 @@ echo "Starting vrx trial..."
 # Run the trial.
 # Note: Increase record period to have faster playback. Decrease record period for slower playback
 RECORD_PERIOD="0.01"
-roslaunch vrx_gazebo vrx.launch gui:=false urdf:=$WAMV_URDF world:=$TRIAL_WORLD extra_gazebo_args:="-r --record_period ${RECORD_PERIOD} --record_path $HOME/.gazebo" verbose:=true non_competition_mode:=false > ~/verbose_output.txt 2>&1 &
+ros2 launch vrx_gz competition.launch.py gui:=false urdf:=$WAMV_URDF world:=$TRIAL_WORLD extra_gazebo_args:="-r --record_period ${RECORD_PERIOD} --record_path $HOME/.gazebo" verbose:=true non_competition_mode:=false > ~/verbose_output.txt 2>&1 &
 roslaunch_pid=$!
 wait_until_gzserver_is_up
 echo -e "${GREEN}OK${NOCOLOR}\n"
 
 # Store topics in rosbag 
 # July 24, 2019 only record task info to save space
 echo "Starting rosbag recording..." 
-rosbag record -O ~/vrx_rostopics.bag /vrx/task/info &
+ros2 bag record -o ~/vrx_rostopics.bag /vrx/task/info &
 echo -e "${GREEN}OK${NOCOLOR}\n"
 
 # Run simulation until shutdown
 echo "Run simulation until gzserver is shutdown by scoring plugin"
 wait_until_gzserver_is_down
-echo "gzserver shut down"
+echo "gz sim server shut down"
 echo -e "${GREEN}OK${NOCOLOR}\n"
 
 # Kill rosnodes
-echo "Killing rosnodes"
-rosnode kill --all
-sleep 1s
+#echo "Killing rosnodes"
+#rosnode kill --all
+#sleep 1s
 
 # Kill roslaunch
-echo "Killing roslaunch pid: ${roslaunch_pid}"
+echo "Killing ros2 launch pid: ${roslaunch_pid}"
 kill -INT ${roslaunch_pid}
 echo -e "${GREEN}OK${NOCOLOR}\n"