diff --git a/README.md b/README.md index abc69ac..dfbce13 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # SINGABOAT-VRX | Virtual RobotX (VRX) Competition - + **COMPETITION:** Virtual RobotX (VRX) Competition 2022 @@ -8,7 +8,7 @@ **INSTITUTE:** Nanyang Technological University, Singapore -**MEMBERS:** Tanmay Vilas Samak, Chinmay Vilas Samak and Chern Peng Lee +**MEMBERS:** Tanmay Samak, Chinmay Samak and Chern Peng Lee **ADVISOR:** Dr. Ming Xie @@ -16,7 +16,7 @@ 1. Robot Operating System (ROS) - Tested with [ROS Noetic Ninjemys](http://wiki.ros.org/noetic) on [Ubuntu 20.04.4 LTS (Focal Fossa)](https://releases.ubuntu.com/20.04/). 2. [Virtual RobotX (VRX)](https://github.com/osrf/vrx) Simulation Environment - Included as `vrx` directory with this `SINGABOAT-VRX` repository. -3. Python3 Dependencies - Listed in `requirements.txt` file of `singaboat_vrx` ROS package. These can be simply installed using the following command: +3. Python3 Dependencies - Listed in `requirements.txt` file of this `SINGABOAT-VRX` repository. These can be simply installed using the following command: ```bash $ pip3 install -r requirements.txt ``` @@ -48,46 +48,18 @@ ## USAGE -1. Launch any VRX task simulation instance. +Launch SINGABOAT-VRX solution(s) for VRX Competition. ```bash -$ roslaunch vrx_gazebo .launch - -$ roslaunch vrx_gazebo station_keeping.launch -$ roslaunch vrx_gazebo wayfinding.launch -$ roslaunch vrx_gazebo perception_task.launch -$ roslaunch vrx_gazebo wildlife.launch -$ roslaunch vrx_gazebo gymkhana.launch -$ roslaunch vrx_gazebo scan_dock_deliver.launch -``` - -2. Launch `singaboat_task_manager` to automatically identify the VRX task and execute the corresponding SINGABOAT-VRX solution algorithm. -```bash -$ roslaunch singaboat_vrx singaboat_vrx.launch - +$ roslaunch singaboat_vrx .launch + +$ roslaunch singaboat_vrx singaboat_station_keeping.launch +$ roslaunch singaboat_vrx singaboat_wayfinding.launch +$ roslaunch singaboat_vrx singaboat_scene_perception.launch +$ roslaunch singaboat_vrx singaboat_semantic_navigation.launch +$ roslaunch singaboat_vrx singaboat_gymkhana_challenge.launch +$ roslaunch singaboat_vrx singaboat_scan_dock_deliver.launch ``` ## DOCKER The docker container image(s) containing all the source code as well as dependencies is available on [Docker Hub](https://hub.docker.com/repository/docker/tinkertwins/singaboat-vrx). - -## VIDEOS - -Demonstration videos are available on [YouTube](https://youtube.com/playlist?list=PLY45pkzWzH982DTT3n9avu5bGrd0cAcfZ). - -| | | -|:-------------------------------------------------------------------------:|:-------------------------------------------------------------------------:| -| [Task Descriptions](https://youtu.be/jI0Y9ovG-Dg) | [Task Solutions](https://youtu.be/x7_Z7U9vJPY) | - -## VRX COMPETITION 2022 - -[Our team worked meticulously](https://github.com/Tinker-Twins/SINGABOAT-VRX/blob/main/media/PNG/Team%20Working%20on%20Tasks.png) to complete all the [6 tasks of VRX Competition 2022](https://github.com/Tinker-Twins/SINGABOAT-VRX/blob/main/media/PNG/Tasks.png) and ultimately emerged as one of the winners of the challange, while also bagging several other special awards. - -| ![Station-Keeping](https://github.com/Tinker-Twins/SINGABOAT-VRX/blob/main/media/GIF/Task%201%20-%20Station-Keeping.gif) | ![Wayfinding](https://github.com/Tinker-Twins/SINGABOAT-VRX/blob/main/media/GIF/Task%202%20-%20Wayfinding.gif) | ![Scene Perception](https://github.com/Tinker-Twins/SINGABOAT-VRX/blob/main/media/GIF/Task%203%20-%20Scene%20Perception.gif) | -|:-----------------------------------------------:|:-------------------------------------------------:|:-----------------------------------------------:| -| [Station-Keeping Task](https://github.com/osrf/vrx/wiki/vrx_2022-station_keeping_task) | [Wayfinding Task](https://github.com/osrf/vrx/wiki/vrx_2022-wayfinding_task) | [Scene Perception Task](https://github.com/osrf/vrx/wiki/vrx_2022-perception_task) | -| ![Semantic Navigation](https://github.com/Tinker-Twins/SINGABOAT-VRX/blob/main/media/GIF/Task%204%20-%20Semantic%20Navigation.gif) | ![Gymkhana Challenge](https://github.com/Tinker-Twins/SINGABOAT-VRX/blob/main/media/GIF/Task%205%20-%20Gymkhana%20Challenge.gif) | ![Scan-Dock-Deliver](https://github.com/Tinker-Twins/SINGABOAT-VRX/blob/main/media/GIF/Task%206%20-%20Scan-Dock-Deliver.gif) | -| [Semantic Navigation Task](https://github.com/osrf/vrx/wiki/vrx_2022-wildlife_task) | [Gymkhana Challenge Task](https://github.com/osrf/vrx/wiki/vrx_2022-gymkhana_task) | [Scan-Dock-Deliver Task](https://github.com/osrf/vrx/wiki/vrx_2022-scan_dock_deliver_task) | - -The detailed scores and ranks of all the teams that qualified for finals of VRX Competition 2022 are available on [VRX GitHub Wiki](https://github.com/osrf/vrx/wiki/vrx_2022-phase3_results), and a summary of results and awards declared during the [VRX 2022 Award Ceremony](https://youtu.be/aVPmrvTCjpg) are available on [VRX Website](https://robotx.org/2022/05/04/vrx-2022-awards-and-final-standings/). - - diff --git a/docker/Dockerfile b/docker/Dockerfile index 6542499..9168a72 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -1,7 +1,7 @@ -# Pull ROS Noetic base image +# Pull ROS Noetic base image. FROM ros:noetic-ros-base -# Insall package dependencies +# Insall package dependencies. RUN echo 'debconf debconf/frontend select Noninteractive' | debconf-set-selections \ && apt update \ && apt install -y \ @@ -16,7 +16,6 @@ RUN echo 'debconf debconf/frontend select Noninteractive' | debconf-set-selectio ros-noetic-key-teleop \ ros-noetic-robot-localization \ ros-noetic-robot-state-publisher \ - ros-noetic-joint-state-publisher \ ros-noetic-rviz \ ros-noetic-teleop-tools \ ros-noetic-teleop-twist-keyboard \ @@ -27,21 +26,15 @@ RUN echo 'debconf debconf/frontend select Noninteractive' | debconf-set-selectio && pip3 install --no-cache-dir --upgrade pip \ && pip3 install --no-cache-dir numpy==1.22.3 scipy==1.8.0 pymap3d==2.7.2 open3d==0.13.0 dubins==1.0.1 -# Set working directory +# Set working directory. WORKDIR /home/$USER -# Install VRX packages +# Install VRX packages. RUN mkdir -p VRX_Workspace/src/ COPY ./ VRX_Workspace/src/ RUN /bin/bash -c 'source /opt/ros/noetic/setup.bash && cd VRX_Workspace && catkin_make' \ && /bin/bash -c 'echo "source /home/$USER/VRX_Workspace/devel/setup.bash" >> ~/.bashrc' \ && /bin/bash -c 'source ~/.bashrc' -# Install VNC server and GUI tools -RUN apt update --fix-missing \ - && apt install -y x11vnc xvfb \ - && mkdir ~/.vnc \ - && x11vnc -storepasswd singaboat-vrx ~/.vnc/passwd - -# Set entrypoint -ENTRYPOINT ["VRX_Workspace/src/SINGABOAT-VRX/singaboat_vrx/scripts/entrypoint.sh"] +# Set entrypoint. +ENTRYPOINT ["VRX_Workspace/src/SINGABOAT-VRX/singaboat_vrx/scripts/singaboat_vrx.sh"] diff --git a/docker/Gazebo_Docker_Config.md b/docker/Gazebo_Docker_Config.md deleted file mode 100644 index 86be05a..0000000 --- a/docker/Gazebo_Docker_Config.md +++ /dev/null @@ -1,20 +0,0 @@ -# Configure Gazebo Simulator Window Inside Docker Container - -1. Change directory to `.gazebo`: -```bash -$ cd -$ cd .gazebo -``` - -2. Edit `gui.ini` file to contain Gazebo simulator window configuration: -```bash -$ nano gui.ini -``` - -3. Enter the following parameters into the configuration file: -```bash -x=0 -y=0 -width=640 -height=720 -``` diff --git a/docker/README.md b/docker/README.md index 08839f2..becd265 100644 --- a/docker/README.md +++ b/docker/README.md @@ -1,110 +1,58 @@ # SINGABOAT-VRX Docker Instructions -#### Building Docker Image from the Dockerfile: - -1. Change directory to `$HOME/VRX_Workspace/src` (catkin workspace for VRX Competition): -```bash -$ cd . -$ cd $HOME/VRX_Workspace/src -``` - -2. Build the docker image from the dockerfile: +1. Change directory to `$HOME/VRX_Workspace/src` (catkin workspace for VRX Competition) and build the docker image: ```bash $ docker build --tag /: -f . -$ docker build --tag tinkertwins/singaboat-vrx:v2022.3 -f SINGABOAT-VRX/docker/Dockerfile . +$ docker build --tag tinkertwins/singaboat-vrx:v2022.2 -f SINGABOAT-VRX/docker/Dockerfile . ``` -#### Contaninerization and Pushing the Container to Docker Hub: - -1. Run the image you created in the previous step inside a container: +2. Create a container with the image you created in the previous step: ```bash $ docker run /: -$ docker run tinkertwins/singaboat-vrx:v2022.3 +$ docker run tinkertwins/singaboat-vrx:v2022.2 ``` -2. In a new terminal window, list all containers and make a note of the desired CONTAINER ID: +3. In a different terminal window, list all containers and take a note of desired CONTAINER ID: ```bash $ docker ps -a ``` -3. Commit changes to Docker Hub: +4. Commit changes to Docker Hub: ```bash $ docker commit -m "" -a "" /: -$ docker commit -m "SINGABOAT-VRX" -a "Tinker Twins" 35b286bf2b9b tinkertwins/singaboat-vrx:v2022.3 +$ docker commit -m "SINGABOAT-VRX" -a "Tinker Twins" 35b286bf2b9b tinkertwins/singaboat-vrx:v2022.2 ``` -4. Login to Docker Hub: +5. Login to Docker Hub: ```bash $ docker login ``` -5. Push the container to Docker Hub, once done, you should be able to see your repository on Docker Hub: +6. Push container image to Docker Hub, once done, you should be able to see your repository on Docker Hub: ```bash $ docker push /: -$ docker push tinkertwins/singaboat-vrx:v2022.3 +$ docker push tinkertwins/singaboat-vrx:v2022.2 ``` -#### Running the Containerized Image: - -1. Run the containerized image (follow either step 1.1 or 1.2): - - 1.1. Run the containerized image in **headless mode** with a name (`--name`) and interactive tty (`-it`), and remove the container upon exiting (`--rm`): - ```bash - $ docker run --rm -it --name /: - $ docker run --rm -it --name singaboat_vrx tinkertwins/singaboat-vrx:v2022.3 - ``` - - 1.2. Run the containerized image in **GUI mode** (requires a VNC viewer) with a name (`--name`) and interactive tty (`-it`), bind TCP port of host to port of the conatainer and publish it to the host system’s interfaces (`-p`), and remove the container upon exiting (`--rm`): - ```bash - $ docker run --rm -it -p --name /: - $ docker run --rm -it -p 5900:5900 --name singaboat_vrx tinkertwins/singaboat-vrx:v2022.3 - ``` - - Launch the VNC viewer (tested with [RealVNC Viewer](https://www.realvnc.com/en/connect/download/viewer/)) and configure the connection parameters (the password for establishing secure connection is `singaboat-vrx`): - - For running VNC viewer on the host machine: - ```bash - $ vncviewer localhost:5900 - ``` - - For running VNC viewer on a remote machine: - ```bash - $ vncviewer ip.address.of.host:5900 - ``` - - You can also configure the connection parameters via GUI of the VNC viewer application. +7. Run the container to test it in interactive tty (i.e., `-it`) mode: +```bash +$ docker run -it --name /: +$ docker run -it --name singaboat_vrx tinkertwins/singaboat-vrx:v2022.2 +``` -2. [Optional] Start additional bash session(s) within the container (each in a new terminal window): +8. In a different terminal window, execute a bash from within the container to access it: ```bash $ docker exec -it bash $ docker exec -it singaboat_vrx bash ``` -3. Once you are done with the intended job, exit the bash session(s): +9. Exit the bash: ```bash $ exit ``` -4. Kill the running container (required only if the containerized image is still running): +10. Kill the container: ```bash $ docker kill $ docker kill singaboat_vrx ``` - -5. Remove the container (required only if the containerized image is not run with the `--rm` option): -```bash -$ docker rm -$ docker rm singaboat_vrx -``` - -#### Cleaning Up Docker Resources: - -Running or caching multiple docker images or containers can quickly eat up a lot of disk space. Hence, it is always a good idea to frequently check docker disk utilization: -```bash -$ docker system df -``` - -In order to avoid utilizing a lot of disk space, it is a good idea to frequently purge docker resources such as images, containers, volumes and networks that are unused or dangling (i.e. not tagged or associated with a container). There are a number of ways with a lot of options to achieve this, please refer appropriate documentation. The easiest way (but a potentially dangerous one) is to use a single command to clean up all the docker resources (dangling or otherwise): -```bash -$ docker system prune -a -``` diff --git a/docker/RViz_Docker_Config.md b/docker/RViz_Docker_Config.md deleted file mode 100644 index 383fa0b..0000000 --- a/docker/RViz_Docker_Config.md +++ /dev/null @@ -1,19 +0,0 @@ -# Configure RViz Window Inside Docker Container - -1. Change directory to `singaboat_vrx/rviz`: -```bash -$ cd /home/$USER/VRX_Workspace/src/SINGABOAT-VRX/singaboat_vrx/rviz -``` - -2. Edit `*.rviz` file to contain RViz window configuration: -```bash -$ nano singaboat_.rviz -``` - -3. Enter the following parameters into the configuration file: -```bash -Height: 720 -Width: 640 -X: 640 -Y: 0 -``` diff --git a/media/GIF/Task 1 - Station-Keeping.gif b/media/GIF/Task 1 - Station-Keeping.gif deleted file mode 100644 index 35e3361..0000000 Binary files a/media/GIF/Task 1 - Station-Keeping.gif and /dev/null differ diff --git a/media/GIF/Task 2 - Wayfinding.gif b/media/GIF/Task 2 - Wayfinding.gif deleted file mode 100644 index 302e0fe..0000000 Binary files a/media/GIF/Task 2 - Wayfinding.gif and /dev/null differ diff --git a/media/GIF/Task 3 - Scene Perception.gif b/media/GIF/Task 3 - Scene Perception.gif deleted file mode 100644 index 52f0890..0000000 Binary files a/media/GIF/Task 3 - Scene Perception.gif and /dev/null differ diff --git a/media/GIF/Task 4 - Semantic Navigation.gif b/media/GIF/Task 4 - Semantic Navigation.gif deleted file mode 100644 index 6cc65fc..0000000 Binary files a/media/GIF/Task 4 - Semantic Navigation.gif and /dev/null differ diff --git a/media/GIF/Task 5 - Gymkhana Challenge.gif b/media/GIF/Task 5 - Gymkhana Challenge.gif deleted file mode 100644 index 049e34e..0000000 Binary files a/media/GIF/Task 5 - Gymkhana Challenge.gif and /dev/null differ diff --git a/media/GIF/Task 6 - Scan-Dock-Deliver.gif b/media/GIF/Task 6 - Scan-Dock-Deliver.gif deleted file mode 100644 index 0b91a1e..0000000 Binary files a/media/GIF/Task 6 - Scan-Dock-Deliver.gif and /dev/null differ diff --git a/media/PNG/Awards.png b/media/PNG/Awards.png deleted file mode 100644 index a64413c..0000000 Binary files a/media/PNG/Awards.png and /dev/null differ diff --git a/media/PNG/Tasks.png b/media/PNG/Tasks.png deleted file mode 100644 index 6ccb571..0000000 Binary files a/media/PNG/Tasks.png and /dev/null differ diff --git a/media/PNG/Team Working on Tasks.png b/media/PNG/Team Working on Tasks.png deleted file mode 100644 index fceec17..0000000 Binary files a/media/PNG/Team Working on Tasks.png and /dev/null differ diff --git a/media/PNG/Team.png b/media/PNG/Team.png deleted file mode 100644 index e048ba2..0000000 Binary files a/media/PNG/Team.png and /dev/null differ diff --git a/singaboat_vrx/CMakeLists.txt b/singaboat_vrx/CMakeLists.txt index 9f334a2..73dc9e9 100755 --- a/singaboat_vrx/CMakeLists.txt +++ b/singaboat_vrx/CMakeLists.txt @@ -35,7 +35,8 @@ catkin_python_setup() # Dynamic reconfigure options generate_dynamic_reconfigure_options( cfg/teleoperation_config.cfg - cfg/inverse_kinematics_config.cfg + cfg/pose_processor_config.cfg + cfg/ik_solver_config.cfg cfg/task_manager_config.cfg cfg/mission_manager_config.cfg cfg/station_keeping_config.cfg @@ -91,7 +92,8 @@ catkin_package(CATKIN_DEPENDS # Install all the script files catkin_install_python(PROGRAMS src/singaboat_teleoperation.py - src/singaboat_inverse_kinematics.py + src/singaboat_pose_processor.py + src/singaboat_ik_solver.py src/singaboat_mission_manager.py src/singaboat_task_manager.py src/singaboat_station_keeping.py @@ -102,13 +104,13 @@ catkin_install_python(PROGRAMS src/singaboat_scan_dock_deliver.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) -# Install all the script config files +# Install all the cfg files install(DIRECTORY cfg/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/cfg) -# Install all the WAM-V config files -install(DIRECTORY wamv/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/wamv) +# Install all the config files +install(DIRECTORY config/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config) # Install all the world files install(DIRECTORY worlds/ diff --git a/singaboat_vrx/README.md b/singaboat_vrx/README.md index ab4ad95..377a6a6 100644 --- a/singaboat_vrx/README.md +++ b/singaboat_vrx/README.md @@ -1,6 +1,6 @@ -# ROS Package of Team SINGABOAT-VRX for VRX Competition +# SINGABOAT-VRX | Virtual RobotX (VRX) Competition - + **COMPETITION:** Virtual RobotX (VRX) Competition 2022 @@ -8,17 +8,15 @@ **INSTITUTE:** Nanyang Technological University, Singapore -**MEMBERS:** Tanmay Vilas Samak, Chinmay Vilas Samak and Chern Peng Lee +**MEMBERS:** Tanmay Samak, Chinmay Samak and Chern Peng Lee **ADVISOR:** Dr. Ming Xie -## USAGE +## ROS Package of Team SINGABOAT-VRX for Virtual RobotX (VRX) Competition -#### Simulate, Execute and Visualize (or not) SINGABOAT-VRX Solution(s) to VRX Tasks - -Launching Gazebo, RViz and SINGABOAT-VRX solution(s) for VRX Competition. +Launch SINGABOAT-VRX solution(s) for VRX Competition. ```bash -$ roslaunch singaboat_vrx singaboat_.launch +$ roslaunch singaboat_vrx .launch $ roslaunch singaboat_vrx singaboat_station_keeping.launch $ roslaunch singaboat_vrx singaboat_wayfinding.launch @@ -27,83 +25,3 @@ $ roslaunch singaboat_vrx singaboat_semantic_navigation.launch $ roslaunch singaboat_vrx singaboat_gymkhana_challenge.launch $ roslaunch singaboat_vrx singaboat_scan_dock_deliver.launch ``` - -In order to run SINGABOAT-VRX solution(s) in headless mode (i.e., without visualizing Gazebo and RViz), make use of the `headless` argument. -```bash -$ roslaunch singaboat_vrx singaboat_.launch headless:=true - -$ roslaunch singaboat_vrx singaboat_station_keeping.launch headless:=true -$ roslaunch singaboat_vrx singaboat_wayfinding.launch headless:=true -$ roslaunch singaboat_vrx singaboat_scene_perception.launch headless:=true -$ roslaunch singaboat_vrx singaboat_semantic_navigation.launch headless:=true -$ roslaunch singaboat_vrx singaboat_gymkhana_challenge.launch headless:=true -$ roslaunch singaboat_vrx singaboat_scan_dock_deliver.launch headless:=true -``` - -#### Execute SINGABOAT-VRX Submission(s) to VRX Competition - -1. Launching only SINGABOAT-VRX solution(s) for VRX Competition (a running VRX task simulation instance expected). -```bash -$ roslaunch singaboat_vrx singaboat_vrx.launch - -``` - -OR - -2. Launching only SINGABOAT-VRX solution for a particular VRX task (a running VRX task simulation instance expected). -```bash -$ roslaunch singaboat_vrx vrx_.launch - -$ roslaunch singaboat_vrx vrx_station_keeping.launch -$ roslaunch singaboat_vrx vrx_wayfinding.launch -$ roslaunch singaboat_vrx vrx_scene_perception.launch -$ roslaunch singaboat_vrx vrx_semantic_navigation.launch -$ roslaunch singaboat_vrx vrx_gymkhana_challenge.launch -$ roslaunch singaboat_vrx vrx_scan_dock_deliver.launch -``` - -#### Record SINGABOAT-VRX Solution(s) to VRX Tasks - -Recording a SINGABOAT-VRX solution simulation instance. -```bash -$ roslaunch singaboat_vrx record_.launch - -$ roslaunch singaboat_vrx record_station_keeping.launch -$ roslaunch singaboat_vrx record_wayfinding.launch -$ roslaunch singaboat_vrx record_scene_perception.launch -$ roslaunch singaboat_vrx record_semantic_navigation.launch -$ roslaunch singaboat_vrx record_gymkhana_challenge.launch -$ roslaunch singaboat_vrx record_scan_dock_deliver.launch -``` - -#### Playback a Recorded SINGABOAT-VRX Solution(s) to VRX Tasks - -Playing back a recorded SINGABOAT-VRX solution simulation instance. -```bash -$ roslaunch singaboat_vrx playback_.launch - -$ roslaunch singaboat_vrx playback_station_keeping.launch -$ roslaunch singaboat_vrx playback_wayfinding.launch -$ roslaunch singaboat_vrx playback_scene_perception.launch -$ roslaunch singaboat_vrx playback_semantic_navigation.launch -$ roslaunch singaboat_vrx playback_gymkhana_challenge.launch -$ roslaunch singaboat_vrx playback_scan_dock_deliver.launch -``` - -#### SINGABOAT-VRX Tools for Prototyping and Debugging Algorithms - -Launching SINGABOAT-VRX tool(s) for prototyping and debugging VRX solution algorithms, which include: -1. `singaboat_build_wamv.launch` - Builds URDF model of SINGABOAT (WAM-V) using `component_config.yaml` and `thruster_config.yaml`. -2. `singaboat_vrx_world.launch` - Launches Gazebo Simulator instance with SINGABOAT (WAM-V) URDF model and all VRX task elements. -3. `singaboat_vrx_rviz.launch` - Launches RViz instance enabling real-time visualization of all WAM-V states and sensor data. -4. `singaboat_teleoperation.launch` - Launches `singaboat_teleoperation` node to teleoperate the WAM-V using standard PC keyboard. -5. `singaboat_inverse_kinematics.launch` - Launches `singaboat_inverse_kinematics` node along with `rqt_publisher` and `rqt_reconfigure` to work on or debug IK module of the WAM-V separately. -```bash -$ roslaunch singaboat_vrx singaboat_.launch - -$ roslaunch singaboat_vrx singaboat_build_wamv.launch -$ roslaunch singaboat_vrx singaboat_vrx_world.launch -$ roslaunch singaboat_vrx singaboat_vrx_rviz.launch -$ roslaunch singaboat_vrx singaboat_teleoperation.launch -$ roslaunch singaboat_vrx singaboat_inverse_kinematics.launch -``` diff --git a/singaboat_vrx/bags/README.md b/singaboat_vrx/bags/README.md deleted file mode 100644 index 569ee67..0000000 --- a/singaboat_vrx/bags/README.md +++ /dev/null @@ -1,33 +0,0 @@ -# SINGABOAT-VRX | Bag Files - -Record and playback bag files for all SINGABOAT-VRX simulation tasks with the `rosbag` command line interface (CLI). Note that the `record` launch files provided in the `singaboat-vrx` package will export recorded bag files to the `bags` directory and the `playback` launch files provided in the `singaboat-vrx` package will read recorded bag files from the `bags` directory. - -***Note**: The recorded ROS bags have been removed to limit the size of this repository.* - -## Recording New Bag Files - -Launch SINGABOAT-VRX `record` launch files. -```bash -$ roslaunch singaboat_vrx record_.launch - -$ roslaunch singaboat_vrx record_station_keeping.launch -$ roslaunch singaboat_vrx record_wayfinding.launch -$ roslaunch singaboat_vrx record_scene_perception.launch -$ roslaunch singaboat_vrx record_semantic_navigation.launch -$ roslaunch singaboat_vrx record_gymkhana_challenge.launch -$ roslaunch singaboat_vrx record_scan_dock_deliver.launch -``` - -## Playing Back Recorded Bag Files - -Launch SINGABOAT-VRX `playback` launch files. -```bash -$ roslaunch singaboat_vrx playback_.launch - -$ roslaunch singaboat_vrx playback_station_keeping.launch -$ roslaunch singaboat_vrx playback_wayfinding.launch -$ roslaunch singaboat_vrx playback_scene_perception.launch -$ roslaunch singaboat_vrx playback_semantic_navigation.launch -$ roslaunch singaboat_vrx playback_gymkhana_challenge.launch -$ roslaunch singaboat_vrx playback_scan_dock_deliver.launch -``` diff --git a/singaboat_vrx/cfg/gymkhana_challenge_config.cfg b/singaboat_vrx/cfg/gymkhana_challenge_config.cfg index 3634d83..5e3cc9c 100644 --- a/singaboat_vrx/cfg/gymkhana_challenge_config.cfg +++ b/singaboat_vrx/cfg/gymkhana_challenge_config.cfg @@ -12,6 +12,4 @@ gen.add("max_goal_tol", double_t, 0, "Maximum goal tolerance (for finding pinger gen.add("collision_tol", double_t, 0, "Obstacle range tolerance for collision avoidance", 17.5, 0, 25.0) gen.add("collision_fov", double_t, 0, "Obstacle field of view for collision avoidance", 40.0, 0, 90.0) -gen.add("debug", bool_t, 0, "Flag to enable/disable debug messages", True) - exit(gen.generate(PACKAGE, "singaboat_vrx", "GymkhanaChallenge")) diff --git a/singaboat_vrx/cfg/inverse_kinematics_config.cfg b/singaboat_vrx/cfg/ik_solver_config.cfg similarity index 82% rename from singaboat_vrx/cfg/inverse_kinematics_config.cfg rename to singaboat_vrx/cfg/ik_solver_config.cfg index 096641b..3749005 100644 --- a/singaboat_vrx/cfg/inverse_kinematics_config.cfg +++ b/singaboat_vrx/cfg/ik_solver_config.cfg @@ -13,6 +13,4 @@ gen.add("v_y_gain", double_t, 0, "Linear velocity gain along Y-axis", 0.35, 0, 1 gen.add("w_z_gain", double_t, 0, "Angular velocity gain about Z-axis", 1.2, 0, 5.0) gen.add("half_beam", double_t, 0, "Half beam width of WAM-V", 1.027135, 0, 5.0) -gen.add("debug", bool_t, 0, "Flag to enable/disable debug messages", True) - -exit(gen.generate(PACKAGE, "singaboat_vrx", "InverseKinematics")) +exit(gen.generate(PACKAGE, "singaboat_vrx", "IKSolver")) diff --git a/singaboat_vrx/cfg/mission_manager_config.cfg b/singaboat_vrx/cfg/mission_manager_config.cfg index 7eb59e7..c49c9fc 100644 --- a/singaboat_vrx/cfg/mission_manager_config.cfg +++ b/singaboat_vrx/cfg/mission_manager_config.cfg @@ -6,11 +6,6 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() -gen.add("min_speed", double_t, 0, "Minimum longitudinal velocity of ASV", 1.0, 0, 2.23) -gen.add("max_speed", double_t, 0, "Maximum longitudinal velocity of ASV", 2.23, 0, 2.23) -gen.add("gps_offset", double_t, 0, "GPS offset w.r.t. WAM-V along X-axis", 0.85, 0, 2.0) -gen.add("pose_tol", double_t, 0, "Pose tolerance to determine whether goal is reached", 0.5, 0, 1.0) - gen.add("H2G_kP", double_t, 0, "Proportional gain of PD controller", 0.45, 0, 1.0) gen.add("H2G_kD", double_t, 0, "Derivative gain of PD controller", 0.08, 0, 1.0) @@ -38,6 +33,4 @@ gen.add("goal_tol", double_t, 0, "Goal tolerance dead-band of go-to-goal PID con gen.add("v_const", double_t, 0, "Proportional gain for linear velocity outside goal tolerance", 5.7, 0, 10.0) gen.add("v_limit", double_t, 0, "Saturation limit for linear velocity outside goal tolerance", 2.23, 0, 2.23) -gen.add("debug", bool_t, 0, "Flag to enable/disable debug messages", False) - exit(gen.generate(PACKAGE, "singaboat_vrx", "MissionManager")) diff --git a/singaboat_vrx/cfg/pose_processor_config.cfg b/singaboat_vrx/cfg/pose_processor_config.cfg new file mode 100644 index 0000000..399d639 --- /dev/null +++ b/singaboat_vrx/cfg/pose_processor_config.cfg @@ -0,0 +1,11 @@ +#!/usr/bin/env python + +PACKAGE = "singaboat_vrx" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +gen.add("gps_offset", double_t, 0, "GPS offset w.r.t. WAM-V along X-axis", 0.85, 0, 2.0) + +exit(gen.generate(PACKAGE, "singaboat_vrx", "PoseProcessor")) diff --git a/singaboat_vrx/cfg/scan_dock_deliver_config.cfg b/singaboat_vrx/cfg/scan_dock_deliver_config.cfg index 639f6e9..db52483 100644 --- a/singaboat_vrx/cfg/scan_dock_deliver_config.cfg +++ b/singaboat_vrx/cfg/scan_dock_deliver_config.cfg @@ -6,18 +6,6 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() -gen.add("k_rho", double_t, 0, "Proportional gain for rho", 0.2, 0, 1.0) -gen.add("k_alpha", double_t, 0, "Proportional gain for alpha", 3.6, 0, 5.0) -gen.add("k_beta", double_t, 0, "Proportional gain for beta", 0.1, 0, 1.0) -gen.add("max_lin_vel", double_t, 0, "Maximum allowed linear velocity in m/s", 2.23, 0, 2.23) -gen.add("max_ang_vel", double_t, 0, "Maximum allowed angular velocity in rad/s", 0.89, 0, 0.89) gen.add("gps_offset", double_t, 0, "GPS offset w.r.t. WAM-V along X-axis", 0.85, 0, 2.0) -gen.add("lidar_offset", double_t, 0, "LIDAR offset w.r.t. WAM-V along X-axis", 0.7, 0, 2.0) -gen.add("caution_distance", double_t, 0, "Caution distance from target pose", 9.0, 0, 10.0) -gen.add("caution_step", double_t, 0, "Step size to decrement caution distance from target pose", 3.0, 0, 10.0) -gen.add("ball_balance", int_t, 0, "Number of balls yet to be shot from ASV", 4, 0, 4) -gen.add("delivery_delay", double_t, 0, "Time delay in seconds from docking to delivery", 7.0, 0, 15.0) - -gen.add("debug", bool_t, 0, "Flag to enable/disable debug messages", False) exit(gen.generate(PACKAGE, "singaboat_vrx", "ScanDockDeliver")) diff --git a/singaboat_vrx/cfg/scene_perception_config.cfg b/singaboat_vrx/cfg/scene_perception_config.cfg index 64eefd2..a08437b 100644 --- a/singaboat_vrx/cfg/scene_perception_config.cfg +++ b/singaboat_vrx/cfg/scene_perception_config.cfg @@ -11,7 +11,7 @@ gen.add("max_gate_width", double_t, 0, "Maximum width of channel gate", 25.0, 0, gen.add("dist_to_gate", double_t, 0, "Distance between the detected gate and WAM-V frame", 25.0, 0, 30.0) gen.add("gps_offset", double_t, 0, "GPS offset w.r.t. WAM-V along X-axis", 0.85, 0, 2.0) gen.add("lidar_offset", double_t, 0, "LIDAR offset w.r.t. WAM-V along X-axis", 0.7, 0, 2.0) - -gen.add("debug", bool_t, 0, "Flag to enable/disable debug messages", False) +gen.add("buoy_detector_debug", bool_t, 0, "Flag to enable/disable buoy detector debug messages", False) +gen.add("gate_detector_debug", bool_t, 0, "Flag to enable/disable gate detector debug messages", False) exit(gen.generate(PACKAGE, "singaboat_vrx", "ScenePerception")) diff --git a/singaboat_vrx/cfg/semantic_navigation_config.cfg b/singaboat_vrx/cfg/semantic_navigation_config.cfg index dd3c008..e7ba988 100644 --- a/singaboat_vrx/cfg/semantic_navigation_config.cfg +++ b/singaboat_vrx/cfg/semantic_navigation_config.cfg @@ -16,7 +16,6 @@ gen.add("gps_offset", double_t, 0, "GPS offset w.r.t. WAM-V along X-axis", 0.85, gen.add("min_radius", double_t, 0, "Minimum radius for circling an animal", 2.0, 0, 5.0) gen.add("max_radius", double_t, 0, "Maximum radius for circling an animal", 10.0, 0, 10.0) gen.add("thresh_dist", double_t, 0, "Threshold distance to consider ASV being near an animal", 20.0, 0, 25.0) - gen.add("debug", bool_t, 0, "Flag to enable/disable debug messages", False) exit(gen.generate(PACKAGE, "singaboat_vrx", "SemanticNavigation")) diff --git a/singaboat_vrx/cfg/station_keeping_config.cfg b/singaboat_vrx/cfg/station_keeping_config.cfg index 9c7ffe4..fabd442 100644 --- a/singaboat_vrx/cfg/station_keeping_config.cfg +++ b/singaboat_vrx/cfg/station_keeping_config.cfg @@ -6,8 +6,6 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() -gen.add("gps_offset", double_t, 0, "GPS offset w.r.t. WAM-V along X-axis", 0.85, 0, 2.0) - gen.add("G2G_kP", double_t, 0, "Proportional gain of PID controller", 22.0, 0, 25.0) gen.add("G2G_kI", double_t, 0, "Integral gain of PID controller", 0.27, 0, 1.0) gen.add("G2G_kD", double_t, 0, "Derivative gain of PID controller", 2.4, 0, 5.0) @@ -32,6 +30,4 @@ gen.add("goal_tol", double_t, 0, "Goal tolerance dead-band of go-to-goal PID con gen.add("v_const", double_t, 0, "Proportional gain for linear velocity outside goal tolerance", 5.7, 0, 10.0) gen.add("v_limit", double_t, 0, "Saturation limit for linear velocity outside goal tolerance", 2.23, 0, 2.23) -gen.add("debug", bool_t, 0, "Flag to enable/disable debug messages", True) - exit(gen.generate(PACKAGE, "singaboat_vrx", "StationKeeping")) diff --git a/singaboat_vrx/cfg/task_manager_config.cfg b/singaboat_vrx/cfg/task_manager_config.cfg index 1d3037e..2a65509 100644 --- a/singaboat_vrx/cfg/task_manager_config.cfg +++ b/singaboat_vrx/cfg/task_manager_config.cfg @@ -13,6 +13,4 @@ gen.add("task_4_launch_path", str_t, 0, "Path to semantic navigation task launch gen.add("task_5_launch_path", str_t, 0, "Path to gymkhana challenge task launch file", "/launch/vrx_gymkhana_challenge.launch") gen.add("task_6_launch_path", str_t, 0, "Path to scan-dock-deliver task launch file", "/launch/vrx_scan_dock_deliver.launch") -gen.add("debug", bool_t, 0, "Flag to enable/disable debug messages", True) - exit(gen.generate(PACKAGE, "singaboat_vrx", "TaskManager")) diff --git a/singaboat_vrx/cfg/teleoperation_config.cfg b/singaboat_vrx/cfg/teleoperation_config.cfg index ff15560..b8a2d6c 100644 --- a/singaboat_vrx/cfg/teleoperation_config.cfg +++ b/singaboat_vrx/cfg/teleoperation_config.cfg @@ -9,6 +9,4 @@ gen = ParameterGenerator() gen.add("thrust_limit", double_t, 0, "Maximum permitted thrust command for ASV", 1.0, 0, 1.0) gen.add("thrust_step", double_t, 0, "Stepping amount to alter thrust command for ASV", 0.2, 0, 1.0) -gen.add("debug", bool_t, 0, "Flag to enable/disable debug messages", True) - exit(gen.generate(PACKAGE, "singaboat_vrx", "Teleoperation")) diff --git a/singaboat_vrx/cfg/wayfinding_config.cfg b/singaboat_vrx/cfg/wayfinding_config.cfg index 687dfc0..471df57 100644 --- a/singaboat_vrx/cfg/wayfinding_config.cfg +++ b/singaboat_vrx/cfg/wayfinding_config.cfg @@ -6,8 +6,6 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() -gen.add("gps_offset", double_t, 0, "GPS offset w.r.t. WAM-V along X-axis", 0.85, 0, 2.0) - gen.add("G2G_kP", double_t, 0, "Proportional gain of PID controller", 22.0, 0, 25.0) gen.add("G2G_kI", double_t, 0, "Integral gain of PID controller", 0.27, 0, 1.0) gen.add("G2G_kD", double_t, 0, "Derivative gain of PID controller", 2.4, 0, 5.0) @@ -34,6 +32,4 @@ gen.add("v_limit", double_t, 0, "Saturation limit for linear velocity outside go gen.add("pos_tol", double_t, 0, "Position tolerance to determine whether goal is reached", 0.5, 0, 1.0) gen.add("rot_tol", double_t, 0, "Orientation tolerance to determine whether goal is reached", 0.1, 0, 0.5) -gen.add("debug", bool_t, 0, "Flag to enable/disable debug messages", True) - exit(gen.generate(PACKAGE, "singaboat_vrx", "Wayfinding")) diff --git a/singaboat_vrx/wamv/component_config.xacro b/singaboat_vrx/config/component_config.xacro similarity index 87% rename from singaboat_vrx/wamv/component_config.xacro rename to singaboat_vrx/config/component_config.xacro index 3003ef0..4cf7510 100644 --- a/singaboat_vrx/wamv/component_config.xacro +++ b/singaboat_vrx/config/component_config.xacro @@ -14,7 +14,7 @@ - + diff --git a/singaboat_vrx/wamv/component_config.yaml b/singaboat_vrx/config/component_config.yaml similarity index 97% rename from singaboat_vrx/wamv/component_config.yaml rename to singaboat_vrx/config/component_config.yaml index 8d2fe70..e75f3be 100644 --- a/singaboat_vrx/wamv/component_config.yaml +++ b/singaboat_vrx/config/component_config.yaml @@ -35,7 +35,7 @@ wamv_imu: wamv_ball_shooter: - name: ball_shooter x: 0.55 - y: 0.1 + y: -0.1 z: 1.3 pitch: ${radians(-45)} yaw: 0.0 diff --git a/singaboat_vrx/wamv/thruster_config.xacro b/singaboat_vrx/config/thruster_config.xacro similarity index 100% rename from singaboat_vrx/wamv/thruster_config.xacro rename to singaboat_vrx/config/thruster_config.xacro diff --git a/singaboat_vrx/wamv/thruster_config.yaml b/singaboat_vrx/config/thruster_config.yaml similarity index 100% rename from singaboat_vrx/wamv/thruster_config.yaml rename to singaboat_vrx/config/thruster_config.yaml diff --git a/singaboat_vrx/launch/playback/playback_gymkhana_challenge.launch b/singaboat_vrx/launch/playback/playback_gymkhana_challenge.launch deleted file mode 100644 index 58933f2..0000000 --- a/singaboat_vrx/launch/playback/playback_gymkhana_challenge.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/singaboat_vrx/launch/playback/playback_scan_dock_deliver.launch b/singaboat_vrx/launch/playback/playback_scan_dock_deliver.launch deleted file mode 100644 index 57f8b83..0000000 --- a/singaboat_vrx/launch/playback/playback_scan_dock_deliver.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/singaboat_vrx/launch/playback/playback_scene_perception.launch b/singaboat_vrx/launch/playback/playback_scene_perception.launch deleted file mode 100644 index 33f30a0..0000000 --- a/singaboat_vrx/launch/playback/playback_scene_perception.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/singaboat_vrx/launch/playback/playback_semantic_navigation.launch b/singaboat_vrx/launch/playback/playback_semantic_navigation.launch deleted file mode 100644 index 56bfd3b..0000000 --- a/singaboat_vrx/launch/playback/playback_semantic_navigation.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/singaboat_vrx/launch/playback/playback_station_keeping.launch b/singaboat_vrx/launch/playback/playback_station_keeping.launch deleted file mode 100644 index 9df9e1a..0000000 --- a/singaboat_vrx/launch/playback/playback_station_keeping.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/singaboat_vrx/launch/playback/playback_wayfinding.launch b/singaboat_vrx/launch/playback/playback_wayfinding.launch deleted file mode 100644 index 3a64345..0000000 --- a/singaboat_vrx/launch/playback/playback_wayfinding.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/singaboat_vrx/launch/tools/singaboat_build_wamv.launch b/singaboat_vrx/launch/singaboat_build_wamv.launch similarity index 79% rename from singaboat_vrx/launch/tools/singaboat_build_wamv.launch rename to singaboat_vrx/launch/singaboat_build_wamv.launch index 23daff4..3b0ea28 100644 --- a/singaboat_vrx/launch/tools/singaboat_build_wamv.launch +++ b/singaboat_vrx/launch/singaboat_build_wamv.launch @@ -3,8 +3,8 @@ - - + + diff --git a/singaboat_vrx/launch/record/record_gymkhana_challenge.launch b/singaboat_vrx/launch/singaboat_gymkhana_challenge.launch similarity index 77% rename from singaboat_vrx/launch/record/record_gymkhana_challenge.launch rename to singaboat_vrx/launch/singaboat_gymkhana_challenge.launch index 7cf5afe..1300186 100644 --- a/singaboat_vrx/launch/record/record_gymkhana_challenge.launch +++ b/singaboat_vrx/launch/singaboat_gymkhana_challenge.launch @@ -28,9 +28,6 @@ - - - @@ -42,13 +39,10 @@ - - - - - - - - + + + + + diff --git a/singaboat_vrx/launch/singaboat_ik_solver.launch b/singaboat_vrx/launch/singaboat_ik_solver.launch new file mode 100644 index 0000000..8fa5dd1 --- /dev/null +++ b/singaboat_vrx/launch/singaboat_ik_solver.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/singaboat_vrx/launch/singaboat_pose_processor.launch b/singaboat_vrx/launch/singaboat_pose_processor.launch new file mode 100644 index 0000000..a26b851 --- /dev/null +++ b/singaboat_vrx/launch/singaboat_pose_processor.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/singaboat_vrx/launch/record/record_scan_dock_deliver.launch b/singaboat_vrx/launch/singaboat_scan_dock_deliver.launch similarity index 69% rename from singaboat_vrx/launch/record/record_scan_dock_deliver.launch rename to singaboat_vrx/launch/singaboat_scan_dock_deliver.launch index 4008331..fd3dca8 100644 --- a/singaboat_vrx/launch/record/record_scan_dock_deliver.launch +++ b/singaboat_vrx/launch/singaboat_scan_dock_deliver.launch @@ -6,8 +6,8 @@ - - + + @@ -28,9 +28,6 @@ - - - @@ -39,13 +36,10 @@ - - - - - - - - + + + + + diff --git a/singaboat_vrx/launch/record/record_scene_perception.launch b/singaboat_vrx/launch/singaboat_scene_perception.launch similarity index 72% rename from singaboat_vrx/launch/record/record_scene_perception.launch rename to singaboat_vrx/launch/singaboat_scene_perception.launch index 189dc54..c3fa683 100644 --- a/singaboat_vrx/launch/record/record_scene_perception.launch +++ b/singaboat_vrx/launch/singaboat_scene_perception.launch @@ -6,9 +6,9 @@ - + - + @@ -28,18 +28,12 @@ - - - - - - - - - - + + + + diff --git a/singaboat_vrx/launch/record/record_semantic_navigation.launch b/singaboat_vrx/launch/singaboat_semantic_navigation.launch similarity index 74% rename from singaboat_vrx/launch/record/record_semantic_navigation.launch rename to singaboat_vrx/launch/singaboat_semantic_navigation.launch index f4ea001..064cbbc 100644 --- a/singaboat_vrx/launch/record/record_semantic_navigation.launch +++ b/singaboat_vrx/launch/singaboat_semantic_navigation.launch @@ -28,21 +28,15 @@ - - - - - - - - - - - + + + + + diff --git a/singaboat_vrx/launch/record/record_station_keeping.launch b/singaboat_vrx/launch/singaboat_station_keeping.launch similarity index 75% rename from singaboat_vrx/launch/record/record_station_keeping.launch rename to singaboat_vrx/launch/singaboat_station_keeping.launch index 4a0dc04..bc92f61 100644 --- a/singaboat_vrx/launch/record/record_station_keeping.launch +++ b/singaboat_vrx/launch/singaboat_station_keeping.launch @@ -16,6 +16,7 @@ + @@ -28,21 +29,18 @@ - - - + + + - - - - - - - - + + + + + diff --git a/singaboat_vrx/launch/tools/singaboat_teleoperation.launch b/singaboat_vrx/launch/singaboat_teleoperation.launch similarity index 100% rename from singaboat_vrx/launch/tools/singaboat_teleoperation.launch rename to singaboat_vrx/launch/singaboat_teleoperation.launch diff --git a/singaboat_vrx/launch/submissions/singaboat_vrx.launch b/singaboat_vrx/launch/singaboat_vrx.launch similarity index 100% rename from singaboat_vrx/launch/submissions/singaboat_vrx.launch rename to singaboat_vrx/launch/singaboat_vrx.launch diff --git a/singaboat_vrx/launch/tools/singaboat_vrx_rviz.launch b/singaboat_vrx/launch/singaboat_vrx_rviz.launch similarity index 83% rename from singaboat_vrx/launch/tools/singaboat_vrx_rviz.launch rename to singaboat_vrx/launch/singaboat_vrx_rviz.launch index 6cf397b..39411ee 100644 --- a/singaboat_vrx/launch/tools/singaboat_vrx_rviz.launch +++ b/singaboat_vrx/launch/singaboat_vrx_rviz.launch @@ -2,13 +2,13 @@ - + + + + - - - diff --git a/singaboat_vrx/launch/tools/singaboat_vrx_world.launch b/singaboat_vrx/launch/singaboat_vrx_world.launch similarity index 100% rename from singaboat_vrx/launch/tools/singaboat_vrx_world.launch rename to singaboat_vrx/launch/singaboat_vrx_world.launch diff --git a/singaboat_vrx/launch/record/record_wayfinding.launch b/singaboat_vrx/launch/singaboat_wayfinding.launch similarity index 74% rename from singaboat_vrx/launch/record/record_wayfinding.launch rename to singaboat_vrx/launch/singaboat_wayfinding.launch index 9ae526b..2a1ceba 100644 --- a/singaboat_vrx/launch/record/record_wayfinding.launch +++ b/singaboat_vrx/launch/singaboat_wayfinding.launch @@ -1,7 +1,7 @@ - + @@ -28,21 +28,18 @@ - - - + + + - - - - - - - - + + + + + diff --git a/singaboat_vrx/launch/submissions/vrx_semantic_navigation.launch b/singaboat_vrx/launch/submissions/vrx_semantic_navigation.launch deleted file mode 100644 index 34c6be8..0000000 --- a/singaboat_vrx/launch/submissions/vrx_semantic_navigation.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - diff --git a/singaboat_vrx/launch/submissions/vrx_station_keeping.launch b/singaboat_vrx/launch/submissions/vrx_station_keeping.launch deleted file mode 100644 index 1642703..0000000 --- a/singaboat_vrx/launch/submissions/vrx_station_keeping.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - diff --git a/singaboat_vrx/launch/submissions/vrx_wayfinding.launch b/singaboat_vrx/launch/submissions/vrx_wayfinding.launch deleted file mode 100644 index 0fc04db..0000000 --- a/singaboat_vrx/launch/submissions/vrx_wayfinding.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - diff --git a/singaboat_vrx/launch/tasks/singaboat_gymkhana_challenge.launch b/singaboat_vrx/launch/tasks/singaboat_gymkhana_challenge.launch deleted file mode 100644 index cb3d3f4..0000000 --- a/singaboat_vrx/launch/tasks/singaboat_gymkhana_challenge.launch +++ /dev/null @@ -1,69 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/singaboat_vrx/launch/tasks/singaboat_scan_dock_deliver.launch b/singaboat_vrx/launch/tasks/singaboat_scan_dock_deliver.launch deleted file mode 100644 index 79f01fc..0000000 --- a/singaboat_vrx/launch/tasks/singaboat_scan_dock_deliver.launch +++ /dev/null @@ -1,66 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/singaboat_vrx/launch/tasks/singaboat_scene_perception.launch b/singaboat_vrx/launch/tasks/singaboat_scene_perception.launch deleted file mode 100644 index d7ddf40..0000000 --- a/singaboat_vrx/launch/tasks/singaboat_scene_perception.launch +++ /dev/null @@ -1,60 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/singaboat_vrx/launch/tasks/singaboat_semantic_navigation.launch b/singaboat_vrx/launch/tasks/singaboat_semantic_navigation.launch deleted file mode 100644 index 2e8fe62..0000000 --- a/singaboat_vrx/launch/tasks/singaboat_semantic_navigation.launch +++ /dev/null @@ -1,63 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/singaboat_vrx/launch/tasks/singaboat_station_keeping.launch b/singaboat_vrx/launch/tasks/singaboat_station_keeping.launch deleted file mode 100644 index 295af43..0000000 --- a/singaboat_vrx/launch/tasks/singaboat_station_keeping.launch +++ /dev/null @@ -1,63 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/singaboat_vrx/launch/tasks/singaboat_wayfinding.launch b/singaboat_vrx/launch/tasks/singaboat_wayfinding.launch deleted file mode 100644 index 5a40596..0000000 --- a/singaboat_vrx/launch/tasks/singaboat_wayfinding.launch +++ /dev/null @@ -1,63 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/singaboat_vrx/launch/tools/singaboat_inverse_kinematics.launch b/singaboat_vrx/launch/tools/singaboat_inverse_kinematics.launch deleted file mode 100644 index 9af9ba0..0000000 --- a/singaboat_vrx/launch/tools/singaboat_inverse_kinematics.launch +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - - diff --git a/singaboat_vrx/launch/submissions/vrx_gymkhana_challenge.launch b/singaboat_vrx/launch/vrx_gymkhana_challenge.launch similarity index 67% rename from singaboat_vrx/launch/submissions/vrx_gymkhana_challenge.launch rename to singaboat_vrx/launch/vrx_gymkhana_challenge.launch index e50c677..2dc841b 100644 --- a/singaboat_vrx/launch/submissions/vrx_gymkhana_challenge.launch +++ b/singaboat_vrx/launch/vrx_gymkhana_challenge.launch @@ -11,7 +11,10 @@ - - + + + + + diff --git a/singaboat_vrx/launch/submissions/vrx_scan_dock_deliver.launch b/singaboat_vrx/launch/vrx_scan_dock_deliver.launch similarity index 59% rename from singaboat_vrx/launch/submissions/vrx_scan_dock_deliver.launch rename to singaboat_vrx/launch/vrx_scan_dock_deliver.launch index 67cc824..81be8b2 100644 --- a/singaboat_vrx/launch/submissions/vrx_scan_dock_deliver.launch +++ b/singaboat_vrx/launch/vrx_scan_dock_deliver.launch @@ -8,7 +8,10 @@ - - + + + + + diff --git a/singaboat_vrx/launch/submissions/vrx_scene_perception.launch b/singaboat_vrx/launch/vrx_scene_perception.launch similarity index 60% rename from singaboat_vrx/launch/submissions/vrx_scene_perception.launch rename to singaboat_vrx/launch/vrx_scene_perception.launch index a4129c4..0044c1a 100644 --- a/singaboat_vrx/launch/submissions/vrx_scene_perception.launch +++ b/singaboat_vrx/launch/vrx_scene_perception.launch @@ -5,4 +5,7 @@ + + + diff --git a/singaboat_vrx/launch/vrx_semantic_navigation.launch b/singaboat_vrx/launch/vrx_semantic_navigation.launch new file mode 100644 index 0000000..9193b5e --- /dev/null +++ b/singaboat_vrx/launch/vrx_semantic_navigation.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/singaboat_vrx/launch/vrx_station_keeping.launch b/singaboat_vrx/launch/vrx_station_keeping.launch new file mode 100644 index 0000000..4d927b0 --- /dev/null +++ b/singaboat_vrx/launch/vrx_station_keeping.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/singaboat_vrx/launch/vrx_wayfinding.launch b/singaboat_vrx/launch/vrx_wayfinding.launch new file mode 100644 index 0000000..ff17d36 --- /dev/null +++ b/singaboat_vrx/launch/vrx_wayfinding.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/singaboat_vrx/package.xml b/singaboat_vrx/package.xml index ccfc0bc..4206ccc 100755 --- a/singaboat_vrx/package.xml +++ b/singaboat_vrx/package.xml @@ -2,7 +2,7 @@ singaboat_vrx - 0.3.0 + 0.2.0 ROS Package of Team SINGABOAT-VRX for Participation in Virtual RobotX (VRX) Competition 2022 Tanmay Vilas Samak diff --git a/singaboat_vrx/rviz/singaboat_gymkhana_challenge.rviz b/singaboat_vrx/rviz/singaboat_gymkhana_challenge.rviz deleted file mode 100644 index c2f3cc6..0000000 --- a/singaboat_vrx/rviz/singaboat_gymkhana_challenge.rviz +++ /dev/null @@ -1,306 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: ~ - Splitter Ratio: 0.5 - Tree Height: 441 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: ~ - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: Object Localization - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 114; 159; 207 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 1000 - Reference Frame: - Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - wamv/ball_shooter_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/ball_shooter_launcher_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - wamv/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/camera_link_optical: - Alpha: 1 - Show Axes: false - Show Trail: false - wamv/camera_post_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/camera_post_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/center_thruster_engine_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/center_thruster_propeller_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/cpu_cases_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/dummy_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/gps_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/imu_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/left_battery_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/left_thruster_engine_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/left_thruster_propeller_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/lidar_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/lidar_post_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/lidar_post_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/pinger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/right_battery_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/right_thruster_engine_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/right_thruster_propeller_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: WAM-V - Robot Description: wamv/robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz/Image - Enabled: true - Image Topic: /rviz/cam_viz - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Object Detection - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: Object Localization - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 5 - Size (m): 0.009999999776482582 - Style: Points - Topic: /wamv/sensors/lidars/lidar/points - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: wamv/base_link - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 9 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.75 - Focal Point: - X: 0.800000011920929 - Y: 0 - Z: 0 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.4000000059604645 - Target Frame: wamv/base_link - Yaw: 3.140000104904175 - Saved: - - Class: rviz/Orbit - Distance: 7 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.75 - Focal Point: - X: 0 - Y: 0 - Z: 0 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Orbit - Near Clip Distance: 0.009999999776482582 - Pitch: 0.6000000238418579 - Target Frame: wamv/base_link - Yaw: 3.140000104904175 -Window Geometry: - Displays: - collapsed: true - Height: 704 - Hide Left Dock: true - Hide Right Dock: true - Object Detection: - collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000001b400000138fc0200000008fb0000001200530065006c0065006300740069006f006e000000003d0000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000016b00000138000000c900fffffffb0000000c00430061006d00650072006100000001980000010b0000000000000000fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001aa00000138fc0200000007fb0000000a00560069006500770073000000016b00000138000000a400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000000160000028d0000005c00fffffffb00000016004c006500660074002000430061006d00650072006100000000f5000000b20000000000000000fb000000160052006900670074002000430061006d00650072006100000001ad000000b20000000000000000fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003740000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000050e0000014ffc0100000002fb00000020004f0062006a00650063007400200044006500740065006300740069006f006e01000000000000050e000000a900fffffffb0000000a00560069006500770073030000004e00000080000002e100000197000000030000050e0000003efc0100000002fb0000000800540069006d006500000000000000050e000004f300fffffffb0000000800540069006d006501000000000000045000000000000000000000050e0000013800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1294 - X: 72 - Y: 27 diff --git a/singaboat_vrx/rviz/singaboat_scan_dock_deliver.rviz b/singaboat_vrx/rviz/singaboat_scan_dock_deliver.rviz deleted file mode 100644 index ec662a0..0000000 --- a/singaboat_vrx/rviz/singaboat_scan_dock_deliver.rviz +++ /dev/null @@ -1,307 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Object Detection1 - Splitter Ratio: 0.5 - Tree Height: 441 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: ~ - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: Object Localization - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 114; 159; 207 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 1000 - Reference Frame: - Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - wamv/ball_shooter_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/ball_shooter_launcher_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - wamv/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/camera_link_optical: - Alpha: 1 - Show Axes: false - Show Trail: false - wamv/camera_post_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/camera_post_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/center_thruster_engine_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/center_thruster_propeller_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/cpu_cases_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/dummy_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/gps_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/imu_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/left_battery_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/left_thruster_engine_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/left_thruster_propeller_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/lidar_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/lidar_post_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/lidar_post_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/pinger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/right_battery_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/right_thruster_engine_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/right_thruster_propeller_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: WAM-V - Robot Description: wamv/robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz/Image - Enabled: true - Image Topic: /rviz/cam_viz - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Object Detection - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: Object Localization - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 5 - Size (m): 0.009999999776482582 - Style: Points - Topic: /wamv/sensors/lidars/lidar/points - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: wamv/base_link - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 9 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.75 - Focal Point: - X: 0.800000011920929 - Y: 0 - Z: 0 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.4000000059604645 - Target Frame: wamv/base_link - Yaw: 3.140000104904175 - Saved: - - Class: rviz/Orbit - Distance: 7 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.75 - Focal Point: - X: 0 - Y: 0 - Z: 0 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Orbit - Near Clip Distance: 0.009999999776482582 - Pitch: 0.6000000238418579 - Target Frame: wamv/base_link - Yaw: 3.140000104904175 -Window Geometry: - Displays: - collapsed: true - Height: 704 - Hide Left Dock: true - Hide Right Dock: true - Object Detection: - collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000001b400000138fc0200000008fb0000001200530065006c0065006300740069006f006e000000003d0000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000016b00000138000000c900fffffffb0000000c00430061006d00650072006100000001980000010b0000000000000000fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001aa00000138fc0200000007fb0000000a00560069006500770073000000016b00000138000000a400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000000160000028d0000005c00fffffffb00000016004c006500660074002000430061006d00650072006100000000f5000000b20000000000000000fb000000160052006900670074002000430061006d00650072006100000001ad000000b20000000000000000fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003740000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000050e0000014ffc0100000002fb00000020004f0062006a00650063007400200044006500740065006300740069006f006e01000000000000050e000000a900fffffffb0000000a00560069006500770073030000004e00000080000002e100000197000000030000050e0000003efc0100000002fb0000000800540069006d006500000000000000050e000004f300fffffffb0000000800540069006d006501000000000000045000000000000000000000050e0000013800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1294 - X: 72 - Y: 27 diff --git a/singaboat_vrx/rviz/singaboat_scene_perception.rviz b/singaboat_vrx/rviz/singaboat_scene_perception.rviz deleted file mode 100644 index 9049f40..0000000 --- a/singaboat_vrx/rviz/singaboat_scene_perception.rviz +++ /dev/null @@ -1,306 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: ~ - Splitter Ratio: 0.5 - Tree Height: 441 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: ~ - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: Object Localization - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 114; 159; 207 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 1000 - Reference Frame: - Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - wamv/ball_shooter_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/ball_shooter_launcher_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - wamv/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/camera_link_optical: - Alpha: 1 - Show Axes: false - Show Trail: false - wamv/camera_post_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/camera_post_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/center_thruster_engine_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/center_thruster_propeller_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/cpu_cases_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/dummy_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/gps_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/imu_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/left_battery_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/left_thruster_engine_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/left_thruster_propeller_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/lidar_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/lidar_post_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/lidar_post_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/pinger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/right_battery_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/right_thruster_engine_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/right_thruster_propeller_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: WAM-V - Robot Description: wamv/robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz/Image - Enabled: true - Image Topic: /rviz/cam_viz - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: Object Detection - Normalize Range: true - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: Object Localization - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 5 - Size (m): 0.009999999776482582 - Style: Points - Topic: /wamv/sensors/lidars/lidar/points - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: wamv/base_link - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 9 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.75 - Focal Point: - X: 0.800000011920929 - Y: 0 - Z: 0 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.6000000238418579 - Target Frame: wamv/base_link - Yaw: 3.140000104904175 - Saved: - - Class: rviz/Orbit - Distance: 7 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.75 - Focal Point: - X: 0 - Y: 0 - Z: 0 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Orbit - Near Clip Distance: 0.009999999776482582 - Pitch: 0.6000000238418579 - Target Frame: wamv/base_link - Yaw: 3.140000104904175 -Window Geometry: - Displays: - collapsed: true - Height: 704 - Hide Left Dock: true - Hide Right Dock: true - Object Detection: - collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000001b400000138fc0200000008fb0000001200530065006c0065006300740069006f006e000000003d0000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000016b00000138000000c900fffffffb0000000c00430061006d00650072006100000001980000010b0000000000000000fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001aa00000138fc0200000007fb0000000a00560069006500770073000000016b00000138000000a400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000000160000028d0000005c00fffffffb00000016004c006500660074002000430061006d00650072006100000000f5000000b20000000000000000fb000000160052006900670074002000430061006d00650072006100000001ad000000b20000000000000000fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003740000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000050e0000014ffc0100000002fb00000020004f0062006a00650063007400200044006500740065006300740069006f006e01000000000000050e000000a900fffffffb0000000a00560069006500770073030000004e00000080000002e100000197000000030000050e0000003efc0100000002fb0000000800540069006d006500000000000000050e000004f300fffffffb0000000800540069006d006501000000000000045000000000000000000000050e0000013800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1294 - X: 72 - Y: 27 diff --git a/singaboat_vrx/rviz/singaboat_semantic_navigation.rviz b/singaboat_vrx/rviz/singaboat_semantic_navigation.rviz deleted file mode 100644 index 09a62f2..0000000 --- a/singaboat_vrx/rviz/singaboat_semantic_navigation.rviz +++ /dev/null @@ -1,260 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: ~ - Splitter Ratio: 0.5 - Tree Height: 441 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: ~ - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 114; 159; 207 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 1000 - Reference Frame: - Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - wamv/ball_shooter_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/ball_shooter_launcher_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - wamv/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/camera_link_optical: - Alpha: 1 - Show Axes: false - Show Trail: false - wamv/camera_post_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/camera_post_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/center_thruster_engine_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/center_thruster_propeller_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/cpu_cases_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/dummy_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/gps_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/imu_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/left_battery_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/left_thruster_engine_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/left_thruster_propeller_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/lidar_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/lidar_post_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/lidar_post_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/pinger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/right_battery_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/right_thruster_engine_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/right_thruster_propeller_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: WAM-V - Robot Description: wamv/robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /rviz/animal_models - Name: Animal Models - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /rviz/animal_labels - Name: Animal Labels - Namespaces: - {} - Queue Size: 100 - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: wamv/base_link - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 9 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.75 - Focal Point: - X: 0.800000011920929 - Y: 0 - Z: 0 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.6000000238418579 - Target Frame: wamv/base_link - Yaw: 3.140000104904175 - Saved: ~ -Window Geometry: - Displays: - collapsed: true - Height: 704 - Hide Left Dock: true - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001b40000028dfc0200000008fb0000001200530065006c0065006300740069006f006e000000003d0000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000160000028d000000c900fffffffb0000000c00430061006d00650072006100000001980000010b0000000000000000fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001aa0000028dfc0200000007fb0000000a0056006900650077007300000000160000028d000000a400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000000160000028d0000005c00fffffffb00000016004c006500660074002000430061006d00650072006100000000f5000000b20000000000000000fb000000160052006900670074002000430061006d00650072006100000001ad000000b20000000000000000fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003740000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000050e000000a9fc0100000002fb0000000a00560069006500770073030000004e00000080000002e100000197fb00000020004f0062006a00650063007400200044006500740065006300740069006f006e00000000000000050e0000000000000000000000030000050e0000003efc0100000002fb0000000800540069006d006500000000000000050e000004f300fffffffb0000000800540069006d006501000000000000045000000000000000000000050e0000028d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1294 - X: 72 - Y: 27 diff --git a/singaboat_vrx/rviz/singaboat_station_keeping.rviz b/singaboat_vrx/rviz/singaboat_station_keeping.rviz deleted file mode 100644 index 543fa7e..0000000 --- a/singaboat_vrx/rviz/singaboat_station_keeping.rviz +++ /dev/null @@ -1,347 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: ~ - Splitter Ratio: 0.5 - Tree Height: 441 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: ~ - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 114; 159; 207 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 1000 - Reference Frame: - Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - wamv/ball_shooter_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/ball_shooter_launcher_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - wamv/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/camera_link_optical: - Alpha: 1 - Show Axes: false - Show Trail: false - wamv/camera_post_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/camera_post_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/center_thruster_engine_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/center_thruster_propeller_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/cpu_cases_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/dummy_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/gps_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/imu_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/left_battery_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/left_thruster_engine_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/left_thruster_propeller_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/lidar_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/lidar_post_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/lidar_post_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/pinger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/right_battery_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/right_thruster_engine_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/right_thruster_propeller_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: WAM-V - Robot Description: wamv/robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: false - goal: - Value: true - wamv/ball_shooter_base_link: - Value: false - wamv/ball_shooter_launcher_link: - Value: false - wamv/base_link: - Value: false - wamv/camera_link: - Value: false - wamv/camera_link_optical: - Value: false - wamv/camera_post_arm_link: - Value: false - wamv/camera_post_link: - Value: false - wamv/center_thruster_engine_link: - Value: false - wamv/center_thruster_propeller_link: - Value: false - wamv/cpu_cases_link: - Value: false - wamv/dummy_link: - Value: false - wamv/gps_link: - Value: false - wamv/imu_link: - Value: false - wamv/left_battery_link: - Value: false - wamv/left_thruster_engine_link: - Value: false - wamv/left_thruster_propeller_link: - Value: false - wamv/lidar_link: - Value: false - wamv/lidar_post_arm_link: - Value: false - wamv/lidar_post_link: - Value: false - wamv/pinger: - Value: false - wamv/right_battery_link: - Value: false - wamv/right_thruster_engine_link: - Value: false - wamv/right_thruster_propeller_link: - Value: false - world: - Value: false - Marker Alpha: 1 - Marker Scale: 5 - Name: Goal - Show Arrows: false - Show Axes: true - Show Names: true - Tree: - world: - goal: - {} - wamv/base_link: - wamv/ball_shooter_base_link: - wamv/ball_shooter_launcher_link: - {} - wamv/camera_post_link: - wamv/camera_post_arm_link: - wamv/camera_link: - wamv/camera_link_optical: - {} - wamv/center_thruster_engine_link: - wamv/center_thruster_propeller_link: - {} - wamv/cpu_cases_link: - {} - wamv/dummy_link: - {} - wamv/gps_link: - {} - wamv/imu_link: - {} - wamv/left_battery_link: - {} - wamv/left_thruster_engine_link: - wamv/left_thruster_propeller_link: - {} - wamv/lidar_post_link: - wamv/lidar_post_arm_link: - wamv/lidar_link: - {} - wamv/pinger: - {} - wamv/right_battery_link: - {} - wamv/right_thruster_engine_link: - wamv/right_thruster_propeller_link: - {} - Update Interval: 0 - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: wamv/base_link - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 9 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.75 - Focal Point: - X: 0.800000011920929 - Y: 0 - Z: 0 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.6000000238418579 - Target Frame: wamv/base_link - Yaw: 3.140000104904175 - Saved: ~ -Window Geometry: - Displays: - collapsed: true - Height: 704 - Hide Left Dock: true - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001b40000028dfc0200000008fb0000001200530065006c0065006300740069006f006e000000003d0000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000160000028d000000c900fffffffb0000000c00430061006d00650072006100000001980000010b0000000000000000fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001aa0000028dfc0200000007fb0000000a0056006900650077007300000000160000028d000000a400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000000160000028d0000005c00fffffffb00000016004c006500660074002000430061006d00650072006100000000f5000000b20000000000000000fb000000160052006900670074002000430061006d00650072006100000001ad000000b20000000000000000fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003740000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000050e000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000050e0000003efc0100000002fb0000000800540069006d006500000000000000050e000004f300fffffffb0000000800540069006d006501000000000000045000000000000000000000050e0000028d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1294 - X: 72 - Y: 27 diff --git a/singaboat_vrx/rviz/singaboat_vrx.rviz b/singaboat_vrx/rviz/singaboat_vrx.rviz index db7071d..53172dc 100644 --- a/singaboat_vrx/rviz/singaboat_vrx.rviz +++ b/singaboat_vrx/rviz/singaboat_vrx.rviz @@ -4,9 +4,9 @@ Panels: Name: Displays Property Tree Widget: Expanded: - - /Status1 + - /Camera1/Status1 Splitter Ratio: 0.5 - Tree Height: 441 + Tree Height: 319 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -17,7 +17,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: Camera + SyncSource: LIDAR - Class: rviz/Views Expanded: - /Current View1 @@ -375,11 +375,11 @@ Window Geometry: Camera: collapsed: false Displays: - collapsed: true + collapsed: false Height: 704 - Hide Left Dock: true + Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000015600000152fc0200000007fb0000001200530065006c0065006300740069006f006e000000003d0000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000015100000152000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001aa0000028dfc0200000007fb0000000a0056006900650077007300000000160000028d000000a400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000000160000028d0000005c00fffffffb00000016004c006500660074002000430061006d00650072006100000000f5000000b20000000000000000fb000000160052006900670074002000430061006d00650072006100000001ad000000b20000000000000000fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003740000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000050e00000133fc0100000002fb0000000c00430061006d00650072006101000000000000050e0000006900fffffffb0000000a00560069006500770073030000004e00000080000002e100000197000000030000050e0000003efc0100000002fb0000000800540069006d006500000000000000050e000004f300fffffffb0000000800540069006d006501000000000000045000000000000000000000050e0000015400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001b40000028dfc0200000008fb0000001200530065006c0065006300740069006f006e000000003d0000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000160000017c000000c900fffffffb0000000c00430061006d00650072006101000001980000010b0000001600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001aa0000028dfc0200000007fb0000000a0056006900650077007300000000160000028d000000a400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000000160000028d0000005c00fffffffb00000016004c006500660074002000430061006d00650072006100000000f5000000b20000000000000000fb000000160052006900670074002000430061006d00650072006100000001ad000000b20000000000000000fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003740000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000050e000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000050e0000003efc0100000002fb0000000800540069006d006500000000000000050e000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003540000028d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 Selection: collapsed: false Time: diff --git a/singaboat_vrx/rviz/singaboat_wayfinding.rviz b/singaboat_vrx/rviz/singaboat_wayfinding.rviz deleted file mode 100644 index 943bade..0000000 --- a/singaboat_vrx/rviz/singaboat_wayfinding.rviz +++ /dev/null @@ -1,356 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: ~ - Splitter Ratio: 0.5 - Tree Height: 441 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: ~ - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" - - Class: rviz/Views - Expanded: - - /Current View1 - - /Current View1/Focal Point1 - Name: Views - Splitter Ratio: 0.5 -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 114; 159; 207 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 1000 - Reference Frame: - Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - wamv/ball_shooter_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/ball_shooter_launcher_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - wamv/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/camera_link_optical: - Alpha: 1 - Show Axes: false - Show Trail: false - wamv/camera_post_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/camera_post_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/center_thruster_engine_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/center_thruster_propeller_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/cpu_cases_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/dummy_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/gps_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/imu_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/left_battery_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/left_thruster_engine_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/left_thruster_propeller_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/lidar_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/lidar_post_arm_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/lidar_post_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/pinger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/right_battery_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/right_thruster_engine_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wamv/right_thruster_propeller_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: WAM-V - Robot Description: wamv/robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: false - wamv/ball_shooter_base_link: - Value: false - wamv/ball_shooter_launcher_link: - Value: false - wamv/base_link: - Value: false - wamv/camera_link: - Value: false - wamv/camera_link_optical: - Value: false - wamv/camera_post_arm_link: - Value: false - wamv/camera_post_link: - Value: false - wamv/center_thruster_engine_link: - Value: false - wamv/center_thruster_propeller_link: - Value: false - wamv/cpu_cases_link: - Value: false - wamv/dummy_link: - Value: false - wamv/gps_link: - Value: false - wamv/imu_link: - Value: false - wamv/left_battery_link: - Value: false - wamv/left_thruster_engine_link: - Value: false - wamv/left_thruster_propeller_link: - Value: false - wamv/lidar_link: - Value: false - wamv/lidar_post_arm_link: - Value: false - wamv/lidar_post_link: - Value: false - wamv/pinger: - Value: false - wamv/right_battery_link: - Value: false - wamv/right_thruster_engine_link: - Value: false - wamv/right_thruster_propeller_link: - Value: false - waypoint_0: - Value: true - waypoint_1: - Value: true - waypoint_2: - Value: true - world: - Value: false - Marker Alpha: 1 - Marker Scale: 5 - Name: Waypoints - Show Arrows: false - Show Axes: true - Show Names: true - Tree: - world: - wamv/base_link: - wamv/ball_shooter_base_link: - wamv/ball_shooter_launcher_link: - {} - wamv/camera_post_link: - wamv/camera_post_arm_link: - wamv/camera_link: - wamv/camera_link_optical: - {} - wamv/center_thruster_engine_link: - wamv/center_thruster_propeller_link: - {} - wamv/cpu_cases_link: - {} - wamv/dummy_link: - {} - wamv/gps_link: - {} - wamv/imu_link: - {} - wamv/left_battery_link: - {} - wamv/left_thruster_engine_link: - wamv/left_thruster_propeller_link: - {} - wamv/lidar_post_link: - wamv/lidar_post_arm_link: - wamv/lidar_link: - {} - wamv/pinger: - {} - wamv/right_battery_link: - {} - wamv/right_thruster_engine_link: - wamv/right_thruster_propeller_link: - {} - waypoint_0: - {} - waypoint_1: - {} - waypoint_2: - {} - Update Interval: 0 - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: wamv/base_link - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 9 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.75 - Focal Point: - X: 0.800000011920929 - Y: 0 - Z: 0 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.6000000238418579 - Target Frame: wamv/base_link - Yaw: 3.140000104904175 - Saved: ~ -Window Geometry: - Displays: - collapsed: true - Height: 704 - Hide Left Dock: true - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001b40000028dfc0200000008fb0000001200530065006c0065006300740069006f006e000000003d0000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000160000028d000000c900fffffffb0000000c00430061006d00650072006100000001980000010b0000000000000000fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001aa0000028dfc0200000007fb0000000a0056006900650077007300000000160000028d000000a400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000000160000028d0000005c00fffffffb00000016004c006500660074002000430061006d00650072006100000000f5000000b20000000000000000fb000000160052006900670074002000430061006d00650072006100000001ad000000b20000000000000000fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003740000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000050e000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000050e0000003efc0100000002fb0000000800540069006d006500000000000000050e000004f300fffffffb0000000800540069006d006501000000000000045000000000000000000000050e0000028d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1294 - X: 72 - Y: 27 diff --git a/singaboat_vrx/scripts/entrypoint.sh b/singaboat_vrx/scripts/entrypoint.sh deleted file mode 100755 index 9d4da3f..0000000 --- a/singaboat_vrx/scripts/entrypoint.sh +++ /dev/null @@ -1,9 +0,0 @@ -#!/bin/bash -set -e - -# Setup ROS environment -source /opt/ros/noetic/setup.bash -source /home/$USER/VRX_Workspace/devel/setup.bash - -# Launch VNC server -x11vnc -forever -usepw -create & /bin/bash diff --git a/singaboat_vrx/src/singaboat_gymkhana_challenge.py b/singaboat_vrx/src/singaboat_gymkhana_challenge.py index 240f510..ebcf88e 100755 --- a/singaboat_vrx/src/singaboat_gymkhana_challenge.py +++ b/singaboat_vrx/src/singaboat_gymkhana_challenge.py @@ -23,13 +23,14 @@ def __init__(self): # Initialize gymkhana challenge self.fsm = FiniteStateMachine() # FiniteStateMachine class instance self.next_gate_pose = GeoPoseStamped() # Keeps track of pose (lat, lon, quat) of the next channel gate + self.exit_detected = False # Boolean flag to check whether channel exit has been detected self.asv_pos_lat = None # ASV position latitude self.asv_pos_lon = None # ASV position longitude self.asv_heading = None # ASV orientation (yaw) self.pinger_range = None # Range measurement from acoustic pinger locater (APL) self.pinger_bearing = None # Bearing measurement from acoustic pinger locater (APL) - self.state_time = rospy.get_time() # State initialization time + self.state_time = rospy.get_time() # Task initialization time self.start_time = time.time() # Records time at the start of algorithm self.config = {} # Gymkhana challenge configuration # ROS infrastructure @@ -56,7 +57,7 @@ def imu_callback(self, msg): def apl_callback(self, msg): self.pinger_range = msg.range self.pinger_bearing = msg.bearing - goal_tol = self.max_goal_tol if self.fsm.current_state == 'Find Pinger' else self.min_goal_tol + goal_tol = self.max_goal_tol if (self.fsm.current_state == 'Find Pinger') else self.min_goal_tol if ((self.fsm.current_state == 'Find Pinger') or (self.fsm.current_state == 'Hold Position')): elapsed = time.time() - self.start_time @@ -101,15 +102,13 @@ def collision_avoidance_callback(self, msg): if w.shape[0] > 0: # Only if there is any obstacle that the WAM-V may collide with wt_avg = numpy.sum(w*y)/numpy.sum(w) # Compute weighted average repulsion = numpy.clip(-4.0/wt_avg, -1.0, 1.0) # Compute repulsive field (according to potential field algorithm) due to obstacles - if self.debug: - print('Obstacles detected towards {}, correcting course by {:.4f} rad'.format('left' if wt_avg > 0 else 'right', self.potential_field_msg.data)) - print() + print('Obstacles detected towards {}, correcting course by {:.4f} rad'.format('left' if wt_avg > 0 else 'right', self.potential_field_msg.data)) else: repulsion = 0.0 # Reset repulsive field self.potential_field_msg.data = repulsion self.potential_field_pub.publish(self.potential_field_msg) - def create_path(self, end_wp, start_wp_mode='PASS', end_wp_mode='SCAN'): + def create_path(self, end_wp, start_wp_mode='PASS', end_wp_mode='PLAN'): ''' Create a path from the current position of ASV to the given end waypoint. @@ -143,7 +142,7 @@ def navigate(self, path): :return: None ''' - goal = MissionGoal(mission=path) # Generate action goal (i.e. mission goal) + goal = MissionGoal(mission = path) # Generate action goal (i.e. mission goal) self.action_client.send_goal(goal) # Send action goal (i.e. mission goal) to the action server (i.e. mission manager) self.action_client.wait_for_result() # Wait for result from the action server (i.e. mission manager) @@ -191,13 +190,12 @@ def initial_state_handler(self): goal.pose.orientation.z = asv_quat[2] goal.pose.orientation.w = asv_quat[3] # Stay at the current location and search for channel entrance - path = self.create_path(goal, start_wp_mode='PASS', end_wp_mode='SCAN') + path = self.create_path(goal, start_wp_mode='PASS', end_wp_mode='PLAN') self.navigate(path) # Define the next state - if rospy.get_time() - self.state_time >= 60: # 60 seconds to detect entry gate, else find pinger - if self.debug: - print("Stuck in the channel, aborting channel navigation...") - print() + if rospy.get_time() - self.state_time >= 30: # 30 seconds to detect entry gate, else find pinger + print("Stuck in the channel, aborting channel navigation...") + print() next_state = 'Find Pinger' else: self.state_time = rospy.get_time() @@ -213,14 +211,13 @@ def entrance_state_handler(self): :return next_state: Name of the next state (depending on the transition logic) ''' - # Navigate towards the center of entry gate and search for gate 1 or channel exit - path = self.create_path(self.next_gate_pose, start_wp_mode='PASS', end_wp_mode='SCAN') + # Navigate towards the center of entry gate + path = self.create_path(self.next_gate_pose, start_wp_mode='PASS', end_wp_mode='PLAN') self.navigate(path) # Define the next state - if rospy.get_time() - self.state_time >= 60: # 60 seconds to detect gate 1, else find pinger - if self.debug: - print("Stuck in the channel, aborting channel navigation...") - print() + if rospy.get_time() - self.state_time >= 30: # 30 seconds to detect gate 1, else find pinger + print("Stuck in the channel, aborting channel navigation...") + print() next_state = 'Find Pinger' else: self.state_time = rospy.get_time() @@ -236,14 +233,13 @@ def gate_1_state_handler(self): :return next_state: Name of the next state (depending on the transition logic) ''' - # Navigate towards the center of gate 1 and search for gate 2 or channel exit - path = self.create_path(self.next_gate_pose, start_wp_mode='PASS', end_wp_mode='SCAN') + # Navigate towards the center of gate 1 + path = self.create_path(self.next_gate_pose, start_wp_mode='PASS', end_wp_mode='PLAN') self.navigate(path) # Define the next state - if rospy.get_time() - self.state_time >= 60: # 60 seconds to detect gate 2, else find pinger - if self.debug: - print("Stuck in the channel, aborting channel navigation...") - print() + if rospy.get_time() - self.state_time >= 30: # 30 seconds to detect gate 2, else find pinger + print("Stuck in the channel, aborting channel navigation...") + print() next_state = 'Find Pinger' else: self.state_time = rospy.get_time() @@ -259,14 +255,13 @@ def gate_2_state_handler(self): :return next_state: Name of the next state (depending on the transition logic) ''' - # Navigate towards the center of gate 2 and search for gate 3 or channel exit - path = self.create_path(self.next_gate_pose, start_wp_mode='PASS', end_wp_mode='SCAN') + # Navigate towards the center of gate 2 + path = self.create_path(self.next_gate_pose, start_wp_mode='PASS', end_wp_mode='PLAN') self.navigate(path) # Define the next state - if rospy.get_time() - self.state_time >= 60: # 60 seconds to detect gate 3, else find pinger - if self.debug: - print("Stuck in the channel, aborting channel navigation...") - print() + if rospy.get_time() - self.state_time >= 30: # 30 seconds to detect gate 3, else find pinger + print("Stuck in the channel, aborting channel navigation...") + print() next_state = 'Find Pinger' else: self.state_time = rospy.get_time() @@ -282,14 +277,13 @@ def gate_3_state_handler(self): :return next_state: Name of the next state (depending on the transition logic) ''' - # Navigate towards the center of gate 3 and search for channel exit - path = self.create_path(self.next_gate_pose, start_wp_mode='PASS', end_wp_mode='SCAN') + # Navigate towards the center of gate 3 + path = self.create_path(self.next_gate_pose, start_wp_mode='PASS', end_wp_mode='PLAN') self.navigate(path) # Define the next state - if rospy.get_time() - self.state_time >= 60: # 60 seconds to detect exit gate, else find pinger - if self.debug: - print("Stuck in the channel, aborting channel navigation...") - print() + if rospy.get_time() - self.state_time >= 30: # 30 seconds to detect exit gate, else find pinger + print("Stuck in the channel, aborting channel navigation...") + print() next_state = 'Find Pinger' else: self.state_time = rospy.get_time() @@ -329,10 +323,9 @@ def find_pinger_state_handler(self): pinger_bearing = asv_bearing - numpy.rad2deg(self.pinger_bearing) # Compute pinger bearing w.r.t. global frame pinger_lat, pinger_lon = self.locate_pinger(self.asv_pos_lat, self.asv_pos_lon, pinger_range, pinger_bearing) # Compute GPS coordinates (lat, lon) of the pinger # Print pinger location - if self.debug: - print("Object Name: Pinger") - print("Object Location: {}° N {}° E".format(pinger_lat, pinger_lon)) - print() + print("Object Name: Pinger") + print("Object Location: {}° N {}° E".format(pinger_lat, pinger_lon)) + print() goal.pose.position.latitude = pinger_lat goal.pose.position.longitude = pinger_lon goal.pose.orientation.x = goal_quat[0] @@ -383,7 +376,6 @@ def config_callback(self, config, level): self.max_goal_tol = config['max_goal_tol'] # Maximum goal tolerance (for finding pinger) self.collision_tol = config['collision_tol'] # Obstacle range tolerance for collision avoidance self.collision_fov = config['collision_fov'] # Obstacle field of view for collision avoidance - self.debug = config['debug'] # Flag to enable/disable debug messages self.config = config return config @@ -438,12 +430,10 @@ def config_callback(self, config, level): rospy.wait_for_message('/vrx/task/info', Task) rospy.wait_for_message('/wamv/sensors/gps/gps/fix', NavSatFix) rospy.wait_for_message('/wamv/sensors/imu/imu/data', Imu) - rospy.wait_for_message('/wamv/sensors/pingers/pinger/range_bearing', RangeBearing) while not rospy.is_shutdown(): gymkhana_challenge_node.fsm.run() rate = rospy.Rate(10) rate.sleep() - except rospy.ROSInterruptException: pass diff --git a/singaboat_vrx/src/singaboat_inverse_kinematics.py b/singaboat_vrx/src/singaboat_ik_solver.py old mode 100755 new mode 100644 similarity index 51% rename from singaboat_vrx/src/singaboat_inverse_kinematics.py rename to singaboat_vrx/src/singaboat_ik_solver.py index f23d86b..c41e2fb --- a/singaboat_vrx/src/singaboat_inverse_kinematics.py +++ b/singaboat_vrx/src/singaboat_ik_solver.py @@ -4,15 +4,15 @@ from dynamic_reconfigure.server import Server from std_msgs.msg import Float32 from geometry_msgs.msg import Twist -from singaboat_vrx.cfg import InverseKinematicsConfig +from singaboat_vrx.cfg import IKSolverConfig from singaboat_vrx.common_utilities import constrain ################################################################################ -class InverseKinematics(): +class IKSolver(): def __init__(self): - # Initialize inverse kinematics - self.config = {} # Inverse kinematics configuration + # Initialize IK solver + self.config = {} # IK solver configuration # ROS infrastructure self.center_thruster_msg = None self.left_thruster_msg = None @@ -22,12 +22,12 @@ def __init__(self): self.right_thruster_pub = None self.dyn_reconf_srv = None - def inverse_kinematics_callback(self, msg): - # Log IK input - print("Lin Vel X: {:.4} m/s".format(msg.linear.x)) - print("Lin Vel Y: {:.4} m/s".format(msg.linear.y)) - print("Ang Vel Z: {:.4} rad/s".format(msg.angular.z)) - print() + def ik_solver_callback(self, msg): + # Log IK solver input + print("IK Solver I/P:") + print(" Lin_Vel_X: %.4f m/s" % (msg.linear.x)) + print(" Lin_Vel_Y: %.4f m/s" % (msg.linear.y)) + print(" Ang_Vel_Z: %.4f rad/s" % (msg.angular.z)) # Comput inverse kinematics # Lin_Vel_X if msg.linear.x >= 0.0: @@ -48,12 +48,11 @@ def inverse_kinematics_callback(self, msg): self.center_thruster_msg.data = constrain(self.center_thruster_msg.data, -1.0, 1.0) self.left_thruster_msg.data = constrain(self.left_thruster_msg.data, -1.0, 1.0) self.right_thruster_msg.data = constrain(self.right_thruster_msg.data, -1.0, 1.0) - # Log IK output - if self.debug: - print("Thrust C: {:.4}".format(self.center_thruster_msg.data)) - print("Thrust L: {:.4}".format(self.left_thruster_msg.data)) - print("Thrust R: {:.4}".format(self.right_thruster_msg.data)) - print() + # Log IK solver output + print("IK Solver O/P:") + print(" Thrust_C: %.4f" % (self.center_thruster_msg.data)) + print(" Thrust_L: %.4f" % (self.left_thruster_msg.data)) + print(" Thrust_R: %.4f" % (self.right_thruster_msg.data)) # Publish messages to WAM-V thrusters self.center_thruster_pub.publish(self.center_thruster_msg) self.left_thruster_pub.publish(self.left_thruster_msg) @@ -61,39 +60,38 @@ def inverse_kinematics_callback(self, msg): def config_callback(self, config, level): # Handle updated configuration values - self.thrust_factor = config['thrust_factor'] # Balancing factor for forward & reverse thrust forces - self.drag_factor = config['drag_factor'] # Balancing factor for longitudinal & lateral drag forces - self.v_x_gain = config['v_x_gain'] # Linear velocity gain along X-axis - self.v_y_gain = config['v_y_gain'] # Linear velocity gain along Y-axis - self.w_z_gain = config['w_z_gain'] # Angular velocity gain about Z-axis - self.half_beam = config['half_beam'] # Half beam width of WAM-V - self.debug = config['debug'] # Flag to enable/disable debug messages + self.thrust_factor = config['thrust_factor'] + self.drag_factor = config['drag_factor'] + self.v_x_gain = config['v_x_gain'] + self.v_y_gain = config['v_y_gain'] + self.w_z_gain = config['w_z_gain'] + self.half_beam = config['half_beam'] self.config = config return config ################################################################################ if __name__ == '__main__': - rospy.init_node('singaboat_inverse_kinematics', anonymous = True) + rospy.init_node('singaboat_ik_solver', anonymous = True) - # InverseKinematics class instance - inverse_kinematics_node = InverseKinematics() + # IKSolver class instance + ik_solver_node = IKSolver() # Dynamic reconfigure server - inverse_kinematics_node.dyn_reconf_srv = Server(InverseKinematicsConfig, inverse_kinematics_node.config_callback) + ik_solver_node.dyn_reconf_srv = Server(IKSolverConfig, ik_solver_node.config_callback) # Messages - inverse_kinematics_node.center_thruster_msg = Float32() - inverse_kinematics_node.left_thruster_msg = Float32() - inverse_kinematics_node.right_thruster_msg = Float32() + ik_solver_node.center_thruster_msg = Float32() + ik_solver_node.left_thruster_msg = Float32() + ik_solver_node.right_thruster_msg = Float32() # Subscriber - rospy.Subscriber('/wamv/cmd_vel', Twist, inverse_kinematics_node.inverse_kinematics_callback) + rospy.Subscriber('/wamv/cmd_vel', Twist, ik_solver_node.ik_solver_callback) # Publishers - inverse_kinematics_node.center_thruster_pub = rospy.Publisher('/wamv/thrusters/center_thruster_thrust_cmd', Float32, queue_size = 10) - inverse_kinematics_node.left_thruster_pub = rospy.Publisher('/wamv/thrusters/left_thruster_thrust_cmd', Float32, queue_size = 10) - inverse_kinematics_node.right_thruster_pub = rospy.Publisher('/wamv/thrusters/right_thruster_thrust_cmd', Float32, queue_size = 10) + ik_solver_node.center_thruster_pub = rospy.Publisher('/wamv/thrusters/center_thruster_thrust_cmd', Float32, queue_size = 10) + ik_solver_node.left_thruster_pub = rospy.Publisher('/wamv/thrusters/left_thruster_thrust_cmd', Float32, queue_size = 10) + ik_solver_node.right_thruster_pub = rospy.Publisher('/wamv/thrusters/right_thruster_thrust_cmd', Float32, queue_size = 10) try: rospy.spin() diff --git a/singaboat_vrx/src/singaboat_mission_manager.py b/singaboat_vrx/src/singaboat_mission_manager.py index e613be9..c264ae9 100755 --- a/singaboat_vrx/src/singaboat_mission_manager.py +++ b/singaboat_vrx/src/singaboat_mission_manager.py @@ -7,15 +7,15 @@ from dynamic_reconfigure.server import Server import rospy import actionlib -from std_msgs.msg import Float32 +from std_msgs.msg import Float32, Float64 from geographic_msgs.msg import GeoPoseStamped from geometry_msgs.msg import Vector3Stamped, Twist from sensor_msgs.msg import NavSatFix, Imu -from singaboat_vrx.cfg import MissionManagerConfig from singaboat_vrx.msg import MissionResult, MissionFeedback, MissionAction -from singaboat_vrx.planning_module import Mission, DubinsPath, PathPlanner +from singaboat_vrx.planning_module import * from singaboat_vrx.control_module import PIDController -from singaboat_vrx.common_utilities import enu_to_gps, gps_to_enu, normalize_angle, quaternion_to_euler +from singaboat_vrx.cfg import MissionManagerConfig +from singaboat_vrx.common_utilities import normalize_angle, quaternion_to_euler ################################################################################ @@ -24,28 +24,61 @@ class MissionManager: `MissionManager` is the highest abstraction layer of the navigation algorithm. It receives a set of waypoints and defines a path connecting them. It makes use of `Mission` and `PathPlanner` to perform the path following task of whole guidance strategy. + + Note: The init_app method is the only public method of this class since it implements an + action sever node, and it is not intended to be imported anywhere else in the system. ''' - def __init__(self): - # Initialize mission manager - self.action_server = None # Action server - self.feedback = MissionFeedback() # Feedback from action server - self.result = MissionResult() # Result of action server - self.mission = None # Mission - self.path_planner = None # Path planner - self.current_wp = Mission.Waypoint(gps_lat = 0, gps_lon = 0) # Current waypoint - self.target_wp = Mission.Waypoint(gps_lat = 0, gps_lon = 0) # Target waypoint - self.mission_complete = False # Boolean flag to check whether the mission has been completed - self.angle_idx = 0 # Angle index for scanning mode - self.repulsion = 0.0 # Repulsive field due to obstacles - self.g2g_heading = 0.0 # Go-to-goal heading - self.cte = 0.0 # Cross-track error - self.he = 0.0 # Heading error - self.current_speed = 0.0 # Current longitudinal linear velocity - self.config = {} # Mission manager configuration - # ROS infrastructure - self.cmd_vel_msg = None - self.cmd_vel_pub = None - self.dyn_reconf_srv = None + def __init__(self, node_name='singaboat_mission_manager'): + self.action_server = None + self.feedback = MissionFeedback() + self.result = MissionResult() + self.mission = 0 + self.path_planner = 0 + self.current_wp = Mission.Waypoint(gps_lat = 0, gps_lon = 0) + self.target_wp = Mission.Waypoint(gps_lat = 0, gps_lon = 0) + self.mission_complete = False + self.g2g_heading = 0.0 + self.cte = 0.0 + self.head_u = 0 + self.head_prev_error = 0 + self.angle_idx = 0 + self.max_speed = 2.23 # TODO: move to config + self.min_speed = 1.0 # TODO: move to config + self.repulsion_heading = 0 + self.current_speed = 0 + self.wind_dir = 0 + self.wind_speed = 0 + self.node_name = node_name + self.rate = 0 + self.cmd_vel_msg = Twist() + self.config = {} + self.debug = False # Flag to enable/disable debug messages | TODO: move to config + + def init_app(self): + ''' + Action server initializer. + + :param: None + + :return: None + ''' + # ROS node initilization + rospy.init_node(self.node_name, anonymous=True) # TODO: move away from here + self.rate = rospy.Rate(10) # TODO: move away from here + # Dynamic reconfigure server + self.dyn_reconf_srv = Server(MissionManagerConfig, self.config_callback) # TODO: move away from here + # Subscribers + rospy.Subscriber('/wamv/sensors/gps/gps/fix', NavSatFix, self.gps_callback, queue_size=1) # TODO: move away from here + rospy.Subscriber('/wamv/sensors/gps/gps/fix_velocity', Vector3Stamped, self.speed_callback, queue_size=1) # TODO: move away from here + rospy.Subscriber('/wamv/sensors/imu/imu/data', Imu, self.imu_callback, queue_size=1) # TODO: move away from here + rospy.Subscriber('/wamv/obstacle_potential_field', Float32, self.collision_avoidance_callback) # TODO: move away from here + rospy.Subscriber('/vrx/debug/wind/direction', Float64, self.wind_direction_callback) # TODO: move away from here + rospy.Subscriber('/vrx/debug/wind/speed', Float64, self.wind_speed_callback) # TODO: move away from here + # Publishers + self.cmd_vel_pub = rospy.Publisher('/wamv/cmd_vel', Twist, queue_size=1) # TODO: move away from here + # Start action server + self.action_server = actionlib.SimpleActionServer(self.node_name, MissionAction, execute_cb=self.run, auto_start = False) # TODO: move away from here + self.action_server.start() def gps_callback(self, msg): if self.current_wp.pose.heading is None: # If no yaw data is available, GPS offset cannot be compensated @@ -54,8 +87,8 @@ def gps_callback(self, msg): lon = msg.longitude x, y, _ = gps_to_enu(lat, lon) # WAM-V frame is +0.85 m offset in local x-axis w.r.t. GPS frame - x += self.gps_offset * math.cos(self.current_wp.pose.heading) - y += self.gps_offset * math.sin(self.current_wp.pose.heading) + x += 0.85 * math.cos(self.current_wp.pose.heading) # TODO: add to config + y += 0.85 * math.sin(self.current_wp.pose.heading) # TODO: add to config # Convert back to GPS coordinates self.current_wp.pose.gps_lat, self.current_wp.pose.gps_lon, _ = enu_to_gps(x, y) @@ -66,20 +99,55 @@ def imu_callback(self, msg): self.current_wp.pose.heading = quaternion_to_euler(msg.orientation)[2] def collision_avoidance_callback(self, msg): - self.repulsion = msg.data + self.repulsion_heading = msg.data + + def wind_direction_callback(self, msg): + self.wind_dir = numpy.deg2rad(msg.data) + + def wind_speed_callback(self, msg): + self.wind_speed = msg.data + + def feed_forward_controller(self, asv_yaw): # TODO: make an independent class in control_module + ''' + Feed-forward controller to compensate disturbances due to wind. + + :param asv_yaw: Current heading of the ASV + + :return thruster_ff_x, thruster_ff_y: Feed-forward thrust commands for longitudinal and lateral thrusters + ''' + x = -self.wind_speed * numpy.cos(self.wind_dir-asv_yaw ) + y = self.wind_speed * numpy.sin(self.wind_dir-asv_yaw ) + # Componsation for lateral motion + if abs(y)<0.5: + thruster_ff_y = 0 + elif y > 0: + thruster_ff_y = 0.03130211 - 0.177514*y + 0.02011851*y**2 - 0.001196402*y**3 + 0.00002102702*y**4 + else: + thruster_ff_y = 0.0009255614 - 0.04251068*y + 0.002257581*y**2 + 0.000376901*y**3 + 0.00001408252*y**4 + # Compensation for longitudinal motion + if abs(x) < 0.5: + thruster_ff_x = 0 + elif x > 0: + thruster_ff_x = -0.01555185 + 0.01927031*x + 0.005994835*x**2 - 0.0006834648*x**3 + 0.00002243099*x**4 + else: + thruster_ff_x = -0.0203281 + 0.1066104*x + 0.008148247*x**2 + 0.0003494099*x**3 + 0.000004174913*x**4 + # Compensation due to asymmetric motor propulsion + if thruster_ff_y < 0: + thruster_ff_y = 2.5*thruster_ff_y # TODO: move to config + return thruster_ff_x, thruster_ff_y def go_to_goal(self): ''' Simple go-to-goal behavior using a PD Controller for angular velocity - and an adaptive controller for longitudinal linear velocity. + and an adaptive controller for linear velocity. :param: None :return: None ''' - # Include the repulsion from local planner for online collision avoidance - if self.repulsion != 0: - self.g2g_heading = self.current_wp.pose.heading + self.repulsion + # Include the repulsion from a local planner for online collision avoidance + if self.repulsion_heading != 0: + self.g2g_heading = self.current_wp.pose.heading + self.repulsion_heading self.max_speed = 1.67 self.min_speed = 0.75 else: @@ -87,11 +155,12 @@ def go_to_goal(self): self.max_speed = 2.23 self.min_speed = 1.0 head_err = normalize_angle(self.g2g_heading - self.current_wp.pose.heading) - # Head-to-goal PD Controller for angular velocity - derivative = head_err - self.he - ang_vel = self.h2g_kp*head_err + self.h2g_kd*derivative - self.he = head_err - # Adaptive controller for longitudinal linear velocity + ff_x,ff_y = self.feed_forward_controller(self.current_wp.pose.heading) + # PD Controller for angular velocity + derivative = head_err - self.head_prev_error + self.head_u = self.h2g_kp*head_err + self.h2g_kd*derivative + self.head_prev_error = head_err + # Adaptive controller for linear velocity up = self.max_speed low = self.min_speed x_up = 0.15 @@ -99,11 +168,11 @@ def go_to_goal(self): m = (up-low)/(x_up-x_low) b = up - m*x_up x = numpy.abs(head_err) - lin_vel = numpy.clip(m*x+b,low,up) + speed = numpy.clip(m*x+b,low,up) # Generate and publish `cmd_vel` message - self.cmd_vel_msg.linear.x = lin_vel - self.cmd_vel_msg.linear.y = 0.0 - self.cmd_vel_msg.angular.z = ang_vel + self.cmd_vel_msg.linear.x = speed + ff_x + self.cmd_vel_msg.linear.y = ff_y + self.cmd_vel_msg.angular.z = self.head_u self.cmd_vel_pub.publish(self.cmd_vel_msg) def station_keeping(self, goal_wp): @@ -151,16 +220,16 @@ def station_keeping(self, goal_wp): self.cmd_vel_pub.publish(self.cmd_vel_msg) return e_pose - def scan_around(self, goal_wp, theta, delay): + def search_pattern(self, goal_wp, theta, delay): ''' Swing left-and-right to search for any detectable patterns within the environment. :param goal_wp: Goal waypoint pose - :param theta : Current heading of the ASV - :param delay : Time delay to carry out scanning operation + :param theta: Current heading of the ASV + :param delay: Time delay to cary out searching operation :return tgt_wp : Target waypoint - :return complete: Boolean flag determining weather the scanning operation has been completed + :return complete: Boolean flag determining weather the searching operation has been completed ''' tgt_wp = Mission.Waypoint(gps_lat = 0, gps_lon = 0) range_1 = numpy.linspace(-theta,theta,10) @@ -188,16 +257,16 @@ def update_path_planner(self): :return: None ''' self.current_wp.convert_to_enu() - mission_complete, self.g2g_heading, self.cte, self.target_wp = self.path_planner.update(current_wp = self.current_wp, speed = self.current_speed) - if ((mission_complete == False) and ((self.target_wp.wp_mode == "PASS") or (self.target_wp.wp_mode == "NONE"))): + mc, self.g2g_heading, self.cte, self.target_wp = self.path_planner.update(current_wp = self.current_wp, speed = self.current_speed) + if ((mc == False) and ((self.target_wp.wp_mode == "PASS") or (self.target_wp.wp_mode == "NONE"))): self.go_to_goal() else: if ((self.current_wp.pose.gps_lat != 0) and (self.target_wp.wp_mode == "HALT")): e_pose = self.station_keeping(self.target_wp) - if e_pose < self.pose_tol: + if e_pose < 0.5: # TODO: add to config self.mission_complete = True - elif ((self.current_wp.pose.gps_lat != 0) and (self.target_wp.wp_mode == "SCAN")): - goal_wp, complete_pattern = self.scan_around(self.target_wp,math.pi/6, 30) + elif ((self.current_wp.pose.gps_lat != 0) and (self.target_wp.wp_mode == "PLAN")): + goal_wp, complete_pattern = self.search_pattern(self.target_wp,math.pi/6, 30) self.station_keeping(goal_wp) if complete_pattern: self.mission_complete = True @@ -210,7 +279,7 @@ def load_mission(self, mission): ''' Loads the external mission into a list of waypoints that can be undestood by the path planner. - :param mission: The list of GeoPoseStamped waypoints provided by the high-level mission planner + :param mission: The list of GeoPoseStamped waypoints provided by the high level mission planner :return: None ''' @@ -229,7 +298,8 @@ def load_mission(self, mission): self.mission = Mission.Path(waypoints=wps) self.path_planner = PathPlanner(mission=self.mission, path_creator=DubinsPath) - def mission_manager(self, goal): + + def run(self, goal): ''' Main execution loop of the action server. @@ -238,12 +308,11 @@ def mission_manager(self, goal): :return feedback, result: Feedback and the final action result ''' self.load_mission(goal.mission) - rate = rospy.Rate(10) # ROS rate while not self.mission_complete: # Check that preempt has not been requested by the client if self.action_server.is_preempt_requested(): if self.debug: - print("{}: Preempted!".format('singaboat_mission_manager')) + print("{}: Preempted!".format(self.node_name)) print() self.action_server.set_preempted() self.result.mission_complete = False @@ -253,68 +322,33 @@ def mission_manager(self, goal): self.feedback.mission_progress = 0.0 # Publish the feedback self.action_server.publish_feedback(self.feedback) - rate.sleep() + self.rate.sleep() # Set the correct responses whenever the mission is completed if self.mission_complete: self.mission_complete = False self.result.mission_complete = True if self.debug: - print("{}: Succeeded!".format('singaboat_mission_manager')) + print("{}: Succeeded!".format(self.node_name)) print() self.action_server.set_succeeded(self.result) def config_callback(self, config, level): # Handle updated configuration values - self.min_speed = config['min_speed'] # Minimum longitudinal velocity of ASV - self.max_speed = config['max_speed'] # Maximum longitudinal velocity of ASV - self.gps_offset = config['gps_offset'] # GPS offset w.r.t. WAM-V along X-axis - self.pose_tol = config['pose_tol'] # Pose tolerance to determine whether goal is reached - self.h2g_kp = config['H2G_kP'] # Proportional gain of head-to-goal PD controller - self.h2g_kd = config['H2G_kD'] # Derivative gain of head-to-goal PD controller - self.pid_g2g = PIDController(config['G2G_kP'], config['G2G_kI'], config['G2G_kD'], config['G2G_kS']) # Go-to-goal PID controller - self.pid_sk_vx = PIDController(config['SK_Vx_kP'], config['SK_Vx_kI'], config['SK_Vx_kD'], config['SK_Vx_kS']) # Station-keeping Vx PID controller - self.pid_sk_vy = PIDController(config['SK_Vy_kP'], config['SK_Vy_kI'], config['SK_Vy_kD'], config['SK_Vy_kS']) # Station-keeping Vy PID controller - self.pid_sk_wz = PIDController(config['SK_Wz_kP'], config['SK_Wz_kI'], config['SK_Wz_kD'], config['SK_Wz_kS']) # Station-keeping Wz PID controller - self.goal_tol = config['goal_tol'] # Goal tolerance dead-band of go-to-goal PID controller - self.v_const = config['v_const'] # Proportional gain for linear velocity outside goal tolerance - self.v_limit = config['v_limit'] # Saturation limit for linear velocity outside goal tolerance - self.debug = config['debug'] # Flag to enable/disable debug messages - self.config = config + self.h2g_kp = config['H2G_kP'] # Proportional gain of head-to-goal PD controller + self.h2g_kd = config['H2G_kD'] # Derivative gain of head-to-goal PD controller + self.pid_g2g = PIDController(config['G2G_kP'], config['G2G_kI'], config['G2G_kD'], config['G2G_kS']) # Go-to-goal PID controller + self.pid_sk_vx = PIDController(config['SK_Vx_kP'], config['SK_Vx_kI'], config['SK_Vx_kD'], config['SK_Vx_kS']) # Station-keeping Vx PID controller + self.pid_sk_vy = PIDController(config['SK_Vy_kP'], config['SK_Vy_kI'], config['SK_Vy_kD'], config['SK_Vy_kS']) # Station-keeping Vy PID controller + self.pid_sk_wz = PIDController(config['SK_Wz_kP'], config['SK_Wz_kI'], config['SK_Wz_kD'], config['SK_Wz_kS']) # Station-keeping Wz PID controller + self.goal_tol = config['goal_tol'] # Goal tolerance dead-band of go-to-goal PID controller + self.v_const = config['v_const'] # Proportional gain for linear velocity outside goal tolerance + self.v_limit = config['v_limit'] # Saturation limit for linear velocity outside goal tolerance + self.config = config return config ################################################################################ if __name__ == "__main__": - rospy.init_node('singaboat_mission_manager', anonymous = True) - - # MissionManager class instance - mission_manager_node = MissionManager() - - # Dynamic reconfigure server - mission_manager_node.dyn_reconf_srv = Server(MissionManagerConfig, mission_manager_node.config_callback) - - # Message - mission_manager_node.cmd_vel_msg = Twist() - - # Subscribers - rospy.Subscriber('/wamv/sensors/gps/gps/fix', NavSatFix, mission_manager_node.gps_callback) - rospy.Subscriber('/wamv/sensors/gps/gps/fix_velocity', Vector3Stamped, mission_manager_node.speed_callback) - rospy.Subscriber('/wamv/sensors/imu/imu/data', Imu, mission_manager_node.imu_callback) - rospy.Subscriber('/wamv/obstacle_potential_field', Float32, mission_manager_node.collision_avoidance_callback) - - # Publisher - mission_manager_node.cmd_vel_pub = rospy.Publisher('/wamv/cmd_vel', Twist, queue_size=1) - - # Action server - mission_manager_node.action_server = actionlib.SimpleActionServer('singaboat_mission_manager', MissionAction, execute_cb = mission_manager_node.mission_manager, auto_start = False) - mission_manager_node.action_server.start() - - # Wait for valid messages to ensure proper state initialization - rospy.wait_for_message('/wamv/sensors/gps/gps/fix', NavSatFix) - rospy.wait_for_message('/wamv/sensors/gps/gps/fix_velocity', Vector3Stamped) - rospy.wait_for_message('/wamv/sensors/imu/imu/data', Imu) - - try: - rospy.spin() - except rospy.ROSInterruptException: - pass + mission_manager = MissionManager(node_name='singaboat_mission_manager') + mission_manager.init_app() + rospy.spin() diff --git a/singaboat_vrx/src/singaboat_pose_processor.py b/singaboat_vrx/src/singaboat_pose_processor.py new file mode 100644 index 0000000..d21dbbf --- /dev/null +++ b/singaboat_vrx/src/singaboat_pose_processor.py @@ -0,0 +1,149 @@ +#!/usr/bin/env python3 + +import math +import rospy +from std_msgs.msg import Int32 +from sensor_msgs.msg import NavSatFix +from sensor_msgs.msg import Imu +from geographic_msgs.msg import GeoPoseStamped +from geographic_msgs.msg import GeoPath +from geometry_msgs.msg import Pose # !!! NOT USED AS PER THE STANDARDS !!! +from dynamic_reconfigure.server import Server +from vrx_gazebo.msg import Task +from singaboat_vrx.cfg import PoseProcessorConfig +from singaboat_vrx.common_utilities import gps_to_enu, quaternion_to_euler + +################################################################################ + +class PoseProcessor: + def __init__(self): + # Initialize pose processor + self.cur_pos_x = self.cur_pos_y = self.cur_yaw = None # Current state + self.cmd_pos_x, self.cmd_pos_y, self.cmd_yaw = ([] for i in range(3)) # Commanded state + self.wp_index = 0 # Waypoint index (starts from 0) + self.cmd_topic_name = None # Topic name for commanded state + self.cmd_topic_type = None # Topic type for commanded state + self.config = {} # Pose processor configuration + # ROS infrastructure + self.wp_count_msg = None + self.cur_pose_msg = None + self.cmd_pose_msg = None + self.wp_count_pub = None + self.cur_pose_pub = None + self.cmd_pose_pub = None + self.dyn_reconf_srv = None + + def cur_pos_callback(self, msg): + if self.cur_yaw is None: # If no yaw data is available, GPS offset cannot be compensated + return + lat = msg.latitude + lon = msg.longitude + self.cur_pos_x, self.cur_pos_y, _ = gps_to_enu(lat, lon) + # WAM-V frame is +0.85 m offset in local x-axis w.r.t. GPS frame + self.cur_pos_x += self.gps_offset * math.cos(self.cur_yaw) + self.cur_pos_y += self.gps_offset * math.sin(self.cur_yaw) + + def cur_rot_callback(self, msg): + self.cur_yaw = quaternion_to_euler(msg.orientation)[2] + + def wp_index_callback(self, msg): + self.wp_index = msg.data + + def cmd_pose_callback(self, msg): + if str(msg._type) == 'geographic_msgs/GeoPoseStamped': + self.wp_count_msg.data = 1 + lat = msg.pose.position.latitude + lon = msg.pose.position.longitude + cmd_pos_x, cmd_pos_y, _ = gps_to_enu(lat, lon) + self.cmd_pos_x.append(cmd_pos_x) + self.cmd_pos_y.append(cmd_pos_y) + cmd_yaw = quaternion_to_euler(msg.pose.orientation)[2] + self.cmd_yaw.append(cmd_yaw) + elif str(msg._type) == 'geographic_msgs/GeoPath': + for i in range(len(msg.poses)): + if msg.poses: # Sanity check + self.wp_count_msg.data = len(msg.poses) + waypoint = msg.poses[i] # Indexing starts from 0 + lat = waypoint.pose.position.latitude + lon = waypoint.pose.position.longitude + cmd_pos_x, cmd_pos_y, _ = gps_to_enu(lat, lon) + self.cmd_pos_x.append(cmd_pos_x) + self.cmd_pos_y.append(cmd_pos_y) + cmd_yaw = quaternion_to_euler(waypoint.pose.orientation)[2] + self.cmd_yaw.append(cmd_yaw) + + def pose_processor(self): + # Current pose + if not [x for x in (self.cur_pos_x, self.cur_pos_y, self.cur_yaw) if x is None]: + self.cur_pose_msg.position.x = self.cur_pos_x + self.cur_pose_msg.position.y = self.cur_pos_y + self.cur_pose_msg.orientation.z = self.cur_yaw + print("Cur_Pose: %.4f m %.4f m %.4f rad" % (self.cur_pose_msg.position.x, self.cur_pose_msg.position.y, self.cur_pose_msg.orientation.z)) + self.cur_pose_pub.publish(self.cur_pose_msg) + # Commanded pose + if self.cmd_pos_x and self.cmd_pos_y and self.cmd_yaw: + self.cmd_pose_msg.position.x = self.cmd_pos_x[self.wp_index] + self.cmd_pose_msg.position.y = self.cmd_pos_y[self.wp_index] + self.cmd_pose_msg.orientation.z = self.cmd_yaw[self.wp_index] + print("WP_Index: %d" % (self.wp_index)) + print("Cmd_Pose: %.4f m %.4f m %.4f rad" % (self.cmd_pose_msg.position.x, self.cmd_pose_msg.position.y, self.cmd_pose_msg.orientation.z)) + self.cmd_pose_pub.publish(self.cmd_pose_msg) + # Waypoint count + print("WP_Count: %d" % (self.wp_count_msg.data)) + self.wp_count_pub.publish(self.wp_count_msg) + + def config_callback(self, config, level): + # Handle updated configuration values + self.gps_offset = config['gps_offset'] + self.config = config + return config + +################################################################################ + +if __name__ == '__main__': + rospy.init_node('singaboat_pose_processor', anonymous = True) + + # PoseProcessor class instance + pose_processor_node = PoseProcessor() + + # Dynamic reconfigure server + pose_processor_node.dyn_reconf_srv = Server(PoseProcessorConfig, pose_processor_node.config_callback) + + # Task specific topic selection logic + task_msg = rospy.wait_for_message('/vrx/task/info', Task) + if task_msg.name == 'station_keeping': + pose_processor_node.cmd_topic_name = '/vrx/station_keeping/goal' + pose_processor_node.cmd_topic_type = GeoPoseStamped + elif task_msg.name == 'wayfinding': + pose_processor_node.cmd_topic_name = '/vrx/wayfinding/waypoints' + pose_processor_node.cmd_topic_type = GeoPath + + # Messages + pose_processor_node.wp_count_msg = Int32() + pose_processor_node.cur_pose_msg = Pose() + pose_processor_node.cmd_pose_msg = Pose() + + # Subscribers + rospy.Subscriber('/wamv/sensors/gps/gps/fix', NavSatFix, pose_processor_node.cur_pos_callback) + rospy.Subscriber('/wamv/sensors/imu/imu/data', Imu, pose_processor_node.cur_rot_callback) + rospy.Subscriber('/vrx/wp_index', Int32, pose_processor_node.wp_index_callback) + rospy.Subscriber(pose_processor_node.cmd_topic_name, pose_processor_node.cmd_topic_type, pose_processor_node.cmd_pose_callback) + + # Publishers + pose_processor_node.wp_count_pub = rospy.Publisher('/vrx/wp_count', Int32, queue_size = 10) + pose_processor_node.cur_pose_pub = rospy.Publisher('/wamv/cur_pose', Pose, queue_size = 10) + pose_processor_node.cmd_pose_pub = rospy.Publisher('/wamv/cmd_pose', Pose, queue_size = 10) + + # Wait for valid messages to ensure proper state initialization + rospy.wait_for_message('/wamv/sensors/gps/gps/fix', NavSatFix) + rospy.wait_for_message('/wamv/sensors/imu/imu/data', Imu) + + # ROS rate + rate = rospy.Rate(20) + + try: + while not rospy.is_shutdown(): + pose_processor_node.pose_processor() + rate.sleep() + except rospy.ROSInterruptException: + pass diff --git a/singaboat_vrx/src/singaboat_scan_dock_deliver.py b/singaboat_vrx/src/singaboat_scan_dock_deliver.py index 6c25585..48ede3f 100755 --- a/singaboat_vrx/src/singaboat_scan_dock_deliver.py +++ b/singaboat_vrx/src/singaboat_scan_dock_deliver.py @@ -6,8 +6,8 @@ import actionlib from std_msgs.msg import Empty from dynamic_reconfigure.server import Server -from geometry_msgs.msg import Pose, Twist # !!! Pose.orientation NOT used as a quaternion !!! -from sensor_msgs.msg import NavSatFix, Imu, Image, PointCloud2 +from geometry_msgs.msg import Pose, Twist +from sensor_msgs.msg import NavSatFix, Imu, Image from cv_bridge import CvBridge, CvBridgeError from geographic_msgs.msg import GeoPoseStamped from vrx_gazebo.msg import Task @@ -15,33 +15,51 @@ from singaboat_vrx.cfg import ScanDockDeliverConfig from singaboat_vrx.perception_module import ColorSequenceDetector, BayDetector from singaboat_vrx.planning_module import Mission -from singaboat_vrx.control_module import PolarController -from singaboat_vrx.common_utilities import quaternion_to_euler, euler_to_quaternion, cartesian_to_polar, euclidean_distance, normalize_angle, local_to_global_tf, gps_to_enu, enu_to_gps +from singaboat_vrx.common_utilities import quaternion_to_euler, euler_to_quaternion, cartesian_to_polar, euclidean_distance, normalize_angle, polar_controller, local_to_global_tf, gps_to_enu, enu_to_gps ################################################################################ # Reference list containing interested colors -interested_colors = ["Red", "Yellow", "Blue", "Green"] +interested_colors = ["red", "yellow", "blue", "green"] ################################################################################ class ScanDockDeliver: - def __init__(self): + def __init__(self, node_name): # Initialize scan-dock-deliver - self.cv_bridge = CvBridge() # CvBridge object - self.image = None # Image + self.node_name = node_name # TODO: move away from here + rospy.init_node(self.node_name, anonymous=True) # TODO: move away from here + self.rate = rospy.Rate(5) # TODO: move away from here + self.color_sequence_detector = ColorSequenceDetector(interested_colors) # ColorSequenceDetector object + self.bay_detector = BayDetector(interested_colors) # BayDetector object + self.target_pose = None # Target bay pose + self.near_dock = False # Whether ASV is near the dock (in front of bays) + self.near_bay = False # Whether ASV is very close to the target bay + self.inside_bay = False # Whether ASV is inside the target bay + self.next_shift_dist = 10 # Pose keeping distance difference to target pose + self.cv_bridge = CvBridge() # CvBridge object + self.image = None # Image self.docking_time = None # Docking time - self.asv_geopose = Mission.Pose(gps_lat=0, gps_lon=0) # ASV pose in GPS coordinates + self.ball_balance = 4 # Ball balance + self.asv_geopose = Mission.Pose(gps_lat=0, gps_lon=0) # ASV pose in GPS coordinates self.asv_cartpose = Pose() # ASV pose in cartesian (ENU) coordinates - self.target_pose = None # Target bay pose - self.near_dock = False # Whether ASV is near the dock (in front of bays) - self.near_bay = False # Whether ASV is very close to the target bay - self.inside_bay = False # Whether ASV is inside the target bay - self.config = {} # Scan-dock-deliver configuration - # ROS infrastructure - self.cmd_vel_pub = None - self.shooter_pub = None - self.dyn_reconf_srv = None + self.debug = False # Flag to enable/disable debug messages | TODO: move to config + self.config = {} # Scan-dock-deliver configuration + self.dyn_reconf_srv = Server(ScanDockDeliverConfig, self.config_callback) # TODO: move away from here + + # Subscribers + rospy.Subscriber('/vrx/task/info', Task, self.task_callback, queue_size=1) # TODO: move away from here + rospy.Subscriber('/wamv/sensors/gps/gps/fix', NavSatFix, self.gps_callback, queue_size=1) # TODO: move away from here + rospy.Subscriber('/wamv/sensors/imu/imu/data', Imu, self.imu_callback, queue_size=1) # TODO: move away from here + rospy.Subscriber('/wamv/sensors/cameras/camera/image_raw', Image, self.camera_callback, queue_size=1) # TODO: move away from here + + # Publishers + self.cmd_vel_pub = rospy.Publisher('/wamv/cmd_vel', Twist, queue_size=1) # TODO: move away from here + self.shooter_pub = rospy.Publisher('/wamv/shooters/ball_shooter/fire', Empty, queue_size=10) # TODO: move away from here + + # Action client + self.action_client = actionlib.SimpleActionClient('singaboat_mission_manager', MissionAction) # TODO: move away from here + self.action_client.wait_for_server() # TODO: move away from here def gps_callback(self, msg): if self.asv_geopose.heading is None: # If no yaw data is available, GPS offset cannot be compensated @@ -70,380 +88,286 @@ def camera_callback(self, msg): print(error) print() - def geopose_stamped(self, pose, wp_mode="PASS"): - ''' - Convert custom geopose to ROS GeoPoseStamped message. - - :param pose : Mission.Pose object - :param wp_mode: Waypoint mode - - :return geopose_stamped: geographic_msgs.GeoPoseStamped object - ''' - geopose_stamped = GeoPoseStamped() - geopose_stamped.header.stamp = rospy.Time.now() - geopose_stamped.header.frame_id = wp_mode - geopose_stamped.pose.position.latitude = pose.gps_lat - geopose_stamped.pose.position.longitude = pose.gps_lon - q = euler_to_quaternion(0, 0, pose.heading) - geopose_stamped.pose.orientation.x = q[0] - geopose_stamped.pose.orientation.y = q[1] - geopose_stamped.pose.orientation.z = q[2] - geopose_stamped.pose.orientation.w = q[3] - return geopose_stamped - - def navigate(self, goal_pos_x, goal_pos_y, goal_rot_z, goal_mode="PASS"): - ''' - Create a path from the current position of ASV to the given waypoint, and send - the path from this node (action client) to the mission manager (action server). - - :param goal_pos_x: X-coordinate of goal pose - :param goal_pos_y: Y-coordinate of goal pose - :param goal_rot_z: Orientation of goal pose - - :return: None - ''' - goal_pose = Mission.Pose(enu_x=goal_pos_x, enu_y=goal_pos_y, heading=goal_rot_z) - goal_geopose_stamped = self.geopose_stamped(goal_pose, wp_mode=goal_mode) - asv_geopose_stamped = self.geopose_stamped(self.asv_geopose) - path = [asv_geopose_stamped, goal_geopose_stamped] - goal = MissionGoal(mission=path) - self.action_client.send_goal(goal) - - def scan(self): - ''' - Command the ASV to wait at its current position and scan the environment. Send - this command from this node (action client) to the mission manager (action server). - - :param: None - - :return: None - ''' - asv_geopose_stamped = self.geopose_stamped(self.asv_geopose, wp_mode="SCAN") - command = [asv_geopose_stamped] - goal = MissionGoal(mission=command) - self.action_client.send_goal(goal) + def task_callback(self, data): + self.task_info = data - def drive_forward(self, distance=2.0): + def to_ros_geopose(self, pose, wp_type="PASS"): ''' - Drive the ASV forward (in the direction of its current heading) by a certain amount of distance. - - :param distance: Amount of distance to move forward + Convert custom geopose to ROS GeoPoseStamped message - :return: None - ''' - asv_pos_x, asv_pos_y, asv_rot_z = self.asv_cartpose.position.x, self.asv_cartpose.position.y, self.asv_cartpose.orientation.z - goal_pos_x, goal_pos_y, goal_rot_z = asv_pos_x + distance * math.cos(asv_rot_z), asv_pos_y + distance * math.sin(asv_rot_z), asv_rot_z - self.navigate(goal_pos_x, goal_pos_y, goal_rot_z) + :param pose : Mission.Pose + :param wp_type: Waypoint mode - def caution_waypoint(self, goal_pos_x, goal_pos_y, goal_rot_z, distance=2.0): + :return: geographic_msgs.GeoPoseStamped ''' - Create a caution waypoint by shifting the goal pose a certain amount of distance towards the WAM-V - to allow room for adjustments through temporary station-keeping. - - :param distance: Amount of distance to shift the goal pose - - :return cwp_pos_x, cwp_pos_y, cwp_rot_z: Pose of the caution waypoint (CWP) - ''' - cwp_pos_x, cwp_pos_y, cwp_rot_z = goal_pos_x - distance * math.cos(goal_rot_z), goal_pos_y - distance * math.sin(goal_rot_z), goal_rot_z - return cwp_pos_x, cwp_pos_y, cwp_rot_z - - def update_target_pose(self, target_pos_x, target_pos_y, target_rot_z): + geopose = GeoPoseStamped() + geopose.header.stamp = rospy.Time.now() + geopose.header.frame_id = wp_type + geopose.pose.position.latitude = pose.gps_lat + geopose.pose.position.longitude = pose.gps_lon + q = euler_to_quaternion(0, 0, pose.heading) + geopose.pose.orientation.x = q[0] + geopose.pose.orientation.y = q[1] + geopose.pose.orientation.z = q[2] + geopose.pose.orientation.w = q[3] + return geopose + + def send_goal(self, xg, yg, thetag, goal_type="PASS"): + goal_pose = Mission.Pose(enu_x=xg, enu_y=yg, heading=thetag) + goal_geopose_stamped = self.to_ros_geopose(goal_pose, wp_type=goal_type) + asv_geopose_stamped = self.to_ros_geopose(self.asv_geopose) + goals = MissionGoal(mission=[asv_geopose_stamped, goal_geopose_stamped]) + self.action_client.send_goal(goals) + + def stay_still(self): + asv_geopose_stamped = self.to_ros_geopose(self.asv_geopose, wp_type="PLAN") + goals = MissionGoal(mission=[asv_geopose_stamped]) + self.action_client.send_goal(goals) + + def move_forward(self, distance=2.0): + x, y, theta = self.asv_cartpose.position.x, self.asv_cartpose.position.y, self.asv_cartpose.orientation.z + xg, yg, thetag = x + distance * math.cos(theta), y + distance * math.sin(theta), theta + self.send_goal(xg, yg, thetag) + + def get_shifted_target_pose(self, x, y, theta, distance=2.0): + x, y, theta = x - distance * math.cos(theta), y - distance * math.sin(theta), theta + return x, y, theta + + def move_to_target(self, target_bay=None, do_shift=True): ''' - Update target pose by filtering out potential abrupt changes. + Move in front of target bay with Dubin's path tracker, dock into the bay with local polar controller + Note: Suffix `r` means pose in robot (WAM-V) or reference frame, suffix `g` means pose in global or ENU frame - :param target_pos_x: X-coordinate of target pose - :param target_pos_y: Y-coordinate of target pose - :param target_rot_z: Orientation of target pose + :param target_bay: pose in robot frame + :param do_shift: whether to shift target pose towards robot to make room for station keeping - :return: None + :return: ''' - assert self.target_pose is not None - if self.debug: - print("Previous Target Pose: {} m, {} m, {}°".format(self.target_pose.position.x, self.target_pose.position.y, numpy.rad2deg(self.target_pose.orientation.z))) - print("Requested Target Pose: {} m, {} m, {}°".format(target_pos_x, target_pos_y, numpy.rad2deg(target_rot_z))) - print() - new_target_pose = Pose() - new_target_pose.position.x = target_pos_x - new_target_pose.position.y = target_pos_y - new_target_pose.orientation.z = target_rot_z - dist = euclidean_distance(self.target_pose, new_target_pose) - angle_diff = numpy.rad2deg(abs(normalize_angle(self.target_pose.orientation.z - new_target_pose.orientation.z))) - if dist < 10 and angle_diff < 20: + if target_bay is not None: + assert len(target_bay) == 3 + # Transform target bay pose to global frame + x, y, theta = self.asv_cartpose.position.x, self.asv_cartpose.position.y, self.asv_cartpose.orientation.z + # Update shifted target pose in robot frame for visualization + xr, yr, thetar = target_bay[0] + 0.7, target_bay[1], target_bay[2] + xrs, yrs, thetars = self.get_shifted_target_pose(xr, yr, thetar, distance=(5 if do_shift else 0)) + xg, yg, thetag = local_to_global_tf(x, y, theta, xrs, yrs, thetars) + if self.target_pose is None: + self.target_pose = Pose() + self.target_pose.position.x = xg + self.target_pose.position.y = yg + self.target_pose.orientation.z = thetag + else: + self.update_target_pose(xg, yg, thetag) # Update target pose + if not self.near_dock: # Move ASV near the dock (i.e. in front of the docking bays) + xg, yg, thetag = self.target_pose.position.x, self.target_pose.position.y, self.target_pose.orientation.z + xm, ym, thetam = self.get_shifted_target_pose(xg, yg, thetag, distance=30) + self.send_goal(xm, ym, thetam, goal_type="HALT") + self.action_client.wait_for_result(timeout=rospy.Duration(30)) + res = self.action_client.get_result() + print("ASV is approaching the dock...") if self.debug: - print("Updating the target pose...") + print("Dock Waypoint Action Result:".format(res)) + print() + self.action_client.cancel_all_goals() + self.near_dock = True + else: # Move ASV near the docking bay + if not self.near_bay: + self.near_bay = self.reached_bay(max_dist=16, max_angle_diff=20) + if self.near_bay: + if not self.inside_bay: # Move ASV inside the docking bay + self.inside_bay = self.reached_bay(max_dist=3.99, max_angle_diff=35) + xg, yg, thetag = self.get_shifted_target_pose(self.target_pose.position.x, self.target_pose.position.y, self.target_pose.orientation.z, distance=self.next_shift_dist) + print("ASV has reached near the target bay, attempting to dock...") + if self.debug: + print("Target Bay Pose: {} m, {} m, {} rad".format(xg, yg, thetag)) print() - self.target_pose.position.x, self.target_pose.position.y, self.target_pose.orientation.z = target_pos_x, target_pos_y, target_rot_z - else: - if self.debug: - print("Requested target pose is very different from the previous target pose.") - print("Not updating the target pose...") + self.send_goal(xg, yg, thetag, goal_type="PARK") + # Wait (station-keeping) for 5 seconds at each intermediate waypoint for safer docking + self.action_client.wait_for_result(timeout=rospy.Duration(5)) + if self.debug: + print("Station-keeping for {} meter shift finished.".format(self.next_shift_dist)) + print() + self.next_shift_dist -= 3 # Set next station-keeping pose 3 meters forward + self.next_shift_dist = max(0, self.next_shift_dist) # Do not exceed target pose + else: # Try to reach target bay with larger speed (using polar controller) + rho, alpha, beta = cartesian_to_polar(self.asv_cartpose, self.target_pose) + cmd_vel_msg = polar_controller(rho, alpha, beta, kr=0.2, ka=3.6, kb=0.1, max_linear_x=2.23, max_angular_z=0.89) # TODO: import PolarController object from control_module + print("ASV has reached near the dock, approaching the target bay...") + if self.debug: + print("rho: {}, alpha: {}, beta: {}, lin_vel_x: {}, ang_vel_z: {}".format(rho, alpha, beta, cmd_vel_msg.linear.x, cmd_vel_msg.angular.z)) print() - if self.debug: - print("Current Target Pose: {} m, {} m, {}°".format(self.target_pose.position.x, self.target_pose.position.y, numpy.rad2deg(self.target_pose.orientation.z))) - print() + self.cmd_vel_pub.publish(cmd_vel_msg) - def reached_near_bay(self, dist_tol=1.0, angle_tol=20.0): + def reached_bay(self, max_dist=1.0, max_angle_diff=20.0): ''' - Check weather the ASV has reached near (within the prescribed tolerance of) the target bay. + Check weather the ASV has reached near the target bay - :param dist_tol : Distance tolerance in meters - :param angle_tol: Orientation tolerance in degrees + :param max_dist : Distance threshold in meters + :param max_angle_diff: Orientation threshold in degrees :return result: Boolean flag determining weather the ASV has reached near the target bay ''' assert self.target_pose is not None dist = euclidean_distance(self.asv_cartpose, self.target_pose) - angle = abs(normalize_angle(self.asv_cartpose.orientation.z - self.target_pose.orientation.z)) + theta = abs(normalize_angle(self.asv_cartpose.orientation.z - self.target_pose.orientation.z)) if self.debug: print("ASV Pose:") print(self.asv_cartpose) - print("Target Pose:") + print("Target Bay Pose:") print(self.target_pose) print() + print("Driving to the target bay...") print("Position Error: {} m".format(dist)) - print("Orientation Error: {}°".format(numpy.rad2deg(angle))) + print("Orientation Error: {}°".format(numpy.rad2deg(theta))) print() - result = dist < dist_tol and angle < numpy.deg2rad(angle_tol) - return result + return dist < max_dist and theta < numpy.deg2rad(max_angle_diff) - def dock(self, target_bay_pose=None, caution=True): + def update_target_pose(self, xg, yg, thetag): ''' - Drive the ASV near the dock (i.e. in front of the docking bays) using Dubin's path tracker, - approach the target bay using polar controller, and perform docking operation with caution - using Dubin's path tracker. + Filter potential abrupt change of target pose to remove noise. - :param target_bay_pose: Target bay pose w.r.t. WAM-V frame - :param caution : Flag to enable/disable creation of caution waypoint(s) + :param xg: Goal position X-coordinate + :param yg: Goal position Y-coordinate + :param thetag: Goal orientation :return: None ''' - if target_bay_pose is not None: - assert len(target_bay_pose) == 3 - # Current pose of the ASV - asv_pos_x, asv_pos_y, asv_rot_z = self.asv_cartpose.position.x, self.asv_cartpose.position.y, self.asv_cartpose.orientation.z - # Bay pose w.r.t. WAM-V frame - bay_pos_x, bay_pos_y, bay_rot_z = target_bay_pose[0] + self.lidar_offset, target_bay_pose[1], target_bay_pose[2] - # Add caution waypoint - cwp_pos_x, cwp_pos_y, cwp_rot_z = self.caution_waypoint(bay_pos_x, bay_pos_y, bay_rot_z, distance=(5 if caution else 0)) - # Transform target pose to global frame - target_pos_x, target_pos_y, target_rot_z = local_to_global_tf(asv_pos_x, asv_pos_y, asv_rot_z, cwp_pos_x, cwp_pos_y, cwp_rot_z) - # Update target pose - if self.target_pose is None: - self.target_pose = Pose() - self.target_pose.position.x = target_pos_x - self.target_pose.position.y = target_pos_y - self.target_pose.orientation.z = target_rot_z - else: - self.update_target_pose(target_pos_x, target_pos_y, target_rot_z) # Update target pose by filtering out potential abrupt changes - # Move ASV near the dock (i.e. in front of the docking bays) - if not self.near_dock: - target_pos_x, target_pos_y, target_rot_z = self.target_pose.position.x, self.target_pose.position.y, self.target_pose.orientation.z - cwp_pos_x, cwp_pos_y, cwp_rot_z = self.caution_waypoint(target_pos_x, target_pos_y, target_rot_z, distance=30) - self.navigate(cwp_pos_x, cwp_pos_y, cwp_rot_z, goal_mode="HALT") - self.action_client.wait_for_result(timeout=rospy.Duration(30)) - self.action_client.cancel_all_goals() - self.near_dock = True - # Move ASV near the target docking bay and perform docking operation + assert self.target_pose is not None + if self.debug: + print("Previous Bay Pose: {} m, {} m, {}°".format(self.target_pose.position.x, self.target_pose.position.y, numpy.rad2deg(self.target_pose.orientation.z))) + print("Alternate Bay Pose: {} m, {} m, {}°".format(xg, yg, numpy.rad2deg(thetag))) + print() + new_target_pose = Pose() + new_target_pose.position.x = xg + new_target_pose.position.y = yg + new_target_pose.orientation.z = thetag + dist = euclidean_distance(self.target_pose, new_target_pose) + angle_diff = numpy.rad2deg(abs(normalize_angle(self.target_pose.orientation.z - thetag))) + if dist < 10 and angle_diff < 20: + xn, yn, thetan = xg, yg, thetag + self.target_pose.position.x, self.target_pose.position.y, self.target_pose.orientation.z = xn, yn, thetan else: - if not self.near_bay: - self.near_bay = self.reached_near_bay(dist_tol=16, angle_tol=20) - # If near the target docking bay, perform docking operation with caution - if self.near_bay: - if not self.inside_bay: # Check whether docking has been completed - self.inside_bay = self.reached_near_bay(dist_tol=4, angle_tol=20) - if self.inside_bay: - print("Docking successful!") - print() - target_pos_x, target_pos_y, target_rot_z = self.caution_waypoint(self.target_pose.position.x, self.target_pose.position.y, self.target_pose.orientation.z, distance=self.caution_distance) - print("ASV has reached near the target bay, attempting to dock...") - if self.debug: - print("Target Bay Pose: {} m, {} m, {} rad".format(target_pos_x, target_pos_y, target_rot_z)) - print() - self.navigate(target_pos_x, target_pos_y, target_rot_z, goal_mode="PARK") - # Wait (station-keeping) for 5 seconds at each intermediate (caution) waypoint for safer docking - self.action_client.wait_for_result(timeout=rospy.Duration(5)) - if self.debug: - print("Successfully passed caution waypoint!") - print() - self.caution_distance -= self.caution_step # Set next intermediate (caution) waypoint 3 meters forward - self.caution_distance = max(0, self.caution_distance) # Do not exceed target pose - # If not near the target docking bay, try to reach the target docking bay with higher speed (using polar controller) - else: - rho, alpha, beta = cartesian_to_polar(self.asv_cartpose, self.target_pose) # Convert target pose to polar coordinates - # Generate and publish `cmd_vel` message - cmd_vel_msg = self.polar_controller.control(rho, alpha, beta) # Polar controller output - print("ASV has reached near the dock, approaching the target bay...") - if self.debug: - print("rho: {}, alpha: {}, beta: {}, lin_vel_x: {}, ang_vel_z: {}".format(rho, alpha, beta, cmd_vel_msg.linear.x, cmd_vel_msg.angular.z)) + if self.debug: + print("New bay pose is far from the previous bay pose.") print() - self.cmd_vel_pub.publish(cmd_vel_msg) - - def deliver(self): - ''' - Drive the ASV to the right corner of the docking bay and then shoot the projectiles. - - :param: None + if self.debug: + print("Updated Bay Pose: {} m, {} m, {}°".format(self.target_pose.position.x, self.target_pose.position.y, numpy.rad2deg(self.target_pose.orientation.z))) + print() - :return: None - ''' + def shoot_balls(self): if self.docking_time is None: - self.docking_time = rospy.get_time() # Update `docking_time` only once after docking + self.docking_time = rospy.get_time() # Update docking_time only once after docking if self.debug: print("Docking Time: {}".format(self.docking_time)) print() - # Drive the ASV to the right corner of the docking bay cmd_vel_msg = Twist() cmd_vel_msg.linear.x = 1.67 - cmd_vel_msg.linear.y = -1.67 + cmd_vel_msg.linear.y = 1.67 cmd_vel_msg.angular.z = 0.0 self.cmd_vel_pub.publish(cmd_vel_msg) if self.debug: print("Elapsed Time: {}".format(rospy.get_time() - self.docking_time)) print() - if rospy.get_time() - self.docking_time >= self.delivery_delay: - # Keep the ASV at the right corner of the docking bay + if rospy.get_time() - self.docking_time >= 7: cmd_vel_msg = Twist() cmd_vel_msg.linear.x = 1.67 - cmd_vel_msg.linear.y = -1.67 + cmd_vel_msg.linear.y = 1.67 cmd_vel_msg.angular.z = 0.0 self.cmd_vel_pub.publish(cmd_vel_msg) - # Shoot the projectiles - print("Shooting projectiles...") + print("Shooting balls...") print() self.shooter_pub.publish() self.ball_balance -= 1 - def exit(self): - ''' - Safely exit the docking bay by driving the ASV backward. - - :param: None - - :return: None - ''' - print("Exiting the docking bay...") - print() + def exit_bay(self): start_time = rospy.get_time() end_time = rospy.get_time() cmd_vel_msg = Twist() - cmd_vel_msg.linear.x = -2.23 # Drive the ASV backward - rate = rospy.Rate(10) + cmd_vel_msg.linear.x = -2.23 + r = rospy.Rate(10) while end_time - start_time < 10: self.cmd_vel_pub.publish(cmd_vel_msg) - rate.sleep() + r.sleep() end_time = rospy.get_time() - def config_callback(self, config, level): - # Handle updated configuration values - self.polar_controller = PolarController(config['k_rho'], config['k_alpha'], config['k_beta'], config['max_lin_vel'], config['max_ang_vel']) # Polar controller - self.gps_offset = config['gps_offset'] # GPS offset w.r.t. WAM-V along X-axis - self.lidar_offset = config['lidar_offset'] # LIDAR offset w.r.t. WAM-V along X-axis - self.caution_distance = config['caution_distance'] # Caution distance from target pose - self.caution_step = config['caution_step'] # Step size to decrement caution distance from target pose - self.ball_balance = config['ball_balance'] # Number of balls yet to be shot from ASV - self.delivery_delay = config['delivery_delay'] # Time delay in seconds from docking to delivery - self.debug = config['debug'] # Flag to enable/disable debug messages - self.config = config - return config - -################################################################################ - -if __name__ == "__main__": - rospy.init_node('singaboat_scan_dock_deliver', anonymous = True) - - # ScanDockDeliver class instance - scan_dock_deliver_node = ScanDockDeliver() - - # Dynamic reconfigure server - scan_dock_deliver_node.dyn_reconf_srv = Server(ScanDockDeliverConfig, scan_dock_deliver_node.config_callback) - - # Color sequence and docking bay detectors - scan_dock_deliver_node.color_sequence_detector = ColorSequenceDetector(interested_colors, debug=scan_dock_deliver_node.debug) # ColorSequenceDetector class instance - scan_dock_deliver_node.bay_detector = BayDetector(interested_colors, debug=scan_dock_deliver_node.debug) # BayDetector class instance - - # Subscribers - rospy.Subscriber('/wamv/sensors/gps/gps/fix', NavSatFix, scan_dock_deliver_node.gps_callback) - rospy.Subscriber('/wamv/sensors/imu/imu/data', Imu, scan_dock_deliver_node.imu_callback) - rospy.Subscriber('/wamv/sensors/cameras/camera/image_raw', Image, scan_dock_deliver_node.camera_callback) - rospy.Subscriber('/wamv/sensors/cameras/camera/image_raw', Image, scan_dock_deliver_node.color_sequence_detector.camera_callback) - rospy.Subscriber('/wamv/sensors/cameras/camera/image_raw', Image, scan_dock_deliver_node.bay_detector.camera_callback) - rospy.Subscriber('/wamv/sensors/lidars/lidar/points', PointCloud2, scan_dock_deliver_node.bay_detector.dock_detector.lidar_callback) - - # Publishers - scan_dock_deliver_node.cmd_vel_pub = rospy.Publisher('/wamv/cmd_vel', Twist, queue_size=1) - scan_dock_deliver_node.shooter_pub = rospy.Publisher('/wamv/shooters/ball_shooter/fire', Empty, queue_size=1) - - # Action client - scan_dock_deliver_node.action_client = actionlib.SimpleActionClient('singaboat_mission_manager', MissionAction) - scan_dock_deliver_node.action_client.wait_for_server() - - # Wait for valid messages to ensure proper state initialization - rospy.wait_for_message('/wamv/sensors/gps/gps/fix', NavSatFix) - rospy.wait_for_message('/wamv/sensors/imu/imu/data', Imu) - rospy.wait_for_message('/wamv/sensors/cameras/camera/image_raw', Image) - rospy.wait_for_message('/wamv/sensors/lidars/lidar/points', PointCloud2) - - try: - rate = rospy.Rate(20) - + def run(self): # Initialization - while scan_dock_deliver_node.image is None: - if scan_dock_deliver_node.debug: - print() + while self.image is None: + if self.debug: print("Initializing...") print() rospy.sleep(rospy.Duration(secs=0, nsecs=1000*1000*500)) - - # 1. SCAN - scan_dock_deliver_node.scan() # Scan and decode color sequence of the light buoy + if self.debug: + print("ASV has all the sensory data ready!") + print() + # Detect and decode color sequence from the light buoy + self.stay_still() print() - print("Scanning light buoy color sequence...") + print("Detecting light buoy color sequence...") + if self.debug: + print("Keeping the still for better color sequence detection...") print() - color, shape = scan_dock_deliver_node.color_sequence_detector.detect() - scan_dock_deliver_node.bay_detector.target_bay_symbol(target_color=color, target_shape=shape) + color, shape = self.color_sequence_detector.detect() + self.bay_detector.set_target_color_shape(target_color=color, target_shape=shape) while not rospy.is_shutdown(): - rate.sleep() - target_bay_pose = None - if not scan_dock_deliver_node.near_dock: - target_bay_pose, all_bay_poses = scan_dock_deliver_node.bay_detector.detect() - theta, plane_cloud = scan_dock_deliver_node.bay_detector.plane_angle_and_cloud() + self.rate.sleep() + bay_pose = None + if not self.near_dock: + bay_pose, all_bay_poses = self.bay_detector.detect() + theta, plane_cloud = self.bay_detector.get_plane_theta_and_cloud() if theta is not None: - if scan_dock_deliver_node.debug: - print("Dock plane angle w.r.t. WAM-V frame: {}°".format(numpy.rad2deg(theta))) + if self.debug: + print("Plane theta in WAM-V frame: {}°".format(numpy.rad2deg(theta))) print() - if scan_dock_deliver_node.near_dock: - target_bay_pose, all_bay_poses = scan_dock_deliver_node.bay_detector.detect(cloud=plane_cloud) - if target_bay_pose is not None: - assert len(target_bay_pose) == 3 - if scan_dock_deliver_node.debug: - print("Dock plane angle got updated from {}° to {}°".format(target_bay_pose[2], theta)) + if self.near_dock: + bay_pose, all_bay_poses = self.bay_detector.detect(cloud=plane_cloud) + + if bay_pose is not None: + assert len(bay_pose) == 3 + if self.debug: + print("Plane theta got updated from {}° to {}°".format(bay_pose[2], theta)) print() - target_bay_pose[2] = theta + bay_pose[2] = theta else: - if scan_dock_deliver_node.debug: - print("Dock plane angle computation failed!") + if self.debug: + print("Plane theta computation failed!") print() - - # 2. DOCK - if target_bay_pose is None and scan_dock_deliver_node.target_pose is None: # If bay detection fails, try moving forward - if scan_dock_deliver_node.debug: + # If bay detection fails, try moving forward + if bay_pose is None and self.target_pose is None: + if self.debug: print("Try moving forward...") print() - scan_dock_deliver_node.drive_forward(distance=5) + self.move_forward(distance=5) continue - if not scan_dock_deliver_node.inside_bay: # If bay is detected successfully, try moving towards it and perform docking operation - if scan_dock_deliver_node.debug: + # If bay is detected successfully, try moving towards it + if not self.inside_bay: + if self.debug: print("Try moving towards the target bay...") print() - scan_dock_deliver_node.dock(target_bay_pose) - - # 3. DELIVER - else: # If docking is successful, try shooting the projectiles - scan_dock_deliver_node.action_client.cancel_all_goals() - scan_dock_deliver_node.deliver() # Shoot projectiles - if scan_dock_deliver_node.ball_balance == 0: - scan_dock_deliver_node.exit() # Exit the docking bay + self.move_to_target(bay_pose) + # If docking is successful, try shooting balls + else: + print("Docking successful!") + print() + self.action_client.cancel_all_goals() + self.shoot_balls() # Shoot balls + if self.ball_balance == 0: + print("Exiting the docking bay...") + print() + self.exit_bay() # Exit the docking bay break + def config_callback(self, config, level): + # Handle updated configuration values + self.gps_offset = config['gps_offset'] # GPS offset w.r.t. WAM-V along X-axis + self.config = config + return config + +################################################################################ + +if __name__ == "__main__": + scan_dock_deliver_node = ScanDockDeliver(node_name='ScanDockDeliver') + try: + scan_dock_deliver_node.run() except rospy.ROSInterruptException: pass diff --git a/singaboat_vrx/src/singaboat_scene_perception.py b/singaboat_vrx/src/singaboat_scene_perception.py index 4188d02..6332b8e 100755 --- a/singaboat_vrx/src/singaboat_scene_perception.py +++ b/singaboat_vrx/src/singaboat_scene_perception.py @@ -19,12 +19,6 @@ if __name__ == '__main__': rospy.init_node('singaboat_scene_perception') - # Wait for valid messages to ensure proper state initialization - rospy.wait_for_message('/wamv/sensors/gps/gps/fix', NavSatFix) - rospy.wait_for_message('/wamv/sensors/imu/imu/data', Imu) - rospy.wait_for_message('/wamv/sensors/cameras/camera/image_raw', Image) - rospy.wait_for_message('/wamv/sensors/lidars/lidar/points', PointCloud2) - task_msg = rospy.wait_for_message('/vrx/task/info', Task) if task_msg.name == 'perception': @@ -32,18 +26,16 @@ scene_perception_node = BuoyDetector() # Dynamic reconfigure server scene_perception_node.dyn_reconf_srv = Server(ScenePerceptionConfig, scene_perception_node.config_callback) - # Messages - scene_perception_node.cam_viz_msg = Image() - scene_perception_node.object_msg = GeoPoseStamped() + # Message + scene_perception_node.object_msg = GeoPoseStamped() # Subscribers rospy.Subscriber('/vrx/task/info', Task, scene_perception_node.task_callback) - rospy.Subscriber('/wamv/sensors/gps/gps/fix', NavSatFix, scene_perception_node.gps_callback) - rospy.Subscriber('/wamv/sensors/imu/imu/data', Imu, scene_perception_node.imu_callback) + rospy.Subscriber('/wamv/sensors/gps/gps/fix', NavSatFix, scene_perception_node.gps_callback,queue_size=1) + rospy.Subscriber('/wamv/sensors/imu/imu/data', Imu, scene_perception_node.imu_callback, queue_size=1) rospy.Subscriber('/wamv/sensors/cameras/camera/image_raw', Image, scene_perception_node.camera_callback) rospy.Subscriber('/wamv/sensors/lidars/lidar/points', PointCloud2, scene_perception_node.lidar_callback) - # Publishers - scene_perception_node.cam_viz_pub = rospy.Publisher('/rviz/cam_viz', Image, queue_size=10) - scene_perception_node.object_pub = rospy.Publisher('/vrx/perception/landmark', GeoPoseStamped, queue_size=10) + # Publisher + scene_perception_node.object_pub = rospy.Publisher('/vrx/perception/landmark', GeoPoseStamped, queue_size=10) # Recursive loop try: rospy.spin() @@ -56,23 +48,24 @@ # Dynamic reconfigure server gate_detector.buoy_detector.dyn_reconf_srv = Server(ScenePerceptionConfig, gate_detector.config_callback) # Messages - gate_detector.buoy_detector.cam_viz_msg = Image() - gate_detector.buoy_detector.object_msg = GeoPoseStamped() + gate_detector.buoy_detector.object_msg = GeoPoseStamped() gate_detector.buoy_detector.obstacle_msg = Float64MultiArray() # Subscribers rospy.Subscriber('/vrx/task/info', Task, gate_detector.buoy_detector.task_callback) - rospy.Subscriber('/wamv/sensors/gps/gps/fix', NavSatFix, gate_detector.buoy_detector.gps_callback) - rospy.Subscriber('/wamv/sensors/imu/imu/data', Imu, gate_detector.buoy_detector.imu_callback) + rospy.Subscriber('/wamv/sensors/gps/gps/fix', NavSatFix, gate_detector.buoy_detector.gps_callback,queue_size=1) + rospy.Subscriber('/wamv/sensors/imu/imu/data', Imu, gate_detector.buoy_detector.imu_callback, queue_size=1) rospy.Subscriber('/wamv/sensors/cameras/camera/image_raw', Image, gate_detector.buoy_detector.camera_callback) rospy.Subscriber('/wamv/sensors/lidars/lidar/points', PointCloud2, gate_detector.buoy_detector.lidar_callback) - rospy.Subscriber('/wamv/sensors/gps/gps/fix', NavSatFix, gate_detector.gps_callback) - rospy.Subscriber('/wamv/sensors/imu/imu/data', Imu, gate_detector.imu_callback) + rospy.Subscriber('/wamv/sensors/gps/gps/fix', NavSatFix, gate_detector.gps_callback,queue_size=1) + rospy.Subscriber('/wamv/sensors/imu/imu/data', Imu, gate_detector.imu_callback, queue_size=1) # Publishers - gate_detector.buoy_detector.cam_viz_pub = rospy.Publisher('/rviz/cam_viz', Image, queue_size=10) - gate_detector.buoy_detector.object_pub = rospy.Publisher('/wamv/detected_objects', GeoPoseStamped, queue_size=10) + gate_detector.buoy_detector.object_pub = rospy.Publisher('/wamv/detected_objects', GeoPoseStamped, queue_size=10) gate_detector.buoy_detector.obstacle_pub = rospy.Publisher('/wamv/detected_obstacles', Float64MultiArray, queue_size=10) + # Wait for valid messages to ensure proper state initialization + rospy.wait_for_message('/wamv/sensors/gps/gps/fix', NavSatFix) + rospy.wait_for_message('/wamv/sensors/imu/imu/data', Imu) # ROS rate - rate = rospy.Rate(20) + rate = rospy.Rate(5) # Recursive loop try: while not rospy.is_shutdown(): diff --git a/singaboat_vrx/src/singaboat_semantic_navigation.py b/singaboat_vrx/src/singaboat_semantic_navigation.py index ad676ad..6f64be3 100755 --- a/singaboat_vrx/src/singaboat_semantic_navigation.py +++ b/singaboat_vrx/src/singaboat_semantic_navigation.py @@ -3,12 +3,9 @@ import math import numpy import rospy -import tf -from std_msgs.msg import ColorRGBA -from visualization_msgs.msg import Marker from sensor_msgs.msg import NavSatFix, Imu from geographic_msgs.msg import GeoPath -from geometry_msgs.msg import Pose, Twist # !!! Pose.orientation NOT used as a quaternion !!! +from geometry_msgs.msg import Pose, Twist from dynamic_reconfigure.server import Server from singaboat_vrx.cfg import SemanticNavigationConfig from singaboat_vrx.control_module import PolarController @@ -29,16 +26,16 @@ class Animal: Defines animals and their attributes. ''' def __init__(self, name, pose, index): - self.name = name # Animal name - self.pose = pose # Animal pose - self.index = index # Animal index - self.action = ANIMAL_ACTION_DICT[self.name] # Action corresponding to a particular animal - self.radius_ideal = ANIMAL_IDEAL_RADIUS_DICT[self.name] # Ideal radius corresponding to a particular animal + self.name = name # Animal name + self.pose = pose # Animal pose + self.index = index # Animal index + self.action = ANIMAL_ACTION_DICT[self.name] # Action corresponding to a particular animal + self.radius_ideal = ANIMAL_IDEAL_RADIUS_DICT[self.name] # Ideal radius corresponding to a particular animal self.radius_critical = ANIMAL_CRITICAL_RADIUS_DICT[self.name] # Critical radius corresponding to a particular animal - self.radius_actual = self.radius_ideal # Actual radius corresponding to a particular animal + self.radius_actual = self.radius_ideal # Actual radius corresponding to a particular animal self.asv_approaching = True # Boolean flag to check whether the ASV is approaching a particular animal - self.asv_circling = False # Boolean flag to check whether the ASV is circling a particular animal - self.done = ANIMAL_STATUS_DICT[self.name] # Boolean flag to check whether a particular animal has been navigated semantically + self.asv_circling = False # Boolean flag to check whether the ASV is circling a particular animal + self.done = ANIMAL_STATUS_DICT[self.name] # Boolean flag to check whether a particular animal has been navigated semantically ################################################################################ @@ -48,15 +45,15 @@ class CircleTracker: ''' def __init__(self, yaw=0): self.cur_yaw = yaw - self.acc_yaw = 0 + self.accumulated_yaw = 0 def update(self, yaw): - self.acc_yaw += normalize_angle(yaw - self.cur_yaw) + self.accumulated_yaw += normalize_angle(yaw - self.cur_yaw) self.cur_yaw = yaw def reset(self, yaw): self.cur_yaw = yaw - self.acc_yaw = 0 + self.accumulated_yaw = 0 ################################################################################ @@ -70,43 +67,32 @@ def __init__(self): self.init_debug = True # Flag to enable/disable initial debug messages self.config = {} # Semantic navigation configuration # ROS infrastructure - self.tf_broadcaster = None - self.animal_model_viz_msg = Marker() - self.animal_label_viz_msg = Marker() - self.animal_model_viz_pub = None - self.animal_label_viz_pub = None - self.cmd_vel_msg = None - self.cmd_vel_pub = None - self.dyn_reconf_srv = None + self.cmd_vel_msg = None + self.cmd_vel_pub = None + self.dyn_reconf_srv = None def gps_callback(self, msg): if self.asv_pose.orientation.z is None: # If no yaw data is available, GPS offset cannot be compensated return lat = msg.latitude lon = msg.longitude - pos_x, pos_y, pos_z = gps_to_enu(lat, lon) + self.asv_pose.position.x, self.asv_pose.position.y, _ = gps_to_enu(lat, lon) # WAM-V frame is +0.85 m offset in local x-axis w.r.t. GPS frame - pos_x += self.gps_offset * math.cos(self.asv_pose.orientation.z) - pos_y += self.gps_offset * math.sin(self.asv_pose.orientation.z) - self.asv_pose.position.x = pos_x - self.asv_pose.position.y = pos_y - self.cur_position = numpy.array([pos_x, pos_y, pos_z]) + self.asv_pose.position.x += 0.85 * math.cos(self.asv_pose.orientation.z) + self.asv_pose.position.y += 0.85 * math.sin(self.asv_pose.orientation.z) def imu_callback(self, msg): self.asv_pose.orientation.z = quaternion_to_euler(msg.orientation)[2] - self.cur_rotation = msg.orientation def wildlife_callback(self, msg): for index, animal in enumerate(msg.poses): name = animal.header.frame_id lat = animal.pose.position.latitude lon = animal.pose.position.longitude - quat = animal.pose.orientation pose = Pose() # Update animal poses only after all animals are detected if self.animals_count: - pose.position.x, pose.position.y, pose.position.z = gps_to_enu(lat, lon) # Position of animals in Z axis is not considered for ASV control (used for visualization only) - pose.orientation = quat # Orientation of animals is not considered for ASV control (used for visualization only) + pose.position.x, pose.position.y, _ = gps_to_enu(lat, lon) # Heading of animals is not concerned self.animals[index].pose = pose # Replace with ENU pose else: self.animals.append(Animal(name, pose, index)) # Append animals to the list @@ -173,14 +159,14 @@ def safe_circling_radius(self, animal_index): self.animals[index].radius_actual = self.animals[index].radius_critical self.animals[animal_index].radius_actual = self.animals[animal_index].radius_ideal elif dist_circumvent - self.animals[animal_index].radius_critical < self.animals[index].radius_critical: - print("Animals to be circled are too close to each other!") print() + print("Animals to be circled are too close to each other!") self.animals[animal_index].radius_actual = self.animals[animal_index].radius_critical else: self.animals[animal_index].radius_actual = self.animals[animal_index].radius_critical if self.debug: - print("Actual radius of {} is {:.4} m".format(self.animals[animal_index].name, float(self.animals[animal_index].radius_actual))) print() + print("Actual radius of {} is {:.4} m".format(self.animals[animal_index].name, float(self.animals[animal_index].radius_actual))) def choose_waypoint(self, rho, alpha, radius, clockwise=True, animal_index=None): ''' @@ -209,8 +195,8 @@ def choose_waypoint(self, rho, alpha, radius, clockwise=True, animal_index=None) if rho > 9 and self.animals[animal_index].asv_approaching: # For platypus and turtle if self.animals[animal_index].action != "circumvent": - print("Approaching {}...".format(self.animals[animal_index].name)) print() + print("Approaching {}...".format(self.animals[animal_index].name)) if self.debug: print("rho: {:.4f} m, r_min: {:.4f} m, r_max: {:.4f} m".format(rho, r_min, r_max)) rho_new = math.sqrt(rho ** 2 - r_min ** 2) @@ -221,8 +207,8 @@ def choose_waypoint(self, rho, alpha, radius, clockwise=True, animal_index=None) alpha_new = alpha - math.asin(r_min / rho) # For crocodile else: - print("Circumventing {}...".format(self.animals[animal_index].name)) print() + print("Circumventing {}...".format(self.animals[animal_index].name)) if self.debug: print("rho: {:.4f} m, r_min: {:.4f} m, r_max: {:.4f} m".format(rho, r_min, r_max)) # If already away from the crocodile @@ -233,8 +219,8 @@ def choose_waypoint(self, rho, alpha, radius, clockwise=True, animal_index=None) alpha_new = alpha - math.asin(r_min / rho) # If getting close to the crocodile else: - print("Getting close to the crocodile, correcting course...") print() + print("Getting close to the crocodile, correcting course...") r_min = rho - 1 if clockwise: alpha_new = alpha + math.asin(r_min / rho) @@ -243,8 +229,8 @@ def choose_waypoint(self, rho, alpha, radius, clockwise=True, animal_index=None) # If ASV is close to the animal (for platypus and turtle) else: direction = "clockwise" if self.animals[animal_index].action == "circle_cw" else "counter-clockwise" - print("Circling {} in {} direction...".format(self.animals[animal_index].name, direction)) print() + print("Circling {} in {} direction...".format(self.animals[animal_index].name, direction)) # If ASV has just reached near the animal if self.animals[animal_index].asv_approaching: self.circle_tracker.reset(self.asv_pose.orientation.z) # Reset circle tracker to start accumulating yaw measurements @@ -253,8 +239,8 @@ def choose_waypoint(self, rho, alpha, radius, clockwise=True, animal_index=None) if rho > r_max: self.animals[animal_index].asv_circling = False # Reset the `asv_circling` flag self.animals[animal_index].asv_approaching = True # Set the `asv_approaching` flag (this also resets the circle tracker according to above logic) - print("Drove out of range while circling {}, attempting again...".format(self.animals[animal_index].name)) print() + print("Drove out of range while circling {}, attempting again...".format(self.animals[animal_index].name)) # If ASV is within range while circling the animal else: self.animals[animal_index].asv_circling = True # Set the `asv_circling` flag @@ -294,13 +280,13 @@ def circumvent_crocodile(self, orig_alpha, clockwise): circumvent_indices = [animal.index for animal in self.animals if animal.action == "circumvent"] # Indices of all crocodiles if len(circumvent_indices) == 0: # If there are no crocodiles if self.debug: - print("There are no crocodiles in the scene!") print() + print("There are no crocodiles in the scene!") return safe_alpha # No need to adjust alpha if len(circumvent_indices) >= 3: # If all (three) animals are crocodiles if self.debug: - print("All the animals in the scene are crocodiles!") print() + print("All the animals in the scene are crocodiles!") assert len(circumvent_indices) < 3 # Confirm that all (three) animals are not crocodiles multi_crocs_near_each_other = False # Boolean flag to check whether multiple (two) crocodiles are present near each other # If multiple (two) crocodiles are present, and their bounding circles are overlapping each other @@ -330,9 +316,9 @@ def circumvent_crocodile(self, orig_alpha, clockwise): if len(alpha_range_dict) == 0: return safe_alpha if self.debug: + print() print("Alpha Range Dictionary:") print(alpha_range_dict) - print() # If multiple (two) crocodiles are present, and their bounding circles are overlapping each other, the ASV cannot traverse between them. # Update `alpha_range_dict` if multi_crocs_near_each_other: @@ -346,8 +332,8 @@ def circumvent_crocodile(self, orig_alpha, clockwise): for key, value in alpha_range_dict.items(): if angle_within_half_plane_range(safe_alpha, value[0], value[1]): if self.debug: - print("Current alpha {:.4} is within {:.4} and {:.4}".format(safe_alpha, value[0], value[1])) print() + print("Current alpha {:.4} is within {:.4} and {:.4}".format(safe_alpha, value[0], value[1])) count += 1 if count == 2: safe_alpha = alpha_range_dict[nearest_rho_index][1] if clockwise else alpha_range_dict[nearest_rho_index][0] @@ -368,13 +354,13 @@ def circle_completed(self, circle_tracker, animal_index): :return circle_completed: Boolean flag indicating whether the ASV has finished circling a particular animal ''' if self.animals[animal_index].action == "circle_cw": # For platypus - return circle_tracker.acc_yaw < -2.0 * math.pi + return circle_tracker.accumulated_yaw < -2.0 * math.pi elif self.animals[animal_index].action == "circle_ccw": # For turtle - return circle_tracker.acc_yaw > 2.0 * math.pi + return circle_tracker.accumulated_yaw > 2.0 * math.pi else: # Circling progress for rocodile need not be checked if self.debug: - print("Animal index {} corresponds to a crocodile, whose circling progress need not be checked!".format(self.animals[animal_index])) print() + print("Animal index {} corresponds to a crocodile, whose circling progress need not be checked!".format(self.animals[animal_index])) return False def task_completed(self): @@ -391,8 +377,6 @@ def task_completed(self): return result def semantic_navigation(self): - # Broadcast transform of `wamv/base_link` frame w.r.t. `world` frame - self.tf_broadcaster.sendTransform((self.cur_position[0], self.cur_position[1], self.cur_position[2]), (self.cur_rotation.x, self.cur_rotation.y, self.cur_rotation.z, self.cur_rotation.w), rospy.Time.now(), 'wamv/base_link', 'world') # Initialize goal pose in polar coordinates rho_new, alpha_new, beta_new = 0, 0, 0 # Make sure ASV's pose is updated before proceeding @@ -434,58 +418,15 @@ def semantic_navigation(self): # Track circling progress if self.circle_completed(self.circle_tracker, next_index): # If target animal has been circled for complete 360° print("Circling Progress: ASV has finished circling the {}".format(self.animals[next_index].name)) - print() self.animals[next_index].done = True # Set the animal's `done` flag self.circle_tracker.reset(0) # Reset the circle tracker else: # If ASV is still circling the target animal - print("Circling Progress: ASV has circled the {} for {:.4}°".format(self.animals[next_index].name, abs(numpy.rad2deg(self.circle_tracker.acc_yaw)))) - print() + print("Circling Progress: ASV has circled the {} for {:.4}°".format(self.animals[next_index].name, abs(numpy.rad2deg(self.circle_tracker.accumulated_yaw)))) if self.debug: print("Next Local Polar Waypoint: {:.4}, {:.4}, {:.4}".format(rho_new, numpy.rad2deg(alpha_new), numpy.rad2deg(beta_new))) - print() # Generate and publish `cmd_vel` message - self.cmd_vel_msg = self.polar_controller.control(rho_new, alpha_new, beta_new) # Polar controller output + self.cmd_vel_msg = self.polar_controller.control(rho_new, alpha_new, beta_new) self.cmd_vel_pub.publish(self.cmd_vel_msg) - # Generate and publish `animal_model_viz` and `animal_label_viz` messages - for j in range(self.animals_count): - self.animal_model_viz_msg.header.frame_id = 'world' - self.animal_label_viz_msg.header.frame_id = 'world' - self.animal_model_viz_msg.header.stamp = rospy.Time.now() - self.animal_label_viz_msg.header.stamp = rospy.Time.now() - self.animal_model_viz_msg.ns = 'animals' - self.animal_label_viz_msg.ns = 'animals' - self.animal_model_viz_msg.action = 0 # 0 corresponds to add/modify object - self.animal_label_viz_msg.action = 0 # 0 corresponds to add/modify object - self.animal_model_viz_msg.type = 10 # 10 corresponds to mesh resource type - self.animal_label_viz_msg.type = 9 # 9 corresponds to text view facing type - if self.animals[j].name=='crocodile': - self.animal_model_viz_msg.id = 0 - self.animal_label_viz_msg.id = 0 - self.animal_model_viz_msg.mesh_resource = 'package://vrx_gazebo/models/crocodile_buoy/meshes/crocodile.dae' - self.animal_label_viz_msg.text = 'crocodile' - elif self.animals[j].name=='platypus': - self.animal_model_viz_msg.id = 1 - self.animal_label_viz_msg.id = 1 - self.animal_model_viz_msg.mesh_resource = 'package://vrx_gazebo/models/platypus_buoy/meshes/platypus.dae' - self.animal_label_viz_msg.text = 'platypus' - elif self.animals[j].name=='turtle': - self.animal_model_viz_msg.id = 2 - self.animal_label_viz_msg.id = 2 - self.animal_model_viz_msg.mesh_resource = 'package://vrx_gazebo/models/turtle_buoy/meshes/turtle.dae' - self.animal_label_viz_msg.text = 'turtle' - self.animal_model_viz_msg.mesh_use_embedded_materials = True - self.animal_model_viz_msg.pose = self.animals[j].pose - self.animal_label_viz_msg.pose.position.x = self.animals[j].pose.position.x - self.animal_label_viz_msg.pose.position.y = self.animals[j].pose.position.y - self.animal_label_viz_msg.pose.position.z = self.animals[j].pose.position.z - 0.4 - self.animal_label_viz_msg.pose.orientation = self.animals[j].pose.orientation - self.animal_model_viz_msg.scale.x = 1.0 - self.animal_model_viz_msg.scale.y = 1.0 - self.animal_model_viz_msg.scale.z = 1.0 - self.animal_label_viz_msg.scale.z = 0.4 - self.animal_label_viz_msg.color = ColorRGBA(1.0, 1.0, 1.0, 1.0) - self.animal_model_viz_pub.publish(self.animal_model_viz_msg) - self.animal_label_viz_pub.publish(self.animal_label_viz_msg) def config_callback(self, config, level): # Handle updated configuration values @@ -509,26 +450,20 @@ def config_callback(self, config, level): # Dynamic reconfigure server semantic_navigation_node.dyn_reconf_srv = Server(SemanticNavigationConfig, semantic_navigation_node.config_callback) - # Transform broadcaster - semantic_navigation_node.tf_broadcaster = tf.TransformBroadcaster() - # Subscribers rospy.Subscriber('/wamv/sensors/gps/gps/fix', NavSatFix, semantic_navigation_node.gps_callback) rospy.Subscriber('/wamv/sensors/imu/imu/data', Imu, semantic_navigation_node.imu_callback) rospy.Subscriber('/vrx/wildlife/animals/poses', GeoPath, semantic_navigation_node.wildlife_callback) - # Publishers - semantic_navigation_node.animal_model_viz_pub = rospy.Publisher('/rviz/animal_models', Marker, queue_size=10) - semantic_navigation_node.animal_label_viz_pub = rospy.Publisher('/rviz/animal_labels', Marker, queue_size=10) - semantic_navigation_node.cmd_vel_pub = rospy.Publisher('/wamv/cmd_vel', Twist, queue_size=10) + # Publisher + semantic_navigation_node.cmd_vel_pub = rospy.Publisher('/wamv/cmd_vel', Twist, queue_size=1) # Wait for valid messages to ensure proper state initialization rospy.wait_for_message('/wamv/sensors/gps/gps/fix', NavSatFix) rospy.wait_for_message('/wamv/sensors/imu/imu/data', Imu) - rospy.wait_for_message('/vrx/wildlife/animals/poses', GeoPath) # ROS rate - rate = rospy.Rate(20) + rate = rospy.Rate(5) try: while not rospy.is_shutdown(): diff --git a/singaboat_vrx/src/singaboat_station_keeping.py b/singaboat_vrx/src/singaboat_station_keeping.py index 08ecde9..9d85b60 100755 --- a/singaboat_vrx/src/singaboat_station_keeping.py +++ b/singaboat_vrx/src/singaboat_station_keeping.py @@ -3,71 +3,46 @@ import math import numpy import rospy -import tf -from sensor_msgs.msg import NavSatFix -from sensor_msgs.msg import Imu -from geographic_msgs.msg import GeoPoseStamped -from geometry_msgs.msg import Twist from dynamic_reconfigure.server import Server +from geometry_msgs.msg import Pose, Twist # !!! NOT USED AS PER THE STANDARDS !!! from singaboat_vrx.cfg import StationKeepingConfig from singaboat_vrx.control_module import PIDController -from singaboat_vrx.common_utilities import gps_to_enu, quaternion_to_euler, euler_to_quaternion, normalize_angle +from singaboat_vrx.common_utilities import normalize_angle ################################################################################ class StationKeeping: def __init__(self): # Initialize station-keeping - self.cur_pos = None # Current 2D position (x, y) - self.cur_rot = None # Current 1D orientation (yaw) - self.cur_position = None # Current 3D position (x, y, z) - self.cur_rotation = None # Current 3D orientation (roll, pitch, yaw) - self.cmd_pos = None # Commanded 2D position (x, y) - self.cmd_rot = None # Commanded 1D orientation (yaw) - self.cmd_position = None # Commanded 3D position (x, y, z) - self.cmd_rotation = None # Commanded 3D orientation (roll, pitch, yaw) - self.time = None # Current timestamp - self.config = {} # Station-keeping configuration + self.cmd_pos = None # Desired 2D position (x, y) + self.cmd_rot = None # Desired 1D orientation (yaw) + self.cur_pos = None # Current 2D position (x, y) + self.cur_rot = None # Current 1D orientation (yaw) + self.time = None # Current timestamp + self.config = {} # Station-keeping configuration # ROS infrastructure - self.tf_broadcaster = None self.cmd_vel_msg = None self.cmd_vel_pub = None self.dyn_reconf_srv = None - def gps_callback(self, msg): - if self.cur_rot is None: # If no yaw data is available, GPS offset cannot be compensated - return - lat = msg.latitude - lon = msg.longitude - pos_x, pos_y, pos_z = gps_to_enu(lat, lon) - # WAM-V frame is +0.85 m offset in local x-axis w.r.t. GPS frame - pos_x += self.gps_offset * math.cos(self.cur_rot[2]) - pos_y += self.gps_offset * math.sin(self.cur_rot[2]) - self.cur_pos = numpy.array([pos_x, pos_y]) - self.cur_position = numpy.array([pos_x, pos_y, pos_z]) - self.cur_rotation = euler_to_quaternion(self.cur_rot[0], self.cur_rot[1], self.cur_rot[2]) - - def imu_callback(self, msg): - self.cur_rot = quaternion_to_euler(msg.orientation) - - def goal_callback(self, msg): - # Transform goal pose from GPS to ENU frame of reference - lat = msg.pose.position.latitude - lon = msg.pose.position.longitude - pos_x, pos_y, pos_z = gps_to_enu(lat, lon) - self.cmd_rot = quaternion_to_euler(msg.pose.orientation) + def cmd_pose_callback(self, msg): + pos_x = msg.position.x + pos_y = msg.position.y + rot_z = msg.orientation.z self.cmd_pos = numpy.array([pos_x, pos_y]) - self.cmd_position = numpy.array([pos_x, pos_y, pos_z]) - self.cmd_rotation = euler_to_quaternion(self.cmd_rot[0], self.cmd_rot[1], self.cmd_rot[2]) + self.cmd_rot = rot_z + + def cur_pose_callback(self, msg): + pos_x = msg.position.x + pos_y = msg.position.y + rot_z = msg.orientation.z + self.cur_pos = numpy.array([pos_x, pos_y]) + self.cur_rot = rot_z def station_keeping(self): - # Broadcast transform of `wamv/base_link` frame w.r.t. `world` frame - self.tf_broadcaster.sendTransform((self.cur_position[0], self.cur_position[1], self.cur_position[2]), (self.cur_rotation[0], self.cur_rotation[1], self.cur_rotation[2], self.cur_rotation[3]), rospy.Time.now(), 'wamv/base_link', 'world') - # Broadcast transform of `goal` frame w.r.t. `world` frame - self.tf_broadcaster.sendTransform((self.cmd_position[0], self.cmd_position[1], self.cmd_position[2]), (self.cmd_rotation[0], self.cmd_rotation[1], self.cmd_rotation[2], self.cmd_rotation[3]), rospy.Time.now(), 'goal', 'world') # Control algorithm self.time = rospy.get_time() - if bool(self.config) and not [x for x in (self.cur_pos, self.cur_rot, self.cmd_pos, self.cmd_rot, self.time) if x is None]: + if bool(self.config) and not [x for x in (self.cmd_pos, self.cmd_rot, self.cur_pos, self.cur_rot, self.time) if x is None]: err_pos = self.cmd_pos - self.cur_pos # Error in position [x_des - x_cur, y_des - y_cur] if numpy.linalg.norm(err_pos) > self.goal_tol: # (Euclidean distance to goal as L2 norm) # If far from goal, head directly towards the goal by controlling Vx & Wz @@ -75,23 +50,24 @@ def station_keeping(self): if lin_vel_x > self.v_limit: # Clamp linear velocity along X-axis lin_vel_x = self.v_limit lin_vel_y = 0.0 - err_rot = normalize_angle(math.atan2(err_pos[1], err_pos[0]) - self.cur_rot[2]) # Error in orientation + err_rot = normalize_angle(math.atan2(err_pos[1], err_pos[0]) - self.cur_rot) # Error in orientation ang_vel_z = self.pid_g2g.control(err_rot, self.time) # PID controller for Wz else: # If near goal, perform fine adjustments in Vx, Vy & Wz for station-keeping - rot_tf = normalize_angle(math.atan2(err_pos[1], err_pos[0]) - self.cur_rot[2]) # G2G rotation transformation + rot_tf = normalize_angle(math.atan2(err_pos[1], err_pos[0]) - self.cur_rot) # G2G rotation transformation err_pos = numpy.array([numpy.linalg.norm(err_pos) * math.cos(rot_tf), numpy.linalg.norm(err_pos) * math.sin(rot_tf)]) # Error in position (in local frame) lin_vel_x = self.pid_sk_vx.control(err_pos[0], self.time) # PID controller for Vx lin_vel_y = self.pid_sk_vy.control(err_pos[1], self.time) # PID controller for Vy - err_rot = normalize_angle(self.cmd_rot[2] - self.cur_rot[2]) # Error in orientation + err_rot = normalize_angle(self.cmd_rot - self.cur_rot) # Error in orientation ang_vel_z = self.pid_sk_wz.control(err_rot, self.time) # PID controller for Wz - print("Station-Keeping Coordinates: {:.4} m, {:.4} m, {:.4} rad".format(self.cmd_pos[0], self.cmd_pos[1], self.cmd_rot[2])) + print("Error_Pos_X: %.4f m" % (err_pos[0])) + print("Error_Pos_Y: %.4f m" % (err_pos[1])) + print("Error_Rot_Z: %.4f rad" % (err_rot)) + print() + print("Lin_Vel_X: %.4f m/s" % (lin_vel_x)) + print("Lin_Vel_Y: %.4f m/s" % (lin_vel_y)) + print("Ang_Vel_Z: %.4f rad/s" % (ang_vel_z)) print() - if self.debug: - print("Error Pos X: {:.4} m".format(err_pos[0])) - print("Error Pos Y: {:.4} m".format(err_pos[1])) - print("Error Rot Z: {:.4} rad".format(err_rot)) - print() # Generate and publish `cmd_vel` message self.cmd_vel_msg.linear.x = lin_vel_x self.cmd_vel_msg.linear.y = lin_vel_y @@ -100,16 +76,14 @@ def station_keeping(self): def config_callback(self, config, level): # Handle updated configuration values - self.gps_offset = config['gps_offset'] # GPS offset w.r.t. WAM-V along X-axis - self.pid_g2g = PIDController(config['G2G_kP'], config['G2G_kI'], config['G2G_kD'], config['G2G_kS']) # Go-to-goal PID controller - self.pid_sk_vx = PIDController(config['SK_Vx_kP'], config['SK_Vx_kI'], config['SK_Vx_kD'], config['SK_Vx_kS']) # Station-keeping Vx PID controller - self.pid_sk_vy = PIDController(config['SK_Vy_kP'], config['SK_Vy_kI'], config['SK_Vy_kD'], config['SK_Vy_kS']) # Station-keeping Vy PID controller - self.pid_sk_wz = PIDController(config['SK_Wz_kP'], config['SK_Wz_kI'], config['SK_Wz_kD'], config['SK_Wz_kS']) # Station-keeping Wz PID controller - self.goal_tol = config['goal_tol'] # Goal tolerance dead-band of go-to-goal PID controller - self.v_const = config['v_const'] # Proportional gain for linear velocity outside goal tolerance - self.v_limit = config['v_limit'] # Saturation limit for linear velocity outside goal tolerance - self.debug = config['debug'] # Flag to enable/disable debug messages - self.config = config + self.pid_g2g = PIDController(config['G2G_kP'], config['G2G_kI'], config['G2G_kD'], config['G2G_kS']) # Go-to-goal PID controller + self.pid_sk_vx = PIDController(config['SK_Vx_kP'], config['SK_Vx_kI'], config['SK_Vx_kD'], config['SK_Vx_kS']) # Station-keeping Vx PID controller + self.pid_sk_vy = PIDController(config['SK_Vy_kP'], config['SK_Vy_kI'], config['SK_Vy_kD'], config['SK_Vy_kS']) # Station-keeping Vy PID controller + self.pid_sk_wz = PIDController(config['SK_Wz_kP'], config['SK_Wz_kI'], config['SK_Wz_kD'], config['SK_Wz_kS']) # Station-keeping Wz PID controller + self.goal_tol = config['goal_tol'] # Goal tolerance dead-band of go-to-goal PID controller + self.v_const = config['v_const'] # Proportional gain for linear velocity outside goal tolerance + self.v_limit = config['v_limit'] # Saturation limit for linear velocity outside goal tolerance + self.config = config return config ################################################################################ @@ -123,25 +97,16 @@ def config_callback(self, config, level): # Dynamic reconfigure server station_keeping_node.dyn_reconf_srv = Server(StationKeepingConfig, station_keeping_node.config_callback) - # Transform broadcaster - station_keeping_node.tf_broadcaster = tf.TransformBroadcaster() - # Message - station_keeping_node.cmd_vel_msg = Twist() + station_keeping_node.cmd_vel_msg = Twist() # Subscribers - rospy.Subscriber('/wamv/sensors/gps/gps/fix', NavSatFix, station_keeping_node.gps_callback) - rospy.Subscriber('/wamv/sensors/imu/imu/data', Imu, station_keeping_node.imu_callback) - rospy.Subscriber('/vrx/station_keeping/goal', GeoPoseStamped, station_keeping_node.goal_callback) + rospy.Subscriber('/wamv/cmd_pose', Pose, station_keeping_node.cmd_pose_callback) + rospy.Subscriber('/wamv/cur_pose', Pose, station_keeping_node.cur_pose_callback) # Publisher station_keeping_node.cmd_vel_pub = rospy.Publisher('/wamv/cmd_vel', Twist, queue_size = 10) - # Wait for valid messages to ensure proper state initialization - rospy.wait_for_message('/wamv/sensors/gps/gps/fix', NavSatFix) - rospy.wait_for_message('/wamv/sensors/imu/imu/data', Imu) - rospy.wait_for_message('/vrx/station_keeping/goal', GeoPoseStamped) - # ROS rate rate = rospy.Rate(20) diff --git a/singaboat_vrx/src/singaboat_task_manager.py b/singaboat_vrx/src/singaboat_task_manager.py index 937f7d5..7932648 100755 --- a/singaboat_vrx/src/singaboat_task_manager.py +++ b/singaboat_vrx/src/singaboat_task_manager.py @@ -35,8 +35,7 @@ def config_callback(config, level): dyn_reconf_srv = Server(TaskManagerConfig, config_callback) # Print team details - if dyn_reconf_srv.config['debug']: - print(info) + print(info) # Get path to singaboat_vrx ROS package rospack = rospkg.RosPack() @@ -45,68 +44,48 @@ def config_callback(config, level): # Launch task-specific launch file uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) roslaunch.configure_logging(uuid) - if dyn_reconf_srv.config['debug']: - print('\nWAITING FOR A VALID VRX TASK...') + print('\nWAITING FOR A VALID VRX TASK...') task_msg = rospy.wait_for_message('/vrx/task/info', Task) if task_msg.name == 'station_keeping': launch = roslaunch.parent.ROSLaunchParent(uuid, [pkg_path + dyn_reconf_srv.config['task_1_launch_path']]) - if dyn_reconf_srv.config['debug']: - print() - print('DETECTED VRX TASK: STATION-KEEPING') - print() - print('LAUNCHING SOLUTION FOR STATION-KEEPING TASK...') - print() + print() + print('LAUNCHING SOLUTION FOR STATION-KEEPING TASK...') + print() launch.start() elif task_msg.name == 'wayfinding': launch = roslaunch.parent.ROSLaunchParent(uuid, [pkg_path + dyn_reconf_srv.config['task_2_launch_path']]) - if dyn_reconf_srv.config['debug']: - print() - print('DETECTED VRX TASK: WAYFINDING') - print() - print('LAUNCHING SOLUTION FOR WAYFINDING TASK...') - print() + print() + print('LAUNCHING SOLUTION FOR WAYFINDING TASK...') + print() launch.start() elif task_msg.name == 'perception': launch = roslaunch.parent.ROSLaunchParent(uuid, [pkg_path + dyn_reconf_srv.config['task_3_launch_path']]) - if dyn_reconf_srv.config['debug']: - print() - print('DETECTED VRX TASK: SCENE PERCEPTION') - print() - print('LAUNCHING SOLUTION FOR SCENE PERCEPTION TASK...') - print() + print() + print('LAUNCHING SOLUTION FOR SCENE PERCEPTION TASK...') + print() launch.start() elif task_msg.name == 'wildlife': launch = roslaunch.parent.ROSLaunchParent(uuid, [pkg_path + dyn_reconf_srv.config['task_4_launch_path']]) - if dyn_reconf_srv.config['debug']: - print() - print('DETECTED VRX TASK: SEMANTIC NAVIGATION') - print() - print('LAUNCHING SOLUTION FOR SEMANTIC NAVIGATION TASK...') - print() + print() + print('LAUNCHING SOLUTION FOR SEMANTIC NAVIGATION TASK...') + print() launch.start() elif task_msg.name == 'gymkhana': launch = roslaunch.parent.ROSLaunchParent(uuid, [pkg_path + dyn_reconf_srv.config['task_5_launch_path']]) - if dyn_reconf_srv.config['debug']: - print() - print('DETECTED VRX TASK: GYMKHANA CHALLENGE') - print() - print('LAUNCHING SOLUTION FOR GYMKHANA CHALLENGE TASK...') - print() + print() + print('LAUNCHING SOLUTION FOR GYMKHANA CHALLENGE TASK...') + print() launch.start() elif task_msg.name == 'scan_dock_deliver': launch = roslaunch.parent.ROSLaunchParent(uuid, [pkg_path + dyn_reconf_srv.config['task_6_launch_path']]) - if dyn_reconf_srv.config['debug']: - print() - print('DETECTED VRX TASK: SCAN-DOCK-DELIVER') - print() - print('LAUNCHING SOLUTION FOR SCAN-DOCK-DELIVER TASK...') - print() + print() + print('LAUNCHING SOLUTION FOR SCAN-DOCK-DELIVER TASK...') + print() launch.start() else: - if dyn_reconf_srv.config['debug']: - print() - print('NO VALID VRX TASK DETECTED!') - print() + print() + print('NO VALID VRX TASK DETECTED!') + print() rospy.signal_shutdown('NO VALID VRX TASK DETECTED!') try: diff --git a/singaboat_vrx/src/singaboat_teleoperation.py b/singaboat_vrx/src/singaboat_teleoperation.py index a1de6e1..d116a82 100755 --- a/singaboat_vrx/src/singaboat_teleoperation.py +++ b/singaboat_vrx/src/singaboat_teleoperation.py @@ -15,7 +15,6 @@ ################################################################################ info = """ - ------------------------------------- SINGABOAT - WAM-V Teleoperation Panel ------------------------------------- @@ -34,7 +33,6 @@ NOTE: Press keys within this terminal ------------------------------------- - """ error = """ @@ -82,8 +80,7 @@ def config_callback(config, level): right_thrust_cmd = 0.0 try: - if dyn_reconf_srv.config['debug']: - print(info) + print(info) while(1): key = get_key() @@ -126,8 +123,7 @@ def config_callback(config, level): right_thruster_pub.publish(right_thrust_cmd) except: - if dyn_reconf_srv.config['debug']: - print(error) + print(error) finally: center_thrust_cmd = 0.0 diff --git a/singaboat_vrx/src/singaboat_vrx/__pycache__/common_utilities.cpython-38.pyc b/singaboat_vrx/src/singaboat_vrx/__pycache__/common_utilities.cpython-38.pyc index 3009d74..948befa 100644 Binary files a/singaboat_vrx/src/singaboat_vrx/__pycache__/common_utilities.cpython-38.pyc and b/singaboat_vrx/src/singaboat_vrx/__pycache__/common_utilities.cpython-38.pyc differ diff --git a/singaboat_vrx/src/singaboat_vrx/__pycache__/control_module.cpython-38.pyc b/singaboat_vrx/src/singaboat_vrx/__pycache__/control_module.cpython-38.pyc index dea29de..450d751 100644 Binary files a/singaboat_vrx/src/singaboat_vrx/__pycache__/control_module.cpython-38.pyc and b/singaboat_vrx/src/singaboat_vrx/__pycache__/control_module.cpython-38.pyc differ diff --git a/singaboat_vrx/src/singaboat_vrx/__pycache__/perception_module.cpython-38.pyc b/singaboat_vrx/src/singaboat_vrx/__pycache__/perception_module.cpython-38.pyc index c0f73cf..9dd071d 100644 Binary files a/singaboat_vrx/src/singaboat_vrx/__pycache__/perception_module.cpython-38.pyc and b/singaboat_vrx/src/singaboat_vrx/__pycache__/perception_module.cpython-38.pyc differ diff --git a/singaboat_vrx/src/singaboat_vrx/__pycache__/planning_module.cpython-38.pyc b/singaboat_vrx/src/singaboat_vrx/__pycache__/planning_module.cpython-38.pyc index 9d6d9f8..a392e9d 100644 Binary files a/singaboat_vrx/src/singaboat_vrx/__pycache__/planning_module.cpython-38.pyc and b/singaboat_vrx/src/singaboat_vrx/__pycache__/planning_module.cpython-38.pyc differ diff --git a/singaboat_vrx/src/singaboat_vrx/common_utilities.py b/singaboat_vrx/src/singaboat_vrx/common_utilities.py index a7cf392..f61e9c3 100755 --- a/singaboat_vrx/src/singaboat_vrx/common_utilities.py +++ b/singaboat_vrx/src/singaboat_vrx/common_utilities.py @@ -4,6 +4,7 @@ import numpy import pymap3d from tf.transformations import euler_from_quaternion, quaternion_from_euler +from geometry_msgs.msg import Twist ################################################################################ @@ -25,18 +26,6 @@ def constrain(input, low, high): output = input return output -def euclidean_distance(start_pose, goal_pose): - ''' - Compute the Euclidean distance between two points. - - :param start_pose: Start pose in cartesian coordinates (x, y, yaw) - :param goal_pose: Goal pose in cartesian coordinates (x, y, yaw) - - :return dist: Euclidean distance between two points in meters - ''' - dist = math.sqrt((start_pose.position.x - goal_pose.position.x) ** 2 + (start_pose.position.y - goal_pose.position.y) ** 2) - return dist - def normalize_angle(theta): ''' Normalize angle within [-pi, pi). @@ -70,37 +59,6 @@ def angle_within_half_plane_range(theta_check, theta_min, theta_max): result = ((cur_to_min > 0) ^ (cur_to_max > 0)) and ((cur_quad == start_quad) | (cur_quad == end_quad)) return result -def quadrant(theta): - ''' - Determine cartesian quadrant of a particular angle. - - :param theta: Angle whose cartesian quadrant is to be determined - - :return quadrant: Cartesian quadrant of the given angle - ''' - c = math.cos(theta) - s = math.sin(theta) - if (c > 0) ^ (s > 0): - quadrant = 2 if c < 0 else 4 - else: - quadrant = 1 if c > 0 else 3 - return quadrant - -def cartesian_to_polar(start_pose, goal_pose): - ''' - Convert cartesian pose coordinates to polar pose coordinates. - - :param start_pose: Start pose in cartesian coordinates (x, y, yaw) - :param goal_pose: Goal pose in cartesian coordinates (x, y, yaw) - - :return rho, alpha, beta: Polar coordinates describing distance to goal, start heading error, goal heading error - ''' - rho = math.sqrt((start_pose.position.x - goal_pose.position.x) ** 2 + (start_pose.position.y - goal_pose.position.y) ** 2) - theta = math.atan2(goal_pose.position.y - start_pose.position.y, goal_pose.position.x - start_pose.position.x) - alpha = normalize_angle(theta - start_pose.orientation.z) - beta = normalize_angle(normalize_angle(goal_pose.orientation.z - start_pose.orientation.z) - alpha) - return rho, alpha, beta - def quaternion_to_euler(quat): ''' Convert ROS Quaternion message to Euler angle representation (roll, pitch, yaw). @@ -222,4 +180,67 @@ def local_to_global_tf(x_ref, y_ref, theta_ref, x_loc, y_loc, theta_loc): x_global, y_global, theta_global = tgt_obj_glb_tf[0][0], tgt_obj_glb_tf[1][0], normalize_angle(theta_ref + theta_loc) return x_global, y_global, theta_global +def euclidean_distance(start_pose, goal_pose): + ''' + Compute the Euclidean distance between two points. + + :param start_pose: Start pose in cartesian coordinates (x, y, yaw) + :param goal_pose: Goal pose in cartesian coordinates (x, y, yaw) + + :return dist: Euclidean distance between two points in meters + ''' + dist = math.sqrt((start_pose.position.x - goal_pose.position.x) ** 2 + (start_pose.position.y - goal_pose.position.y) ** 2) + return dist + +def cartesian_to_polar(start_pose, goal_pose): + ''' + Convert cartesian pose coordinates to polar pose coordinates. + + :param start_pose: Start pose in cartesian coordinates (x, y, yaw) + :param goal_pose: Goal pose in cartesian coordinates (x, y, yaw) + + :return rho, alpha, beta: Polar coordinates describing distance to goal, start heading error, goal heading error + ''' + rho = math.sqrt((start_pose.position.x - goal_pose.position.x) ** 2 + (start_pose.position.y - goal_pose.position.y) ** 2) + theta = math.atan2(goal_pose.position.y - start_pose.position.y, goal_pose.position.x - start_pose.position.x) + alpha = normalize_angle(theta - start_pose.orientation.z) + beta = normalize_angle(normalize_angle(goal_pose.orientation.z - start_pose.orientation.z) - alpha) + return rho, alpha, beta + +def quadrant(theta): + ''' + Determine cartesian quadrant of a particular angle. + + :param theta: Angle whose cartesian quadrant is to be determined + + :return quadrant: Cartesian quadrant of the given angle + ''' + c = math.cos(theta) + s = math.sin(theta) + if (c > 0) ^ (s > 0): + quadrant = 2 if c < 0 else 4 + else: + quadrant = 1 if c > 0 else 3 + return quadrant + +################################################################################ + +def polar_controller(rho, alpha, beta, kr=0.1, ka=0.8, kb=-0.15, max_linear_x=1.0, max_angular_z=1.0): + ''' + The function that returns proportional controlled clipped cmd velocity + :param rho: distance (of line segment) between 2 points in meter + :param alpha: segment wrt start yaw in radian + :param beta: goal yaw wrt segment extension in radian + :param kr: P param for rho + :param ka: P param for alpha + :param kb: P param for beta + :return: P controlled cmd vel + :param max_linear_x: maximum allowed linear velocity in m/s + :param max_angular_z: maximum allowed angular velocity in rad/s + ''' + t = Twist() + t.linear.x = min(kr * rho, max_linear_x) + t.angular.z = min(ka * alpha + kb * beta, max_angular_z) + return t + ################################################################################ diff --git a/singaboat_vrx/src/singaboat_vrx/control_module.py b/singaboat_vrx/src/singaboat_vrx/control_module.py index 102b54e..1b0f412 100755 --- a/singaboat_vrx/src/singaboat_vrx/control_module.py +++ b/singaboat_vrx/src/singaboat_vrx/control_module.py @@ -11,15 +11,15 @@ class PIDController: accumulated error (integral action) and rate of change of error (derivative action). ''' def __init__(self, kP, kI, kD, kS): - self.kP = kP # Proportional gain - self.kI = kI # Integral gain - self.kD = kD # Derivative gain - self.kS = kS # Saturation constant (error history buffer size) - self.err_int = 0 # Error integral - self.err_dif = 0 # Error difference + self.kP = kP # Proportional gain + self.kI = kI # Integral gain + self.kD = kD # Derivative gain + self.kS = kS # Saturation constant (error history buffer size) + self.err_int = 0 # Error integral + self.err_dif = 0 # Error difference self.err_prev = 0 # Previous error self.err_hist = queue.Queue(self.kS) # Limited buffer of error history - self.t_prev = 0 # Previous time + self.t_prev = 0 # Previous time def control(self, err, t): ''' @@ -50,9 +50,9 @@ class PolarController: start heading error (alpha) and goal heading error (beta). ''' def __init__(self, k_rho, k_alpha, k_beta, max_lin_vel, max_ang_vel): - self.k_rho = k_rho # Proportional gain for rho - self.k_alpha = k_alpha # Proportional gain for alpha - self.k_beta = k_beta # Proportional gain for beta + self.k_rho = k_rho # Proportional gain for rho + self.k_alpha = k_alpha # Proportional gain for alpha + self.k_beta = k_beta # Proportional gain for beta self.max_lin_vel = max_lin_vel # Maximum allowed linear velocity in m/s self.max_ang_vel = max_ang_vel # Maximum allowed angular velocity in rad/s diff --git a/singaboat_vrx/src/singaboat_vrx/perception_module.py b/singaboat_vrx/src/singaboat_vrx/perception_module.py index 86ccdb8..aaa1f62 100755 --- a/singaboat_vrx/src/singaboat_vrx/perception_module.py +++ b/singaboat_vrx/src/singaboat_vrx/perception_module.py @@ -11,8 +11,9 @@ from itertools import combinations, groupby from copy import deepcopy import rospy -from geometry_msgs.msg import Pose # !!! Pose.orientation NOT used as a quaternion !!! -from sensor_msgs.msg import Image +from std_msgs.msg import Float64MultiArray +from geometry_msgs.msg import Pose # !!! NOT USED AS PER THE STANDARDS !!! +from sensor_msgs.msg import Image, PointCloud2 from vrx_gazebo.srv import ColorSequence from singaboat_vrx.common_utilities import gps_to_enu, enu_to_gps, quaternion_to_euler, euler_to_quaternion, local_to_global_tf @@ -35,13 +36,11 @@ def __init__(self): self.detected_obstacles = [] # List containing positions (in local frame) of detected obstacles self.config = {} # Buoy detector configuration # ROS infrastructure - self.cam_viz_msg = None - self.cam_viz_pub = None - self.object_msg = None - self.object_pub = None - self.obstacle_msg = None - self.obstacle_pub = None - self.dyn_reconf_srv = None + self.object_msg = None + self.object_pub = None + self.obstacle_msg = None + self.obstacle_pub = None + self.dyn_reconf_srv = None def task_callback(self, msg): self.task_name = msg.name @@ -77,16 +76,16 @@ def camera_callback(self, msg): # WEATHER DETECTION ALGORITHM #-----------------------------# # ROI for weather detection - img_roi_weather_detection = cv_image[520:720, 900:1180] + crop_img_weather_detection = cv_image[520:720, 900:1180] # Convert from RGB to HSV - hsv_img_weather_detection = cv2.cvtColor(img_roi_weather_detection, cv2.COLOR_BGR2HSV) + hsv_img_weather_detection = cv2.cvtColor(crop_img_weather_detection, cv2.COLOR_BGR2HSV) # Clolr thresholds for weather detection lower_gray = numpy.array([0, 0, 0]) upper_gray = numpy.array([100, 255, 255]) # Threshold gray color to get binary image mask_gray = cv2.inRange(hsv_img_weather_detection, lower_gray, upper_gray) # Filter out gray pixels - res_gray = cv2.bitwise_and(img_roi_weather_detection, img_roi_weather_detection, mask=mask_gray) + res_gray = cv2.bitwise_and(crop_img_weather_detection, crop_img_weather_detection, mask=mask_gray) # Compute weather score by averaging gray pixel values weather_score = res_gray[..., 2].mean() if weather_score > 15: @@ -101,10 +100,10 @@ def camera_callback(self, msg): txt = "Weather: " + weather txt_origin = (5, 15) txt_color = (0, 255, 0) - img_roi_weather_detection = cv2.putText(res_gray.copy(), txt, txt_origin, cv2.FONT_HERSHEY_SIMPLEX, 0.75, txt_color, 1, cv2.LINE_AA) + crop_img_weather_detection = cv2.putText(res_gray.copy(), txt, txt_origin, cv2.FONT_HERSHEY_SIMPLEX, 0.5, txt_color, 1, cv2.LINE_AA) # Display weather detection image if self.debug: - cv2.imshow("Weather Detection", img_roi_weather_detection) + cv2.imshow("Weather Detection", crop_img_weather_detection) cv2.waitKey(1) #----------------------------# # OBJECT DETECTION ALGORITHM @@ -228,7 +227,6 @@ def camera_callback(self, msg): cY = int(M["m01"] / M["m00"]) rgb_objects_tmp.append("mb_marker_buoy_black") # Append semantic label rgb_objects_tmp.append([cX, cY]) # Append object position (in image coordinates) - cv2.putText(crop_img_object_detection, "Black Marker Buoy", (cX+25, cY-25), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 0), 2, cv2.LINE_AA) # Add object label in cyan color else: # Round buoy is circular and has higher aspect ratio # Compute center of the contour M = cv2.moments(contours[i]) @@ -236,7 +234,6 @@ def camera_callback(self, msg): cY = int(M["m01"] / M["m00"]) rgb_objects_tmp.append("mb_round_buoy_black") # Append semantic label rgb_objects_tmp.append([cX, cY]) # Append object position (in image coordinates) - cv2.putText(crop_img_object_detection, "Black Round Buoy", (cX+25, cY-25), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 0), 2, cv2.LINE_AA) # Add object label in cyan color if self.debug: print() else: @@ -272,7 +269,6 @@ def camera_callback(self, msg): cY = int(M["m01"] / M["m00"]) rgb_objects_tmp.append("mb_marker_buoy_red") # Append semantic label rgb_objects_tmp.append([cX,cY]) # Append object position (in image coordinates) - cv2.putText(crop_img_object_detection, "Red Marker Buoy", (cX+25, cY-25), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 0), 2, cv2.LINE_AA) # Add object label in cyan color if self.debug: print() else: @@ -308,7 +304,6 @@ def camera_callback(self, msg): cY = int(M["m01"] / M["m00"]) rgb_objects_tmp.append("mb_round_buoy_orange") # Append semantic label rgb_objects_tmp.append([cX, cY]) # Append object position (in image coordinates) - cv2.putText(crop_img_object_detection, "Orange Round Buoy", (cX+25, cY-25), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 0), 2, cv2.LINE_AA) # Add object label in cyan color if self.debug: print() else: @@ -344,7 +339,6 @@ def camera_callback(self, msg): cY = int(M["m01"] / M["m00"]) rgb_objects_tmp.append("mb_marker_buoy_green") # Append semantic label rgb_objects_tmp.append([cX, cY]) # Append object position (in image coordinates) - cv2.putText(crop_img_object_detection, "Green Marker Buoy", (cX+25, cY-25), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 0), 2, cv2.LINE_AA) # Add object label in cyan color if self.debug: print() else: @@ -381,7 +375,6 @@ def camera_callback(self, msg): if cY > 20: # Disregard false detections due to sky rgb_objects_tmp.append("mb_marker_buoy_white") # Append semantic label rgb_objects_tmp.append([cX, cY]) # Append object position (in image coordinates) - cv2.putText(crop_img_object_detection, "White Marker Buoy", (cX+25, cY-25), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (255, 255, 0), 2, cv2.LINE_AA) # Add object label in cyan color if self.debug: print() else: @@ -392,12 +385,6 @@ def camera_callback(self, msg): if self.debug: cv2.imshow("Object Detection", crop_img_object_detection) cv2.waitKey(1) - # Generate and publish `cam_viz` message - try: # Try converting OpenCV image to ROS Image message - self.cam_viz_msg = self.cv_bridge.cv2_to_imgmsg(crop_img_object_detection, encoding="bgr8") - except CvBridgeError as error: - print(error) - self.cam_viz_pub.publish(self.cam_viz_msg) # Update `rgb_objects` list rgb_objects = rgb_objects_tmp if self.task_name == "perception": @@ -663,7 +650,7 @@ def config_callback(self, config, level): # Handle updated configuration values self.gps_offset = config['gps_offset'] # GPS offset w.r.t. WAM-V along X-axis self.lidar_offset = config['lidar_offset'] # LIDAR offset w.r.t. WAM-V along X-axis - self.debug = config['debug'] # Flag to enable/disable debug messages + self.debug = config['buoy_detector_debug'] # Flag to enable/disable buoy detector debug messages self.config = config return config @@ -671,62 +658,62 @@ def config_callback(self, config, level): class GateDetector(): ''' - Detects (classifies and localizes) channel gates constituted of different + Detects (classifies and localizes) gymkhana channel gates constituted of different marker buoys by making use of `BuoyDetector`. ''' def __init__(self): # Initialize gate detector - self.buoy_detector = BuoyDetector() # BuoyDetector class instance - self.asv_pose = Pose() # Current ASV pose in global frame + self.buoy_detector = BuoyDetector() # BuoyDetector class instance + self.asv_pose = Pose() # Current ASV pose in global frame self.detected_objects = [] # List of currently detected objects - self.channel_buoys = ["mb_marker_buoy_red", "mb_marker_buoy_white", "mb_marker_buoy_green", "mb_marker_buoy_black"] # Reference list of channel defining buoys + self.channel_buoys = ["mb_marker_buoy_red", "mb_marker_buoy_white", "mb_marker_buoy_green", "mb_marker_buoy_black"] # Reference list of channel defining buoys # Red buoy variables - self.red_buoy_pos = [] # Position (x, y) of red buoy w.r.t. WAM-V frame + self.red_buoy_pos = [] # Position (x, y) of red buoy w.r.t. WAM-V frame self.red_buoy_detected = False # Boolean flag to check whether a red buoy has been detected # White buoy variables - self.white_buoy_pos = [] # Position (x, y) of white buoy w.r.t. WAM-V frame - self.white_buoy_enu = [] # Position (x, y) of white buoy w.r.t. global frame + self.white_buoy_pos = [] # Position (x, y) of white buoy w.r.t. WAM-V frame + self.white_buoy_enu = [] # Position (x, y) of white buoy w.r.t. global frame self.white_buoy_detected = False # Boolean flag to check whether a white buoy has been detected # Green buoy variables - self.green_buoy_pos = [] # Position (x, y) of currently detected green buoy w.r.t. WAM-V frame - self.green_buoy_enu = [] # Position (x, y) of currently detected green buoy w.r.t. global frame - self.green_buoy_1_pos = [] # Position (x, y) of green buoy 1 w.r.t. WAM-V frame - self.green_buoy_1_enu = [] # Position (x, y) of green buoy 1 w.r.t. global frame - self.green_buoy_2_pos = [] # Position (x, y) of green buoy 2 w.r.t. WAM-V frame - self.green_buoy_2_enu = [] # Position (x, y) of green buoy 2 w.r.t. global frame - self.green_buoy_3_pos = [] # Position (x, y) of green buoy 3 w.r.t. WAM-V frame - self.green_buoy_3_enu = [] # Position (x, y) of green buoy 3 w.r.t. global frame + self.green_buoy_pos = [] # Position (x, y) of currently detected green buoy w.r.t. WAM-V frame + self.green_buoy_enu = [] # Position (x, y) of currently detected green buoy w.r.t. global frame + self.green_buoy_1_pos = [] # Position (x, y) of green buoy 1 w.r.t. WAM-V frame + self.green_buoy_1_enu = [] # Position (x, y) of green buoy 1 w.r.t. global frame + self.green_buoy_2_pos = [] # Position (x, y) of green buoy 2 w.r.t. WAM-V frame + self.green_buoy_2_enu = [] # Position (x, y) of green buoy 2 w.r.t. global frame + self.green_buoy_3_pos = [] # Position (x, y) of green buoy 3 w.r.t. WAM-V frame + self.green_buoy_3_enu = [] # Position (x, y) of green buoy 3 w.r.t. global frame self.green_buoy_detected = False # Boolean flag to check whether a green buoy has been detected # Black buoy variables - self.black_buoy_pos = [] # Position (x, y) of black buoy w.r.t. WAM-V frame - self.black_buoy_enu = [] # Position (x, y) of black buoy w.r.t. global frame + self.black_buoy_pos = [] # Position (x, y) of black buoy w.r.t. WAM-V frame + self.black_buoy_enu = [] # Position (x, y) of black buoy w.r.t. global frame self.black_buoy_detected = False # Boolean flag to check whether a black buoy has been detected # Entry gate variables - self.entrance_position = [] + self.entrance_position = [] self.entrance_orientation = None - self.entrance_detected = False - self.entrance_reported = False + self.entrance_detected = False + self.entrance_reported= False # Middle gate variables - self.gate_position = [] - self.gate_1_position = [] + self.gate_position = [] + self.gate_1_position = [] self.gate_1_orientation = None - self.gate_1_detected = False - self.gate_1_reported = False - self.gate_2_position = [] + self.gate_1_detected = False + self.gate_1_reported = False + self.gate_2_position = [] self.gate_2_orientation = None - self.gate_2_detected = False - self.gate_2_reported = False - self.gate_3_position = [] + self.gate_2_detected = False + self.gate_2_reported = False + self.gate_3_position = [] self.gate_3_orientation = None - self.gate_3_detected = False - self.gate_3_reported = False + self.gate_3_detected = False + self.gate_3_reported = False # Exit gate variables - self.exit_position = [] + self.exit_position = [] self.exit_orientation = None - self.exit_detected = False - self.exit_reported = False + self.exit_detected = False + self.exit_reported = False # ROS infrastructure - self.config = {} # Gate detector configuration + self.config = {} # Gate detector configuration self.dyn_reconf_srv = None # Dynamic reconfigure server def gps_callback(self, msg): @@ -1005,14 +992,14 @@ def config_callback(self, config, level): # Buoy detector parameters self.buoy_detector.gps_offset = config['gps_offset'] # GPS offset w.r.t. WAM-V along X-axis self.buoy_detector.lidar_offset = config['lidar_offset'] # LIDAR offset w.r.t. WAM-V along X-axis - self.buoy_detector.debug = config['debug'] # Flag to enable/disable buoy detector debug messages + self.buoy_detector.debug = config['buoy_detector_debug'] # Flag to enable/disable buoy detector debug messages # Gate detector parameters self.min_gate_width = config['min_gate_width'] # Minimum width of channel gate self.max_gate_width = config['max_gate_width'] # Maximum width of channel gate self.dist_to_gate = config['dist_to_gate'] # Distance between the detected gate and WAM-V frame self.gps_offset = config['gps_offset'] # GPS offset w.r.t. WAM-V along X-axis self.lidar_offset = config['lidar_offset'] # LIDAR offset w.r.t. WAM-V along X-axis - self.debug = config['debug'] # Flag to enable/disable gate detector debug messages + self.debug = config['gate_detector_debug'] # Flag to enable/disable gate detector debug messages self.config = config return config @@ -1024,24 +1011,24 @@ class ShapeDetector: --------------- Contour Shapes: --------------- - Unknown : If the shape has less than 3 vertices - Triangle : If the shape has 3 vertices - Square : If the shape has 4 vertices and approximately 1:1 aspect ratio - Horizontal Rectangle : If the shape has 4 vertices and >1 aspect ratio - Vertical Rectangle : If the shape has 4 vertices and <1 aspect ratio - Cross : If the shape has 12 vertices - Circle : Otherwise + unknown = Not defined (if the shape has less than 3 vertices) + triangle = Triangle (if the shape has 3 vertices) + square = Square (if the shape has 4 vertices and 1:1 aspect ratio) + rectangle_horizontal = Horizontal rectangle (if the shape has 4 vertices and >1 aspect ratio) + rectangle_vertical = Vertical rectangle (if the shape has 4 vertices and <1 aspect ratio) + cross = Cross (if the shape has 12 vertices) + circle = Circle (otherwise) ''' def __init__(self): pass - def detect(self, contour, eps=0.05, ar_tol=0.25): + def detect(self, contour, eps=0.05, ratio=0.5): ''' Detect approximated shape of a contour. :param contour: OpenCV `contour` object :param eps : Maximum distance between the original curve and its approximation - :param ar_tol : Tolerance of aspect ratio (width/height) to differentiate between square and rectangle + :param ratio : Tolerance of aspect ratio (width/height) to differentiate between square and rectangle :return shape: Approximated shape of a contour ''' @@ -1049,76 +1036,76 @@ def detect(self, contour, eps=0.05, ar_tol=0.25): approx = cv2.approxPolyDP(contour, eps * perimeter, True) # Approximation of shape of the contour # If the shape has less than 3 vertices, it is probably a line or a point if len(approx) <= 2: - shape = "Unknown" + shape = "unknown" # If the shape has 3 vertices, it is a triangle elif len(approx) == 3: - shape = "Triangle" + shape = "triangle" # If the shape has 4 vertices, it is either a square or a rectangle elif len(approx) == 4: (x, y, w, h) = cv2.boundingRect(approx) # Fit a bounding box to the approximated shape of the contour aspect_ratio = w / float(h) # Calculate aspect ratio of the bounding box # If the shape has aspect ratio that is approximately equal to 1, it is a square - if (1-ar_tol) <= aspect_ratio <= (1+ar_tol): - shape = "Square" + if (1-ratio) <= aspect_ratio <= (1+ratio): + shape = "square" # If the shape has aspect ratio that is more than 1, it is a horizontal rectangle - elif aspect_ratio > (1+ar_tol): - shape = "Horizontal Rectangle" + elif aspect_ratio > (1+ratio): + shape = "rectangle_horizontal" # If the shape has aspect ratio that is less than 1, it is a vertical rectangle else: - shape = "Vertical Rectangle" + shape = "rectangle_vertical" # If the shape has 12 vertices, it is a cross elif len(approx) == 12: - shape = "Cross" + shape = "cross" # Otherwise, the shape is assumed to be a circle else: - shape = "Circle" + shape = "circle" return shape ################################################################################ # Reference dictionaries storing HSV thresholding constants (1 to 3 means from brightest to darkest) -HSV_DICT_1 = {"Red": {"lower": [0, 180, 146], "upper": [0, 255, 255]}, - "Orange": {"lower": [0, 206, 50], "upper": [71, 255, 255]}, - "Black": {"lower": [0, 0, 0], "upper": [0, 0, 22]}, - "Green": {"lower": [55, 100, 101], "upper": [90, 255, 255]}, - "White": {"lower": [0, 0, 67], "upper": [51, 0, 151]}, - "Gray": {"lower": [0, 0, 0], "upper": [100, 255, 255]}, - "Blue": {"lower": [103, 84, 222], "upper": [219, 255, 255]}, - "Yellow": {"lower": [6, 84, 208], "upper": [30, 255, 255]}} -HSV_DICT_2 = {"Red": {"lower": [0, 201, 0], "upper": [19, 255, 255]}, - "Orange": {"lower": [0, 206, 50], "upper": [71, 255, 255]}, - "Black": {"lower": [0, 0, 0], "upper": [0, 0, 22]}, - "Green": {"lower": [55, 108, 93], "upper": [98, 255, 161]}, - "White": {"lower": [0, 0, 67], "upper": [51, 0, 151]}, - "Gray": {"lower": [0, 0, 0], "upper": [100, 255, 255]}, - "Blue": {"lower": [115, 84, 22], "upper": [219, 255, 255]}, - "Yellow": {"lower": [6, 217, 119], "upper": [46, 255, 139]}} -HSV_DICT_3 = {"Red": {"lower": [0, 201, 0], "upper": [19, 255, 255]}, - "Orange": {"lower": [0, 206, 50], "upper": [71, 255, 255]}, - "Black": {"lower": [0, 0, 0], "upper": [0, 0, 22]}, - "Green": {"lower": [55, 204, 29], "upper": [99, 255, 146]}, - "White": {"lower": [0, 0, 67], "upper": [51, 0, 151]}, - "Gray": {"lower": [0, 0, 0], "upper": [100, 255, 255]}, - "Blue": {"lower": [115, 39, 45], "upper": [129, 255, 255]}, - "Yellow": {"lower": [6, 217, 16], "upper": [46, 255, 150]}} +HSV_DICT_1 = {"red": {"lower": [0, 180, 146], "upper": [0, 255, 255]}, + "orange": {"lower": [0, 206, 50], "upper": [71, 255, 255]}, + "black": {"lower": [0, 0, 0], "upper": [0, 0, 22]}, + "green": {"lower": [55, 100, 101], "upper": [90, 255, 255]}, + "white": {"lower": [0, 0, 67], "upper": [51, 0, 151]}, + "grey": {"lower": [0, 0, 0], "upper": [100, 255, 255]}, + "blue": {"lower": [103, 84, 222], "upper": [219, 255, 255]}, + "yellow": {"lower": [6, 84, 208], "upper": [30, 255, 255]}} +HSV_DICT_2 = {"red": {"lower": [0, 201, 0], "upper": [19, 255, 255]}, + "orange": {"lower": [0, 206, 50], "upper": [71, 255, 255]}, + "black": {"lower": [0, 0, 0], "upper": [0, 0, 22]}, + "green": {"lower": [55, 108, 93], "upper": [98, 255, 161]}, + "white": {"lower": [0, 0, 67], "upper": [51, 0, 151]}, + "grey": {"lower": [0, 0, 0], "upper": [100, 255, 255]}, + "blue": {"lower": [115, 84, 22], "upper": [219, 255, 255]}, + "yellow": {"lower": [6, 217, 119], "upper": [46, 255, 139]}} +HSV_DICT_3 = {"red": {"lower": [0, 201, 0], "upper": [19, 255, 255]}, + "orange": {"lower": [0, 206, 50], "upper": [71, 255, 255]}, + "black": {"lower": [0, 0, 0], "upper": [0, 0, 22]}, + "green": {"lower": [55, 204, 29], "upper": [99, 255, 146]}, + "white": {"lower": [0, 0, 67], "upper": [51, 0, 151]}, + "grey": {"lower": [0, 0, 0], "upper": [100, 255, 255]}, + "blue": {"lower": [115, 39, 45], "upper": [129, 255, 255]}, + "yellow": {"lower": [6, 217, 16], "upper": [46, 255, 150]}} # Reference dictionary storing blob area thresholding constants -MIN_BLOB_AREA_DICT = {"Red": 8000, - "Orange": 15000, - "Black": 15000, - "Green": 15000, - "White": 15000, - "Gray": 15000, - "Blue": 15000, - "Yellow": 10000} +MIN_BLOB_AREA_DICT = {"red": 8000, + "orange": 15000, + "black": 15000, + "green": 15000, + "white": 15000, + "grey": 15000, + "blue": 15000, + "yellow": 10000} # Reference dictionary storing contour area thresholding constants -MIN_CONTOUR_AREA_DICT = {"Red": 10, - "Orange": 10, - "Black": 10, - "Green": 10, - "White": 10, - "Gray": 10, - "Blue": 10, - "Yellow": 3} +MIN_CONTOUR_AREA_DICT = {"red": 10, + "orange": 10, + "black": 10, + "green": 10, + "white": 10, + "grey": 10, + "blue": 10, + "yellow": 3} ################################################################################ @@ -1126,24 +1113,21 @@ class ColoredRegionDetector: ''' Detects colored regions within an image by making use of `ShapeDetector`. ''' - def __init__(self, colors, debug=False): - self.shape_detector = ShapeDetector() # ShapeDetector class instance - self.cv_bridge = CvBridge() # CvBridge object - self.colors = colors # List of interested colors to be filtered from image - self.masks = {} # Masks for each color - self.contours = {} # Contours of each color - self.areas = {} # Areas of contours of each color - self.centroids = {} # Centroids of blobs of each color - self.shapes = {} # Shapes of contours of each color - self.image = None # Current camera frame - self.img_roi = None # Cropped current camera frame based on region of interest (ROI) - self.image_weather = None # Image for weather detection - self.img_roi_weather = None # Cropped image for weather detection based on region of interest (ROI) - self.record_time = 0 # Count record time - self.brightness = [0, 0, 0, 0, 0] # Record the brightness - self.debug = debug # Flag to enable/disable colored region detector debug messages - # ROS infrastructure - self.cam_viz_pub = rospy.Publisher('/rviz/cam_viz', Image, queue_size=10) + def __init__(self, colors): + self.colors = colors # List of interested colors to be filtered from image + self.masks = {} # Masks for each color + self.contours = {} # Contours of each color + self.areas = {} # Areas of contours of each color + self.centroids = {} # Centroids of blobs of each color + self.shapes = {} # Shapes of contours of each color + self.image = None # Image of current frame + self.img_roi = None # Cropped image of current frame + self.image_weather = None # Image for weather detection + self.crop_img_weather = None # Cropped image for weather detection + self.record_time = 0 # Count record time + self.brightness = [0, 0, 0, 0, 0] # Record the brightness + self.shape_detector = ShapeDetector() # ShapeDetector class instance + self.debug = False # Flag to enable/disable colored region detector debug messages | TODO: move to config def reset(self): ''' @@ -1180,10 +1164,10 @@ def hsv_dict(self): # Record brightness of every instant (used for averaging) i = int(self.record_time / record_cycle) - 1 self.brightness[i] = weather_score - # Use real-time brightness before averaging data is ready + # Use real-time brightness, before averaging data is ready avg_brightness = weather_score else: - # Use average brightness once averaging data is ready + # Use average brightness, once averaging data is ready avg_brightness = numpy.mean(self.brightness) # Display weather detection image if self.debug: @@ -1191,8 +1175,8 @@ def hsv_dict(self): txt_brightness_avg = "Average Brightness: " + str(round(avg_brightness, 2)) txt_origin = (5, 15) txt_color = (0, 255, 0) - self.img_roi_weather = cv2.putText(img_weather.copy(), txt_brightness_avg, txt_origin, cv2.FONT_HERSHEY_SIMPLEX, 0.75, txt_color, 1, cv2.LINE_AA) - cv2.imshow("Weather Detection", self.img_roi_weather) + self.crop_img_weather = cv2.putText(img_weather.copy(), txt_brightness_avg, txt_origin, cv2.FONT_HERSHEY_SIMPLEX, 0.5, txt_color, 1, cv2.LINE_AA) + cv2.imshow("Weather Detection", self.crop_img_weather) # Choose HSV thresholding dictionary based on brightness if avg_brightness > 55: hsv_dict = HSV_DICT_1 @@ -1206,7 +1190,7 @@ def detect(self, image=None, weather_image=None): ''' Detect colored regions within an image. - :param image : Image for colored region detection + :param image : Image in which colored regions are to be detected :param weather_image: Reference image for weather detection :return colored_regions: Colored regions within the image @@ -1220,7 +1204,7 @@ def detect(self, image=None, weather_image=None): # Crop ROI self.img_roi = self.image[0:535, 0:] hsv_img = cv2.cvtColor(self.img_roi, cv2.COLOR_BGR2HSV) - # Choose HSV thresholding dictionary based on brightness + # Fetch Choose HSV thresholding dictionary based on brightness hsv_dict = self.hsv_dict() # Update mask and contours for each color for color in self.colors: @@ -1228,7 +1212,7 @@ def detect(self, image=None, weather_image=None): self.masks[color] = cv2.inRange(hsv_img, numpy.array(hsv_dict[color]["lower"]), numpy.array(hsv_dict[color]["upper"])) area = cv2.moments(self.masks[color], False)['m00'] if self.debug: - print("Color: {}\tArea: {:.2f}".format(color, area)) + print("Color: {},\tArea: {:.2f}".format(color, area)) print() if area > MIN_BLOB_AREA_DICT[color]: # Reduce noise @@ -1254,7 +1238,7 @@ def detect(self, image=None, weather_image=None): self.contours[color] = [contour] self.areas[color] = [area] # Update color to shapes map - shape = self.shape_detector.detect(contour, eps=0.03, ar_tol=0.25) + shape = self.shape_detector.detect(contour, eps=0.03, ratio=0.4) if color in self.shapes: self.shapes[color].append(shape) else: @@ -1275,8 +1259,9 @@ def detect(self, image=None, weather_image=None): if self.debug: print("Unrecognized Color: {}".format(color)) print() - colored_regions = self.color_to_centroid(drop_shapes=["Horizontal Rectangle"]) # Drop detections pertaining to horizontal rectangles - self.display_detections() # Display colored region detections (color contours labelled with shape text) + colored_regions = self.color_to_centroid(drop_shapes=["rectangle_horizontal"]) # Drop detections pertaining to horizontal rectangles + if self.debug: + self.display_detections() # Display colored region detections (color contours labelled with shape text) return colored_regions def color_to_centroid(self, drop_shapes=None): @@ -1287,7 +1272,7 @@ def color_to_centroid(self, drop_shapes=None): :return output: Formatted color-to-centroid detections ''' - drop = ["Unknown"] + drop = ["unknown"] if drop_shapes is not None: drop.extend(drop_shapes) assert len(self.contours) == len(self.shapes) @@ -1314,7 +1299,7 @@ def color_to_shape(self, drop_shapes=None): :return output: Formatted color-to-shape detections ''' - drop = ["Unknown"] + drop = ["unknown"] if drop_shapes is not None: drop.extend(drop_shapes) if self.debug: @@ -1345,7 +1330,7 @@ def color_shape_to_centroid(self): assert color in self.centroids assert len(shapes) == len(self.centroids[color]) for shape, centroid in zip(shapes, self.centroids[color]): - output[(color, shape)] = centroid # Assumes that no two objects have the same color and shape + output[(color, shape)] = centroid # Assumes that no objects have the same color and shape return output def display_detections(self): @@ -1362,45 +1347,34 @@ def display_detections(self): for color, contour in self.contours.items(): contours.extend(contour) for i in range(len(contour)): - # Add detection color and shape text to the image - txt_detection_color = color - if str(self.shapes[color][i]) == "Vertical Rectangle": - txt_detection_shape = "Light Buoy Display" - elif str(self.shapes[color][i]) == "Horizontal Rectangle": - txt_detection_shape = "Rectangle" - else: - txt_detection_shape = str(self.shapes[color][i]) + # Add shape text to the image + txt_shape = str(self.shapes[color][i]) txt_color = (255, 255, 0) # Cyan color - cv2.putText(contour_image, txt_detection_color, (self.centroids[color][i][0], self.centroids[color][i][1]-35), cv2.FONT_HERSHEY_SIMPLEX, 0.75, txt_color, 2, cv2.LINE_AA) - cv2.putText(contour_image, txt_detection_shape, (self.centroids[color][i][0], self.centroids[color][i][1]-20), cv2.FONT_HERSHEY_SIMPLEX, 0.75, txt_color, 2, cv2.LINE_AA) + cv2.putText(contour_image, txt_shape, (self.centroids[color][i][0], self.centroids[color][i][1]-20), cv2.FONT_HERSHEY_SIMPLEX, 0.5, txt_color, 2, cv2.LINE_AA) cv2.drawContours(contour_image, contours, -1, (255, 255, 0), 2) # Draw the contours in cyan color - if self.debug: - # Display colored region detection image - cv2.imshow("Colored Region Detection", contour_image) - cv2.waitKey(1) - # Generate and publish `cam_viz` message - try: # Try converting OpenCV image to ROS Image message - cam_viz_msg = self.cv_bridge.cv2_to_imgmsg(contour_image, encoding="bgr8") - except CvBridgeError as error: - print(error) - self.cam_viz_pub.publish(cam_viz_msg) + # Display contour image + cv2.imshow("Colored Region Detection", contour_image) + cv2.waitKey(1) ################################################################################ class ColorSequenceDetector: ''' - Detects and decodes light buoy color sequence by making use of `ColoredRegionDetector`. + Detects color sequence displayed by the light buoy. ''' - def __init__(self, colors, timeout=30, debug=False): - self.colored_region_detector = ColoredRegionDetector(colors=colors, debug=debug) # ColoredRegionDetector class instance - self.cv_bridge = CvBridge() # CvBridge object - self.image = None # Current camera frame - self.color_sequence = [] # List to store the color sequence - self.black_index = 0 # Index of black color in the color sequence (used to correct the order of color sequence) - self.init_time = rospy.get_time() # Initial timestamp (time when an object of this class is instantiated) - self.last_color_time = rospy.get_time() # Timestamp when last color was detected - self.timeout = timeout # Color sequence detector timeout in seconds - self.debug = debug # Flag to enable/disable color sequence detector debug messages + def __init__(self, colors, timeout=30): + self.colored_region_detector = ColoredRegionDetector(colors=colors) + self.color_sequence = [] + self.last_color_time = rospy.get_time() + self.timeout = timeout + self.init_time = rospy.get_time() + self.black_index = 0 + self.buoy_centroid = None + self.cv_bridge = CvBridge() + self.image = None + self.debug = False # Flag to enable/disable color sequence detector debug messages | TODO: move to config + + rospy.Subscriber('/wamv/sensors/cameras/camera/image_raw', Image, self.camera_callback, queue_size=1) # TODO: move away from here def camera_callback(self, msg): # Try converting ROS Image message to OpenCV image @@ -1412,51 +1386,59 @@ def camera_callback(self, msg): def detect(self): ''' - Detect and decode color sequence displayed by the light buoy. + Detect color sequence displayed by th light buoy. :param: None :return: None ''' while self.image is None: - rospy.sleep(rospy.Duration(secs=0, nsecs=1000*1000*500)) # Wait for camera data + if self.debug: + print("Waiting for camera data...") + print() + rospy.sleep(rospy.Duration(secs=0, nsecs=1000*1000*500)) + if self.debug: + print("Camera data is ready!") + print() rate = rospy.Rate(5) while not rospy.is_shutdown(): image = self.image - image_weather = self.image - # Detect colored regions + image_weather = self.image #image_mid self.colored_region_detector.detect(image, image_weather) - color_to_shape = self.colored_region_detector.color_to_shape(drop_shapes=["Horizontal Rectangle"]) + color_to_shape = self.colored_region_detector.color_to_shape(drop_shapes=["rectangle_horizontal"]) if len(color_to_shape) == 0: if self.debug: - print("No regular-shaped colored regions detected!") + print("No regular-shaped colors detected!") print() if self.debug: - print("Detected Colored Regions:") + print("All detected colors and shapes:") print(color_to_shape) print() # Update color sequence updated_for_this_frame = False for color, shapes in color_to_shape.items(): for idx, shape in enumerate(shapes): - if shape == "Vertical Rectangle": + if shape == "rectangle_vertical": + # Update signal buoy centroid + self.buoy_centroid = self.colored_region_detector.centroids[color][idx] updated_for_this_frame = True self.push_color(color) break - else: - time_diff = rospy.get_time() - self.last_color_time # Use time difference to detect black color + else: # Use time difference to detect black signal + time_diff = rospy.get_time() - self.last_color_time if time_diff > 1.5: self.black_index = len(self.color_sequence) if self.debug: - print("Light buoy display not detected for too long, recording black index as {}".format(self.black_index)) + print("No rectangle detected for too long, record black index as {}".format(self.black_index)) + print(color_to_shape) print() - if updated_for_this_frame: - break # Only update once for each frame + if updated_for_this_frame: # Only update once for each image frame + break if self.debug: print("Current Color Sequence: {}".format(self.color_sequence)) print() # Check if color sequence is complete and correct - if self.sequence_complete(): + if self.is_sequence_complete(): if self.debug: print("Complete Color Sequence: {}".format(self.color_sequence)) print() @@ -1464,9 +1446,10 @@ def detect(self): if self.debug: print("Sorted Color Sequence: {}".format(self.color_sequence)) print() + print() print("Color Sequence: {}".format(self.color_sequence)) print() - if self.sequence_valid(): + if self.is_sequence_valid(): if self.debug: print("Color sequence is valid!") print() @@ -1485,53 +1468,48 @@ def detect(self): print("Color sequence is invalid, starting over...") print() self.color_sequence = [] # Start over - # Check timeout (either no signal or detection failed) + # Check timeout, either no signal or detection failed if rospy.get_time() - self.init_time > self.timeout: print("Color sequence detector timeout, dock to a random bay!") print() - return "Random", "Random" + return "random", "random" rate.sleep() def push_color(self, color): ''' Push unique color into the color sequence. Start over if some color(s) is/are missed. - :param color: New color to be pushed into the color sequence + :param color: New color :return: None ''' - # If no color has been pushed so far, push the current color irrespective of any checks if len(self.color_sequence) == 0: self.color_sequence.append(color) self.last_color_time = rospy.get_time() return - # Check if any color(s) is/are missed: - # If color sequence is not yet complete and current color was already detected in the past (except for immediately previous detection), it means that some color(s) is/are missed! - # Case 1: Start over from current color detection if any color(s) is/are missed - # Case 2: Update color sequence if the current color was never detected before - # Case 3: Do nothing if the current color is same as the last color - color_skipped = False + skip_repeated = False for i in range(len(self.color_sequence) - 1): - if color == self.color_sequence[i]: # Case 1 - color_skipped = True + if color == self.color_sequence[i]: + skip_repeated = True break - if color_skipped: + if skip_repeated: if self.debug: print("Missed a color!") print() self.color_sequence = [color] self.black_index = 0 - elif color != self.color_sequence[-1]: # Case 2 + elif color != self.color_sequence[-1]: self.color_sequence.append(color) + # Do nothing if the input color is same as the last stored color self.last_color_time = rospy.get_time() - def sequence_complete(self): + def is_sequence_complete(self): ''' - Check whether color sequence is detected completely (3 or more colors should be detected). + Check whether color sequence is detected completely. :param: None - :return result: Boolean flag determining whether the color sequence is detected completely + :return result: Boolean flag determining whether color sequence is detected completely ''' result = len(self.color_sequence) >= 3 return result @@ -1545,15 +1523,15 @@ def sort_sequence(self): :return: None ''' assert len(self.color_sequence) == 3 - if self.debug: - print("Black index is {}".format(self.black_index)) - print() new_sequence = [] for i in range(3): new_sequence.append(self.color_sequence[(i + self.black_index) % 3]) self.color_sequence = new_sequence + if self.debug: + print("Black index is {}".format(self.black_index)) + print() - def sequence_valid(self): + def is_sequence_valid(self): ''' Check whether the detected color sequence is valid (3 colors should be distinct). @@ -1561,12 +1539,12 @@ def sequence_valid(self): :return result: Boolean flag determining whether the detected color sequence is valid ''' - result = len(self.color_sequence) == 3 and self.color_sequence[0] != self.color_sequence[1] and self.color_sequence[1] != self.color_sequence[2] and self.color_sequence[2] != self.color_sequence[0] + result = len(self.color_sequence) == 3 and self.color_sequence[0] != self.color_sequence[1] and self.color_sequence[1] != self.color_sequence[2] and self.color_sequence[0] != self.color_sequence[2] return result def report_sequence(self): ''' - Reports the detected color sequence to the VRX Scan-Dock-Deliver service. + Reports the detected color sequence to the VRX service. :param: None @@ -1588,23 +1566,23 @@ def decode_sequence(self): :param: None - :return color, shape: Color and shape of the target docking bay placard + :return color, shape: Color and shape of the docking bay placard ''' assert len(self.color_sequence) == 3 color = self.color_sequence[0] - shape_color_code = self.color_sequence[2] - shape = "Unknown" - if shape_color_code == "Red": - shape = "Circle" - elif shape_color_code == "Green": - shape = "Triangle" - elif shape_color_code == "Blue": - shape = "Cross" - elif shape_color_code == "Yellow": - shape = "Horizontal Rectangle" + shape_color = self.color_sequence[2] + shape = "unknown" + if shape_color == "red": + shape = "circle" + elif shape_color == "green": + shape = "triangle" + elif shape_color == "blue": + shape = "cross" + elif shape_color == "yellow": + shape = "rectangle" else: if self.debug: - print("Unknown Color Code: {}".format(shape_color_code)) + print("Unknown Color: {}".format(shape_color)) print() return color, shape @@ -1614,12 +1592,14 @@ class DockDetector: ''' Detects dock using point cloud data from LIDAR. ''' - def __init__(self, debug=False): - self.o3d_cloud = None # Open3D point cloud - self.cloud = None # Point cloud + def __init__(self): + self.cloud = None # Point cloud self.cropped_cloud = None # Point cloud within restriced ROI - self.clusters = [] # Point cloud clusters - self.debug = debug # Flag to enable/disable dock detector debug messages + self.clusters = [] # Point cloud clusters + self.o3d_cloud = None # Open3D point cloud + self.debug = False # Flag to enable/disable dock detector debug messages | TODO: move to config + + rospy.Subscriber("/wamv/sensors/lidars/lidar/points", PointCloud2, self.lidar_callback, queue_size=1) # TODO: move away from here def lidar_callback(self, msg): cloud_data = list(pc2.read_points(msg, skip_nans=True, field_names=("x", "y", "z"))) @@ -1628,6 +1608,17 @@ def lidar_callback(self, msg): self.o3d_cloud = open3d.geometry.PointCloud() self.o3d_cloud.points = open3d.utility.Vector3dVector(numpy.array(cloud_data)) + def is_valid(self): + ''' + Check whether the received point cloud is valid (i.e. does it have points). + + :param: None + + :return result: Boolean flag indicating whether the received point cloud has points + ''' + result = self.cloud.has_points() and self.cropped_cloud.has_points() + return result + def reset(self): ''' Reset all detections. @@ -1640,15 +1631,33 @@ def reset(self): self.cropped_cloud = None self.clusters = [] - def point_cloud_valid(self): + def cluster(self): ''' - Check whether the received point cloud is valid (i.e. does it have points). + Detect and group local point cloud clusters together. :param: None - :return result: Boolean flag indicating whether the received point cloud has points + :return result: Boolean result of the clustering operation ''' - result = self.cloud.has_points() and self.cropped_cloud.has_points() + if not self.is_valid(): + if self.debug: + print("Received invalid point cloud, cannot perform clustering!") + print() + result = False + return result + self.clusters = numpy.array(self.cropped_cloud.cluster_dbscan(eps=1.5, min_points=3, print_progress=False)) + if len(self.clusters) > 0: + max_cluster = self.clusters.max() + if self.debug: + print("Number of Point Cloud Clusters: {}".format(max_cluster + 1)) + print() + # Assign different colors to different clusters + colors = plt.get_cmap("tab20")(self.clusters / (max_cluster if max_cluster > 0 else 1)) + colors[self.clusters < 0] = 0 # Note: cluster == -1 denotes a noise point + self.cropped_cloud.colors = open3d.utility.Vector3dVector(colors[:, :3]) + result = True + return result + result = False return result def update_cloud(self, cloud): @@ -1665,7 +1674,7 @@ def update_cloud(self, cloud): else: self.cloud = cloud self.cropped_cloud = cloud - result = self.point_cloud_valid() + result = self.is_valid() return result def crop_cloud(self): @@ -1684,35 +1693,6 @@ def crop_cloud(self): print() self.cropped_cloud = self.cropped_cloud.crop(box) - def cluster(self): - ''' - Detect and group local point cloud clusters together. - - :param: None - - :return result: Boolean result of the clustering operation - ''' - if not self.point_cloud_valid(): # Sanity check - if self.debug: - print("Received invalid point cloud, cannot perform clustering!") - print() - result = False - return result - self.clusters = numpy.array(self.cropped_cloud.cluster_dbscan(eps=1.5, min_points=3, print_progress=False)) - if len(self.clusters) > 0: - max_cluster = self.clusters.max() - if self.debug: - print("Number of Point Cloud Clusters: {}".format(max_cluster + 1)) - print() - # Assign different colors to different clusters - colors = plt.get_cmap("tab20")(self.clusters / (max_cluster if max_cluster > 0 else 1)) - colors[self.clusters < 0] = 0 # Note: cluster == -1 denotes a noise point - self.cropped_cloud.colors = open3d.utility.Vector3dVector(colors[:, :3]) - result = True - return result - result = False - return result - def filter_bays_by_collinear_centroids(self, cloud=None): ''' Filter docking bays based on collinearity w.r.t. their centroids. @@ -1752,14 +1732,14 @@ def filter_bays_by_collinear_centroids(self, cloud=None): x, y, _ = collapsed_centroids[bay_id] bays.append([x, y, theta]) if self.debug: - print("Bay poses w.r.t. WAM-V:") + print("Bay positions w.r.t. WAM-V:") print(bays) print() return bays def filter_bays_by_plane_normals(self, cloud=None): ''' - Filter docking bays based on collinearity w.r.t. their plane normals (i.e. their heading). + Filter docking bays based on their collinearity in terms of heading. :param cloud: Updated point cloud @@ -1780,7 +1760,7 @@ def filter_bays_by_plane_normals(self, cloud=None): points = numpy.asarray(self.cropped_cloud.points) filtered_points = [] for idx, normal in enumerate(normals): - if abs(normal[2]) < 0.2: # Only keep horizontal plane normals + if abs(normal[2]) < 0.2: # Only keep horizontal points filtered_points.append(points[idx]) if self.debug: print("Number of points before applying plane filter: {}".format(len(points))) @@ -1814,7 +1794,7 @@ def filter_bays_by_centroids_and_normals(self, centroids, normals): :return bays: List of bays satisfying the orientation and distance criteria ''' bays = [] - if centroids is None or normals is None: # Sanity check + if centroids is None or normals is None: if self.debug: print("Cannot filter docking bays without any centroids or normals!") print() @@ -1865,14 +1845,14 @@ def filter_bays_by_centroids_and_normals(self, centroids, normals): print("Similar Horizontal Normal IDs:") print(similar_ids) print() - # Keep planes with appropriate inter-centroid distance + # Keep planes with centroids far away bay_ids = [] - for sim_ids in similar_ids: - if len(sim_ids) == 3: + for simi_ids in similar_ids: + if len(simi_ids) == 3: if self.debug: print("Got 3 similar IDs!") print() - all_xy = [(centroids[i][0], centroids[i][1]) for i in sim_ids] + all_xy = [(centroids[i][0], centroids[i][1]) for i in simi_ids] is_bay = True for i in range(2): x1, y1 = all_xy[i] @@ -1885,13 +1865,13 @@ def filter_bays_by_centroids_and_normals(self, centroids, normals): if self.debug: print("These 3 centroids meet bay interval requirements!") print() - bay_ids = sim_ids + bay_ids = simi_ids break - # At least 2 docking bays should be detected - if len(sim_ids) > 1: + # At least 2 docking bays + if len(simi_ids) > 1: hori_dists = [] - for id in sim_ids: - # Try to merge clusters in the vertical plane + for id in simi_ids: + # Try to merge clusters in a vertical plane x, y, z = centroids[id] dist = numpy.sqrt(x ** 2 + y ** 2) far_enough = True @@ -1902,7 +1882,7 @@ def filter_bays_by_centroids_and_normals(self, centroids, normals): if far_enough: hori_dists.append(dist) bay_ids.append(id) - # Only consider the first similar group since bay pattern is unique in the scene + # Currently only consider the first similar group since bay pattern is unique in the scene if len(bay_ids) > 1: break if self.debug: @@ -1918,7 +1898,7 @@ def filter_bays_by_centroids_and_normals(self, centroids, normals): # Note: Corner case is bays stand across negative Y-axis of WAM-V frame bays = sorted(bays, key=lambda elem: math.atan2(elem[1], elem[0]), reverse=True) if self.debug: - print("Bay poses w.r.t. WAM-V:") + print("Bay positions w.r.t. WAM-V:") print(bays) print() return bays @@ -1929,18 +1909,17 @@ def filter_plane(self, cloud=None): :param cloud: Updated point cloud - :return plane_model, inlier_cloud: Plane model (i.e. [a, b, c, d] in ax + by + cz + d = 0), inliers of the point cloud + :return plane_params: (a, b, c, d) in ax + by + cz + d = 0, inliers of the point cloud ''' self.reset() self.update_cloud(cloud) self.crop_cloud() - if self.point_cloud_valid(): + if self.is_valid(): [a, b, c, d], inliers = self.cropped_cloud.segment_plane(distance_threshold=0.05, ransac_n=5, num_iterations=1000) - plane_model = [a, b, c, d] - inlier_cloud = self.cropped_cloud.select_by_index(inliers) - return plane_model, inlier_cloud - plane_model = inlier_cloud = None - return plane_model, inlier_cloud + plane_params = [a, b, c, d], self.cropped_cloud.select_by_index(inliers) + return plane_params + plane_params = None, None + return plane_params def cluster_centroids(self): ''' @@ -1976,7 +1955,7 @@ def cluster_bounding_box_centroids(self): :param: None - :return centroids: Centroids of boxes bounding the point cloud clusters + :return centroids: Centroids of point cloud clusters ''' assert len(self.clusters) > 0 max_cluster = self.clusters.max() @@ -2026,11 +2005,11 @@ def collapse_centroids(self, centroids, threshold=2): def cluster_normals(self): ''' - Compute cluster point normals. + Compute cluster (plane) normals. :param: None - :return normals_avg: Average point normal within each cluster + :return normals_avg: Average point normals within each cluster ''' assert len(self.clusters) > 0 self.cropped_cloud.estimate_normals(search_param=open3d.geometry.KDTreeSearchParamHybrid(radius=2, max_nn=30)) @@ -2046,7 +2025,7 @@ def cluster_normals(self): for normal in normals: outward_normals.append(-normal) self.cropped_cloud.normals = open3d.utility.Vector3dVector(numpy.array(outward_normals)) - # Average point normals within each cluster to get point normals + # Average point normals within each cluster to get cluster (plane) normals normals = numpy.asarray(self.cropped_cloud.normals) max_cluster = self.clusters.max() normals_avg = numpy.zeros((max_cluster + 1, 3)) @@ -2068,7 +2047,7 @@ def collinearity_test(self, centroids): :param centroids: Point cloud cluster centroids - :return ids: IDs of 3 centroids that form a line (if any) + :return ids: IDs of 3 centroids that form a line (if there is any) ''' centroids_num = len(centroids) if centroids_num <= 3: @@ -2110,7 +2089,7 @@ def collinearity_test(self, centroids): def average_normal_angle(self, centroids, ids): ''' - Compute average normal angle of point cloud cluster centroids. + Compute average normal angle of point cloud cluster centroids :param centroids: Point cloud cluster centroids :param ids : Point cloud cluster centroid IDs @@ -2139,13 +2118,16 @@ class BayDetector: ''' Detects docking bays by making use of `ColoredRegionDetector` and `DockDetector`. ''' - def __init__(self, colors, debug=False): - self.dock_detector = DockDetector(debug=debug) # DockDetector class instance - self.colored_region_detector = ColoredRegionDetector(colors, debug=debug) # ColoredRegionDetector class instance - self.cv_bridge = CvBridge() # CvBridge object - self.color = None # Color of target docking bay placard symbol - self.shape = None # Shape of target docking bay placard symbol - self.debug = debug # Flag to enable/disable bay detector debug messages + def __init__(self, colors): + self.color = None + self.shape = None + self.dock_detector = DockDetector() + self.colored_region_detector = ColoredRegionDetector(colors) + self.cv_bridge = CvBridge() # CvBridge object + self.debug = False # Flag to enable/disable bay detector debug messages | TODO: move to config + + rospy.Subscriber('/wamv/sensors/cameras/camera/image_raw', Image, self.camera_callback, queue_size=1) # TODO: move away from here + self.obstacle_pub = rospy.Publisher('/obstacle_list', Float64MultiArray, queue_size=10) # TODO: move away from here def camera_callback(self, msg): # Try converting ROS Image message to OpenCV image @@ -2155,50 +2137,115 @@ def camera_callback(self, msg): print(error) print() - def detect(self, cloud=None): - ''' - Detect docking bays and identify the target docking bay. + def set_target_color_shape(self, target_color, target_shape): + self.color = target_color + self.shape = target_shape - :param cloud: Updated point cloud + def get_sorted_colored_shapes(self): + if self.image is None: + return {} + image = self.image + image_weather = self.image + self.colored_region_detector.detect(image, image_weather) + color_shape_to_centroid = self.colored_region_detector.color_shape_to_centroid() + if self.debug: + print("Blobs before filtering by shape:") + print(color_shape_to_centroid) + print() + colorshape2centroid_new = self.filter_by_shape(color_shape_to_centroid, drop_shapes=["rectangle_horizontal"]) # Remove items that have unwanted shapes + if self.debug: + print("Blobs after filtering by shape:") + print(colorshape2centroid_new) + print() + # Sort by centroid x value from left to right (i.e. from small to large) + colorshape2centroid_new = {k: v for k, v in sorted(color_shape_to_centroid.items(), key=lambda item: item[1][0])} + if self.debug: + print("Sorted blobs:") + print(colorshape2centroid_new) + print() + return colorshape2centroid_new - :return target_bay_pose, all_bay_poses: Pose of the target bay, Poses of all the bays - ''' - target_bay_pose = None - all_bay_poses = self.dock_detector.filter_bays_by_plane_normals(cloud) - bay_num = len(all_bay_poses) + def filter_by_shape(self, color_shape_to_centroid, drop_shapes=None): + drop = ["unknown"] + if drop_shapes is not None: + drop.extend(drop_shapes) + if self.debug: + print("Dropping following shapes for bay detection:") + print(drop) + print() + another = color_shape_to_centroid + for (color, shape), centroid in color_shape_to_centroid.copy().items(): + if shape in drop: + another.pop((color, shape), None) + return another + + def obstacle_publish(self): + obstacle = Float64MultiArray() + obstacle_pos_list = [] + centroids = self.dock_detector.cluster_centroids() + if len(centroids) > 0: + for i in range(len(centroids)): + obstacle_pos_list.append((centroids[i, 0:2]).tolist()) + obstacle.data = sum(obstacle_pos_list, []) + self.obstacle_pub.publish(obstacle) + + def get_plane_theta_and_cloud(self): + plane_model, inlier_cloud = self.dock_detector.filter_plane() + if plane_model is not None and inlier_cloud is not None: + [a, b, c, d] = plane_model + if self.debug: + print("Plane Model: {}, {}, {}, {}".format(a, b, c, d)) + print() + # Make sure gradient of plane is basically horizontal (i.e. bay plane is vertical) + theta = math.atan2(b, a) if abs(c) < 0.1 else None + return theta, inlier_cloud + if self.debug: + print("Plane filter failed!") + print() + return None, None + + def detect(self, detect_bay_only=False, cloud=None): + bay = None + bay_poses = self.dock_detector.filter_bays_by_plane_normals(cloud) + bay_num = len(bay_poses) if bay_num != 3: if self.debug: - print("Plane filter did not detect 3 bays, trying line filter...") + print("Plane filter did not detect 3 bays, try line filter!") print() - all_bay_poses = self.dock_detector.filter_bays_by_collinear_centroids(cloud) - bay_num = len(all_bay_poses) + bay_poses = self.dock_detector.filter_bays_by_collinear_centroids(cloud) + bay_num = len(bay_poses) if bay_num == 0: if self.debug: print("Line filter did not detect any bays!") print() - return target_bay_pose, all_bay_poses + return bay, bay_poses + if bay_num == 3 and detect_bay_only: + bay = bay_poses[1] + if self.debug: + print("Plane filter detectd 3 bays, but return the middle bay pose!") + print() + return bay, bay_poses if self.debug: print("Plane filter detected {} bays.".format(bay_num)) print() - color_shape_to_centroid = self.sort_bay_symbols() + color_shape_to_centroid = self.get_sorted_colored_shapes() cs_num = len(color_shape_to_centroid) if cs_num == 0: if self.debug: print("No blobs detected, no need to continue...") print() - return target_bay_pose, all_bay_poses + return bay, bay_poses if self.debug: print("Number of blobs: {}".format(cs_num)) print("Number of bays: {}".format(bay_num)) print() - if self.color == "Random" or self.shape == "Random": - print("Unspecified docking bay placard color or shape, docking to a random bay...") - idx = numpy.random.randint(3) - print("Bay Number: {}".format(idx+1)) - target_bay_pose = all_bay_poses[idx] - print("Bay Pose: {}".format(target_bay_pose)) + if self.color == "random" or self.shape == "random": print() - return target_bay_pose, all_bay_poses + print("Unspecified docking bay placard color or shape, docking to the left bay...") + bay = bay_poses[0] + print("Bay Pose: {}".format(bay)) + print() + return bay, bay_poses if cs_num == bay_num: if self.debug: print("Number of detected blobs matched the bay count!") @@ -2207,85 +2254,24 @@ def detect(self, cloud=None): # According to task description, each bay has unique color and shape # Since shape detection is unstable, just use color if cs[0] == self.color: + print() print("Docking bay placard color and shape matched, docking to the target bay...") - print("Placard Symbol Color: {}".format(self.color)) - shape = "Rectangle" if self.shape == "Horizontal Rectangle" else self.shape - print("Placard Symbol Shape: {}".format(shape)) - print("Bay Number: {}".format(idx+1)) - target_bay_pose = all_bay_poses[idx] - print("Bay Pose: {}".format(target_bay_pose)) + bay = bay_poses[idx] + print("Bay Pose: {}".format(bay)) print() - return target_bay_pose, all_bay_poses + return bay, bay_poses if cs_num == 3: - print("Docking bay placard color or shape did not match, docking to a random bay...") - idx = numpy.random.randint(3) - print("Bay Number: {}".format(idx+1)) - target_bay_pose = all_bay_poses[idx] - print("Bay Pose: {}".format(target_bay_pose)) print() - return target_bay_pose, all_bay_poses + print("Docking bay placard color or shape did not match, docking to the middle bay...") + bay = bay_poses[1] + print("Bay Pose: {}".format(bay)) + print() + return bay, bay_poses if self.debug: print("Cannot find target color and shape!") else: if self.debug: print("Number of detected blobs did not match the bay count!") - return target_bay_pose, all_bay_poses - - def target_bay_symbol(self, target_color, target_shape): - ''' - Load color and shape of target docking bay placard symbol. - - :param target_color: Color of target docking bay placard symbol - :param target_shape: Shape of target docking bay placard symbol - - :return: None - ''' - self.color = target_color - self.shape = target_shape - - def sort_bay_symbols(self): - ''' - Sort docking bay placard symbols (colors and shapes) from left to right. - - :param: None - - :return sorted_bay_symbols: Docking bay placard symbols (colors and shapes) sorted from left to right. - ''' - if self.image is None: # Sanity check - return {} - image = self.image - image_weather = self.image - self.colored_region_detector.detect(image, image_weather) - color_shape_to_centroid = self.colored_region_detector.color_shape_to_centroid() - # Sort by centroid X-coordinate value (i.e. from left to right) - sorted_bay_symbols = {k: v for k, v in sorted(color_shape_to_centroid.items(), key=lambda item: item[1][0])} - if self.debug: - print("Sorted Colored Shapes:") - print(sorted_bay_symbols) - print() - return sorted_bay_symbols - - def plane_angle_and_cloud(self): - ''' - Compute angle and point cloud of the dock plane. - - :param: None - - :return theta, inlier_cloud: Angle of dock plane, Point cloud of dock plane - ''' - plane_model, inlier_cloud = self.dock_detector.filter_plane() - if plane_model is not None and inlier_cloud is not None: - [a, b, c, d] = plane_model - if self.debug: - print("Plane Model: {}, {}, {}, {}".format(a, b, c, d)) - print() - # Make sure gradient of plane is basically horizontal (i.e. dock plane is vertical) - theta = math.atan2(b, a) if abs(c) < 0.1 else None - return theta, inlier_cloud - if self.debug: - print("Plane filter failed!") - print() - theta = inlier_cloud = None - return theta, inlier_cloud + return bay, bay_poses ################################################################################ diff --git a/singaboat_vrx/src/singaboat_vrx/planning_module.py b/singaboat_vrx/src/singaboat_vrx/planning_module.py index 0892d28..535cde2 100755 --- a/singaboat_vrx/src/singaboat_vrx/planning_module.py +++ b/singaboat_vrx/src/singaboat_vrx/planning_module.py @@ -107,7 +107,7 @@ class Waypoint: PASS = Drive-through waypoint (used for general path tracking) HALT = Temporary station-keeping waypoint (used for accurate path tracking) PARK = Indefinite station-keeping waypoint (used for station-keeping) - SCAN = Scanning waypoint (used for halting and scanning around) + PLAN = Planning waypoint (used for planning a path to the next waypoint) ''' def __init__(self, wp_mode = "NONE", gps_lat = -1, gps_lon = -1, enu_x = -1, enu_y = -1, heading = 0): self.wp_mode = wp_mode @@ -194,26 +194,26 @@ def measure_distance(self, q0, q1): class PathPlanner: def __init__(self, mission, path_creator, handover_offset=5): - self.mission = mission # Mission - self.path_creator = path_creator # Path creator - self.path_hdlr = path_creator(mission) # Path creater object - self.mission_complete = False # Mission completion flag - self.lap_count = 1 # Number of times to repeat the mission - self.lap_counter = 0 # Lap counter - self.original_path = self.path_hdlr.path # Original path - self.working_path = self.original_path.copy() # Working path - self.working_index = 0 # Index of waypoint on the working path - self.original_index = 0 # Index of waypoint on the original path - self.current_wp = self.original_path[0] # Current waypoint - self.next_wp = self.original_path[0] # Next waypoint - self.proj_heading = 0 # Projected heading at the goal - self.desired_heading = 0 # Desired (go-to-goal) heading of the WAM-V + self.mission = mission # Mission + self.path_creator = path_creator # Path creator + self.path_hdlr = path_creator(mission) # Path creater object + self.mission_complete = False # Mission completion flag + self.lap_count = 1 # Number of times to repeat the mission + self.lap_counter = 0 # Lap counter + self.original_path = self.path_hdlr.path # Original path + self.working_path = self.original_path.copy() # Working path + self.working_index = 0 # Index of waypoint on the working path + self.original_index = 0 # Index of waypoint on the original path + self.current_wp = self.original_path[0] # Current waypoint + self.next_wp = self.original_path[0] # Next waypoint + self.proj_heading = 0 # Projected heading at the goal + self.desired_heading = 0 # Desired (go-to-goal) heading of the WAM-V self.cross_track_error = 0 # Cross-track error in meters - self.beta_hat = 0 # Integral term for computing the desired heading using ILOS guidance method - self.look_ahead_dist = 30 # Lookahead distance in meters - self.min_wp_dist = 25 # Minimum distance between two waypoints so as to allow the WAM-V to get on track - self.max_deviation = 10 # Maximum permissible deviation from path before triggering a replan - self.handover_offset = handover_offset # Distance before the end of the path to handover control of the WAMV-V + self.beta_hat = 0 # Integral term for computing the desired heading using ILOS guidance method + self.look_ahead = 30 # Lookahead distance in meters + self.min_wp_dist = 25 # Minimum distance between two waypoints so as to allow the WAM-V to get on track + self.max_deviation = 10 # Maximum permissible deviation from path before triggering a replan + self.handover_offset = handover_offset # Distance before the end of the path to handover control of the WAMV-V def measure_distance(self, wp1, wp2): ''' @@ -291,7 +291,7 @@ def replan(self, asv_pose, current_wp_idx, original_path): :return recovery_path: Re-planned path ''' - target_idx = int(min(current_wp_idx + numpy.floor(self.look_ahead_dist/self.path_hdlr.step_size), len(original_path)-numpy.floor(self.min_wp_dist/self.path_hdlr.step_size))) + target_idx = int(min(current_wp_idx + numpy.floor(self.look_ahead/self.path_hdlr.step_size), len(original_path)-numpy.floor(self.min_wp_dist/self.path_hdlr.step_size))) start = asv_pose end = copy.deepcopy(original_path[target_idx]) mission = Mission.Path(waypoints=[start, end]) diff --git a/singaboat_vrx/src/singaboat_wayfinding.py b/singaboat_vrx/src/singaboat_wayfinding.py index c9b62dd..8286a1c 100755 --- a/singaboat_vrx/src/singaboat_wayfinding.py +++ b/singaboat_vrx/src/singaboat_wayfinding.py @@ -3,87 +3,54 @@ import math import numpy import rospy -import tf -from sensor_msgs.msg import NavSatFix -from sensor_msgs.msg import Imu -from geographic_msgs.msg import GeoPath -from geometry_msgs.msg import Twist from dynamic_reconfigure.server import Server +from std_msgs.msg import Int32 +from geometry_msgs.msg import Pose, Twist # !!! NOT USED AS PER THE STANDARDS !!! from singaboat_vrx.cfg import WayfindingConfig from singaboat_vrx.control_module import PIDController -from singaboat_vrx.common_utilities import gps_to_enu, quaternion_to_euler, euler_to_quaternion, normalize_angle +from singaboat_vrx.common_utilities import normalize_angle ################################################################################ class Wayfinding: def __init__(self): # Initialize wayfinding - self.cur_pos = None # Current 2D position (x, y) - self.cur_rot = None # Current 1D orientation (yaw) - self.cur_position = None # Current 3D position (x, y, z) - self.cur_rotation = None # Current 3D orientation (roll, pitch, yaw) - self.wps_pos_x = [] # List of desired position (x) of all waypoints - self.wps_pos_y = [] # List of desired position (y) of all waypoints - self.wps_rot_z = [] # List of desired orientation (yaw) of all waypoints - self.wps_position = [] # List of desired 3D position (x, y, z) of all waypoints - self.wps_rotation = [] # List of desired 3D orientation (roll, pitch, yaw) of all waypoints - self.wp_count = None # Waypoint count (starts from 1) - self.wp_index = 0 # Waypoint index (starts from 0) - self.cmd_pos = None # Commanded 2D position (x, y) - self.cmd_rot = None # Commanded 1D orientation (yaw) - self.time = None # Current timestamp - self.config = {} # Wayfinding configuration + self.cmd_pos = None # Desired 2D position (x, y) + self.cmd_rot = None # Desired 1D orientation (yaw) + self.cur_pos = None # Current 2D position (x, y) + self.cur_rot = None # Current 1D orientation (yaw) + self.wp_count = None # Waypoint count (starts from 1) + self.wp_index = 0 # Waypoint index (starts from 0) + self.time = None # Current timestamp + self.config = {} # Wayfinding configuration # ROS infrastructure - self.tf_broadcaster = None + self.wp_index_msg = None self.cmd_vel_msg = None + self.wp_index_pub = None self.cmd_vel_pub = None self.dyn_reconf_srv = None - def gps_callback(self, msg): - if self.cur_rot is None: # If no yaw data is available, GPS offset cannot be compensated - return - lat = msg.latitude - lon = msg.longitude - pos_x, pos_y, pos_z = gps_to_enu(lat, lon) - # WAM-V frame is +0.85 m offset in local x-axis w.r.t. GPS frame - pos_x += self.gps_offset * math.cos(self.cur_rot[2]) - pos_y += self.gps_offset * math.sin(self.cur_rot[2]) + def wp_count_callback(self, msg): + self.wp_count = msg.data + + def cmd_pose_callback(self, msg): + pos_x = msg.position.x + pos_y = msg.position.y + rot_z = msg.orientation.z + self.cmd_pos = numpy.array([pos_x, pos_y]) + self.cmd_rot = rot_z + + def cur_pose_callback(self, msg): + pos_x = msg.position.x + pos_y = msg.position.y + rot_z = msg.orientation.z self.cur_pos = numpy.array([pos_x, pos_y]) - self.cur_position = numpy.array([pos_x, pos_y, pos_z]) - self.cur_rotation = euler_to_quaternion(self.cur_rot[0], self.cur_rot[1], self.cur_rot[2]) - - def imu_callback(self, msg): - self.cur_rot = quaternion_to_euler(msg.orientation) - - def waypoint_callback(self, msg): - for i in range(len(msg.poses)): - if msg.poses: # Sanity check - self.wp_count = len(msg.poses) - waypoint = msg.poses[i] # Indexing starts from 0 - lat = waypoint.pose.position.latitude - lon = waypoint.pose.position.longitude - pos_x, pos_y, pos_z = gps_to_enu(lat, lon) - rot_x = quaternion_to_euler(waypoint.pose.orientation)[0] - rot_y = quaternion_to_euler(waypoint.pose.orientation)[1] - rot_z = quaternion_to_euler(waypoint.pose.orientation)[2] - self.wps_pos_x.append(pos_x) - self.wps_pos_y.append(pos_y) - self.wps_rot_z.append(rot_z) - self.wps_position.append(numpy.array([pos_x, pos_y, pos_z])) - self.wps_rotation.append(euler_to_quaternion(rot_x, rot_y, rot_z)) + self.cur_rot = rot_z def wayfinding(self): - # Broadcast transform of `wamv/base_link` frame w.r.t. `world` frame - self.tf_broadcaster.sendTransform((self.cur_position[0], self.cur_position[1], self.cur_position[2]), (self.cur_rotation[0], self.cur_rotation[1], self.cur_rotation[2], self.cur_rotation[3]), rospy.Time.now(), 'wamv/base_link', 'world') - # Broadcast transform of `waypoint_i` frame w.r.t. `world` frame - for i in range(self.wp_count): - frame_id = 'waypoint_' + str(i) - self.tf_broadcaster.sendTransform((self.wps_position[i][0], self.wps_position[i][1], self.wps_position[i][2]), (self.wps_rotation[i][0], self.wps_rotation[i][1], self.wps_rotation[i][2], self.wps_rotation[i][3]), rospy.Time.now(), frame_id, 'world') # Proceed to current waypoint until it is actually reached - self.cmd_pos = numpy.array([self.wps_pos_x[self.wp_index], self.wps_pos_y[self.wp_index]]) - self.cmd_rot = self.wps_rot_z[self.wp_index] self.time = rospy.get_time() - if bool(self.config) and not [x for x in (self.cur_pos, self.cur_rot, self.cmd_pos, self.cmd_rot, self.wp_count, self.time) if x is None]: + if bool(self.config) and not [x for x in (self.wp_count, self.cmd_pos, self.cmd_rot, self.cur_pos, self.cur_rot, self.time) if x is None]: err_pos = self.cmd_pos - self.cur_pos # Error in position [x_des - x_cur, y_des - y_cur] if numpy.linalg.norm(err_pos) > self.goal_tol: # (Euclidean distance to goal as L2 norm) # If far from goal, head directly towards the goal by controlling Vx & Wz @@ -91,24 +58,27 @@ def wayfinding(self): if lin_vel_x > self.v_limit: # Clamp linear velocity along X-axis lin_vel_x = self.v_limit lin_vel_y = 0.0 - err_rot = normalize_angle(math.atan2(err_pos[1], err_pos[0]) - self.cur_rot[2]) # Error in orientation + err_rot = normalize_angle(math.atan2(err_pos[1], err_pos[0]) - self.cur_rot) # Error in orientation ang_vel_z = self.pid_g2g.control(err_rot, self.time) # PID controller for Wz else: # If near goal, perform fine adjustments in Vx, Vy & Wz for station-keeping - rot_tf = normalize_angle(math.atan2(err_pos[1], err_pos[0]) - self.cur_rot[2]) # G2G rotation transformation + rot_tf = normalize_angle(math.atan2(err_pos[1], err_pos[0]) - self.cur_rot) # G2G rotation transformation err_pos = numpy.array([numpy.linalg.norm(err_pos) * math.cos(rot_tf), numpy.linalg.norm(err_pos) * math.sin(rot_tf)]) # Error in position (in local frame) lin_vel_x = self.pid_sk_vx.control(err_pos[0], self.time) # PID controller for Vx lin_vel_y = self.pid_sk_vy.control(err_pos[1], self.time) # PID controller for Vy - err_rot = normalize_angle(self.cmd_rot - self.cur_rot[2]) # Error in orientation + err_rot = normalize_angle(self.cmd_rot - self.cur_rot) # Error in orientation ang_vel_z = self.pid_sk_wz.control(err_rot, self.time) # PID controller for Wz - print("Wayfinding to waypoint {} of {}...".format(self.wp_index+1, self.wp_count)) # Counting and indexing starts from 1 - print("Current Target Waypoint: {:.4} m, {:.4} m, {:.4} rad".format(self.cmd_pos[0], self.cmd_pos[1], self.cmd_rot)) + print("WP_Count: %d" % (self.wp_count)) + print("WP_Index: %d" % (self.wp_index)) + print() + print("Error_Pos_X: %.4f m" % (err_pos[0])) + print("Error_Pos_Y: %.4f m" % (err_pos[1])) + print("Error_Rot_Z: %.4f rad" % (err_rot)) + print() + print("Lin_Vel_X: %.4f m/s" % (lin_vel_x)) + print("Lin_Vel_Y: %.4f m/s" % (lin_vel_y)) + print("Ang_Vel_Z: %.4f rad/s" % (ang_vel_z)) print() - if self.debug: - print("Error Pos X: {:.4} m".format(err_pos[0])) - print("Error Pos Y: {:.4} m".format(err_pos[1])) - print("Error Rot Z: {:.4} rad".format(err_rot)) - print() # Generate and publish `cmd_vel` message self.cmd_vel_msg.linear.x = lin_vel_x self.cmd_vel_msg.linear.y = lin_vel_y @@ -121,21 +91,22 @@ def wayfinding(self): # If all waypoints have been visited, target the first waypoint again if self.wp_index >= self.wp_count: self.wp_index = 0 + # Generate and publish `wp_index` message + self.wp_index_msg.data = self.wp_index + self.wp_index_pub.publish(self.wp_index_msg) def config_callback(self, config, level): # Handle updated configuration values - self.gps_offset = config['gps_offset'] # GPS offset w.r.t. WAM-V along X-axis - self.pid_g2g = PIDController(config['G2G_kP'], config['G2G_kI'], config['G2G_kD'], config['G2G_kS']) # Go-to-goal PID controller - self.pid_sk_vx = PIDController(config['SK_Vx_kP'], config['SK_Vx_kI'], config['SK_Vx_kD'], config['SK_Vx_kS']) # Station-keeping Vx PID controller - self.pid_sk_vy = PIDController(config['SK_Vy_kP'], config['SK_Vy_kI'], config['SK_Vy_kD'], config['SK_Vy_kS']) # Station-keeping Vy PID controller - self.pid_sk_wz = PIDController(config['SK_Wz_kP'], config['SK_Wz_kI'], config['SK_Wz_kD'], config['SK_Wz_kS']) # Station-keeping Wz PID controller - self.goal_tol = config['goal_tol'] # Goal tolerance dead-band of go-to-goal PID controller - self.v_const = config['v_const'] # Proportional gain for linear velocity outside goal tolerance - self.v_limit = config['v_limit'] # Saturation limit for linear velocity outside goal tolerance - self.pos_tol = config['pos_tol'] # Position tolerance to determine whether goal is reached - self.rot_tol = config['rot_tol'] # Orientation tolerance to determine whether goal is reached - self.debug = config['debug'] # Flag to enable/disable debug messages - self.config = config + self.pid_g2g = PIDController(config['G2G_kP'], config['G2G_kI'], config['G2G_kD'], config['G2G_kS']) # Go-to-goal PID controller + self.pid_sk_vx = PIDController(config['SK_Vx_kP'], config['SK_Vx_kI'], config['SK_Vx_kD'], config['SK_Vx_kS']) # Station-keeping Vx PID controller + self.pid_sk_vy = PIDController(config['SK_Vy_kP'], config['SK_Vy_kI'], config['SK_Vy_kD'], config['SK_Vy_kS']) # Station-keeping Vy PID controller + self.pid_sk_wz = PIDController(config['SK_Wz_kP'], config['SK_Wz_kI'], config['SK_Wz_kD'], config['SK_Wz_kS']) # Station-keeping Wz PID controller + self.goal_tol = config['goal_tol'] # Goal tolerance dead-band of go-to-goal PID controller + self.v_const = config['v_const'] # Proportional gain for linear velocity outside goal tolerance + self.v_limit = config['v_limit'] # Saturation limit for linear velocity outside goal tolerance + self.pos_tol = config['pos_tol'] # Position tolerance to determine whether goal is reached + self.rot_tol = config['rot_tol'] # Orientation tolerance to determine whether goal is reached + self.config = config return config ################################################################################ @@ -149,25 +120,19 @@ def config_callback(self, config, level): # Dynamic reconfigure server wayfinding_node.dyn_reconf_srv = Server(WayfindingConfig, wayfinding_node.config_callback) - # Transform broadcaster - wayfinding_node.tf_broadcaster = tf.TransformBroadcaster() - - # Message + # Messages + wayfinding_node.wp_index_msg = Int32() wayfinding_node.cmd_vel_msg = Twist() # Subscribers - rospy.Subscriber('/wamv/sensors/gps/gps/fix', NavSatFix, wayfinding_node.gps_callback) - rospy.Subscriber('/wamv/sensors/imu/imu/data', Imu, wayfinding_node.imu_callback) - rospy.Subscriber('/vrx/wayfinding/waypoints', GeoPath, wayfinding_node.waypoint_callback) + rospy.Subscriber('/vrx/wp_count', Int32, wayfinding_node.wp_count_callback) + rospy.Subscriber('/wamv/cmd_pose', Pose, wayfinding_node.cmd_pose_callback) + rospy.Subscriber('/wamv/cur_pose', Pose, wayfinding_node.cur_pose_callback) - # Publisher + # Publishers + wayfinding_node.wp_index_pub = rospy.Publisher('/vrx/wp_index', Int32, queue_size = 10) wayfinding_node.cmd_vel_pub = rospy.Publisher('/wamv/cmd_vel', Twist, queue_size = 10) - # Wait for valid messages to ensure proper state initialization - rospy.wait_for_message('/wamv/sensors/gps/gps/fix', NavSatFix) - rospy.wait_for_message('/wamv/sensors/imu/imu/data', Imu) - rospy.wait_for_message('/vrx/wayfinding/waypoints', GeoPath) - # ROS rate rate = rospy.Rate(20) diff --git a/singaboat_vrx/urdf/README.md b/singaboat_vrx/urdf/README.md index 558b511..259460b 100644 --- a/singaboat_vrx/urdf/README.md +++ b/singaboat_vrx/urdf/README.md @@ -1,6 +1,6 @@ # SINGABOAT-VRX WAM-V Building Instructions -1. Change directory to `/singaboat_vrx/wamv` and edit the `component_config.yaml` and `thruster_config.yaml` files as required. +1. Change directory to `/singaboat_vrx/config` and edit the `component_config.yaml` and `thruster_config.yaml` files as required. 2. Launch the `singaboat_build_wamv.launch` file to build WAM-V as per the specifications of `component_config.yaml` and `thruster_config.yaml` files. ```bash @@ -9,4 +9,4 @@ $ roslaunch .launch $ roslaunch singaboat_vrx singaboat_build_wamv.launch ``` -3. The built WAM-V will be saved to `/singaboat_vrx/urdf` directory as `singaboat.urdf` file by default (configurable through `singaboat_build_wamv.launch` file in `/singaboat_vrx/launch/tools` directory). +3. The built WAM-V will be saved to `/singaboat_vrx/urdf` directory as `singaboat.urdf` file by default (configurable through `singaboat_build_wamv.launch` file in `/singaboat_vrx/launch` directory). diff --git a/singaboat_vrx/urdf/singaboat.urdf b/singaboat_vrx/urdf/singaboat.urdf index 1f2f340..c25a807 100644 --- a/singaboat_vrx/urdf/singaboat.urdf +++ b/singaboat_vrx/urdf/singaboat.urdf @@ -884,7 +884,7 @@ - + diff --git a/singaboat_vrx/wamv/README.md b/singaboat_vrx/wamv/README.md deleted file mode 100644 index 558b511..0000000 --- a/singaboat_vrx/wamv/README.md +++ /dev/null @@ -1,12 +0,0 @@ -# SINGABOAT-VRX WAM-V Building Instructions - -1. Change directory to `/singaboat_vrx/wamv` and edit the `component_config.yaml` and `thruster_config.yaml` files as required. - -2. Launch the `singaboat_build_wamv.launch` file to build WAM-V as per the specifications of `component_config.yaml` and `thruster_config.yaml` files. -```bash -$ roslaunch .launch - -$ roslaunch singaboat_vrx singaboat_build_wamv.launch -``` - -3. The built WAM-V will be saved to `/singaboat_vrx/urdf` directory as `singaboat.urdf` file by default (configurable through `singaboat_build_wamv.launch` file in `/singaboat_vrx/launch/tools` directory). diff --git a/singaboat_vrx/worlds/README.md b/singaboat_vrx/worlds/README.md index 5a26312..340debb 100644 --- a/singaboat_vrx/worlds/README.md +++ b/singaboat_vrx/worlds/README.md @@ -1,6 +1,6 @@ # SINGABOAT-VRX World Building Instructions -1. Change directory to `/singaboat_vrx/worlds` and create/edit a `.world.xacro` file. +1. Change directory to `/singaboat_vrx/worlds` and create/edit a `.world.xacro` file: 2. Build the Gazebo world: ```bash diff --git a/vrx/vrx_gazebo/launch/gymkhana.launch b/vrx/vrx_gazebo/launch/gymkhana.launch index 2e4fcf8..38cfd95 100644 --- a/vrx/vrx_gazebo/launch/gymkhana.launch +++ b/vrx/vrx_gazebo/launch/gymkhana.launch @@ -7,14 +7,14 @@ - - + + @@ -39,11 +39,11 @@ - - - + + + - + diff --git a/vrx/vrx_gazebo/launch/perception_task.launch b/vrx/vrx_gazebo/launch/perception_task.launch index bdb73a2..105bd5c 100644 --- a/vrx/vrx_gazebo/launch/perception_task.launch +++ b/vrx/vrx_gazebo/launch/perception_task.launch @@ -7,13 +7,13 @@ - - + + diff --git a/vrx/vrx_gazebo/launch/scan_dock_deliver.launch b/vrx/vrx_gazebo/launch/scan_dock_deliver.launch index dfb5e69..a9ba0d2 100644 --- a/vrx/vrx_gazebo/launch/scan_dock_deliver.launch +++ b/vrx/vrx_gazebo/launch/scan_dock_deliver.launch @@ -7,8 +7,6 @@ - - @@ -36,11 +34,11 @@ - - - + + + - + diff --git a/vrx/vrx_gazebo/launch/wayfinding.launch b/vrx/vrx_gazebo/launch/wayfinding.launch index 68ca059..9e5ed08 100644 --- a/vrx/vrx_gazebo/launch/wayfinding.launch +++ b/vrx/vrx_gazebo/launch/wayfinding.launch @@ -7,8 +7,6 @@ - - @@ -38,7 +36,7 @@ - + diff --git a/vrx/vrx_gazebo/launch/wildlife.launch b/vrx/vrx_gazebo/launch/wildlife.launch index d377ebd..a905aae 100644 --- a/vrx/vrx_gazebo/launch/wildlife.launch +++ b/vrx/vrx_gazebo/launch/wildlife.launch @@ -7,8 +7,6 @@ - - @@ -36,11 +34,11 @@ - - - + + + - + diff --git a/vrx/vrx_gazebo/models/crocodile_buoy/meshes/Crocodile_BaseColor.png b/vrx/vrx_gazebo/models/crocodile_buoy/meshes/Crocodile_BaseColor.png deleted file mode 100755 index 3eab4b1..0000000 Binary files a/vrx/vrx_gazebo/models/crocodile_buoy/meshes/Crocodile_BaseColor.png and /dev/null differ diff --git a/vrx/vrx_gazebo/models/crocodile_buoy/meshes/crocodile.dae b/vrx/vrx_gazebo/models/crocodile_buoy/meshes/crocodile.dae index c3e2541..78621c0 100755 --- a/vrx/vrx_gazebo/models/crocodile_buoy/meshes/crocodile.dae +++ b/vrx/vrx_gazebo/models/crocodile_buoy/meshes/crocodile.dae @@ -2,10 +2,16 @@ + FBX COLLADA exporter + 2021-08-23T17:33:35Z + 2021-08-23T17:33:35Z + + + <unit meter="0.025400" name="centimeter"/> <up_axis>Z_UP</up_axis> </asset> diff --git a/vrx/vrx_gazebo/models/mb_marker_buoy_red/meshes/mb_marker_buoy.dae b/vrx/vrx_gazebo/models/mb_marker_buoy_red/meshes/mb_marker_buoy.dae index 625afd0..5fcf67a 100755 --- a/vrx/vrx_gazebo/models/mb_marker_buoy_red/meshes/mb_marker_buoy.dae +++ b/vrx/vrx_gazebo/models/mb_marker_buoy_red/meshes/mb_marker_buoy.dae @@ -2,10 +2,16 @@ <COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1"> <asset> <contributor> + <author/> <authoring_tool>FBX COLLADA exporter</authoring_tool> + <comments/> </contributor> <created>2021-07-13T17:47:10Z</created> + <keywords/> <modified>2021-07-13T17:47:10Z</modified> + <revision/> + <subject/> + <title/> <unit meter="0.304800" name="centimeter"/> <up_axis>Y_UP</up_axis> </asset> diff --git a/vrx/vrx_gazebo/models/mb_round_buoy_orange/meshes/mb_round_buoy.dae b/vrx/vrx_gazebo/models/mb_round_buoy_orange/meshes/mb_round_buoy.dae index 973b380..825db7a 100755 --- a/vrx/vrx_gazebo/models/mb_round_buoy_orange/meshes/mb_round_buoy.dae +++ b/vrx/vrx_gazebo/models/mb_round_buoy_orange/meshes/mb_round_buoy.dae @@ -2,10 +2,16 @@ <COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1"> <asset> <contributor> + <author/> <authoring_tool>FBX COLLADA exporter</authoring_tool> + <comments/> </contributor> <created>2021-07-13T17:50:17Z</created> + <keywords/> <modified>2021-07-13T17:50:17Z</modified> + <revision/> + <subject/> + <title/> <unit meter="0.304800" name="centimeter"/> <up_axis>Y_UP</up_axis> </asset> diff --git a/vrx/vrx_gazebo/models/platypus_buoy/meshes/Platypus_BaseColor.png b/vrx/vrx_gazebo/models/platypus_buoy/meshes/Platypus_BaseColor.png deleted file mode 100755 index 28483c0..0000000 Binary files a/vrx/vrx_gazebo/models/platypus_buoy/meshes/Platypus_BaseColor.png and /dev/null differ diff --git a/vrx/vrx_gazebo/models/platypus_buoy/meshes/platypus.dae b/vrx/vrx_gazebo/models/platypus_buoy/meshes/platypus.dae index da6f88d..f1912eb 100755 --- a/vrx/vrx_gazebo/models/platypus_buoy/meshes/platypus.dae +++ b/vrx/vrx_gazebo/models/platypus_buoy/meshes/platypus.dae @@ -2,10 +2,16 @@ <COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1"> <asset> <contributor> + <author/> <authoring_tool>FBX COLLADA exporter</authoring_tool> + <comments/> </contributor> <created>2021-08-23T17:43:23Z</created> + <keywords/> <modified>2021-08-23T17:43:23Z</modified> + <revision/> + <subject/> + <title/> <unit meter="0.025400" name="centimeter"/> <up_axis>Z_UP</up_axis> </asset> diff --git a/vrx/vrx_gazebo/models/turtle_buoy/meshes/Turtle_BaseColor.png b/vrx/vrx_gazebo/models/turtle_buoy/meshes/Turtle_BaseColor.png deleted file mode 100755 index 8cecfb5..0000000 Binary files a/vrx/vrx_gazebo/models/turtle_buoy/meshes/Turtle_BaseColor.png and /dev/null differ diff --git a/vrx/vrx_gazebo/models/turtle_buoy/meshes/turtle.dae b/vrx/vrx_gazebo/models/turtle_buoy/meshes/turtle.dae index 0eb3fe0..1f42fb9 100755 --- a/vrx/vrx_gazebo/models/turtle_buoy/meshes/turtle.dae +++ b/vrx/vrx_gazebo/models/turtle_buoy/meshes/turtle.dae @@ -2,10 +2,16 @@ <COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1"> <asset> <contributor> + <author/> <authoring_tool>FBX COLLADA exporter</authoring_tool> + <comments/> </contributor> <created>2021-08-23T18:22:27Z</created> + <keywords/> <modified>2021-08-23T18:22:27Z</modified> + <revision/> + <subject/> + <title/> <unit meter="0.010000" name="centimeter"/> <up_axis>Z_UP</up_axis> </asset> diff --git a/vrx/vrx_gazebo/worlds/2022_phase2/gymkhana0.world b/vrx/vrx_gazebo/worlds/2022_phase2/gymkhana0.world index 576e3df..70c001b 100644 --- a/vrx/vrx_gazebo/worlds/2022_phase2/gymkhana0.world +++ b/vrx/vrx_gazebo/worlds/2022_phase2/gymkhana0.world @@ -172,7 +172,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_phase2/gymkhana1.world b/vrx/vrx_gazebo/worlds/2022_phase2/gymkhana1.world index e82ef48..da75689 100644 --- a/vrx/vrx_gazebo/worlds/2022_phase2/gymkhana1.world +++ b/vrx/vrx_gazebo/worlds/2022_phase2/gymkhana1.world @@ -172,7 +172,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_phase2/gymkhana2.world b/vrx/vrx_gazebo/worlds/2022_phase2/gymkhana2.world index 3e4cc5a..2c07db1 100644 --- a/vrx/vrx_gazebo/worlds/2022_phase2/gymkhana2.world +++ b/vrx/vrx_gazebo/worlds/2022_phase2/gymkhana2.world @@ -172,7 +172,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_phase2/gymkhana3.world b/vrx/vrx_gazebo/worlds/2022_phase2/gymkhana3.world index 597f54e..0ff6587 100644 --- a/vrx/vrx_gazebo/worlds/2022_phase2/gymkhana3.world +++ b/vrx/vrx_gazebo/worlds/2022_phase2/gymkhana3.world @@ -172,7 +172,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_phase2/gymkhana4.world b/vrx/vrx_gazebo/worlds/2022_phase2/gymkhana4.world index 8deabf9..091f6ad 100644 --- a/vrx/vrx_gazebo/worlds/2022_phase2/gymkhana4.world +++ b/vrx/vrx_gazebo/worlds/2022_phase2/gymkhana4.world @@ -172,7 +172,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_phase2/gymkhana5.world b/vrx/vrx_gazebo/worlds/2022_phase2/gymkhana5.world index f70776b..81f897b 100644 --- a/vrx/vrx_gazebo/worlds/2022_phase2/gymkhana5.world +++ b/vrx/vrx_gazebo/worlds/2022_phase2/gymkhana5.world @@ -172,7 +172,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_phase2/perception0.world b/vrx/vrx_gazebo/worlds/2022_phase2/perception0.world index d99a665..4312ae2 100644 --- a/vrx/vrx_gazebo/worlds/2022_phase2/perception0.world +++ b/vrx/vrx_gazebo/worlds/2022_phase2/perception0.world @@ -172,7 +172,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_phase2/perception1.world b/vrx/vrx_gazebo/worlds/2022_phase2/perception1.world index e3b5980..0479360 100644 --- a/vrx/vrx_gazebo/worlds/2022_phase2/perception1.world +++ b/vrx/vrx_gazebo/worlds/2022_phase2/perception1.world @@ -172,7 +172,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_phase2/perception2.world b/vrx/vrx_gazebo/worlds/2022_phase2/perception2.world index 8e74793..7e6d163 100644 --- a/vrx/vrx_gazebo/worlds/2022_phase2/perception2.world +++ b/vrx/vrx_gazebo/worlds/2022_phase2/perception2.world @@ -172,7 +172,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_phase2/perception3.world b/vrx/vrx_gazebo/worlds/2022_phase2/perception3.world index fff92e1..8b3d583 100644 --- a/vrx/vrx_gazebo/worlds/2022_phase2/perception3.world +++ b/vrx/vrx_gazebo/worlds/2022_phase2/perception3.world @@ -172,7 +172,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_phase2/perception4.world b/vrx/vrx_gazebo/worlds/2022_phase2/perception4.world index fb78320..b7357c1 100644 --- a/vrx/vrx_gazebo/worlds/2022_phase2/perception4.world +++ b/vrx/vrx_gazebo/worlds/2022_phase2/perception4.world @@ -172,7 +172,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_phase2/perception5.world b/vrx/vrx_gazebo/worlds/2022_phase2/perception5.world index de7b1f1..0bc6dc9 100644 --- a/vrx/vrx_gazebo/worlds/2022_phase2/perception5.world +++ b/vrx/vrx_gazebo/worlds/2022_phase2/perception5.world @@ -172,7 +172,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_phase2/scan_dock_deliver0.world b/vrx/vrx_gazebo/worlds/2022_phase2/scan_dock_deliver0.world index 8cd7fcc..18f078a 100644 --- a/vrx/vrx_gazebo/worlds/2022_phase2/scan_dock_deliver0.world +++ b/vrx/vrx_gazebo/worlds/2022_phase2/scan_dock_deliver0.world @@ -172,7 +172,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_phase2/scan_dock_deliver1.world b/vrx/vrx_gazebo/worlds/2022_phase2/scan_dock_deliver1.world index 8dd421a..78821e4 100644 --- a/vrx/vrx_gazebo/worlds/2022_phase2/scan_dock_deliver1.world +++ b/vrx/vrx_gazebo/worlds/2022_phase2/scan_dock_deliver1.world @@ -172,7 +172,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_phase2/scan_dock_deliver2.world b/vrx/vrx_gazebo/worlds/2022_phase2/scan_dock_deliver2.world index 782af5a..6dbbb65 100644 --- a/vrx/vrx_gazebo/worlds/2022_phase2/scan_dock_deliver2.world +++ b/vrx/vrx_gazebo/worlds/2022_phase2/scan_dock_deliver2.world @@ -172,7 +172,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_phase2/scan_dock_deliver3.world b/vrx/vrx_gazebo/worlds/2022_phase2/scan_dock_deliver3.world index 77d8482..4ca5751 100644 --- a/vrx/vrx_gazebo/worlds/2022_phase2/scan_dock_deliver3.world +++ b/vrx/vrx_gazebo/worlds/2022_phase2/scan_dock_deliver3.world @@ -172,7 +172,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_phase2/scan_dock_deliver4.world b/vrx/vrx_gazebo/worlds/2022_phase2/scan_dock_deliver4.world index 32642a1..0385017 100644 --- a/vrx/vrx_gazebo/worlds/2022_phase2/scan_dock_deliver4.world +++ b/vrx/vrx_gazebo/worlds/2022_phase2/scan_dock_deliver4.world @@ -172,7 +172,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_phase2/scan_dock_deliver5.world b/vrx/vrx_gazebo/worlds/2022_phase2/scan_dock_deliver5.world index 4e56256..06cfdbb 100644 --- a/vrx/vrx_gazebo/worlds/2022_phase2/scan_dock_deliver5.world +++ b/vrx/vrx_gazebo/worlds/2022_phase2/scan_dock_deliver5.world @@ -172,7 +172,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_phase2/stationkeeping0.world b/vrx/vrx_gazebo/worlds/2022_phase2/stationkeeping0.world index e4c8661..52ba83d 100644 --- a/vrx/vrx_gazebo/worlds/2022_phase2/stationkeeping0.world +++ b/vrx/vrx_gazebo/worlds/2022_phase2/stationkeeping0.world @@ -199,7 +199,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_phase2/stationkeeping1.world b/vrx/vrx_gazebo/worlds/2022_phase2/stationkeeping1.world index 468f437..aace939 100644 --- a/vrx/vrx_gazebo/worlds/2022_phase2/stationkeeping1.world +++ b/vrx/vrx_gazebo/worlds/2022_phase2/stationkeeping1.world @@ -199,7 +199,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_phase2/stationkeeping2.world b/vrx/vrx_gazebo/worlds/2022_phase2/stationkeeping2.world index 77bf8d4..b62217f 100644 --- a/vrx/vrx_gazebo/worlds/2022_phase2/stationkeeping2.world +++ b/vrx/vrx_gazebo/worlds/2022_phase2/stationkeeping2.world @@ -199,7 +199,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_phase2/stationkeeping3.world b/vrx/vrx_gazebo/worlds/2022_phase2/stationkeeping3.world index 4a1ada3..cd442c1 100644 --- a/vrx/vrx_gazebo/worlds/2022_phase2/stationkeeping3.world +++ b/vrx/vrx_gazebo/worlds/2022_phase2/stationkeeping3.world @@ -199,7 +199,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_phase2/stationkeeping4.world b/vrx/vrx_gazebo/worlds/2022_phase2/stationkeeping4.world index 4408a24..923d489 100644 --- a/vrx/vrx_gazebo/worlds/2022_phase2/stationkeeping4.world +++ b/vrx/vrx_gazebo/worlds/2022_phase2/stationkeeping4.world @@ -199,7 +199,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_phase2/stationkeeping5.world b/vrx/vrx_gazebo/worlds/2022_phase2/stationkeeping5.world index 0d8a8b2..86f05d8 100644 --- a/vrx/vrx_gazebo/worlds/2022_phase2/stationkeeping5.world +++ b/vrx/vrx_gazebo/worlds/2022_phase2/stationkeeping5.world @@ -199,7 +199,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_phase2/wayfinding0.world b/vrx/vrx_gazebo/worlds/2022_phase2/wayfinding0.world index 7cf4760..c85006b 100644 --- a/vrx/vrx_gazebo/worlds/2022_phase2/wayfinding0.world +++ b/vrx/vrx_gazebo/worlds/2022_phase2/wayfinding0.world @@ -209,7 +209,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_phase2/wayfinding1.world b/vrx/vrx_gazebo/worlds/2022_phase2/wayfinding1.world index 619b130..a0e7e80 100644 --- a/vrx/vrx_gazebo/worlds/2022_phase2/wayfinding1.world +++ b/vrx/vrx_gazebo/worlds/2022_phase2/wayfinding1.world @@ -212,7 +212,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_phase2/wayfinding2.world b/vrx/vrx_gazebo/worlds/2022_phase2/wayfinding2.world index de54a6c..63eb6a2 100644 --- a/vrx/vrx_gazebo/worlds/2022_phase2/wayfinding2.world +++ b/vrx/vrx_gazebo/worlds/2022_phase2/wayfinding2.world @@ -215,7 +215,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_phase2/wayfinding3.world b/vrx/vrx_gazebo/worlds/2022_phase2/wayfinding3.world index ce53b23..98b77c9 100644 --- a/vrx/vrx_gazebo/worlds/2022_phase2/wayfinding3.world +++ b/vrx/vrx_gazebo/worlds/2022_phase2/wayfinding3.world @@ -209,7 +209,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_phase2/wayfinding4.world b/vrx/vrx_gazebo/worlds/2022_phase2/wayfinding4.world index f9dc70a..34cdfb0 100644 --- a/vrx/vrx_gazebo/worlds/2022_phase2/wayfinding4.world +++ b/vrx/vrx_gazebo/worlds/2022_phase2/wayfinding4.world @@ -212,7 +212,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_phase2/wayfinding5.world b/vrx/vrx_gazebo/worlds/2022_phase2/wayfinding5.world index eb40707..d6b6b59 100644 --- a/vrx/vrx_gazebo/worlds/2022_phase2/wayfinding5.world +++ b/vrx/vrx_gazebo/worlds/2022_phase2/wayfinding5.world @@ -212,7 +212,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_phase2/wildlife0.world b/vrx/vrx_gazebo/worlds/2022_phase2/wildlife0.world index 8ddd87c..39c053c 100644 --- a/vrx/vrx_gazebo/worlds/2022_phase2/wildlife0.world +++ b/vrx/vrx_gazebo/worlds/2022_phase2/wildlife0.world @@ -172,7 +172,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_phase2/wildlife1.world b/vrx/vrx_gazebo/worlds/2022_phase2/wildlife1.world index 5a21b4b..72bf2e4 100644 --- a/vrx/vrx_gazebo/worlds/2022_phase2/wildlife1.world +++ b/vrx/vrx_gazebo/worlds/2022_phase2/wildlife1.world @@ -172,7 +172,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_phase2/wildlife2.world b/vrx/vrx_gazebo/worlds/2022_phase2/wildlife2.world index 8b0c01e..c7b1f2e 100644 --- a/vrx/vrx_gazebo/worlds/2022_phase2/wildlife2.world +++ b/vrx/vrx_gazebo/worlds/2022_phase2/wildlife2.world @@ -172,7 +172,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_phase2/wildlife3.world b/vrx/vrx_gazebo/worlds/2022_phase2/wildlife3.world index b37c982..8b981af 100644 --- a/vrx/vrx_gazebo/worlds/2022_phase2/wildlife3.world +++ b/vrx/vrx_gazebo/worlds/2022_phase2/wildlife3.world @@ -172,7 +172,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_phase2/wildlife4.world b/vrx/vrx_gazebo/worlds/2022_phase2/wildlife4.world index f0fdbd3..f4f6490 100644 --- a/vrx/vrx_gazebo/worlds/2022_phase2/wildlife4.world +++ b/vrx/vrx_gazebo/worlds/2022_phase2/wildlife4.world @@ -172,7 +172,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_phase2/wildlife5.world b/vrx/vrx_gazebo/worlds/2022_phase2/wildlife5.world index 313d2a1..2ffcb79 100644 --- a/vrx/vrx_gazebo/worlds/2022_phase2/wildlife5.world +++ b/vrx/vrx_gazebo/worlds/2022_phase2/wildlife5.world @@ -172,7 +172,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_practice/gymkhana0.world b/vrx/vrx_gazebo/worlds/2022_practice/gymkhana0.world index 57e4c13..3ca25bd 100644 --- a/vrx/vrx_gazebo/worlds/2022_practice/gymkhana0.world +++ b/vrx/vrx_gazebo/worlds/2022_practice/gymkhana0.world @@ -168,7 +168,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_practice/gymkhana1.world b/vrx/vrx_gazebo/worlds/2022_practice/gymkhana1.world index 01ab599..4ef1bb6 100644 --- a/vrx/vrx_gazebo/worlds/2022_practice/gymkhana1.world +++ b/vrx/vrx_gazebo/worlds/2022_practice/gymkhana1.world @@ -168,7 +168,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_practice/gymkhana2.world b/vrx/vrx_gazebo/worlds/2022_practice/gymkhana2.world index 4999b04..926d684 100644 --- a/vrx/vrx_gazebo/worlds/2022_practice/gymkhana2.world +++ b/vrx/vrx_gazebo/worlds/2022_practice/gymkhana2.world @@ -168,7 +168,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_practice/perception0.world b/vrx/vrx_gazebo/worlds/2022_practice/perception0.world index c1df604..d75ded6 100644 --- a/vrx/vrx_gazebo/worlds/2022_practice/perception0.world +++ b/vrx/vrx_gazebo/worlds/2022_practice/perception0.world @@ -168,7 +168,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_practice/perception1.world b/vrx/vrx_gazebo/worlds/2022_practice/perception1.world index c074792..557bf01 100644 --- a/vrx/vrx_gazebo/worlds/2022_practice/perception1.world +++ b/vrx/vrx_gazebo/worlds/2022_practice/perception1.world @@ -168,7 +168,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_practice/perception2.world b/vrx/vrx_gazebo/worlds/2022_practice/perception2.world index 6d969c3..245ed4c 100644 --- a/vrx/vrx_gazebo/worlds/2022_practice/perception2.world +++ b/vrx/vrx_gazebo/worlds/2022_practice/perception2.world @@ -168,7 +168,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_practice/scan_dock_deliver0.world b/vrx/vrx_gazebo/worlds/2022_practice/scan_dock_deliver0.world index 305c976..2f1788b 100644 --- a/vrx/vrx_gazebo/worlds/2022_practice/scan_dock_deliver0.world +++ b/vrx/vrx_gazebo/worlds/2022_practice/scan_dock_deliver0.world @@ -168,7 +168,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_practice/scan_dock_deliver1.world b/vrx/vrx_gazebo/worlds/2022_practice/scan_dock_deliver1.world index be1bbaa..b3bbfe7 100644 --- a/vrx/vrx_gazebo/worlds/2022_practice/scan_dock_deliver1.world +++ b/vrx/vrx_gazebo/worlds/2022_practice/scan_dock_deliver1.world @@ -168,7 +168,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_practice/scan_dock_deliver2.world b/vrx/vrx_gazebo/worlds/2022_practice/scan_dock_deliver2.world index 799ba5d..ec6dcc5 100644 --- a/vrx/vrx_gazebo/worlds/2022_practice/scan_dock_deliver2.world +++ b/vrx/vrx_gazebo/worlds/2022_practice/scan_dock_deliver2.world @@ -168,7 +168,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_practice/stationkeeping0.world b/vrx/vrx_gazebo/worlds/2022_practice/stationkeeping0.world index b804fbc..6242563 100644 --- a/vrx/vrx_gazebo/worlds/2022_practice/stationkeeping0.world +++ b/vrx/vrx_gazebo/worlds/2022_practice/stationkeeping0.world @@ -195,7 +195,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_practice/stationkeeping1.world b/vrx/vrx_gazebo/worlds/2022_practice/stationkeeping1.world index cdf9c4f..f339688 100644 --- a/vrx/vrx_gazebo/worlds/2022_practice/stationkeeping1.world +++ b/vrx/vrx_gazebo/worlds/2022_practice/stationkeeping1.world @@ -195,7 +195,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_practice/stationkeeping2.world b/vrx/vrx_gazebo/worlds/2022_practice/stationkeeping2.world index e92534e..2d68549 100644 --- a/vrx/vrx_gazebo/worlds/2022_practice/stationkeeping2.world +++ b/vrx/vrx_gazebo/worlds/2022_practice/stationkeeping2.world @@ -195,7 +195,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_practice/wayfinding0.world b/vrx/vrx_gazebo/worlds/2022_practice/wayfinding0.world index a8045bc..ddd9697 100644 --- a/vrx/vrx_gazebo/worlds/2022_practice/wayfinding0.world +++ b/vrx/vrx_gazebo/worlds/2022_practice/wayfinding0.world @@ -205,7 +205,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_practice/wayfinding1.world b/vrx/vrx_gazebo/worlds/2022_practice/wayfinding1.world index 4f8bd94..5c761b8 100644 --- a/vrx/vrx_gazebo/worlds/2022_practice/wayfinding1.world +++ b/vrx/vrx_gazebo/worlds/2022_practice/wayfinding1.world @@ -208,7 +208,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_practice/wayfinding2.world b/vrx/vrx_gazebo/worlds/2022_practice/wayfinding2.world index 44301fa..d4143a9 100644 --- a/vrx/vrx_gazebo/worlds/2022_practice/wayfinding2.world +++ b/vrx/vrx_gazebo/worlds/2022_practice/wayfinding2.world @@ -211,7 +211,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_practice/wildlife0.world b/vrx/vrx_gazebo/worlds/2022_practice/wildlife0.world index fc921f4..bef10ac 100644 --- a/vrx/vrx_gazebo/worlds/2022_practice/wildlife0.world +++ b/vrx/vrx_gazebo/worlds/2022_practice/wildlife0.world @@ -168,7 +168,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_practice/wildlife1.world b/vrx/vrx_gazebo/worlds/2022_practice/wildlife1.world index dce2e97..277040e 100644 --- a/vrx/vrx_gazebo/worlds/2022_practice/wildlife1.world +++ b/vrx/vrx_gazebo/worlds/2022_practice/wildlife1.world @@ -168,7 +168,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/2022_practice/wildlife2.world b/vrx/vrx_gazebo/worlds/2022_practice/wildlife2.world index 4d1c810..e571b20 100644 --- a/vrx/vrx_gazebo/worlds/2022_practice/wildlife2.world +++ b/vrx/vrx_gazebo/worlds/2022_practice/wildlife2.world @@ -168,7 +168,6 @@ <topic_wind_direction>/vrx/debug/wind/direction</topic_wind_direction> </plugin> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> diff --git a/vrx/vrx_gazebo/worlds/stationkeeping_task.world.xacro b/vrx/vrx_gazebo/worlds/stationkeeping_task.world.xacro index a1b774b..5bb7144 100644 --- a/vrx/vrx_gazebo/worlds/stationkeeping_task.world.xacro +++ b/vrx/vrx_gazebo/worlds/stationkeeping_task.world.xacro @@ -24,7 +24,8 @@ </wind_objs> </xacro:usv_wind_gazebo> - <plugin name="stationkeeping_scoring_plugin" + + <plugin name="stationkeeping_scoring_plugin" filename="libstationkeeping_scoring_plugin.so"> <vehicle>wamv</vehicle> <task_name>station_keeping</task_name> diff --git a/vrx/vrx_gazebo/worlds/sydneyregatta.xacro b/vrx/vrx_gazebo/worlds/sydneyregatta.xacro index 62f097f..ec7fb0f 100644 --- a/vrx/vrx_gazebo/worlds/sydneyregatta.xacro +++ b/vrx/vrx_gazebo/worlds/sydneyregatta.xacro @@ -3,7 +3,6 @@ <world xmlns:xacro="http://ros.org/wiki/xacro"> <xacro:macro name="sydneyregatta"> <scene> - <shadows>0</shadows> <sky> <clouds> <speed>12</speed> @@ -12,7 +11,6 @@ <grid>0</grid> <origin_visual>0</origin_visual> </scene> - <!-- Estimated latitude/longitude of sydneyregatta from satellite imagery --> <spherical_coordinates> diff --git a/vrx/vrx_gazebo/worlds/wildlife_task.world.xacro b/vrx/vrx_gazebo/worlds/wildlife_task.world.xacro index ed9bd48..d05f2d4 100644 --- a/vrx/vrx_gazebo/worlds/wildlife_task.world.xacro +++ b/vrx/vrx_gazebo/worlds/wildlife_task.world.xacro @@ -19,6 +19,7 @@ </wind_objs> </xacro:usv_wind_gazebo> + <!-- The VRX animal buoys --> <include> <name>crocodile_buoy</name>