├── .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 | [](https://travis-ci.org/RoboMaster/RoboRTS)
3 | [](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 |
--------------------------------------------------------------------------------