├── .gitignore ├── .travis.yml ├── COPYING.txt ├── README.md ├── images ├── robot.jpg └── system.png ├── license.txt ├── roborts └── package.xml ├── roborts_base ├── CMakeLists.txt ├── chassis │ ├── chassis.cpp │ └── chassis.h ├── cmake_module │ └── FindGlog.cmake ├── config │ └── roborts_base_parameter.yaml ├── gimbal │ ├── gimbal.cpp │ └── gimbal.h ├── package.xml ├── referee_system │ ├── referee_system.cpp │ └── referee_system.h ├── roborts_base_config.h ├── roborts_base_node.cpp ├── roborts_sdk │ ├── dispatch │ │ ├── dispatch.h │ │ ├── execution.cpp │ │ ├── execution.h │ │ ├── handle.cpp │ │ └── handle.h │ ├── hardware │ │ ├── hardware_interface.h │ │ ├── serial_device.cpp │ │ └── serial_device.h │ ├── protocol │ │ ├── protocol.cpp │ │ ├── protocol.h │ │ └── protocol_define.h │ ├── sdk.h │ ├── test │ │ └── sdk_test.cpp │ └── utilities │ │ ├── circular_buffer.h │ │ ├── crc.h │ │ ├── log.h │ │ └── memory_pool.h └── ros_dep.h ├── roborts_bringup ├── CMakeLists.txt ├── launch │ ├── base.launch │ ├── mapping.launch │ ├── mapping_stage.launch │ ├── roborts.launch │ ├── roborts_stage.launch │ ├── slam_gmapping.xml │ └── static_tf.launch ├── maps │ ├── icra2018.pgm │ ├── icra2018.yaml │ ├── icra2019.pgm │ └── icra2019.yaml ├── package.xml ├── rviz │ ├── mapping.rviz │ ├── roborts.rviz │ └── teb_test.rviz ├── scripts │ ├── udev │ │ ├── create_udev_rules.sh │ │ ├── delete_udev_rules.sh │ │ └── roborts.rules │ └── upstart │ │ ├── create_upstart_service.sh │ │ ├── delete_upstart_service.sh │ │ ├── jetson_clocks.sh │ │ ├── max-performance.service │ │ ├── max_performance.sh │ │ ├── roborts-start.sh │ │ └── roborts.service └── worlds │ ├── icra2018.world │ └── icra2019.world ├── roborts_camera ├── CMakeLists.txt ├── camera_base.h ├── camera_node.cpp ├── camera_node.h ├── camera_param.cpp ├── camera_param.h ├── cmake_module │ └── FindProtoBuf.cmake ├── config │ └── camera_param.prototxt ├── package.xml ├── proto │ └── camera_param.proto ├── test │ └── image_capture.cpp └── uvc │ ├── CMakeLists.txt │ ├── uvc_driver.cpp │ └── uvc_driver.h ├── roborts_common ├── CMakeLists.txt ├── cmake_module │ ├── FindEigen3.cmake │ ├── FindGTEST.cmake │ ├── FindGlog.cmake │ └── FindProtoBuf.cmake ├── include │ ├── alg_factory │ │ ├── CMakeLists.txt │ │ ├── algorithm_factory.h │ │ └── bind_this.h │ ├── io │ │ ├── CMakeLists.txt │ │ └── io.h │ ├── state │ │ ├── CMakeLists.txt │ │ ├── error_code.h │ │ └── node_state.h │ └── timer │ │ ├── CMakeLists.txt │ │ └── timer.h ├── math │ ├── CMakeLists.txt │ ├── geometry.h │ └── math.h └── package.xml ├── roborts_costmap ├── CMakeLists.txt ├── cmake_module │ ├── FindEigen3.cmake │ └── FindProtoBuf.cmake ├── config │ ├── costmap_parameter_config_for_decision.prototxt │ ├── costmap_parameter_config_for_global_plan.prototxt │ ├── costmap_parameter_config_for_local_plan.prototxt │ ├── inflation_layer_config.prototxt │ ├── inflation_layer_config_min.prototxt │ ├── obstacle_layer_config.prototxt │ └── static_layer_config.prototxt ├── include │ └── costmap │ │ ├── costmap_2d.h │ │ ├── costmap_interface.h │ │ ├── costmap_layer.h │ │ ├── costmap_math.h │ │ ├── footprint.h │ │ ├── inflation_layer.h │ │ ├── layer.h │ │ ├── layered_costmap.h │ │ ├── map_common.h │ │ ├── observation.h │ │ ├── observation_buffer.h │ │ ├── obstacle_layer.h │ │ └── static_layer.h ├── package.xml ├── proto │ ├── costmap_parameter_setting.proto │ ├── inflation_layer_setting.proto │ ├── obstacle_layer_setting.proto │ └── static_layer_setting.proto └── src │ ├── costmap_2d.cpp │ ├── costmap_interface.cpp │ ├── costmap_layer.cpp │ ├── costmap_math.cpp │ ├── footprint.cpp │ ├── inflation_layer.cpp │ ├── layer.cpp │ ├── layered_costmap.cpp │ ├── observation_buffer.cpp │ ├── obstacle_layer.cpp │ ├── static_layer.cpp │ └── test_costmap.cpp ├── roborts_decision ├── CMakeLists.txt ├── behavior_test.cpp ├── behavior_tree │ ├── behavior_node.h │ ├── behavior_state.h │ └── behavior_tree.h ├── blackboard │ └── blackboard.h ├── cmake_module │ ├── FindEigen3.cmake │ └── FindProtoBuf.cmake ├── config │ └── decision.prototxt ├── example_behavior │ ├── back_boot_area_behavior.h │ ├── chase_behavior.h │ ├── escape_behavior.h │ ├── goal_behavior.h │ ├── line_iterator.h │ ├── patrol_behavior.h │ └── search_behavior.h ├── executor │ ├── chassis_executor.cpp │ ├── chassis_executor.h │ ├── gimbal_executor.cpp │ └── gimbal_executor.h ├── package.xml └── proto │ └── decision.proto ├── roborts_detection ├── CMakeLists.txt ├── armor_detection │ ├── CMakeLists.txt │ ├── armor_detection_algorithms.h │ ├── armor_detection_base.h │ ├── armor_detection_client.cpp │ ├── armor_detection_node.cpp │ ├── armor_detection_node.h │ ├── config │ │ └── armor_detection.prototxt │ ├── constraint_set │ │ ├── CMakeLists.txt │ │ ├── config │ │ │ └── constraint_set.prototxt │ │ ├── constraint_set.cpp │ │ ├── constraint_set.h │ │ └── proto │ │ │ └── constraint_set.proto │ ├── gimbal_control.cpp │ ├── gimbal_control.h │ └── proto │ │ └── armor_detection.proto ├── cmake_module │ ├── FindEigen3.cmake │ └── FindProtoBuf.cmake ├── package.xml └── util │ ├── CMakeLists.txt │ └── cv_toolbox.h ├── roborts_localization ├── CMakeLists.txt ├── amcl │ ├── CMakeLists.txt │ ├── amcl.cpp │ ├── amcl.h │ ├── amcl_config.h │ ├── config │ │ └── amcl.yaml │ ├── map │ │ ├── amcl_map.cpp │ │ └── amcl_map.h │ ├── particle_filter │ │ ├── particle_filter.cpp │ │ ├── particle_filter.h │ │ ├── particle_filter_gaussian_pdf.cpp │ │ ├── particle_filter_gaussian_pdf.h │ │ ├── particle_filter_kdtree.cpp │ │ ├── particle_filter_kdtree.h │ │ └── particle_filter_sample.h │ └── sensors │ │ ├── sensor_laser.cpp │ │ ├── sensor_laser.h │ │ ├── sensor_odom.cpp │ │ └── sensor_odom.h ├── cmake_module │ ├── FindEigen3.cmake │ └── FindGlog.cmake ├── config │ └── localization.yaml ├── localization_config.h ├── localization_math.cpp ├── localization_math.h ├── localization_node.cpp ├── localization_node.h ├── log.h ├── package.xml └── types.h ├── roborts_msgs ├── CMakeLists.txt ├── action │ ├── ArmorDetection.action │ ├── GlobalPlanner.action │ └── LocalPlanner.action ├── msg │ ├── GimbalAngle.msg │ ├── GimbalRate.msg │ ├── ObstacleMsg.msg │ ├── ShootInfo.msg │ ├── ShootState.msg │ ├── TwistAccel.msg │ └── referee_system │ │ ├── BonusStatus.msg │ │ ├── GameResult.msg │ │ ├── GameStatus.msg │ │ ├── GameSurvivor.msg │ │ ├── ProjectileSupply.msg │ │ ├── RobotBonus.msg │ │ ├── RobotDamage.msg │ │ ├── RobotHeat.msg │ │ ├── RobotShoot.msg │ │ ├── RobotStatus.msg │ │ └── SupplierStatus.msg ├── package.xml └── srv │ ├── FricWhl.srv │ ├── GimbalMode.srv │ └── ShootCmd.srv ├── roborts_planning ├── CMakeLists.txt ├── cmake_module │ ├── FindEigen3.cmake │ ├── FindG2O.cmake │ ├── FindProtoBuf.cmake │ └── FindSUITESPARSE.cmake ├── global_planner │ ├── CMakeLists.txt │ ├── a_star_planner │ │ ├── CMakeLists.txt │ │ ├── a_star_planner.cpp │ │ ├── a_star_planner.h │ │ ├── config │ │ │ └── a_star_planner_config.prototxt │ │ └── proto │ │ │ └── a_star_planner_config.proto │ ├── config │ │ └── global_planner_config.prototxt │ ├── global_planner_algorithms.h │ ├── global_planner_base.h │ ├── global_planner_node.cpp │ ├── global_planner_node.h │ ├── global_planner_test.cpp │ └── proto │ │ └── global_planner_config.proto ├── local_planner │ ├── CMakeLists.txt │ ├── README.md │ ├── config │ │ └── local_planner.prototxt │ ├── include │ │ └── local_planner │ │ │ ├── data_base.h │ │ │ ├── data_converter.h │ │ │ ├── distance_calculation.h │ │ │ ├── line_iterator.h │ │ │ ├── local_planner_algorithms.h │ │ │ ├── local_planner_base.h │ │ │ ├── local_planner_node.h │ │ │ ├── local_visualization.h │ │ │ ├── obstacle.h │ │ │ ├── odom_info.h │ │ │ ├── optimal_base.h │ │ │ ├── proto │ │ │ └── local_planner.proto │ │ │ ├── robot_footprint_model.h │ │ │ ├── robot_position_cost.h │ │ │ └── utility_tool.h │ ├── src │ │ ├── local_planner_node.cpp │ │ ├── local_visualization.cpp │ │ ├── obstacles.cpp │ │ ├── odom_info.cpp │ │ ├── robot_position_cost.cpp │ │ ├── teb_test.cpp │ │ └── vel_converter.cpp │ └── timed_elastic_band │ │ ├── CMakeLists.txt │ │ ├── config │ │ ├── hcp.prototxt │ │ ├── recovery.prototxt │ │ └── timed_elastic_band.prototxt │ │ ├── include │ │ └── timed_elastic_band │ │ │ ├── proto │ │ │ └── timed_elastic_band.proto │ │ │ ├── teb_acceleration_eage.h │ │ │ ├── teb_base_eage.h │ │ │ ├── teb_kinematics_edge.h │ │ │ ├── teb_local_planner.h │ │ │ ├── teb_obstacle_eage.h │ │ │ ├── teb_optimal.h │ │ │ ├── teb_penalties.h │ │ │ ├── teb_prefer_rotdir_edge.h │ │ │ ├── teb_time_optimal_eage.h │ │ │ ├── teb_velocity_eage.h │ │ │ ├── teb_vertex_console.h │ │ │ ├── teb_vertex_pose.h │ │ │ ├── teb_vertex_timediff.h │ │ │ ├── teb_via_point_edge.h │ │ │ └── timed_elastic_band.hpp │ │ └── src │ │ ├── teb_local_planner.cpp │ │ ├── teb_optimal.cpp │ │ └── teb_vertex_console.cpp └── package.xml └── roborts_tracking ├── CMakeLists.txt ├── KCFcpp ├── CMakeLists.txt ├── KCFCpp.sh ├── KCFLabCpp.sh ├── LICENSE ├── README.md └── src │ ├── ffttools.hpp │ ├── fhog.cpp │ ├── fhog.hpp │ ├── kcftracker.cpp │ ├── kcftracker.hpp │ ├── labdata.hpp │ ├── recttools.hpp │ ├── runtracker.cpp │ └── tracker.h ├── package.xml ├── tracking_test.cpp ├── tracking_utility.cpp └── tracking_utility.h /.gitignore: -------------------------------------------------------------------------------- 1 | *.pb.cc 2 | *.pb.h 3 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | sudo: required 2 | 3 | language: 4 | - generic 5 | 6 | service: 7 | - docker 8 | 9 | before_install: 10 | - docker pull kevinlad/ros-nav-ubuntu-64 11 | 12 | install: 13 | - docker run -dit --name rrts_test kevinlad/ros-nav-ubuntu-64 14 | - docker exec rrts_test bash rrts_build_test.sh 15 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # RoboRTS 2 | [![Build Status](https://travis-ci.org/RoboMaster/RoboRTS.svg?branch=master)](https://travis-ci.org/RoboMaster/RoboRTS) 3 | [![Gitter](https://badges.gitter.im/RoboMaster/RoboRTS.svg)](https://gitter.im/RoboMaster/RoboRTS?utm_source=badge&utm_medium=badge&utm_campaign=pr-badge) 4 | 5 | ## Introduction 6 | 7 | 8 | 9 | RoboRTS is an open source software stack for Real-Time Strategy research on mobile robots, developed by RoboMaster and motivated by [RoboMaster AI Challenge](#competition) 10 | 11 | The framework of RoboRTS consists two parts: autonomous mobile robot layer and intelligent decision-making layer. 12 | 13 | 14 | 15 | The autonomous mobile robot layer alone can let a robot, offically supported for RoboMaster AI Robot platform, to demonstrate a certain level of intelligence. On its on-board computer runs perception, motion planning, decision modules. It is fully supported in ROS with community driven as well as platform-customized codes and examples. On its MCU, an RT low-level robot controller is implemented to govern the robot driving system. 16 | 17 | **TODO:** Intelligent decision-making layer includes a multi-agent decision-making framework and a game simulator, it will be released soon in the future. 18 | 19 | ## Tutorial 20 | 21 | For more information about RoboMaster AI Robot platform and RoboRTS framework, please refer to [RoboRTS Tutorial](https://robomaster.github.io/RoboRTS-Tutorial/#/) 22 | 23 | ## Competition 24 | 25 | RoboMaster holds AI Challenge since 2017. In the challenge, multiple robots should fight with each other on a game field automatically under different rules. 26 | 27 | For more information, please refer to 28 | 29 | - [DJI RoboMaster 2019 ICRA AI Challenge](https://icra2019.org/competitions/dji-robomaster-ai-challenge) 30 | 31 | - [DJI RoboMaster 2018 ICRA AI Challenge](https://icra2018.org/dji-robomaster-ai-challenge/) 32 | 33 | ## Copyright and License 34 | 35 | RoboRTS is provided under the [GPL-v3](COPYING). 36 | -------------------------------------------------------------------------------- /images/robot.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoboMaster/RoboRTS/b6b8f811781b00caf956303f4b63a2d5af518fcf/images/robot.jpg -------------------------------------------------------------------------------- /images/system.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoboMaster/RoboRTS/b6b8f811781b00caf956303f4b63a2d5af518fcf/images/system.png -------------------------------------------------------------------------------- /roborts/package.xml: -------------------------------------------------------------------------------- 1 | 2 | roborts 3 | 1.0.0 4 | 5 | The roborts meta package provides all the functionality for RoboMaster AI Robot 6 | 7 | RoboMaster 8 | RoboMaster 9 | GPL 3.0 10 | https://github.com/RoboMaster/RoboRTS 11 | 12 | catkin 13 | roborts_base 14 | roborts_bringup 15 | roborts_camera 16 | roborts_common 17 | roborts_costmap 18 | roborts_decision 19 | roborts_localization 20 | roborts_msgs 21 | roborts_planning 22 | roborts_tracking 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /roborts_base/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(roborts_base) 2 | cmake_minimum_required(VERSION 3.1) 3 | set(CMAKE_CXX_STANDARD 14) 4 | set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake_module) 5 | set(CMAKE_BUILD_TYPE Release) 6 | find_package(Threads REQUIRED) 7 | find_package(Glog REQUIRED) 8 | find_package(catkin REQUIRED COMPONENTS 9 | roscpp 10 | tf 11 | roborts_msgs 12 | ) 13 | 14 | catkin_package() 15 | 16 | add_executable(roborts_base_node 17 | roborts_base_node.cpp 18 | chassis/chassis.cpp 19 | gimbal/gimbal.cpp 20 | referee_system/referee_system.cpp 21 | roborts_sdk/dispatch/execution.cpp 22 | roborts_sdk/dispatch/handle.cpp 23 | roborts_sdk/protocol/protocol.cpp 24 | roborts_sdk/hardware/serial_device.cpp 25 | ) 26 | target_link_libraries(roborts_base_node PUBLIC 27 | Threads::Threads 28 | ${GLOG_LIBRARY} 29 | ${catkin_LIBRARIES}) 30 | target_include_directories(roborts_base_node PUBLIC 31 | ${catkin_INCLUDE_DIRS}) 32 | add_dependencies(roborts_base_node roborts_msgs_generate_messages) 33 | 34 | -------------------------------------------------------------------------------- /roborts_base/cmake_module/FindGlog.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Glog 2 | # 3 | # The following variables are optionally searched for defaults 4 | # GLOG_ROOT_DIR: Base directory where all GLOG components are found 5 | # 6 | # The following are set after configuration is done: 7 | # GLOG_FOUND 8 | # GLOG_INCLUDE_DIRS 9 | # GLOG_LIBRARIES 10 | # GLOG_LIBRARYRARY_DIRS 11 | 12 | include(FindPackageHandleStandardArgs) 13 | 14 | set(GLOG_ROOT_DIR "" CACHE PATH "Folder contains Google glog") 15 | 16 | if(WIN32) 17 | find_path(GLOG_INCLUDE_DIR glog/logging.h 18 | PATHS ${GLOG_ROOT_DIR}/src/windows) 19 | else() 20 | find_path(GLOG_INCLUDE_DIR glog/logging.h 21 | PATHS ${GLOG_ROOT_DIR}) 22 | endif() 23 | 24 | if(MSVC) 25 | find_library(GLOG_LIBRARY_RELEASE libglog_static 26 | PATHS ${GLOG_ROOT_DIR} 27 | PATH_SUFFIXES Release) 28 | 29 | find_library(GLOG_LIBRARY_DEBUG libglog_static 30 | PATHS ${GLOG_ROOT_DIR} 31 | PATH_SUFFIXES Debug) 32 | 33 | set(GLOG_LIBRARY optimized ${GLOG_LIBRARY_RELEASE} debug ${GLOG_LIBRARY_DEBUG}) 34 | else() 35 | find_library(GLOG_LIBRARY glog 36 | PATHS ${GLOG_ROOT_DIR} 37 | PATH_SUFFIXES lib lib64) 38 | endif() 39 | 40 | find_package_handle_standard_args(Glog DEFAULT_MSG GLOG_INCLUDE_DIR GLOG_LIBRARY) 41 | 42 | if(GLOG_FOUND) 43 | set(GLOG_INCLUDE_DIRS ${GLOG_INCLUDE_DIR}) 44 | set(GLOG_LIBRARIES ${GLOG_LIBRARY}) 45 | message(STATUS "Found glog (include: ${GLOG_INCLUDE_DIR}, library: ${GLOG_LIBRARY})") 46 | mark_as_advanced(GLOG_ROOT_DIR GLOG_LIBRARY_RELEASE GLOG_LIBRARY_DEBUG 47 | GLOG_LIBRARY GLOG_INCLUDE_DIR) 48 | endif() 49 | -------------------------------------------------------------------------------- /roborts_base/config/roborts_base_parameter.yaml: -------------------------------------------------------------------------------- 1 | serial_port : "/dev/serial_sdk" -------------------------------------------------------------------------------- /roborts_base/package.xml: -------------------------------------------------------------------------------- 1 | 2 | roborts_base 3 | 1.0.0 4 | 5 | The roborts_base package provides protocol sdk for RoboMaster AI Robot 6 | 7 | RoboMaster 8 | RoboMaster 9 | GPL 3.0 10 | https://github.com/RoboMaster/RoboRTS 11 | 12 | catkin 13 | 14 | roscpp 15 | rospy 16 | tf 17 | roborts_msgs 18 | 19 | roscpp 20 | rospy 21 | tf 22 | roborts_msgs 23 | 24 | 25 | -------------------------------------------------------------------------------- /roborts_base/referee_system/referee_system.h: -------------------------------------------------------------------------------- 1 | #ifndef ROBORTS_BASE_REFEREE_SYSTEM_H 2 | #define ROBORTS_BASE_REFEREE_SYSTEM_H 3 | 4 | #include "../roborts_sdk/sdk.h" 5 | #include "../ros_dep.h" 6 | 7 | namespace roborts_base { 8 | /** 9 | * @brief ROS API for referee system module 10 | */ 11 | class RefereeSystem { 12 | public: 13 | /** 14 | * @brief Constructor of referee system including initialization of sdk and ROS 15 | * @param handle handler of sdk 16 | */ 17 | RefereeSystem(std::shared_ptr handle); 18 | /** 19 | * @brief Destructor of referee system 20 | */ 21 | ~RefereeSystem() = default; 22 | private: 23 | /** 24 | * @brief Initialization of sdk 25 | */ 26 | void SDK_Init(); 27 | /** 28 | * @brief Initialization of ROS 29 | */ 30 | void ROS_Init(); 31 | 32 | /** Game Related **/ 33 | void GameStateCallback(const std::shared_ptr raw_game_status); 34 | 35 | void GameResultCallback(const std::shared_ptr raw_game_result); 36 | 37 | void GameSurvivorCallback(const std::shared_ptr raw_game_survivor); 38 | 39 | /** Field Related **/ 40 | void GameEventCallback(const std::shared_ptr raw_game_event); 41 | 42 | void SupplierStatusCallback(const std::shared_ptr raw_supplier_status); 43 | 44 | /** Robot Related **/ 45 | void RobotStatusCallback(const std::shared_ptr raw_robot_status); 46 | 47 | void RobotHeatCallback(const std::shared_ptr raw_robot_heat); 48 | 49 | void RobotBonusCallback(const std::shared_ptr raw_robot_bonus); 50 | 51 | void RobotDamageCallback(const std::shared_ptr raw_robot_damage); 52 | 53 | void RobotShootCallback(const std::shared_ptr raw_robot_shoot); 54 | 55 | /** ROS Related **/ 56 | void ProjectileSupplyCallback(const roborts_msgs::ProjectileSupply::ConstPtr projectile_supply); 57 | 58 | //! sdk handler 59 | std::shared_ptr handle_; 60 | 61 | std::shared_ptr> projectile_supply_pub_; 62 | 63 | //! ros node handler 64 | ros::NodeHandle ros_nh_; 65 | //! ros subscriber for projectile supply 66 | ros::Subscriber ros_sub_projectile_supply_; 67 | 68 | ros::Publisher ros_game_status_pub_; 69 | ros::Publisher ros_game_result_pub_ ; 70 | ros::Publisher ros_game_survival_pub_; 71 | 72 | ros::Publisher ros_bonus_status_pub_; 73 | ros::Publisher ros_supplier_status_pub_; 74 | 75 | ros::Publisher ros_robot_status_pub_; 76 | ros::Publisher ros_robot_heat_pub_; 77 | ros::Publisher ros_robot_bonus_pub_; 78 | ros::Publisher ros_robot_damage_pub_; 79 | ros::Publisher ros_robot_shoot_pub_; 80 | 81 | uint8_t robot_id_ = 0xFF; 82 | }; 83 | } 84 | 85 | #endif //ROBORTS_BASE_REFEREE_SYSTEM_H 86 | -------------------------------------------------------------------------------- /roborts_base/roborts_base_config.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of  11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | 18 | #ifndef ROBORTS_BASE_CONFIG_H 19 | #define ROBORTS_BASE_CONFIG_H 20 | #include 21 | 22 | namespace roborts_base{ 23 | 24 | struct Config { 25 | void GetParam(ros::NodeHandle *nh) { 26 | nh->param("serial_port", serial_port, "/dev/serial_sdk"); 27 | } 28 | std::string serial_port; 29 | }; 30 | 31 | } 32 | #endif //ROBORTS_BASE_CONFIG_H -------------------------------------------------------------------------------- /roborts_base/roborts_base_node.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of  11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | 18 | #include "gimbal/gimbal.h" 19 | #include "chassis/chassis.h" 20 | #include "referee_system/referee_system.h" 21 | #include "roborts_base_config.h" 22 | 23 | 24 | int main(int argc, char **argv){ 25 | ros::init(argc, argv, "roborts_base_node"); 26 | ros::NodeHandle nh; 27 | roborts_base::Config config; 28 | config.GetParam(&nh); 29 | auto handle = std::make_shared(config.serial_port); 30 | if(!handle->Init()) return 1; 31 | 32 | roborts_base::Chassis chassis(handle); 33 | roborts_base::Gimbal gimbal(handle); 34 | roborts_base::RefereeSystem referee_system(handle); 35 | while(ros::ok()) { 36 | 37 | handle->Spin(); 38 | ros::spinOnce(); 39 | usleep(1000); 40 | } 41 | 42 | } -------------------------------------------------------------------------------- /roborts_base/roborts_sdk/dispatch/execution.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of  11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | 18 | #include "execution.h" 19 | 20 | namespace roborts_sdk { 21 | Executor::Executor(std::shared_ptr handle) : handle_(handle) {} 22 | std::shared_ptr Executor::GetHandle() { 23 | return handle_; 24 | } 25 | 26 | void Executor::ExecuteSubscription(const std::shared_ptr& subscription) { 27 | auto message_header = subscription->CreateMessageHeader(); 28 | std::shared_ptr message = subscription->CreateMessage(); 29 | 30 | bool ret = GetHandle()->GetProtocol()->Take( 31 | subscription->GetCommandInfo().get(), 32 | message_header.get(), 33 | message.get()); 34 | if (ret) { 35 | subscription->HandleMessage(message_header, message); 36 | } else { 37 | // DLOG_ERROR<<"take message failed!"; 38 | } 39 | //TODO: add return message; 40 | //subscription->return_message(message); 41 | } 42 | void Executor::ExecuteService(const std::shared_ptr& service) { 43 | auto request_header = service->CreateRequestHeader(); 44 | std::shared_ptr request = service->CreateRequest(); 45 | 46 | bool ret = GetHandle()->GetProtocol()->Take( 47 | service->GetCommandInfo().get(), 48 | request_header.get(), 49 | request.get()); 50 | if (ret) { 51 | service->HandleRequest(request_header, request); 52 | } else { 53 | DLOG_ERROR << "take request failed!"; 54 | } 55 | } 56 | void Executor::ExecuteClient(const std::shared_ptr& client) { 57 | auto request_header = client->CreateRequestHeader(); 58 | std::shared_ptr response = client->CreateResponse(); 59 | 60 | bool ret = GetHandle()->GetProtocol()->Take( 61 | client->GetCommandInfo().get(), 62 | request_header.get(), 63 | response.get()); 64 | 65 | if (ret) { 66 | client->HandleResponse(request_header, response); 67 | } else { 68 | // DLOG_ERROR<<"take response failed!"; 69 | } 70 | } 71 | } -------------------------------------------------------------------------------- /roborts_base/roborts_sdk/dispatch/execution.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of  11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | 18 | #ifndef ROBORTS_SDK_EXECUTION_H 19 | #define ROBORTS_SDK_EXECUTION_H 20 | #include "dispatch.h" 21 | 22 | namespace roborts_sdk { 23 | class Handle; 24 | class SubscriptionBase; 25 | class PublisherBase; 26 | class ClientBase; 27 | class ServiceBase; 28 | 29 | /** 30 | * @brief Executor class to provide execute interface for subscriber, service server and client in the dispatch layer 31 | */ 32 | class Executor { 33 | public: 34 | /** 35 | * @brief Constructor of Executor 36 | * @param handle Pointer of handle which consists of handler of executor, protocol layer and hardware layer 37 | */ 38 | Executor(std::shared_ptr handle); 39 | ~Executor() = default; 40 | /** 41 | * @brief Get the handle 42 | * @return Pointer of handle 43 | */ 44 | std::shared_ptr GetHandle(); 45 | /** 46 | * @brief Given the subscription, invoke its callback function 47 | * @param subscription Subscription base pointer of certain command 48 | */ 49 | void ExecuteSubscription(const std::shared_ptr& subscription); 50 | /** 51 | * @brief Given the service, invoke its callback function and call the protocol layer to send response/ack 52 | * @param service Service base pointer of certain command 53 | */ 54 | void ExecuteService(const std::shared_ptr& service); 55 | /** 56 | * @brief Given the client, call the protocol layer to send command and wait for the response/ack 57 | * @param client Client base pointer of certain command 58 | */ 59 | void ExecuteClient(const std::shared_ptr& client); 60 | 61 | private: 62 | //! pointer of handle 63 | std::shared_ptr handle_; 64 | }; 65 | } 66 | #endif //ROBORTS_SDK_EXECUTION_H 67 | -------------------------------------------------------------------------------- /roborts_base/roborts_sdk/dispatch/handle.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of  11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | 18 | #include "handle.h" 19 | 20 | namespace roborts_sdk { 21 | Handle::Handle(std::string serial_port) { 22 | serial_port_ = serial_port; 23 | device_ = std::make_shared(serial_port_, 921600); 24 | protocol_ = std::make_shared(device_); 25 | 26 | } 27 | bool Handle::Init(){ 28 | if (!device_->Init()) { 29 | LOG_ERROR<<"Can not open device: " <Init()) { 35 | LOG_ERROR<<"Protocol initialization failed."; 36 | return false; 37 | } 38 | executor_ = std::make_shared(shared_from_this()); 39 | LOG_INFO<<"Initialization of protocol layer and dispatch layer succeeded. "; 40 | return true; 41 | } 42 | std::shared_ptr& Handle::GetProtocol() { 43 | return protocol_; 44 | } 45 | 46 | void Handle::Spin() { 47 | 48 | for (auto sub :subscription_factory_) { 49 | executor_->ExecuteSubscription(sub); 50 | } 51 | for (auto client :client_factory_) { 52 | executor_->ExecuteClient(client); 53 | } 54 | for (auto service :service_factory_) { 55 | executor_->ExecuteService(service); 56 | } 57 | } 58 | } -------------------------------------------------------------------------------- /roborts_base/roborts_sdk/hardware/hardware_interface.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of  11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | 18 | #ifndef ROBORTS_SDK_HARDWARE_INTERFACE_H 19 | #define ROBORTS_SDK_HARDWARE_INTERFACE_H 20 | 21 | namespace roborts_sdk{ 22 | /** 23 | * @brief Abstract class for hardware as an interface 24 | */ 25 | class HardwareInterface{ 26 | public: 27 | HardwareInterface(){}; 28 | virtual ~HardwareInterface() = default; 29 | protected: 30 | virtual bool Init() = 0; 31 | virtual int Read(uint8_t *buf, int len) = 0; 32 | virtual int Write(const uint8_t *buf, int len) = 0; 33 | }; 34 | } 35 | #endif //ROBORTS_SDK_HARDWARE_INTERFACE_H 36 | -------------------------------------------------------------------------------- /roborts_base/roborts_sdk/sdk.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of  11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | 18 | #ifndef ROBORTS_SDK_H 19 | #define ROBORTS_SDK_H 20 | 21 | #include "dispatch/handle.h" 22 | #include "dispatch/execution.h" 23 | #include "dispatch/dispatch.h" 24 | #include "protocol/protocol_define.h" 25 | 26 | #endif //ROBORTS_SDK_H 27 | -------------------------------------------------------------------------------- /roborts_base/roborts_sdk/test/sdk_test.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of  11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | 18 | #include 19 | 20 | #include "../sdk.h" 21 | #include "../protocol/protocol_define.h" 22 | 23 | int main() { 24 | auto h=std::make_shared(); 25 | int count; 26 | 27 | /*-----------Subscriber Test-------------*/ 28 | auto func = [&count] (const std::shared_ptr message) -> void{ 29 | DLOG_INFO<<"chassis_msg_"<position_x_mm); 30 | count++; 31 | }; 32 | auto func2 = [&count] (const std::shared_ptr message) -> void{ 33 | DLOG_INFO<<"chassis_msg_"<error; 34 | count++; 35 | }; 36 | auto func3 = [&count] (const std::shared_ptr message) -> void{ 37 | DLOG_INFO<<"chassis_msg_"<pit_relative_angle; 38 | count++; 39 | }; 40 | auto sub1=h->CreateSubscriber(CHASSIS_CMD_SET,PUSH_CHASSIS_INFO,CHASSIS_ADDRESS,MANIFOLD2_ADDRESS,func); 41 | auto sub2=h->CreateSubscriber(CHASSIS_CMD_SET,PUSH_UWB_INFO,CHASSIS_ADDRESS,MANIFOLD2_ADDRESS,func2); 42 | auto sub3=h->CreateSubscriber(GIMBAL_CMD_SET,PUSH_GIMBAL_INFO,GIMBAL_ADDRESS,BROADCAST_ADDRESS,func3); 43 | 44 | /*-----------Publisher Test-------------*/ 45 | auto pub1 = h->CreatePublisher(CHASSIS_CMD_SET,CTRL_CHASSIS_SPEED,MANIFOLD2_ADDRESS,CHASSIS_ADDRESS); 46 | p_chassis_speed_t chassis_speed; 47 | chassis_speed.rotate_x_offset=0; 48 | chassis_speed.rotate_y_offset=0; 49 | chassis_speed.vx=100; 50 | chassis_speed.vy=0; 51 | chassis_speed.vw=0; 52 | chassis_speed.res=0; 53 | pub1->Publish(chassis_speed); 54 | 55 | /*-----------Client Test-------------*/ 56 | auto client1=h->CreateClient(CHASSIS_CMD_SET,SET_CHASSIS_MODE,MANIFOLD2_ADDRESS,CHASSIS_ADDRESS); 57 | auto mode = std::make_shared(SEPARATE_GIMBAL); 58 | 59 | client1->AsyncSendRequest(mode,[](Client::SharedFuture){ 60 | std::cout<<"get!"<Spin(); 65 | usleep(10); 66 | } 67 | 68 | 69 | } -------------------------------------------------------------------------------- /roborts_base/roborts_sdk/utilities/log.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of  11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | 18 | #ifndef ROBORTS_SDK_LOG_H 19 | #define ROBORTS_SDK_LOG_H 20 | #include 21 | #include 22 | #include "glog/logging.h" 23 | #include "glog/raw_logging.h" 24 | 25 | 26 | #define LOG_INFO LOG(INFO) 27 | #define LOG_WARNING LOG(WARNING) 28 | #define LOG_ERROR LOG(ERROR) 29 | #define LOG_FATAL LOG(FATAL) 30 | 31 | #define LOG_INFO_IF(condition) LOG_IF(INFO,condition) 32 | #define LOG_WARNING_IF(condition) LOG_IF(WARNING,condition) 33 | #define LOG_ERROR_IF(condition) LOG_IF(ERROR,condition) 34 | #define LOG_FATAL_IF(condition) LOG_IF(FATAL,condition) 35 | 36 | #define LOG_INFO_EVERY(freq) LOG_EVERY_N(INFO, freq) 37 | #define LOG_WARNING_EVERY(freq) LOG_EVERY_N(WARNING, freq) 38 | #define LOG_ERROR_EVERY(freq) LOG_EVERY_N(ERROR, freq) 39 | 40 | #define DLOG_INFO DLOG(INFO) 41 | #define DLOG_WARNING DLOG(WARNING) 42 | #define DLOG_ERROR DLOG(WARNING) 43 | 44 | #define LOG_WARNING_FIRST_N(times) LOG_FIRST_N(WARNING, times) 45 | 46 | 47 | class GLogWrapper { 48 | public: 49 | GLogWrapper(char* program) { 50 | google::InitGoogleLogging(program); 51 | FLAGS_stderrthreshold=google::WARNING; 52 | FLAGS_colorlogtostderr=true; 53 | FLAGS_v = 3; 54 | google::InstallFailureSignalHandler(); 55 | } 56 | 57 | ~GLogWrapper() 58 | { 59 | google::ShutdownGoogleLogging(); 60 | } 61 | }; 62 | 63 | 64 | #endif //ROBORTS_SDK_LOG_H 65 | -------------------------------------------------------------------------------- /roborts_base/ros_dep.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of  11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | 18 | #ifndef ROBORTS_SDK_PROTOCOL_DEFINE_ROS_H 19 | #define ROBORTS_SDK_PROTOCOL_DEFINE_ROS_H 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | //Chassis 28 | #include "roborts_msgs/TwistAccel.h" 29 | 30 | //Gimbal 31 | #include "roborts_msgs/GimbalAngle.h" 32 | #include "roborts_msgs/GimbalRate.h" 33 | #include "roborts_msgs/GimbalMode.h" 34 | #include "roborts_msgs/ShootCmd.h" 35 | #include "roborts_msgs/FricWhl.h" 36 | 37 | //Referee System 38 | #include "roborts_msgs/BonusStatus.h" 39 | #include "roborts_msgs/GameResult.h" 40 | #include "roborts_msgs/GameStatus.h" 41 | #include "roborts_msgs/GameSurvivor.h" 42 | #include "roborts_msgs/ProjectileSupply.h" 43 | #include "roborts_msgs/RobotBonus.h" 44 | #include "roborts_msgs/RobotDamage.h" 45 | #include "roborts_msgs/RobotHeat.h" 46 | #include "roborts_msgs/RobotShoot.h" 47 | #include "roborts_msgs/RobotStatus.h" 48 | #include "roborts_msgs/SupplierStatus.h" 49 | 50 | #endif //ROBORTS_SDK_PROTOCOL_DEFINE_ROS_H 51 | -------------------------------------------------------------------------------- /roborts_bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(roborts_bringup) 2 | cmake_minimum_required(VERSION 3.1) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake_module) 6 | set(CMAKE_BUILD_TYPE Release) 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | roborts_base 10 | roborts_common 11 | roborts_localization 12 | roborts_costmap 13 | roborts_msgs 14 | roborts_planning 15 | ) 16 | 17 | catkin_package() 18 | 19 | 20 | -------------------------------------------------------------------------------- /roborts_bringup/launch/base.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /roborts_bringup/launch/mapping.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /roborts_bringup/launch/mapping_stage.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /roborts_bringup/launch/roborts.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /roborts_bringup/launch/roborts_stage.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /roborts_bringup/launch/slam_gmapping.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /roborts_bringup/launch/static_tf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 9 | 10 | -------------------------------------------------------------------------------- /roborts_bringup/maps/icra2018.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoboMaster/RoboRTS/b6b8f811781b00caf956303f4b63a2d5af518fcf/roborts_bringup/maps/icra2018.pgm -------------------------------------------------------------------------------- /roborts_bringup/maps/icra2018.yaml: -------------------------------------------------------------------------------- 1 | image: icra2018.pgm 2 | resolution: 0.05 3 | origin: [0, 0, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /roborts_bringup/maps/icra2019.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoboMaster/RoboRTS/b6b8f811781b00caf956303f4b63a2d5af518fcf/roborts_bringup/maps/icra2019.pgm -------------------------------------------------------------------------------- /roborts_bringup/maps/icra2019.yaml: -------------------------------------------------------------------------------- 1 | image: icra2019.pgm 2 | resolution: 0.05 3 | origin: [0, 0, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /roborts_bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | roborts_bringup 3 | 1.0.0 4 | 5 | The roborts_bringup package provides quick bringup for RoboMaster AI Robot 6 | 7 | RoboMaster 8 | RoboMaster 9 | GPL 3.0 10 | https://github.com/RoboMaster/RoboRTS 11 | 12 | catkin 13 | 14 | roborts_base 15 | roborts_common 16 | roborts_localization 17 | roborts_costmap 18 | roborts_msgs 19 | roborts_planning 20 | roborts_tf 21 | 22 | roborts_base 23 | roborts_common 24 | roborts_localization 25 | roborts_costmap 26 | roborts_msgs 27 | roborts_tf 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /roborts_bringup/scripts/udev/create_udev_rules.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | echo "Remap the device port(ttyUSBX) to alias" 3 | echo "USB connection for serial" 4 | 5 | echo "Check them using the command : ls -l /dev|grep ttyUSB" 6 | echo "Start to copy udev rules to /etc/udev/rules.d/" 7 | 8 | sudo cp `rospack find roborts_bringup`/scripts/udev/roborts.rules /etc/udev/rules.d 9 | echo " " 10 | echo "Restarting udev" 11 | echo "" 12 | sudo service udev reload 13 | sudo service udev restart 14 | echo "Finish " 15 | -------------------------------------------------------------------------------- /roborts_bringup/scripts/udev/delete_udev_rules.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | echo "Delete remap the device port(ttyUSBX) to alias" 3 | echo "USB connection for serial" 4 | 5 | echo "Delete udev in the /etc/udev/rules.d/" 6 | sudo rm /etc/udev/rules.d/roborts.rules 7 | echo " " 8 | echo "Restarting udev" 9 | echo "" 10 | sudo service udev reload 11 | sudo service udev restart 12 | echo "Finish" 13 | -------------------------------------------------------------------------------- /roborts_bringup/scripts/udev/roborts.rules: -------------------------------------------------------------------------------- 1 | # set the udev rule 2 | KERNEL=="ttyACM*", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", MODE:="0777", SYMLINK+="serial_sdk" 3 | KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0777", SYMLINK+="rplidar" 4 | -------------------------------------------------------------------------------- /roborts_bringup/scripts/upstart/create_upstart_service.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | echo " " 3 | echo "Start to copy script files to /home/dji" 4 | echo "" 5 | cp `rospack find roborts_bringup`/scripts/upstart/roborts-start.sh /home/dji 6 | cp `rospack find roborts_bringup`/scripts/upstart/max_performance.sh /home/dji 7 | echo " " 8 | echo "Start to copy service files to /lib/systemd/system/" 9 | echo "" 10 | sudo cp `rospack find roborts_bringup`/scripts/upstart/max-performance.service /lib/systemd/system/ 11 | sudo cp `rospack find roborts_bringup`/scripts/upstart/roborts.service /lib/systemd/system/ 12 | echo " " 13 | echo "Enable max-performance and roborts service for upstart! " 14 | echo "" 15 | sudo systemctl enable max-performance.service 16 | sudo systemctl enable roborts.service 17 | echo "Finish " 18 | -------------------------------------------------------------------------------- /roborts_bringup/scripts/upstart/delete_upstart_service.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | echo " " 3 | echo "Start to remove script files from /home/dji" 4 | echo "" 5 | rm /home/dji/roborts-start.sh 6 | rm /home/dji/max_performance.sh 7 | echo " " 8 | echo "Start to remove service files from /lib/systemd/system/" 9 | echo "" 10 | sudo rm /lib/systemd/system/max-performance.service 11 | sudo rm /lib/systemd/system/roborts.service 12 | echo " " 13 | echo "Disable max-performance and roborts service for upstart! " 14 | echo "" 15 | sudo systemctl disable max-performance.service 16 | sudo systemctl disable roborts.service 17 | echo "Finish" 18 | -------------------------------------------------------------------------------- /roborts_bringup/scripts/upstart/max-performance.service: -------------------------------------------------------------------------------- 1 | [Unit] 2 | Description=Turn up performance 3 | After=multi-user.target 4 | [Service] 5 | Type=oneshot 6 | ExecStart=/bin/sh /home/dji/max_performance.sh 7 | 8 | [Install] 9 | WantedBy=multi-user.target 10 | -------------------------------------------------------------------------------- /roborts_bringup/scripts/upstart/max_performance.sh: -------------------------------------------------------------------------------- 1 | #/usr/bash 2 | sleep 10 3 | echo dji | sudo -S sudo "/home/dji/jetson_clocks.sh" 4 | echo "Max performance" 5 | # disable wifi power saving 6 | 7 | echo dji | sudo -S sudo iw dev wlan0 set power_save off 8 | echo "Set power_save off " 9 | -------------------------------------------------------------------------------- /roborts_bringup/scripts/upstart/roborts-start.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | bash -c "source /home/dji/.bashrc && roslaunch roborts_bringup roborts.launch" 3 | -------------------------------------------------------------------------------- /roborts_bringup/scripts/upstart/roborts.service: -------------------------------------------------------------------------------- 1 | [Unit] 2 | Description=roborts startup 3 | After=network-online.target 4 | [Service] 5 | User=root 6 | Type=simple 7 | Restart=on-failure 8 | ExecStart=/home/dji/roborts-start.sh 9 | 10 | [Install] 11 | WantedBy=multi-user.target 12 | -------------------------------------------------------------------------------- /roborts_bringup/worlds/icra2018.world: -------------------------------------------------------------------------------- 1 | define block model 2 | ( 3 | size [0.4 0.4 1.2] 4 | ranger_return 1 5 | ) 6 | 7 | define frontcamera camera 8 | ( 9 | size [ 0.050 0.050 0.0500 ] 10 | range [ 0.301 8.0 ] 11 | resolution [ 640 480 ] 12 | fov [ 120 40 ] 13 | pantilt [ 0 0 ] 14 | alwayson 1 15 | ) 16 | 17 | define rplidar ranger 18 | ( 19 | sensor( 20 | range_max 8.0 21 | fov 360 22 | samples 360 23 | ) 24 | # generic model properties 25 | color "black" 26 | size [ 0.050 0.050 0.100 ] 27 | ) 28 | 29 | define rmcar position 30 | ( 31 | size [0.6 0.45 0.460] 32 | origin [0 0 0 0] 33 | gui_nose 1 34 | drive "omni" 35 | # frontcamera(pose [ 0 0 0 0 ]) 36 | rplidar(pose [ 0 0 0 0 ]) 37 | odom_error [0.03 0.03 0.00 0.05] 38 | # [ xmin xmax ymin ymax zmin zmax amin amax ] 39 | velocity_bounds [-2 2 -2 2 -2 2 -90 90 ] 40 | acceleration_bounds [-2 2 -2 2 -2 2 -90 90] 41 | ranger_return 1 42 | ) 43 | 44 | define floorplan model 45 | ( 46 | # sombre, sensible, artistic 47 | color "gray30" 48 | 49 | # most maps will need a bounding box 50 | boundary 0 51 | 52 | gui_nose 0 53 | gui_grid 0 54 | 55 | gui_outline 0 56 | gripper_return 0 57 | fiducial_return 0 58 | ranger_return 1 59 | ) 60 | 61 | # set the resolution of the underlying raytrace model in meters 62 | resolution 0.01 63 | 64 | interval_sim 50#83 # simulation timestep in milliseconds 65 | interval_real 50#83 66 | 67 | window 68 | ( 69 | size [ 745.000 448.000 ] 70 | rotate [ 0 0 ] 71 | scale 29 72 | ) 73 | 74 | # load an environment bitmap 75 | floorplan 76 | ( 77 | name "RoboMaster Map" 78 | bitmap "../maps/icra2018.pgm" 79 | size [8.35 5.5 1.000] 80 | pose [4.175 2.775 0 0 ] 81 | ) 82 | 83 | # throw in a robot 84 | rmcar(pose [ 1 1 0 0 ] name "rmcar0" color "blue" ) 85 | # throw in a block for test 86 | block(pose [ 7.79 3.45 0 0.3 ] color "red" ) 87 | -------------------------------------------------------------------------------- /roborts_bringup/worlds/icra2019.world: -------------------------------------------------------------------------------- 1 | define block model 2 | ( 3 | size [0.4 0.4 1.2] 4 | ranger_return 1 5 | ) 6 | 7 | define frontcamera camera 8 | ( 9 | size [ 0.050 0.050 0.0500 ] 10 | range [ 0.301 8.0 ] 11 | resolution [ 640 480 ] 12 | fov [ 120 40 ] 13 | pantilt [ 0 0 ] 14 | alwayson 1 15 | ) 16 | 17 | define rplidar ranger 18 | ( 19 | sensor( 20 | range_max 8.0 21 | fov 270 22 | samples 270 23 | ) 24 | # generic model properties 25 | color "black" 26 | size [ 0.050 0.050 0.100 ] 27 | ) 28 | 29 | define rmcar position 30 | ( 31 | size [0.6 0.45 0.460] 32 | origin [0 0 0 0] 33 | gui_nose 1 34 | drive "omni" 35 | # frontcamera(pose [ 0 0 0 0 ]) 36 | rplidar(pose [ 0 0 0 0 ]) 37 | odom_error [0.03 0.03 0.00 0.05] 38 | # [ xmin xmax ymin ymax zmin zmax amin amax ] 39 | velocity_bounds [-2 2 -2 2 -2 2 -90 90 ] 40 | acceleration_bounds [-2 2 -2 2 -2 2 -90 90] 41 | ranger_return 1 42 | ) 43 | 44 | define floorplan model 45 | ( 46 | # sombre, sensible, artistic 47 | color "gray30" 48 | 49 | # most maps will need a bounding box 50 | boundary 0 51 | 52 | gui_nose 0 53 | gui_grid 0 54 | 55 | gui_outline 0 56 | gripper_return 0 57 | fiducial_return 0 58 | ranger_return 1 59 | ) 60 | 61 | # set the resolution of the underlying raytrace model in meters 62 | resolution 0.01 63 | 64 | interval_sim 50#83 # simulation timestep in milliseconds 65 | interval_real 50#83 66 | 67 | window 68 | ( 69 | size [ 745.000 448.000 ] 70 | rotate [ 0 0 ] 71 | scale 29 72 | ) 73 | 74 | # load an environment bitmap 75 | floorplan 76 | ( 77 | name "RoboMaster Map" 78 | bitmap "../maps/icra2019.pgm" 79 | size [8.15 5.15 1.000] 80 | pose [4.075 2.575 0 0 ] 81 | ) 82 | 83 | # throw in a robot 84 | rmcar(pose [ 1 1 0 0 ] name "rmcar0" color "blue" ) 85 | # throw in a block for test 86 | block(pose [ 7.79 3.45 0 0.3 ] color "red" ) 87 | -------------------------------------------------------------------------------- /roborts_camera/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(roborts_camera) 2 | 3 | set(CMAKE_CXX_STANDARD 14) 4 | set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake_module) 5 | set(CMAKE_BUILD_TYPE Release) 6 | add_definitions(-w -Wno-dev) 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | roscpp 10 | tf 11 | actionlib 12 | roborts_common 13 | cv_bridge 14 | image_transport 15 | ) 16 | 17 | find_package(ProtoBuf REQUIRED) 18 | find_package(OpenCV 3 REQUIRED) 19 | 20 | catkin_package( 21 | INCLUDE_DIRS 22 | ) 23 | 24 | add_subdirectory(uvc) 25 | 26 | #camera parameter 27 | file(GLOB CameraParamProtoFiles "${CMAKE_CURRENT_SOURCE_DIR}/proto/camera_param.proto") 28 | rrts_protobuf_generate_cpp(${CMAKE_CURRENT_SOURCE_DIR}/proto 29 | CameraParamProtoSrc 30 | CameraParamProtoHds 31 | ${CameraParamProtoFiles} 32 | ) 33 | 34 | #camera param 35 | add_library(roborts_camera_param 36 | SHARED 37 | ${CameraParamProtoSrc} 38 | ${CameraParamProtoHds} 39 | camera_param.cpp 40 | ) 41 | 42 | target_link_libraries(roborts_camera_param 43 | PUBLIC 44 | ${catkin_LIBRARIES} 45 | ${OpenCV_LIBRARIES} 46 | ${PROTOBUF_LIBRARIES} 47 | ) 48 | 49 | target_include_directories(roborts_camera_param 50 | PUBLIC 51 | ${catkin_INCLUDE_DIRS} 52 | ) 53 | 54 | #camera_node 55 | add_executable(${PROJECT_NAME}_node 56 | camera_node.cpp 57 | ) 58 | 59 | target_link_libraries(${PROJECT_NAME}_node 60 | PRIVATE 61 | driver::uvc_driver 62 | roborts_camera_param 63 | ${catkin_LIBRARIES} 64 | ${OpenCV_LIBRARIES} 65 | ) 66 | 67 | target_include_directories(${PROJECT_NAME}_node 68 | PRIVATE 69 | ${OpenCV_INCLUDE_DIRECTORIES} 70 | ${catkin_INCLUDE_DIRS} 71 | ) 72 | 73 | #cap_img 74 | 75 | add_executable(image_capture_test 76 | test/image_capture.cpp 77 | ) 78 | 79 | target_link_libraries(image_capture_test 80 | PRIVATE 81 | driver::uvc_driver 82 | roborts_camera_param 83 | ${catkin_LIBRARIES} 84 | ${OpenCV_LIBRARIES} 85 | ) 86 | 87 | target_include_directories(image_capture_test 88 | PRIVATE 89 | ${OpenCV_INCLUDE_DIRECTORIES} 90 | ) 91 | -------------------------------------------------------------------------------- /roborts_camera/camera_base.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of  11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | 18 | #ifndef ROBORTS_CAMERA_CAMERA_BASE_H 19 | #define ROBORTS_CAMERA_CAMERA_BASE_H 20 | 21 | #include 22 | #include 23 | 24 | #include "state/error_code.h" 25 | 26 | namespace roborts_camera 27 | { 28 | /** 29 | * @brief Camera base class for the camera factory 30 | */ 31 | class CameraBase { 32 | public: 33 | /** 34 | * @brief Constructor of CameraBase 35 | * @param camera_info Information and parameters of camera 36 | */ 37 | explicit CameraBase(CameraInfo camera_info):camera_info_(camera_info),camera_initialized_(false){}; 38 | virtual ~CameraBase() = default; 39 | /** 40 | * @brief Start to read camera 41 | * @param img Image data in form of cv::Mat to be read 42 | */ 43 | virtual void StartReadCamera(cv::Mat &img) = 0; 44 | 45 | protected: 46 | //! flag for camera initialization 47 | bool camera_initialized_; 48 | //! information and parameters of camera 49 | CameraInfo camera_info_; 50 | }; 51 | } //namespace roborts_camera 52 | 53 | 54 | #endif //ROBORTS_CAMERA_CAMERA_BASE_H -------------------------------------------------------------------------------- /roborts_camera/camera_node.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of  11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | 18 | #include 19 | 20 | #include "camera_node.h" 21 | 22 | namespace roborts_camera{ 23 | CameraNode::CameraNode() { 24 | camera_num_ = camera_param_.GetCameraParam().size(); 25 | img_pubs_.resize(camera_num_); 26 | camera_threads_.resize(camera_num_); 27 | camera_driver_.resize(camera_num_); 28 | 29 | for (unsigned int i = 0; i < camera_num_; i++) { 30 | auto camera_info = camera_param_.GetCameraParam()[i]; 31 | nhs_.push_back(ros::NodeHandle(camera_info.camera_name)); 32 | image_transport::ImageTransport it(nhs_.at(i)); 33 | img_pubs_[i] = it.advertiseCamera("image_raw", 1, true); 34 | //create the selected camera driver 35 | camera_driver_[i] = roborts_common::AlgorithmFactory::CreateAlgorithm(camera_info.camera_type,camera_info); 36 | } 37 | 38 | StartThread(); 39 | } 40 | 41 | void CameraNode::StartThread() { 42 | running_ = true; 43 | for (unsigned int i = 0; i < camera_num_; i++) { 44 | camera_threads_[i] = std::thread(&CameraNode::Update, this, i); 45 | } 46 | } 47 | 48 | void CameraNode::Update(const unsigned int index) { 49 | cv::Mat img; 50 | bool camera_info_send = false; 51 | while(running_) { 52 | camera_driver_[index]->StartReadCamera(img); 53 | if(!img.empty()) { 54 | sensor_msgs::ImagePtr img_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", img).toImageMsg(); 55 | img_msg->header.frame_id = camera_param_.GetCameraParam()[index].camera_name; 56 | img_msg->header.stamp = ros::Time::now(); 57 | 58 | camera_param_.GetCameraParam()[index].ros_camera_info->header.stamp = img_msg->header.stamp; 59 | img_pubs_[index].publish(img_msg, camera_param_.GetCameraParam()[index].ros_camera_info); 60 | } 61 | } 62 | } 63 | 64 | void CameraNode::StoptThread() { 65 | //TODO: To be implemented 66 | } 67 | 68 | CameraNode::~CameraNode() { 69 | running_ = false; 70 | for (auto &iter: camera_threads_) { 71 | if (iter.joinable()) 72 | iter.join(); 73 | } 74 | } 75 | } //namespace roborts_camera 76 | 77 | void SignalHandler(int signal){ 78 | if(ros::isInitialized() && ros::isStarted() && ros::ok() && !ros::isShuttingDown()){ 79 | ros::shutdown(); 80 | } 81 | } 82 | 83 | int main(int argc, char **argv){ 84 | signal(SIGINT, SignalHandler); 85 | signal(SIGTERM,SignalHandler); 86 | ros::init(argc, argv, "roborts_camera_node", ros::init_options::NoSigintHandler); 87 | roborts_camera::CameraNode camera_test; 88 | ros::AsyncSpinner async_spinner(1); 89 | async_spinner.start(); 90 | ros::waitForShutdown(); 91 | return 0; 92 | } 93 | -------------------------------------------------------------------------------- /roborts_camera/camera_node.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of  11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | 18 | #ifndef ROBORTS_CAMERA_CAMERA_NODE_H 19 | #define ROBORTS_CAMERA_CAMERA_NODE_H 20 | 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include "uvc/uvc_driver.h" 30 | 31 | #include "camera_param.h" 32 | #include "camera_base.h" 33 | #include "alg_factory/algorithm_factory.h" 34 | #include "io/io.h" 35 | 36 | namespace roborts_camera{ 37 | 38 | class CameraNode{ 39 | public: 40 | /** 41 | * @brief Construcor of camera node 42 | * @details Read the camera parameter, create the camera drivers based on camera factory 43 | * and start all camera threads for update images. 44 | */ 45 | explicit CameraNode(); 46 | /** 47 | * @brief Start all the threads for camera drivers to update and publish the image data 48 | */ 49 | void StartThread(); 50 | /** 51 | * @brief Invoke each camera driver to read the image data and publish using ROS image_transport with corresponding parameter 52 | */ 53 | void Update(const unsigned int camera_num_); 54 | /** 55 | * @brief Stop to read image. 56 | */ 57 | void StoptThread(); 58 | ~CameraNode(); 59 | private: 60 | //! drivers of different cameras inherited from camera base 61 | std::vector> camera_driver_; 62 | //! camera parameters 63 | CameraParam camera_param_; 64 | //! number of cameras 65 | unsigned long camera_num_; 66 | //! flag of running 67 | bool running_; 68 | //! threads of different cameras 69 | std::vector camera_threads_; 70 | 71 | //! ROS node handlers of different cameras 72 | std::vector nhs_; 73 | //! ROS image transport camera publisher to publish image data 74 | std::vector img_pubs_; 75 | 76 | }; 77 | } //namespace roborts_camera 78 | 79 | #endif //ROBORTS_CAMERA_CAMERA_NODE_H 80 | -------------------------------------------------------------------------------- /roborts_camera/config/camera_param.prototxt: -------------------------------------------------------------------------------- 1 | camera: { 2 | camera_name: "back_camera" 3 | camera_type: "uvc" 4 | camera_path: "/dev/video0" 5 | 6 | camera_matrix { 7 | data: 839.923052 8 | data: 0.0 9 | data: 340.780730 10 | data: 0.0 11 | data: 837.671081 12 | data: 261.766523 13 | data: 0.0 14 | data: 0.0 15 | data: 1.0 16 | } 17 | 18 | camera_distortion { 19 | data: 0.082613 20 | data: 0.043275 21 | data: 0.002486 22 | data: -0.000823 23 | data: 0.0 24 | } 25 | 26 | resolution { 27 | width: 640 28 | height: 360 29 | 30 | width_offset: 0 31 | height_offset: 0 32 | } 33 | 34 | fps: 100 35 | auto_exposure: 1 36 | exposure_value: 70 37 | exposure_time: 10 38 | auto_white_balance: 1 39 | auto_gain: 0 40 | } 41 | 42 | -------------------------------------------------------------------------------- /roborts_camera/package.xml: -------------------------------------------------------------------------------- 1 | 2 | roborts_camera 3 | 1.0.0 4 | 5 | The roborts camera package provides camera interface for RoboMaster AI Robot 6 | 7 | RoboMaster 8 | RoboMaster 9 | GPL 3.0 10 | https://github.com/RoboMaster/RoboRTS 11 | 12 | catkin 13 | 14 | actionlib 15 | roscpp 16 | rospy 17 | roborts_common 18 | 19 | actionlib 20 | roscpp 21 | rospy 22 | roborts_common 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /roborts_camera/proto/camera_param.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto2"; 2 | package roborts_camera; 3 | 4 | message CameraMatrix { 5 | repeated double data = 1; 6 | } 7 | 8 | message CameraDistortion { 9 | repeated double data = 1; 10 | } 11 | 12 | message Resolution { 13 | required uint32 width = 1; 14 | required uint32 height = 2; 15 | 16 | required uint32 width_offset = 3; 17 | required uint32 height_offset = 4; 18 | } 19 | 20 | message Camera { 21 | required string camera_name = 1; 22 | required string camera_type = 2; 23 | required string camera_path = 3; 24 | required CameraMatrix camera_matrix = 4; 25 | required CameraDistortion camera_distortion = 5; 26 | 27 | optional uint32 fps = 6; 28 | optional Resolution resolution = 7; 29 | 30 | optional bool auto_exposure = 8; //1 open, 0 close 31 | optional uint32 exposure_value = 9; 32 | optional uint32 exposure_time = 10;//us 33 | 34 | optional bool auto_white_balance = 11; //1 open, 0 close 35 | 36 | optional bool auto_gain = 12;////1 open, 0 close 37 | 38 | optional uint32 contrast = 13; 39 | } 40 | 41 | message Cameras { 42 | repeated Camera camera = 1; 43 | } 44 | -------------------------------------------------------------------------------- /roborts_camera/test/image_capture.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | 18 | #include 19 | #include 20 | #include 21 | //opencv 22 | #include "opencv2/opencv.hpp" 23 | //ros 24 | #include "image_transport/image_transport.h" 25 | #include "cv_bridge/cv_bridge.h" 26 | 27 | std::string topic_name = "back_camera"; 28 | cv::VideoWriter writer(topic_name+".avi", CV_FOURCC('M', 'J', 'P', 'G'), 25.0, cv::Size(640, 360)); 29 | cv::Mat src_img; 30 | 31 | void ReceiveImg(const sensor_msgs::ImageConstPtr &msg) { 32 | src_img = cv_bridge::toCvShare(msg, "bgr8")->image.clone(); 33 | writer.write(src_img); 34 | } 35 | 36 | int main(int argc, char **argv) { 37 | 38 | ros::init(argc, argv, "image_capture"); 39 | ros::NodeHandle nh; 40 | 41 | image_transport::ImageTransport it(nh); 42 | 43 | image_transport::Subscriber sub = it.subscribe(topic_name, 20, boost::bind(&ReceiveImg, _1)); 44 | 45 | ros::AsyncSpinner async_spinner(1); 46 | async_spinner.start(); 47 | ros::waitForShutdown(); 48 | return 0; 49 | } 50 | -------------------------------------------------------------------------------- /roborts_camera/uvc/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(uvc_driver) 2 | 3 | #uvc_driver 4 | add_library(uvc_driver 5 | SHARED 6 | uvc_driver.cpp 7 | ) 8 | 9 | target_link_libraries(uvc_driver 10 | PRIVATE 11 | ${catkin_LIBRARIES} 12 | ${OpenCV_LIBRARIES} 13 | ) 14 | 15 | target_include_directories(uvc_driver 16 | PRIVATE 17 | ${catkin_INCLUDE_DIRS} 18 | ${OpenCV_INCLUDE_DIRECTORIES} 19 | ) 20 | 21 | add_library(driver::uvc_driver ALIAS uvc_driver) 22 | -------------------------------------------------------------------------------- /roborts_camera/uvc/uvc_driver.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of  11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | 24 | #include "uvc_driver.h" 25 | 26 | namespace roborts_camera { 27 | UVCDriver::UVCDriver(CameraInfo camera_info): 28 | CameraBase(camera_info){ 29 | } 30 | 31 | void UVCDriver::StartReadCamera(cv::Mat &img) { 32 | if(!camera_initialized_){ 33 | camera_info_.cap_handle.open(camera_info_.camera_path); 34 | SetCameraExposure(camera_info_.camera_path, camera_info_.exposure_value); 35 | ROS_ASSERT_MSG(camera_info_.cap_handle.isOpened(), "Cannot open %s .", cameras_[camera_num].video_path.c_str()); 36 | camera_initialized_ = true; 37 | } 38 | else { 39 | camera_info_.cap_handle >> img; 40 | } 41 | } 42 | 43 | void UVCDriver::StopReadCamera() { 44 | //TODO: To be implemented 45 | } 46 | 47 | void UVCDriver::SetCameraExposure(std::string id, int val) 48 | { 49 | int cam_fd; 50 | if ((cam_fd = open(id.c_str(), O_RDWR)) == -1) { 51 | std::cerr << "Camera open error" << std::endl; 52 | } 53 | 54 | struct v4l2_control control_s; 55 | control_s.id = V4L2_CID_AUTO_WHITE_BALANCE; 56 | control_s.value = camera_info_.auto_white_balance; 57 | ioctl(cam_fd, VIDIOC_S_CTRL, &control_s); 58 | 59 | control_s.id = V4L2_CID_EXPOSURE_AUTO; 60 | control_s.value = V4L2_EXPOSURE_MANUAL; 61 | ioctl(cam_fd, VIDIOC_S_CTRL, &control_s); 62 | 63 | // Set exposure value 64 | control_s.id = V4L2_CID_EXPOSURE_ABSOLUTE; 65 | control_s.value = val; 66 | ioctl(cam_fd, VIDIOC_S_CTRL, &control_s); 67 | close(cam_fd); 68 | } 69 | 70 | UVCDriver::~UVCDriver() { 71 | } 72 | 73 | } //namespace roborts_camera 74 | -------------------------------------------------------------------------------- /roborts_camera/uvc/uvc_driver.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of  11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | 18 | #ifndef ROBORTS_CAMERA_UVC_DRIVER_H 19 | #define ROBORTS_CAMERA_UVC_DRIVER_H 20 | 21 | #include 22 | 23 | #include "ros/ros.h" 24 | #include "opencv2/opencv.hpp" 25 | #include "actionlib/server/simple_action_server.h" 26 | 27 | #include "../camera_param.h" 28 | #include "../camera_base.h" 29 | #include "alg_factory/algorithm_factory.h" 30 | #include "io/io.h" 31 | 32 | namespace roborts_camera { 33 | /** 34 | * @brief UVC Camera class, product of the camera factory inherited from CameraBase 35 | */ 36 | class UVCDriver: public CameraBase { 37 | public: 38 | /** 39 | * @brief Constructor of UVCDriver 40 | * @param camera_info Information and parameters of camera 41 | */ 42 | explicit UVCDriver(CameraInfo camera_info); 43 | /** 44 | * @brief Start to read uvc camera 45 | * @param img Image data in form of cv::Mat to be read 46 | */ 47 | void StartReadCamera(cv::Mat &img) override; 48 | /** 49 | * @brief Stop to read uvc camera 50 | */ 51 | void StopReadCamera(); 52 | ~UVCDriver() override; 53 | private: 54 | /** 55 | * @brief Set camera exposure 56 | * @param id Camera path 57 | * @param val Camera exposure value 58 | */ 59 | void SetCameraExposure(std::string id, int val); 60 | //! Initialization of camera read 61 | bool read_camera_initialized_; 62 | }; 63 | 64 | roborts_common::REGISTER_ALGORITHM(CameraBase, "uvc", UVCDriver, CameraInfo); 65 | 66 | } //namespace roborts_camera 67 | #endif //ROBORTS_CAMERA_UVC_DRIVER_H 68 | -------------------------------------------------------------------------------- /roborts_common/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(roborts_common) 2 | cmake_minimum_required(VERSION 3.1) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake_module) 6 | set(CMAKE_BUILD_TYPE Release) 7 | 8 | 9 | find_package(catkin REQUIRED COMPONENTS 10 | roscpp 11 | roborts_msgs 12 | actionlib 13 | ) 14 | 15 | find_package(Boost REQUIRED) 16 | find_package(Eigen3 REQUIRED) 17 | 18 | catkin_package( 19 | INCLUDE_DIRS 20 | include 21 | ) 22 | include_directories(include) 23 | 24 | install(DIRECTORY include/${PROJECT_NAME}/ 25 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 26 | FILES_MATCHING PATTERN "*.h" 27 | PATTERN ".svn" EXCLUDE 28 | ) 29 | 30 | -------------------------------------------------------------------------------- /roborts_common/cmake_module/FindGTEST.cmake: -------------------------------------------------------------------------------- 1 | find_path(GTEST_INCLUDE_DIR gtest.h /usr/include/gtest) 2 | find_library(GTEST_LIBRARY NAMES gtest PATH /usr/lib) 3 | if(GTEST_INCLUDE_DIR AND GTEST_LIBRARY) 4 | set(GTEST_FOUND TRUE) 5 | endif(GTEST_INCLUDE_DIR AND GTEST_LIBRARY) 6 | if(GTEST_FOUND) 7 | if(NOT GTEST_FIND_QUIETLY) 8 | message(STATUS "Found GTEST:" ${GTEST_LIBRARY}) 9 | endif(NOT GTEST_FIND_QUIETLY) 10 | endif(GTEST_FOUND) 11 | -------------------------------------------------------------------------------- /roborts_common/cmake_module/FindGlog.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Glog 2 | # 3 | # The following variables are optionally searched for defaults 4 | # GLOG_ROOT_DIR: Base directory where all GLOG components are found 5 | # 6 | # The following are set after configuration is done: 7 | # GLOG_FOUND 8 | # GLOG_INCLUDE_DIRS 9 | # GLOG_LIBRARIES 10 | # GLOG_LIBRARYRARY_DIRS 11 | 12 | include(FindPackageHandleStandardArgs) 13 | 14 | set(GLOG_ROOT_DIR "" CACHE PATH "Folder contains Google glog") 15 | 16 | if(WIN32) 17 | find_path(GLOG_INCLUDE_DIR glog/logging.h 18 | PATHS ${GLOG_ROOT_DIR}/src/windows) 19 | else() 20 | find_path(GLOG_INCLUDE_DIR glog/logging.h 21 | PATHS ${GLOG_ROOT_DIR}) 22 | endif() 23 | 24 | if(MSVC) 25 | find_library(GLOG_LIBRARY_RELEASE libglog_static 26 | PATHS ${GLOG_ROOT_DIR} 27 | PATH_SUFFIXES Release) 28 | 29 | find_library(GLOG_LIBRARY_DEBUG libglog_static 30 | PATHS ${GLOG_ROOT_DIR} 31 | PATH_SUFFIXES Debug) 32 | 33 | set(GLOG_LIBRARY optimized ${GLOG_LIBRARY_RELEASE} debug ${GLOG_LIBRARY_DEBUG}) 34 | else() 35 | find_library(GLOG_LIBRARY glog 36 | PATHS ${GLOG_ROOT_DIR} 37 | PATH_SUFFIXES lib lib64) 38 | endif() 39 | 40 | find_package_handle_standard_args(Glog DEFAULT_MSG GLOG_INCLUDE_DIR GLOG_LIBRARY) 41 | 42 | if(GLOG_FOUND) 43 | set(GLOG_INCLUDE_DIRS ${GLOG_INCLUDE_DIR}) 44 | set(GLOG_LIBRARIES ${GLOG_LIBRARY}) 45 | message(STATUS "Found glog (include: ${GLOG_INCLUDE_DIR}, library: ${GLOG_LIBRARY})") 46 | mark_as_advanced(GLOG_ROOT_DIR GLOG_LIBRARY_RELEASE GLOG_LIBRARY_DEBUG 47 | GLOG_LIBRARY GLOG_INCLUDE_DIR) 48 | endif() 49 | -------------------------------------------------------------------------------- /roborts_common/include/alg_factory/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1) 2 | project(alg_factory) 3 | 4 | #common::io 5 | add_library(alg_factory INTERFACE) 6 | 7 | target_include_directories(alg_factory INTERFACE include/) 8 | 9 | #add_library(roborts_common::alg_factory ALIAS alg_factory) -------------------------------------------------------------------------------- /roborts_common/include/alg_factory/bind_this.h: -------------------------------------------------------------------------------- 1 | #ifndef RRTS_BIND_THIS_H 2 | #define RRTS_BIND_THIS_H 3 | namespace std 4 | { 5 | template // begin with 0 here! 6 | struct placeholder_template 7 | {}; 8 | 9 | template 10 | struct is_placeholder > 11 | : integral_constant // the one is important 12 | {}; 13 | } // end of namespace std 14 | 15 | 16 | template 17 | struct int_sequence 18 | {}; 19 | 20 | template 21 | struct make_int_sequence 22 | : make_int_sequence 23 | {}; 24 | 25 | template 26 | struct make_int_sequence<0, Is...> 27 | : int_sequence 28 | {}; 29 | 30 | template 31 | auto bind_this_sub(R (U::*p)(Args...), U * pp, int_sequence) 32 | -> decltype(std::bind(p, pp, std::placeholder_template{}...)) 33 | { 34 | return std::bind(p, pp, std::placeholder_template{}...); 35 | } 36 | 37 | // binds a member function only for the this pointer using std::bind 38 | template 39 | auto bind_this(R (U::*p)(Args...), U * pp) 40 | -> decltype(bind_this_sub(p, pp, make_int_sequence< sizeof...(Args) >{})) 41 | { 42 | return bind_this_sub(p, pp, make_int_sequence< sizeof...(Args) >{}); 43 | } 44 | 45 | // utility 46 | template 47 | auto bind_this_sub(R (U::*p)(Args...) const, U * pp, int_sequence) 48 | -> decltype(std::bind(p, pp, std::placeholder_template{}...)) 49 | { 50 | return std::bind(p, pp, std::placeholder_template{}...); 51 | } 52 | 53 | // binds a member function only for the this pointer using std::bind 54 | template 55 | auto bind_this(R (U::*p)(Args...) const, U * pp) 56 | -> decltype(bind_this_sub(p, pp, make_int_sequence< sizeof...(Args) >{})) 57 | { 58 | return bind_this_sub(p, pp, make_int_sequence< sizeof...(Args) >{}); 59 | } 60 | #endif //RRTS_BIND_THIS_H 61 | -------------------------------------------------------------------------------- /roborts_common/include/io/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(io) 2 | 3 | #common::io 4 | add_library(io INTERFACE) 5 | 6 | target_sources(io INTERFACE io.h) 7 | target_link_libraries(io INTERFACE ${PROTOBUF_LIBRARIES}) 8 | -------------------------------------------------------------------------------- /roborts_common/include/state/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(roborts_state) 2 | 3 | #common::io 4 | add_library(roborts_state INTERFACE) 5 | 6 | target_include_directories(roborts_state INTERFACE include/) 7 | -------------------------------------------------------------------------------- /roborts_common/include/state/node_state.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of  11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | 18 | #ifndef RRTS_NODE_STATE_H 19 | #define RRTS_NODE_STATE_H 20 | namespace roborts_common{ 21 | 22 | enum NodeState{ 23 | IDLE, 24 | RUNNING, 25 | PAUSE, 26 | SUCCESS, 27 | FAILURE 28 | }; 29 | } //namespace roborts_common 30 | 31 | #endif //RRTS_NODE_STATE_H 32 | -------------------------------------------------------------------------------- /roborts_common/include/timer/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(roborts_timer) 2 | 3 | #common::io 4 | add_library(roborts_timer INTERFACE) 5 | 6 | target_include_directories(roborts_timer INTERFACE include/) -------------------------------------------------------------------------------- /roborts_common/include/timer/timer.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of  11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | 18 | #ifndef TIMER_H 19 | #define TIMER_H 20 | #include 21 | #include 22 | 23 | 24 | #define TIMER_START(FUNC) boost::timer t_##FUNC; 25 | #define TIMER_END(FUNC) std::cout << "[" << #FUNC << "]" << "cost time: " << t_##FUNC.elapsed() << std::endl; 26 | 27 | #endif // TIMER_H 28 | -------------------------------------------------------------------------------- /roborts_common/math/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1) 2 | project(roborts_math) 3 | 4 | #common::math 5 | add_library(roborts_math INTERFACE 6 | 7 | ) 8 | 9 | target_sources(roborts_math INTERFACE 10 | geometry.h 11 | math.h 12 | ) 13 | 14 | target_include_directories(${PROJECT_NAME} 15 | PUBLIC 16 | ${EIGEN3_INCLUDE_DIR} 17 | ) 18 | 19 | target_link_libraries(${PROJECT_NAME} 20 | PUBLIC 21 | ${EIGEN3_LIBRARIES} 22 | ${catkin_LIBRARIES} 23 | ${GLOG_LIBRARY} 24 | ) 25 | -------------------------------------------------------------------------------- /roborts_common/package.xml: -------------------------------------------------------------------------------- 1 | 2 | roborts_common 3 | 1.0.0 4 | 5 | The roborts common package provides all the common functions for RoboMaster AI Robot 6 | 7 | RoboMaster 8 | RoboMaster 9 | GPL 3.0 10 | https://github.com/RoboMaster/RoboRTS 11 | 12 | catkin 13 | 14 | actionlib 15 | roscpp 16 | roborts_msgs 17 | rospy 18 | 19 | actionlib 20 | roscpp 21 | roborts_msgs 22 | rospy 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /roborts_costmap/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(roborts_costmap) 2 | cmake_minimum_required(VERSION 3.1) 3 | set(CMAKE_CXX_STANDARD 14) 4 | 5 | set(CMAKE_BUILD_TYPE Release) 6 | set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake_module) 7 | find_package(Eigen3 REQUIRED) 8 | find_package(ProtoBuf REQUIRED) 9 | find_package(catkin REQUIRED COMPONENTS 10 | roscpp 11 | tf 12 | pcl_conversions 13 | pcl_ros 14 | roborts_common 15 | laser_geometry 16 | sensor_msgs 17 | geometry_msgs 18 | ) 19 | 20 | find_package(PCL 1.7 REQUIRED) 21 | find_package(Boost REQUIRED) 22 | 23 | catkin_package( 24 | INCLUDE_DIRS 25 | include 26 | ) 27 | 28 | 29 | #proto file 30 | file(GLOB ProtoFiles "${CMAKE_CURRENT_SOURCE_DIR}/proto/*.proto") 31 | 32 | rrts_protobuf_generate_cpp(${CMAKE_CURRENT_SOURCE_DIR}/proto 33 | CostmapProtoSrcs 34 | CostmapProtoHeaders 35 | ${ProtoFiles} 36 | ) 37 | 38 | include_directories(include/costmap proto/) 39 | aux_source_directory(src/. SRC_LIST) 40 | list(REMOVE_ITEM SRC_LIST "src/test_costmap.cpp") 41 | 42 | #lib project 43 | add_library(roborts_costmap 44 | SHARED 45 | ${SRC_LIST} 46 | ${CostmapProtoHeaders} 47 | ${CostmapProtoSrcs} 48 | ) 49 | 50 | target_include_directories(roborts_costmap 51 | PUBLIC 52 | ${catkin_INCLUDE_DIRS} 53 | ${EIGEN3_INCLUDE_DIRS} 54 | ) 55 | 56 | target_link_libraries(roborts_costmap 57 | PUBLIC 58 | ${catkin_LIBRARIES} 59 | ${PROTOBUF_LIBRARIES} 60 | ) 61 | 62 | #test project 63 | add_executable(test_costmap src/test_costmap.cpp) 64 | 65 | target_include_directories(test_costmap 66 | PUBLIC 67 | ${catkin_INCLUDE_DIRS} 68 | ${EIGEN3_INCLUDE_DIRS} 69 | ) 70 | 71 | target_link_libraries(test_costmap 72 | roborts_costmap 73 | ${catkin_LIBRARIES} 74 | ${PROTOBUF_LIBRARIES} 75 | ) 76 | 77 | list(APPEND catkin_LIBRARIES roborts_costmap) 78 | 79 | install(DIRECTORY include 80 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 81 | FILES_MATCHING PATTERN "*.h" 82 | PATTERN ".svn" EXCLUDE 83 | ) 84 | 85 | 86 | -------------------------------------------------------------------------------- /roborts_costmap/config/costmap_parameter_config_for_decision.prototxt: -------------------------------------------------------------------------------- 1 | para_basic { 2 | is_raw_rosmessage: true 3 | is_debug: true 4 | } 5 | para_costmap_interface { 6 | global_frame: "map" 7 | robot_base_frame: "base_link" 8 | footprint_padding: 0.1 9 | transform_tolerance: 3.0 10 | distance_threshold: 1.0 11 | map_width: 10.0 12 | map_height: 10.0 13 | map_resolution: 0.05 14 | map_origin_x: 0.0 15 | map_origin_y: 0.0 16 | is_tracking_unknown: false 17 | is_rolling_window: false 18 | has_obstacle_layer: true 19 | has_static_layer: true 20 | inflation_file_path: "/config/inflation_layer_config.prototxt" 21 | map_update_frequency: 5 22 | } 23 | footprint { 24 | point { 25 | x: -0.3 26 | y: -0.225 27 | } 28 | point { 29 | x: -0.3 30 | y: 0.225 31 | } 32 | point { 33 | x: 0.3 34 | y: 0.225 35 | } 36 | point { 37 | x: 0.3 38 | y: -0.225 39 | } 40 | } 41 | -------------------------------------------------------------------------------- /roborts_costmap/config/costmap_parameter_config_for_global_plan.prototxt: -------------------------------------------------------------------------------- 1 | para_basic { 2 | is_raw_rosmessage: true 3 | is_debug: true 4 | } 5 | para_costmap_interface { 6 | global_frame: "map" 7 | robot_base_frame: "base_link" 8 | footprint_padding: 0.1 9 | transform_tolerance: 3.0 10 | distance_threshold: 1.0 11 | map_width: 10.0 12 | map_height: 10.0 13 | map_resolution: 0.05 14 | map_origin_x: 0.0 15 | map_origin_y: 0.0 16 | is_tracking_unknown: false 17 | is_rolling_window: false 18 | has_obstacle_layer: true 19 | has_static_layer: true 20 | inflation_file_path: "/config/inflation_layer_config.prototxt" 21 | map_update_frequency: 5 22 | } 23 | footprint { 24 | point { 25 | x: -0.3 26 | y: -0.225 27 | } 28 | point { 29 | x: -0.3 30 | y: 0.225 31 | } 32 | point { 33 | x: 0.3 34 | y: 0.225 35 | } 36 | point { 37 | x: 0.3 38 | y: -0.225 39 | } 40 | } 41 | -------------------------------------------------------------------------------- /roborts_costmap/config/costmap_parameter_config_for_local_plan.prototxt: -------------------------------------------------------------------------------- 1 | para_basic { 2 | is_raw_rosmessage: true 3 | is_debug: true 4 | } 5 | para_costmap_interface { 6 | global_frame: "odom" 7 | robot_base_frame: "base_link" 8 | footprint_padding: 0.05 9 | transform_tolerance: 1.0 10 | distance_threshold: 1.0 11 | map_width: 5.0 12 | map_height: 3.0 13 | map_resolution: 0.04 14 | map_origin_x: 0.0 15 | map_origin_y: 0.0 16 | is_tracking_unknown: false 17 | is_rolling_window: true 18 | has_obstacle_layer: true 19 | has_static_layer: false 20 | inflation_file_path: "/config/inflation_layer_config_min.prototxt" 21 | map_update_frequency: 10 22 | } 23 | footprint { 24 | point { 25 | x: -0.3 26 | y: -0.225 27 | } 28 | point { 29 | x: -0.3 30 | y: 0.225 31 | } 32 | point { 33 | x: 0.3 34 | y: 0.225 35 | } 36 | point { 37 | x: 0.3 38 | y: -0.225 39 | } 40 | } 41 | -------------------------------------------------------------------------------- /roborts_costmap/config/inflation_layer_config.prototxt: -------------------------------------------------------------------------------- 1 | inflation_radius: 0.7 2 | cost_scaling_factor: 10 3 | is_raw_rosmessage: true 4 | is_debug: true 5 | -------------------------------------------------------------------------------- /roborts_costmap/config/inflation_layer_config_min.prototxt: -------------------------------------------------------------------------------- 1 | inflation_radius: 0.7 2 | cost_scaling_factor: 2.5 3 | is_raw_rosmessage: true 4 | is_debug: true 5 | -------------------------------------------------------------------------------- /roborts_costmap/config/obstacle_layer_config.prototxt: -------------------------------------------------------------------------------- 1 | observation_keep_time: 0 2 | expected_update_rate: 10 3 | min_obstacle_height: 0.0 4 | max_obstacle_height: 2.0 5 | obstacle_range: 5 6 | raytrace_range: 5 7 | transform_tolerance: 0.3 8 | topic_string: "scan" 9 | sensor_frame: "base_laser_link" 10 | inf_is_valid: false 11 | clearing: true 12 | footprint_clearing_enabled: false 13 | marking: true 14 | is_debug: true 15 | 16 | 17 | -------------------------------------------------------------------------------- /roborts_costmap/config/static_layer_config.prototxt: -------------------------------------------------------------------------------- 1 | first_map_only: false 2 | subscribe_to_updates: false 3 | track_unknown_space: true 4 | use_maximum: true 5 | unknown_cost_value: -1 6 | lethal_threshold: 100 7 | trinary_map: true 8 | topic_name: "map" 9 | is_raw_rosmessage: true 10 | is_debug: true -------------------------------------------------------------------------------- /roborts_costmap/include/costmap/map_common.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of  11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | /********************************************************************* 18 | * 19 | * Software License Agreement (BSD License) 20 | * 21 | * Copyright (c) 2008, 2013, Willow Garage, Inc. 22 | * All rights reserved. 23 | * 24 | * Redistribution and use in source and binary forms, with or without 25 | * modification, are permitted provided that the following conditions 26 | * are met: 27 | * 28 | * * Redistributions of source code must retain the above copyright 29 | * notice, this list of conditions and the following disclaimer. 30 | * * Redistributions in binary form must reproduce the above 31 | * copyright notice, this list of conditions and the following 32 | * disclaimer in the documentation and/or other materials provided 33 | * with the distribution. 34 | * * Neither the name of Willow Garage, Inc. nor the names of its 35 | * contributors may be used to endorse or promote products derived 36 | * from this software without specific prior written permission. 37 | * 38 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 39 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 40 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 41 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 42 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 43 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 44 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 45 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 46 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 47 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 48 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 49 | * POSSIBILITY OF SUCH DAMAGE. 50 | * 51 | *********************************************************************/ 52 | 53 | #ifndef ROBORTS_COSTMAP_MAP_COMMON_H 54 | #define ROBORTS_COSTMAP_MAP_COMMON_H 55 | 56 | #include 57 | #include 58 | #include 59 | #include 60 | #include 61 | #include 62 | 63 | #include "io/io.h" 64 | 65 | namespace roborts_costmap { 66 | static const unsigned char NO_INFORMATION = 255; 67 | static const unsigned char LETHAL_OBSTACLE = 254; 68 | static const unsigned char INSCRIBED_INFLATED_OBSTACLE = 253; 69 | static const unsigned char FREE_SPACE = 0; 70 | } //namespace roborts_costmap 71 | 72 | #endif //ROBORTS_COSTMAP_MAP_COMMON_H 73 | -------------------------------------------------------------------------------- /roborts_costmap/package.xml: -------------------------------------------------------------------------------- 1 | 2 | roborts_costmap 3 | 1.0.0 4 | 5 | The roborts costmap package provides costmap representation of the environment for RoboMaster AI Robot 6 | 7 | RoboMaster 8 | RoboMaster 9 | GPL 3.0 10 | https://github.com/RoboMaster/RoboRTS 11 | 12 | catkin 13 | 14 | actionlib 15 | roscpp 16 | rospy 17 | roborts_common 18 | 19 | actionlib 20 | roscpp 21 | rospy 22 | roborts_common 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /roborts_costmap/proto/costmap_parameter_setting.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto2"; 2 | package roborts_costmap; 3 | 4 | message ParaBasic { 5 | required bool is_raw_rosmessage = 1; 6 | optional bool is_debug = 2; 7 | } 8 | message ParaCostmapInterface { 9 | required string global_frame = 1; 10 | required string robot_base_frame = 2; 11 | required double footprint_padding = 3; 12 | required double transform_tolerance = 4; 13 | required double distance_threshold = 5; 14 | required double map_width = 6; 15 | required double map_height = 7; 16 | required double map_origin_x = 8; 17 | required double map_origin_y = 9; 18 | required double map_resolution = 10; 19 | required bool is_tracking_unknown = 11; 20 | required bool is_rolling_window = 12; 21 | required bool has_obstacle_layer = 13; 22 | required bool has_static_layer = 14; 23 | required string inflation_file_path = 15; 24 | required double map_update_frequency = 16; 25 | } 26 | message Point { 27 | required double x = 1; 28 | required double y = 2; 29 | } 30 | message Footprint { 31 | repeated Point point = 1; 32 | } 33 | 34 | message ParaCollection { 35 | required ParaCostmapInterface para_costmap_interface = 1; 36 | required Footprint footprint = 2; 37 | required ParaBasic para_basic = 3; 38 | } 39 | -------------------------------------------------------------------------------- /roborts_costmap/proto/inflation_layer_setting.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto2"; 2 | package roborts_costmap; 3 | 4 | message ParaInflationLayer { 5 | required double inflation_radius = 1; 6 | required double cost_scaling_factor = 2; 7 | required bool is_debug = 3; 8 | required bool is_raw_rosmessage = 4; 9 | } 10 | 11 | 12 | -------------------------------------------------------------------------------- /roborts_costmap/proto/obstacle_layer_setting.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto2"; 2 | package roborts_costmap; 3 | message ParaObstacleLayer { 4 | required double observation_keep_time = 1; 5 | required double expected_update_rate = 2; 6 | required double min_obstacle_height = 3; 7 | required double max_obstacle_height = 4; 8 | required double obstacle_range = 5; 9 | required double raytrace_range = 6; 10 | required double transform_tolerance = 7; 11 | required string topic_string = 8; 12 | required string sensor_frame = 9; 13 | required bool inf_is_valid = 10; 14 | required bool clearing = 11; 15 | required bool marking = 12; 16 | required bool footprint_clearing_enabled = 13; 17 | required bool is_debug = 14; 18 | } 19 | -------------------------------------------------------------------------------- /roborts_costmap/proto/static_layer_setting.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto2"; 2 | package roborts_costmap; 3 | 4 | message ParaStaticLayer { 5 | required bool first_map_only = 1; 6 | required bool subscribe_to_updates = 2; 7 | required bool track_unknown_space = 3; 8 | required bool use_maximum = 4; 9 | required int32 unknown_cost_value = 5; 10 | required bool trinary_map = 6; 11 | required int32 lethal_threshold = 7; 12 | required string topic_name = 8; 13 | required bool is_raw_rosmessage = 9; 14 | required bool is_debug = 10; 15 | } 16 | -------------------------------------------------------------------------------- /roborts_costmap/src/layer.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of  11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | /********************************************************************* 18 | * 19 | * Software License Agreement (BSD License) 20 | * 21 | * Copyright (c) 2008, 2013, Willow Garage, Inc. 22 | * All rights reserved. 23 | * 24 | * Redistribution and use in source and binary forms, with or without 25 | * modification, are permitted provided that the following conditions 26 | * are met: 27 | * 28 | * * Redistributions of source code must retain the above copyright 29 | * notice, this list of conditions and the following disclaimer. 30 | * * Redistributions in binary form must reproduce the above 31 | * copyright notice, this list of conditions and the following 32 | * disclaimer in the documentation and/or other materials provided 33 | * with the distribution. 34 | * * Neither the name of Willow Garage, Inc. nor the names of its 35 | * contributors may be used to endorse or promote products derived 36 | * from this software without specific prior written permission. 37 | * 38 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 39 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 40 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 41 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 42 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 43 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 44 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 45 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 46 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 47 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 48 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 49 | * POSSIBILITY OF SUCH DAMAGE. 50 | * 51 | *********************************************************************/ 52 | #include "layer.h" 53 | 54 | namespace roborts_costmap { 55 | 56 | Layer::Layer() 57 | : layered_costmap_(NULL), is_current_(false), is_enabled_(false), name_(), tf_(NULL) {} 58 | 59 | void Layer::Initialize(CostmapLayers *parent, std::string name, tf::TransformListener *tf) { 60 | layered_costmap_ = parent; 61 | name_ = name; 62 | tf_ = tf; 63 | OnInitialize(); 64 | } 65 | 66 | const std::vector &Layer::GetFootprint() const { 67 | return layered_costmap_->GetFootprint(); 68 | } 69 | 70 | } //namespace roborts_costmap -------------------------------------------------------------------------------- /roborts_decision/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(roborts_decision) 2 | cmake_minimum_required(VERSION 3.1) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake_module) 6 | set(CMAKE_BUILD_TYPE Release) 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | roscpp 10 | tf 11 | nav_msgs 12 | geometry_msgs 13 | actionlib 14 | roborts_common 15 | roborts_msgs 16 | roborts_costmap 17 | ) 18 | 19 | find_package(Eigen3 REQUIRED) 20 | find_package(ProtoBuf REQUIRED) 21 | 22 | include_directories( 23 | include 24 | ${catkin_INCLUDE_DIRS} 25 | ) 26 | 27 | #generate proto files 28 | file(GLOB ProtoFiles "${CMAKE_CURRENT_SOURCE_DIR}/proto/*.proto") 29 | rrts_protobuf_generate_cpp(${CMAKE_CURRENT_SOURCE_DIR}/proto 30 | DecisionProtoSrc 31 | DecisionProtoHds 32 | ${ProtoFiles} 33 | ) 34 | 35 | catkin_package() 36 | 37 | add_executable(behavior_test_node 38 | ${DecisionProtoHds} 39 | ${DecisionProtoSrc} 40 | example_behavior/escape_behavior.h 41 | example_behavior/back_boot_area_behavior.h 42 | example_behavior/chase_behavior.h 43 | example_behavior/patrol_behavior.h 44 | example_behavior/search_behavior.h 45 | behavior_test.cpp 46 | executor/chassis_executor.cpp 47 | ) 48 | 49 | target_link_libraries(behavior_test_node 50 | PRIVATE 51 | roborts_costmap 52 | ${catkin_LIBRARIES} 53 | ${PROTOBUF_LIBRARIES} 54 | ) 55 | add_dependencies(behavior_test_node 56 | roborts_msgs_generate_messages) 57 | -------------------------------------------------------------------------------- /roborts_decision/behavior_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "executor/chassis_executor.h" 4 | 5 | #include "example_behavior/back_boot_area_behavior.h" 6 | #include "example_behavior/escape_behavior.h" 7 | #include "example_behavior/chase_behavior.h" 8 | #include "example_behavior/search_behavior.h" 9 | #include "example_behavior/patrol_behavior.h" 10 | #include "example_behavior/goal_behavior.h" 11 | 12 | void Command(); 13 | char command = '0'; 14 | 15 | int main(int argc, char **argv) { 16 | ros::init(argc, argv, "behavior_test_node"); 17 | std::string full_path = ros::package::getPath("roborts_decision") + "/config/decision.prototxt"; 18 | 19 | auto chassis_executor = new roborts_decision::ChassisExecutor; 20 | auto blackboard = new roborts_decision::Blackboard(full_path); 21 | 22 | roborts_decision::BackBootAreaBehavior back_boot_area_behavior(chassis_executor, blackboard, full_path); 23 | roborts_decision::ChaseBehavior chase_behavior(chassis_executor, blackboard, full_path); 24 | roborts_decision::SearchBehavior search_behavior(chassis_executor, blackboard, full_path); 25 | roborts_decision::EscapeBehavior escape_behavior(chassis_executor, blackboard, full_path); 26 | roborts_decision::PatrolBehavior patrol_behavior(chassis_executor, blackboard, full_path); 27 | roborts_decision::GoalBehavior goal_behavior(chassis_executor, blackboard); 28 | 29 | auto command_thread= std::thread(Command); 30 | ros::Rate rate(10); 31 | while(ros::ok()){ 32 | ros::spinOnce(); 33 | switch (command) { 34 | //back to boot area 35 | case '1': 36 | back_boot_area_behavior.Run(); 37 | break; 38 | //patrol 39 | case '2': 40 | patrol_behavior.Run(); 41 | break; 42 | //chase. 43 | case '3': 44 | chase_behavior.Run(); 45 | break; 46 | //search 47 | case '4': 48 | search_behavior.Run(); 49 | break; 50 | //escape. 51 | case '5': 52 | escape_behavior.Run(); 53 | break; 54 | //goal. 55 | case '6': 56 | goal_behavior.Run(); 57 | break; 58 | case 27: 59 | if (command_thread.joinable()){ 60 | command_thread.join(); 61 | } 62 | return 0; 63 | default: 64 | break; 65 | } 66 | rate.sleep(); 67 | } 68 | 69 | 70 | return 0; 71 | } 72 | 73 | void Command() { 74 | 75 | while (command != 27) { 76 | std::cout << "**************************************************************************************" << std::endl; 77 | std::cout << "*********************************please send a command********************************" << std::endl; 78 | std::cout << "1: back boot area behavior" << std::endl 79 | << "2: patrol behavior" << std::endl 80 | << "3: chase_behavior" << std::endl 81 | << "4: search behavior" << std::endl 82 | << "5: escape behavior" << std::endl 83 | << "6: goal behavior" << std::endl 84 | << "esc: exit program" << std::endl; 85 | std::cout << "**************************************************************************************" << std::endl; 86 | std::cout << "> "; 87 | std::cin >> command; 88 | if (command != '1' && command != '2' && command != '3' && command != '4' && command != '5' && command != '6' && command != 27) { 89 | std::cout << "please input again!" << std::endl; 90 | std::cout << "> "; 91 | std::cin >> command; 92 | } 93 | 94 | } 95 | } 96 | 97 | -------------------------------------------------------------------------------- /roborts_decision/behavior_tree/behavior_state.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by autocar on 18-12-21. 3 | // 4 | 5 | #ifndef ROBORTS_DECISION_BEHAVIOR_STATE_H 6 | #define ROBORTS_DECISION_BEHAVIOR_STATE_H 7 | namespace roborts_decision { 8 | /** 9 | * @brief Behavior state 10 | */ 11 | enum class BehaviorState { 12 | RUNNING, ///< Running state in process 13 | SUCCESS, ///< Success state as result 14 | FAILURE, ///< Failure state as result 15 | IDLE, ///< Idle state, state as default or after cancellation 16 | }; 17 | } 18 | #endif //ROBORTS_DECISION_BEHAVIOR_STATE_H 19 | -------------------------------------------------------------------------------- /roborts_decision/behavior_tree/behavior_tree.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of  11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | 18 | #ifndef ROBORTS_DECISION_BEHAVIOR_TREE_H 19 | #define ROBORTS_DECISION_BEHAVIOR_TREE_H 20 | 21 | #include 22 | 23 | #include 24 | 25 | #include "behavior_node.h" 26 | 27 | namespace roborts_decision{ 28 | /** 29 | * @brief Behavior tree class to initialize and execute the whole tree 30 | */ 31 | class BehaviorTree { 32 | public: 33 | /** 34 | * @brief Constructor of BehaviorTree 35 | * @param root_node root node of the behavior tree 36 | * @param cycle_duration tick duration of the behavior tree (unit ms) 37 | */ 38 | BehaviorTree(BehaviorNode::Ptr root_node, int cycle_duration) : 39 | root_node_(root_node), 40 | cycle_duration_(cycle_duration) {} 41 | /** 42 | * @brief Loop to tick the behavior tree 43 | */ 44 | void Run() { 45 | 46 | unsigned int frame = 0; 47 | while (ros::ok() ) { 48 | 49 | std::chrono::steady_clock::time_point start_time = std::chrono::steady_clock::now(); 50 | // Update the blackboard data 51 | ros::spinOnce(); 52 | ROS_INFO("Frame : %d", frame); 53 | root_node_->Run(); 54 | 55 | std::chrono::steady_clock::time_point end_time = std::chrono::steady_clock::now(); 56 | std::chrono::milliseconds execution_duration = 57 | std::chrono::duration_cast(end_time - start_time); 58 | std::chrono::milliseconds sleep_time = cycle_duration_ - execution_duration; 59 | 60 | if (sleep_time > std::chrono::microseconds(0)) { 61 | 62 | std::this_thread::sleep_for(sleep_time); 63 | ROS_INFO("Excution Duration: %ld / %ld ms", cycle_duration_.count(), cycle_duration_.count()); 64 | 65 | } else { 66 | 67 | ROS_WARN("The time tick once is %ld beyond the expected time %ld", execution_duration.count(), cycle_duration_.count()); 68 | 69 | } 70 | ROS_INFO("----------------------------------"); 71 | frame++; 72 | } 73 | } 74 | private: 75 | //! root node of the behavior tree 76 | BehaviorNode::Ptr root_node_; 77 | //! tick duration of the behavior tree (unit ms) 78 | std::chrono::milliseconds cycle_duration_; 79 | 80 | }; 81 | 82 | }//namespace roborts_decision 83 | 84 | #endif //ROBORTS_DECISION_BEHAVIOR_TREE_H 85 | -------------------------------------------------------------------------------- /roborts_decision/example_behavior/goal_behavior.h: -------------------------------------------------------------------------------- 1 | #ifndef ROBORTS_DECISION_GOAL_BEHAVIOR_H 2 | #define ROBORTS_DECISION_GOAL_BEHAVIOR_H 3 | 4 | 5 | #include "io/io.h" 6 | 7 | #include "../blackboard/blackboard.h" 8 | #include "../executor/chassis_executor.h" 9 | #include "../behavior_tree/behavior_state.h" 10 | 11 | 12 | namespace roborts_decision { 13 | class GoalBehavior { 14 | public: 15 | GoalBehavior(ChassisExecutor* &chassis_executor, 16 | Blackboard* &blackboard) : 17 | chassis_executor_(chassis_executor), 18 | blackboard_(blackboard) { } 19 | 20 | void Run() { 21 | if(blackboard_->IsNewGoal()){ 22 | chassis_executor_->Execute(blackboard_->GetGoal()); 23 | } 24 | } 25 | 26 | void Cancel() { 27 | chassis_executor_->Cancel(); 28 | } 29 | 30 | BehaviorState Update() { 31 | return chassis_executor_->Update(); 32 | } 33 | 34 | ~GoalBehavior() = default; 35 | 36 | private: 37 | //! executor 38 | ChassisExecutor* const chassis_executor_; 39 | 40 | //! perception information 41 | Blackboard* const blackboard_; 42 | 43 | //! planning goal 44 | geometry_msgs::PoseStamped planning_goal_; 45 | 46 | }; 47 | } 48 | 49 | #endif //ROBORTS_DECISION_GOAL_BEHAVIOR_H 50 | -------------------------------------------------------------------------------- /roborts_decision/example_behavior/line_iterator.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of  11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | 18 | #ifndef ROBORTS_DECISION_LINE_ITERATOR_H 19 | #define ROBORTS_DECISION_LINE_ITERATOR_H 20 | 21 | #include 22 | 23 | namespace roborts_decision { 24 | 25 | class FastLineIterator { 26 | // this method is a modified version of base_local_planner/line_iterator.h 27 | public: 28 | FastLineIterator( int x0, int y0, int x1, int y1 ) 29 | : x0_( x0 ) 30 | , y0_( y0 ) 31 | , x1_( x1 ) 32 | , y1_( y1 ) 33 | , x_( x0 ) 34 | , y_( y0 ) 35 | , deltax_(abs(x1 - x0)) 36 | , deltay_(abs(y1 - y0)) 37 | , curpixel_( 0 ) { 38 | 39 | xinc1_ = (x1 - x0) >0 ?1:-1; 40 | xinc2_ = (x1 - x0) >0 ?1:-1; 41 | 42 | yinc1_ = (y1 - y0) >0 ?1:-1; 43 | yinc2_ = (y1 - y0) >0 ?1:-1; 44 | 45 | if( deltax_ >= deltay_ ) { 46 | xinc1_ = 0; 47 | yinc2_ = 0; 48 | den_ = 2 * deltax_; 49 | num_ = deltax_; 50 | numadd_ = 2 * deltay_; 51 | numpixels_ = deltax_; 52 | } else { 53 | xinc2_ = 0; 54 | yinc1_ = 0; 55 | den_ = 2 * deltay_; 56 | num_ = deltay_; 57 | numadd_ = 2 * deltax_; 58 | numpixels_ = deltay_; 59 | } 60 | } 61 | ~FastLineIterator() = default; 62 | bool IsValid() const { 63 | return curpixel_ <= numpixels_; 64 | } 65 | 66 | void Advance() { 67 | num_ += numadd_; 68 | if( num_ >= den_ ) { 69 | num_ -= den_; 70 | x_ += xinc1_; 71 | y_ += yinc1_; 72 | } 73 | x_ += xinc2_; 74 | y_ += yinc2_; 75 | 76 | curpixel_++; 77 | } 78 | 79 | int GetX() const { return x_; } 80 | int GetY() const { return y_; } 81 | 82 | int GetX0() const { return x0_; } 83 | int GetY0() const { return y0_; } 84 | 85 | int GetX1() const { return x1_; } 86 | int GetY1() const { return y1_; } 87 | 88 | private: 89 | int x0_; 90 | int y0_; 91 | int x1_; 92 | int y1_; 93 | 94 | int x_; 95 | int y_; 96 | 97 | int deltax_; 98 | int deltay_; 99 | 100 | int curpixel_; 101 | 102 | int xinc1_, xinc2_, yinc1_, yinc2_; 103 | int den_, num_, numadd_, numpixels_; 104 | }; 105 | 106 | } // namespace roborts_decision 107 | 108 | #endif //ROBORTS_DECISION_LINE_ITERATOR_H 109 | 110 | -------------------------------------------------------------------------------- /roborts_decision/example_behavior/patrol_behavior.h: -------------------------------------------------------------------------------- 1 | #ifndef ROBORTS_DECISION_PATROL_BEHAVIOR_H 2 | #define ROBORTS_DECISION_PATROL_BEHAVIOR_H 3 | 4 | #include "io/io.h" 5 | 6 | #include "../blackboard/blackboard.h" 7 | #include "../executor/chassis_executor.h" 8 | #include "../behavior_tree/behavior_state.h" 9 | #include "../proto/decision.pb.h" 10 | 11 | #include "line_iterator.h" 12 | 13 | namespace roborts_decision { 14 | class PatrolBehavior { 15 | public: 16 | PatrolBehavior(ChassisExecutor* &chassis_executor, 17 | Blackboard* &blackboard, 18 | const std::string & proto_file_path) : chassis_executor_(chassis_executor), 19 | blackboard_(blackboard) { 20 | 21 | patrol_count_ = 0; 22 | point_size_ = 0; 23 | 24 | if (!LoadParam(proto_file_path)) { 25 | ROS_ERROR("%s can't open file", __FUNCTION__); 26 | } 27 | 28 | } 29 | 30 | void Run() { 31 | 32 | auto executor_state = Update(); 33 | 34 | std::cout << "state: " << (int)(executor_state) << std::endl; 35 | 36 | if (executor_state != BehaviorState::RUNNING) { 37 | 38 | if (patrol_goals_.empty()) { 39 | ROS_ERROR("patrol goal is empty"); 40 | return; 41 | } 42 | 43 | std::cout << "send goal" << std::endl; 44 | chassis_executor_->Execute(patrol_goals_[patrol_count_]); 45 | patrol_count_ = ++patrol_count_ % point_size_; 46 | 47 | } 48 | } 49 | 50 | void Cancel() { 51 | chassis_executor_->Cancel(); 52 | } 53 | 54 | BehaviorState Update() { 55 | return chassis_executor_->Update(); 56 | } 57 | 58 | bool LoadParam(const std::string &proto_file_path) { 59 | roborts_decision::DecisionConfig decision_config; 60 | if (!roborts_common::ReadProtoFromTextFile(proto_file_path, &decision_config)) { 61 | return false; 62 | } 63 | 64 | point_size_ = (unsigned int)(decision_config.point().size()); 65 | patrol_goals_.resize(point_size_); 66 | for (int i = 0; i != point_size_; i++) { 67 | patrol_goals_[i].header.frame_id = "map"; 68 | patrol_goals_[i].pose.position.x = decision_config.point(i).x(); 69 | patrol_goals_[i].pose.position.y = decision_config.point(i).y(); 70 | patrol_goals_[i].pose.position.z = decision_config.point(i).z(); 71 | 72 | tf::Quaternion quaternion = tf::createQuaternionFromRPY(decision_config.point(i).roll(), 73 | decision_config.point(i).pitch(), 74 | decision_config.point(i).yaw()); 75 | patrol_goals_[i].pose.orientation.x = quaternion.x(); 76 | patrol_goals_[i].pose.orientation.y = quaternion.y(); 77 | patrol_goals_[i].pose.orientation.z = quaternion.z(); 78 | patrol_goals_[i].pose.orientation.w = quaternion.w(); 79 | } 80 | 81 | return true; 82 | } 83 | 84 | ~PatrolBehavior() = default; 85 | 86 | private: 87 | //! executor 88 | ChassisExecutor* const chassis_executor_; 89 | 90 | //! perception information 91 | Blackboard* const blackboard_; 92 | 93 | //! patrol buffer 94 | std::vector patrol_goals_; 95 | unsigned int patrol_count_; 96 | unsigned int point_size_; 97 | 98 | }; 99 | } 100 | 101 | #endif //ROBORTS_DECISION_PATROL_BEHAVIOR_H 102 | -------------------------------------------------------------------------------- /roborts_decision/executor/gimbal_executor.cpp: -------------------------------------------------------------------------------- 1 | #include "gimbal_executor.h" 2 | namespace roborts_decision{ 3 | GimbalExecutor::GimbalExecutor():excution_mode_(ExcutionMode::IDLE_MODE), 4 | execution_state_(BehaviorState::IDLE){ 5 | ros::NodeHandle nh; 6 | cmd_gimbal_angle_pub_ = nh.advertise("cmd_gimbal_angle", 1); 7 | cmd_gimbal_rate_pub_ = nh.advertise("cmd_gimbal_rate", 1); 8 | 9 | } 10 | 11 | void GimbalExecutor::Execute(const roborts_msgs::GimbalAngle &gimbal_angle){ 12 | excution_mode_ = ExcutionMode::ANGLE_MODE; 13 | cmd_gimbal_angle_pub_.publish(gimbal_angle); 14 | } 15 | 16 | void GimbalExecutor::Execute(const roborts_msgs::GimbalRate &gimbal_rate){ 17 | excution_mode_ = ExcutionMode::RATE_MODE; 18 | cmd_gimbal_rate_pub_.publish(gimbal_rate); 19 | } 20 | 21 | BehaviorState GimbalExecutor::Update(){ 22 | switch (excution_mode_){ 23 | case ExcutionMode::IDLE_MODE: 24 | execution_state_ = BehaviorState::IDLE; 25 | break; 26 | 27 | case ExcutionMode::ANGLE_MODE: 28 | execution_state_ = BehaviorState::RUNNING; 29 | break; 30 | 31 | case ExcutionMode::RATE_MODE: 32 | execution_state_ = BehaviorState::RUNNING; 33 | break; 34 | 35 | default: 36 | ROS_ERROR("Wrong Execution Mode"); 37 | } 38 | return execution_state_; 39 | } 40 | 41 | void GimbalExecutor::Cancel(){ 42 | switch (excution_mode_){ 43 | case ExcutionMode::IDLE_MODE: 44 | ROS_WARN("Nothing to be canceled."); 45 | break; 46 | 47 | case ExcutionMode::ANGLE_MODE: 48 | cmd_gimbal_rate_pub_.publish(zero_gimbal_rate_); 49 | excution_mode_ = ExcutionMode::IDLE_MODE; 50 | break; 51 | 52 | case ExcutionMode::RATE_MODE: 53 | cmd_gimbal_rate_pub_.publish(zero_gimbal_rate_); 54 | excution_mode_ = ExcutionMode::IDLE_MODE; 55 | break; 56 | 57 | default: 58 | ROS_ERROR("Wrong Execution Mode"); 59 | } 60 | 61 | } 62 | } -------------------------------------------------------------------------------- /roborts_decision/executor/gimbal_executor.h: -------------------------------------------------------------------------------- 1 | #ifndef ROBORTS_DECISION_GIMBAL_EXECUTOR_H 2 | #define ROBORTS_DECISION_GIMBAL_EXECUTOR_H 3 | #include "ros/ros.h" 4 | 5 | #include "roborts_msgs/GimbalAngle.h" 6 | #include "roborts_msgs/GimbalRate.h" 7 | 8 | #include "../behavior_tree/behavior_state.h" 9 | namespace roborts_decision{ 10 | /*** 11 | * @brief Gimbal Executor to execute different abstracted task for gimbal module 12 | */ 13 | class GimbalExecutor{ 14 | public: 15 | /** 16 | * @brief Gimbal execution mode for different tasks 17 | */ 18 | enum class ExcutionMode{ 19 | IDLE_MODE, ///< Default idle mode with no task 20 | ANGLE_MODE, ///< Angle task mode 21 | RATE_MODE ///< Rate task mode 22 | }; 23 | /** 24 | * @brief Constructor of GimbalExecutor 25 | */ 26 | GimbalExecutor(); 27 | ~GimbalExecutor() = default; 28 | /*** 29 | * @brief Execute the gimbal angle task with publisher 30 | * @param gimbal_angle Given gimbal angle 31 | */ 32 | void Execute(const roborts_msgs::GimbalAngle &gimbal_angle); 33 | /*** 34 | * @brief Execute the gimbal rate task with publisher 35 | * @param gimbal_rate Given gimbal rate 36 | */ 37 | void Execute(const roborts_msgs::GimbalRate &gimbal_rate); 38 | /** 39 | * @brief Update the current gimbal executor state 40 | * @return Current gimbal executor state(same with behavior state) 41 | */ 42 | BehaviorState Update(); 43 | /** 44 | * @brief Cancel the current task and deal with the mode transition 45 | */ 46 | void Cancel(); 47 | 48 | private: 49 | //! execution mode of the executor 50 | ExcutionMode excution_mode_; 51 | //! execution state of the executor (same with behavior state) 52 | BehaviorState execution_state_; 53 | 54 | //! gimbal rate control publisher in ROS 55 | ros::Publisher cmd_gimbal_rate_pub_; 56 | //! zero gimbal rate in form of ROS roborts_msgs::GimbalRate 57 | roborts_msgs::GimbalRate zero_gimbal_rate_; 58 | 59 | //! gimbal angle control publisher in ROS 60 | ros::Publisher cmd_gimbal_angle_pub_; 61 | 62 | 63 | }; 64 | } 65 | 66 | 67 | #endif //ROBORTS_DECISION_GIMBAL_EXECUTOR_H -------------------------------------------------------------------------------- /roborts_decision/package.xml: -------------------------------------------------------------------------------- 1 | 2 | roborts_decision 3 | 1.0.0 4 | 5 | The roborts decision package provides decision-making for RoboMaster AI Robot 6 | 7 | RoboMaster 8 | RoboMaster 9 | GPL 3.0 10 | https://github.com/RoboMaster/RoboRTS 11 | 12 | catkin 13 | 14 | actionlib 15 | roscpp 16 | roborts_msgs 17 | rospy 18 | roborts_common 19 | roborts_costmap 20 | 21 | actionlib 22 | roscpp 23 | roborts_msgs 24 | rospy 25 | roborts_common 26 | roborts_costmap 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /roborts_decision/proto/decision.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto2"; 2 | package roborts_decision; 3 | 4 | message Point { 5 | optional float x = 1; 6 | optional float y = 2; 7 | optional float z = 3; 8 | 9 | optional float roll = 4; 10 | optional float pitch = 5; 11 | optional float yaw = 6; 12 | } 13 | 14 | message EscapeConstraint { 15 | optional float left_x_limit = 1; 16 | optional float right_x_limit = 2; 17 | optional float robot_x_limit = 3; 18 | optional float left_random_min_x = 4; 19 | optional float left_random_max_x = 5; 20 | optional float right_random_min_x = 6; 21 | optional float right_random_max_x = 7; 22 | } 23 | 24 | message SearchConstraint { 25 | optional float x_limit = 1; 26 | optional float y_limit = 2; 27 | } 28 | 29 | message WhirlVel { 30 | optional float angle_x_vel = 1; 31 | optional float angle_y_vel = 2; 32 | optional float angle_z_vel = 3; 33 | } 34 | 35 | message MultiRobot { 36 | optional Point start_position = 7; 37 | } 38 | 39 | message DecisionConfig { 40 | repeated Point point = 1; 41 | optional bool simulate = 2 [default = false]; 42 | optional bool master = 3 [default = false]; 43 | optional EscapeConstraint escape = 4; 44 | repeated Point buff_point = 5; 45 | optional SearchConstraint search_limit = 6; 46 | optional WhirlVel whirl_vel = 7; 47 | optional MultiRobot wing_bot = 8; 48 | optional MultiRobot master_bot = 9; 49 | optional Point wing_bot_task_point = 10; 50 | repeated Point search_region_1 = 11; 51 | repeated Point search_region_2 = 12; 52 | repeated Point search_region_3 = 13; 53 | repeated Point search_region_4 = 14; 54 | } 55 | 56 | -------------------------------------------------------------------------------- /roborts_detection/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(roborts_detection) 2 | 3 | set(CMAKE_CXX_STANDARD 14) 4 | set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake_module) 5 | set(CMAKE_BUILD_TYPE Release) 6 | set(COMPILE_PROTO 0) 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | roscpp 10 | tf 11 | actionlib 12 | cv_bridge 13 | image_transport 14 | std_msgs 15 | sensor_msgs 16 | roborts_msgs 17 | roborts_common 18 | ) 19 | 20 | find_package(ProtoBuf REQUIRED) 21 | find_package(OpenCV 3 REQUIRED) 22 | find_package(Eigen3 REQUIRED) 23 | 24 | catkin_package() 25 | 26 | include_directories(${EIGEN3_INCLUDE_DIR} util) 27 | 28 | add_subdirectory(util) 29 | add_subdirectory(armor_detection) 30 | #add_subdirectory(color_detection) 31 | -------------------------------------------------------------------------------- /roborts_detection/armor_detection/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(armor_detection) 2 | 3 | add_subdirectory(constraint_set) 4 | 5 | 6 | file(GLOB ProtoFiles "${CMAKE_CURRENT_SOURCE_DIR}/proto/*.proto") 7 | rrts_protobuf_generate_cpp(${CMAKE_CURRENT_SOURCE_DIR}/proto 8 | ArmorDetectionProtoSrc 9 | ArmorDetectionProtoHds 10 | ${ProtoFiles}) 11 | 12 | 13 | #armor detection node 14 | add_executable(${PROJECT_NAME}_node 15 | ${ArmorDetectionProtoSrc} 16 | ${ArmorDetectionProtoHds} 17 | armor_detection_node.cpp 18 | gimbal_control.cpp 19 | ) 20 | 21 | target_link_libraries(armor_detection_node 22 | PRIVATE 23 | detection::tool 24 | detection::constraint_set 25 | ${PROTOBUF_LIBRARIES} 26 | ${catkin_LIBRARIES} 27 | ${OpenCV_LIBRARIES} 28 | ) 29 | 30 | target_include_directories(armor_detection_node 31 | PRIVATE 32 | ${catkin_INCLUDE_DIRS} 33 | ${OpenCV_INCLUDE_DIRECTORIES} 34 | ) 35 | add_dependencies(armor_detection_node 36 | roborts_msgs_generate_messages) 37 | 38 | #armor_detection_client 39 | add_executable(${PROJECT_NAME}_client 40 | armor_detection_client.cpp 41 | ) 42 | 43 | target_include_directories(armor_detection_client 44 | PRIVATE 45 | ${catkin_INCLUDE_DIRS} 46 | ${OpenCV_INCLUDE_DIRECTORIES} 47 | ) 48 | 49 | target_link_libraries(armor_detection_client 50 | PRIVATE 51 | detection::tool 52 | ${PROTOBUF_LIBRARIES} 53 | ${catkin_LIBRARIES} 54 | ) 55 | add_dependencies(armor_detection_client 56 | roborts_msgs_generate_messages) 57 | -------------------------------------------------------------------------------- /roborts_detection/armor_detection/armor_detection_algorithms.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of  11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | 18 | #ifndef ROBORTS_DETECTION_ARMOR_DETECTION_ALGORITHMS_H 19 | #define ROBORTS_DETECTION_ARMOR_DETECTION_ALGORITHMS_H 20 | 21 | /** 22 | * Add your own algorithm harders here. 23 | */ 24 | #include "constraint_set/constraint_set.h" 25 | 26 | #endif // ROBORTS_DETECTION_ARMOR_DETECTION_ALGORITHMS_H -------------------------------------------------------------------------------- /roborts_detection/armor_detection/armor_detection_base.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of  11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | 18 | #ifndef ROBORTS_DETECTION_ARMOR_DETECTION_BASE_H 19 | #define ROBORTS_DETECTION_ARMOR_DETECTION_BASE_H 20 | 21 | #include 22 | #include "state/error_code.h" 23 | #include "../util/cv_toolbox.h" 24 | 25 | namespace roborts_detection { 26 | 27 | using roborts_common::ErrorInfo; 28 | 29 | class ArmorDetectionBase { 30 | public: 31 | ArmorDetectionBase(std::shared_ptr cv_toolbox) 32 | : cv_toolbox_(cv_toolbox) 33 | { }; 34 | virtual void LoadParam() = 0; 35 | virtual ErrorInfo DetectArmor(bool &detected, cv::Point3f &target_3d) = 0; 36 | virtual void SetThreadState(bool thread_state) = 0; 37 | virtual ~ArmorDetectionBase() = default; 38 | protected: 39 | std::shared_ptr cv_toolbox_; 40 | }; 41 | } //namespace roborts_detection 42 | 43 | #endif //ROBORTS_DETECTION_ARMOR_DETECTION_BASE_H -------------------------------------------------------------------------------- /roborts_detection/armor_detection/armor_detection_client.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of  11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | 18 | #include 19 | #include 20 | #include "roborts_msgs/ArmorDetectionAction.h" 21 | #include 22 | 23 | int main(int argc, char **argv) { 24 | ros::init(argc, argv, "armor_detection_node_test_client"); 25 | 26 | // create the action client 27 | actionlib::SimpleActionClient ac("armor_detection_node_action", true); 28 | //roborts_msgs::ArmorDetectionResult node_result; 29 | ROS_INFO("Waiting for action server to start."); 30 | ac.waitForServer(); 31 | ROS_INFO("Start."); 32 | roborts_msgs::ArmorDetectionGoal goal; 33 | 34 | char command = '0'; 35 | 36 | while (command != '4') { 37 | std::cout << "**************************************************************************************" << std::endl; 38 | std::cout << "*********************************please send a command********************************" << std::endl; 39 | std::cout << "1: start the action" << std::endl 40 | << "2: pause the action" << std::endl 41 | << "3: stop the action" << std::endl 42 | << "4: exit the program" << std::endl; 43 | std::cout << "**************************************************************************************" << std::endl; 44 | std::cout << "> "; 45 | std::cin >> command; 46 | if (command != '1' && command != '2' && command != '3' && command != '4') { 47 | std::cout << "please inpugain!" << std::endl; 48 | std::cout << "> "; 49 | std::cin >> command; 50 | } 51 | 52 | switch (command) { 53 | //start thread. 54 | case '1': 55 | goal.command = 1; 56 | ROS_INFO("I am running the request"); 57 | ac.sendGoal(goal); 58 | break; 59 | //pause thread. 60 | case '2': 61 | goal.command = 2; 62 | ROS_INFO("Action server will pause."); 63 | ac.sendGoal(goal); 64 | //stop thread. 65 | case '3': 66 | goal.command = 3; 67 | ROS_INFO("I am cancelling the request"); 68 | ac.cancelGoal(); 69 | break; 70 | default: 71 | break; 72 | } 73 | } 74 | return 0; 75 | } 76 | -------------------------------------------------------------------------------- /roborts_detection/armor_detection/config/armor_detection.prototxt: -------------------------------------------------------------------------------- 1 | name: "constraint_set" 2 | selected_algorithm: "constraint_set" 3 | undetected_armor_delay: 10 4 | camera_name: "back_camera" 5 | camera_gimbal_transform { 6 | offset_x :0 7 | offset_y :10 8 | offset_z :9 9 | offset_pitch :-2.5 10 | offset_yaw : 4.5 11 | } 12 | 13 | projectile_model_info { 14 | init_v: 15 # Launching projectile velocity 15 | init_k: 0.026 16 | } -------------------------------------------------------------------------------- /roborts_detection/armor_detection/constraint_set/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(constraint_set) 2 | 3 | file(GLOB ProtoFiles "${CMAKE_CURRENT_SOURCE_DIR}/proto/*.proto") 4 | rrts_protobuf_generate_cpp(${CMAKE_CURRENT_SOURCE_DIR}/proto 5 | ConstraintSetProtoSrc 6 | ConstraintSetProtoHds 7 | ${ProtoFiles} 8 | ) 9 | 10 | add_library(constraint_set 11 | SHARED 12 | ${ConstraintSetProtoSrc} 13 | ${ConstraintSetProtoHds} 14 | constraint_set.cpp 15 | ) 16 | 17 | add_library(detection::constraint_set ALIAS constraint_set) 18 | 19 | target_link_libraries(${PROJECT_NAME} 20 | PUBLIC 21 | detection::tool 22 | ${catkin_LIBRARIES} 23 | ${OpenCV_LIBRARIES} 24 | ) 25 | 26 | target_include_directories(${PROJECT_NAME} 27 | PUBLIC 28 | ${catkin_INCLUDE_DIRS} 29 | ) 30 | -------------------------------------------------------------------------------- /roborts_detection/armor_detection/constraint_set/config/constraint_set.prototxt: -------------------------------------------------------------------------------- 1 | threshold { 2 | light_max_aspect_ratio: 20.0 3 | light_min_area: 1 4 | light_max_angle: 20.0 5 | light_max_angle_diff: 10.0 6 | 7 | armor_max_angle: 15.0 8 | armor_min_area: 100 9 | armor_max_aspect_ratio: 4 10 | armor_max_pixel_val: 150.0 11 | armor_max_stddev: 90 12 | armor_max_mean: 90 13 | 14 | color_thread: 100 15 | blue_thread: 90 16 | red_thread: 50 17 | } 18 | 19 | signal_recognization { 20 | max_wait_fps: 100 21 | min_pulse_angle: 0.4 22 | min_num: 5 23 | } 24 | 25 | armor_size { 26 | width: 120 # mm 27 | height: 60 # mm 28 | } 29 | 30 | using_hsv: false 31 | enable_debug: true 32 | enable_neon: false 33 | enemy_color: BLUE 34 | -------------------------------------------------------------------------------- /roborts_detection/armor_detection/constraint_set/proto/constraint_set.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto2"; 2 | package roborts_detection; 3 | 4 | message Threshold { 5 | //for light 6 | required float light_max_aspect_ratio = 1; 7 | required float light_min_area = 2; 8 | required float light_max_angle = 3; 9 | required float light_max_angle_diff = 4; 10 | //for armor 11 | required float armor_max_angle = 5; 12 | required float armor_min_area = 6; 13 | required float armor_max_aspect_ratio = 7; 14 | required float armor_max_pixel_val = 8; 15 | required float armor_max_stddev = 9; 16 | required float armor_max_mean = 10; 17 | 18 | required float color_thread = 11; 19 | required float blue_thread = 12; 20 | required float red_thread = 13; 21 | } 22 | 23 | message ArmorSize { 24 | required float width = 1; 25 | required float height = 2; 26 | } 27 | 28 | enum EnemyColor { 29 | BLUE = 0; 30 | RED = 1; 31 | } 32 | 33 | message SignalRecognization { 34 | required uint32 max_wait_fps = 1; 35 | required float min_pulse_angle = 2; 36 | required uint32 min_num = 3; 37 | } 38 | 39 | message ConstraintSetConfig { 40 | required bool enable_debug = 1; 41 | required bool enable_neon = 2; 42 | required bool using_hsv = 3; 43 | required Threshold threshold = 4; 44 | required ArmorSize armor_size = 5; 45 | required EnemyColor enemy_color = 6; 46 | required SignalRecognization signal_recognization = 7; 47 | } 48 | -------------------------------------------------------------------------------- /roborts_detection/armor_detection/gimbal_control.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | 17 | ***************************************************************************/ 18 | #include 19 | #include 20 | 21 | #include "gimbal_control.h" 22 | 23 | namespace roborts_detection { 24 | 25 | void GimbalContrl::Init(float x,float y,float z,float pitch,float yaw, float init_v, float init_k) { 26 | offset_.x = x; 27 | offset_.y = y; 28 | offset_.z = z; 29 | offset_pitch_ = pitch; 30 | offset_yaw_ = yaw; 31 | init_v_ = init_v; 32 | init_k_ = init_k; 33 | } 34 | 35 | //air friction is considered 36 | float GimbalContrl::BulletModel(float x, float v, float angle) { //x:m,v:m/s,angle:rad 37 | float t, y; 38 | t = (float)((exp(init_k_ * x) - 1) / (init_k_ * v * cos(angle))); 39 | y = (float)(v * sin(angle) * t - GRAVITY * t * t / 2); 40 | return y; 41 | } 42 | 43 | //x:distance , y: height 44 | float GimbalContrl::GetPitch(float x, float y, float v) { 45 | float y_temp, y_actual, dy; 46 | float a; 47 | y_temp = y; 48 | // by iteration 49 | for (int i = 0; i < 20; i++) { 50 | a = (float) atan2(y_temp, x); 51 | y_actual = BulletModel(x, v, a); 52 | dy = y - y_actual; 53 | y_temp = y_temp + dy; 54 | if (fabsf(dy) < 0.001) { 55 | break; 56 | } 57 | //printf("iteration num %d: angle %f,temp target y:%f,err of y:%f\n",i+1,a*180/3.1415926535,yTemp,dy); 58 | } 59 | return a; 60 | 61 | } 62 | 63 | void GimbalContrl::Transform(cv::Point3f &postion, float &pitch, float &yaw) { 64 | pitch = 65 | -GetPitch((postion.z + offset_.z) / 100, -(postion.y + offset_.y) / 100, 15) + (float)(offset_pitch_ * 3.1415926535 / 180); 66 | //yaw positive direction :anticlockwise 67 | yaw = -(float) (atan2(postion.x + offset_.x, postion.z + offset_.z)) + (float)(offset_yaw_ * 3.1415926535 / 180); 68 | } 69 | 70 | } // roborts_detection 71 | 72 | 73 | 74 | -------------------------------------------------------------------------------- /roborts_detection/armor_detection/gimbal_control.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | #ifndef ROBORTS_DETECTION_GIMBAL_CONTROL_H 18 | #define ROBORTS_DETECTION_GIMBAL_CONTROL_H 19 | #include 20 | #include 21 | 22 | namespace roborts_detection{ 23 | 24 | const double PI = 3.1415926535; 25 | const float GRAVITY = 9.78; 26 | 27 | 28 | /** 29 | * @brief The class can make a transformation: the 3D position of enemy --> pitch,yaw angle of gimbal. 30 | * For more derails, see projectile_model.pdf 31 | * TODO: add enemy motion estimation 32 | */ 33 | 34 | class GimbalContrl 35 | { 36 | private: 37 | /** 38 | * @brief Calculate the actual y value with air resistance 39 | * @param x the distanc 40 | * @param v Projectile velocity 41 | * @param angle Pitch angle 42 | * @return The actual y value in the gimbal coordinate 43 | */ 44 | float BulletModel(float x,float v,float angle); 45 | /** 46 | * @brief Get the gimbal control angle 47 | * @param x Distance from enemy(the armor selected to shoot) to gimbal 48 | * @param y Value of y in gimbal coordinate. 49 | * @param v Projectile velocity 50 | * @return Gimbal pitch angle 51 | */ 52 | float GetPitch(float x,float y,float v); 53 | 54 | public: 55 | /** 56 | * @brief Init the Transformation matrix from camera to gimbal //TODO: write in ros tf 57 | * @param x Translate x 58 | * @param y Translate y 59 | * @param z Translate z 60 | * @param pitch Rotate pitch 61 | * @param yaw Rotate yaw 62 | */ 63 | void Init(float x,float y,float z,float pitch,float yaw, float init_v, float init_k); 64 | /** 65 | * @brief Get the gimbal control info. 66 | * @param postion Enemy position(actually it should be the target armor). 67 | * @param pitch Input and output actual pitch angle 68 | * @param yaw Input and output actual yaw angle 69 | */ 70 | void Transform(cv::Point3f &postion,float &pitch,float &yaw); 71 | 72 | private: 73 | //! Transformation matrix between camera coordinate system and gimbal coordinate system. 74 | //! Translation unit: cm 75 | cv::Point3f offset_; 76 | //! Rotation matrix unit: degree 77 | float offset_pitch_; 78 | float offset_yaw_; 79 | 80 | //! Initial value 81 | float init_v_; 82 | float init_k_; 83 | 84 | }; 85 | 86 | } //namespace roborts_detection 87 | 88 | #endif //ROBORTS_DETECTION_GIMBAL_CONTROL_H 89 | -------------------------------------------------------------------------------- /roborts_detection/armor_detection/proto/armor_detection.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto2"; 2 | package roborts_detection; 3 | 4 | message CameraGimbalTransform{ 5 | required float offset_x = 1; 6 | required float offset_y = 2; 7 | required float offset_z = 3; 8 | required float offset_pitch = 4; 9 | required float offset_yaw = 5; 10 | } 11 | 12 | message ProjectileModelInfo { 13 | optional float init_v = 1; 14 | optional float init_k = 2; 15 | } 16 | 17 | message ArmorDetectionAlgorithms { 18 | repeated string name = 1; 19 | optional string selected_algorithm = 2; 20 | optional uint32 undetected_armor_delay = 3; 21 | optional string camera_name = 4; 22 | required CameraGimbalTransform camera_gimbal_transform= 5; 23 | optional ProjectileModelInfo projectile_model_info = 6; 24 | } 25 | -------------------------------------------------------------------------------- /roborts_detection/package.xml: -------------------------------------------------------------------------------- 1 | 2 | roborts_detection 3 | 1.0.0 4 | 5 | The roborts_detection pakcage provides object detection for RoboMaster AI Robot 6 | 7 | RoboMaster 8 | RoboMaster 9 | GPL 3.0 10 | https://github.com/RoboMaster/RoboRTS 11 | 12 | catkin 13 | 14 | actionlib 15 | roscpp 16 | rospy 17 | roborts_common 18 | roborts_msgs 19 | roborts_camera 20 | 21 | actionlib 22 | roscpp 23 | rospy 24 | roborts_common 25 | roborts_msgs 26 | roborts_camera 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /roborts_detection/util/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(detection_util) 2 | 3 | add_library(cv_toolbox INTERFACE) 4 | target_sources(cv_toolbox INTERFACE "${CMAKE_CURRENT_SOURCE_DIR}") 5 | target_link_libraries(cv_toolbox INTERFACE ${catkin_LIBRARIES} 6 | ) 7 | target_include_directories(cv_toolbox 8 | INTERFACE 9 | ${catkin_INCLUDE_DIRS} 10 | ) 11 | add_library(detection::tool ALIAS cv_toolbox) -------------------------------------------------------------------------------- /roborts_localization/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(roborts_localization) 2 | cmake_minimum_required(VERSION 3.1) 3 | 4 | set(CMAKE_BUILD_TYPE Release) 5 | set(CMAKE_CXX_STANDARD 14) 6 | 7 | set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake_module) 8 | 9 | find_package(Threads REQUIRED) 10 | find_package(Eigen3 REQUIRED) 11 | find_package(Glog REQUIRED) 12 | find_package(catkin REQUIRED COMPONENTS 13 | roscpp 14 | tf 15 | nav_msgs 16 | message_filters 17 | sensor_msgs 18 | ) 19 | catkin_package() 20 | 21 | add_subdirectory(amcl) 22 | 23 | include_directories( 24 | src 25 | ) 26 | 27 | add_executable( 28 | localization_node 29 | localization_node.cpp 30 | localization_node.h 31 | localization_math.cpp 32 | ) 33 | 34 | target_link_libraries(localization_node 35 | PUBLIC 36 | localization::amcl 37 | ${EIGEN3_LIBRARIES} 38 | ${catkin_LIBRARIES} 39 | ${GLOG_LIBRARY} 40 | ) 41 | 42 | target_include_directories(localization_node PUBLIC 43 | ${catkin_INCLUDE_DIRS}) 44 | -------------------------------------------------------------------------------- /roborts_localization/amcl/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(amcl) 2 | 3 | #find_package(catkin REQUIRED COMPONENTS 4 | # roscpp 5 | # tf 6 | # nav_msgs 7 | # message_filters 8 | # sensor_msgs 9 | # ) 10 | 11 | add_library(amcl 12 | amcl.cpp 13 | amcl.h 14 | amcl_config.h 15 | ${CMAKE_CURRENT_SOURCE_DIR}/map/amcl_map.cpp 16 | ${CMAKE_CURRENT_SOURCE_DIR}/particle_filter/particle_filter.cpp 17 | ${CMAKE_CURRENT_SOURCE_DIR}/particle_filter/particle_filter_gaussian_pdf.cpp 18 | ${CMAKE_CURRENT_SOURCE_DIR}/particle_filter/particle_filter_kdtree.cpp 19 | ${CMAKE_CURRENT_SOURCE_DIR}/particle_filter/particle_filter_sample.h 20 | ${CMAKE_CURRENT_SOURCE_DIR}/sensors/sensor_laser.cpp 21 | ${CMAKE_CURRENT_SOURCE_DIR}/sensors/sensor_odom.cpp) 22 | 23 | target_include_directories(${PROJECT_NAME} 24 | PUBLIC 25 | ${CMAKE_CURRENT_SOURCE_DIR} 26 | ${CMAKE_CURRENT_SOURCE_DIR}/../ 27 | ${EIGEN3_INCLUDE_DIR} 28 | ${catkin_INCLUDE_DIRS}) 29 | 30 | target_link_libraries(${PROJECT_NAME} 31 | PUBLIC 32 | ${EIGEN3_LIBRARIES} 33 | ${catkin_LIBRARIES} 34 | ${GLOG_LIBRARY} 35 | ) 36 | 37 | add_library(localization::amcl ALIAS amcl) 38 | -------------------------------------------------------------------------------- /roborts_localization/amcl/config/amcl.yaml: -------------------------------------------------------------------------------- 1 | use_map_topic : true 2 | first_map_only : false 3 | gui_publish_rate : 10 4 | 5 | laser_min_range : 0.15 6 | laser_max_range : 8.0 7 | laser_max_beams : 30 8 | 9 | min_particles : 70 10 | max_particles : 5000 11 | 12 | kld_err : 0.05 13 | kld_z : 0.99 14 | 15 | laser_model : LASER_MODEL_LIKELIHOOD_FIELD_PROB 16 | z_hit : 0.5 17 | z_rand : 0.5 18 | sigma_hit : 0.2 19 | lambda_short : 0.1 20 | laser_likelihood_max_dist : 5.0 21 | do_beamskip : false 22 | beam_skip_distance : 0.5 23 | beam_skip_threshold : 0.3 24 | beam_skip_error_threshold : 0.9 25 | 26 | 27 | odom_model : ODOM_MODEL_OMNI 28 | odom_alpha1 : 0.005 29 | odom_alpha2 : 0.005 30 | odom_alpha3 : 0.01 31 | odom_alpha4 : 0.005 32 | odom_alpha5 : 0.003 33 | 34 | update_min_d : 0.05 35 | update_min_a : 0.03 36 | 37 | resample_interval : 1 38 | transform_tolerance : 0.1 39 | recovery_alpha_slow : 0.001 40 | recovery_alpha_fast : 0.1 41 | 42 | use_global_localization : false 43 | random_heading : true 44 | laser_filter_weight : 0.4 45 | 46 | max_uwb_particles : 10 47 | uwb_cov_x : 0.06 48 | uwb_cov_y : 0.06 49 | resample_uwb_factor : 4.0 50 | -------------------------------------------------------------------------------- /roborts_localization/amcl/particle_filter/particle_filter_gaussian_pdf.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of  11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | 18 | /* 19 | * Player - One Hell of a Robot Server 20 | * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 21 | * gerkey@usc.edu kaspers@robotics.usc.edu 22 | * 23 | * This library is free software; you can redistribute it and/or 24 | * modify it under the terms of the GNU Lesser General Public 25 | * License as published by the Free Software Foundation; either 26 | * version 2.1 of the License, or (at your option) any later version. 27 | * 28 | * This library is distrmathibuted in the hope that it will be useful, 29 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 30 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 31 | * Lesser General Public License for more details. 32 | * 33 | * You should have received a copy of the GNU Lesser General Public 34 | * License along with this library; if not, write to the Free Software 35 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 36 | * 37 | */ 38 | 39 | #include "particle_filter_gaussian_pdf.h" 40 | 41 | namespace roborts_localization { 42 | 43 | ParticleFilterGaussianPdf::ParticleFilterGaussianPdf(const Vec3d &mean, 44 | const Mat3d &covariance) { 45 | this->mean_ = mean; 46 | this->covariance_ = covariance; 47 | math::EigenDecomposition(this->covariance_, this->covariance_rotation_, this->covariance_diagonal_); 48 | } 49 | 50 | Vec3d ParticleFilterGaussianPdf::GenerateSample() { 51 | int i = 0; 52 | Vec3d random_vec; 53 | Vec3d mean_vec; 54 | 55 | for (i = 0; i < this->covariance_diagonal_.size(); i++) { 56 | double sigma = this->covariance_diagonal_(i); 57 | random_vec(i) = math::RandomGaussianNumByStdDev(sigma); 58 | } 59 | 60 | for (i = 0; i < this->mean_.size(); i++) { 61 | mean_vec(i) = this->mean_(i); 62 | for (int j = 0; j < this->mean_.size(); ++j) { 63 | mean_vec(i) += this->covariance_rotation_(i, j) * random_vec(j); 64 | } 65 | } 66 | 67 | return mean_vec; 68 | } 69 | 70 | }// roborts_localization 71 | 72 | 73 | -------------------------------------------------------------------------------- /roborts_localization/amcl/particle_filter/particle_filter_gaussian_pdf.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of  11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | 18 | /* 19 | * Player - One Hell of a Robot Server 20 | * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 21 | * gerkey@usc.edu kaspers@robotics.usc.edu 22 | * 23 | * This library is free software; you can redistribute it and/or 24 | * modify it under the terms of the GNU Lesser General Public 25 | * License as published by the Free Software Foundation; either 26 | * version 2.1 of the License, or (at your option) any later version. 27 | * 28 | * This library is distributed in the hope that it will be useful, 29 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 30 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 31 | * Lesser General Public License for more details. 32 | * 33 | * You should have received a copy of the GNU Lesser General Public 34 | * License along with this library; if not, write to the Free Software 35 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 36 | * 37 | */ 38 | 39 | #ifndef ROBORTS_LOCALIZATION_AMCL_PARTICLE_FILTER_PARTICLE_FILTER_GAUSSIAN_PDF_H 40 | #define ROBORTS_LOCALIZATION_AMCL_PARTICLE_FILTER_PARTICLE_FILTER_GAUSSIAN_PDF_H 41 | 42 | #include 43 | 44 | #include "localization_math.h" 45 | #include "log.h" 46 | 47 | namespace roborts_localization { 48 | 49 | /** 50 | * @brief Gaussian pdf class 51 | */ 52 | class ParticleFilterGaussianPdf { 53 | 54 | public: 55 | /** 56 | * @brief Default constructor 57 | */ 58 | ParticleFilterGaussianPdf() = default; 59 | /** 60 | * @brief Create a gaussian pdf by mean and covariance 61 | * @param mean Mean to initialize the gaussian pdf 62 | * @param covariance Covariance initialize the gaussian pdf 63 | */ 64 | ParticleFilterGaussianPdf(const Vec3d &mean, const Mat3d &covariance); 65 | 66 | /** 67 | * @brief Generate random pose particle sample 68 | * @return Return the random pose particle sample 69 | */ 70 | Vec3d GenerateSample(); 71 | 72 | private: 73 | 74 | Vec3d mean_; 75 | Mat3d covariance_; 76 | 77 | // Decomposed covariance matrix (rotation * diagonal) 78 | Mat3d covariance_rotation_; 79 | Vec3d covariance_diagonal_; 80 | 81 | }; 82 | 83 | }// roborts_localization 84 | 85 | #endif //ROBORTS_LOCALIZATION_AMCL_PARTICLE_FILTER_PARTICLE_FILTER_GAUSSIAN_PDF_H 86 | -------------------------------------------------------------------------------- /roborts_localization/cmake_module/FindGlog.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Glog 2 | # 3 | # The following variables are optionally searched for defaults 4 | # GLOG_ROOT_DIR: Base directory where all GLOG components are found 5 | # 6 | # The following are set after configuration is done: 7 | # GLOG_FOUND 8 | # GLOG_INCLUDE_DIRS 9 | # GLOG_LIBRARIES 10 | # GLOG_LIBRARYRARY_DIRS 11 | 12 | include(FindPackageHandleStandardArgs) 13 | 14 | set(GLOG_ROOT_DIR "" CACHE PATH "Folder contains Google glog") 15 | 16 | if(WIN32) 17 | find_path(GLOG_INCLUDE_DIR glog/logging.h 18 | PATHS ${GLOG_ROOT_DIR}/src/windows) 19 | else() 20 | find_path(GLOG_INCLUDE_DIR glog/logging.h 21 | PATHS ${GLOG_ROOT_DIR}) 22 | endif() 23 | 24 | if(MSVC) 25 | find_library(GLOG_LIBRARY_RELEASE libglog_static 26 | PATHS ${GLOG_ROOT_DIR} 27 | PATH_SUFFIXES Release) 28 | 29 | find_library(GLOG_LIBRARY_DEBUG libglog_static 30 | PATHS ${GLOG_ROOT_DIR} 31 | PATH_SUFFIXES Debug) 32 | 33 | set(GLOG_LIBRARY optimized ${GLOG_LIBRARY_RELEASE} debug ${GLOG_LIBRARY_DEBUG}) 34 | else() 35 | find_library(GLOG_LIBRARY glog 36 | PATHS ${GLOG_ROOT_DIR} 37 | PATH_SUFFIXES lib lib64) 38 | endif() 39 | 40 | find_package_handle_standard_args(Glog DEFAULT_MSG GLOG_INCLUDE_DIR GLOG_LIBRARY) 41 | 42 | if(GLOG_FOUND) 43 | set(GLOG_INCLUDE_DIRS ${GLOG_INCLUDE_DIR}) 44 | set(GLOG_LIBRARIES ${GLOG_LIBRARY}) 45 | message(STATUS "Found glog (include: ${GLOG_INCLUDE_DIR}, library: ${GLOG_LIBRARY})") 46 | mark_as_advanced(GLOG_ROOT_DIR GLOG_LIBRARY_RELEASE GLOG_LIBRARY_DEBUG 47 | GLOG_LIBRARY GLOG_INCLUDE_DIR) 48 | endif() 49 | -------------------------------------------------------------------------------- /roborts_localization/config/localization.yaml: -------------------------------------------------------------------------------- 1 | odom_frame : "odom" 2 | base_frame : "base_link" 3 | global_frame : "map" 4 | laser_topic_name : "scan" 5 | map_topic_name : "map" 6 | init_pose_topic_name : "initialpose" 7 | transform_tolerance : 0.1 8 | #8.40 #1.0 9 | #14.35 #1.0 10 | initial_pose_x : 1 11 | initial_pose_y : 1 12 | initial_pose_a : 3.14 13 | initial_cov_xx : 0.1 14 | initial_cov_yy : 0.1 15 | initial_cov_aa : 0.1 16 | 17 | enable_uwb : false 18 | uwb_frame_id : "uwb" 19 | uwb_topic_name : "uwb" 20 | use_sim_uwb : false 21 | uwb_correction_frequency : 20 22 | -------------------------------------------------------------------------------- /roborts_localization/localization_config.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of  11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | 18 | #include 19 | 20 | #ifndef ROBORTS_LOCALIZATION_LOCALIZATION_CONFIG_H 21 | #define ROBORTS_LOCALIZATION_LOCALIZATION_CONFIG_H 22 | 23 | namespace roborts_localization { 24 | 25 | // Get Parameters from ROS parameter server 26 | struct LocalizationConfig { 27 | void GetParam(ros::NodeHandle *nh) { 28 | nh->param("odom_frame", odom_frame_id, "odom"); 29 | nh->param("base_frame", base_frame_id, "base_link"); 30 | nh->param("global_frame", global_frame_id, "map"); 31 | nh->param("laser_topic_name", laser_topic_name, "scan"); 32 | nh->param("map_topic_name", map_topic_name, "map"); 33 | nh->param("init_pose_topic_name", init_pose_topic_name, "initialpose"); 34 | nh->param("transform_tolerance", transform_tolerance, 0.1); 35 | nh->param("initial_pose_x", initial_pose_x, 1); 36 | nh->param("initial_pose_y", initial_pose_y, 1); 37 | nh->param("initial_pose_a", initial_pose_a, 0); 38 | nh->param("initial_cov_xx", initial_cov_xx, 0.1); 39 | nh->param("initial_cov_yy", initial_cov_yy, 0.1); 40 | nh->param("initial_cov_aa", initial_cov_aa, 0.1); 41 | nh->param("enable_uwb", enable_uwb, false); 42 | nh->param("uwb_frame_id", uwb_frame_id, "uwb"); 43 | nh->param("uwb_topic_name", uwb_topic_name, "uwb"); 44 | nh->param("use_sim_uwb", use_sim_uwb, false); 45 | nh->param("uwb_correction_frequency", uwb_correction_frequency, 20); 46 | nh->param("publish_visualize", publish_visualize, true); 47 | } 48 | std::string odom_frame_id; 49 | std::string base_frame_id; 50 | std::string global_frame_id; 51 | 52 | std::string laser_topic_name; 53 | std::string map_topic_name; 54 | std::string init_pose_topic_name; 55 | 56 | double transform_tolerance; 57 | 58 | double initial_pose_x; 59 | double initial_pose_y; 60 | double initial_pose_a; 61 | double initial_cov_xx; 62 | double initial_cov_yy; 63 | double initial_cov_aa; 64 | 65 | bool publish_visualize; 66 | 67 | bool enable_uwb; 68 | std::string uwb_frame_id; 69 | std::string uwb_topic_name; 70 | bool use_sim_uwb; 71 | int uwb_correction_frequency; 72 | 73 | }; 74 | 75 | }// roborts_localization 76 | 77 | #endif //ROBORTS_LOCALIZATION_LOCALIZATION_CONFIG_H 78 | -------------------------------------------------------------------------------- /roborts_localization/log.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of  11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | 18 | #ifndef ROBORTS_LOCALIZATION_LOG_H 19 | #define ROBORTS_LOCALIZATION_LOG_H 20 | 21 | #include 22 | #include "glog/logging.h" 23 | #include "glog/raw_logging.h" 24 | #include 25 | 26 | //#define DLOG_INFO std::cout 27 | //#define DLOG_ERROR std::cout 28 | 29 | // GLog Wrapper 30 | namespace roborts_localization { 31 | 32 | #define LOG_INFO LOG(INFO) 33 | #define LOG_WARNING LOG(WARNING) 34 | #define LOG_ERROR LOG(ERROR) 35 | #define LOG_FATAL LOG(FATAL) 36 | 37 | #define LOG_INFO_IF(condition) LOG_IF(INFO,condition) 38 | #define LOG_WARNING_IF(condition) LOG_IF(WARNING,condition) 39 | #define LOG_ERROR_IF(condition) LOG_IF(ERROR,condition) 40 | #define LOG_FATAL_IF(condition) LOG_IF(FATAL,condition) 41 | 42 | #define LOG_INFO_EVERY(freq) LOG_EVERY_N(INFO, freq) 43 | #define LOG_WARNING_EVERY(freq) LOG_EVERY_N(WARNING, freq) 44 | #define LOG_ERROR_EVERY(freq) LOG_EVERY_N(ERROR, freq) 45 | 46 | #define DLOG_INFO DLOG(INFO) 47 | #define DLOG_WARNING DLOG(WARNING) 48 | #define DLOG_ERROR DLOG(WARNING) 49 | 50 | #define LOG_WARNING_FIRST_N(times) LOG_FIRST_N(WARNING, times) 51 | 52 | class GLogWrapper { 53 | public: 54 | GLogWrapper(char *program) { 55 | google::InitGoogleLogging(program); 56 | FLAGS_stderrthreshold = google::WARNING; 57 | FLAGS_colorlogtostderr = true; 58 | FLAGS_v = 3; 59 | google::InstallFailureSignalHandler(); 60 | } 61 | 62 | ~GLogWrapper() { 63 | google::ShutdownGoogleLogging(); 64 | } 65 | }; 66 | 67 | }// roborts_localization 68 | 69 | #endif //ROBORTS_LOCALIZATION_LOG_H 70 | -------------------------------------------------------------------------------- /roborts_localization/package.xml: -------------------------------------------------------------------------------- 1 | 2 | roborts_localization 3 | 1.0.0 4 | 5 | The roborts localization package provides localization for RoboMaster AI Robot 6 | 7 | RoboMaster 8 | RoboMaster 9 | GPL 3.0 10 | https://github.com/RoboMaster/RoboRTS 11 | 12 | catkin 13 | 14 | roscpp 15 | rospy 16 | tf 17 | nav_msgs 18 | sensor_msgs 19 | message_filters 20 | 21 | roscpp 22 | rospy 23 | tf 24 | nav_msgs 25 | sensor_msgs 26 | message_filters 27 | 28 | 29 | -------------------------------------------------------------------------------- /roborts_localization/types.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of  11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | 18 | #ifndef ROBORTS_LOCALIZATION_TYPES_H 19 | #define ROBORTS_LOCALIZATION_TYPES_H 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | namespace roborts_localization { 28 | 29 | // Use eigen3 as base data structure 30 | using Vec3d = Eigen::Vector3d; 31 | using Vec4d = Eigen::Vector4d; 32 | 33 | using Mat3d = Eigen::Matrix3d; 34 | using Mat2d = Eigen::Matrix2d; 35 | 36 | using MatX2d = Eigen::MatrixX2d; 37 | 38 | }// roborts_localization 39 | 40 | #endif //ROBORTS_LOCALIZATION_TYPES_H 41 | -------------------------------------------------------------------------------- /roborts_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(roborts_msgs) 2 | cmake_minimum_required(VERSION 3.1) 3 | set(CMAKE_CXX_STANDARD 14) 4 | set(CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}) 5 | find_package(catkin REQUIRED COMPONENTS 6 | std_msgs 7 | nav_msgs 8 | geometry_msgs 9 | actionlib 10 | message_generation 11 | ) 12 | 13 | add_action_files( 14 | DIRECTORY action 15 | FILES 16 | LocalPlanner.action 17 | GlobalPlanner.action 18 | ArmorDetection.action 19 | ) 20 | 21 | add_message_files( 22 | DIRECTORY msg 23 | FILES 24 | TwistAccel.msg 25 | GimbalAngle.msg 26 | GimbalRate.msg 27 | ObstacleMsg.msg 28 | ShootInfo.msg 29 | ShootState.msg 30 | ) 31 | 32 | add_message_files( 33 | DIRECTORY msg/referee_system 34 | FILES 35 | BonusStatus.msg 36 | GameResult.msg 37 | GameStatus.msg 38 | GameSurvivor.msg 39 | ProjectileSupply.msg 40 | RobotBonus.msg 41 | RobotDamage.msg 42 | RobotHeat.msg 43 | RobotShoot.msg 44 | RobotStatus.msg 45 | SupplierStatus.msg 46 | ) 47 | 48 | add_service_files( 49 | DIRECTORY srv 50 | FILES 51 | GimbalMode.srv 52 | FricWhl.srv 53 | ShootCmd.srv 54 | ) 55 | 56 | generate_messages( 57 | DEPENDENCIES 58 | std_msgs 59 | geometry_msgs 60 | nav_msgs 61 | ) 62 | 63 | catkin_package() 64 | -------------------------------------------------------------------------------- /roborts_msgs/action/ArmorDetection.action: -------------------------------------------------------------------------------- 1 | #Send a flag to server to control the thread start, pause, restart and stop 2 | #command == 1 start 3 | #command == 2 pause 4 | #command == 3 stop 5 | int32 command 6 | --- 7 | float32 result 8 | --- 9 | #feedback 10 | bool detected 11 | int32 error_code 12 | string error_msg 13 | geometry_msgs/PoseStamped enemy_pos 14 | -------------------------------------------------------------------------------- /roborts_msgs/action/GlobalPlanner.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | int32 command 3 | geometry_msgs/PoseStamped goal 4 | --- 5 | #result definition RUNNING = 0, SUCCESS = 1, FAILURE = 2 6 | int32 error_code 7 | --- 8 | #feedback 9 | nav_msgs/Path path 10 | int32 error_code 11 | string error_msg -------------------------------------------------------------------------------- /roborts_msgs/action/LocalPlanner.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | nav_msgs/Path route 3 | --- 4 | #result definition RUNNING = 0, SUCCESS = 1, FAILURE = 2 5 | int32 error_code 6 | --- 7 | #feedback 8 | int32 error_code 9 | string error_msg -------------------------------------------------------------------------------- /roborts_msgs/msg/GimbalAngle.msg: -------------------------------------------------------------------------------- 1 | #gimbal feedback angle data 2 | bool yaw_mode 3 | bool pitch_mode 4 | float64 yaw_angle 5 | float64 pitch_angle 6 | -------------------------------------------------------------------------------- /roborts_msgs/msg/GimbalRate.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | float64 pitch_rate 3 | float64 yaw_rate 4 | -------------------------------------------------------------------------------- /roborts_msgs/msg/ObstacleMsg.msg: -------------------------------------------------------------------------------- 1 | # Message that contains a list of polygon shaped obstacles. 2 | # Special types: 3 | # Polygon with 1 vertex: Point obstacle 4 | # Polygon with 2 vertices: Line obstacle 5 | # Polygon with more than 2 vertices: First and last points are assumed to be connected 6 | # 7 | # If optional properties (orientaions or velocities) are provided, 8 | # each container size must match the number of obstacles 9 | # otherwise let them empty. 10 | 11 | 12 | std_msgs/Header header 13 | 14 | # Actual obstacle positions (polygon descriptions) 15 | geometry_msgs/PolygonStamped[] obstacles 16 | 17 | # Obstacle IDs [optional] 18 | # Specify IDs in order to provide (temporal) relationships 19 | # between obstacles among multiple messages. 20 | # If provided it must be size(ids) = size(obstacles) 21 | uint32[] ids 22 | 23 | # Individual orientations (centroid) [optional] 24 | # If provided it must be size(orientations) = size(obstacles) 25 | geometry_msgs/QuaternionStamped[] orientations 26 | 27 | # Individual velocities (centroid) [optional] 28 | # If provided it must be size(velocities) = size(obstacles) 29 | geometry_msgs/TwistWithCovariance[] velocities 30 | 31 | -------------------------------------------------------------------------------- /roborts_msgs/msg/ShootInfo.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | #Remain bullets number 4 | int16 remain_bullet 5 | 6 | #Bullets fired number 7 | int16 sent_bullet 8 | 9 | #Friction running status 10 | bool fric_wheel_run -------------------------------------------------------------------------------- /roborts_msgs/msg/ShootState.msg: -------------------------------------------------------------------------------- 1 | int32 single_shoot 2 | int32 continue_shoot 3 | int32 run_friction_whell 4 | int32 friction_whell_speed -------------------------------------------------------------------------------- /roborts_msgs/msg/TwistAccel.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/Twist twist 2 | geometry_msgs/Accel accel -------------------------------------------------------------------------------- /roborts_msgs/msg/referee_system/BonusStatus.msg: -------------------------------------------------------------------------------- 1 | #bonus zone status 2 | uint8 UNOCCUPIED = 0 3 | uint8 BEING_OCCUPIED= 1 4 | uint8 OCCUPIED = 2 5 | uint8 red_bonus 6 | uint8 blue_bonus -------------------------------------------------------------------------------- /roborts_msgs/msg/referee_system/GameResult.msg: -------------------------------------------------------------------------------- 1 | #game result 2 | uint8 DRAW=0 3 | uint8 RED_WIN=1 4 | uint8 BLUE_WIN=2 5 | uint8 result 6 | -------------------------------------------------------------------------------- /roborts_msgs/msg/referee_system/GameStatus.msg: -------------------------------------------------------------------------------- 1 | #game status 2 | uint8 PRE_MATCH = 0 3 | uint8 SETUP = 1 4 | uint8 INIT = 2 5 | uint8 FIVE_SEC_CD = 3 6 | uint8 ROUND = 4 7 | uint8 CALCULATION = 5 8 | uint8 game_status 9 | uint16 remaining_time 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /roborts_msgs/msg/referee_system/GameSurvivor.msg: -------------------------------------------------------------------------------- 1 | #robot survival 2 | bool red3 3 | bool red4 4 | bool blue3 5 | bool blue4 6 | -------------------------------------------------------------------------------- /roborts_msgs/msg/referee_system/ProjectileSupply.msg: -------------------------------------------------------------------------------- 1 | #projectile supply 2 | uint8 number 3 | -------------------------------------------------------------------------------- /roborts_msgs/msg/referee_system/RobotBonus.msg: -------------------------------------------------------------------------------- 1 | #robot bonus 2 | bool bonus 3 | -------------------------------------------------------------------------------- /roborts_msgs/msg/referee_system/RobotDamage.msg: -------------------------------------------------------------------------------- 1 | #robot damage 2 | uint8 ARMOR = 0 3 | uint8 OFFLINE = 1 4 | uint8 EXCEED_HEAT = 2 5 | uint8 EXCEED_POWER = 3 6 | uint8 damage_type 7 | uint8 FORWARD = 0 8 | uint8 LEFT = 1 9 | uint8 BACKWARD = 2 10 | uint8 RIGHT = 3 11 | uint8 damage_source 12 | 13 | 14 | -------------------------------------------------------------------------------- /roborts_msgs/msg/referee_system/RobotHeat.msg: -------------------------------------------------------------------------------- 1 | #robot power and heat data 2 | uint16 chassis_volt 3 | uint16 chassis_current 4 | float64 chassis_power 5 | uint16 chassis_power_buffer 6 | uint16 shooter_heat 7 | 8 | -------------------------------------------------------------------------------- /roborts_msgs/msg/referee_system/RobotShoot.msg: -------------------------------------------------------------------------------- 1 | #robot shoot data 2 | uint8 frequency 3 | float64 speed 4 | -------------------------------------------------------------------------------- /roborts_msgs/msg/referee_system/RobotStatus.msg: -------------------------------------------------------------------------------- 1 | #robot status 2 | uint8 id 3 | uint8 level 4 | uint16 remain_hp 5 | uint16 max_hp 6 | uint16 heat_cooling_limit 7 | uint16 heat_cooling_rate 8 | bool gimbal_output 9 | bool chassis_output 10 | bool shooter_output -------------------------------------------------------------------------------- /roborts_msgs/msg/referee_system/SupplierStatus.msg: -------------------------------------------------------------------------------- 1 | #supplier status 2 | uint8 CLOSE = 0 3 | uint8 PREPARING = 1 4 | uint8 SUPPLYING = 2 5 | uint8 status 6 | 7 | -------------------------------------------------------------------------------- /roborts_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | roborts_msgs 3 | 1.0.0 4 | 5 | The roborts msgs package provides all the messages for RoboMaster AI Robot 6 | 7 | RoboMaster 8 | RoboMaster 9 | GPL 3.0 10 | https://github.com/RoboMaster/RoboRTS 11 | 12 | catkin 13 | 14 | actionlib 15 | std_msgs 16 | geometry_msgs 17 | nav_msgs 18 | sensor_msgs 19 | message_generation 20 | 21 | 22 | actionlib 23 | std_msgs 24 | geometry_msgs 25 | nav_msgs 26 | sensor_msgs 27 | message_runtime 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /roborts_msgs/srv/FricWhl.srv: -------------------------------------------------------------------------------- 1 | bool open 2 | --- 3 | bool received 4 | -------------------------------------------------------------------------------- /roborts_msgs/srv/GimbalMode.srv: -------------------------------------------------------------------------------- 1 | uint8 gimbal_mode 2 | --- 3 | bool received 4 | -------------------------------------------------------------------------------- /roborts_msgs/srv/ShootCmd.srv: -------------------------------------------------------------------------------- 1 | uint8 STOP = 0 2 | uint8 ONCE = 1 3 | uint8 CONTINUOUS = 2 4 | uint8 mode 5 | uint8 number 6 | --- 7 | bool received 8 | -------------------------------------------------------------------------------- /roborts_planning/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(roborts_planning) 2 | cmake_minimum_required(VERSION 3.1) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake_module) 6 | set(CMAKE_BUILD_TYPE Release) 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | roscpp 10 | tf 11 | pcl_conversions 12 | pcl_ros 13 | nav_msgs 14 | geometry_msgs 15 | actionlib 16 | roborts_common 17 | roborts_msgs 18 | interactive_markers 19 | roborts_costmap 20 | ) 21 | 22 | find_package(G2O REQUIRED) 23 | find_package(SUITESPARSE REQUIRED) 24 | find_package(Eigen3 REQUIRED) 25 | find_package(PCL 1.7 REQUIRED) 26 | find_package(ProtoBuf REQUIRED) 27 | find_package(Boost REQUIRED) 28 | 29 | catkin_package() 30 | 31 | add_subdirectory(global_planner) 32 | add_subdirectory(local_planner) 33 | 34 | 35 | -------------------------------------------------------------------------------- /roborts_planning/global_planner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(global_planner) 2 | 3 | add_subdirectory(a_star_planner) 4 | set(CMAKE_BUILD_TYPE Release) 5 | file(GLOB ProtoFiles "${CMAKE_CURRENT_SOURCE_DIR}/proto/*.proto") 6 | rrts_protobuf_generate_cpp(${CMAKE_CURRENT_SOURCE_DIR}/proto GlobalPlannerProtoSrc GlobalPlannerProtoHds ${ProtoFiles}) 7 | 8 | include_directories(${catkin_INCLUDE_DIRS}) 9 | 10 | add_executable(${PROJECT_NAME}_node 11 | ${GlobalPlannerProtoSrc} 12 | ${GlobalPlannerProtoHds} 13 | global_planner_node.cpp 14 | ) 15 | target_link_libraries(${PROJECT_NAME}_node 16 | PRIVATE 17 | planning::global_planner::a_star_planner 18 | roborts_costmap 19 | ${catkin_LIBRARIES} 20 | ${Boost_LIBRARIES} 21 | ${GLOG_LIBRARY} 22 | ) 23 | add_dependencies(${PROJECT_NAME}_node 24 | roborts_msgs_generate_messages) 25 | 26 | add_executable(${PROJECT_NAME}_test 27 | global_planner_test.cpp) 28 | target_link_libraries(${PROJECT_NAME}_test 29 | PRIVATE 30 | ${catkin_LIBRARIES} 31 | ) 32 | add_dependencies(${PROJECT_NAME}_test 33 | roborts_msgs_generate_messages) 34 | 35 | 36 | -------------------------------------------------------------------------------- /roborts_planning/global_planner/a_star_planner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(a_star_planner) 2 | set(CMAKE_BUILD_TYPE Release) 3 | file(GLOB ProtoFiles "${CMAKE_CURRENT_SOURCE_DIR}/proto/*.proto") 4 | rrts_protobuf_generate_cpp(${CMAKE_CURRENT_SOURCE_DIR}/proto AStarPlannerConfigProtoSrc AStarPlannerConfigProtoHds ${ProtoFiles}) 5 | 6 | include_directories(${catkin_INCLUDE_DIRS}) 7 | 8 | add_library(${PROJECT_NAME} 9 | SHARED 10 | ${AStarPlannerConfigProtoSrc} 11 | ${AStarPlannerConfigProtoHds} 12 | a_star_planner.cpp 13 | ) 14 | target_link_libraries(${PROJECT_NAME} 15 | PUBLIC 16 | roborts_costmap 17 | ) 18 | add_library(planning::global_planner::${PROJECT_NAME} ALIAS ${PROJECT_NAME}) 19 | -------------------------------------------------------------------------------- /roborts_planning/global_planner/a_star_planner/config/a_star_planner_config.prototxt: -------------------------------------------------------------------------------- 1 | inaccessible_cost: 253 2 | heuristic_factor: 1.0 3 | goal_search_tolerance: 0.45 -------------------------------------------------------------------------------- /roborts_planning/global_planner/a_star_planner/proto/a_star_planner_config.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto2"; 2 | package roborts_global_planner; 3 | 4 | message AStarPlannerConfig { 5 | optional uint32 inaccessible_cost = 1 [default = 253]; 6 | optional float heuristic_factor = 2 [default = 1.0]; 7 | optional float goal_search_tolerance = 3 [default = 0.25]; 8 | } 9 | -------------------------------------------------------------------------------- /roborts_planning/global_planner/config/global_planner_config.prototxt: -------------------------------------------------------------------------------- 1 | name: "a_star_planner" 2 | selected_algorithm: "a_star_planner" 3 | frequency: 3 4 | max_retries: 5 5 | goal_distance_tolerance: 0.15 6 | goal_angle_tolerance: 0.15 7 | -------------------------------------------------------------------------------- /roborts_planning/global_planner/global_planner_algorithms.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of  11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | #ifndef ROBORTS_PLANNING_GLOBAL_PLANNER_GLOBAL_PLANNER_ALGORITHM_H 18 | #define ROBORTS_PLANNING_GLOBAL_PLANNER_GLOBAL_PLANNER_ALGORITHM_H 19 | 20 | #include "a_star_planner/a_star_planner.h" 21 | 22 | #endif // ROBORTS_PLANNING_GLOBAL_PLANNER_GLOBAL_PLANNER_ALGORITHM_H -------------------------------------------------------------------------------- /roborts_planning/global_planner/global_planner_base.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of  11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | #ifndef ROBORTS_PLANNING_GLOBAL_PLANNER_GLOBAL_PLANNER_BASE_H 18 | #define ROBORTS_PLANNING_GLOBAL_PLANNER_GLOBAL_PLANNER_BASE_H 19 | 20 | #include "state/error_code.h" 21 | 22 | #include "costmap/costmap_interface.h" 23 | 24 | namespace roborts_global_planner{ 25 | 26 | class GlobalPlannerBase { 27 | public: 28 | typedef std::shared_ptr CostmapPtr; 29 | 30 | GlobalPlannerBase(CostmapPtr costmap_ptr) 31 | : costmap_ptr_(costmap_ptr) { 32 | }; 33 | virtual ~GlobalPlannerBase() = default; 34 | 35 | virtual roborts_common::ErrorInfo Plan(const geometry_msgs::PoseStamped &start, 36 | const geometry_msgs::PoseStamped &goal, 37 | std::vector &path) = 0; 38 | 39 | protected: 40 | CostmapPtr costmap_ptr_; 41 | }; 42 | 43 | } //namespace roborts_global_planner 44 | 45 | 46 | #endif // ROBORTS_PLANNING_GLOBAL_PLANNER_GLOBAL_PLANNER_BASE_H 47 | -------------------------------------------------------------------------------- /roborts_planning/global_planner/global_planner_test.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of  11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | #include "roborts_msgs/GlobalPlannerAction.h" 25 | #include "state/error_code.h" 26 | 27 | using roborts_common::ErrorCode; 28 | 29 | class GlobalPlannerTest{ 30 | public: 31 | GlobalPlannerTest(): 32 | global_planner_actionlib_client_("global_planner_node_action", true){ 33 | ros::NodeHandle rviz_nh("move_base_simple"); 34 | goal_sub_ = rviz_nh.subscribe("goal", 1, 35 | &GlobalPlannerTest::GoalCallback,this); 36 | 37 | global_planner_actionlib_client_.waitForServer(); 38 | } 39 | ~GlobalPlannerTest() = default; 40 | 41 | void GoalCallback(const geometry_msgs::PoseStamped::ConstPtr & goal){ 42 | ROS_INFO("Get new goal."); 43 | command_.goal = *goal; 44 | global_planner_actionlib_client_.sendGoal(command_, 45 | boost::bind(&GlobalPlannerTest::DoneCallback, this, _1, _2), 46 | boost::bind(&GlobalPlannerTest::ActiveCallback, this), 47 | boost::bind(&GlobalPlannerTest::FeedbackCallback, this, _1) 48 | ); 49 | } 50 | 51 | void DoneCallback(const actionlib::SimpleClientGoalState& state, const roborts_msgs::GlobalPlannerResultConstPtr& result){ 52 | ROS_INFO("The goal is done with %s!",state.toString().c_str()); 53 | } 54 | void ActiveCallback() { 55 | ROS_INFO("Action server has recived the goal, the goal is active!"); 56 | } 57 | void FeedbackCallback(const roborts_msgs::GlobalPlannerFeedbackConstPtr& feedback){ 58 | if (feedback->error_code != ErrorCode::OK) { 59 | ROS_INFO("%s", feedback->error_msg.c_str()); 60 | } 61 | if (!feedback->path.poses.empty()) { 62 | ROS_INFO("Get Path!"); 63 | } 64 | } 65 | private: 66 | ros::Subscriber goal_sub_; 67 | roborts_msgs::GlobalPlannerGoal command_; 68 | actionlib::SimpleActionClient global_planner_actionlib_client_; 69 | }; 70 | 71 | int main(int argc, char **argv) { 72 | ros::init(argc, argv, "global_planner_test"); 73 | GlobalPlannerTest global_planner_test; 74 | ros::spin(); 75 | return 0; 76 | } 77 | 78 | -------------------------------------------------------------------------------- /roborts_planning/global_planner/proto/global_planner_config.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto2"; 2 | package roborts_global_planner; 3 | 4 | message GlobalPlannerConfig { 5 | repeated string name = 1; 6 | optional string selected_algorithm = 2; 7 | required int32 frequency = 3; 8 | required int32 max_retries = 4; 9 | required double goal_distance_tolerance = 5; 10 | required double goal_angle_tolerance = 6; 11 | } 12 | -------------------------------------------------------------------------------- /roborts_planning/local_planner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0) 2 | project(local_planner) 3 | 4 | 5 | set(EXTERNAL_INCLUDE_DIRS 6 | ${EIGEN3_INCLUDE_DIR} 7 | ${SUITESPARSE_INCLUDE_DIRS} 8 | ${G2O_INCLUDE_DIR} 9 | ${catkin_INCLUDE_DIRS} 10 | ) 11 | set(EXTERNAL_LIBS 12 | ${SUITESPARSE_LIBRARIES} 13 | ${G2O_LIBRARIES} 14 | ${catkin_LIBRARIES} 15 | ${PROTOBUF_LIBRARIES} 16 | roborts_costmap 17 | ) 18 | 19 | include_directories(${EXTERNAL_INCLUDE_DIRS} include/ timed_elastic_band/include/) 20 | 21 | add_subdirectory(timed_elastic_band) 22 | 23 | file(GLOB ProtoFiles "${CMAKE_CURRENT_SOURCE_DIR}/include/${PROJECT_NAME}/proto/*.proto") 24 | rrts_protobuf_generate_cpp(${CMAKE_CURRENT_SOURCE_DIR}/include/${PROJECT_NAME}/proto 25 | LocalPlanProtoSrc 26 | LocalPlanProtoHds 27 | ${ProtoFiles} 28 | ) 29 | 30 | aux_source_directory(src/. SRC_LIST) 31 | 32 | #local planner node 33 | add_executable(${PROJECT_NAME}_node 34 | ${LocalPlanProtoSrc} 35 | ${LocalPlanProtoHds} 36 | src/local_planner_node.cpp 37 | src/local_visualization.cpp 38 | src/obstacles.cpp 39 | src/odom_info.cpp 40 | src/robot_position_cost.cpp 41 | ) 42 | 43 | target_link_libraries(${PROJECT_NAME}_node 44 | PRIVATE 45 | planning::timed_elastic_band 46 | ${EXTERNAL_LIBS} 47 | ) 48 | add_dependencies(${PROJECT_NAME}_node 49 | roborts_msgs_generate_messages) 50 | 51 | #local palnner test 52 | add_executable(teb_test 53 | ${LocalPlanProtoSrc} 54 | ${LocalPlanProtoHds} 55 | src/teb_test.cpp 56 | src/local_visualization.cpp 57 | src/obstacles.cpp 58 | src/odom_info.cpp 59 | src/robot_position_cost.cpp 60 | ) 61 | 62 | target_link_libraries(teb_test 63 | PRIVATE 64 | planning::timed_elastic_band 65 | ${EXTERNAL_LIBS} 66 | ) 67 | add_dependencies(teb_test 68 | roborts_msgs_generate_messages) 69 | 70 | #vel converter 71 | add_executable(vel_converter 72 | src/vel_converter.cpp 73 | ) 74 | 75 | target_link_libraries(vel_converter 76 | PRIVATE 77 | ${EXTERNAL_LIBS} 78 | ) 79 | add_dependencies(vel_converter 80 | roborts_msgs_generate_messages) 81 | -------------------------------------------------------------------------------- /roborts_planning/local_planner/README.md: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RoboMaster/RoboRTS/b6b8f811781b00caf956303f4b63a2d5af518fcf/roborts_planning/local_planner/README.md -------------------------------------------------------------------------------- /roborts_planning/local_planner/config/local_planner.prototxt: -------------------------------------------------------------------------------- 1 | name: "timed_elastic_band" 2 | name: "DWA" 3 | selected_algorithm: "timed_elastic_band" 4 | frequency: 40 5 | -------------------------------------------------------------------------------- /roborts_planning/local_planner/include/local_planner/local_planner_algorithms.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of  11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | 18 | #ifndef ROBORTS_PLANNING_LOCAL_PLANNER_ALGORITHMS_H 19 | #define ROBORTS_PLANNING_LOCAL_PLANNER_ALGORITHMS_H 20 | /** 21 | * @brief all local planner algorithm's header file should be include in this file 22 | */ 23 | 24 | /** 25 | * @brief timed elastic band algorithm's header file 26 | */ 27 | #include "timed_elastic_band/teb_local_planner.h" 28 | 29 | #endif //ROBORTS_PLANNING_LOCAL_PLANNER_ALGORITHMS_H 30 | 31 | -------------------------------------------------------------------------------- /roborts_planning/local_planner/include/local_planner/local_visualization.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of  11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | #ifndef ROBORTS_PLANNING_LOCAL_PLANNER_LOCAL_VISUALIZATION_H 18 | #define ROBORTS_PLANNING_LOCAL_PLANNER_LOCAL_VISUALIZATION_H 19 | 20 | #include 21 | 22 | #include 23 | #include 24 | 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #include "timed_elastic_band/teb_vertex_console.h" 33 | 34 | 35 | namespace roborts_local_planner { 36 | 37 | /** 38 | * @brief Class use to visualize local planner algorithm's trajectory 39 | */ 40 | class LocalVisualization { 41 | public: 42 | /** 43 | * @brief Constructor 44 | */ 45 | LocalVisualization(); 46 | /** 47 | * @brief Constructor initialize this class 48 | * @param nh Ros node handle 49 | * @param visualize_frame Visualize frame 50 | */ 51 | LocalVisualization(ros::NodeHandle& nh, const std::string& visualize_frame); 52 | /** 53 | * @brief Initialize visualize frame and ros param 54 | * @param nh ros node handle 55 | * @param visualize_frame Visualize frame 56 | */ 57 | void Initialization(ros::NodeHandle& nh, const std::string& visualize_frame); 58 | /** 59 | * @brief publish trajectory 60 | * @param vertex_console Robot trajectory from local planner algorithm 61 | */ 62 | void PublishLocalPlan(const TebVertexConsole &vertex_console) const; 63 | 64 | protected: 65 | 66 | //! trajectory publisher 67 | ros::Publisher local_planner_; 68 | //! trajectory pose publisher 69 | ros::Publisher pose_pub_; 70 | 71 | //! visualize frame 72 | std::string visual_frame_ = "map"; 73 | 74 | //! initialize state 75 | bool initialized_; 76 | 77 | }; 78 | 79 | typedef boost::shared_ptr LocalVisualizationPtr; 80 | 81 | } // namespace roborts_local_planner 82 | 83 | #endif // ROBORTS_PLANNING_LOCAL_PLANNER_LOCAL_VISUALIZATION_H 84 | 85 | -------------------------------------------------------------------------------- /roborts_planning/local_planner/include/local_planner/odom_info.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of  11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | 18 | #ifndef ROBORTS_PLANNING_LOCAL_PLANNER_ODOM_INFO_H 19 | #define ROBORTS_PLANNING_LOCAL_PLANNER_ODOM_INFO_H 20 | 21 | #include 22 | 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | 31 | namespace roborts_local_planner { 32 | 33 | /** 34 | * @brief A class of odometry 35 | */ 36 | class OdomInfo { 37 | public: 38 | 39 | /** 40 | * Constructor 41 | * @param topic Topic of odom info, default is null 42 | */ 43 | OdomInfo(std::string topic = ""); 44 | ~OdomInfo() {} 45 | 46 | /** 47 | * @brief Odom's callback function 48 | * @param msg Odom's messages 49 | */ 50 | void OdomCB(const nav_msgs::Odometry::ConstPtr& msg); 51 | 52 | /** 53 | * @brief Get velocity 54 | * @param vel Velocity 55 | */ 56 | void GetVel(tf::Stamped& vel); 57 | 58 | /** 59 | * @brief Set Odom callback 60 | * @param topic topic of odom info 61 | */ 62 | void SetTopic(std::string topic); 63 | 64 | /** 65 | * @brief Get odom topic 66 | * @return Odom topic name 67 | */ 68 | std::string GetTopic() const { return topic_; } 69 | 70 | private: 71 | 72 | //! odom topic name 73 | std::string topic_; 74 | //! subscriber 75 | ros::Subscriber sub_; 76 | //! odom info 77 | nav_msgs::Odometry odom_; 78 | //! odom mutex 79 | boost::mutex mutex_; 80 | //! odom frame id 81 | std::string frame_id_; 82 | 83 | }; 84 | 85 | } // namespace roborts_local_planner 86 | #endif //ROBORTS_PLANNING_LOCAL_PLANNER_ODOM_INFO_H 87 | -------------------------------------------------------------------------------- /roborts_planning/local_planner/include/local_planner/proto/local_planner.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto2"; 2 | package roborts_local_planner; 3 | 4 | message LocalAlgorithms { 5 | repeated string name = 1; 6 | optional string selected_algorithm = 2; 7 | optional double frequency = 3; 8 | } 9 | -------------------------------------------------------------------------------- /roborts_planning/local_planner/src/local_visualization.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of  11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | 18 | #include "local_planner/local_visualization.h" 19 | 20 | namespace roborts_local_planner { 21 | LocalVisualization::LocalVisualization() : initialized_(false){ 22 | 23 | } 24 | LocalVisualization::LocalVisualization(ros::NodeHandle &nh, const std::string &visualize_frame) : initialized_(false){ 25 | Initialization(nh, visualize_frame); 26 | } 27 | void LocalVisualization::Initialization(ros::NodeHandle &nh, const std::string &visualize_frame) { 28 | if (initialized_) { 29 | 30 | } 31 | 32 | visual_frame_ = visualize_frame; 33 | local_planner_ = nh.advertise("trajectory", 1); 34 | pose_pub_ = nh.advertise("pose", 1); 35 | 36 | initialized_ = true; 37 | } 38 | 39 | void LocalVisualization::PublishLocalPlan(const TebVertexConsole& vertex_console) const{ 40 | 41 | nav_msgs::Path local_plan; 42 | local_plan.header.frame_id = visual_frame_; 43 | local_plan.header.stamp = ros::Time::now(); 44 | 45 | for (int i = 0; i . 16 | ***************************************************************************/ 17 | 18 | #include "local_planner/odom_info.h" 19 | 20 | namespace roborts_local_planner { 21 | OdomInfo::OdomInfo(std::string topic) { 22 | SetTopic(topic); 23 | } 24 | 25 | void OdomInfo::OdomCB(const nav_msgs::Odometry::ConstPtr& msg) { 26 | 27 | boost::mutex::scoped_lock lock(mutex_); 28 | odom_.twist.twist.linear.x = msg->twist.twist.linear.x; 29 | odom_.twist.twist.linear.y = msg->twist.twist.linear.y; 30 | odom_.twist.twist.angular.z = msg->twist.twist.angular.z; 31 | odom_.child_frame_id = msg->child_frame_id; 32 | 33 | } 34 | 35 | void OdomInfo::GetVel(tf::Stamped& vel) { 36 | geometry_msgs::Twist temp_vel;{ 37 | boost::mutex::scoped_lock lock(mutex_); 38 | temp_vel.linear.x = odom_.twist.twist.linear.x; 39 | temp_vel.linear.y = odom_.twist.twist.linear.y; 40 | temp_vel.angular.z = odom_.twist.twist.angular.z; 41 | 42 | vel.frame_id_ = odom_.child_frame_id; 43 | } 44 | vel.setData(tf::Transform(tf::createQuaternionFromYaw(temp_vel.angular.z), 45 | tf::Vector3(temp_vel.linear.x, temp_vel.linear.y, 0))); 46 | vel.stamp_ = ros::Time(); 47 | } 48 | 49 | void OdomInfo::SetTopic(std::string topic) 50 | { 51 | if (topic_ == topic) { 52 | return; 53 | } else { 54 | topic_ = topic; 55 | if (topic == "") { 56 | sub_.shutdown(); 57 | return; 58 | } else { 59 | ros::NodeHandle nh; 60 | sub_ = nh.subscribe(topic_, 1, boost::bind( &OdomInfo::OdomCB, this, _1 )); 61 | } 62 | } 63 | } 64 | 65 | } // namespace roborts_local_planner -------------------------------------------------------------------------------- /roborts_planning/local_planner/timed_elastic_band/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1) 2 | project(timed_elastic_band) 3 | 4 | 5 | aux_source_directory(src/. SRC_LIST) 6 | 7 | file(GLOB ProtoFiles "${CMAKE_CURRENT_SOURCE_DIR}/include/${PROJECT_NAME}/proto/*.proto") 8 | rrts_protobuf_generate_cpp( 9 | ${CMAKE_CURRENT_SOURCE_DIR}/include/${PROJECT_NAME}/proto 10 | timed_elastic_bandProtoSrc 11 | timed_elastic_bandProtoHds 12 | ${ProtoFiles}) 13 | 14 | add_library(${PROJECT_NAME} 15 | SHARED 16 | ${timed_elastic_bandProtoSrc} 17 | ${timed_elastic_bandProtoHds} 18 | ${SRC_LIST} 19 | ) 20 | 21 | target_link_libraries(${PROJECT_NAME} 22 | PUBLIC 23 | ${EXTERNAL_LIBS} 24 | ) 25 | add_library(planning::timed_elastic_band ALIAS timed_elastic_band) 26 | -------------------------------------------------------------------------------- /roborts_planning/local_planner/timed_elastic_band/config/hcp.prototxt: -------------------------------------------------------------------------------- 1 | hcp_opt { 2 | enable_homotopy_class_planning: True 3 | enable_multithreading: True 4 | simple_exploration: False 5 | max_number_classes: 4 6 | selection_obst_cost_scale: 1.0 7 | selection_prefer_initial_plan: 0.5 8 | selection_viapoint_cost_scale: 1.0 9 | selection_cost_hysteresis: 0.9 10 | selection_alternative_time_cost: False 11 | roadmap_graph_no_samples: 15 12 | roadmap_graph_area_width: 5 13 | roadmap_graph_area_length_scale: 1.2 14 | h_signature_prescaler: 0.5 15 | h_signature_threshold: 0.1 16 | obstacle_keypoint_offset: 0.1 17 | obstacle_heading_threshold: 0.45 18 | viapoints_all_candidates: True 19 | visualize_hc_graph: False 20 | } 21 | 22 | -------------------------------------------------------------------------------- /roborts_planning/local_planner/timed_elastic_band/config/recovery.prototxt: -------------------------------------------------------------------------------- 1 | recovery_info { 2 | shrink_horizon_backup: True 3 | shrink_horizon_min_duration: 10 4 | oscillation_recovery: true 5 | oscillation_v_eps: 0.1 6 | oscillation_omega_eps: 0.1 7 | oscillation_recovery_min_duration: 10 8 | oscillation_filter_duration: 10 9 | } 10 | 11 | -------------------------------------------------------------------------------- /roborts_planning/local_planner/timed_elastic_band/config/timed_elastic_band.prototxt: -------------------------------------------------------------------------------- 1 | opt_frame { 2 | odom_frame: "odom" 3 | map_frame: "map" 4 | } 5 | 6 | trajectory_opt { 7 | teb_autosize: True 8 | dt_ref: 0.15 9 | dt_hysteresis: 0.01 10 | global_plan_overwrite_orientation: true 11 | allow_init_with_backwards_motion: false 12 | global_plan_viapoint_sep: 1.5 13 | via_points_ordered: False 14 | max_global_plan_lookahead_dist: 2.0 15 | exact_arc_length: False 16 | force_reinit_new_goal_dist: 0.8 17 | feasibility_check_no_poses: 5 18 | publish_feedback: False 19 | min_samples: 3 20 | max_samples: 200 21 | } 22 | 23 | kinematics_opt { 24 | max_vel_x: 2.5 25 | max_vel_x_backwards: 0.2 26 | max_vel_y: 2.5 27 | max_vel_theta: 2.5 28 | acc_lim_x: 2.6 29 | acc_lim_y: 2.6 30 | acc_lim_theta: 2.6 31 | min_turning_radius: 0 32 | wheelbase: 0 33 | cmd_angle_instead_rotvel: false 34 | } 35 | 36 | tolerance_opt { 37 | xy_goal_tolerance: 0.1 38 | yaw_goal_tolerance: 0.1 39 | free_goal_vel: False 40 | } 41 | 42 | obstacles_opt { 43 | min_obstacle_dist: 0.32 #0.8 44 | inflation_dist: 0 45 | include_costmap_obstacles: True 46 | costmap_obstacles_behind_robot_dist: 0.1 47 | obstacle_poses_affected: 30 48 | legacy_obstacle_association: False 49 | obstacle_association_cutoff_factor: 5.0 50 | obstacle_association_force_inclusion_factor: 1.5 51 | } 52 | 53 | robot_type { 54 | type: POINT 55 | robot_vertices { 56 | x: 0.3 57 | y: 0.225 58 | } 59 | robot_vertices { 60 | x: -0.3 61 | y: 0.225 62 | } 63 | robot_vertices { 64 | x: -0.3 65 | y: -0.225 66 | } 67 | robot_vertices { 68 | x: 0.3 69 | y: -0.225 70 | } 71 | } 72 | 73 | optimize_info { 74 | no_inner_iterations: 5 75 | no_outer_iterations: 4 76 | optimization_activate: True 77 | optimization_verbose: False 78 | penalty_epsilon: 0.1 79 | weight_max_vel_x: 1 80 | weight_max_vel_y: 1 81 | weight_max_vel_theta: 3 82 | weight_acc_lim_x: 1 83 | weight_acc_lim_y: 1 84 | weight_acc_lim_theta: 3 85 | weight_kinematics_nh: 1 86 | weight_kinematics_forward_drive: 1 87 | weight_kinematics_turning_radius: 0.0 88 | weight_optimaltime: 1 89 | weight_obstacle: 50 90 | weight_inflation: 0.1 91 | weight_dynamic_obstacle: 10 92 | weight_viapoint: 10 93 | weight_adapt_factor: 2.0 94 | weight_prefer_rotdir: 0 95 | } 96 | 97 | -------------------------------------------------------------------------------- /roborts_planning/package.xml: -------------------------------------------------------------------------------- 1 | 2 | roborts_planning 3 | 1.0.0 4 | 5 | The roborts planning package provides motion planning for RoboMaster AI Robot 6 | 7 | RoboMaster 8 | RoboMaster 9 | GPL 3.0 10 | https://github.com/RoboMaster/RoboRTS 11 | 12 | catkin 13 | 14 | actionlib 15 | roscpp 16 | roborts_msgs 17 | rospy 18 | roborts_common 19 | roborts_costmap 20 | 21 | actionlib 22 | roscpp 23 | roborts_msgs 24 | rospy 25 | roborts_common 26 | roborts_costmap 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /roborts_tracking/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0) 2 | project(roborts_tracking) 3 | set(CMAKE_BUILD_TYPE Release) 4 | find_package(OpenCV REQUIRED) 5 | find_package(catkin REQUIRED COMPONENTS 6 | roscpp 7 | tf 8 | roborts_msgs 9 | ) 10 | 11 | if(NOT WIN32) 12 | ADD_DEFINITIONS("-std=c++11") 13 | endif(NOT WIN32) 14 | 15 | catkin_package() 16 | 17 | add_library(kcf_tracker STATIC 18 | KCFcpp/src/fhog.cpp 19 | KCFcpp/src/kcftracker.cpp) 20 | target_link_libraries(kcf_tracker ${OpenCV_LIBRARIES}) 21 | target_include_directories(kcf_tracker PRIVATE KCFcpp/src) 22 | 23 | add_executable(roborts_tracking_test 24 | tracking_test.cpp 25 | tracking_utility.cpp) 26 | target_link_libraries(roborts_tracking_test 27 | kcf_tracker 28 | ${OpenCV_LIBS} 29 | ${catkin_LIBRARIES}) 30 | target_include_directories(roborts_tracking_test 31 | PUBLIC 32 | ${catkin_INCLUDE_DIRS}) 33 | add_dependencies(roborts_tracking_test 34 | roborts_msgs_generate_messages) 35 | 36 | -------------------------------------------------------------------------------- /roborts_tracking/KCFcpp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | 3 | project(test) 4 | 5 | find_package(OpenCV REQUIRED) 6 | 7 | if(NOT WIN32) 8 | ADD_DEFINITIONS("-std=c++0x -O3") 9 | endif(NOT WIN32) 10 | 11 | include_directories(src) 12 | FILE(GLOB_RECURSE sourcefiles "src/*.cpp") 13 | add_executable( KCF ${sourcefiles} ) 14 | target_link_libraries( KCF ${OpenCV_LIBS}) 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /roborts_tracking/KCFcpp/KCFCpp.sh: -------------------------------------------------------------------------------- 1 | 2 | KCF 3 | 4 | -------------------------------------------------------------------------------- /roborts_tracking/KCFcpp/KCFLabCpp.sh: -------------------------------------------------------------------------------- 1 | 2 | KCF lab 3 | 4 | -------------------------------------------------------------------------------- /roborts_tracking/KCFcpp/LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2015, Joao Faro 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of KCFcpp nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | -------------------------------------------------------------------------------- /roborts_tracking/KCFcpp/README.md: -------------------------------------------------------------------------------- 1 | # C++ KCF Tracker 2 | This package includes a C++ class with several tracking methods based on the Kernelized Correlation Filter (KCF) [1, 2]. 3 | It also includes an executable to interface with the VOT benchmark. 4 | 5 | [1] J. F. Henriques, R. Caseiro, P. Martins, J. Batista, 6 | "High-Speed Tracking with Kernelized Correlation Filters", TPAMI 2015. 7 | 8 | [2] J. F. Henriques, R. Caseiro, P. Martins, J. Batista, 9 | "Exploiting the Circulant Structure of Tracking-by-detection with Kernels", ECCV 2012. 10 | 11 | 12 | Authors: Joao Faro, Christian Bailer, Joao F. Henriques 13 | Contacts: joaopfaro@gmail.com, Christian.Bailer@dfki.de, henriques@isr.uc.pt 14 | Institute of Systems and Robotics - University of Coimbra / Department of Augmented Vision DFKI 15 | 16 | ### Algorithms (in this folder) ### 17 | 18 | "KCFC++", command: ./KCF 19 | Description: KCF on HOG features, ported to C++ OpenCV. The original Matlab tracker placed 3rd in VOT 2014. 20 | 21 | "KCFLabC++", command: ./KCF lab 22 | Description: KCF on HOG and Lab features, ported to C++ OpenCV. The Lab features are computed by quantizing CIE-Lab colors into 15 centroids, obtained from natural images by k-means. 23 | 24 | The CSK tracker [2] is also implemented as a bonus, simply by using raw grayscale as features (the filter becomes single-channel). 25 | 26 | ### Compilation instructions ### 27 | There are no external dependencies other than OpenCV 3.0.0. Tested on a freshly installed Ubuntu 14.04. 28 | 29 | 1) cmake CMakeLists.txt 30 | 2) make 31 | 32 | ### Running instructions ### 33 | 34 | The runtracker.cpp is prepared to be used with the VOT toolkit. The executable "KCF" should be called as: 35 | 36 | ./KCF [OPTION_1] [OPTION_2] [...] 37 | 38 | Options available: 39 | 40 | gray - Use raw gray level features as in [1]. 41 | hog - Use HOG features as in [2]. 42 | lab - Use Lab colorspace features. This option will also enable HOG features by default. 43 | singlescale - Performs single-scale detection, using a variable-size window. 44 | fixed_window - Keep the window size fixed when in single-scale mode (multi-scale always used a fixed window). 45 | show - Show the results in a window. 46 | 47 | To include it in your project, without the VOT toolkit you just need to: 48 | 49 | // Create the KCFTracker object with one of the available options 50 | KCFTracker tracker(HOG, FIXEDWINDOW, MULTISCALE, LAB); 51 | 52 | // Give the first frame and the position of the object to the tracker 53 | tracker.init( Rect(xMin, yMin, width, height), frame ); 54 | 55 | // Get the position of the object for the new frame 56 | result = tracker.update(frame); 57 | -------------------------------------------------------------------------------- /roborts_tracking/KCFcpp/src/labdata.hpp: -------------------------------------------------------------------------------- 1 | const int nClusters = 15; 2 | float data[nClusters][3] = { 3 | {161.317504, 127.223401, 128.609333}, 4 | {142.922425, 128.666965, 127.532319}, 5 | {67.879757, 127.721830, 135.903311}, 6 | {92.705062, 129.965717, 137.399500}, 7 | {120.172257, 128.279647, 127.036493}, 8 | {195.470568, 127.857070, 129.345415}, 9 | {41.257102, 130.059468, 132.675336}, 10 | {12.014861, 129.480555, 127.064714}, 11 | {226.567086, 127.567831, 136.345727}, 12 | {154.664210, 131.676606, 156.481669}, 13 | {121.180447, 137.020793, 153.433743}, 14 | {87.042204, 137.211742, 98.614874}, 15 | {113.809537, 106.577104, 157.818094}, 16 | {81.083293, 170.051905, 148.904079}, 17 | {45.015485, 138.543124, 102.402528}}; -------------------------------------------------------------------------------- /roborts_tracking/KCFcpp/src/tracker.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: BasicTracker.h 3 | * Author: Joao F. Henriques, Joao Faro, Christian Bailer 4 | * Contact address: henriques@isr.uc.pt, joaopfaro@gmail.com, Christian.Bailer@dfki.de 5 | * Instute of Systems and Robotics- University of COimbra / Department Augmented Vision DFKI 6 | * 7 | * This source code is provided for for research purposes only. For a commercial license or a different use case please contact us. 8 | * You are not allowed to publish the unmodified sourcecode on your own e.g. on your webpage. Please refer to the official download page instead. 9 | * If you want to publish a modified/extended version e.g. because you wrote a publication with a modified version of the sourcecode you need our 10 | * permission (Please contact us for the permission). 11 | * 12 | * We reserve the right to change the license of this sourcecode anytime to BSD, GPL or LGPL. 13 | * By using the sourcecode you agree to possible restrictions and requirements of these three license models so that the license can be changed 14 | * anytime without you knowledge. 15 | */ 16 | 17 | 18 | 19 | #pragma once 20 | 21 | #include 22 | #include 23 | 24 | class Tracker 25 | { 26 | public: 27 | Tracker() {} 28 | virtual ~Tracker() { } 29 | 30 | virtual void init(const cv::Rect &roi, cv::Mat image) = 0; 31 | virtual cv::Rect update( cv::Mat image)=0; 32 | 33 | 34 | protected: 35 | cv::Rect_ _roi; 36 | }; 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /roborts_tracking/package.xml: -------------------------------------------------------------------------------- 1 | 2 | roborts_tracking 3 | 1.0.0 4 | 5 | The roborts_tracking package provides object tracking for RoboMaster AI Robot 6 | 7 | RoboMaster 8 | RoboMaster 9 | GPL 3.0 10 | https://github.com/RoboMaster/RoboRTS 11 | 12 | catkin 13 | 14 | roscpp 15 | rospy 16 | tf 17 | roborts_msgs 18 | 19 | roscpp 20 | rospy 21 | tf 22 | roborts_msgs 23 | 24 | 25 | -------------------------------------------------------------------------------- /roborts_tracking/tracking_utility.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of  11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | 18 | #include "tracking_utility.h" 19 | #include 20 | void TrackingUtility::mouseCallback(int event, int x, int y, int f, void *p) 21 | { 22 | TrackingUtility *u = reinterpret_cast(p); 23 | if(u->state != STATE_IDLE) 24 | { 25 | std::cout << "Currently tracking, press s key to stop" << std::endl; 26 | return; 27 | } 28 | 29 | switch(event) 30 | { 31 | case CV_EVENT_LBUTTONDOWN : 32 | u->mouseClicked = true; 33 | u->roiSelected = false; 34 | u->P1 = cv::Point(x,y); 35 | u->P2 = cv::Point(x,y); 36 | break; 37 | 38 | case CV_EVENT_LBUTTONUP : 39 | u->P2 = cv::Point(x,y); 40 | u->mouseClicked=false; 41 | if(u->P2 != u->P1) 42 | { 43 | u->roiSelected = true; 44 | } 45 | break; 46 | 47 | case CV_EVENT_MOUSEMOVE : 48 | if(u->mouseClicked) 49 | { 50 | u->P2 = cv::Point(x,y); 51 | } 52 | break; 53 | 54 | default : 55 | break; 56 | } 57 | 58 | if(u->mouseClicked) 59 | { 60 | u->roi = cv::Rect(u->P1, u->P2); 61 | printf("Current Region of Interest: %d, %d, %d, %d\n", u->roi.tl().x, u->roi.tl().y, u->roi.br().x, u->roi.br().y); 62 | } 63 | } 64 | 65 | cv::Rect TrackingUtility::getROI() 66 | { 67 | return roi; 68 | } 69 | 70 | TrackingUtility::TrackingState TrackingUtility::getState() 71 | { 72 | return state; 73 | } 74 | 75 | void TrackingUtility::startTracker() 76 | { 77 | state = STATE_ONGOING; 78 | } 79 | 80 | void TrackingUtility::stopTracker() 81 | { 82 | state = STATE_IDLE; 83 | } 84 | 85 | void TrackingUtility::getKey(char c) 86 | { 87 | switch(c) 88 | { 89 | case 'g': 90 | if( (state == STATE_IDLE) && (roiSelected == true)) 91 | { 92 | state = STATE_INIT; 93 | } 94 | break; 95 | 96 | case 's': 97 | if( state == STATE_ONGOING ) 98 | { 99 | state = STATE_STOP; 100 | roi = cv::Rect(0,0,0,0); //when we press s, should clear bounding box 101 | } 102 | break; 103 | 104 | default: 105 | break; 106 | } 107 | } -------------------------------------------------------------------------------- /roborts_tracking/tracking_utility.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * Copyright (C) 2019 RoboMaster. 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of  11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | ***************************************************************************/ 17 | 18 | #ifndef TEST_TRACKING_UTILITY_H 19 | #define TEST_TRACKING_UTILITY_H 20 | #include "opencv2/opencv.hpp" 21 | /*! @brief 22 | * The TrackingUtility Class handles simple start and stop tracking logic 23 | */ 24 | class TrackingUtility 25 | { 26 | public: 27 | TrackingUtility() 28 | : P1(0,0), 29 | P2(0,0), 30 | roi(0,0,0,0), 31 | mouseClicked(false), 32 | roiSelected(false), 33 | state(STATE_IDLE) 34 | { 35 | } 36 | 37 | typedef enum TrackingState 38 | { 39 | STATE_IDLE, 40 | STATE_INIT, 41 | STATE_ONGOING, 42 | STATE_STOP 43 | } TrackingState; 44 | 45 | static void mouseCallback(int event, int x, int y, int f, void *p); 46 | 47 | cv::Point P1; 48 | cv::Point P2; 49 | bool mouseClicked; 50 | cv::Rect roi; 51 | 52 | /*! 53 | * start_tracking is set true when you select a region and press 'g' 54 | * is set to false when you press 's' 55 | */ 56 | bool roiSelected; 57 | TrackingState state; 58 | TrackingState getState(); 59 | 60 | void startTracker(); 61 | void stopTracker(); 62 | 63 | cv::Rect getROI(); 64 | void getKey(char c); 65 | }; 66 | #endif //TEST_TRACKING_UTILITY_H 67 | --------------------------------------------------------------------------------