diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..22ba5c3 --- /dev/null +++ b/.clang-format @@ -0,0 +1,62 @@ +BasedOnStyle: "Google" +--- +Language: Cpp +AccessModifierOffset: -2 +ConstructorInitializerIndentWidth: 2 +AlignAfterOpenBracket: "DontAlign" +AlignEscapedNewlinesLeft: false +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AllowShortFunctionsOnASingleLine: "Inline" +AlwaysBreakTemplateDeclarations: true +AlwaysBreakBeforeMultilineStrings: true +BreakBeforeBinaryOperators: false +BreakBeforeTernaryOperators: false +BreakConstructorInitializersBeforeComma: true +BinPackParameters: false +ColumnLimit: 100 +ConstructorInitializerAllOnOneLineOrOnePerLine: true +DerivePointerBinding: false +PointerBindsToType: true +ExperimentalAutoDetectBinPacking: false +IndentCaseLabels: true +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: "None" +ObjCSpaceBeforeProtocolList: true +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 60 +PenaltyBreakString: 1 +PenaltyBreakFirstLessLess: 1000 +PenaltyExcessCharacter: 1000 +PenaltyReturnTypeOnItsOwnLine: 90 +SpacesBeforeTrailingComments: 2 +Cpp11BracedListStyle: false +Standard: "Auto" +IndentWidth: 2 +TabWidth: 2 +UseTab: "Never" +IndentFunctionDeclarationAfterType: false +SpacesInParentheses: false +SpacesInAngles: false +SpaceInEmptyParentheses: false +SpacesInCStyleCastParentheses: false +SpaceAfterControlStatementKeyword: true +SpaceBeforeAssignmentOperators: true +ContinuationIndentWidth: 4 +SortIncludes: false +SpaceAfterCStyleCast: false +BreakBeforeBraces: "Custom" +BraceWrapping: + AfterClass: true + AfterControlStatement: false + AfterEnum: true + AfterFunction: true + AfterNamespace: true + AfterStruct: true + AfterUnion: true + BeforeCatch: false + BeforeElse: false + IndentBraces: false +PointerAlignment: Middle \ No newline at end of file diff --git a/CHANGELOG.md b/CHANGELOG.md new file mode 100644 index 0000000..b16de6a --- /dev/null +++ b/CHANGELOG.md @@ -0,0 +1,6 @@ +# Changelog for package qrb_ros_amr_service + +## 1.0.0 (2024-11-28) + +- Initial version release for Humble +- Contributors: Xiaowei Zhang \ No newline at end of file diff --git a/CODE-OF-CONDUCT.md b/CODE-OF-CONDUCT.md new file mode 100644 index 0000000..19f7fb8 --- /dev/null +++ b/CODE-OF-CONDUCT.md @@ -0,0 +1,132 @@ +# Contributor Covenant Code of Conduct + +## Our Pledge + +We as members, contributors, and leaders pledge to make participation in our +community a harassment-free experience for everyone, regardless of age, body +size, visible or invisible disability, ethnicity, sex characteristics, gender +identity and expression, level of experience, education, socio-economic status, +nationality, personal appearance, race, caste, color, religion, or sexual +identity and orientation. + +We pledge to act and interact in ways that contribute to an open, welcoming, +diverse, inclusive, and healthy community. + +## Our Standards + +Examples of behavior that contributes to a positive environment for our +community include: + +* Demonstrating empathy and kindness toward other people +* Being respectful of differing opinions, viewpoints, and experiences +* Giving and gracefully accepting constructive feedback +* Accepting responsibility and apologizing to those affected by our mistakes, + and learning from the experience +* Focusing on what is best not just for us as individuals, but for the overall + community + +Examples of unacceptable behavior include: + +* The use of sexualized language or imagery, and sexual attention or advances of + any kind +* Trolling, insulting or derogatory comments, and personal or political attacks +* Public or private harassment +* Publishing others' private information, such as a physical or email address, + without their explicit permission +* Other conduct which could reasonably be considered inappropriate in a + professional setting + +## Enforcement Responsibilities + +Community leaders are responsible for clarifying and enforcing our standards of +acceptable behavior and will take appropriate and fair corrective action in +response to any behavior that they deem inappropriate, threatening, offensive, +or harmful. + +Community leaders have the right and responsibility to remove, edit, or reject +comments, commits, code, wiki edits, issues, and other contributions that are +not aligned to this Code of Conduct, and will communicate reasons for moderation +decisions when appropriate. + +## Scope + +This Code of Conduct applies within all community spaces, and also applies when +an individual is officially representing the community in public spaces. +Examples of representing our community include using an official email address, +posting via an official social media account, or acting as an appointed +representative at an online or offline event. + +## Enforcement + +Instances of abusive, harassing, or otherwise unacceptable behavior may be +reported to the community leaders responsible for enforcement at +[GitHub.QuIC.CoC](mailto:GitHub.QuIC.CoC@qti.qualcomm.com?subject=GitHub%20QuIC%20Code%20of%20Conduct%20Report). +All complaints will be reviewed and investigated promptly and fairly. + +All community leaders are obligated to respect the privacy and security of the +reporter of any incident. + +## Enforcement Guidelines + +Community leaders will follow these Community Impact Guidelines in determining +the consequences for any action they deem in violation of this Code of Conduct: + +### 1. Correction + +**Community Impact**: Use of inappropriate language or other behavior deemed +unprofessional or unwelcome in the community. + +**Consequence**: A private, written warning from community leaders, providing +clarity around the nature of the violation and an explanation of why the +behavior was inappropriate. A public apology may be requested. + +### 2. Warning + +**Community Impact**: A violation through a single incident or series of +actions. + +**Consequence**: A warning with consequences for continued behavior. No +interaction with the people involved, including unsolicited interaction with +those enforcing the Code of Conduct, for a specified period of time. This +includes avoiding interactions in community spaces as well as external channels +like social media. Violating these terms may lead to a temporary or permanent +ban. + +### 3. Temporary Ban + +**Community Impact**: A serious violation of community standards, including +sustained inappropriate behavior. + +**Consequence**: A temporary ban from any sort of interaction or public +communication with the community for a specified period of time. No public or +private interaction with the people involved, including unsolicited interaction +with those enforcing the Code of Conduct, is allowed during this period. +Violating these terms may lead to a permanent ban. + +### 4. Permanent Ban + +**Community Impact**: Demonstrating a pattern of violation of community +standards, including sustained inappropriate behavior, harassment of an +individual, or aggression toward or disparagement of classes of individuals. + +**Consequence**: A permanent ban from any sort of public interaction within the +community. + +## Attribution + +This Code of Conduct is adapted from the [Contributor Covenant][homepage], +version 2.1, available at +[https://www.contributor-covenant.org/version/2/1/code_of_conduct.html][v2.1]. + +Community Impact Guidelines were inspired by +[Mozilla's code of conduct enforcement ladder][Mozilla CoC]. + +For answers to common questions about this code of conduct, see the FAQ at +[https://www.contributor-covenant.org/faq][FAQ]. Translations are available at +[https://www.contributor-covenant.org/translations][translations]. + +[homepage]: https://www.contributor-covenant.org +[v2.1]: https://www.contributor-covenant.org/version/2/1/code_of_conduct.html +[Mozilla CoC]: https://github.com/mozilla/diversity +[FAQ]: https://www.contributor-covenant.org/faq +[translations]: https://www.contributor-covenant.org/translations diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 0000000..d46bd39 --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,63 @@ +## Contributing to qrb_ros_amr_service + +Hi there! +We’re thrilled that you’d like to contribute to this project. +Your help is essential for keeping this project great and for making it better. + +## Branching Strategy + +In general, contributors should develop on branches based off of `main` and pull requests should be made against `main`. + +## Submitting a pull request + +1. Please read our [code of conduct](CODE-OF-CONDUCT.md) and [license](LICENSE). +1. [Fork](https://github.com/quic-qrb-ros/qrb_ros_amr_service/fork) and clone the repository. + + ```bash + git clone https://github.com/quic-qrb-ros/qrb_ros_amr_service.git + ``` + +1. Create a new branch based on `main`: + + ```bash + git checkout -b main + ``` + +1. Create an upstream `remote` to make it easier to keep your branches up-to-date: + + ```bash + git remote add upstream https://github.com/quic-qrb-ros/qrb_ros_amr_service.git + ``` + +1. Make your changes, add tests, and make sure the tests still pass. +1. Commit your changes using the [DCO](http://developercertificate.org/). You can attest to the DCO by commiting with the **-s** or **--signoff** options or manually adding the "Signed-off-by": + + ```bash + git commit -s -m "Really useful commit message"` + ``` + +1. After committing your changes on the topic branch, sync it with the upstream branch: + + ```bash + git pull --rebase upstream main + ``` + +1. Push to your fork. + + ```bash + git push -u origin + ``` + + The `-u` is shorthand for `--set-upstream`. This will set up the tracking reference so subsequent runs of `git push` or `git pull` can omit the remote and branch. + +1. [Submit a pull request](https://github.com/quic-qrb-ros/qrb_ros_amr_service/pulls) from your branch to `main`. +1. Pat yourself on the back and wait for your pull request to be reviewed. + +Here are a few things you can do that will increase the likelihood of your pull request to be accepted: + +- Follow the existing style where possible. **INSERT LINK TO STYLE, e.g. PEP8 for python** +- Write tests. +- Keep your change as focused as possible. + If you want to make multiple independent changes, please consider submitting them as separate pull requests. +- Write a [good commit message](http://tbaggery.com/2008/04/19/a-note-about-git-commit-messages.html). +- It's a good idea to arrange a discussion with other developers to ensure there is consensus on large features, architecture changes, and other core code changes. PR reviews will go much faster when there are no surprises. diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..f576d11 --- /dev/null +++ b/LICENSE @@ -0,0 +1,28 @@ +BSD 3-Clause License + +Copyright (c) 2024, QUIC QRB ROS + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/README.md b/README.md index 32c47c0..9af7630 100644 --- a/README.md +++ b/README.md @@ -1 +1,81 @@ # qrb_ros_amr_service + +qrb_ros_amr_service is a package to manage the AMR behavior, such as navigation, mapping, return charging station. + +## Overview + +Qualcomm amr service provides ROS interfaces that application can use these interfaces to start a P2P navigation or +Follow path, amr service will control the base return the charging station when battery level is low. + +qrb_amr_manager, which is a dynamic library, it manages the amr behavior. + +## Build + +Currently, we only support use QCLINUX to build + +1. Setup environments follow this document 's [Set up the cross-compile environment.](https://docs.qualcomm.com/bundle/publicresource/topics/80-65220-2/develop-your-first-application_6.html?product=1601111740013072&facet=Qualcomm%20Intelligent%20Robotics%20(QIRP)%20Product%20SDK&state=releasecandidate) part + +2. Create `ros_ws` directory in `/qirp-sdk/` + +3. Clone this repository under `/qirp-sdk/ros_ws` + ```bash + git clone https://github.com/quic-qrb-ros/qrb_ros_interfaces.git + git clone https://github.com/quic-qrb-ros/qrb_ros_amr_service.git + ``` +4. Build this project + ```bash + export AMENT_PREFIX_PATH="${OECORE_TARGET_SYSROOT}/usr;${OECORE_NATIVE_SYSROOT}/usr" + export PYTHONPATH=${PYTHONPATH}:${OECORE_TARGET_SYSROOT}/usr/lib/python3.10/site-packages + export Python3_NumPy_INCLUDE_DIR=${OECORE_TARGET_SYSROOT}/usr/lib/python3.10/site-packages/numpy/core/include + colcon build --merge-install --cmake-args \ + -DPython3_NumPy_INCLUDE_DIR=${Python3_NumPy_INCLUDE_DIR} \ + -DPYTHON_SOABI=cpython-310-aarch64-linux-gnu \ + -DCMAKE_STAGING_PREFIX=$(pwd)/install \ + -DCMAKE_PREFIX_PATH=$(pwd)/install/share \ + -DBUILD_TESTING=OFF + ``` +5. Push to the device & Install + ```bash + cd `/qirp-sdk/ros_ws/install` + tar czvf qrb_ros_amr_service.tar.gz lib share + scp qrb_ros_amr_service.tar.gz root@[ip-addr]:/opt/ + ssh root@[ip-addr] + (ssh) tar -zxf /opt/qrb_ros_amr_service.tar.gz -C /opt/qcom/qirp-sdk/usr/ + ``` + +## Run + +This package supports running it directly from the command or by dynamically adding it to the ros2 component container. + +a.Run with command + +1. Source this file to set up the environment on your device: + ```bash + ssh root@[ip-addr] + (ssh) export HOME=/opt + (ssh) source /opt/qcom/qirp-sdk/qirp-setup.sh + (ssh) export ROS_DOMAIN_ID=xx + (ssh) source /usr/bin/ros_setup.bash + ``` + +2. Use this command to run this package + ```bash + (ssh) ros2 launch qrb_ros_amr qrb_ros_amr_bringup.launch.py + ``` +## Packages + +Will update in the future. + +## Resources + +- [ROS2 Type Adaption](https://ros.org/reps/rep-2007.html) + +## Contributions + +Thanks for your interest in contributing to qrb_ros_amr_service! Please read our [Contributions Page](CONTRIBUTING.md) for more information on contributing features or bug fixes. We look forward to your participation! + +## License + +qrb_ros_imu is licensed under the BSD-3-clause "New" or "Revised" License. + +Check out the [LICENSE](LICENSE) for more details. diff --git a/qrb_amr_manager/CMakeLists.txt b/qrb_amr_manager/CMakeLists.txt new file mode 100644 index 0000000..2c22c0d --- /dev/null +++ b/qrb_amr_manager/CMakeLists.txt @@ -0,0 +1,56 @@ +cmake_minimum_required(VERSION 3.8) +project(qrb_amr_manager) + +set(library_name qrb_amr_manager) + +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) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(geometry_msgs REQUIRED) + +include_directories(${PROJECT_SOURCE_DIR}/include) + +add_library(${library_name} SHARED + src/amr_manager.cpp + src/amr_state_machine.cpp + src/low_power_manager.cpp +) + +aux_source_directory(./src SRC_LIST) + +ament_target_dependencies(qrb_amr_manager + "rclcpp" + "rclcpp_action" + "geometry_msgs" +) + +install(TARGETS qrb_amr_manager + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(TARGETS ${PROJECT_NAME} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib +) + + +file(GLOB_RECURSE HEADER_FILES + "${CMAKE_SOURCE_DIR}/include/*.h" + "${CMAKE_SOURCE_DIR}/include/*.hpp" +) + +install(FILES ${HEADER_FILES} + DESTINATION include/${PROJECT_NAME}/ +) + +install(DIRECTORY + include + DESTINATION include/${PROJECT_NAME}/ +) + +ament_package() diff --git a/qrb_amr_manager/include/amr_manager.hpp b/qrb_amr_manager/include/amr_manager.hpp new file mode 100644 index 0000000..60856e5 --- /dev/null +++ b/qrb_amr_manager/include/amr_manager.hpp @@ -0,0 +1,105 @@ +/* + * Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved. + * SPDX-License-Identifier: BSD-3-Clause-Clear + */ + +#ifndef AMR_MANAGER_HPP_ +#define AMR_MANAGER_HPP_ + +#include +#include + +#include "amr_state_machine.hpp" +#include "low_power_manager.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace qrb +{ +namespace amr_manager +{ + +typedef std::function start_p2p_func_t; +typedef std::function start_follow_path_func_t; +typedef std::function & ids)> + start_waypoint_follow_path_func_t; +typedef std::function sub_cmd_func_t; +typedef std::function start_charging_func_t; +typedef std::function change_mode_func_t; +typedef std::function notify_exception_func_t; +typedef std::function send_amr_state_changed_func_t; +typedef std::function navigate_to_charging_func_t; +typedef std::function slam_command_func_t; +typedef std::function publish_twist_func_t; + +/** + * @class navigation_controller::AMRManager + * @desc The AMRManager create nodes to control the Navigation. + */ +class AMRManager +{ +public: + /** + * @desc A constructor for AMRManager + */ + AMRManager(); + + /** + * @desc A destructor for AMRManager + */ + ~AMRManager(); + + // ExceptionSubscriber + void process_event(int event); + void process_event(int event, int error_code); + + // CmdActionServer + bool check_potential_state(int cmd); + void process_cmd(int cmd, void * buffer, size_t len); + void process_cmd(int cmd, uint32_t goal_id, std::vector & ids); + + // APIServiceServer + void init_amr(); + void release_amr(); + void me_completed(); + + // SubCmdServer + void process_sub_cmd(int msg); + + // AMRStatusTransporter + void notify_amr_state_changed(int state); + void notify_battery_changed(float battery_vol); + void notify_charging_state_changed(uint8_t state); + void register_start_charging_callback(start_charging_func_t cb); + void register_notify_exception_callback(notify_exception_func_t cb); + void register_send_amr_state_changed_callback(send_amr_state_changed_func_t cb); + void register_publish_twist_callback(publish_twist_func_t cb); + + // NavigationActionClient + void register_start_p2p_nav_callback(start_p2p_func_t cb); + void register_start_follow_path_callback(start_follow_path_func_t cb); + void register_start_waypoint_follow_path_callback(start_waypoint_follow_path_func_t cb); + void register_sub_cmd_callback(sub_cmd_func_t cb); + void register_navigate_to_charging_callback(navigate_to_charging_func_t cb); + + // AMRModeServiceClient + void register_change_mode_callback(change_mode_func_t cb); + + // MappingServiceServer + bool start_mapping(); + bool stop_mapping(); + bool load_map(); + + // CartographerServiceClient + void register_slam_command_callback(slam_command_func_t cb); + +private: + std::shared_ptr state_machine_; + std::shared_ptr low_power_; + + void init(); + + rclcpp::Logger logger_{ rclcpp::get_logger("amr_manager") }; +}; +} // namespace amr_manager +} // namespace qrb +#endif // AMR_MANAGER_HPP_ \ No newline at end of file diff --git a/qrb_amr_manager/include/amr_state_machine.hpp b/qrb_amr_manager/include/amr_state_machine.hpp new file mode 100644 index 0000000..53bb9a1 --- /dev/null +++ b/qrb_amr_manager/include/amr_state_machine.hpp @@ -0,0 +1,297 @@ +/* + * Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved. + * SPDX-License-Identifier: BSD-3-Clause-Clear + */ + +#ifndef AMR_STATE_MACHINE_HPP_ +#define AMR_STATE_MACHINE_HPP_ + +#include "message_queue.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "geometry_msgs/msg/twist.hpp" +#include +#include +#include +#include +#include +#include +#include + +using namespace std; + +#define ANGULAR_VELOCITY 0.19 // the min angular velocity for arm + +namespace qrb +{ +namespace amr_manager +{ + +typedef std::function start_p2p_func_t; +typedef std::function start_follow_path_func_t; +typedef std::function & ids)> start_waypoint_follow_path_func_t; +typedef std::function sub_cmd_func_t; +typedef std::function start_charging_func_t; +typedef std::function notify_exception_func_t; +typedef std::function send_amr_state_changed_func_t; +typedef std::function navigate_to_charging_func_t; +typedef std::function change_mode_func_t; +typedef std::function slam_command_func_t; +typedef std::function publish_twist_func_t; + +enum class Slam_Command +{ + StartMapping = 1, + StopMapping = 2, + SaveMap = 3, + LoadMap = 4, + StartLocalization = 5, + Relocalization = 6, +}; + +class Command +{ +public: + const static int ME = 1; + const static int AE = 2; + const static int P2PNAV = 3; + const static int FOLLOW_PATH = 4; + const static int CHARGING = 5; + const static int WAYPOINT_FOLLOW_PATH = 6; + const static int SUB_CMD = 7; + const static int OTHER = 8; + + static std::string cmd_to_string(int cmd) + { + std::string message; + switch (cmd) { + case ME: + message = "ME"; + break; + case AE: + message = "AE"; + break; + case P2PNAV: + message = "P2PNAV"; + break; + case FOLLOW_PATH: + message = "FOLLOW_PATH"; + break; + case CHARGING: + message = "CHARGING"; + break; + case WAYPOINT_FOLLOW_PATH: + message = "WAYPOINT_FOLLOW_PATH"; + break; + case SUB_CMD: + message = "SUB_CMD"; + break; + case OTHER: + message = "OTHER"; + break; + default: + message = "INVAILD"; + break; + } + return message; + } +}; + +class SubCommand +{ +public: + const static int CANCEL = 1; + const static int PAUSE = 2; + const static int RESUME = 3; + + static std::string to_string(int cmd) + { + std::string message; + switch (cmd) { + case CANCEL: + message = "CANCEL"; + break; + case PAUSE: + message = "PAUSE"; + break; + case RESUME: + message = "RESUME"; + break; + default: + message = "INVAILD"; + break; + } + return message; + } +}; + +class AMRStateMachine +{ +private: + std::mutex mtx_; + std::condition_variable * cv_; + bool * sub_cmd_success_; + void * path_buffer_; + size_t path_len_; + bool send_mapper_cmd_; + bool send_navigator_cmd_; + bool has_map_; + int current_state_; + bool running_; + + MessageQueue queue_; + std::shared_ptr thread_handle_msg_; + start_p2p_func_t start_p2p_cb_; + start_follow_path_func_t start_follow_path_cb_; + start_waypoint_follow_path_func_t start_waypoint_follow_path_cb_; + sub_cmd_func_t sub_cmd_cb_; + start_charging_func_t start_charging_cb_; + notify_exception_func_t notify_exception_cb_; + send_amr_state_changed_func_t send_amr_state_changed_cb_; + navigate_to_charging_func_t navigate_to_charging_cb_; + slam_command_func_t slam_command_cb_; + publish_twist_func_t publish_twist_cb_; + std::shared_ptr return_charging_station_thread_; + + rclcpp::Logger logger_{ rclcpp::get_logger("amr_state_machine") }; + + void enter_idle_state(); + void enter_ready_state(); + void enter_on_ae_state(); + void enter_on_me_state(); + void enter_me_done_state(); + void enter_on_p2p_state(const Message & msg); + void enter_on_follow_path_state(const Message & msg); + void enter_on_waypoint_follow_path_state(const Message & msg); + void enter_on_error_state(); + void enter_p2p_wait_state(); + void enter_follow_path_wait_state(); + void enter_low_power_charging_state(); + void enter_on_return_charging_state(); + void create_return_charging_station_thread(); + void return_charging_station_function(void * arg); + void destory_return_charging_station_thread(); + + void * get_current_path(); + void return_charging_station(); + + bool handle_message(const Message & msg); + void handle_msg(); + void handle_failed(const Message & msg); + + void save_map(); + void enter_localization_state(); + bool enter_localization_mode(); + bool get_relocalization_state(); + void start_rotation(); + void stop_rotation(); + void switch_charger_mode(); + void switch_navigation_mode(); + void notify_state_machine_changed(); + void send_me_finish_message(); + void send_me_message(); + void send_me_completed_message(); + void send_relocalization_pass_message(); + void send_return_charging_message(); + void update_state(int last_state, int state); + +public: + const static int IN_ACTIVE = 1; + const static int IDLE = 2; + const static int READY = 3; + const static int ON_AE = 4; + const static int ON_FOLLOW_PATH = 5; + const static int FOLLOW_PATH_WAIT = 6; + const static int ON_P2PNAV = 7; + const static int P2PNAV_WAIT = 8; + const static int ON_RETURN_CHARGING = 9; + const static int LOW_POWER_CHARGING = 10; + const static int ON_ERROR = 11; + const static int ON_ME = 12; + const static int ME_DONE = 13; + const static int LOCALIZATION = 14; + + std::string get_current_state(); + void register_start_p2p_nav_callback(start_p2p_func_t cb); + void register_start_follow_path_callback(start_follow_path_func_t cb); + void register_start_waypoint_follow_path_callback(start_waypoint_follow_path_func_t cb); + void register_sub_cmd_callback(sub_cmd_func_t cb); + void register_start_charging_callback(start_charging_func_t cb); + void register_notify_exception_callback(notify_exception_func_t cb); + void register_send_amr_state_changed_callback(send_amr_state_changed_func_t cb); + void register_navigate_to_charging_callback(navigate_to_charging_func_t cb); + void register_slam_command_callback(slam_command_func_t cb); + void register_publish_twist_callback(publish_twist_func_t cb); + + void process_cmd(int cmd, void * buffer = nullptr, size_t len = -1); + void process_cmd(int cmd, uint32_t goal_id, vector & ids); + void process_sub_cmd(int message); + void process_event(int event); + void process_event(int event, uint8_t error_code); + + void init_amr(); + void release_amr(); + bool load_map(); + bool start_mapping(); + bool stop_mapping(); + bool check_potential_state(int cmd); + + AMRStateMachine(); + ~AMRStateMachine(); + + std::string state_to_string(int msg) + { + std::string message; + switch (msg) { + case IN_ACTIVE: + message = "IN_ACTIVE"; + break; + case IDLE: + message = "IDLE"; + break; + case READY: + message = "READY"; + break; + case ON_AE: + message = "ON_AE"; + break; + case ON_ME: + message = "ON_ME"; + break; + case ME_DONE: + message = "ME_DONE"; + break; + case ON_FOLLOW_PATH: + message = "ON_FOLLOW_PATH"; + break; + case FOLLOW_PATH_WAIT: + message = "FOLLOW_PATH_WAIT"; + break; + case ON_P2PNAV: + message = "ON_P2PNAV"; + break; + case P2PNAV_WAIT: + message = "P2PNAV_WAIT"; + break; + case ON_RETURN_CHARGING: + message = "ON_RETURN_CHARGING"; + break; + case LOW_POWER_CHARGING: + message = "LOW_POWER_CHARGING"; + break; + case ON_ERROR: + message = "ON_ERROR"; + break; + case LOCALIZATION: + message = "LOCALIZATION"; + break; + default: + message = "INVALID_STATE"; + break; + } + return message; + } +}; +} // namespace amr_manager +} // namespace qrb +#endif // AMR_STATE_MACHINE_HPP_ \ No newline at end of file diff --git a/qrb_amr_manager/include/low_power_manager.hpp b/qrb_amr_manager/include/low_power_manager.hpp new file mode 100644 index 0000000..de6c02e --- /dev/null +++ b/qrb_amr_manager/include/low_power_manager.hpp @@ -0,0 +1,69 @@ +/* + * Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved. + * SPDX-License-Identifier: BSD-3-Clause-Clear + */ + +#ifndef LOW_POWER_MANAGER_HPP_ +#define LOW_POWER_MANAGER_HPP_ + +#include "amr_state_machine.hpp" +#include "rclcpp/rclcpp.hpp" +#include + +namespace qrb +{ +namespace amr_manager +{ + +enum class ChargeCmd +{ + START = 0, // start charging + END = 1, // end charging +}; + +enum class ChargerState +{ + UNKNOWN = 0, + IDLE = 3, // charging module is idle state + SEARCHING = 5, // charging module is searching the infrared + CONTROLLING = 6, // charging module is controlling the robot + FORCE_CHARGING = 7, // charging can not be interrupted + CHARGING = 1, + CHARGING_DONE = 4, // completed charging + ERROR = 8, +}; + +enum class SetControlMode +{ + APPLICATION = 0, + CHARGER = 1, + REMOTE_CONTROLLER = 2, +}; + +class LowPowerManager +{ +private: + std::shared_ptr state_machine_; + void send_event(const int event); + void set_charger_mode(); + void set_navigation_mode(); + int get_amr_state(); + + float current_battery_voltage_level_; + uint8_t charging_station_; + std::mutex mtx_; + int amr_state_; + change_mode_func_t change_mode_cb_; + rclcpp::Logger logger_{ rclcpp::get_logger("low_power_manager") }; + +public: + void register_change_mode_callback(change_mode_func_t cb); + void notify_amr_state_changed(int state); + void notify_battery_changed(float battery_vol); + void notify_charging_state_changed(uint8_t state); + LowPowerManager(std::shared_ptr & state_machine); + ~LowPowerManager(); +}; +} // namespace amr_manager +} // namespace qrb +#endif // LOW_POWER_MANAGER_HPP_ \ No newline at end of file diff --git a/qrb_amr_manager/include/message.hpp b/qrb_amr_manager/include/message.hpp new file mode 100644 index 0000000..224698d --- /dev/null +++ b/qrb_amr_manager/include/message.hpp @@ -0,0 +1,129 @@ +/* + * Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved. + * SPDX-License-Identifier: BSD-3-Clause-Clear + */ + +#ifndef MESSAGE_HPP_ +#define MESSAGE_HPP_ + +#include +#include +#include + +namespace qrb +{ +namespace amr_manager +{ +class Message +{ +public: + const static int AE_FINISH = 1; + const static int P2PNAV_FINISH = 2; + const static int FOLLOW_PATH_FINISH = 3; + const static int LOW_POWER = 4; + const static int NORMAL_POWER = 5; + const static int RETURN_CHARGING_FINISH = 6; + const static int AMR_EXCEPTION = 7; + const static int AMR_NORMAL = 8; + const static int ME_FINISH = 9; + const static int RETURN_CHARGING = 10; + + const static int ME = 21; + const static int AE = 22; + const static int P2PNAV = 23; + const static int FOLLOW_PATH = 24; + const static int WAYPOINT_FOLLOW_PATH = 25; + + const static int CANCEL = 31; + const static int PAUSE = 32; + const static int RESUME = 33; + + const static int INIT_AMR = 41; + const static int RELEASE_AMR = 42; + const static int ME_COMPLETED = 43; + const static int RELOCALIZATION_PASS = 44; + + int type; + void * param; + size_t len; + uint32_t goal_id; + uint8_t error_code; + std::vector ids; + + static std::string msg_to_string(int msg) + { + std::string message; + switch (msg) { + case AE_FINISH: + message = "AE_FINISH"; + break; + case ME_FINISH: + message = "ME_FINISH"; + break; + case P2PNAV_FINISH: + message = "P2PNAV_FINISH"; + break; + case FOLLOW_PATH_FINISH: + message = "FOLLOW_PATH_FINISH"; + break; + case NORMAL_POWER: + message = "NORMAL_POWER"; + break; + case LOW_POWER: + message = "LOW_POWER"; + break; + case RETURN_CHARGING_FINISH: + message = "RETURN_CHARGING_FINISH"; + break; + case RETURN_CHARGING: + message = "RETURN_CHARGING"; + break; + case AMR_EXCEPTION: + message = "AMR_EXCEPTION"; + break; + case AMR_NORMAL: + message = "AMR_NORMAL"; + break; + case ME: + message = "ME"; + break; + case AE: + message = "AE"; + break; + case P2PNAV: + message = "P2PNAV"; + break; + case FOLLOW_PATH: + message = "FOLLOW_PATH"; + break; + case RELEASE_AMR: + message = "RELEASE_AMR"; + break; + case INIT_AMR: + message = "INIT_AMR"; + break; + case ME_COMPLETED: + message = "ME_COMPLETED"; + break; + case RESUME: + message = "RESUME"; + break; + case PAUSE: + message = "PAUSE"; + break; + case CANCEL: + message = "CANCEL"; + break; + case RELOCALIZATION_PASS: + message = "RELOCALIZATION_PASS"; + break; + default: + message = "INVALID"; + break; + } + return message; + } +}; +} // namespace amr_manager +} // namespace qrb +#endif // MESSAGE_HPP_ \ No newline at end of file diff --git a/qrb_amr_manager/include/message_queue.hpp b/qrb_amr_manager/include/message_queue.hpp new file mode 100644 index 0000000..5f20c15 --- /dev/null +++ b/qrb_amr_manager/include/message_queue.hpp @@ -0,0 +1,93 @@ +/* + * Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved. + * SPDX-License-Identifier: BSD-3-Clause-Clear + */ + +#ifndef MESSAGE_QUEUE_HPP_ +#define MESSAGE_QUEUE_HPP_ + +#include "message.hpp" + +#include +#include +#include +#include + +namespace qrb +{ +namespace amr_manager +{ + +class MessageQueue +{ +public: + void push(const Message & msg) + { + std::unique_lock lck(mtx_); + if (msg.type == Message::AMR_EXCEPTION) { + is_error_ = true; + error_code_ = msg.error_code; + } else if (msg.type == Message::LOW_POWER) { + is_low_power_ = true; + } else if (msg.type == Message::INIT_AMR) { + is_init_ = true; + } else if (msg.type == Message::RELEASE_AMR) { + is_release_ = true; + } else { + queue_.push(msg); + } + cv_.notify_one(); + } + + void wait(Message & msg) + { + std::unique_lock lck(mtx_); + while (!queue_.size() && !is_error_ && !is_low_power_ && !is_init_ && !is_release_) + cv_.wait(lck); + if (is_init_) { + msg.type = Message::INIT_AMR; + is_init_ = false; + } else if (is_release_) { + msg.type = Message::RELEASE_AMR; + is_release_ = false; + } else if (is_error_) { + msg.type = Message::AMR_EXCEPTION; + msg.error_code = error_code_; + is_error_ = false; + } else if (is_low_power_) { + msg.type = Message::LOW_POWER; + is_low_power_ = false; + } else { + msg = queue_.front(); + queue_.pop(); + } + } + + size_t size() + { + std::unique_lock lck(mtx_); + return queue_.size(); + } + + void notify() + { + std::unique_lock lck(mtx_); + Message msg; + msg.type = 0; + queue_.push(msg); + cv_.notify_one(); + } + +private: + std::queue queue_; + std::mutex mtx_; + std::condition_variable cv_; + bool is_error_ = false; + uint8_t error_code_ = 0; + bool is_low_power_ = false; + bool is_init_ = false; + bool is_release_ = false; +}; +} // namespace amr_manager +} // namespace qrb +#endif // MESSAGE_QUEUE_HPP_ \ No newline at end of file diff --git a/qrb_amr_manager/package.xml b/qrb_amr_manager/package.xml new file mode 100644 index 0000000..d5fbbe1 --- /dev/null +++ b/qrb_amr_manager/package.xml @@ -0,0 +1,25 @@ + + + + qrb_amr_manager + 1.0.0 + Manage AMR behavior + + BSD-3-Clause-Clear + + xiaowz + xiaowz + + ament_cmake + + rclcpp + rclcpp_action + geometry_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/qrb_amr_manager/src/amr_manager.cpp b/qrb_amr_manager/src/amr_manager.cpp new file mode 100644 index 0000000..5664b40 --- /dev/null +++ b/qrb_amr_manager/src/amr_manager.cpp @@ -0,0 +1,167 @@ +/* + * Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved. + * SPDX-License-Identifier: BSD-3-Clause-Clear + */ + +#include +#include +#include +#include +#include +#include + +#include "amr_manager.hpp" + +namespace qrb +{ +namespace amr_manager +{ + +AMRManager::AMRManager() +{ + printf("AMRManager:\n"); + init(); +} + +AMRManager::~AMRManager() {} + +void AMRManager::register_start_p2p_nav_callback(start_p2p_func_t cb) +{ + state_machine_->register_start_p2p_nav_callback(cb); +} + +void AMRManager::register_start_follow_path_callback(start_follow_path_func_t cb) +{ + state_machine_->register_start_follow_path_callback(cb); +} + +void AMRManager::register_start_waypoint_follow_path_callback(start_waypoint_follow_path_func_t cb) +{ + state_machine_->register_start_waypoint_follow_path_callback(cb); +} + +void AMRManager::register_change_mode_callback(change_mode_func_t cb) +{ + low_power_->register_change_mode_callback(cb); +} + +void AMRManager::register_sub_cmd_callback(sub_cmd_func_t cb) +{ + state_machine_->register_sub_cmd_callback(cb); +} + +void AMRManager::register_start_charging_callback(start_charging_func_t cb) +{ + state_machine_->register_start_charging_callback(cb); +} + +void AMRManager::register_notify_exception_callback(notify_exception_func_t cb) +{ + state_machine_->register_notify_exception_callback(cb); +} + +void AMRManager::register_send_amr_state_changed_callback(send_amr_state_changed_func_t cb) +{ + state_machine_->register_send_amr_state_changed_callback(cb); +} + +void AMRManager::register_navigate_to_charging_callback(navigate_to_charging_func_t cb) +{ + state_machine_->register_navigate_to_charging_callback(cb); +} + +void AMRManager::register_slam_command_callback(slam_command_func_t cb) +{ + state_machine_->register_slam_command_callback(cb); +} + +void AMRManager::register_publish_twist_callback(publish_twist_func_t cb) +{ + state_machine_->register_publish_twist_callback(cb); +} + +void AMRManager::process_event(int event) +{ + state_machine_->process_event(event, 0); +} + +void AMRManager::process_event(int event, int error_code) +{ + state_machine_->process_event(event, (uint8_t)error_code); +} + +bool AMRManager::check_potential_state(int cmd) +{ + return state_machine_->check_potential_state(cmd); +} + +void AMRManager::process_cmd(int cmd, void * buffer, size_t len) +{ + state_machine_->process_cmd(cmd, buffer, len); +} + +void AMRManager::process_cmd(int cmd, uint32_t goal_id, vector & ids) +{ + state_machine_->process_cmd(cmd, goal_id, ids); +} + +void AMRManager::init_amr() +{ + state_machine_->init_amr(); +} + +void AMRManager::release_amr() +{ + state_machine_->release_amr(); +} + +void AMRManager::me_completed() +{ + state_machine_->process_event(Message::ME_COMPLETED, 0); +} + +void AMRManager::process_sub_cmd(int msg) +{ + state_machine_->process_sub_cmd(msg); +} + +void AMRManager::notify_amr_state_changed(int state) +{ + low_power_->notify_amr_state_changed(state); +} + +void AMRManager::notify_battery_changed(float battery_vol) +{ + low_power_->notify_battery_changed(battery_vol); +} + +void AMRManager::notify_charging_state_changed(uint8_t state) +{ + low_power_->notify_charging_state_changed(state); +} + +bool AMRManager::load_map() +{ + return state_machine_->load_map(); +} + +bool AMRManager::start_mapping() +{ + return state_machine_->start_mapping(); +} + +bool AMRManager::stop_mapping() +{ + return state_machine_->stop_mapping(); +} + +void AMRManager::init() +{ + RCLCPP_INFO(logger_, "init"); + + state_machine_ = std::shared_ptr(new AMRStateMachine()); + + low_power_ = std::shared_ptr(new LowPowerManager(state_machine_)); +} +} // namespace amr_manager +} // namespace qrb diff --git a/qrb_amr_manager/src/amr_state_machine.cpp b/qrb_amr_manager/src/amr_state_machine.cpp new file mode 100644 index 0000000..08221a8 --- /dev/null +++ b/qrb_amr_manager/src/amr_state_machine.cpp @@ -0,0 +1,800 @@ +/* + * Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved. + * SPDX-License-Identifier: BSD-3-Clause-Clear + */ + +#include "amr_state_machine.hpp" +#include "unistd.h" + +namespace qrb +{ +namespace amr_manager +{ + +AMRStateMachine::AMRStateMachine() +{ + running_ = true; + current_state_ = IN_ACTIVE; + thread_handle_msg_ = + std::make_shared(std::mem_fn(&AMRStateMachine::handle_msg), this); + send_mapper_cmd_ = false; + send_navigator_cmd_ = false; +} + +AMRStateMachine::~AMRStateMachine() +{ + running_ = false; + queue_.notify(); + if (thread_handle_msg_->joinable()) { + thread_handle_msg_->join(); + } +} + +void AMRStateMachine::register_start_p2p_nav_callback(start_p2p_func_t cb) +{ + RCLCPP_INFO(logger_, "register_start_p2p_nav_callback"); + start_p2p_cb_ = cb; +} + +void AMRStateMachine::register_start_follow_path_callback(start_follow_path_func_t cb) +{ + RCLCPP_INFO(logger_, "register_start_follow_path_callback"); + start_follow_path_cb_ = cb; +} + +void AMRStateMachine::register_start_waypoint_follow_path_callback( + start_waypoint_follow_path_func_t cb) +{ + RCLCPP_INFO(logger_, "register_start_waypoint_follow_path_callback"); + start_waypoint_follow_path_cb_ = cb; +} + +void AMRStateMachine::register_sub_cmd_callback(sub_cmd_func_t cb) +{ + RCLCPP_INFO(logger_, "register_sub_cmd_callback"); + sub_cmd_cb_ = cb; +} + +void AMRStateMachine::register_start_charging_callback(start_charging_func_t cb) +{ + RCLCPP_INFO(logger_, "register_start_charging_callback"); + start_charging_cb_ = cb; +} + +void AMRStateMachine::register_notify_exception_callback(notify_exception_func_t cb) +{ + RCLCPP_INFO(logger_, "register_notify_exception_callback"); + notify_exception_cb_ = cb; +} + +void AMRStateMachine::register_send_amr_state_changed_callback(send_amr_state_changed_func_t cb) +{ + RCLCPP_INFO(logger_, "register_send_amr_state_changed_callback"); + send_amr_state_changed_cb_ = cb; +} + +void AMRStateMachine::register_navigate_to_charging_callback(navigate_to_charging_func_t cb) +{ + RCLCPP_INFO(logger_, "register_navigate_to_charging_callback"); + navigate_to_charging_cb_ = cb; +} + +void AMRStateMachine::register_slam_command_callback(slam_command_func_t cb) +{ + RCLCPP_INFO(logger_, "register_slam_command_callback"); + slam_command_cb_ = cb; +} + +void AMRStateMachine::register_publish_twist_callback(publish_twist_func_t cb) +{ + publish_twist_cb_ = cb; +} + +void AMRStateMachine::init_amr() +{ + Message msg; + msg.type = Message::INIT_AMR; + queue_.push(msg); +} + +void AMRStateMachine::release_amr() +{ + Message msg; + msg.type = Message::RELEASE_AMR; + queue_.push(msg); +} + +bool AMRStateMachine::load_map() +{ + bool result; + slam_command_cb_((uint8_t)Slam_Command::LoadMap, result); + if (!result) { + RCLCPP_INFO(logger_, "Load map failed"); + return false; + } + + send_me_finish_message(); + return true; +} + +bool AMRStateMachine::start_mapping() +{ + bool result; + slam_command_cb_((uint8_t)Slam_Command::StartMapping, result); + if (!result) { + RCLCPP_INFO(logger_, "Start mapping failed"); + return false; + } + + send_me_message(); + return true; +} + +bool AMRStateMachine::stop_mapping() +{ + bool result; + slam_command_cb_((uint8_t)Slam_Command::StopMapping, result); + if (!result) { + RCLCPP_INFO(logger_, "Stop mapping failed"); + return false; + } + + send_me_completed_message(); + return true; +} + +void AMRStateMachine::process_cmd(int cmd, void * buffer, size_t len) +{ + Message msg; + msg.type = cmd; + msg.param = buffer; + msg.len = len; + queue_.push(msg); +} + +void AMRStateMachine::process_cmd(int cmd, uint32_t goal_id, vector & ids) +{ + Message msg; + msg.type = cmd; + msg.goal_id = goal_id; + msg.ids.assign(ids.begin(), ids.end()); + queue_.push(msg); +} + +void AMRStateMachine::process_sub_cmd(int message) +{ + Message msg; + msg.type = message; + queue_.push(msg); +} + +void AMRStateMachine::process_event(int event, uint8_t error_code) +{ + Message msg; + msg.type = event; + msg.error_code = error_code; + queue_.push(msg); +} + +void AMRStateMachine::process_event(int event) +{ + Message msg; + msg.type = event; + queue_.push(msg); +} + +void AMRStateMachine::handle_msg() +{ + Message msg; + bool execute = true; + while (running_) { + queue_.wait(msg); + execute = handle_message(msg); + if (!execute) { + handle_failed(msg); + continue; + } + } +} + +bool AMRStateMachine::handle_message(const Message & msg) +{ + RCLCPP_INFO(logger_, "Receive message: %s", Message::msg_to_string(msg.type).c_str()); + int last_state = current_state_; + switch (msg.type) { + case Message::INIT_AMR: + if (current_state_ == AMRStateMachine::IN_ACTIVE) { + if (!has_map_) { + update_state(last_state, AMRStateMachine::IDLE); + } else { + update_state(last_state, AMRStateMachine::READY); + } + } + break; + case Message::RELEASE_AMR: + if (current_state_ == AMRStateMachine::ON_AE) { + // TODO: + } else if (current_state_ == AMRStateMachine::ON_P2PNAV) { + sub_cmd_cb_(true, SubCommand::CANCEL); + } else if (current_state_ == AMRStateMachine::ON_FOLLOW_PATH) { + sub_cmd_cb_(false, SubCommand::CANCEL); + } + update_state(last_state, AMRStateMachine::IN_ACTIVE); + break; + case Message::ME_COMPLETED: + if (current_state_ != AMRStateMachine::ON_ME) { + return false; + } + update_state(last_state, AMRStateMachine::ME_DONE); + enter_me_done_state(); + break; + case Message::ME: + if (current_state_ != AMRStateMachine::IDLE && current_state_ != AMRStateMachine::READY) { + return false; + } + update_state(last_state, AMRStateMachine::ON_ME); + enter_on_me_state(); + break; + case Message::AE: + if (current_state_ != AMRStateMachine::IDLE && current_state_ != AMRStateMachine::READY) { + return false; + } + update_state(last_state, AMRStateMachine::ON_AE); + enter_on_ae_state(); + break; + case Message::P2PNAV: + if (current_state_ != AMRStateMachine::READY && + current_state_ != AMRStateMachine::FOLLOW_PATH_WAIT && + current_state_ != AMRStateMachine::P2PNAV_WAIT) { + return false; + } + if (current_state_ == AMRStateMachine::FOLLOW_PATH_WAIT) { + RCLCPP_INFO(logger_, "Cancel last follow path"); + sub_cmd_cb_(false, SubCommand::CANCEL); + usleep(2 * 1000 * 1000); + } + + update_state(last_state, AMRStateMachine::ON_P2PNAV); + enter_on_p2p_state(msg); + break; + case Message::FOLLOW_PATH: + if (current_state_ != AMRStateMachine::READY && + current_state_ != AMRStateMachine::P2PNAV_WAIT && + current_state_ != AMRStateMachine::FOLLOW_PATH_WAIT) { + return false; + } + if (current_state_ == AMRStateMachine::P2PNAV_WAIT) { + RCLCPP_INFO(logger_, "Cancel last p2p navigation"); + sub_cmd_cb_(true, SubCommand::CANCEL); + usleep(2 * 1000 * 1000); + } + + update_state(last_state, AMRStateMachine::ON_FOLLOW_PATH); + enter_on_follow_path_state(msg); + break; + case Message::WAYPOINT_FOLLOW_PATH: + if (current_state_ != AMRStateMachine::READY && + current_state_ != AMRStateMachine::P2PNAV_WAIT && + current_state_ != AMRStateMachine::FOLLOW_PATH_WAIT) { + return false; + } + update_state(last_state, AMRStateMachine::ON_FOLLOW_PATH); + enter_on_waypoint_follow_path_state(msg); + break; + case Message::AE_FINISH: + if (current_state_ == AMRStateMachine::ON_AE || current_state_ == AMRStateMachine::IDLE) { + update_state(last_state, AMRStateMachine::LOCALIZATION); + has_map_ = true; + } + break; + case Message::ME_FINISH: + if ((current_state_ == AMRStateMachine::IDLE) || + (current_state_ == AMRStateMachine::ME_DONE)) { + update_state(last_state, AMRStateMachine::LOCALIZATION); + has_map_ = true; + enter_localization_state(); + } + break; + case Message::RELOCALIZATION_PASS: + if (current_state_ == AMRStateMachine::LOCALIZATION) { + update_state(last_state, AMRStateMachine::READY); + } + break; + case Message::P2PNAV_FINISH: + if (current_state_ == AMRStateMachine::ON_P2PNAV) { + update_state(last_state, AMRStateMachine::READY); + } + break; + case Message::FOLLOW_PATH_FINISH: + if (current_state_ == AMRStateMachine::ON_FOLLOW_PATH) { + update_state(last_state, AMRStateMachine::READY); + } + break; + case Message::LOW_POWER: + if (current_state_ == AMRStateMachine::ON_ME) { + if (!has_map_) { + update_state(last_state, AMRStateMachine::IDLE); + enter_idle_state(); + } else { + enter_ready_state(); + update_state(last_state, AMRStateMachine::READY); + } + } else if (current_state_ == AMRStateMachine::READY || + current_state_ == AMRStateMachine::ON_FOLLOW_PATH || + current_state_ == AMRStateMachine::FOLLOW_PATH_WAIT || + current_state_ == AMRStateMachine::ON_P2PNAV || + current_state_ == AMRStateMachine::P2PNAV_WAIT) { + update_state(last_state, AMRStateMachine::READY); + enter_ready_state(); + send_return_charging_message(); + } + break; + case Message::NORMAL_POWER: + if (current_state_ == AMRStateMachine::LOW_POWER_CHARGING) { + update_state(last_state, AMRStateMachine::READY); + enter_ready_state(); + } + break; + case Message::AMR_EXCEPTION: { + enter_on_error_state(); + update_state(last_state, AMRStateMachine::ON_ERROR); + uint8_t error_code = msg.error_code; + notify_exception_cb_(true, error_code); + } break; + case Message::AMR_NORMAL: + if (current_state_ == AMRStateMachine::ON_ERROR) { + update_state(last_state, AMRStateMachine::READY); + enter_ready_state(); + notify_exception_cb_(false, 0); + } + break; + case Message::CANCEL: + if (current_state_ == AMRStateMachine::ON_ME) { + if (!has_map_) { + update_state(last_state, AMRStateMachine::IDLE); + enter_idle_state(); + } else { + enter_ready_state(); + update_state(last_state, AMRStateMachine::READY); + } + } else if (current_state_ == AMRStateMachine::ON_FOLLOW_PATH || + current_state_ == AMRStateMachine::ON_P2PNAV || + current_state_ == AMRStateMachine::P2PNAV_WAIT || + current_state_ == AMRStateMachine::FOLLOW_PATH_WAIT || + current_state_ == AMRStateMachine::ON_RETURN_CHARGING) { + enter_ready_state(); + update_state(last_state, AMRStateMachine::READY); + } else { + return false; + } + break; + case Message::PAUSE: + RCLCPP_INFO(logger_, "Message::PAUSE:,current_state_=%d", current_state_); + if (current_state_ == AMRStateMachine::ON_FOLLOW_PATH) { + update_state(last_state, AMRStateMachine::FOLLOW_PATH_WAIT); + enter_follow_path_wait_state(); + } else if (current_state_ == AMRStateMachine::ON_P2PNAV) { + update_state(last_state, AMRStateMachine::P2PNAV_WAIT); + enter_p2p_wait_state(); + } else { + return false; + } + break; + case Message::RESUME: + if (current_state_ == AMRStateMachine::FOLLOW_PATH_WAIT) { + update_state(last_state, AMRStateMachine::ON_FOLLOW_PATH); + sub_cmd_cb_(false, SubCommand::RESUME); + } else if (current_state_ == AMRStateMachine::P2PNAV_WAIT) { + update_state(last_state, AMRStateMachine::ON_P2PNAV); + sub_cmd_cb_(true, SubCommand::RESUME); + } else { + return false; + } + break; + case Message::RETURN_CHARGING_FINISH: + if (current_state_ == AMRStateMachine::ON_RETURN_CHARGING) { + update_state(last_state, AMRStateMachine::LOW_POWER_CHARGING); + enter_low_power_charging_state(); + destory_return_charging_station_thread(); + } else { + return false; + } + break; + case Message::RETURN_CHARGING: + if (current_state_ == AMRStateMachine::READY) { + update_state(last_state, AMRStateMachine::ON_RETURN_CHARGING); + enter_on_return_charging_state(); + } else { + return false; + } + break; + default: + break; + } + RCLCPP_INFO(logger_, "Finish handle_message"); + return true; +} + +void AMRStateMachine::notify_state_machine_changed() +{ + RCLCPP_INFO(logger_, "send_amr_state_changed:%s", get_current_state().c_str()); + if (send_amr_state_changed_cb_ != nullptr) { + send_amr_state_changed_cb_(current_state_); + } else { + RCLCPP_ERROR(logger_, "send_amr_state_changed_cb_ is null"); + } +} + +void AMRStateMachine::handle_failed(const Message & msg) +{ + // TODO: +} + +std::string AMRStateMachine::get_current_state() +{ + return state_to_string(current_state_); +} + +void AMRStateMachine::enter_idle_state() +{ + if (current_state_ == AMRStateMachine::ON_AE) { + // TODO: + return; + } +} + +void AMRStateMachine::enter_ready_state() +{ + if (send_navigator_cmd_) { + if ((current_state_ == AMRStateMachine::FOLLOW_PATH_WAIT) || + (current_state_ == AMRStateMachine::ON_FOLLOW_PATH)) { + RCLCPP_INFO(logger_, "Cancel follow path"); + sub_cmd_cb_(false, SubCommand::CANCEL); + } else if ((current_state_ == AMRStateMachine::ON_P2PNAV) || + (current_state_ == AMRStateMachine::P2PNAV_WAIT)) { + RCLCPP_INFO(logger_, "Cancel p2p navigaiton"); + sub_cmd_cb_(true, SubCommand::CANCEL); + } else if (current_state_ == AMRStateMachine::ON_RETURN_CHARGING) { + RCLCPP_INFO(logger_, "Cancel return charging station"); + sub_cmd_cb_(true, SubCommand::CANCEL); + destory_return_charging_station_thread(); + } + send_navigator_cmd_ = false; + } else if (send_mapper_cmd_) { + RCLCPP_INFO(logger_, "Cancel auto mapping navigaiton"); + // TODO: + send_mapper_cmd_ = false; + } +} + +void AMRStateMachine::enter_me_done_state() +{ + send_mapper_cmd_ = true; + save_map(); + send_me_finish_message(); +} + +void AMRStateMachine::enter_on_me_state() +{ + send_mapper_cmd_ = true; +} + +void AMRStateMachine::enter_on_ae_state() +{ + send_mapper_cmd_ = true; + // TODO: +} + +void AMRStateMachine::enter_on_p2p_state(const Message & msg) +{ + send_navigator_cmd_ = true; + start_charging_cb_(false); + start_p2p_cb_(msg.param); +} + +void AMRStateMachine::enter_on_follow_path_state(const Message & msg) +{ + send_navigator_cmd_ = true; + path_buffer_ = msg.param; + start_charging_cb_(false); + start_follow_path_cb_(path_buffer_); +} + +void AMRStateMachine::enter_on_waypoint_follow_path_state(const Message & msg) +{ + send_navigator_cmd_ = true; + uint32_t goal_id = msg.goal_id; + vector ids; + ids.assign(msg.ids.begin(), msg.ids.end()); + start_charging_cb_(false); + start_waypoint_follow_path_cb_(goal_id, ids); +} + +void AMRStateMachine::enter_on_error_state() +{ + if (send_navigator_cmd_) { + RCLCPP_INFO(logger_, "Send cancel nav command when amr is error, state = %d", current_state_); + if (current_state_ == AMRStateMachine::FOLLOW_PATH_WAIT || + current_state_ == AMRStateMachine::ON_FOLLOW_PATH) { + sub_cmd_cb_(false, SubCommand::CANCEL); + } else if (current_state_ == AMRStateMachine::ON_P2PNAV || + current_state_ == AMRStateMachine::P2PNAV_WAIT) { + sub_cmd_cb_(true, SubCommand::CANCEL); + } else { + RCLCPP_INFO(logger_, "Check the nav state when amr is error"); + } + send_navigator_cmd_ = false; + } else if (send_mapper_cmd_) { + // TODO: + RCLCPP_INFO(logger_, "Send cancel mapping command when amr is error"); + send_mapper_cmd_ = false; + } else { + RCLCPP_INFO(logger_, "Nothing to do when amr is error"); + } +} + +void AMRStateMachine::enter_follow_path_wait_state() +{ + sub_cmd_cb_(false, SubCommand::PAUSE); +} + +void AMRStateMachine::enter_on_return_charging_state() +{ + send_navigator_cmd_ = true; + create_return_charging_station_thread(); +} + +void AMRStateMachine::create_return_charging_station_thread() +{ + // create sub thread + RCLCPP_INFO(logger_, "Create return charging station thread"); + auto fun = [this]() -> void { + pthread_setname_np(pthread_self(), "return_charging"); + return_charging_station_function(this); + }; + return_charging_station_thread_ = std::make_shared(fun); + RCLCPP_INFO(logger_, "Create return charging station thread successfully"); +} + +void AMRStateMachine::return_charging_station_function(void * arg) +{ + AMRStateMachine * p = (AMRStateMachine *)arg; + RCLCPP_INFO(p->logger_, "return_charging_station_function"); + return_charging_station(); + start_charging_cb_(true); +} + +void AMRStateMachine::destory_return_charging_station_thread() +{ + RCLCPP_INFO(logger_, "destory_return_charging_station_thread"); + if (return_charging_station_thread_ && return_charging_station_thread_->joinable()) { + return_charging_station_thread_->join(); + RCLCPP_INFO(logger_, "return charging station thread end"); + return_charging_station_thread_ = nullptr; + } + RCLCPP_INFO(logger_, "finish destory return charging station thread"); +} + +void AMRStateMachine::enter_p2p_wait_state() +{ + // cancel goal, start goal again when resume + sub_cmd_cb_(true, SubCommand::PAUSE); +} + +void AMRStateMachine::enter_low_power_charging_state() +{ + // TODO: +} + +void * AMRStateMachine::get_current_path() +{ + return path_buffer_; +} + +void AMRStateMachine::return_charging_station() +{ + navigate_to_charging_cb_(); +} + +bool AMRStateMachine::check_potential_state(int cmd) +{ + RCLCPP_INFO(logger_, "Receive cmd: %s, current_state:%s", Command::cmd_to_string(cmd).c_str(), + get_current_state().c_str()); + if (current_state_ == ON_ERROR) { + RCLCPP_ERROR(logger_, "check_potential_state(%d,%d) return false", cmd, current_state_); + return false; + } + + if (cmd == Command::OTHER) { + RCLCPP_INFO(logger_, "check_potential_state(%d,%d) return true", cmd, current_state_); + return true; + } + if (((cmd == Command::AE) || (cmd == Command::ME)) && + ((current_state_ == IDLE) || (current_state_ == READY))) { + RCLCPP_INFO(logger_, "check_potential_state(%d,%d) return true", cmd, current_state_); + return true; + } + if ((cmd == Command::P2PNAV) && + ((current_state_ == READY) || (current_state_ == FOLLOW_PATH_WAIT) || + (current_state_ == P2PNAV_WAIT))) { + RCLCPP_INFO(logger_, "check_potential_state(%d,%d) return true", cmd, current_state_); + return true; + } + if (((cmd == Command::FOLLOW_PATH) || (cmd == Command::WAYPOINT_FOLLOW_PATH)) && + ((current_state_ == READY) || (current_state_ == FOLLOW_PATH_WAIT) || + (current_state_ == P2PNAV_WAIT))) { + RCLCPP_INFO(logger_, "check_potential_state(%d,%d) return true", cmd, current_state_); + return true; + } + if ((cmd == Command::CHARGING) && ((current_state_ == IDLE) || (current_state_ == READY))) { + RCLCPP_INFO(logger_, "check_potential_state(%d,%d) return true", cmd, current_state_); + return true; + } + if ((cmd == Command::SUB_CMD) && + ((current_state_ == ON_P2PNAV) || (current_state_ == ON_FOLLOW_PATH) || + (current_state_ == P2PNAV_WAIT) || (current_state_ == FOLLOW_PATH_WAIT) || + (current_state_ == ON_RETURN_CHARGING) || (current_state_ == ON_ME))) { + RCLCPP_INFO(logger_, "check_potential_state(%d,%d) return true", cmd, current_state_); + return true; + } + + RCLCPP_INFO(logger_, "check_potential_state(%d,%d) return false", cmd, current_state_); + return false; +} + +void AMRStateMachine::save_map() +{ + if (slam_command_cb_ == nullptr) { + RCLCPP_ERROR(logger_, "slam_command_cb_ is nullptr"); + return; + } + + bool result; + slam_command_cb_((uint8_t)Slam_Command::SaveMap, result); + if (!result) { + RCLCPP_ERROR(logger_, "Save map failed"); + } +} + +void AMRStateMachine::enter_localization_state() +{ + bool result = enter_localization_mode(); + if (!result) { + RCLCPP_ERROR(logger_, "Enter localization mode failed"); + return; + } + + result = get_relocalization_state(); + if (result) { + RCLCPP_INFO(logger_, "Current localization is ready"); + send_relocalization_pass_message(); + return; + } + + start_rotation(); + + while (true) { + usleep(1000 * 1000); // sleep 1s + RCLCPP_INFO(logger_, "Wait 1 second"); + result = get_relocalization_state(); + if (result) { + RCLCPP_INFO(logger_, "Current localization is ready"); + stop_rotation(); + send_relocalization_pass_message(); + return; + } + } +} + +bool AMRStateMachine::enter_localization_mode() +{ + if (slam_command_cb_ == nullptr) { + RCLCPP_ERROR(logger_, "slam_command_cb_ is nullptr"); + return false; + } + + bool result; + slam_command_cb_((uint8_t)Slam_Command::StartLocalization, result); + if (result) { + RCLCPP_INFO(logger_, "Enter localization mode success"); + } else { + RCLCPP_ERROR(logger_, "Enter localization mode failed"); + } + return result; +} + +bool AMRStateMachine::get_relocalization_state() +{ + if (slam_command_cb_ == nullptr) { + RCLCPP_ERROR(logger_, "slam_command_cb_ is nullptr"); + return false; + } + + bool result; + slam_command_cb_((uint8_t)Slam_Command::Relocalization, result); + if (result) { + RCLCPP_INFO(logger_, "relocalization is pass"); + } else { + RCLCPP_ERROR(logger_, "relocalization is failed"); + } + return result; +} + +void AMRStateMachine::start_rotation() +{ + geometry_msgs::msg::Twist twist; + twist.linear.x = 0; + twist.linear.y = 0; + twist.linear.z = 0; + twist.angular.x = 0; + twist.angular.y = 0; + twist.angular.z = ANGULAR_VELOCITY; + if (publish_twist_cb_ != nullptr) { + publish_twist_cb_(twist); + } else { + RCLCPP_ERROR(logger_, "publish_twist_cb_ is nullptr"); + } +} + +void AMRStateMachine::stop_rotation() +{ + geometry_msgs::msg::Twist twist; + twist.linear.x = 0; + twist.linear.y = 0; + twist.linear.z = 0; + twist.angular.x = 0; + twist.angular.y = 0; + twist.angular.z = 0; + if (publish_twist_cb_ != nullptr) { + publish_twist_cb_(twist); + } else { + RCLCPP_ERROR(logger_, "publish_twist_cb_ is nullptr"); + } +} + +void AMRStateMachine::send_me_finish_message() +{ + Message msg; + msg.type = Message::ME_FINISH; + queue_.push(msg); +} + +void AMRStateMachine::send_me_message() +{ + Message msg; + msg.type = Message::ME; + queue_.push(msg); +} + +void AMRStateMachine::send_me_completed_message() +{ + Message msg; + msg.type = Message::ME_COMPLETED; + queue_.push(msg); +} + +void AMRStateMachine::send_relocalization_pass_message() +{ + Message msg; + msg.type = Message::RELOCALIZATION_PASS; + queue_.push(msg); +} + +void AMRStateMachine::send_return_charging_message() +{ + Message msg; + msg.type = Message::RETURN_CHARGING; + queue_.push(msg); +} + +void AMRStateMachine::update_state(int last_state, int state) +{ + current_state_ = state; + if (last_state != current_state_) { + notify_state_machine_changed(); + } + RCLCPP_INFO(logger_, "current state: %s, last state: %s", get_current_state().c_str(), + state_to_string(last_state).c_str()); +} +} // namespace amr_manager +} // namespace qrb \ No newline at end of file diff --git a/qrb_amr_manager/src/low_power_manager.cpp b/qrb_amr_manager/src/low_power_manager.cpp new file mode 100644 index 0000000..9efa829 --- /dev/null +++ b/qrb_amr_manager/src/low_power_manager.cpp @@ -0,0 +1,104 @@ +/* + * Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved. + * SPDX-License-Identifier: BSD-3-Clause-Clear + */ + +#include "low_power_manager.hpp" + +#define LOW_POWER_VOLTAGE_LEVEL 22 + +namespace qrb +{ +namespace amr_manager +{ + +LowPowerManager::LowPowerManager(std::shared_ptr & state_machine) + : state_machine_(state_machine) +{ +} + +LowPowerManager::~LowPowerManager() {} + +void LowPowerManager::register_change_mode_callback(change_mode_func_t cb) +{ + RCLCPP_INFO(logger_, "register_change_mode_callback"); + change_mode_cb_ = cb; +} + +void LowPowerManager::notify_amr_state_changed(int state) +{ + RCLCPP_INFO(logger_, "notify_amr_state_changed"); + std::unique_lock lck(mtx_); + amr_state_ = state; +} + +void LowPowerManager::notify_battery_changed(float battery_vol) +{ + if (battery_vol <= LOW_POWER_VOLTAGE_LEVEL) { + RCLCPP_INFO(logger_, "Start return charging station"); + send_event(Message::LOW_POWER); + } + current_battery_voltage_level_ = battery_vol; +} + +void LowPowerManager::notify_charging_state_changed(uint8_t state) +{ + charging_station_ = state; + switch (state) { + case (uint8_t)ChargerState::IDLE: + RCLCPP_INFO(logger_, "Charger idle"); + set_navigation_mode(); + break; + case (uint8_t)ChargerState::SEARCHING: + RCLCPP_INFO(logger_, "Searching..."); + break; + case (uint8_t)ChargerState::CONTROLLING: + RCLCPP_INFO(logger_, "Controlling..."); + set_charger_mode(); + break; + case (uint8_t)ChargerState::FORCE_CHARGING: + RCLCPP_INFO(logger_, "Attached charger pie"); + send_event(Message::RETURN_CHARGING_FINISH); + break; + case (uint8_t)ChargerState::CHARGING: + RCLCPP_INFO(logger_, "Charging..."); + send_event(Message::NORMAL_POWER); + set_navigation_mode(); + break; + case (uint8_t)ChargerState::CHARGING_DONE: + RCLCPP_INFO(logger_, "Charging done"); + set_navigation_mode(); + break; + case (uint8_t)ChargerState::ERROR: + RCLCPP_ERROR(logger_, "Charging station is error"); + set_navigation_mode(); + break; + default: + break; + } +} + +void LowPowerManager::send_event(const int event) +{ + state_machine_->process_event(event); +} + +void LowPowerManager::set_charger_mode() +{ + if (get_amr_state() == AMRStateMachine::ON_RETURN_CHARGING) { + change_mode_cb_((uint8_t)SetControlMode::CHARGER); + } +} + +void LowPowerManager::set_navigation_mode() +{ + change_mode_cb_((uint8_t)SetControlMode::APPLICATION); +} + +int LowPowerManager::get_amr_state() +{ + std::unique_lock lck(mtx_); + return amr_state_; +} +} // namespace amr_manager +} // namespace qrb \ No newline at end of file diff --git a/qrb_ros_amr/CMakeLists.txt b/qrb_ros_amr/CMakeLists.txt new file mode 100644 index 0000000..6652615 --- /dev/null +++ b/qrb_ros_amr/CMakeLists.txt @@ -0,0 +1,101 @@ +cmake_minimum_required(VERSION 3.8) +project(qrb_ros_amr) + +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_auto REQUIRED) +find_package(qrb_ros_amr_msgs REQUIRED) +find_package(qrb_ros_navigation_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(nav2_msgs REQUIRED) +find_package(nav_2d_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rcutils REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(qrb_ros_robot_base_msgs REQUIRED) +find_package(qrb_ros_slam_msgs REQUIRED) +ament_auto_find_build_dependencies() + +include_directories( + ${PROJECT_SOURCE_DIR}/include + ${PROJECT_SOURCE_DIR}/include/topic + ${PROJECT_SOURCE_DIR}/include/service + ${PROJECT_SOURCE_DIR}/include/action + ${PROJECT_SOURCE_DIR}/include/amr_manager/ + ${EIGEN3_INCLUDE_DIRS} + ${PROJECT_SOURCE_DIR}/../install/qrb_amr_manager/include/qrb_amr_manager/ + ${PROJECT_SOURCE_DIR}/../install/qrb_amr_manager/include/qrb_amr_manager/include/ + ${PROJECT_SOURCE_DIR}/../install/include/qrb_amr_manager/ + ${PROJECT_SOURCE_DIR}/../install/include/qrb_amr_manager/include/ + ${CMAKE_SYSROOT}/usr/include/qrb_amr_manager/ + ${CMAKE_SYSROOT}/usr/include/qrb_amr_manager/include/ + ) +include_directories(${JSON_INC_PATH}) + +set(source + src/main.cpp + src/amr_controller.cpp + src/topic/amr_status_transporter.cpp + src/topic/developer_mode_subscriber.cpp + src/topic/exception_subscriber.cpp + src/service/api_service_server.cpp + src/service/sub_cmd_service_server.cpp + src/service/cartographer_service_client.cpp + src/service/amr_mode_service_client.cpp + src/service/mapping_service_server.cpp + src/action/auto_mapper_action_client.cpp + src/action/cmd_action_server.cpp + src/action/navigation_action_client.cpp +) + + +aux_source_directory(./src SRC_LIST) + +ament_auto_add_executable(qrb_ros_amr ${SRC_LIST} ${source}) + +ament_target_dependencies(qrb_ros_amr + "qrb_ros_amr_msgs" + "qrb_ros_navigation_msgs" + "nav_msgs" + "nav2_msgs" + "nav_2d_msgs" + "geometry_msgs" + "rclcpp" + "rclcpp_action" + "std_msgs" + "rcutils" + "tf2" + "tf2_ros" + "tf2_geometry_msgs" + "sensor_msgs" + "qrb_ros_robot_base_msgs" + "qrb_ros_slam_msgs" +) + + + +target_link_libraries(qrb_ros_amr + "${PROJECT_SOURCE_DIR}/../build/qrb_amr_manager/libqrb_amr_manager.so" +) + +install(TARGETS qrb_ros_amr + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + + + +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ +) + +ament_package() diff --git a/qrb_ros_amr/include/action/auto_mapper_action_client.hpp b/qrb_ros_amr/include/action/auto_mapper_action_client.hpp new file mode 100644 index 0000000..6c7699d --- /dev/null +++ b/qrb_ros_amr/include/action/auto_mapper_action_client.hpp @@ -0,0 +1,48 @@ +/* + * Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved. + * SPDX-License-Identifier: BSD-3-Clause-Clear + */ + +#ifndef AUTO_MAPPER_ACTION_CLIENT_HPP_ +#define AUTO_MAPPER_ACTION_CLIENT_HPP_ + +#include "rclcpp_action/rclcpp_action.hpp" +#include "qrb_ros_amr_msgs/action/ae.hpp" +#include "amr_manager.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include + +using AE = qrb_ros_amr_msgs::action::AE; +using GoalHandleAE = rclcpp_action::ClientGoalHandle; +using Pose = geometry_msgs::msg::PoseStamped; + +using namespace qrb::amr_manager; + +namespace qrb_ros +{ +namespace amr +{ + +class AutoMapperActionClient : public rclcpp::Node +{ +private: + rclcpp_action::Client::SharedPtr client_ptr_; + std::shared_ptr amr_manager_; + + void create_client(); + void send_goal(); + void goal_response_callback(GoalHandleAE::SharedPtr goal_handle); + void feedback_callback(GoalHandleAE::SharedPtr, + const std::shared_ptr feedback); + void result_callback(const GoalHandleAE::WrappedResult & result); + +public: + AutoMapperActionClient(std::shared_ptr & amr_manager, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + ~AutoMapperActionClient(); + void start_ae(); + void cancel_goal(); +}; +} // namespace amr +} // namespace qrb_ros +#endif // AUTO_MAPPER_ACTION_CLIENT_HPP_ \ No newline at end of file diff --git a/qrb_ros_amr/include/action/cmd_action_server.hpp b/qrb_ros_amr/include/action/cmd_action_server.hpp new file mode 100644 index 0000000..b073411 --- /dev/null +++ b/qrb_ros_amr/include/action/cmd_action_server.hpp @@ -0,0 +1,100 @@ +/* + * Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved. + * SPDX-License-Identifier: BSD-3-Clause-Clear + */ + +#ifndef CMD_ACTION_SERVER_HPP_ +#define CMD_ACTION_SERVER_HPP_ + +#include "rclcpp_action/rclcpp_action.hpp" +#include "qrb_ros_amr_msgs/action/cmd.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "amr_manager.hpp" +#include +#include + +using Pose = geometry_msgs::msg::PoseStamped; +using Cmd = qrb_ros_amr_msgs::action::Cmd; +using GoalHandleCmd = rclcpp_action::ServerGoalHandle; + +using namespace qrb::amr_manager; + +namespace qrb_ros +{ +namespace amr +{ + +class Command +{ +public: + const static int ME = 1; + const static int AE = 2; + const static int P2PNAV = 3; + const static int FOLLOW_PATH = 4; + const static int CHARGING = 5; + const static int WaypointFollowPath = 6; + const static int SUB_CMD = 7; + const static int OTHER = 8; + + static std::string cmd_to_string(int cmd) + { + std::string message; + switch (cmd) { + case ME: + message = "ME"; + break; + case AE: + message = "AE"; + break; + case P2PNAV: + message = "P2PNAV"; + break; + case FOLLOW_PATH: + message = "FOLLOW_PATH"; + break; + case CHARGING: + message = "CHARGING"; + break; + case SUB_CMD: + message = "SUB_CMD"; + break; + case OTHER: + message = "OTHER"; + break; + default: + message = "INVAILD"; + break; + } + return message; + } +}; + +class CmdActionServer : public rclcpp::Node +{ +private: + rclcpp_action::Server::SharedPtr server_ptr_; + std::shared_ptr amr_manager_; + bool is_waiting_command_; + void create_server(); + rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal); + rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr goal_handle); + static std::shared_ptr record_goal_; + + void handle_accepted(const std::shared_ptr goal_handle); + void execute(const std::shared_ptr goal_handle); + void set_waiting_command_flag(bool flag); + + rclcpp::Logger logger_{ rclcpp::get_logger("cmd_action_server") }; + +public: + CmdActionServer(std::shared_ptr & amr_manager, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + ~CmdActionServer(); + bool is_waiting_command_feedback(); + static void command_status_completed(bool cmd_status); + static void motion_pose_changed(Pose pose); +}; +} // namespace amr +} // namespace qrb_ros +#endif // CMD_ACTION_SERVER_HPP_ \ No newline at end of file diff --git a/qrb_ros_amr/include/action/navigation_action_client.hpp b/qrb_ros_amr/include/action/navigation_action_client.hpp new file mode 100644 index 0000000..e930b97 --- /dev/null +++ b/qrb_ros_amr/include/action/navigation_action_client.hpp @@ -0,0 +1,110 @@ +/* + * Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved. + * SPDX-License-Identifier: BSD-3-Clause-Clear + */ + +#ifndef NAVIGATION_ACTION_CLIENT_HPP_ +#define NAVIGATION_ACTION_CLIENT_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "nav2_msgs/action/navigate_to_pose.hpp" +#include "nav2_msgs/action/follow_path.hpp" +#include "qrb_ros_navigation_msgs/action/follow_path.hpp" +#include "qrb_ros_amr_msgs/srv/sub_cmd.hpp" +#include "nav_msgs/msg/path.hpp" +#include "amr_manager.hpp" +#include + +using PoseStamped = geometry_msgs::msg::PoseStamped; +using Path = nav_msgs::msg::Path; +using FollowPath = nav2_msgs::action::FollowPath; +using WaypointFollowPath = qrb_ros_navigation_msgs::action::FollowPath; +using P2P = nav2_msgs::action::NavigateToPose; +using GoalHandleFollow = rclcpp_action::ClientGoalHandle; +using GoalHandleWaypointFollowPath = rclcpp_action::ClientGoalHandle; +using GoalHandleP2P = rclcpp_action::ClientGoalHandle; + +using SubCmd = qrb_ros_amr_msgs::srv::SubCmd; +using namespace qrb::amr_manager; + +#define SERVICE_TIMEOUT_DURATION 5 // timeout is 5 seconds + +namespace qrb_ros +{ +namespace amr +{ + +class NavigationActionClient : public rclcpp::Node +{ +private: + PoseStamped temp_pose_; + Path temp_path_; + PoseStamped charging_station_; + std::shared_ptr amr_manager_; + + rclcpp_action::Client::SharedPtr follow_path_client_ptr_; + rclcpp_action::Client::SharedPtr waypoint_follow_path_client_ptr_; + rclcpp_action::Client::SharedPtr p2p_client_ptr_; + rclcpp_action::GoalUUID p2p_uuid_; + rclcpp_action::GoalUUID follow_uuid_; + bool is_pausing_ = false; + bool is_return_charging_ = false; + std::mutex mtx_; + std::condition_variable cv_; + + std::shared_ptr> follow_path_service_client_; + bool response_result_; + qrb::amr_manager::start_p2p_func_t p2p_nav_callback_; + qrb::amr_manager::start_follow_path_func_t follow_path_callback_; + qrb::amr_manager::start_waypoint_follow_path_func_t waypoint_follow_path_callback_; + qrb::amr_manager::sub_cmd_func_t sub_cmd_callback_; + qrb::amr_manager::navigate_to_charging_func_t navigate_to_charging_callback_; + rclcpp::Logger logger_{ rclcpp::get_logger("navigation_action_client") }; + + void create_nav_client(); + void p2p_send_goal(void * buffer = NULL); + void follow_send_goal(void * path); + void waypoint_follow_send_goal(uint32_t goal, std::vector & ids); + + void p2p_goal_response_callback(GoalHandleP2P::SharedPtr goal_handle); + void p2p_feedback_callback(GoalHandleP2P::SharedPtr, + const std::shared_ptr feedback); + void p2p_result_callback(const GoalHandleP2P::WrappedResult & result); + void follow_goal_response_callback(GoalHandleFollow::SharedPtr goal_handle); + void follow_feedback_callback(GoalHandleFollow::SharedPtr, + const std::shared_ptr feedback); + void follow_result_callback(const GoalHandleFollow::WrappedResult & result); + void waypoint_follow_goal_response_callback(GoalHandleWaypointFollowPath::SharedPtr goal_handle); + void waypoint_follow_feedback_callback(GoalHandleWaypointFollowPath::SharedPtr, + const std::shared_ptr feedback); + void waypoint_follow_result_callback(const GoalHandleWaypointFollowPath::WrappedResult & result); + + void charging_station_goal_response_callback(GoalHandleP2P::SharedPtr goal_handle); + void charging_station_feedback_callback(GoalHandleP2P::SharedPtr, + const std::shared_ptr feedback); + void charging_station_result_callback(const GoalHandleP2P::WrappedResult & result); + void generate_pose(double & x, double & y, double & z, PoseStamped & pose); + void handle_sub_cmd(bool p2p, uint8_t sub_cmd); + + bool wait_for_service(); + bool send_and_wait_for_request(SubCmd::Request::SharedPtr request); + +public: + NavigationActionClient(std::shared_ptr & amr_manager, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + ~NavigationActionClient(); + void start_p2p_nav(void * buffer); + void start_follow_path(void * path); + void start_waypoint_follow_path(uint32_t goal, std::vector & ids); + void cancel_p2p_navigation(); + void cancel_follow_path(); + void pause_p2p_navigation(); + void pause_follow_path(); + void resume_p2p_navigation(); + void resume_follow_path(); + void navigate_to_charging(); +}; +} // namespace amr +} // namespace qrb_ros +#endif // NAVIGATION_ACTION_CLIENT_HPP_ \ No newline at end of file diff --git a/qrb_ros_amr/include/amr_controller.hpp b/qrb_ros_amr/include/amr_controller.hpp new file mode 100644 index 0000000..101b606 --- /dev/null +++ b/qrb_ros_amr/include/amr_controller.hpp @@ -0,0 +1,60 @@ +/* + * Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved. + * SPDX-License-Identifier: BSD-3-Clause-Clear + */ + +#ifndef AMR_CONTROLLER_HPP_ +#define AMR_CONTROLLER_HPP_ + +#include "api_service_server.hpp" +#include "mapping_service_server.hpp" +#include "cmd_action_server.hpp" +#include "sub_cmd_service_server.hpp" +#include "amr_status_transporter.hpp" +#include "auto_mapper_action_client.hpp" +#include "map_subscriber.hpp" +#include "exception_subscriber.hpp" +#include "developer_mode_subscriber.hpp" +#include "navigation_action_client.hpp" +#include "amr_mode_service_client.hpp" +#include "cartographer_service_client.hpp" +#include "amr_manager.hpp" + +using namespace qrb::amr_manager; + +namespace qrb_ros +{ +namespace amr +{ +class APIServiceServer; + +class AMRController +{ +private: + std::shared_ptr amr_manager_; + std::shared_ptr api_server_; + std::shared_ptr mapping_server_; + std::shared_ptr dev_sub_; + std::shared_ptr cmd_server_; + std::shared_ptr sub_cmd_server_; + std::shared_ptr amr_status_transporter_; + std::shared_ptr auto_mapper_client_; + std::shared_ptr nav_client_; + std::shared_ptr exception_sub_; + std::shared_ptr low_power_; + std::shared_ptr cartographer_service_client_; + std::shared_ptr amr_mode_client_; + + void init_nodes(); + + rclcpp::Logger logger_{ rclcpp::get_logger("amr_controller") }; + +public: + AMRController(); + ~AMRController(); + + std::shared_ptr executor_; +}; +} // namespace amr +} // namespace qrb_ros +#endif // AMR_CONTROLLER_HPP_ \ No newline at end of file diff --git a/qrb_ros_amr/include/amr_manager/amr_manager.hpp b/qrb_ros_amr/include/amr_manager/amr_manager.hpp new file mode 100644 index 0000000..03303c6 --- /dev/null +++ b/qrb_ros_amr/include/amr_manager/amr_manager.hpp @@ -0,0 +1,105 @@ +/* + * Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved. + * SPDX-License-Identifier: BSD-3-Clause-Clear + */ + +#ifndef AMR_MANAGER_HPP_ +#define AMR_MANAGER_HPP_ + +#include "amr_state_machine.hpp" +#include "low_power_manager.hpp" +#include "rclcpp/rclcpp.hpp" +#include "common.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include + +namespace qrb +{ +namespace amr_manager +{ + +typedef std::function start_p2p_func_t; +typedef std::function start_follow_path_func_t; +typedef std::function & ids)> + start_waypoint_follow_path_func_t; +typedef std::function sub_cmd_func_t; +typedef std::function start_charging_func_t; +typedef std::function change_mode_func_t; +typedef std::function notify_exception_func_t; +typedef std::function send_amr_state_changed_func_t; +typedef std::function navigate_to_charging_func_t; +typedef std::function slam_command_func_t; +typedef std::function publish_twist_func_t; + +/** + * @class navigation_controller::AMRManager + * @desc The AMRManager create nodes to control the Navigation. + */ +class AMRManager +{ +public: + /** + * @desc A constructor for AMRManager + */ + AMRManager(); + + /** + * @desc A destructor for AMRManager + */ + ~AMRManager(); + + // ExceptionSubscriber + void process_event(int event); + void process_event(int event, int error_code); + + // CmdActionServer + bool check_potential_state(int cmd); + void process_cmd(int cmd, void * buffer, size_t len); + void process_cmd(int cmd, uint32_t goal_id, std::vector & ids); + + // APIServiceServer + void init_amr(); + void release_amr(); + void me_completed(); + + // SubCmdServer + void process_sub_cmd(int msg); + + // AMRStatusTransporter + void notify_amr_state_changed(int state); + void notify_battery_changed(float battery_vol); + void notify_charging_state_changed(uint8_t state); + void register_start_charging_callback(start_charging_func_t cb); + void register_notify_exception_callback(notify_exception_func_t cb); + void register_send_amr_state_changed_callback(send_amr_state_changed_func_t cb); + void register_publish_twist_callback(publish_twist_func_t cb); + + // NavigationActionClient + void register_start_p2p_nav_callback(start_p2p_func_t cb); + void register_start_follow_path_callback(start_follow_path_func_t cb); + void register_start_waypoint_follow_path_callback(start_waypoint_follow_path_func_t cb); + void register_sub_cmd_callback(sub_cmd_func_t cb); + void register_navigate_to_charging_callback(navigate_to_charging_func_t cb); + + // AMRModeServiceClient + void register_change_mode_callback(change_mode_func_t cb); + + // MappingServiceServer + bool start_mapping(); + bool stop_mapping(); + bool load_map(); + + // CartographerServiceClient + void register_slam_command_callback(slam_command_func_t cb); + +private: + std::shared_ptr state_machine_; + std::shared_ptr low_power_; + + void init(); + + rclcpp::Logger logger_{ rclcpp::get_logger("amr_manager") }; +}; +} // namespace amr_manager +} // namespace qrb +#endif // AMR_MANAGER_HPP_ \ No newline at end of file diff --git a/qrb_ros_amr/include/amr_manager/common.hpp b/qrb_ros_amr/include/amr_manager/common.hpp new file mode 100644 index 0000000..d6502fc --- /dev/null +++ b/qrb_ros_amr/include/amr_manager/common.hpp @@ -0,0 +1,47 @@ +/* + * Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved. + * SPDX-License-Identifier: BSD-3-Clause-Clear + */ + +#ifndef COMMON_HPP_ +#define COMMON_HPP_ + +namespace qrb +{ +namespace amr_manager +{ +class API +{ +public: + const static int INIT_AMR = 1; + const static int RELEASE_AMR = 2; + const static int ENABLE_DEVELOPER_MODE = 11; + const static int ME_COMPLETED = 12; + + static std::string to_string(int cmd) + { + std::string message; + switch (cmd) { + case INIT_AMR: + message = "INIT_AMR"; + break; + case RELEASE_AMR: + message = "RELEASE_AMR"; + break; + case ENABLE_DEVELOPER_MODE: + message = "ENABLE_DEVELOPER_MODE"; + break; + case ME_COMPLETED: + message = "ME_COMPLETED"; + break; + default: + message = "INVAILD API"; + break; + } + return message; + } +}; + +} // namespace amr_manager +} // namespace qrb +#endif // COMMON_HPP_ \ No newline at end of file diff --git a/qrb_ros_amr/include/service/amr_mode_service_client.hpp b/qrb_ros_amr/include/service/amr_mode_service_client.hpp new file mode 100644 index 0000000..5b064ae --- /dev/null +++ b/qrb_ros_amr/include/service/amr_mode_service_client.hpp @@ -0,0 +1,55 @@ +/* + * Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved. + * SPDX-License-Identifier: BSD-3-Clause-Clear + */ + +#ifndef AMR_MODE_SERVICE_CLIENT_HPP_ +#define AMR_MODE_SERVICE_CLIENT_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "amr_manager.hpp" +#include "qrb_ros_robot_base_msgs/srv/set_control_mode.hpp" + +using SetBaseControlMode = qrb_ros_robot_base_msgs::srv::SetControlMode; +using namespace qrb::amr_manager; + +namespace qrb_ros +{ +namespace amr +{ +/** + * @enum amr_controller::AMRControlMode + * @desc Enum class representing mode of amr control. + */ +enum class AMRControlMode +{ + Remote_Controller = 1, // Control AMR moving by remote controller + Navigation = 2, // Control AMR moving by navigator +}; + +class AMRModeServiceClient : public rclcpp::Node +{ +private: + rclcpp::Client::SharedPtr client_; + std::shared_ptr amr_manager_; + std::mutex mtx_; + std::condition_variable cv_; + bool execute_result_; + qrb::amr_manager::change_mode_func_t change_mode_callback_; + rclcpp::Logger logger_{ rclcpp::get_logger("amr_mode_service_client") }; + + void send_request(const SetBaseControlMode::Request::SharedPtr request); + +public: + bool change_mode(uint8_t mode); + bool reset_odometer(); + explicit AMRModeServiceClient(std::shared_ptr & amr_manager, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + ~AMRModeServiceClient(); +}; +} // namespace amr +} // namespace qrb_ros +#endif // AMR_MODE_SERVICE_CLIENT_HPP_ diff --git a/qrb_ros_amr/include/service/api_service_server.hpp b/qrb_ros_amr/include/service/api_service_server.hpp new file mode 100644 index 0000000..07bb333 --- /dev/null +++ b/qrb_ros_amr/include/service/api_service_server.hpp @@ -0,0 +1,44 @@ +/* + * Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved. + * SPDX-License-Identifier: BSD-3-Clause-Clear + */ + +#ifndef API_SERVICE_SERVER_HPP_ +#define API_SERVICE_SERVER_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "qrb_ros_amr_msgs/srv/api.hpp" +#include "amr_controller.hpp" +#include "developer_mode_subscriber.hpp" +#include "amr_manager.hpp" +#include +#include + +using namespace qrb::amr_manager; + +namespace qrb_ros +{ +namespace amr +{ + +class APIServiceServer : public rclcpp::Node +{ +private: + rclcpp::Service::SharedPtr srv_ptr_; + std::shared_ptr amr_manager_; + std::shared_ptr dev_sub_; + + void init_service(); + void handle_api(const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + +public: + APIServiceServer(std::shared_ptr & amr_manager, + std::shared_ptr & dev_sub, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + ~APIServiceServer(); +}; +} // namespace amr +} // namespace qrb_ros +#endif // API_SERVICE_SERVER_HPP_ \ No newline at end of file diff --git a/qrb_ros_amr/include/service/cartographer_service_client.hpp b/qrb_ros_amr/include/service/cartographer_service_client.hpp new file mode 100644 index 0000000..7594174 --- /dev/null +++ b/qrb_ros_amr/include/service/cartographer_service_client.hpp @@ -0,0 +1,80 @@ +/* + * Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved. + * SPDX-License-Identifier: BSD-3-Clause-Clear + */ + +#ifndef CARTOGRAPHER_SERVICE_CLIENT_HPP_ +#define CARTOGRAPHER_SERVICE_CLIENT_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "amr_manager.hpp" +#include "qrb_ros_slam_msgs/srv/slam_command.hpp" +#include "qrb_ros_slam_msgs/msg/command_code.hpp" +#include "qrb_ros_navigation_msgs/srv/virtual_path.hpp" + +using namespace qrb::amr_manager; +using SlamCommand = qrb_ros_slam_msgs::srv::SlamCommand; +using CommandCode = qrb_ros_slam_msgs::msg::CommandCode; +using VirtualPath = qrb_ros_navigation_msgs::srv::VirtualPath; + +#define SERVICE_TIMEOUT_DURATION 5 // timeout is 5 seconds +#define RemoveWaypointAndVirtualPath 29 + +namespace qrb_ros +{ +namespace amr +{ + +enum class Slam_Command +{ + StartMapping = 1, + StopMapping = 2, + SaveMap = 3, + LoadMap = 4, + StartLocalization = 5, + Relocalization = 6, +}; + +/** + * @class amr_controller::CartographerServiceClientNode + * @desc The CartographerServiceClientNode create service client to send requests to + * notify cartographer. + */ +class CartographerServiceClient : public rclcpp::Node +{ +public: + CartographerServiceClient(std::shared_ptr & amr_manager); + ~CartographerServiceClient(); + +private: + bool send_api_request(SlamCommand::Request::SharedPtr request); + bool wait_for_service(); + bool send_and_wait_for_request(SlamCommand::Request::SharedPtr request); + bool send_api_request2(VirtualPath::Request::SharedPtr request); + bool wait_for_service2(); + bool send_and_wait_for_response2(VirtualPath::Request::SharedPtr request); + + void init_client(); + bool handle_slam_command(uint8_t cmd); + bool start_mapping(); + bool stop_mapping(); + bool save_map(); + bool load_map(); + bool start_localization(); + bool relocalization(); + bool remove_waypoint_and_virtual_path(); + + std::shared_ptr> client_; + std::shared_ptr> virtual_path_client_; + std::shared_ptr amr_manager_; + std::mutex mtx_; + std::condition_variable cv_; + bool response_result_; + uint8_t virtual_path_response_result_; + slam_command_func_t slam_command_callback_; + rclcpp::Logger logger_{ rclcpp::get_logger("cartographer_service_client") }; +}; + +} // namespace amr +} // namespace qrb_ros +#endif // CARTOGRAPHER_SERVICE_CLIENT_HPP_ diff --git a/qrb_ros_amr/include/service/mapping_service_server.hpp b/qrb_ros_amr/include/service/mapping_service_server.hpp new file mode 100644 index 0000000..fb40022 --- /dev/null +++ b/qrb_ros_amr/include/service/mapping_service_server.hpp @@ -0,0 +1,49 @@ +/* + * Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved. + * SPDX-License-Identifier: BSD-3-Clause-Clear + */ + +#ifndef MAPPING_SERVICE_SERVER_HPP_ +#define MAPPING_SERVICE_SERVER_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "qrb_ros_amr_msgs/srv/mapping.hpp" +#include "amr_manager.hpp" +#include +#include + +using namespace qrb::amr_manager; +using Mapping = qrb_ros_amr_msgs::srv::Mapping; + +namespace qrb_ros +{ +namespace amr +{ +enum class Mapping_Cmd +{ + LOAD_MAP = 1, + START_MAPPING = 2, + STOP_MAPPING = 3, +}; + +class MappingServiceServer : public rclcpp::Node +{ +private: + rclcpp::Service::SharedPtr srv_ptr_; + std::shared_ptr amr_manager_; + + void init_server(); + void handle_mapping(const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + + rclcpp::Logger logger_{ rclcpp::get_logger("mapping_service_server") }; + +public: + MappingServiceServer(std::shared_ptr & amr_manager, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + ~MappingServiceServer(); +}; +} // namespace amr +} // namespace qrb_ros +#endif // MAPPING_SERVICE_SERVER_HPP_ \ No newline at end of file diff --git a/qrb_ros_amr/include/service/sub_cmd_service_server.hpp b/qrb_ros_amr/include/service/sub_cmd_service_server.hpp new file mode 100644 index 0000000..4b19ec0 --- /dev/null +++ b/qrb_ros_amr/include/service/sub_cmd_service_server.hpp @@ -0,0 +1,43 @@ +/* + * Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved. + * SPDX-License-Identifier: BSD-3-Clause-Clear + */ + +#ifndef SUB_CMD_SERVICE_SERVER_HPP_ +#define SUB_CMD_SERVICE_SERVER_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "qrb_ros_amr_msgs/srv/sub_cmd.hpp" +#include "amr_manager.hpp" +#include +#include +#include +#include + +using namespace qrb::amr_manager; + +namespace qrb_ros +{ +namespace amr +{ + +class SubCmdServer : public rclcpp::Node +{ +private: + rclcpp::Service::SharedPtr srv_ptr_; + std::shared_ptr amr_manager_; + + void init_service(); + void handle_sub_cmd(const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + +public: + SubCmdServer(std::shared_ptr & amr_manager, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + ~SubCmdServer(); +}; +} // namespace amr +} // namespace qrb_ros + +#endif // SUB_CMD_SERVICE_SERVER_HPP_ \ No newline at end of file diff --git a/qrb_ros_amr/include/topic/amr_status_transporter.hpp b/qrb_ros_amr/include/topic/amr_status_transporter.hpp new file mode 100644 index 0000000..7ed1ad0 --- /dev/null +++ b/qrb_ros_amr/include/topic/amr_status_transporter.hpp @@ -0,0 +1,122 @@ +/* + * Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved. + * SPDX-License-Identifier: BSD-3-Clause-Clear + */ + +#ifndef AMR_STATUS_TRANSPORTER_HPP_ +#define AMR_STATUS_TRANSPORTER_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "qrb_ros_amr_msgs/msg/amr_status.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "qrb_ros_amr_msgs/msg/wheel_status.hpp" +#include "qrb_ros_amr_msgs/msg/battery_info.hpp" +#include "qrb_ros_robot_base_msgs/msg/charger_cmd.hpp" +#include "sensor_msgs/msg/battery_state.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "amr_manager.hpp" + +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" +#include "tf2/LinearMath/Quaternion.h" +#include "nav_2d_msgs/msg/twist2_d.hpp" +#include "nav_2d_msgs/msg/twist2_d_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" + +#include + +using OccupancyGrid = nav_msgs::msg::OccupancyGrid; +using PoseStamped = geometry_msgs::msg::PoseStamped; +using ChargerCmd = qrb_ros_robot_base_msgs::msg::ChargerCmd; + +using namespace qrb::amr_manager; + +namespace qrb_ros +{ +namespace amr +{ +class LowPowerManager; + +/** + * @enum amr_controller::StatusID + * @desc Enum class representing the changed status. + */ +enum class StatusID +{ + ALL = 0, // all items need be updated + AMR_Exception = 1, // AMR happens an exception or restores to normal + Battery_Level = 2, // Battery level of AMR is changed + Pose = 3, // Pose of AMR is changed + State_Machine = 4, // State of AMR is changed + Velocity = 5, // The linear or angular velocities of wheels is changed +}; + +class AMRStatusTransporter : public rclcpp::Node +{ +private: + rclcpp::Publisher::SharedPtr pub_; + rclcpp::Publisher::SharedPtr charger_pub_; + rclcpp::Subscription::SharedPtr wheel_sub_; + rclcpp::Subscription::SharedPtr battery_sub_; + rclcpp::Subscription::SharedPtr pose_sub_; + std::shared_ptr amr_manager_; + + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + std::string target_frame_ = "map"; + std::string source_frame_ = "base_link"; + PoseStamped source_pose_; + PoseStamped target_pose_; + PoseStamped last_pose_; + rclcpp::TimerBase::SharedPtr timer_{ nullptr }; + std::mutex mtx_; + bool active_ = false; + bool tf_working_; + uint64_t count_; + + qrb::amr_manager::start_charging_func_t start_charging_callback_; + qrb::amr_manager::notify_exception_func_t notify_exception_callback_; + qrb::amr_manager::send_amr_state_changed_func_t send_amr_state_changed_callback_; + + rclcpp::Publisher::SharedPtr twist_pub_; + publish_twist_func_t publish_twist_cb_; + + rclcpp::Subscription::SharedPtr vel_sub_; + nav_2d_msgs::msg::Twist2DStamped odom_velocity_; + long last_time_; + rclcpp::Logger logger_{ rclcpp::get_logger("amr_status_transporter") }; + + void wheel_status_callback(const qrb_ros_amr_msgs::msg::WheelStatus::SharedPtr msg); + void init_publisher(); + void init_subscription(); + void send_amr_status(const qrb_ros_amr_msgs::msg::AMRStatus msg); + + void init_tf_subscriber(); + void convert_tf_to_pose(); + void update_amr_pose(PoseStamped & pose); + bool is_pose_change(); + bool is_equal(double a, double b); + void pose_changed_callback(const PoseStamped::SharedPtr pose); + void battery_status_callback(const sensor_msgs::msg::BatteryState::SharedPtr msg); + void send_velocity(geometry_msgs::msg::Twist & velocity); + void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg); + +public: + void notify_exception_event(bool exception, uint8_t error_code); + void send_amr_state_changed(int state); + void update_map(OccupancyGrid map); + void stop_charging(); + void start_charging(); + void notify_battery_changed(float voltage); + AMRStatusTransporter(std::shared_ptr & amr_manager, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + ~AMRStatusTransporter(); + qrb_ros_amr_msgs::msg::AMRStatus message_; +}; +} // namespace amr +} // namespace qrb_ros +#endif // AMR_STATUS_TRANSPORTER_HPP_ \ No newline at end of file diff --git a/qrb_ros_amr/include/topic/developer_mode_subscriber.hpp b/qrb_ros_amr/include/topic/developer_mode_subscriber.hpp new file mode 100644 index 0000000..ed3ba8b --- /dev/null +++ b/qrb_ros_amr/include/topic/developer_mode_subscriber.hpp @@ -0,0 +1,156 @@ +/* + * Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved. + * SPDX-License-Identifier: BSD-3-Clause-Clear + */ + +#ifndef DEVELOPER_MODE_SUBSCRIBER_HPP_ +#define DEVELOPER_MODE_SUBSCRIBER_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/int16.hpp" +#include "amr_manager.hpp" +#include +#include "exception_subscriber.hpp" +#include "amr_status_transporter.hpp" + +using namespace qrb::amr_manager; + +namespace qrb_ros +{ +namespace amr +{ +/** + * @enum amr_controller::DEBUG_MSG + * @desc Enum class representing debug message. + */ +enum class DEBUG_MSG +{ + Init_AMR = 1, + Release_AMR = 2, + ME = 3, + AE = 4, + ME_Finish = 5, + AE_Finish = 6, + ME_Completed = 7, + P2PNav = 8, + FollowPath = 9, + Pause = 10, + Resume = 11, + Cancel = 12, + P2PNav_Finish = 13, + FollowPath_Finish = 14, + Low_Power = 15, + Normal_Power = 16, + Return_Charging = 17, + Return_Charging_Finish = 18, + AMR_Exception = 19, + AMR_Normal = 20, + Relocalization_Pass = 21, +}; + +class StringUtil +{ +public: + static std::string msg_to_string(int cmd) + { + std::string str; + switch ((DEBUG_MSG)cmd) { + case DEBUG_MSG::Init_AMR: + str = "Init_AMR"; + break; + case DEBUG_MSG::Release_AMR: + str = "Release_AMR"; + break; + case DEBUG_MSG::ME: + str = "ME"; + break; + case DEBUG_MSG::AE: + str = "AE"; + break; + case DEBUG_MSG::ME_Finish: + str = "ME_Finish"; + break; + case DEBUG_MSG::AE_Finish: + str = "AE_Finish"; + break; + case DEBUG_MSG::ME_Completed: + str = "ME_Completed"; + break; + case DEBUG_MSG::P2PNav: + str = "P2PNav"; + break; + case DEBUG_MSG::FollowPath: + str = "FollowPath"; + break; + case DEBUG_MSG::Pause: + str = "Pause"; + break; + case DEBUG_MSG::Resume: + str = "Resume"; + break; + case DEBUG_MSG::Cancel: + str = "Cancel"; + break; + case DEBUG_MSG::P2PNav_Finish: + str = "P2PNav_Finish"; + break; + case DEBUG_MSG::FollowPath_Finish: + str = "FollowPath_Finish"; + break; + case DEBUG_MSG::Low_Power: + str = "Low_Power"; + break; + case DEBUG_MSG::Normal_Power: + str = "Normal_Power"; + break; + case DEBUG_MSG::Return_Charging: + str = "Return_Charging"; + break; + case DEBUG_MSG::Return_Charging_Finish: + str = "Return_Charging_Finish"; + break; + case DEBUG_MSG::AMR_Exception: + str = "AMR_Exception"; + break; + case DEBUG_MSG::AMR_Normal: + str = "AMR_Normal"; + break; + case DEBUG_MSG::Relocalization_Pass: + str = "Relocalization_Pass"; + break; + default: + str = "Not support debug command"; + break; + } + return str; + } +}; + +class DeveloperModeSubscriber : public rclcpp::Node +{ +private: + rclcpp::Subscription::SharedPtr sub_; + rclcpp::Subscription::SharedPtr debug_error_sub_; + std::shared_ptr exception_sub_; + std::shared_ptr tansporter_; + std::shared_ptr amr_manager_; + bool enable_developer_mode_; + rclcpp::Logger logger_{ rclcpp::get_logger("developer_mode_subscriber") }; + + void sub_callback(const std_msgs::msg::Int16::ConstSharedPtr msg); + void debug_error_sub_callback(const std_msgs::msg::Int16::ConstSharedPtr msg); + void init_subscription(); + void send_debug_message(int event); + +public: + DeveloperModeSubscriber(std::shared_ptr & amr_manager, + std::shared_ptr & exception_sub, + std::shared_ptr & tansporter, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + ~DeveloperModeSubscriber(); + + void set_developer_mode(bool val); +}; +} // namespace amr +} // namespace qrb_ros +#endif // DEVELOPER_MODE_SUBSCRIBER_HPP_ \ No newline at end of file diff --git a/qrb_ros_amr/include/topic/exception_subscriber.hpp b/qrb_ros_amr/include/topic/exception_subscriber.hpp new file mode 100644 index 0000000..261c059 --- /dev/null +++ b/qrb_ros_amr/include/topic/exception_subscriber.hpp @@ -0,0 +1,39 @@ +/* + * Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved. + * SPDX-License-Identifier: BSD-3-Clause-Clear + */ + +#ifndef EXCEPTION_SUBSCRIBER_HPP_ +#define EXCEPTION_SUBSCRIBER_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "amr_manager.hpp" +#include "qrb_ros_robot_base_msgs/msg/error.hpp" +#include + +using namespace qrb::amr_manager; + +namespace qrb_ros +{ +namespace amr +{ +class ExceptionSubscriber : public rclcpp::Node +{ +private: + rclcpp::Subscription::SharedPtr sub_; + std::shared_ptr amr_manager_; + rclcpp::Logger logger_{ rclcpp::get_logger("exception_subscriber") }; + + void subscriber_callback(const qrb_ros_robot_base_msgs::msg::Error::SharedPtr msg); + void init_subscription(); + +public: + ExceptionSubscriber(std::shared_ptr & amr_manager, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + ~ExceptionSubscriber(); + + void handle_debug_msg(uint8_t error); +}; +} // namespace amr +} // namespace qrb_ros +#endif // EXCEPTION_SUBSCRIBER_HPP_ \ No newline at end of file diff --git a/qrb_ros_amr/include/topic/map_subscriber.hpp b/qrb_ros_amr/include/topic/map_subscriber.hpp new file mode 100644 index 0000000..0caf729 --- /dev/null +++ b/qrb_ros_amr/include/topic/map_subscriber.hpp @@ -0,0 +1,45 @@ +/* + * Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved. + * SPDX-License-Identifier: BSD-3-Clause-Clear + */ + +#ifndef MAP_SUBSCRIBER_HPP_ +#define MAP_SUBSCRIBER_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "amr_status_transporter.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "amr_manager.hpp" +#include + +using OccupancyGrid = nav_msgs::msg::OccupancyGrid; + +using namespace qrb::amr_manager; + +namespace qrb_ros +{ +namespace amr +{ + +class MapSubscriber : public rclcpp::Node +{ +private: + rclcpp::Subscription::SharedPtr sub_; + std::shared_ptr amr_status_transporter_; + std::shared_ptr amr_manager_; + + void init_subscription(); + bool read_map(); + void save_map(OccupancyGrid map); + int receive_map_count_ = 0; + +public: + MapSubscriber(std::shared_ptr & amr_manager, + std::shared_ptr & amr_status_transporter, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + ~MapSubscriber(); + void subscriber_callback(const OccupancyGrid::SharedPtr map); +}; +} // namespace amr +} // namespace qrb_ros +#endif // MAP_SUBSCRIBER_HPP_ \ No newline at end of file diff --git a/qrb_ros_amr/launch/qrb_ros_amr_bringup.launch.py b/qrb_ros_amr/launch/qrb_ros_amr_bringup.launch.py new file mode 100644 index 0000000..681a97b --- /dev/null +++ b/qrb_ros_amr/launch/qrb_ros_amr_bringup.launch.py @@ -0,0 +1,21 @@ +# Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved. +# SPDX-License-Identifier: BSD-3-Clause-Clear + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +from launch_ros.substitutions import FindPackageShare +from launch.substitutions import PathJoinSubstitution + + +def generate_launch_description(): + """Launch file to bring up qti amr controller node standalone.""" + + qrb_ros_amr_node = Node( + package='qrb_ros_amr', + executable='qrb_ros_amr', + ) + + return LaunchDescription([qrb_ros_amr_node]) diff --git a/qrb_ros_amr/package.xml b/qrb_ros_amr/package.xml new file mode 100644 index 0000000..b34fda8 --- /dev/null +++ b/qrb_ros_amr/package.xml @@ -0,0 +1,28 @@ + + + + qrb_ros_amr + 1.0.0 + ROS node of AMR service + + BSD-3-Clause-Clear + + xiaowz + xiaowz + + ament_cmake_auto + + ament_lint_auto + ament_lint_common + + qrb_ros_amr_msgs + qrb_ros_navigation_msgs + qrb_ros_robot_base_msgs + qrb_ros_slam_msgs + qrb_amr_manager + nav_2d_msgs + + + ament_cmake + + diff --git a/qrb_ros_amr/src/action/auto_mapper_action_client.cpp b/qrb_ros_amr/src/action/auto_mapper_action_client.cpp new file mode 100644 index 0000000..c8ef475 --- /dev/null +++ b/qrb_ros_amr/src/action/auto_mapper_action_client.cpp @@ -0,0 +1,94 @@ +/* + * Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved. + * SPDX-License-Identifier: BSD-3-Clause-Clear + */ + +#include "amr_manager.hpp" +#include "auto_mapper_action_client.hpp" +#include "cmd_action_server.hpp" + +namespace qrb_ros +{ +namespace amr +{ +AutoMapperActionClient::AutoMapperActionClient(std::shared_ptr & amr_manager, + const rclcpp::NodeOptions & options) + : Node("auto_mapper_client", options), amr_manager_(amr_manager) +{ + create_client(); +} + +AutoMapperActionClient::~AutoMapperActionClient() {} + +void AutoMapperActionClient::start_ae() +{ + send_goal(); +} + +void AutoMapperActionClient::cancel_goal() +{ + RCLCPP_INFO(this->get_logger(), "cancel all goal"); + this->client_ptr_->async_cancel_all_goals(); +} + +void AutoMapperActionClient::create_client() +{ + this->client_ptr_ = rclcpp_action::create_client(this->get_node_base_interface(), + this->get_node_graph_interface(), this->get_node_logging_interface(), + this->get_node_waitables_interface(), "ae"); +} + +void AutoMapperActionClient::send_goal() +{ + using namespace std::placeholders; + auto goal_msg = AE::Goal(); + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.goal_response_callback = + std::bind(&AutoMapperActionClient::goal_response_callback, this, _1); + send_goal_options.feedback_callback = + std::bind(&AutoMapperActionClient::feedback_callback, this, _1, _2); + send_goal_options.result_callback = std::bind(&AutoMapperActionClient::result_callback, this, _1); + this->client_ptr_->async_send_goal(goal_msg, send_goal_options); +} + +void AutoMapperActionClient::goal_response_callback(GoalHandleAE::SharedPtr goal_handle) +{ + if (!goal_handle) { + RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); + CmdActionServer::command_status_completed(false); + } else { + RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); + } +} + +void AutoMapperActionClient::feedback_callback(GoalHandleAE::SharedPtr, + const std::shared_ptr feedback) +{ + RCLCPP_INFO(this->get_logger(), "Publish feedback!"); + CmdActionServer::motion_pose_changed(feedback->current_pose); +} + +void AutoMapperActionClient::result_callback(const GoalHandleAE::WrappedResult & result) +{ + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + break; + case rclcpp_action::ResultCode::ABORTED: + RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); + CmdActionServer::command_status_completed(false); + return; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); + CmdActionServer::command_status_completed(false); + return; + default: + RCLCPP_ERROR(this->get_logger(), "Unknown result code"); + return; + } + RCLCPP_INFO(this->get_logger(), "Goal finished!"); + CmdActionServer::command_status_completed(true); + amr_manager_->process_event(Message::AE_FINISH); +} + +} // namespace amr +} // namespace qrb_ros \ No newline at end of file diff --git a/qrb_ros_amr/src/action/cmd_action_server.cpp b/qrb_ros_amr/src/action/cmd_action_server.cpp new file mode 100644 index 0000000..b37e97e --- /dev/null +++ b/qrb_ros_amr/src/action/cmd_action_server.cpp @@ -0,0 +1,152 @@ +/* + * Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved. + * SPDX-License-Identifier: BSD-3-Clause-Clear + */ + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "qrb_ros_amr_msgs/action/cmd.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "cmd_action_server.hpp" +#include +#include + +namespace qrb_ros +{ +namespace amr +{ +std::shared_ptr CmdActionServer::record_goal_ = NULL; + +CmdActionServer::CmdActionServer(std::shared_ptr & amr_manager, + const rclcpp::NodeOptions & options) + : Node("cmd_action_server", options), amr_manager_(amr_manager) +{ + create_server(); +} + +CmdActionServer::~CmdActionServer() +{ + record_goal_ = nullptr; +} + +void CmdActionServer::motion_pose_changed(Pose pose) +{ + using namespace std; + auto feedback = std::make_shared(); + feedback->current_pose = pose; + if (record_goal_ == nullptr) { + // navigator to charging_station will run into here + return; + } + record_goal_->publish_feedback(feedback); + RCLCPP_DEBUG(rclcpp::get_logger("cmd_action_server"), "Publish feedback"); +} + +void CmdActionServer::command_status_completed(bool cmdStatus) +{ + using namespace std; + auto finish = std::make_shared(); + finish->result = cmdStatus; + if (record_goal_ == nullptr) { + RCLCPP_INFO(rclcpp::get_logger("cmd_action_server"), "server global handle is nullptr"); + return; + } + + if (!cmdStatus) { + if (record_goal_->is_canceling()) { + record_goal_->canceled(finish); + RCLCPP_INFO(rclcpp::get_logger("cmd_action_server"), "Goal canceled"); + } else { + record_goal_->abort(finish); + RCLCPP_INFO(rclcpp::get_logger("cmd_action_server"), "Goal abort"); + } + return; + } + + record_goal_->succeed(finish); + record_goal_ = nullptr; + RCLCPP_INFO(rclcpp::get_logger("cmd_action_server"), "Goal finished!"); +} + +bool CmdActionServer::is_waiting_command_feedback() +{ + return is_waiting_command_; +} + +void CmdActionServer::create_server() +{ + using namespace std::placeholders; + this->server_ptr_ = rclcpp_action::create_server(this->get_node_base_interface(), + this->get_node_clock_interface(), this->get_node_logging_interface(), + this->get_node_waitables_interface(), "cmd", + std::bind(&CmdActionServer::handle_goal, this, _1, _2), + std::bind(&CmdActionServer::handle_cancel, this, _1), + std::bind(&CmdActionServer::handle_accepted, this, _1)); +} + +void CmdActionServer::set_waiting_command_flag(bool flag) +{ + is_waiting_command_ = flag; +} + +rclcpp_action::GoalResponse CmdActionServer::handle_goal(const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal) +{ + (void)uuid; + RCLCPP_INFO(logger_, "Received request to handle goal"); + int cmd = goal->command; + bool succeed = amr_manager_->check_potential_state(cmd); + if (succeed) { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } + return rclcpp_action::GoalResponse::REJECT; +} + +rclcpp_action::CancelResponse CmdActionServer::handle_cancel( + const std::shared_ptr goal_handle) +{ + const auto goal = goal_handle->get_goal(); + RCLCPP_INFO(logger_, "Received request to cancel goal: %s", + (Command::cmd_to_string(goal->command)).c_str()); + return rclcpp_action::CancelResponse::ACCEPT; +} + +void CmdActionServer::handle_accepted(const std::shared_ptr goal_handle) +{ + using namespace std::placeholders; + std::thread{ std::bind(&CmdActionServer::execute, this, _1), goal_handle }.detach(); +} + +void CmdActionServer::execute(const std::shared_ptr goal_handle) +{ + const auto goal = goal_handle->get_goal(); + RCLCPP_INFO(logger_, "execute goal : %s", (Command::cmd_to_string(goal->command)).c_str()); + void * ptr; + if (goal->command == Command::AE) { + amr_manager_->process_cmd(Message::AE, nullptr, 0); + } else if (goal->command == Command::ME) { + amr_manager_->process_cmd(Message::ME, nullptr, 0); + } else if (goal->command == Command::P2PNAV) { + ptr = (void *)&(goal->goal); + RCLCPP_INFO(logger_, "execute goal size: %d", sizeof(goal->goal)); + amr_manager_->process_cmd(Message::P2PNAV, ptr, sizeof(goal->goal)); + } else if (goal->command == Command::FOLLOW_PATH) { + ptr = (void *)&(goal->path); + RCLCPP_INFO(logger_, "execute goal size: %d", sizeof(goal->goal)); + amr_manager_->process_cmd(Message::FOLLOW_PATH, ptr, sizeof(goal->path)); + } else if (goal->command == Command::CHARGING) { + amr_manager_->process_cmd(Message::RETURN_CHARGING, nullptr, 0); + } else if (goal->command == Command::WaypointFollowPath) { + uint32_t goal_id = goal->goal_id; + std::vector ids; + if (goal->passing_waypoint_ids.size() != 0) { + ids.assign(goal->passing_waypoint_ids.begin(), goal->passing_waypoint_ids.end()); + } + RCLCPP_INFO(logger_, "execute goal id =%d, passing_waypoint size: %d", goal_id, + sizeof(goal->passing_waypoint_ids)); + amr_manager_->process_cmd(Message::WAYPOINT_FOLLOW_PATH, goal_id, ids); + } + record_goal_ = goal_handle; +} +} // namespace amr +} // namespace qrb_ros \ No newline at end of file diff --git a/qrb_ros_amr/src/action/navigation_action_client.cpp b/qrb_ros_amr/src/action/navigation_action_client.cpp new file mode 100644 index 0000000..e219521 --- /dev/null +++ b/qrb_ros_amr/src/action/navigation_action_client.cpp @@ -0,0 +1,521 @@ +/* + * Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved. + * SPDX-License-Identifier: BSD-3-Clause-Clear + */ + +#include "amr_manager.hpp" +#include "navigation_action_client.hpp" +#include "cmd_action_server.hpp" +#include "sub_cmd_service_server.hpp" + +#include +#include + +constexpr char const * service_name = "follow_path_sub_cmd"; + +namespace qrb_ros +{ +namespace amr +{ +NavigationActionClient::NavigationActionClient(std::shared_ptr & amr_manager, + const rclcpp::NodeOptions & options) + : Node("navigator_client", options), amr_manager_(amr_manager) +{ + create_nav_client(); +} + +NavigationActionClient::~NavigationActionClient() {} + +void NavigationActionClient::create_nav_client() +{ + this->follow_path_client_ptr_ = rclcpp_action::create_client( + this->get_node_base_interface(), this->get_node_graph_interface(), + this->get_node_logging_interface(), this->get_node_waitables_interface(), "followpath"); + + this->waypoint_follow_path_client_ptr_ = rclcpp_action::create_client( + this->get_node_base_interface(), this->get_node_graph_interface(), + this->get_node_logging_interface(), this->get_node_waitables_interface(), "wfollowpath"); + + this->p2p_client_ptr_ = rclcpp_action::create_client(this->get_node_base_interface(), + this->get_node_graph_interface(), this->get_node_logging_interface(), + this->get_node_waitables_interface(), "p2p"); + + follow_path_service_client_ = this->create_client(service_name); + + p2p_nav_callback_ = [&](void * buffer) { start_p2p_nav(buffer); }; + amr_manager_->register_start_p2p_nav_callback(p2p_nav_callback_); + + follow_path_callback_ = [&](void * path) { start_follow_path(path); }; + amr_manager_->register_start_follow_path_callback(follow_path_callback_); + + waypoint_follow_path_callback_ = [&](uint32_t goal, std::vector & ids) { + start_waypoint_follow_path(goal, ids); + }; + amr_manager_->register_start_waypoint_follow_path_callback(waypoint_follow_path_callback_); + + sub_cmd_callback_ = [&](bool p2p, uint8_t sub_cmd) { handle_sub_cmd(p2p, sub_cmd); }; + amr_manager_->register_sub_cmd_callback(sub_cmd_callback_); + + navigate_to_charging_callback_ = [&](void) { navigate_to_charging(); }; + amr_manager_->register_navigate_to_charging_callback(navigate_to_charging_callback_); +} + +void NavigationActionClient::start_p2p_nav(void * buffer) +{ + p2p_send_goal(buffer); +} + +void NavigationActionClient::start_follow_path(void * path) +{ + follow_send_goal(path); +} + +void NavigationActionClient::start_waypoint_follow_path(uint32_t goal, std::vector & ids) +{ + waypoint_follow_send_goal(goal, ids); +} + +void NavigationActionClient::waypoint_follow_send_goal(uint32_t goal, std::vector & ids) +{ + using namespace std::placeholders; + auto goal_msg = WaypointFollowPath::Goal(); + + goal_msg.goal = goal; + uint32_t len = ids.size(); + if (len != 0) { + goal_msg.passing_waypoint_ids.assign(ids.begin(), ids.end()); + } + + RCLCPP_INFO(logger_, "waypoint_follow_send_goal(%d)", len); + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.goal_response_callback = + std::bind(&NavigationActionClient::waypoint_follow_goal_response_callback, this, _1); + send_goal_options.feedback_callback = + std::bind(&NavigationActionClient::waypoint_follow_feedback_callback, this, _1, _2); + send_goal_options.result_callback = + std::bind(&NavigationActionClient::waypoint_follow_result_callback, this, _1); + this->waypoint_follow_path_client_ptr_->async_send_goal(goal_msg, send_goal_options); +} + +void NavigationActionClient::follow_send_goal(void * path = NULL) +{ + using namespace std::placeholders; + auto goal_msg = FollowPath::Goal(); + if (path != NULL) { + temp_path_ = *reinterpret_cast(path); + } + goal_msg.path = temp_path_; + uint32_t len = goal_msg.path.poses.size(); + RCLCPP_INFO(logger_, "follow_path_send_goal(%d)", len); + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.goal_response_callback = + std::bind(&NavigationActionClient::follow_goal_response_callback, this, _1); + send_goal_options.feedback_callback = + std::bind(&NavigationActionClient::follow_feedback_callback, this, _1, _2); + send_goal_options.result_callback = + std::bind(&NavigationActionClient::follow_result_callback, this, _1); + this->follow_path_client_ptr_->async_send_goal(goal_msg, send_goal_options); +} + +void NavigationActionClient::p2p_send_goal(void * buffer) +{ + using namespace std::placeholders; + auto goal_msg = P2P::Goal(); + if (buffer != NULL) { + temp_pose_ = *reinterpret_cast(buffer); + } + + double x = temp_pose_.pose.position.x; + double y = temp_pose_.pose.position.y; + tf2::Quaternion quat(temp_pose_.pose.orientation.x, temp_pose_.pose.orientation.y, + temp_pose_.pose.orientation.z, temp_pose_.pose.orientation.w); + double roll, pitch, yaw; + tf2::Matrix3x3 m(quat); + m.getRPY(roll, pitch, yaw); + double z = yaw; + RCLCPP_INFO(logger_, "p2p_send_goal(%f,%f,%f)", x, y, z); + + goal_msg.pose = temp_pose_; + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.goal_response_callback = + std::bind(&NavigationActionClient::p2p_goal_response_callback, this, _1); + send_goal_options.feedback_callback = + std::bind(&NavigationActionClient::p2p_feedback_callback, this, _1, _2); + send_goal_options.result_callback = + std::bind(&NavigationActionClient::p2p_result_callback, this, _1); + this->p2p_client_ptr_->async_send_goal(goal_msg, send_goal_options); +} + +void NavigationActionClient::handle_sub_cmd(bool p2p, uint8_t sub_cmd) +{ + RCLCPP_INFO(logger_, "handle_sub_cmd, p2p=%d, sub_cmd=%d", p2p, sub_cmd); + if (p2p) { + if (sub_cmd == SubCommand::CANCEL) { + cancel_p2p_navigation(); + } else if (sub_cmd == SubCommand::PAUSE) { + pause_p2p_navigation(); + } else if (sub_cmd == SubCommand::RESUME) { + resume_p2p_navigation(); + } + } else { + if (sub_cmd == SubCommand::CANCEL) { + cancel_follow_path(); + } else if (sub_cmd == SubCommand::PAUSE) { + pause_follow_path(); + } else if (sub_cmd == SubCommand::RESUME) { + resume_follow_path(); + } + } +} + +void NavigationActionClient::cancel_p2p_navigation() +{ + is_pausing_ = true; + this->p2p_client_ptr_->async_cancel_all_goals(); +} + +void NavigationActionClient::cancel_follow_path() +{ + is_pausing_ = true; + this->waypoint_follow_path_client_ptr_->async_cancel_all_goals(); + this->follow_path_client_ptr_->async_cancel_all_goals(); +} + +void NavigationActionClient::pause_p2p_navigation() +{ + is_pausing_ = true; + this->p2p_client_ptr_->async_cancel_all_goals(); +} + +void NavigationActionClient::pause_follow_path() +{ + RCLCPP_INFO(logger_, "pause_follow_path"); + is_pausing_ = true; + + // get request + auto request = std::make_shared(); + request->subcommand = SubCommand::PAUSE; + + if (!wait_for_service()) { + return; + } + + // send request + send_and_wait_for_request(request); +} + +void NavigationActionClient::resume_p2p_navigation() +{ + is_pausing_ = false; + p2p_send_goal(); +} + +void NavigationActionClient::resume_follow_path() +{ + RCLCPP_INFO(logger_, "resume_follow_path"); + is_pausing_ = false; + + // get request + auto request = std::make_shared(); + request->subcommand = SubCommand::RESUME; + + if (!wait_for_service()) { + return; + } + + // send request + send_and_wait_for_request(request); +} + +void NavigationActionClient::p2p_goal_response_callback(GoalHandleP2P::SharedPtr goal_handle) +{ + if (!goal_handle) { + RCLCPP_ERROR(logger_, "Goal was rejected by server"); + CmdActionServer::command_status_completed(false); + amr_manager_->process_event(Message::CANCEL); + } else { + RCLCPP_INFO(logger_, "Goal accepted by server, waiting for result"); + p2p_uuid_ = goal_handle->get_goal_id(); + } +} + +void NavigationActionClient::p2p_feedback_callback(GoalHandleP2P::SharedPtr, + const std::shared_ptr feedback) +{ + RCLCPP_DEBUG(logger_, "[P2P] Publish feedback"); + CmdActionServer::motion_pose_changed(feedback->current_pose); +} + +void NavigationActionClient::p2p_result_callback(const GoalHandleP2P::WrappedResult & result) +{ + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: { + RCLCPP_INFO(logger_, "Receive P2P result = SUCCEEDED"); + RCLCPP_INFO(logger_, "[P2P] Goal finished!"); + CmdActionServer::command_status_completed(true); + amr_manager_->process_event(Message::P2PNAV_FINISH); + return; + } + case rclcpp_action::ResultCode::ABORTED: { + RCLCPP_INFO(logger_, "Receive P2P result = ABORTED"); + if (is_pausing_) { + return; + } + RCLCPP_ERROR(logger_, "Goal(p2p) was aborted"); + CmdActionServer::command_status_completed(false); + amr_manager_->process_event(Message::CANCEL); + return; + } + case rclcpp_action::ResultCode::CANCELED: { + RCLCPP_INFO(logger_, "Receive P2P result = CANCELED"); + if (is_pausing_) { + return; + } + RCLCPP_ERROR(logger_, "Goal(p2p) was canceled"); + CmdActionServer::command_status_completed(false); + amr_manager_->process_event(Message::CANCEL); + return; + } + default: + RCLCPP_ERROR(logger_, "Unknown result code"); + return; + } +} + +void NavigationActionClient::follow_goal_response_callback(GoalHandleFollow::SharedPtr goal_handle) +{ + if (!goal_handle) { + RCLCPP_ERROR(logger_, "Goal was rejected by server"); + CmdActionServer::command_status_completed(false); + amr_manager_->process_event(Message::CANCEL); + } else { + RCLCPP_INFO(logger_, "Goal accepted by server, waiting for result"); + follow_uuid_ = goal_handle->get_goal_id(); + } +} + +void NavigationActionClient::follow_feedback_callback(GoalHandleFollow::SharedPtr, + const std::shared_ptr feedback) +{ + RCLCPP_DEBUG(logger_, "[FOLLOW_PATH] Publish feedback"); +} + +void NavigationActionClient::follow_result_callback(const GoalHandleFollow::WrappedResult & result) +{ + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: { + RCLCPP_INFO(logger_, "Receive follow path result = SUCCEEDED"); + RCLCPP_INFO(logger_, "[FOLLOW_PATH] Goal finished!"); + CmdActionServer::command_status_completed(true); + amr_manager_->process_event(Message::FOLLOW_PATH_FINISH); + return; + } + case rclcpp_action::ResultCode::ABORTED: { + RCLCPP_INFO(logger_, "Receive follow path result = ABORTED"); + if (is_pausing_) { + return; + } + RCLCPP_ERROR(logger_, "Goal was aborted"); + CmdActionServer::command_status_completed(false); + amr_manager_->process_event(Message::CANCEL); + return; + } + case rclcpp_action::ResultCode::CANCELED: { + RCLCPP_INFO(logger_, "Receive follow path result = CANCELED"); + if (is_pausing_) { + return; + } + RCLCPP_ERROR(logger_, "Goal(follow path) was canceled"); + CmdActionServer::command_status_completed(false); + amr_manager_->process_event(Message::CANCEL); + return; + } + default: + RCLCPP_ERROR(logger_, "Unknown result code"); + return; + } +} + +void NavigationActionClient::waypoint_follow_goal_response_callback( + GoalHandleWaypointFollowPath::SharedPtr goal_handle) +{ + if (!goal_handle) { + RCLCPP_ERROR(logger_, "Goal was rejected by server"); + CmdActionServer::command_status_completed(false); + amr_manager_->process_event(Message::CANCEL); + } else { + RCLCPP_INFO(logger_, "Goal accepted by server, waiting for result"); + follow_uuid_ = goal_handle->get_goal_id(); + } +} + +void NavigationActionClient::waypoint_follow_feedback_callback( + GoalHandleWaypointFollowPath::SharedPtr, + const std::shared_ptr feedback) +{ + uint32_t passing_waypoint_id = feedback->passing_waypoint_id; + RCLCPP_INFO(logger_, "[WAYPOINT_FOLLOW_PATH] Publish feedback, passing_waypoint_id = %d", + passing_waypoint_id); +} + +void NavigationActionClient::waypoint_follow_result_callback( + const GoalHandleWaypointFollowPath::WrappedResult & result) +{ + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: { + RCLCPP_INFO(logger_, "[WAYPOINT_FOLLOW_PATH] Goal finished!"); + CmdActionServer::command_status_completed(true); + amr_manager_->process_event(Message::FOLLOW_PATH_FINISH); + return; + } + case rclcpp_action::ResultCode::ABORTED: { + if (is_pausing_) { + return; + } + RCLCPP_ERROR(logger_, "Goal was aborted"); + CmdActionServer::command_status_completed(false); + amr_manager_->process_event(Message::CANCEL); + return; + } + case rclcpp_action::ResultCode::CANCELED: { + RCLCPP_INFO(logger_, "Receive wfollow path result = CANCELED"); + if (is_pausing_) { + return; + } + RCLCPP_ERROR(logger_, "Goal(wfollow path) was canceled"); + CmdActionServer::command_status_completed(false); + amr_manager_->process_event(Message::CANCEL); + return; + } + default: + RCLCPP_ERROR(logger_, "Unknown result code"); + return; + } +} + +void NavigationActionClient::navigate_to_charging() +{ + using namespace std::placeholders; + + auto goal = P2P::Goal(); + double charging_station_x = 0.3; + double charging_station_y = 0; + double charging_station_angle = 0; + + generate_pose(charging_station_x, charging_station_y, charging_station_angle, charging_station_); + + RCLCPP_INFO(logger_, "Return charger station(%f,%f,%f)", charging_station_x, charging_station_y, + charging_station_angle); + + goal.pose = charging_station_; + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.goal_response_callback = + std::bind(&NavigationActionClient::charging_station_goal_response_callback, this, _1); + send_goal_options.feedback_callback = + std::bind(&NavigationActionClient::charging_station_feedback_callback, this, _1, _2); + send_goal_options.result_callback = + std::bind(&NavigationActionClient::charging_station_result_callback, this, _1); + this->p2p_client_ptr_->async_send_goal(goal, send_goal_options); + + std::unique_lock lck(mtx_); + cv_.wait(lck); +} + +void NavigationActionClient::charging_station_goal_response_callback( + GoalHandleP2P::SharedPtr goal_handle) +{ + if (!goal_handle) { + RCLCPP_ERROR(logger_, "Goal of going charging station was rejected by server"); + cv_.notify_one(); + } else { + RCLCPP_INFO(logger_, "Goal of going charging station accepted by server, waiting for result"); + } +} + +void NavigationActionClient::charging_station_feedback_callback(GoalHandleP2P::SharedPtr, + const std::shared_ptr feedback) +{ + RCLCPP_DEBUG(logger_, "[Charging station] Publish feedback"); + CmdActionServer::motion_pose_changed(feedback->current_pose); +} + +void NavigationActionClient::charging_station_result_callback( + const GoalHandleP2P::WrappedResult & result) +{ + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + break; + case rclcpp_action::ResultCode::ABORTED: + RCLCPP_ERROR(logger_, "Goal was aborted"); + break; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_ERROR(logger_, "Goal(charging) was canceled"); + break; + default: + RCLCPP_ERROR(logger_, "Unknown result code"); + break; + } + RCLCPP_INFO(logger_, "[Charging station] Goal finished!"); + cv_.notify_one(); + return; +} + +void NavigationActionClient::generate_pose(double & x, double & y, double & z, PoseStamped & pose) +{ + pose.pose.position.x = x; + pose.pose.position.y = y; + pose.pose.position.z = 0; + + tf2::Quaternion quat; + quat.setRPY(0, 0, z); + pose.pose.orientation.x = quat.x(); + pose.pose.orientation.y = quat.y(); + pose.pose.orientation.z = quat.z(); + pose.pose.orientation.w = quat.w(); +} + +bool NavigationActionClient::wait_for_service() +{ + uint32_t wait = 0; + // wait for connectting service + while (!follow_path_service_client_->wait_for_service(std::chrono::seconds(1))) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(logger_, "Interrupted while waiting for the service. Exiting."); + return false; + } + RCLCPP_INFO(logger_, "service not available, waiting again..."); + + wait++; + if (wait >= SERVICE_TIMEOUT_DURATION) { + RCLCPP_ERROR(logger_, "Wait for service timeout, Exiting."); + return false; + } + } + return true; +} + +bool NavigationActionClient::send_and_wait_for_request(SubCmd::Request::SharedPtr request) +{ + RCLCPP_INFO(logger_, "send_and_wait_for_request"); + using ServiceResponseFuture = rclcpp::Client::SharedFuture; + auto response_received_callback = [this](ServiceResponseFuture future) { + std::unique_lock lck(mtx_); + auto result = future.get(); + response_result_ = result->result; + cv_.notify_one(); + }; + + auto future_result = + follow_path_service_client_->async_send_request(request, response_received_callback); + std::unique_lock lck(mtx_); + cv_.wait(lck); + + if (response_result_) { + RCLCPP_INFO(logger_, "api is excuted successful"); + } else { + RCLCPP_ERROR(logger_, "api is excuted failed"); + } + return response_result_; +} +} // namespace amr +} // namespace qrb_ros diff --git a/qrb_ros_amr/src/amr_controller.cpp b/qrb_ros_amr/src/amr_controller.cpp new file mode 100644 index 0000000..7ed958e --- /dev/null +++ b/qrb_ros_amr/src/amr_controller.cpp @@ -0,0 +1,67 @@ +/* + * Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved. + * SPDX-License-Identifier: BSD-3-Clause-Clear + */ + +#include "amr_controller.hpp" +#include + +namespace qrb_ros +{ +namespace amr +{ + +AMRController::AMRController() +{ + init_nodes(); + RCLCPP_INFO(logger_, "Init amr node"); +} + +AMRController::~AMRController() {} + +void AMRController::init_nodes() +{ + executor_ = std::shared_ptr( + new rclcpp::executors::MultiThreadedExecutor()); + amr_manager_ = std::shared_ptr(new AMRManager()); + + exception_sub_ = std::shared_ptr(new ExceptionSubscriber(amr_manager_)); + + mapping_server_ = std::shared_ptr(new MappingServiceServer(amr_manager_)); + + cmd_server_ = std::shared_ptr(new CmdActionServer(amr_manager_)); + + sub_cmd_server_ = std::shared_ptr(new SubCmdServer(amr_manager_)); + + auto_mapper_client_ = + std::shared_ptr(new AutoMapperActionClient(amr_manager_)); + + nav_client_ = std::shared_ptr(new NavigationActionClient(amr_manager_)); + + cartographer_service_client_ = + std::shared_ptr(new CartographerServiceClient(amr_manager_)); + + amr_mode_client_ = std::shared_ptr(new AMRModeServiceClient(amr_manager_)); + + amr_status_transporter_ = + std::shared_ptr(new AMRStatusTransporter(amr_manager_)); + + dev_sub_ = std::shared_ptr( + new DeveloperModeSubscriber(amr_manager_, exception_sub_, amr_status_transporter_)); + + api_server_ = std::shared_ptr(new APIServiceServer(amr_manager_, dev_sub_)); + + executor_->add_node(api_server_); + executor_->add_node(mapping_server_); + executor_->add_node(dev_sub_); + executor_->add_node(exception_sub_); + executor_->add_node(cmd_server_); + executor_->add_node(sub_cmd_server_); + executor_->add_node(auto_mapper_client_); + executor_->add_node(nav_client_); + executor_->add_node(cartographer_service_client_); + executor_->add_node(amr_mode_client_); + executor_->add_node(amr_status_transporter_); +} +} // namespace amr +} // namespace qrb_ros diff --git a/qrb_ros_amr/src/main.cpp b/qrb_ros_amr/src/main.cpp new file mode 100644 index 0000000..5dad709 --- /dev/null +++ b/qrb_ros_amr/src/main.cpp @@ -0,0 +1,20 @@ +/* + * Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved. + * SPDX-License-Identifier: BSD-3-Clause-Clear + */ + +#include +#include "rclcpp/rclcpp.hpp" +#include "amr_controller.hpp" +#include + +using namespace std; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + qrb_ros::amr::AMRController control; + (control.executor_)->spin(); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/qrb_ros_amr/src/service/amr_mode_service_client.cpp b/qrb_ros_amr/src/service/amr_mode_service_client.cpp new file mode 100644 index 0000000..55f7c80 --- /dev/null +++ b/qrb_ros_amr/src/service/amr_mode_service_client.cpp @@ -0,0 +1,69 @@ +/* + * Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved. + * SPDX-License-Identifier: BSD-3-Clause-Clear + */ + +#include "amr_mode_service_client.hpp" + +#include +#include +#include +#include + +using namespace std::chrono_literals; + +namespace qrb_ros +{ +namespace amr +{ +AMRModeServiceClient::AMRModeServiceClient(std::shared_ptr & amr_manager, + const rclcpp::NodeOptions & options) + : Node("amr_mode", options), amr_manager_(amr_manager) +{ + client_ = this->create_client("set_control_mode"); + execute_result_ = false; + + change_mode_callback_ = [&](uint8_t mode) { change_mode(mode); }; + amr_manager_->register_change_mode_callback(change_mode_callback_); +} + +AMRModeServiceClient::~AMRModeServiceClient() {} + +void AMRModeServiceClient::send_request(const SetBaseControlMode::Request::SharedPtr request) +{ + while (!client_->wait_for_service(1s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(logger_, "Interrupted while waiting for the service. Exiting."); + return; + } + RCLCPP_INFO(logger_, "service not available, waiting again..."); + } + using ServiceResponseFuture = rclcpp::Client::SharedFuture; + auto response_received_callback = [this](ServiceResponseFuture future) { + std::unique_lock lck(mtx_); + auto result = future.get(); + execute_result_ = result->result; + cv_.notify_one(); + }; + auto future_result = client_->async_send_request(request, response_received_callback); + std::unique_lock lck(mtx_); + cv_.wait(lck); +} + +bool AMRModeServiceClient::change_mode(uint8_t mode) +{ + RCLCPP_INFO(logger_, "change mode: %d", mode); + auto request = std::make_shared(); + request->mode = mode; + send_request(request); + return execute_result_; +} + +bool AMRModeServiceClient::reset_odometer() +{ + RCLCPP_INFO(logger_, "reset odom"); + // TODO + return execute_result_; +} +} // namespace amr +} // namespace qrb_ros diff --git a/qrb_ros_amr/src/service/api_service_server.cpp b/qrb_ros_amr/src/service/api_service_server.cpp new file mode 100644 index 0000000..3eb9ae5 --- /dev/null +++ b/qrb_ros_amr/src/service/api_service_server.cpp @@ -0,0 +1,72 @@ +/* + * Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved. + * SPDX-License-Identifier: BSD-3-Clause-Clear + */ + +#include "rclcpp/rclcpp.hpp" +#include "qrb_ros_amr_msgs/srv/api.hpp" +#include "nav_msgs/msg/path.hpp" +#include "api_service_server.hpp" +#include "amr_controller.hpp" +#include "cmd_action_server.hpp" + +namespace qrb_ros +{ +namespace amr +{ +APIServiceServer::APIServiceServer(std::shared_ptr & amr_manager, + std::shared_ptr & dev_sub, + const rclcpp::NodeOptions & options) + : Node("api_server", options), amr_manager_(amr_manager), dev_sub_(dev_sub) +{ + init_service(); +} + +APIServiceServer::~APIServiceServer() {} + +void APIServiceServer::init_service() +{ + using namespace std::placeholders; + srv_ptr_ = create_service( + "api", std::bind(&APIServiceServer::handle_api, this, _1, _2, _3)); +} + +void APIServiceServer::handle_api(const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response) +{ + bool succeed = amr_manager_->check_potential_state(Command::OTHER); + if (!succeed) { + RCLCPP_INFO(this->get_logger(), "current state is error"); + response->result = false; + return; + } + + int api = request->api_id; + + switch (api) { + case API::INIT_AMR: + amr_manager_->init_amr(); + break; + case API::RELEASE_AMR: + amr_manager_->release_amr(); + break; + case API::ME_COMPLETED: + amr_manager_->me_completed(); + break; + case API::ENABLE_DEVELOPER_MODE: { + bool val = request->developer_mode; + dev_sub_->set_developer_mode(val); + break; + } + default: + RCLCPP_INFO(this->get_logger(), "Incoming request api_id unknown"); + break; + } + RCLCPP_INFO( + this->get_logger(), "Incoming request api_id: %s", API::to_string(request->api_id).c_str()); + response->result = true; + return; +} +} // namespace amr +} // namespace qrb_ros \ No newline at end of file diff --git a/qrb_ros_amr/src/service/cartographer_service_client.cpp b/qrb_ros_amr/src/service/cartographer_service_client.cpp new file mode 100644 index 0000000..8de0b3e --- /dev/null +++ b/qrb_ros_amr/src/service/cartographer_service_client.cpp @@ -0,0 +1,227 @@ +/* + * Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved. + * SPDX-License-Identifier: BSD-3-Clause-Clear + */ + +#include "cartographer_service_client.hpp" + +constexpr char const * node_name = "cartographer_service_client"; +constexpr char const * service_name = "qrb_slam_command"; +constexpr char const * virtual_path_service_name = "virtual_path"; + +namespace qrb_ros +{ +namespace amr +{ +CartographerServiceClient::CartographerServiceClient(std::shared_ptr & amr_manager) + : Node(node_name), amr_manager_(amr_manager) +{ + RCLCPP_INFO(logger_, "Creating"); + init_client(); + + slam_command_callback_ = [&](uint8_t cmd, bool & result) { result = handle_slam_command(cmd); }; + amr_manager_->register_slam_command_callback(slam_command_callback_); +} + +bool CartographerServiceClient::handle_slam_command(uint8_t cmd) +{ + if (cmd == (uint8_t)Slam_Command::StartMapping) { + return start_mapping(); + } else if (cmd == (uint8_t)Slam_Command::StopMapping) { + return stop_mapping(); + } else if (cmd == (uint8_t)Slam_Command::SaveMap) { + return save_map(); + } else if (cmd == (uint8_t)Slam_Command::LoadMap) { + return load_map(); + } else if (cmd == (uint8_t)Slam_Command::StartLocalization) { + return start_localization(); + } else if (cmd == (uint8_t)Slam_Command::Relocalization) { + return relocalization(); + } + + RCLCPP_ERROR(logger_, "cmd(%d) is error", cmd); + return false; +} + +CartographerServiceClient::~CartographerServiceClient() +{ + RCLCPP_INFO(logger_, "Destroying"); +} + +bool CartographerServiceClient::start_mapping() +{ + RCLCPP_INFO(logger_, "start_mapping"); + if (!remove_waypoint_and_virtual_path()) { + RCLCPP_ERROR(logger_, "Remove waypoint and virtual path failed"); + return false; + } + + auto request = std::make_shared(); + request->command_id = CommandCode::START_MAPPING; + + return send_api_request(request); +} + +bool CartographerServiceClient::stop_mapping() +{ + RCLCPP_INFO(logger_, "stop_mapping"); + auto request = std::make_shared(); + request->command_id = CommandCode::FINISH_MAPPING; + + return send_api_request(request); +} + +bool CartographerServiceClient::save_map() +{ + RCLCPP_INFO(logger_, "save_map"); + auto request = std::make_shared(); + request->command_id = CommandCode::SAVE_MAP; + + return send_api_request(request); +} + +bool CartographerServiceClient::load_map() +{ + RCLCPP_INFO(logger_, "load_map"); + auto request = std::make_shared(); + request->command_id = CommandCode::LOAD_MAP; + + return send_api_request(request); +} + +bool CartographerServiceClient::start_localization() +{ + RCLCPP_INFO(logger_, "start_localization"); + auto request = std::make_shared(); + request->command_id = CommandCode::START_LOCALIZATION; + + return send_api_request(request); +} + +bool CartographerServiceClient::relocalization() +{ + RCLCPP_INFO(logger_, "relocalization"); + auto request = std::make_shared(); + request->command_id = CommandCode::VALIDATE_RELOCALIZATION; + + return send_api_request(request); +} + +bool CartographerServiceClient::send_api_request(SlamCommand::Request::SharedPtr request) +{ + if (!wait_for_service()) { + return false; + } + + return send_and_wait_for_request(request); +} + +bool CartographerServiceClient::wait_for_service() +{ + uint32_t wait = 0; + // wait for connectting service + while (!client_->wait_for_service(std::chrono::seconds(1))) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(logger_, "Interrupted while waiting for the service. Exiting."); + return false; + } + RCLCPP_INFO(logger_, "service not available, waiting again..."); + wait++; + if (wait >= SERVICE_TIMEOUT_DURATION) { + RCLCPP_ERROR(logger_, "Wait for service timeout, Exiting."); + return false; + } + } + return true; +} + +bool CartographerServiceClient::send_and_wait_for_request(SlamCommand::Request::SharedPtr request) +{ + using ServiceResponseFuture = rclcpp::Client::SharedFuture; + auto response_received_callback = [this](ServiceResponseFuture future) { + std::unique_lock lck(mtx_); + auto result = future.get(); + response_result_ = result->response.result; + cv_.notify_one(); + }; + auto future_result = client_->async_send_request(request, response_received_callback); + std::unique_lock lck(mtx_); + cv_.wait(lck); + + if (response_result_) { + RCLCPP_INFO(logger_, "api is excuted successful"); + } else { + RCLCPP_ERROR(logger_, "api is excuted failed"); + } + return response_result_; +} + +void CartographerServiceClient::init_client() +{ + RCLCPP_INFO(logger_, "init_client"); + client_ = this->create_client(service_name); + virtual_path_client_ = this->create_client(virtual_path_service_name); +} + +bool CartographerServiceClient::remove_waypoint_and_virtual_path() +{ + RCLCPP_INFO(logger_, "remove_waypoint_and_virtual_path"); + auto request = std::make_shared(); + request->api_id = RemoveWaypointAndVirtualPath; + + return send_api_request2(request); +} + +bool CartographerServiceClient::send_api_request2(VirtualPath::Request::SharedPtr request) +{ + if (!wait_for_service2()) { + return false; + } + + return send_and_wait_for_response2(request); +} + +bool CartographerServiceClient::wait_for_service2() +{ + uint32_t wait = 0; + // wait for connectting service + while (!virtual_path_client_->wait_for_service(std::chrono::seconds(1))) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(logger_, "Interrupted while waiting for the service. Exiting."); + return false; + } + RCLCPP_INFO(logger_, "service not available, waiting again..."); + + wait++; + if (wait >= SERVICE_TIMEOUT_DURATION) { + RCLCPP_ERROR(logger_, "Wait for service timeout, Exiting."); + return false; + } + } + return true; +} + +bool CartographerServiceClient::send_and_wait_for_response2(VirtualPath::Request::SharedPtr request) +{ + using ServiceResponseFuture = rclcpp::Client::SharedFuture; + auto response_received_callback = [this](ServiceResponseFuture future) { + std::unique_lock lck(mtx_); + auto result = future.get(); + virtual_path_response_result_ = result->result; + cv_.notify_one(); + }; + + auto future_result = + virtual_path_client_->async_send_request(request, response_received_callback); + std::unique_lock lck(mtx_); + cv_.wait(lck); + + if (virtual_path_response_result_ > 0) { + RCLCPP_INFO(logger_, "handle virutla path successful"); + return true; + } + RCLCPP_ERROR(logger_, "handle virutla path failed"); + return false; +} +} // namespace amr +} // namespace qrb_ros \ No newline at end of file diff --git a/qrb_ros_amr/src/service/mapping_service_server.cpp b/qrb_ros_amr/src/service/mapping_service_server.cpp new file mode 100644 index 0000000..989ec3a --- /dev/null +++ b/qrb_ros_amr/src/service/mapping_service_server.cpp @@ -0,0 +1,47 @@ +/* + * Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved. + * SPDX-License-Identifier: BSD-3-Clause-Clear + */ + +#include "mapping_service_server.hpp" + +namespace qrb_ros +{ +namespace amr +{ +MappingServiceServer::MappingServiceServer(std::shared_ptr & amr_manager, + const rclcpp::NodeOptions & options) + : Node("mapping_server", options), amr_manager_(amr_manager) +{ + RCLCPP_INFO(logger_, "MappingServiceServer"); + init_server(); +} + +MappingServiceServer::~MappingServiceServer() {} + +void MappingServiceServer::init_server() +{ + using namespace std::placeholders; + srv_ptr_ = this->create_service( + "amr_mapping", std::bind(&MappingServiceServer::handle_mapping, this, _1, _2, _3)); +} + +void MappingServiceServer::handle_mapping(const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response) +{ + uint8_t cmd = request->cmd; + RCLCPP_INFO(logger_, "handle_mapping, cmd=%d", cmd); + + bool result; + if (cmd == (uint8_t)Mapping_Cmd::START_MAPPING) { + result = amr_manager_->start_mapping(); + } else if (cmd == (uint8_t)Mapping_Cmd::STOP_MAPPING) { + result = amr_manager_->stop_mapping(); + } else if (cmd == (uint8_t)Mapping_Cmd::LOAD_MAP) { + result = amr_manager_->load_map(); + } + response->result = result; +} +} // namespace amr +} // namespace qrb_ros \ No newline at end of file diff --git a/qrb_ros_amr/src/service/sub_cmd_service_server.cpp b/qrb_ros_amr/src/service/sub_cmd_service_server.cpp new file mode 100644 index 0000000..2caf9ef --- /dev/null +++ b/qrb_ros_amr/src/service/sub_cmd_service_server.cpp @@ -0,0 +1,66 @@ +/* + * Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved. + * SPDX-License-Identifier: BSD-3-Clause-Clear + */ + +#include "rclcpp/rclcpp.hpp" +#include "qrb_ros_amr_msgs/srv/sub_cmd.hpp" +#include "sub_cmd_service_server.hpp" +#include "cmd_action_server.hpp" +#include "amr_manager.hpp" +#include +#include + +namespace qrb_ros +{ +namespace amr +{ +SubCmdServer::SubCmdServer(std::shared_ptr & amr_manager, + const rclcpp::NodeOptions & options) + : Node("sub_cmd_server", options), amr_manager_(amr_manager) +{ + init_service(); +} + +SubCmdServer::~SubCmdServer() {} + +void SubCmdServer::init_service() +{ + using namespace std::placeholders; + srv_ptr_ = this->create_service( + "sub_cmd", std::bind(&SubCmdServer::handle_sub_cmd, this, _1, _2, _3)); +} + +void SubCmdServer::handle_sub_cmd(const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response) +{ + RCLCPP_INFO(this->get_logger(), "handle_sub_cmd"); + (void)request_header; + int cmd = request->subcommand; + RCLCPP_INFO( + this->get_logger(), "Incoming request sub cmd: %s", (SubCommand::to_string(cmd)).c_str()); + bool succeed = amr_manager_->check_potential_state(Command::SUB_CMD); + if (!succeed) { + RCLCPP_INFO(this->get_logger(), "current state is not ON_Nav or ON_FollowPath"); + response->result = false; + return; + } + + int msg; + if (cmd == SubCommand::CANCEL) { + msg = Message::CANCEL; + } else if (cmd == SubCommand::PAUSE) { + msg = Message::PAUSE; + } else if (cmd == SubCommand::RESUME) { + msg = Message::RESUME; + } else { + RCLCPP_INFO(this->get_logger(), "error: unkown sub command"); + response->result = false; + return; + } + amr_manager_->process_sub_cmd(msg); + response->result = true; +} +} // namespace amr +} // namespace qrb_ros \ No newline at end of file diff --git a/qrb_ros_amr/src/topic/amr_status_transporter.cpp b/qrb_ros_amr/src/topic/amr_status_transporter.cpp new file mode 100644 index 0000000..0ae65f6 --- /dev/null +++ b/qrb_ros_amr/src/topic/amr_status_transporter.cpp @@ -0,0 +1,327 @@ +/* + * Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved. + * SPDX-License-Identifier: BSD-3-Clause-Clear + */ + +#include "amr_status_transporter.hpp" +#include "amr_manager.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "amr_manager/common.hpp" + +using OccupancyGrid = nav_msgs::msg::OccupancyGrid; +using namespace std::chrono_literals; + +namespace qrb_ros +{ +namespace amr +{ +AMRStatusTransporter::AMRStatusTransporter(std::shared_ptr & amr_manager, + const rclcpp::NodeOptions & options) + : Node("amr_status_transporter", options), amr_manager_(amr_manager) +{ + RCLCPP_INFO(logger_, "AMRStatusTransporter"); + init_subscription(); + init_tf_subscriber(); + init_publisher(); + last_time_ = 0; +} + +AMRStatusTransporter::~AMRStatusTransporter() {} + +void AMRStatusTransporter::init_publisher() +{ + pub_ = create_publisher("amr_status", 10); + + charger_pub_ = create_publisher("charger_cmd", 10); + + start_charging_callback_ = [&](bool start) { + if (start) { + start_charging(); + } else { + stop_charging(); + } + }; + amr_manager_->register_start_charging_callback(start_charging_callback_); + + notify_exception_callback_ = [&](bool exception, uint8_t error_code) { + if (exception) { + notify_exception_event(true, error_code); + } else { + notify_exception_event(false, 0); + } + }; + amr_manager_->register_notify_exception_callback(notify_exception_callback_); + + send_amr_state_changed_callback_ = [&](int state) { send_amr_state_changed(state); }; + amr_manager_->register_send_amr_state_changed_callback(send_amr_state_changed_callback_); + + twist_pub_ = create_publisher("cmd_vel", 10); + + publish_twist_cb_ = [&](geometry_msgs::msg::Twist & velocity) { send_velocity(velocity); }; + amr_manager_->register_publish_twist_callback(publish_twist_cb_); +} + +void AMRStatusTransporter::init_subscription() +{ + using namespace std::placeholders; + wheel_sub_ = create_subscription( + "wheel_status", 10, std::bind(&AMRStatusTransporter::wheel_status_callback, this, _1)); + battery_sub_ = create_subscription( + "battery", 10, std::bind(&AMRStatusTransporter::battery_status_callback, this, _1)); + pose_sub_ = create_subscription( + "amr_pose", 10, std::bind(&AMRStatusTransporter::pose_changed_callback, this, _1)); + vel_sub_ = create_subscription("odom", rclcpp::SystemDefaultsQoS(), + std::bind(&AMRStatusTransporter::odom_callback, this, std::placeholders::_1)); +} + +void AMRStatusTransporter::notify_exception_event(bool exception, uint8_t error_code) +{ + message_.header.stamp = get_clock()->now(); + message_.amr_exception = exception; + message_.error_code = error_code; + message_.status_change_id = (int)StatusID::AMR_Exception; + send_amr_status(message_); +} + +void AMRStatusTransporter::send_amr_status(const qrb_ros_amr_msgs::msg::AMRStatus msg) +{ + pub_->publish(msg); +} + +void AMRStatusTransporter::odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) +{ + double vel_x = msg->twist.twist.linear.x; + double vel_y = msg->twist.twist.linear.y; + double vel_z = msg->twist.twist.angular.z; + if (is_equal(vel_x, odom_velocity_.velocity.x) && is_equal(vel_y, odom_velocity_.velocity.y) && + is_equal(vel_z, odom_velocity_.velocity.theta)) { + RCLCPP_DEBUG(logger_, "velocity is not changed"); + return; + } + + long now = rclcpp::Clock().now().seconds(); + long duration = now - last_time_; + if (duration < 1) { + RCLCPP_DEBUG(logger_, "duration=%d", duration); + return; + } + last_time_ = now; + + message_.status_change_id = (int)StatusID::Velocity; + message_.vel.x = vel_x; + message_.vel.y = vel_y; + message_.vel.z = vel_z; + send_amr_status(message_); + odom_velocity_.velocity.x = vel_x; + odom_velocity_.velocity.y = vel_y; + odom_velocity_.velocity.theta = vel_z; + RCLCPP_DEBUG(logger_, "velocity_x:%f,velocity_y:%f,velocity_theta:%f", vel_x, vel_y, vel_z); +} + +void AMRStatusTransporter::wheel_status_callback( + const qrb_ros_amr_msgs::msg::WheelStatus::SharedPtr msg) +{ + // TODO: +} + +void AMRStatusTransporter::send_amr_state_changed(int state) +{ + RCLCPP_INFO(logger_, "send_amr_state_changed"); + message_.status_change_id = (int)StatusID::State_Machine; + message_.current_state = state; + send_amr_status(message_); + amr_manager_->notify_amr_state_changed(state); +} + +void AMRStatusTransporter::init_tf_subscriber() +{ + std::unique_lock lck(mtx_); + tf_buffer_ = std::make_unique(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + tf2::Quaternion q; + q.setRPY(0.0f, 0.0f, 0.0f); // yaw, pitch, roll + + // The center pose of the robot in the radar coordinate system. + // if source_pose set header.stamp, the tf transform will error. + source_pose_.pose.position.x = 0.0; + source_pose_.pose.position.y = 0.0; + source_pose_.pose.position.z = 0.0; + source_pose_.pose.orientation.x = q.x(); + source_pose_.pose.orientation.y = q.y(); + source_pose_.pose.orientation.z = q.z(); + source_pose_.pose.orientation.w = q.w(); + source_pose_.header.frame_id = source_frame_; + + // the center pose of robot in the map corrdinate system. + target_pose_.pose.position.x = 0.0; + target_pose_.pose.position.y = 0.0; + target_pose_.pose.position.z = 0.0; + target_pose_.pose.orientation.x = q.w(); + target_pose_.pose.orientation.y = q.y(); + target_pose_.pose.orientation.z = q.z(); + target_pose_.pose.orientation.w = q.w(); + target_pose_.header.stamp = this->now(); + target_pose_.header.frame_id = target_frame_; + + timer_ = this->create_wall_timer(1s, std::bind(&AMRStatusTransporter::convert_tf_to_pose, this)); +} + +void AMRStatusTransporter::convert_tf_to_pose() +{ + std::unique_lock lck(mtx_); + try { + // get the transforstamped that pose change from radar coordinate to map coordinate. + geometry_msgs::msg::TransformStamped t = + tf_buffer_->lookupTransform(target_frame_, source_frame_, tf2::TimePointZero); + + // transform the robot center pose from radar coordinate to map coordinate. + PoseStamped pose = tf_buffer_->transform(source_pose_, target_frame_, std::chrono::seconds(10)); + + if (target_pose_.header.stamp == pose.header.stamp) { + return; + } + + target_pose_.pose.position.x = pose.pose.position.x; + target_pose_.pose.position.y = pose.pose.position.y; + target_pose_.pose.position.z = pose.pose.position.z; + target_pose_.pose.orientation.x = pose.pose.orientation.x; + target_pose_.pose.orientation.y = pose.pose.orientation.y; + target_pose_.pose.orientation.z = pose.pose.orientation.z; + target_pose_.pose.orientation.w = pose.pose.orientation.w; + target_pose_.header.stamp = pose.header.stamp; + target_pose_.header.frame_id = pose.header.frame_id; + if (!is_pose_change()) { + return; + } + + RCLCPP_DEBUG(logger_, "transform pose(%f, %f, %f, %f, %f, %f, %f)", + target_pose_.pose.position.x, target_pose_.pose.position.y, target_pose_.pose.position.z, + target_pose_.pose.orientation.x, target_pose_.pose.orientation.y, + target_pose_.pose.orientation.z, target_pose_.pose.orientation.w); + + RCLCPP_DEBUG(logger_, "transform pose: header:(%d, %s)", target_pose_.header.stamp, + target_pose_.header.frame_id.c_str()); + + tf_working_ = true; + update_amr_pose(target_pose_); + last_pose_ = target_pose_; + } catch (const tf2::TransformException & ex) { + if ((count_ % 60) == 0) { + RCLCPP_ERROR(logger_, "Could not transform %s to %s: %s", source_frame_.c_str(), + target_frame_.c_str(), ex.what()); + } + tf_working_ = false; + count_++; + return; + } +} + +void AMRStatusTransporter::update_amr_pose(PoseStamped & pose) +{ + RCLCPP_DEBUG(logger_, "update_amr_pose"); + message_.status_change_id = (int)StatusID::Pose; + message_.current_pose = pose; + send_amr_status(message_); +} + +bool AMRStatusTransporter::is_pose_change() +{ + if (is_equal(target_pose_.pose.position.x, last_pose_.pose.position.x) && + is_equal(target_pose_.pose.position.y, last_pose_.pose.position.y) && + is_equal(target_pose_.pose.position.z, last_pose_.pose.position.z) && + is_equal(target_pose_.pose.orientation.x, last_pose_.pose.orientation.x) && + is_equal(target_pose_.pose.orientation.y, last_pose_.pose.orientation.y) && + is_equal(target_pose_.pose.orientation.z, last_pose_.pose.orientation.z) && + is_equal(target_pose_.pose.orientation.w, last_pose_.pose.orientation.w)) { + return false; + } + return true; +} + +bool AMRStatusTransporter::is_equal(double a, double b) +{ + double delta = fabs(a - b); + if (delta < 0.02) { + return true; + } + return false; +} + +void AMRStatusTransporter::pose_changed_callback(const PoseStamped::SharedPtr pose) +{ + std::unique_lock lck(mtx_); + target_pose_.pose.position.x = pose->pose.position.x; + target_pose_.pose.position.y = pose->pose.position.y; + target_pose_.pose.position.z = pose->pose.position.z; + target_pose_.pose.orientation.x = pose->pose.orientation.x; + target_pose_.pose.orientation.y = pose->pose.orientation.y; + target_pose_.pose.orientation.z = pose->pose.orientation.z; + target_pose_.pose.orientation.w = pose->pose.orientation.w; + target_pose_.header.stamp = pose->header.stamp; + target_pose_.header.frame_id = pose->header.frame_id; + if (!tf_working_) { + RCLCPP_INFO(logger_, "Update AMR pose(%f, %f, %f, %f, %f, %f, %f)", + target_pose_.pose.position.x, target_pose_.pose.position.y, target_pose_.pose.position.z, + target_pose_.pose.orientation.x, target_pose_.pose.orientation.y, + target_pose_.pose.orientation.z, target_pose_.pose.orientation.w); + update_amr_pose(target_pose_); + } +} + +void AMRStatusTransporter::battery_status_callback( + const sensor_msgs::msg::BatteryState::SharedPtr msg) +{ + float voltage = msg->voltage; + float current = msg->current; + uint8_t state = msg->power_supply_status; + message_.status_change_id = (int)StatusID::Battery_Level; + message_.header = msg->header; + message_.battery_vol = voltage; + + if (voltage != 0) { + RCLCPP_INFO( + logger_, "battery changed voltage=%.2f, current=%.2f, state=%d", voltage, current, state); + amr_manager_->notify_battery_changed(voltage); + amr_manager_->notify_charging_state_changed(state); + send_amr_status(message_); + } +} + +void AMRStatusTransporter::notify_battery_changed(float voltage) +{ + message_.status_change_id = (int)StatusID::Battery_Level; + message_.battery_vol = voltage; + + RCLCPP_INFO(logger_, "battery changed voltage=%.2f", voltage); + if (voltage != 0) { + amr_manager_->notify_battery_changed(voltage); + } + + send_amr_status(message_); +} + +void AMRStatusTransporter::start_charging() +{ + RCLCPP_INFO(logger_, "Start charging"); + ChargerCmd msg; + msg.cmd == ChargerCmd::START_CHARGING; + charger_pub_->publish(msg); +} + +void AMRStatusTransporter::stop_charging() +{ + RCLCPP_INFO(logger_, "Stop charging"); + ChargerCmd msg; + msg.cmd == ChargerCmd::STOP_CHARGING; + charger_pub_->publish(msg); +} + +void AMRStatusTransporter::send_velocity(geometry_msgs::msg::Twist & velocity) +{ + RCLCPP_INFO(logger_, "send velocity(%.2f, %.2f, %.2f) to robot", velocity.linear.x, + velocity.linear.y, velocity.angular.z); + twist_pub_->publish(velocity); +} +} // namespace amr +} // namespace qrb_ros \ No newline at end of file diff --git a/qrb_ros_amr/src/topic/developer_mode_subscriber.cpp b/qrb_ros_amr/src/topic/developer_mode_subscriber.cpp new file mode 100644 index 0000000..d36215e --- /dev/null +++ b/qrb_ros_amr/src/topic/developer_mode_subscriber.cpp @@ -0,0 +1,142 @@ +/* + * Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved. + * SPDX-License-Identifier: BSD-3-Clause-Clear + */ + +#include "developer_mode_subscriber.hpp" + +namespace qrb_ros +{ +namespace amr +{ +DeveloperModeSubscriber::DeveloperModeSubscriber(std::shared_ptr & amr_manager, + std::shared_ptr & exception_sub, + std::shared_ptr & tansporter, + const rclcpp::NodeOptions & options) + : Node("developer_sub", options) + , amr_manager_(amr_manager) + , exception_sub_(exception_sub) + , tansporter_(tansporter) +{ + RCLCPP_INFO(logger_, "DeveloperModeSubscriber"); + init_subscription(); +} + +DeveloperModeSubscriber::~DeveloperModeSubscriber() {} + +void DeveloperModeSubscriber::init_subscription() +{ + using namespace std::placeholders; + + sub_ = this->create_subscription( + "test", 10, std::bind(&DeveloperModeSubscriber::sub_callback, this, _1)); + debug_error_sub_ = this->create_subscription("debug_exception", 10, + std::bind(&DeveloperModeSubscriber::debug_error_sub_callback, this, _1)); +} + +void DeveloperModeSubscriber::sub_callback(const std_msgs::msg::Int16::ConstSharedPtr msg) +{ + if (!enable_developer_mode_) { + return; + } + RCLCPP_INFO(logger_, "subscribe callback"); + int16_t debug_msg = msg->data; + send_debug_message(debug_msg); +} + +void DeveloperModeSubscriber::send_debug_message(int msg) +{ + RCLCPP_INFO(logger_, "send_debug_message = %s", StringUtil::msg_to_string(msg).c_str()); + switch ((DEBUG_MSG)msg) { + case DEBUG_MSG::Init_AMR: + amr_manager_->init_amr(); + break; + case DEBUG_MSG::Release_AMR: + amr_manager_->release_amr(); + break; + case DEBUG_MSG::ME: + amr_manager_->process_cmd(Message::ME, nullptr, 0); + break; + case DEBUG_MSG::AE: + amr_manager_->process_cmd(Message::AE, nullptr, 0); + break; + case DEBUG_MSG::ME_Finish: + amr_manager_->process_event(Message::ME_FINISH); + break; + case DEBUG_MSG::AE_Finish: + amr_manager_->process_event(Message::AE_FINISH); + break; + case DEBUG_MSG::ME_Completed: + amr_manager_->me_completed(); + break; + case DEBUG_MSG::Pause: + amr_manager_->process_event(Message::PAUSE); + break; + case DEBUG_MSG::Resume: + amr_manager_->process_event(Message::RESUME); + break; + case DEBUG_MSG::Cancel: + amr_manager_->process_event(Message::CANCEL); + break; + case DEBUG_MSG::P2PNav_Finish: + amr_manager_->process_event(Message::P2PNAV_FINISH); + break; + case DEBUG_MSG::FollowPath_Finish: + amr_manager_->process_event(Message::FOLLOW_PATH_FINISH); + break; + case DEBUG_MSG::Low_Power: { + float battery_vol = 21; + tansporter_->notify_battery_changed(battery_vol); + break; + } + case DEBUG_MSG::Normal_Power: { + float battery_vol = 24; + tansporter_->notify_battery_changed(battery_vol); + amr_manager_->process_event(Message::NORMAL_POWER); + break; + } + case DEBUG_MSG::Return_Charging: + amr_manager_->process_cmd(Message::RETURN_CHARGING, nullptr, 0); + break; + case DEBUG_MSG::Return_Charging_Finish: + amr_manager_->process_event(Message::RETURN_CHARGING_FINISH); + break; + case DEBUG_MSG::AMR_Exception: { + int error_code = 1; // this is test error code + amr_manager_->process_event(Message::AMR_EXCEPTION, error_code); + break; + } + case DEBUG_MSG::AMR_Normal: + amr_manager_->process_event(Message::AMR_NORMAL); + break; + case DEBUG_MSG::Relocalization_Pass: + amr_manager_->process_event(Message::RELOCALIZATION_PASS); + break; + default: + RCLCPP_ERROR(logger_, "Not support debug message = %d", msg); + break; + } +} + +void DeveloperModeSubscriber::set_developer_mode(bool val) +{ + enable_developer_mode_ = val; + if (enable_developer_mode_) { + RCLCPP_INFO(logger_, "Enable developer mode"); + } else { + RCLCPP_INFO(logger_, "Disable developer mode"); + } +} + +void DeveloperModeSubscriber::debug_error_sub_callback( + const std_msgs::msg::Int16::ConstSharedPtr msg) +{ + if (!enable_developer_mode_) { + return; + } + uint8_t debug_msg = msg->data; + RCLCPP_INFO(logger_, "debug error subscribe callback, debug_msg=%d", debug_msg); + exception_sub_->handle_debug_msg(debug_msg); +} +} // namespace amr +} // namespace qrb_ros diff --git a/qrb_ros_amr/src/topic/exception_subscriber.cpp b/qrb_ros_amr/src/topic/exception_subscriber.cpp new file mode 100644 index 0000000..f94a555 --- /dev/null +++ b/qrb_ros_amr/src/topic/exception_subscriber.cpp @@ -0,0 +1,79 @@ +/* + * Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved. + * SPDX-License-Identifier: BSD-3-Clause-Clear + */ + +#include "rclcpp/rclcpp.hpp" +#include "exception_subscriber.hpp" +#include "amr_manager.hpp" + +namespace qrb_ros +{ +namespace amr +{ +ExceptionSubscriber::ExceptionSubscriber(std::shared_ptr & amr_manager, + const rclcpp::NodeOptions & options) + : Node("exception_sub", options), amr_manager_(amr_manager) +{ + init_subscription(); +} + +ExceptionSubscriber::~ExceptionSubscriber() {} + +void ExceptionSubscriber::init_subscription() +{ + using namespace std::placeholders; + sub_ = create_subscription( + "robot_base_error", 10, std::bind(&ExceptionSubscriber::subscriber_callback, this, _1)); +} + +void ExceptionSubscriber::subscriber_callback( + const qrb_ros_robot_base_msgs::msg::Error::SharedPtr msg) +{ + int message; + uint8_t error = msg->type; + + if (error == qrb_ros_robot_base_msgs::msg::Error::ERROR_WATCHDOG) { + message = Message::AMR_EXCEPTION; + RCLCPP_ERROR(logger_, "Receive watchdog error"); + } else if (error == qrb_ros_robot_base_msgs::msg::Error::ERROR_MOTOR) { + message = Message::AMR_EXCEPTION; + RCLCPP_ERROR(logger_, "Receive motor error"); + } else if (error == qrb_ros_robot_base_msgs::msg::Error::ERROR_OTHER) { + RCLCPP_ERROR(logger_, "Receive other error"); + return; + } else if (error == qrb_ros_robot_base_msgs::msg::Error::ERROR_CHARGER) { + RCLCPP_ERROR(logger_, "Receive changer error"); + return; + } else { + RCLCPP_ERROR(logger_, "Receive message = %d", error); + message = Message::AMR_NORMAL; + } + amr_manager_->process_event(message, error); +} + +void ExceptionSubscriber::handle_debug_msg(uint8_t error) +{ + int message; + + if (error == qrb_ros_robot_base_msgs::msg::Error::ERROR_WATCHDOG) { + message = Message::AMR_EXCEPTION; + RCLCPP_ERROR(logger_, "Receive watchdog error"); + } else if (error == qrb_ros_robot_base_msgs::msg::Error::ERROR_MOTOR) { + message = Message::AMR_EXCEPTION; + RCLCPP_ERROR(logger_, "Receive motor error"); + } else if (error == qrb_ros_robot_base_msgs::msg::Error::ERROR_OTHER) { + RCLCPP_ERROR(logger_, "Receive other error"); + return; + } else if (error == qrb_ros_robot_base_msgs::msg::Error::ERROR_CHARGER) { + RCLCPP_ERROR(logger_, "Receive changer error"); + return; + } else { + RCLCPP_ERROR(logger_, "Receive message = %d", error); + message = Message::AMR_NORMAL; + } + RCLCPP_INFO(logger_, "error = %d", error); + amr_manager_->process_event(message, error); +} +} // namespace amr +} // namespace qrb_ros diff --git a/qrb_ros_amr/src/topic/map_subscriber.cpp b/qrb_ros_amr/src/topic/map_subscriber.cpp new file mode 100644 index 0000000..524864e --- /dev/null +++ b/qrb_ros_amr/src/topic/map_subscriber.cpp @@ -0,0 +1,64 @@ +/* + * Copyright (c) 2024 Qualcomm Innovation Center, Inc. All rights reserved. + * SPDX-License-Identifier: BSD-3-Clause-Clear + */ + +#include "map_subscriber.hpp" +#include "amr_status_transporter.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include +#include +#include + +using OccupancyGrid = nav_msgs::msg::OccupancyGrid; + +#define MAP_PATH "map.txt" + +namespace qrb_ros +{ +namespace amr +{ +MapSubscriber::MapSubscriber(std::shared_ptr & amr_manager, + std::shared_ptr & amr_status_transporter, + const rclcpp::NodeOptions & options) + : Node("map_sub", options) + , amr_status_transporter_(amr_status_transporter) + , amr_manager_(amr_manager) +{ + init_subscription(); + read_map(); +} + +MapSubscriber::~MapSubscriber() {} + +void MapSubscriber::init_subscription() +{ + using namespace std::placeholders; + sub_ = create_subscription( + "map", 10, std::bind(&MapSubscriber::subscriber_callback, this, _1)); +} + +void MapSubscriber::subscriber_callback(const OccupancyGrid::SharedPtr map) +{ + if (receive_map_count_ > 1) { + return; + } + RCLCPP_INFO(this->get_logger(), "receive a map info"); + amr_status_transporter_->update_map(*map); + save_map(*map); + receive_map_count_++; +} + +void MapSubscriber::save_map(OccupancyGrid map) +{ + RCLCPP_INFO(this->get_logger(), "start save file"); + + RCLCPP_INFO(this->get_logger(), "finish save file"); +} + +bool MapSubscriber::read_map() +{ + return false; +} +} // namespace amr +} // namespace qrb_ros \ No newline at end of file