diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index b309cc5..808cbb3 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -13,6 +13,7 @@ RUN apt-get update \ && apt-get install -y ssh \ python3-pip \ curl \ + rsync \ && rm -rf /var/lib/apt/lists/* RUN echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME \ @@ -26,7 +27,7 @@ RUN mkdir -p /home/ws/build /home/ws/install /home/ws/log \ RUN ( \ echo "source /opt/ros/iron/setup.bash"; \ echo "source /home/ws/install/setup.bash"; \ - echo "source /home/ws/src/openmower/config/sim-config.env.sh"; \ + echo "source /home/ws/.devcontainer/openmower_config.env"; \ echo "export DISPLAY=:0"; \ ) >> /home/$USERNAME/.bashrc @@ -38,4 +39,11 @@ RUN ( \ RUN sed -i 's/#Port 22/Port 2222/' /etc/ssh/sshd_config RUN mkdir /run/sshd -CMD ["/usr/sbin/sshd", "-D", "-e", "-f", "/etc/ssh/sshd_config"] +COPY entrypoint.sh /entrypoint.sh +RUN chmod +x /entrypoint.sh + +USER $USERNAME + +ENTRYPOINT ["/entrypoint.sh"] + +CMD ["/usr/bin/sudo", "/usr/sbin/sshd", "-D", "-e", "-f", "/etc/ssh/sshd_config"] diff --git a/.devcontainer/Makefile b/.devcontainer/Makefile new file mode 100644 index 0000000..054f63c --- /dev/null +++ b/.devcontainer/Makefile @@ -0,0 +1,5 @@ +groot-build: + docker buildx build --push --platform linux/arm64/v8,linux/amd64 --tag jkaflik/behaviortree-groot:latest -f groot.Dockerfile . + +groot-publish: + docker push jkaflik/behaviortree-groot:latest diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 04609e5..62b6e97 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -19,6 +19,5 @@ ] } }, - "containerUser": "openmower", - "postCreateCommand": "sudo apt update && rosdep update && make custom-deps deps" + "containerUser": "openmower" } \ No newline at end of file diff --git a/.devcontainer/docker-compose.yaml b/.devcontainer/docker-compose.yaml index fb5eee8..5a17634 100644 --- a/.devcontainer/docker-compose.yaml +++ b/.devcontainer/docker-compose.yaml @@ -24,6 +24,8 @@ services: - "8765:8765" # Foxglove Studio websocket - "9002:9002" # Gazebo websocket volumes: + - ./home:/home/openmower/.openmower + - /opt/ros - ../:/home/ws - build:/home/ws/build # keep container pre-build - install:/home/ws/install # keep container pre-build @@ -34,6 +36,26 @@ services: - xserver depends_on: - xserver + groot: + profiles: + - with_groot + image: jkaflik/behaviortree-groot:latest + ipc: host + pid: host + security_opt: + - seccomp:unconfined + cap_add: + - SYS_PTRACE + volumes: + - ../:/home/ws + environment: + - DISPLAY=:0 + volumes_from: + - xserver + - workspace + depends_on: + - xserver + - workspace volumes: build: install: diff --git a/.devcontainer/entrypoint.sh b/.devcontainer/entrypoint.sh new file mode 100644 index 0000000..7464287 --- /dev/null +++ b/.devcontainer/entrypoint.sh @@ -0,0 +1,15 @@ +#!/usr/bin/env bash +set -e + +if [ ! -d "/home/ws" ]; then + echo "ERROR: /home/ws does not exist. Please mount your workspace to /home/ws" + exit 1 +fi + +cd /home/ws + +sudo apt update +rosdep update +make custom-deps deps + +exec "$@" diff --git a/.devcontainer/groot.Dockerfile b/.devcontainer/groot.Dockerfile new file mode 100644 index 0000000..e3f748d --- /dev/null +++ b/.devcontainer/groot.Dockerfile @@ -0,0 +1,36 @@ +FROM ubuntu:jammy + +ENV DEBIAN_FRONTEND=noninteractive +RUN apt-get update && apt-get install -y -q \ + git \ + build-essential \ + cmake \ + python3 \ + qtbase5-dev \ + libqt5svg5-dev \ + libzmq3-dev \ + libdw-dev \ + libtinfo-dev \ + libncurses5-dev \ + libncursesw5-dev + +RUN git clone https://github.com/BehaviorTree/Groot.git &&\ + cd Groot &&\ + git submodule update --init --recursive &&\ + cd depend/BehaviorTree.CPP &&\ + mkdir build ; cd build &&\ + cmake .. &&\ + make &&\ + make install &&\ + cd /Groot &&\ + mkdir build; cd build &&\ + cmake .. &&\ + make &&\ + # add alias for Groot GUI (simply type "Groot" in terminal) + echo 'alias Groot="cd /Groot/build && ./Groot"' >> ~/.bashrc + +# fixes: "error while loading shared libraries: libbehaviortree_cpp_v3.so: cannot open shared object" +RUN ldconfig + +WORKDIR /Groot/build +CMD ./Groot diff --git a/.devcontainer/home/map.geojson b/.devcontainer/home/map.geojson new file mode 100644 index 0000000..3361e66 --- /dev/null +++ b/.devcontainer/home/map.geojson @@ -0,0 +1,237 @@ +{ + "type": "FeatureCollection", + "features": [ + { + "type": "Feature", + "properties": { + "id": "0859c0c1-ec2d-4197-96b7-2d12f6cf1cd9", + "name": "backyard", + "type": "lawn", + "fill": "#00ff00" + }, + "geometry": { + "coordinates": [ + [ + [ + -43.20007309019093, + -22.899998069679626 + ], + [ + -43.20006688758278, + -22.900006871946417 + ], + [ + -43.200002346928954, + -22.899972743856168 + ], + [ + -43.19999715014916, + -22.899979847441557 + ], + [ + -43.200002011652515, + -22.899984943491773 + ], + [ + -43.20000352039554, + -22.899987877581054 + ], + [ + -43.200001508737785, + -22.89999529001645 + ], + [ + -43.19999882652931, + -22.899998687383203 + ], + [ + -43.199989774073345, + -22.900001003768594 + ], + [ + -43.19998390674169, + -22.900001621472214 + ], + [ + -43.199976027752285, + -22.89999992278834 + ], + [ + -43.199966304744464, + -22.899997143125205 + ], + [ + -43.19991584568825, + -22.89996363273525 + ], + [ + -43.199949708576725, + -22.89991792269754 + ], + [ + -43.20007309019093, + -22.899998069679626 + ] + ] + ], + "type": "Polygon" + }, + "id": 0 + }, + { + "type": "Feature", + "properties": { + "id": "6387f8e9-bc4e-48a5-95e2-fedd3b3b5c09", + "name": "driveway", + "type": "navigation", + "fill": "#ffffff" + }, + "geometry": { + "coordinates": [ + [ + [ + -43.19991601332646, + -22.89996363273525 + ], + [ + -43.19996664002096, + -22.899996988699343 + ], + [ + -43.19995825811796, + -22.90000887948092 + ], + [ + -43.19990629031864, + -22.8999759867976 + ], + [ + -43.19991601332646, + -22.89996363273525 + ] + ] + ], + "type": "Polygon" + }, + "id": 1 + }, + { + "type": "Feature", + "properties": { + "id": "a7a3e913-5e9e-4959-8044-059f40356996", + "type": "lawn", + "fill": "#00ff00" + }, + "geometry": { + "coordinates": [ + [ + [ + -43.19990654164289, + -22.899975703751664 + ], + [ + -43.19995834180392, + -22.900008596436095 + ], + [ + -43.19996638843159, + -22.899996860080364 + ], + [ + -43.199998742577066, + -22.899998713189134 + ], + [ + -43.20000008368183, + -22.899983116188707 + ], + [ + -43.200020703164, + -22.90003299569868 + ], + [ + -43.20000259825318, + -22.900055850700596 + ], + [ + -43.199891286578776, + -22.899990528624386 + ], + [ + -43.19990654164289, + -22.899975703751664 + ] + ] + ], + "type": "Polygon" + }, + "id": 2 + }, + { + "type": "Feature", + "properties": { + "fill": "#ff0000", + "type": "obstacle", + "name": "tree", + "id": "90cd67cb-a606-4878-be1c-2c08f8fc085d" + }, + "geometry": { + "coordinates": [ + [ + [ + -43.19996538260324, + -22.899949451369395 + ], + [ + -43.199974435058095, + -22.899951304478776 + ], + [ + -43.199975776162916, + -22.899958871343117 + ], + [ + -43.199966891345184, + -22.899964585096825 + ], + [ + -43.1999563301473, + -22.899959180193974 + ], + [ + -43.19995666542377, + -22.899953312014105 + ], + [ + -43.19996538260324, + -22.899949451369395 + ] + ] + ], + "type": "Polygon" + }, + "id": 3 + }, + { + "type": "Feature", + "properties": { + "id": "0706aa36-bf67-4831-9472-be2c4275815d", + "type": "docking_station", + "name": "First docking station" + }, + "geometry": { + "coordinates": [ + [ + -43.1999118467661, + -22.899969157137434 + ], + [ + -43.19991519952754, + -22.89997131909847 + ] + ], + "type": "LineString" + } + } + ] +} \ No newline at end of file diff --git a/src/openmower/config/sim-config.env.sh b/.devcontainer/openmower_config.env similarity index 53% rename from src/openmower/config/sim-config.env.sh rename to .devcontainer/openmower_config.env index 8aed56b..349d53d 100644 --- a/src/openmower/config/sim-config.env.sh +++ b/.devcontainer/openmower_config.env @@ -1,2 +1,3 @@ export OM_DATUM_LAT=-22.9 export OM_DATUM_LONG=-43.2 +export OM_MAP_PATH=$HOME/.openmower/map.geojson \ No newline at end of file diff --git a/Makefile b/Makefile index 164dfbd..431e067 100644 --- a/Makefile +++ b/Makefile @@ -25,6 +25,9 @@ sim: run: ros2 launch src/openmower/launch/openmower.launch.py +dev: + cd .devcontainer && docker-compose up -d + run-realsense: ros2 launch realsense2_camera rs_launch.py initial_reset:=true pointcloud.enable:=true publish_tf:=false enable_infra1:=true enable_depth:=true enable_gyro:=true enable_accel:=true unite_imu_method:=1 diff --git a/docs/.vitepress/config.mts b/docs/.vitepress/config.mts index e41a71f..42a3bf1 100644 --- a/docs/.vitepress/config.mts +++ b/docs/.vitepress/config.mts @@ -34,6 +34,7 @@ export default withMermaid({ { text: 'ROS workspace', link: '/ros-workspace' }, { text: 'Mainboard firmware', link: '/omros2-firmware' }, { text: 'Robot localization', link: '/localization' }, + { text: 'Map management', link: '/map-management' }, ] }, { diff --git a/docs/devcontainer.md b/docs/devcontainer.md index 5b8a092..1daf45d 100644 --- a/docs/devcontainer.md +++ b/docs/devcontainer.md @@ -25,9 +25,10 @@ For CLion users there is an alternative approach described in [CLion development ## Detailed -Devcontainer comes up with two containers configured with Docker Compose: +Devcontainer comes up with some containers configured with Docker Compose: - `workspace` - main container with all the tools and dependencies and mounted workspace - `xserver` - container with X server and VNC server for GUI applications +- `groot` - container with groot, a GUI for [BehaviourTree.CPP](https://www.behaviortree.dev/). It does not start by default. See [GRoot](./groot.md) for more details. -Both containers share the same X server socket, so GUI applications can be run from the `workspace` container and displayed in the `xserver` container. +All containers share the same X server socket, so GUI applications can be run from the `workspace` container and displayed in the `xserver` container. VNC server in `xserver` runs a web server on port `12345` with a VNC client. You can access it by opening [`http://localhost:12345`](http://localhost:12345) in your browser. diff --git a/docs/getting-started.md b/docs/getting-started.md index 17334cc..4aa288b 100644 --- a/docs/getting-started.md +++ b/docs/getting-started.md @@ -30,6 +30,8 @@ This section is not complete yet. It will be updated as the project progresses. - :white_check_mark: IMU - :white_check_mark: GPS - :construction: Simulation using Gazebo +- :construction: [Map management](https://github.com/jkaflik/OpenMowerROS2/issues/19) +- :pencil2: [Mower logic](https://github.com/jkaflik/OpenMowerROS2/issues/9) ### Roadmap diff --git a/docs/groot.md b/docs/groot.md new file mode 100644 index 0000000..55557f9 --- /dev/null +++ b/docs/groot.md @@ -0,0 +1,29 @@ +--- +title: Working with GRoot +--- + +# {{ $frontmatter.title }} + +## Overview + +GRoot is a GUI for [BehaviourTree.CPP](https://www.behaviortree.dev/). It is a great tool for debugging and visualizing behaviour trees. + +![GRoot](https://raw.githubusercontent.com/BehaviorTree/Groot/master/groot-screenshot.png) + +::: warning +BehaviourTree.CPP is not yet implemented in this project. See [issue](https://github.com/jkaflik/OpenMowerROS2/issues/9). +::: + +## Run GRoot + +Project comes with a prebuilt GRoot image and dedicated docker compose declaration. It does not run by default. To run it, use the following command: + +```bash +$ cd .devcontainer && docker compose up groot +``` + +You can access it by opening [VNC client `http://localhost:12345`](http://localhost:12345) in your browser. + +::: tip +Learn more about [VNC client](devcontainer#detailed). +::: diff --git a/docs/map-management.md b/docs/map-management.md new file mode 100644 index 0000000..b5ee000 --- /dev/null +++ b/docs/map-management.md @@ -0,0 +1,46 @@ +--- +title: Map management +--- +# {{ $frontmatter.title }} + +## Overview + +The map management is a ROS package responsible for managing map. It is a drop-in replacement for [nav2 map server](https://navigation.ros.org/configuration/packages/configuring-map-server.html). + +Map provides a static 2D environment representation used for robot navigation. +The main concept is to have a map that consists of a set of areas. Map is created by combining multiple areas together. Obstacle areas are decoupled from navigation areas and can be added or removed at any time. + +The second concept is a pose of docking station. It consists of a position and orientation. Position is a middle of the charging connectors. Orientation heading towards the robot's connectors. + +### ROS message definition + +<<< ../src/open_mower_map_server/msg/Map.msg + +## Area + +Each area is a polygon that can be either free to navigate or occupied by an obstacle. It's uniquely identified by an ID. +Area can be added, removed or updated at any time. + +Area name is used to identify the area. It's not required to be unique, but it's recommended to use unique names to avoid confusion. + +### Types + +- **Exclusion area** is an area that is not allowed to be occupied by the robot. It is used to exclude areas that are not safe for the robot to drive on. For example, a pond or a flower bed. +- **Navigation area** is an area that is allowed to be occupied by the robot. It is used to define the area where the robot is allowed to drive on. For example, a driveway. +- **Lawn area** is an area that is allowed to be occupied by the robot. It is used to define the area where the robot is allowed to drive on and execute mowing pattern. + +### ROS message definition + +<<< ../src/open_mower_map_server/msg/Area.msg + + +## Docking station + +Docking station is a position where the robot can charge its batteries. It's uniquely identified by an ID. +Docking station can be added, removed or updated at any time. + +Pose is a position and orientation of the docking station. Position is a middle of the charging connectors. Orientation heading towards the robot's connectors. + +### ROS message definition + +<<< ../src/open_mower_map_server/msg/DockingStation.msg diff --git a/docs/omros2-firmware.md b/docs/omros2-firmware.md index c87534a..9be16ab 100644 --- a/docs/omros2-firmware.md +++ b/docs/omros2-firmware.md @@ -18,6 +18,7 @@ This firmware supports only the recent OpenMower mainboard v0.13.x. * :white_check_mark: `std_msgs/Bool` message published on `/power/charger_present` topic * :white_check_mark: OpenMower charging logic * :white_check_mark: ping micro-ROS agent, if not responding for some time, reboot +* :red_circle: safety features ## Flash diff --git a/docs/ros-workspace.md b/docs/ros-workspace.md index c684499..8c27033 100644 --- a/docs/ros-workspace.md +++ b/docs/ros-workspace.md @@ -7,6 +7,14 @@ It's no different for OpenMowerROS2. The root of the repository is a ROS workspa ## Main packages +### Map management + +The map management is a ROS package responsible for managing map. It is a drop-in replacement for [nav2 map server](https://navigation.ros.org/configuration/packages/configuring-map-server.html). + +More information can be found in [map management](map-management.md) section. + +## External packages + There are few biggest chunks to mention: - [ros2_control](https://control.ros.org/master/index.html) - differential drive controller, but also a hardware layer provider diff --git a/docs/simulator.md b/docs/simulator.md index 04d057e..a7bd846 100644 --- a/docs/simulator.md +++ b/docs/simulator.md @@ -38,7 +38,7 @@ It's possible to run the simulator with GUI on external host machine. I haven't - :white_check_mark: GPS sensor - :white_check_mark: IMU sensor - :white_check_mark: can of Coke -- :construction: Emulate OpenMowerROS2 firmware +- :construction: [Emulate OpenMowerROS2 firmware](https://github.com/jkaflik/OpenMowerROS2/issues/8) ## World definition diff --git a/src/.gitignore b/src/.gitignore index ab7b72e..d7716c9 100644 --- a/src/.gitignore +++ b/src/.gitignore @@ -1,3 +1,4 @@ /nav2_bringup /vesc -/transport_drivers \ No newline at end of file +/transport_drivers +src/lib/** \ No newline at end of file diff --git a/src/open_mower_map_server/CMakeLists.txt b/src/open_mower_map_server/CMakeLists.txt new file mode 100644 index 0000000..cba5242 --- /dev/null +++ b/src/open_mower_map_server/CMakeLists.txt @@ -0,0 +1,79 @@ +cmake_minimum_required(VERSION 3.8) +project(open_mower_map_server) + +if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif () + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +find_package(rclcpp REQUIRED) +find_package(nlohmann_json REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(robot_localization REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) + +add_executable(map_server_node + src/node_main.cpp + include/open_mower_map_server/map_server_node.hpp + src/map_server_node.cpp + src/geo_json_map.cpp + include/open_mower_map_server/geo_json_map.hpp + include/open_mower_map_server/polygon_iterator.hpp + include/open_mower_map_server/some_gaussian_filter.hpp + include/open_mower_map_server/polygon_utils.hpp + include/open_mower_map_server/polygon_utils.hpp) +target_include_directories(map_server_node PUBLIC + $ + $) +target_compile_features(map_server_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/Area.msg" + "msg/Map.msg" + "msg/DockingStation.msg" + DEPENDENCIES + std_msgs + geometry_msgs + nav_msgs + tf2 + tf2_geometry_msgs +) + +ament_target_dependencies(map_server_node + std_msgs + geometry_msgs + nav_msgs + rclcpp + robot_localization + tf2 + tf2_geometry_msgs +) + +rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") + +target_link_libraries(map_server_node "${cpp_typesupport_target}") +target_link_libraries(map_server_node nlohmann_json::nlohmann_json) + +install(TARGETS map_server_node + DESTINATION lib/${PROJECT_NAME}) + +if (BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif () + +ament_package() diff --git a/src/open_mower_map_server/include/open_mower_map_server/geo_json_map.hpp b/src/open_mower_map_server/include/open_mower_map_server/geo_json_map.hpp new file mode 100644 index 0000000..250ae6e --- /dev/null +++ b/src/open_mower_map_server/include/open_mower_map_server/geo_json_map.hpp @@ -0,0 +1,39 @@ +#pragma once + +#include +#include +#include + +#include "map_server_node.hpp" + + +using json = nlohmann::json; + +namespace open_mower_map_server { + class GeoJSONMap : public MapIO { + public: + explicit GeoJSONMap(std::string path, MapServerNode::SharedPtr map_server_node); + void save(msg::Map map); + msg::Map load(); + + private: + void parsePolygonFeature(msg::Map &map, const json &feature); + geometry_msgs::msg::Point32 parsePoint(json::const_reference value) const; + geometry_msgs::msg::PoseStamped calculateTwoPointsPose(geometry_msgs::msg::Point32 origin, geometry_msgs::msg::Point32 point32); + void parseLineStringFeature(msg::Map &map, const json &feature); + json pointToCoordinates(const geometry_msgs::msg::Point& point) const; + json pointToCoordinates(const geometry_msgs::msg::Point32& point) const; + json mapAreaToGeoJSONFeature(const msg::Area& area); + geometry_msgs::msg::Point movePointTowardsOrientation(const geometry_msgs::msg::Point& point, + const geometry_msgs::msg::Quaternion& quaternion, + double x) const; + json dockingStationToGeoJSONFeature(const msg::DockingStation& docking_station); + + MapServerNode::SharedPtr node_; + rclcpp::Client::SharedPtr from_ll_client_; + rclcpp::Client::SharedPtr to_ll_client_; + + std::string path_; + }; + +} // OpenMowerMapServer diff --git a/src/open_mower_map_server/include/open_mower_map_server/map_server_node.hpp b/src/open_mower_map_server/include/open_mower_map_server/map_server_node.hpp new file mode 100644 index 0000000..d382fe6 --- /dev/null +++ b/src/open_mower_map_server/include/open_mower_map_server/map_server_node.hpp @@ -0,0 +1,52 @@ +#pragma once + +#include +#include + +#include "open_mower_map_server/msg/map.hpp" +#include "open_mower_map_server/some_gaussian_filter.hpp" + +namespace open_mower_map_server { + class MapIO { + public: + virtual ~MapIO() = default; + + virtual void save(open_mower_map_server::msg::Map map) = 0; + + virtual open_mower_map_server::msg::Map load() = 0; + }; + + class MapServerNode final : public rclcpp::Node { + public: + void publishMap(); + explicit MapServerNode(const rclcpp::NodeOptions &options); + + ~MapServerNode() override = default; + + private: + std::vector areasWithExclusionsLast(std::vector areas); + nav_msgs::msg::OccupancyGrid mapToOccupancyGrid(open_mower_map_server::msg::Map map); + + MapIO *map_io_; + + void fillGridWithPolygon(nav_msgs::msg::OccupancyGrid &occupancy_grid, + const geometry_msgs::msg::Polygon &polygon, + uint8_t value); + + // parameters + std::string map_type_; + std::string map_file_; + bool use_gaussian_blur_; + + void configureGaussianBlur(); + + std::shared_ptr param_subscriber_; + + void configureMap(); + + SomeGaussianFilter *gaussian_filter_; + open_mower_map_server::msg::Map current_map_; + rclcpp::Publisher::SharedPtr occupancy_grid_publisher_; + rclcpp::Publisher::SharedPtr map_publisher_; + }; +} diff --git a/src/open_mower_map_server/include/open_mower_map_server/polygon_iterator.hpp b/src/open_mower_map_server/include/open_mower_map_server/polygon_iterator.hpp new file mode 100644 index 0000000..9c47e54 --- /dev/null +++ b/src/open_mower_map_server/include/open_mower_map_server/polygon_iterator.hpp @@ -0,0 +1,105 @@ +#pragma once + +#include +#include +#include + +namespace open_mower_map_server { + + class PolygonGridIterator { + public: + PolygonGridIterator(const geometry_msgs::msg::Polygon& polygon, double resolution) + : polygon_(polygon), resolution_(resolution) { + minimumBoundingRectangle(); + + // Initialize x_, y_ to start from the bottom-left corner of the MBR + x_ = min_x_; + y_ = min_y_; + } + + void minimumBoundingRectangle() { + float min_x = std::numeric_limits::infinity(); + float min_y = std::numeric_limits::infinity(); + float max_x = -std::numeric_limits::infinity(); + float max_y = -std::numeric_limits::infinity(); + + for (const auto& point : polygon_.points) { + if (point.x < min_x) min_x = point.x; + if (point.x > max_x) max_x = point.x; + if (point.y < min_y) min_y = point.y; + if (point.y > max_y) max_y = point.y; + } + + min_x_ = min_x; + min_y_ = min_y; + max_x_ = max_x; + max_y_ = max_y; + } + + PolygonGridIterator(const PolygonGridIterator& other) + : polygon_(other.polygon_), resolution_(other.resolution_), x_(other.x_), y_(other.y_) {} + + bool next() { + while (x_ <= max_x_ && y_ <= max_y_) { + x_ += resolution_; + if (x_ > max_x_) { + x_ = min_x_; + y_++; + } + + if (isPointInsidePolygon(x_, y_, polygon_.points)) { + return true; + } + } + + return false; + } + + bool operator==(const PolygonGridIterator& other) const { + return polygon_ == other.polygon_ && resolution_ == other.resolution_ && x_ == other.x_ && y_ == other.y_; + } + + bool operator!=(const PolygonGridIterator& other) const { + return !(*this == other); + } + + const geometry_msgs::msg::Point operator*() const { + auto p = geometry_msgs::msg::Point(); + p.x = x_; + p.y = y_; + p.z = 0; + return p; + } + + private: + bool isPointInsidePolygon(float x, float y, const std::vector& vertices) { + bool isInside = false; + int n = vertices.size(); + + // Loop through all edges of the polygon + for (int i = 0, j = n - 1; i < n; j = i++) { + float xi = vertices[i].x, yi = vertices[i].y; + float xj = vertices[j].x, yj = vertices[j].y; + + // Check if the point is in one of the edges + if (((yi > y) != (yj > y)) && + (x < (xj - xi) * (y - yi) / (yj - yi) + xi)) { + isInside = !isInside; + } + } + + return isInside; + } + + const geometry_msgs::msg::Polygon& polygon_; + double resolution_; + + double x_; + double y_; + float min_x_; + float min_y_; + float max_x_; + float max_y_; + }; + +} diff --git a/src/open_mower_map_server/include/open_mower_map_server/polygon_utils.hpp b/src/open_mower_map_server/include/open_mower_map_server/polygon_utils.hpp new file mode 100644 index 0000000..018c2ef --- /dev/null +++ b/src/open_mower_map_server/include/open_mower_map_server/polygon_utils.hpp @@ -0,0 +1,18 @@ +#pragma once + +#include + +inline double polygonArea(const geometry_msgs::msg::Polygon& polygon) +{ + double area = 0; + + for (size_t i = 0; i < polygon.points.size(); i++) + { + const auto& p1 = polygon.points[i]; + const auto& p2 = polygon.points[(i + 1) % polygon.points.size()]; + + area += (p1.x * p2.y - p2.x * p1.y); + } + + return area / 2; +} \ No newline at end of file diff --git a/src/open_mower_map_server/include/open_mower_map_server/some_gaussian_filter.hpp b/src/open_mower_map_server/include/open_mower_map_server/some_gaussian_filter.hpp new file mode 100644 index 0000000..3acb2cb --- /dev/null +++ b/src/open_mower_map_server/include/open_mower_map_server/some_gaussian_filter.hpp @@ -0,0 +1,46 @@ +#pragma once + +#include +#include +#include +#include +#include + +class SomeGaussianFilter { +private: + std::vector kernel_; + int kernel_size_; + int half_kernel_size_; + +public: + SomeGaussianFilter(const std::vector& kernel) : kernel_(kernel) { + const int length = kernel_.size(); + kernel_size_ = std::sqrt(length); + if (kernel_size_ * kernel_size_ != length) { + throw std::invalid_argument("Invalid kernel size: Kernel must form a square matrix"); + } + + half_kernel_size_ = kernel_size_ / 2; + } + + void apply(std::vector> &image, int width, int height) const { + // Create an output vector to store the result + std::vector> buffer(width * height); + + // Apply convolution + for (int x = half_kernel_size_; x < width - half_kernel_size_; ++x) { + for (int y = half_kernel_size_; y < height - half_kernel_size_; ++y) { + float newValue = 0; + for (int i = -half_kernel_size_; i <= half_kernel_size_; ++i) { + for (int j = -half_kernel_size_; j <= half_kernel_size_; ++j) { + float kernelValue = kernel_[(j + half_kernel_size_) * kernel_size_ + (i + half_kernel_size_)]; + newValue += image[(y + j) * width + (x + i)] * kernelValue; + } + } + buffer[y * width + x] = std::round(newValue); + } + } + + std::move(buffer.begin(), buffer.end(), image.begin()); + } +}; diff --git a/src/open_mower_map_server/msg/Area.msg b/src/open_mower_map_server/msg/Area.msg new file mode 100644 index 0000000..284f881 --- /dev/null +++ b/src/open_mower_map_server/msg/Area.msg @@ -0,0 +1,8 @@ +uint8 TYPE_EXCLUSION = 0 +uint8 TYPE_NAVIGATION = 1 +uint8 TYPE_LAWN = 2 + +string id # id of the zone +string name # name of the zone +uint8 type # type of the area. 0 = obstacle, 1 = navigation, 1 = lawn +geometry_msgs/PolygonStamped area # area of the zone diff --git a/src/open_mower_map_server/msg/DockingStation.msg b/src/open_mower_map_server/msg/DockingStation.msg new file mode 100644 index 0000000..8f1c012 --- /dev/null +++ b/src/open_mower_map_server/msg/DockingStation.msg @@ -0,0 +1,3 @@ +string id # id of the docking station +string name # name of the docking station +geometry_msgs/PoseStamped pose # Usually a middle of the charging connectors. Orientation heading towards the robot's connectors. \ No newline at end of file diff --git a/src/open_mower_map_server/msg/Map.msg b/src/open_mower_map_server/msg/Map.msg new file mode 100644 index 0000000..fb207a5 --- /dev/null +++ b/src/open_mower_map_server/msg/Map.msg @@ -0,0 +1,3 @@ +std_msgs/Header header +Area[] areas +DockingStation[] docking_stations diff --git a/src/open_mower_map_server/package.xml b/src/open_mower_map_server/package.xml new file mode 100644 index 0000000..8d518ff --- /dev/null +++ b/src/open_mower_map_server/package.xml @@ -0,0 +1,28 @@ + + + + open_mower_map_server + 0.0.0 + TODO: Package description + openmower + TODO: License declaration + + geometry_msgs + nav_msgs + tf2 + + nlohmann_json_schema_validator_vendor + + ament_cmake + + ament_lint_auto + ament_lint_common + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + diff --git a/src/open_mower_map_server/src/geo_json_map.cpp b/src/open_mower_map_server/src/geo_json_map.cpp new file mode 100644 index 0000000..c993b35 --- /dev/null +++ b/src/open_mower_map_server/src/geo_json_map.cpp @@ -0,0 +1,305 @@ +#include "open_mower_map_server/geo_json_map.hpp" +#include +#include + +namespace open_mower_map_server +{ + GeoJSONMap::GeoJSONMap(std::string path, MapServerNode::SharedPtr map_server_node) + : path_(path), node_(map_server_node) + { + if (path_.empty()) + { + throw std::invalid_argument("GeoJSON file path cannot be empty"); + } + + from_ll_client_ = node_->create_client("fromLL"); + while (!from_ll_client_->wait_for_service(std::chrono::seconds(1))) + { + if (!rclcpp::ok()) + { + RCLCPP_ERROR(node_->get_logger(), + "Interrupted while waiting for the robot localization /fromLL service. Exiting."); + rclcpp::shutdown(); + } + + RCLCPP_INFO(node_->get_logger(), "service not available, waiting for the robot localization /fromLL..."); + } + + to_ll_client_ = node_->create_client("toLL"); + while (!to_ll_client_->wait_for_service(std::chrono::seconds(1))) + { + if (!rclcpp::ok()) + { + RCLCPP_ERROR(node_->get_logger(), + "Interrupted while waiting for the robot localization /toLL service. Exiting."); + rclcpp::shutdown(); + } + + RCLCPP_INFO(node_->get_logger(), "service not available, waiting for the robot localization /toLL..."); + } + } + + json GeoJSONMap::pointToCoordinates(const geometry_msgs::msg::Point& point) const + { + auto request = std::make_shared(); + request->map_point.x = point.x; + request->map_point.y = point.y; + auto result = to_ll_client_->async_send_request(request); + if (rclcpp::spin_until_future_complete(node_, result, std::chrono::seconds(10)) != + rclcpp::FutureReturnCode::SUCCESS) + { + throw std::runtime_error("Could not convert map coordinates to LL"); + } + + auto v = *result.get(); + json p = json::array(); + p.push_back(v.ll_point.longitude); + p.push_back(v.ll_point.latitude); + + return p; + } + + json GeoJSONMap::pointToCoordinates(const geometry_msgs::msg::Point32& point) const + { + auto request = std::make_shared(); + request->map_point.x = point.x; + request->map_point.y = point.y; + auto result = to_ll_client_->async_send_request(request); + if (rclcpp::spin_until_future_complete(node_, result, std::chrono::seconds(1)) != + rclcpp::FutureReturnCode::SUCCESS) + { + throw std::runtime_error("Could not convert map coordinates to LL"); + } + + auto v = *result.get(); + json p = json::array(); + p.push_back(v.ll_point.longitude); + p.push_back(v.ll_point.latitude); + + return p; + } + + json GeoJSONMap::mapAreaToGeoJSONFeature(const msg::Area& area) + { + json feature; + feature["type"] = "Feature"; + feature["properties"]["id"] = area.id; + feature["properties"]["name"] = area.name; + feature["properties"]["type"] = area.type == msg::Area::TYPE_NAVIGATION + ? "navigation" + : ( + area.type == msg::Area::TYPE_LAWN ? "lawn" : "exclusion"); + + feature["geometry"]["type"] = "Polygon"; + feature["geometry"]["coordinates"] = json::array(); + + json coordinates = json::array(); + for (const auto& point : area.area.polygon.points) + { + json p = pointToCoordinates(point); + coordinates.push_back(p); + } + feature["geometry"]["coordinates"].push_back(coordinates); + + return feature; + } + + geometry_msgs::msg::Point GeoJSONMap::movePointTowardsOrientation(const geometry_msgs::msg::Point& point, + const geometry_msgs::msg::Quaternion& quaternion, + double x) const + { + tf2::Quaternion tf2Quaternion; + tf2::fromMsg(quaternion, tf2Quaternion); + tf2::Matrix3x3 mat(tf2Quaternion); + + double roll, pitch, yaw; + mat.getRPY(roll, pitch, yaw); + + geometry_msgs::msg::Point outputPoint; + + outputPoint.x = point.x + x * std::cos(yaw); + outputPoint.y = point.y + x * std::sin(yaw); + outputPoint.z = point.z; + + return outputPoint; + } + + json GeoJSONMap::dockingStationToGeoJSONFeature(const msg::DockingStation& docking_station) + { + json feature; + feature["type"] = "Feature"; + feature["properties"]["id"] = docking_station.id; + feature["properties"]["name"] = docking_station.name; + feature["properties"]["type"] = "docking_station"; + feature["geometry"]["type"] = "LineString"; + + auto coordinates = json::array(); + + // calculate orientation point + auto orientationPoint = movePointTowardsOrientation(docking_station.pose.pose.position, + docking_station.pose.pose.orientation, 0.5); + + coordinates.push_back(pointToCoordinates(docking_station.pose.pose.position)); + coordinates.push_back(pointToCoordinates(orientationPoint)); + + feature["geometry"]["coordinates"] = coordinates; + + return feature; + } + + msg::Map GeoJSONMap::load() + { + std::ifstream f(path_, std::ios::in); + if (!f.is_open()) + { + throw std::runtime_error("Could not open GeoJSON file: " + path_); + } + + json data = json::parse(f); + + if (data["type"] != "FeatureCollection") + { + throw std::runtime_error("GeoJSON file is not a FeatureCollection type"); + } + + msg::Map map; + map.header.stamp = node_->now(); // or use the modification time of the file? + map.header.frame_id = node_->get_parameter("world_frame").as_string(); + + for (const auto& feature : data["features"]) + { + if (feature["type"] != "Feature") + { + RCLCPP_WARN(node_->get_logger(), "Non-feature object found in GeoJSON file"); + continue; + } + + if (feature["geometry"]["type"] == "Polygon") + { + parsePolygonFeature(map, feature); + continue; + } + + if (feature["geometry"]["type"] == "LineString") + { + parseLineStringFeature(map, feature); + continue; + } + + RCLCPP_WARN(node_->get_logger(), "Unsupported geometry type: %s", + feature["geometry"]["type"].get().c_str()); + } + return map; + } + + void GeoJSONMap::save(msg::Map map) + { + json data; + data["type"] = "FeatureCollection"; + data["features"] = json::array(); + + for (const auto& area : map.areas) + { + auto feature = mapAreaToGeoJSONFeature(area); + data["features"].push_back(feature); + } + + for (const auto& docking_station : map.docking_stations) + { + auto feature = dockingStationToGeoJSONFeature(docking_station); + data["features"].push_back(feature); + } + } + + void GeoJSONMap::parsePolygonFeature(msg::Map& map, const json& feature) + { + msg::Area area; + area.id = feature["properties"].value("id", ""); + area.name = feature["properties"].value("name", ""); + + area.type = feature["properties"]["type"] == "navigation" + ? msg::Area::TYPE_NAVIGATION + : ( + feature["properties"]["type"] == "lawn" ? msg::Area::TYPE_LAWN : msg::Area::TYPE_EXCLUSION); + + for (const auto& ll : feature["geometry"]["coordinates"][0]) + { + auto p = parsePoint(ll); + area.area.polygon.points.push_back(p); + } + + map.areas.push_back(area); + } + + geometry_msgs::msg::Point32 GeoJSONMap::parsePoint(json::const_reference value) const + { + geometry_msgs::msg::Point32 p; + + auto request = std::make_shared(); + request->ll_point.latitude = value[1]; + request->ll_point.longitude = value[0]; + auto result = from_ll_client_->async_send_request(request); + if (rclcpp::spin_until_future_complete(node_, result, std::chrono::seconds(10)) != + rclcpp::FutureReturnCode::SUCCESS) + { + throw std::runtime_error("Could not convert LL to map coordinates"); + } + + auto v = *result.get(); + p.x = v.map_point.x; + p.y = v.map_point.y; + + return p; + } + + geometry_msgs::msg::PoseStamped GeoJSONMap::calculateTwoPointsPose(geometry_msgs::msg::Point32 origin, + geometry_msgs::msg::Point32 point32) + { + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = origin.x; + pose.pose.position.y = origin.y; + + const double yaw = atan2(point32.y - origin.y, point32.x - origin.x); + pose.pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), yaw)); + + return pose; + } + + void GeoJSONMap::parseLineStringFeature(msg::Map& map, const json& feature) + { + if (feature["properties"]["type"] != "docking_station") + { + RCLCPP_WARN(node_->get_logger(), "Unsupported line string type: %s", + feature["properties"]["type"].get().c_str()); + return; + } + + msg::DockingStation docking_station; + docking_station.id = feature["properties"]["id"]; + docking_station.name = feature["properties"]["name"]; + + // docking station line string must have two coordinates + // the first one is the origin of the docking station (usually a middle of the charging connectors) + // the second point is used to determine the orientation of the docking station (towards the robot's connectors) + + if (feature["geometry"]["coordinates"].size() < 2) + { + RCLCPP_WARN(node_->get_logger(), "Docking station line string must have at least two coordinates"); + return; + } + + if (feature["geometry"]["coordinates"][0].size() > 2) + { + RCLCPP_WARN(node_->get_logger(), + "Docking station expected to have two coordinates, but has %d. Reading only the first two.", + static_cast(feature["geometry"]["coordinates"][0].size())); + } + + const geometry_msgs::msg::Point32 origin = parsePoint(feature["geometry"]["coordinates"][0]); + const geometry_msgs::msg::Point32 orientation = parsePoint(feature["geometry"]["coordinates"][1]); + + docking_station.pose = calculateTwoPointsPose(origin, orientation); + + map.docking_stations.push_back(docking_station); + } +} // OpenMowerMapServer diff --git a/src/open_mower_map_server/src/map_server_node.cpp b/src/open_mower_map_server/src/map_server_node.cpp new file mode 100644 index 0000000..c22d5ae --- /dev/null +++ b/src/open_mower_map_server/src/map_server_node.cpp @@ -0,0 +1,196 @@ +#include "open_mower_map_server/map_server_node.hpp" +#include "open_mower_map_server/polygon_iterator.hpp" +#include "open_mower_map_server/polygon_utils.hpp" +#include "open_mower_map_server/geo_json_map.hpp" + +namespace open_mower_map_server +{ + MapServerNode::MapServerNode(const rclcpp::NodeOptions& options) : rclcpp::Node("map_server_node", options) + { + configureMap(); + configureGaussianBlur(); + + declare_parameter("world_frame", "map"); + + auto occupancy_grid_topic_name_ = declare_parameter("grid.topic_name", "map_grid"); + occupancy_grid_publisher_ = this->create_publisher(occupancy_grid_topic_name_, + rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + auto map_topic_name_ = declare_parameter("topic_name", "map"); + map_publisher_ = this->create_publisher(map_topic_name_, + rclcpp::QoS( + rclcpp::KeepLast(1)).transient_local().reliable()); + + publishMap(); + } + + void MapServerNode::publishMap() + { + current_map_ = map_io_->load(); + + // log areas + RCLCPP_INFO(get_logger(), "Loaded %lu areas", current_map_.areas.size()); + RCLCPP_INFO(get_logger(), "%-10s %-20s %-10s %10s", "ID", "Name", "Type", "Area"); + + for (const auto& area : current_map_.areas) + { + std::string type = area.type == msg::Area::TYPE_NAVIGATION + ? "navigation" + : ( + area.type == msg::Area::TYPE_LAWN ? "lawn" : "exclusion"); + + auto calculated = polygonArea(area.area.polygon); + + RCLCPP_INFO(get_logger(), "%-10s %-20s %-10s %10.2fsqmt", area.id.c_str(), area.name.c_str(), type.c_str(), + calculated); + } + + // log docks + RCLCPP_INFO(get_logger(), "Loaded %lu docks", current_map_.docking_stations.size()); + RCLCPP_INFO(get_logger(), "%-10s %-20s", "ID", "Name"); + + for (const auto& dock : current_map_.docking_stations) + { + RCLCPP_INFO(get_logger(), "%-10s %-20s", dock.id.c_str(), dock.name.c_str()); + } + + RCLCPP_INFO(get_logger(), "Publishing map"); + map_publisher_->publish(current_map_); + RCLCPP_INFO(get_logger(), "Publishing occupancy grid"); + occupancy_grid_publisher_->publish(mapToOccupancyGrid(current_map_)); + } + + void MapServerNode::configureMap() + { + map_type_ = declare_parameter("type", "geojson"); + + if (map_type_ == "geojson") + { + map_file_ = declare_parameter("path", "~/.openmower/map.geojson"); + RCLCPP_INFO(get_logger(), "Using GeoJSON map: %s", map_file_.c_str()); + map_io_ = new GeoJSONMap(map_file_, std::shared_ptr(this, [](MapServerNode*) + { + })); + } + else + { + throw std::runtime_error("Unsupported map type: " + map_type_); + } + } + + void MapServerNode::configureGaussianBlur() + { + use_gaussian_blur_ = declare_parameter("grid.use_gaussian_blur", false); + if (use_gaussian_blur_) + { + RCLCPP_INFO(get_logger(), "Using Gaussian blur"); + + std::vector kernel = declare_parameter("map.gaussian_blur.kernel", + std::vector{1, 2, 1, 2, 4, 2, 1, 2, 1}); + std::vector kernel_float(kernel.begin(), kernel.end()); + + gaussian_filter_ = new SomeGaussianFilter(kernel_float); + } + } + + std::vector MapServerNode::areasWithExclusionsLast(std::vector areas) + { + std::sort(areas.begin(), areas.end(), [](const msg::Area::SharedPtr& a, const msg::Area::SharedPtr& b) + { + if (a->type == b->type) + { + return a->type == msg::Area::TYPE_EXCLUSION; + } + + return a->type < b->type; + }); + + return areas; + } + + nav_msgs::msg::OccupancyGrid MapServerNode::mapToOccupancyGrid(open_mower_map_server::msg::Map map) + { + float minX, minY, maxX, maxY; + + for (const auto& area : map.areas) + { + for (const auto& point : area.area.polygon.points) + { + minX = std::min(minX, point.x); + minY = std::min(minY, point.y); + maxX = std::max(maxX, point.x); + maxY = std::max(maxY, point.y); + } + } + + maxX += 1; + maxY += 1; + minX -= 1; + minY -= 1; + + nav_msgs::msg::OccupancyGrid occupancy_grid; + occupancy_grid.header = map.header; + + // The origin of the map [m, m, rad]. This is the real-world pose of the bottom left corner of cell (0,0) in the map. + occupancy_grid.info.origin.position.x = minX; + occupancy_grid.info.origin.position.y = minY; + + // cell size is in meters + occupancy_grid.info.resolution = 0.1; + + // occupancy grid width/height is in cells, not meters + occupancy_grid.info.width = (maxX - minX) / occupancy_grid.info.resolution; + occupancy_grid.info.height = (maxY - minY) / occupancy_grid.info.resolution; + + occupancy_grid.info.map_load_time = this->now(); + + occupancy_grid.data = std::vector(occupancy_grid.info.width * occupancy_grid.info.height, -1); + + for (const auto& area : map.areas) + { + uint8_t value = area.type == msg::Area::TYPE_EXCLUSION ? 1 : 0; + + fillGridWithPolygon(occupancy_grid, area.area.polygon, value); + } + + RCLCPP_INFO(get_logger(), "Occupancy grid size: %dm x %dm (%.2fm resolution)", occupancy_grid.info.width, + occupancy_grid.info.height, occupancy_grid.info.resolution); + + if (gaussian_filter_) + { + RCLCPP_INFO(get_logger(), "Applying Gaussian filter"); + + gaussian_filter_->apply(occupancy_grid.data, occupancy_grid.info.width, occupancy_grid.info.height); + } + + return occupancy_grid; + } + + void MapServerNode::fillGridWithPolygon(nav_msgs::msg::OccupancyGrid& occupancy_grid, + const geometry_msgs::msg::Polygon& polygon, uint8_t value) + { + auto iterator = PolygonGridIterator(polygon, occupancy_grid.info.resolution); + while (iterator.next()) + { + auto point = *iterator; + auto x = point.x - occupancy_grid.info.origin.position.x; + auto y = point.y - occupancy_grid.info.origin.position.y; + auto index = (int) (x / occupancy_grid.info.resolution) + (int) (y / occupancy_grid.info.resolution) * occupancy_grid.info.width; + + if (index >= occupancy_grid.data.size()) + { + RCLCPP_ERROR(this->get_logger(), "Index out of bounds: %d Grid size: %lu", index, + occupancy_grid.data.size()); + + continue; + } + + if (occupancy_grid.data[index] > -1) + { + continue; // do not overwrite existing values + } + + occupancy_grid.data[index] = value; + } + } +} diff --git a/src/open_mower_map_server/src/node_main.cpp b/src/open_mower_map_server/src/node_main.cpp new file mode 100644 index 0000000..0573461 --- /dev/null +++ b/src/open_mower_map_server/src/node_main.cpp @@ -0,0 +1,18 @@ +#include + +#include + +#include "open_mower_map_server/map_server_node.hpp" + +int main(int argc, char **argv) { + // Force flush of the stdout buffer. + setvbuf(stdout, nullptr, _IONBF, BUFSIZ); + + rclcpp::init(argc, argv); + + rclcpp::spin(std::make_shared(rclcpp::NodeOptions())); + + rclcpp::shutdown(); + + return 0; +} \ No newline at end of file diff --git a/src/open_mower_map_server/test/load_geojson_map.test.py b/src/open_mower_map_server/test/load_geojson_map.test.py new file mode 100644 index 0000000..c26767b --- /dev/null +++ b/src/open_mower_map_server/test/load_geojson_map.test.py @@ -0,0 +1,72 @@ +import os +import sys + +from threading import Event +from threading import Thread +import unittest + +import launch +import launch.actions +import launch_ros.actions +import launch_testing.actions +from launch_testing.io_handler import ActiveIoHandler +import launch_testing.markers +import pytest +import rclpy +from rclpy.node import Node +from std_msgs.msg import String +from open_mower_map_server.msg import Map + +@pytest.mark.launch_test +@launch_testing.markers.keep_alive +def generate_test_description(): + return launch.LaunchDescription([ + launch_ros.actions.Node( + # executable=sys.executable, + package='open_mower_map_server', + executable='map_server_node', + name='open_mower_map_server', + output='screen', + parameters=[{ + 'type': 'geojson', + 'path': os.path.join(os.getcwd(), 'src', 'open_mower_map_server', 'test', 'test_map.geojson'), + }], + ), + launch_ros.actions.Node( + package='robot_localization', + executable='navsat_transform_node', + name='navsat_transform_node', + parameters=[{ + 'datum': [-22.9, -43.2, 0.0], + }], + ), + launch_testing.actions.ReadyToTest() + ]) + +class TestOpenMowerMapServer(unittest.TestCase): + @classmethod + def setUpClass(self): + rclpy.init() + + @classmethod + def tearDownClass(self): + rclpy.shutdown() + + def setUp(self): + self.node = rclpy.create_node('test_open_mower_map_server') + + def tearDown(self): + self.node.destroy_node() + + def test_map(self): + self.node.get_logger().info('Waiting for map...') + self.event = Event() + + self.node.create_subscription( + Map, + 'map', + lambda msg: self.event.set(), + 1 + ) + self.event.wait(10.0) + self.assertTrue(self.event.is_set()) \ No newline at end of file diff --git a/src/open_mower_map_server/test/test_map.geojson b/src/open_mower_map_server/test/test_map.geojson new file mode 100644 index 0000000..3361e66 --- /dev/null +++ b/src/open_mower_map_server/test/test_map.geojson @@ -0,0 +1,237 @@ +{ + "type": "FeatureCollection", + "features": [ + { + "type": "Feature", + "properties": { + "id": "0859c0c1-ec2d-4197-96b7-2d12f6cf1cd9", + "name": "backyard", + "type": "lawn", + "fill": "#00ff00" + }, + "geometry": { + "coordinates": [ + [ + [ + -43.20007309019093, + -22.899998069679626 + ], + [ + -43.20006688758278, + -22.900006871946417 + ], + [ + -43.200002346928954, + -22.899972743856168 + ], + [ + -43.19999715014916, + -22.899979847441557 + ], + [ + -43.200002011652515, + -22.899984943491773 + ], + [ + -43.20000352039554, + -22.899987877581054 + ], + [ + -43.200001508737785, + -22.89999529001645 + ], + [ + -43.19999882652931, + -22.899998687383203 + ], + [ + -43.199989774073345, + -22.900001003768594 + ], + [ + -43.19998390674169, + -22.900001621472214 + ], + [ + -43.199976027752285, + -22.89999992278834 + ], + [ + -43.199966304744464, + -22.899997143125205 + ], + [ + -43.19991584568825, + -22.89996363273525 + ], + [ + -43.199949708576725, + -22.89991792269754 + ], + [ + -43.20007309019093, + -22.899998069679626 + ] + ] + ], + "type": "Polygon" + }, + "id": 0 + }, + { + "type": "Feature", + "properties": { + "id": "6387f8e9-bc4e-48a5-95e2-fedd3b3b5c09", + "name": "driveway", + "type": "navigation", + "fill": "#ffffff" + }, + "geometry": { + "coordinates": [ + [ + [ + -43.19991601332646, + -22.89996363273525 + ], + [ + -43.19996664002096, + -22.899996988699343 + ], + [ + -43.19995825811796, + -22.90000887948092 + ], + [ + -43.19990629031864, + -22.8999759867976 + ], + [ + -43.19991601332646, + -22.89996363273525 + ] + ] + ], + "type": "Polygon" + }, + "id": 1 + }, + { + "type": "Feature", + "properties": { + "id": "a7a3e913-5e9e-4959-8044-059f40356996", + "type": "lawn", + "fill": "#00ff00" + }, + "geometry": { + "coordinates": [ + [ + [ + -43.19990654164289, + -22.899975703751664 + ], + [ + -43.19995834180392, + -22.900008596436095 + ], + [ + -43.19996638843159, + -22.899996860080364 + ], + [ + -43.199998742577066, + -22.899998713189134 + ], + [ + -43.20000008368183, + -22.899983116188707 + ], + [ + -43.200020703164, + -22.90003299569868 + ], + [ + -43.20000259825318, + -22.900055850700596 + ], + [ + -43.199891286578776, + -22.899990528624386 + ], + [ + -43.19990654164289, + -22.899975703751664 + ] + ] + ], + "type": "Polygon" + }, + "id": 2 + }, + { + "type": "Feature", + "properties": { + "fill": "#ff0000", + "type": "obstacle", + "name": "tree", + "id": "90cd67cb-a606-4878-be1c-2c08f8fc085d" + }, + "geometry": { + "coordinates": [ + [ + [ + -43.19996538260324, + -22.899949451369395 + ], + [ + -43.199974435058095, + -22.899951304478776 + ], + [ + -43.199975776162916, + -22.899958871343117 + ], + [ + -43.199966891345184, + -22.899964585096825 + ], + [ + -43.1999563301473, + -22.899959180193974 + ], + [ + -43.19995666542377, + -22.899953312014105 + ], + [ + -43.19996538260324, + -22.899949451369395 + ] + ] + ], + "type": "Polygon" + }, + "id": 3 + }, + { + "type": "Feature", + "properties": { + "id": "0706aa36-bf67-4831-9472-be2c4275815d", + "type": "docking_station", + "name": "First docking station" + }, + "geometry": { + "coordinates": [ + [ + -43.1999118467661, + -22.899969157137434 + ], + [ + -43.19991519952754, + -22.89997131909847 + ] + ], + "type": "LineString" + } + } + ] +} \ No newline at end of file diff --git a/src/openmower/config/nav2_params.yaml b/src/openmower/config/nav2_params.yaml index ea49920..6471378 100644 --- a/src/openmower/config/nav2_params.yaml +++ b/src/openmower/config/nav2_params.yaml @@ -198,15 +198,15 @@ local_costmap: height: 3 resolution: 0.05 # robot_radius: 0.5 - footprint: "[ [0.21, 0.195], [0.21, -0.195], [-0.21, -0.195], [-0.21, 0.195] ]" +# footprint: "[ [0.21, 0.195], [0.21, -0.195], [-0.21, -0.195], [-0.21, 0.195] ]" plugins: # - voxel_layer - static_layer - - inflation_layer - inflation_layer: - plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 - inflation_radius: 0.55 +# - inflation_layer +# inflation_layer: +# plugin: "nav2_costmap_2d::InflationLayer" +# cost_scaling_factor: 3.0 +# inflation_radius: 0.55 # voxel_layer: # plugin: "nav2_costmap_2d::VoxelLayer" # enabled: True @@ -239,7 +239,7 @@ global_costmap: plugins: - static_layer # - obstacle_layer - - inflation_layer +# - inflation_layer # obstacle_layer: # plugin: "nav2_costmap_2d::ObstacleLayer" # enabled: True @@ -250,16 +250,12 @@ global_costmap: static_layer: plugin: "nav2_costmap_2d::StaticLayer" map_subscribe_transient_local: True - inflation_layer: - plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 - inflation_radius: 0.55 +# inflation_layer: +# plugin: "nav2_costmap_2d::InflationLayer" +# cost_scaling_factor: 3.0 +# inflation_radius: 0.55 always_send_full_costmap: True -map_server: - ros__parameters: - yaml_filename: "map.yaml" - map_saver: ros__parameters: use_sim_time: True diff --git a/src/openmower/config/twist_mux.yaml b/src/openmower/config/twist_mux.yaml index 97411fc..ee0a7a6 100644 --- a/src/openmower/config/twist_mux.yaml +++ b/src/openmower/config/twist_mux.yaml @@ -2,13 +2,9 @@ twist_mux: ros__parameters: topics: navigation: - topic : cmd_vel + topic : cmd_vel_nav timeout : 0.5 priority: 10 - tracker: - topic : cmd_vel_tracker - timeout : 0.5 - priority: 20 joystick: topic : cmd_vel_joy timeout : 0.5 diff --git a/src/openmower/launch/localization.launch.py b/src/openmower/launch/localization.launch.py index 5312072..3ead4e0 100644 --- a/src/openmower/launch/localization.launch.py +++ b/src/openmower/launch/localization.launch.py @@ -3,7 +3,7 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable +from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable, TimerAction from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node from nav2_common.launch import RewrittenYaml @@ -35,7 +35,8 @@ def generate_launch_description(): # Create our own temporary YAML files that include substitutions param_substitutions = { 'use_sim_time': use_sim_time, - 'yaml_filename': map_yaml_file} + 'yaml_filename': map_yaml_file, + } configured_params = RewrittenYaml( source_file=params_file, @@ -71,30 +72,20 @@ def generate_launch_description(): default_value=os.path.join(package_path, 'config', 'nav2_params.yaml'), description='Full path to the ROS2 parameters file to use'), - Node( - package='nav2_map_server', - executable='map_server', + TimerAction(period=3.0, actions=[Node( + package='open_mower_map_server', + executable='map_server_node', name='map_server', output='screen', - parameters=[configured_params], - remappings=remappings), - - # Node( - # package='nav2_amcl', - # executable='amcl', - # name='amcl', - # output='screen', - # parameters=[configured_params], - # remappings=remappings), - - Node( - package='nav2_lifecycle_manager', - executable='lifecycle_manager', - name='lifecycle_manager_localization', - output='screen', - parameters=[{'use_sim_time': use_sim_time}, - {'autostart': autostart}, - {'node_names': lifecycle_nodes}]), + parameters=[{ + 'use_sim_time': use_sim_time, + 'path': os.getenv('OM_MAP_PATH'), + }], + remappings=[ + ('map_grid', 'map'), # occupancy grid topic + ('map', 'mowing_map'), # map topic + ], + )]), Node( package='robot_localization', diff --git a/src/openmower/launch/nav2.launch.py b/src/openmower/launch/nav2.launch.py index dd974bd..fd5e97a 100644 --- a/src/openmower/launch/nav2.launch.py +++ b/src/openmower/launch/nav2.launch.py @@ -1,29 +1,52 @@ +# Copyright (c) 2018 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable -from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import LoadComposableNodes, SetParameter from launch_ros.actions import Node +from launch_ros.descriptions import ComposableNode, ParameterFile from nav2_common.launch import RewrittenYaml def generate_launch_description(): + # Get the launch directory bringup_dir = get_package_share_directory('openmower') namespace = LaunchConfiguration('namespace') use_sim_time = LaunchConfiguration('use_sim_time') autostart = LaunchConfiguration('autostart') params_file = LaunchConfiguration('params_file') - default_bt_xml_filename = LaunchConfiguration('default_bt_xml_filename') - map_subscribe_transient_local = LaunchConfiguration('map_subscribe_transient_local') + use_composition = LaunchConfiguration('use_composition') + container_name = LaunchConfiguration('container_name') + container_name_full = (namespace, '/', container_name) + use_respawn = LaunchConfiguration('use_respawn') + log_level = LaunchConfiguration('log_level') lifecycle_nodes = ['controller_server', + 'smoother_server', 'planner_server', - # 'recoveries_server', - # 'bt_navigator', - 'waypoint_follower'] + 'behavior_server', + 'bt_navigator', + 'waypoint_follower', + 'velocity_smoother'] # Map fully qualified names to relative ones so the node's namespace can be prepended. # In case of the transforms (tf), currently, there doesn't seem to be a better alternative @@ -36,94 +59,217 @@ def generate_launch_description(): # Create our own temporary YAML files that include substitutions param_substitutions = { - 'use_sim_time': use_sim_time, - 'default_bt_xml_filename': default_bt_xml_filename, - 'autostart': autostart, - 'map_subscribe_transient_local': map_subscribe_transient_local} - - configured_params = RewrittenYaml( - source_file=params_file, - root_key=namespace, - param_rewrites=param_substitutions, - convert_types=True) - - return LaunchDescription([ - # Set env var to print messages to stdout immediately - SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), - - DeclareLaunchArgument( - 'namespace', default_value='', - description='Top-level namespace'), - - DeclareLaunchArgument( - 'use_sim_time', default_value='false', - description='Use simulation (Gazebo) clock if true'), - - DeclareLaunchArgument( - 'autostart', default_value='true', - description='Automatically startup the nav2 stack'), - - DeclareLaunchArgument( - 'params_file', - default_value=os.path.join(bringup_dir, 'config', 'nav2_params.yaml'), - description='Full path to the ROS2 parameters file to use'), - - DeclareLaunchArgument( - 'default_bt_xml_filename', - default_value=os.path.join( - get_package_share_directory('nav2_bt_navigator'), - 'behavior_trees', 'navigate_w_replanning_and_recovery.xml'), - description='Full path to the behavior tree xml file to use'), - - DeclareLaunchArgument( - 'map_subscribe_transient_local', default_value='false', - description='Whether to set the map subscriber QoS to transient local'), - - Node( - package='nav2_controller', - executable='controller_server', - output='screen', - parameters=[configured_params], - remappings=remappings), - - Node( - package='nav2_planner', - executable='planner_server', - name='planner_server', - output='screen', - parameters=[configured_params], - remappings=remappings), - - # Node( - # package='nav2_recoveries', - # executable='recoveries_server', - # name='recoveries_server', - # output='screen', - # parameters=[configured_params], - # remappings=remappings), - - # Node( - # package='nav2_bt_navigator', - # executable='bt_navigator', - # name='bt_navigator', - # output='screen', - # parameters=[configured_params], - # remappings=remappings), - - Node( - package='nav2_waypoint_follower', - executable='waypoint_follower', - name='waypoint_follower', - output='screen', - parameters=[configured_params], - remappings=remappings), - - Node( - package='nav2_lifecycle_manager', - executable='lifecycle_manager', - name='lifecycle_manager_navigation', - output='screen', - parameters=[{'use_sim_time': use_sim_time}, - {'autostart': autostart}, - {'node_names': lifecycle_nodes}]), - ]) \ No newline at end of file + 'autostart': autostart} + + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True), + allow_substs=True) + + stdout_linebuf_envvar = SetEnvironmentVariable( + 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') + + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Top-level namespace') + + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation (Gazebo) clock if true') + + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'config', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes') + + declare_autostart_cmd = DeclareLaunchArgument( + 'autostart', default_value='true', + description='Automatically startup the nav2 stack') + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', default_value='False', + description='Use composed bringup if True') + + declare_container_name_cmd = DeclareLaunchArgument( + 'container_name', default_value='nav2_container', + description='the name of conatiner that nodes will load in if use composition') + + declare_use_respawn_cmd = DeclareLaunchArgument( + 'use_respawn', default_value='False', + description='Whether to respawn if a node crashes. Applied when composition is disabled.') + + declare_log_level_cmd = DeclareLaunchArgument( + 'log_level', default_value='info', + description='log level') + + load_nodes = GroupAction( + condition=IfCondition(PythonExpression(['not ', use_composition])), + actions=[ + SetParameter('use_sim_time', use_sim_time), + Node( + package='nav2_controller', + executable='controller_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]), + Node( + package='nav2_smoother', + executable='smoother_server', + name='smoother_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_planner', + executable='planner_server', + name='planner_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_behaviors', + executable='behavior_server', + name='behavior_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_bt_navigator', + executable='bt_navigator', + name='bt_navigator', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_waypoint_follower', + executable='waypoint_follower', + name='waypoint_follower', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_velocity_smoother', + executable='velocity_smoother', + name='velocity_smoother', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings + + [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_navigation', + output='screen', + arguments=['--ros-args', '--log-level', log_level], + parameters=[{'autostart': autostart}, + {'node_names': lifecycle_nodes}]), + ] + ) + + load_composable_nodes = GroupAction( + condition=IfCondition(use_composition), + actions=[ + SetParameter('use_sim_time', use_sim_time), + LoadComposableNodes( + target_container=container_name_full, + composable_node_descriptions=[ + ComposableNode( + package='nav2_controller', + plugin='nav2_controller::ControllerServer', + name='controller_server', + parameters=[configured_params], + remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]), + ComposableNode( + package='nav2_smoother', + plugin='nav2_smoother::SmootherServer', + name='smoother_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_planner', + plugin='nav2_planner::PlannerServer', + name='planner_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_behaviors', + plugin='behavior_server::BehaviorServer', + name='behavior_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_bt_navigator', + plugin='nav2_bt_navigator::BtNavigator', + name='bt_navigator', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_waypoint_follower', + plugin='nav2_waypoint_follower::WaypointFollower', + name='waypoint_follower', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_velocity_smoother', + plugin='nav2_velocity_smoother::VelocitySmoother', + name='velocity_smoother', + parameters=[configured_params], + remappings=remappings + + [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]), + ComposableNode( + package='nav2_lifecycle_manager', + plugin='nav2_lifecycle_manager::LifecycleManager', + name='lifecycle_manager_navigation', + parameters=[{'autostart': autostart, + 'node_names': lifecycle_nodes}]), + ], + ) + ] + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Set environment variables + ld.add_action(stdout_linebuf_envvar) + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_use_composition_cmd) + ld.add_action(declare_container_name_cmd) + ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_log_level_cmd) + # Add the actions to launch all of the navigation nodes + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) + + return ld diff --git a/src/openmower/launch/sim.launch.py b/src/openmower/launch/sim.launch.py index 0f70d61..b3ea0bb 100644 --- a/src/openmower/launch/sim.launch.py +++ b/src/openmower/launch/sim.launch.py @@ -138,6 +138,6 @@ def generate_launch_description(): ), gz_spawn_entity, localization, - nav2, + # nav2, foxglove_bridge, ]) diff --git a/utils/install-custom-deps.sh b/utils/install-custom-deps.sh index 010c7ad..c5dfa1b 100755 --- a/utils/install-custom-deps.sh +++ b/utils/install-custom-deps.sh @@ -7,16 +7,17 @@ cd "$SCRIPT_PATH/.." # Install custom dependencies using vcstool -vcs import src --force --shallow < custom_deps.yaml +mkdir -p src/lib +vcs import src/lib --force --shallow < custom_deps.yaml # check if arch is arm64 if [ "$(uname -m)" = "aarch64" ]; then # check if src/nav2_bringup does not exist if [ ! -d "src/nav2_bringup" ]; then curl -L https://api.github.com/repos/ros-planning/navigation2/tarball/1.2.2 \ - | tar xz -C src/ --wildcards "*/nav2_bringup" --strip-components=1 + | tar xz -C src/lib --wildcards "*/nav2_bringup" --strip-components=1 # remove "turtlebot3_gazebo" dependency from nav2_bringup/package.xml since it has not arm64 build - sed -i '/turtlebot3_gazebo/d' src/nav2_bringup/package.xml + sed -i '/turtlebot3_gazebo/d' src/lib/nav2_bringup/package.xml fi fi