├── .gitignore ├── .gitmodules ├── LICENSE ├── README.md ├── docs ├── AdaptiveInformativeSamplingwithEnvironmentPartitioningforHeterogeneousMulti-RobotSystems.pdf ├── hector.png ├── heterogeneous_sampling_simulation.gif ├── jackal.jpeg ├── partition.png ├── pelican.jpg └── result_fig4.png ├── setup.sh └── src ├── CMakeLists.txt ├── sampling_agent ├── CMakeLists.txt ├── include │ └── sampling_agent │ │ ├── hector_agent.h │ │ ├── hector_agent_params.h │ │ ├── jackal_agent.h │ │ ├── jackal_agent_params.h │ │ ├── pelican_agent.h │ │ ├── pelican_agent_params.h │ │ ├── sampling_agent.h │ │ └── sampling_agent_params.h ├── launch │ ├── hector_simulation.launch │ └── jackal_simulation.launch ├── node │ └── sampling_agent_node.cpp ├── package.xml └── src │ ├── hector_agent.cpp │ ├── hector_agent_params.cpp │ ├── jackal_agent.cpp │ ├── jackal_agent_params.cpp │ ├── pelican_agent.cpp │ ├── pelican_agent_params.cpp │ ├── sampling_agent.cpp │ └── sampling_agent_params.cpp ├── sampling_core ├── CMakeLists.txt ├── config │ ├── scenario0_hetero.yaml │ ├── scenario0_homo.yaml │ ├── scenario1_hetero.yaml │ ├── scenario1_homo.yaml │ ├── scenario2_hetero.yaml │ ├── scenario2_homo.yaml │ ├── scenario3_hetero.yaml │ └── scenario3_homo.yaml ├── include │ └── sampling_core │ │ ├── sampling_core.h │ │ ├── sampling_core_params.h │ │ └── sampling_core_performance_evaluation.h ├── launch │ ├── heterogeneous_adaptive_sampling.launch │ └── homogeneous_adaptive_sampling.launch ├── node │ └── heterogeneous_adaptive_sampling_node.cpp ├── package.xml └── src │ ├── sampling_core.cpp │ ├── sampling_core_params.cpp │ └── sampling_core_performance_evaluation.cpp ├── sampling_data ├── CMakeLists.txt ├── location │ ├── wifi_2_routers.txt │ └── wifi_3_routers.txt ├── measurement │ ├── artificial_wifi_3_routers.txt │ ├── wifi_2_routers.txt │ └── wifi_3_routers.txt └── package.xml ├── sampling_gazebo_simulation ├── CMakeLists.txt ├── launch │ ├── one_ugv_one_uav_simulation.launch │ ├── sampling_world.launch │ ├── spawn_hector.launch │ ├── two_ugv_one_uav_simulation.launch │ └── two_ugv_simulation.launch ├── package.xml ├── rviz │ └── samlping_visualization.rviz └── worlds │ ├── scenario0.world │ ├── scenario1.world │ ├── scenario2.world │ └── scenario3.world ├── sampling_measurement ├── CMakeLists.txt ├── launch │ ├── measurement_simulation_server.launch │ ├── simulation_measurement.launch │ └── temperature_server.launch ├── package.xml └── script │ ├── measurement_simulation_server.py │ └── temperature_server.py ├── sampling_modeling ├── CMakeLists.txt ├── launch │ ├── sampling_model.launch │ └── sampling_modeling.launch ├── package.xml └── script │ ├── gp.py │ ├── gp_util.py │ ├── mixture_gp.py │ └── sampling_modeling_node.py ├── sampling_msgs ├── CMakeLists.txt ├── msg │ ├── AgentLocation.msg │ ├── Sample.msg │ └── SamplingPerformance.msg ├── package.xml └── srv │ ├── AddSampleToModel.srv │ ├── AddTestPositionToModel.srv │ ├── KillAgent.srv │ ├── MeasurementService.srv │ ├── ModelPredict.srv │ ├── ReportSampleService.srv │ ├── RequestLocation.srv │ ├── RequestMeasurement.srv │ ├── RequestTemperatureMeasurement.srv │ ├── SamplingGoal.srv │ └── StopAgent.srv ├── sampling_online_learning ├── CMakeLists.txt ├── include │ └── sampling_online_learning │ │ └── online_learning_handler.h ├── package.xml └── src │ └── online_learning_handler.cpp ├── sampling_partition ├── CMakeLists.txt ├── config │ └── heterogeneiry_property.config ├── include │ └── sampling_partition │ │ ├── heterogeneity.h │ │ ├── heterogeneity_distance.h │ │ ├── heterogeneity_distance_dependent.h │ │ ├── heterogeneity_params.h │ │ ├── heterogeneity_topography_dependent.h │ │ ├── weighted_voronoi_partition.h │ │ └── weighted_voronoi_partition_params.h ├── launch │ └── partition.launch ├── map │ └── test_map.txt ├── node │ └── partition_node.cpp ├── package.xml ├── script │ └── generate_map.py └── src │ ├── heterogeneity.cpp │ ├── heterogeneity_distance.cpp │ ├── heterogeneity_distance_dependent.cpp │ ├── heterogeneity_topography_dependent.cpp │ ├── weighted_voronoi_partition.cpp │ └── weighted_voronoi_partition_params.cpp ├── sampling_utils ├── CMakeLists.txt ├── include │ └── sampling_utils │ │ ├── utils.h │ │ └── utils_impl.h └── package.xml └── sampling_visualization ├── CMakeLists.txt ├── include └── sampling_visualization │ ├── agent_visualization_handler.h │ ├── grid_visualization_handler.h │ ├── sampling_visualization_params.h │ └── sampling_visualization_utils.h ├── package.xml └── src ├── agent_visualization_handler.cpp ├── grid_visualization_handler.cpp ├── sampling_visualization_params.cpp └── sampling_visualization_utils.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | /build 2 | /devel 3 | /bags 4 | .vscode/ 5 | .catkin_tools/ 6 | *.pyc 7 | logs/ 8 | *.bag 9 | .catkin_workspace 10 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "src/simulation/hector_quadrotor_navigation"] 2 | path = src/simulation/hector_quadrotor_navigation 3 | url = https://github.com/yangggzhang/hector_quadrotor_navigation.git 4 | [submodule "src/simulation/multi_jackal"] 5 | path = src/simulation/multi_jackal 6 | url = https://github.com/NicksSimulationsROS/multi_jackal.git 7 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2020, Yang Zhang 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | [![ROS Distro: Melodic](https://img.shields.io/badge/ROS-Melodic-blue.svg)](http://wiki.ros.org/melodic) 2 | [![License: BSD](https://img.shields.io/badge/License-BSD-yellow.svg)](./LICENSE)
3 | 4 | # Adaptive Informative Sampling with Environment Partitioning for Heterogeneous Multi-Robot Systems # 5 | by [Yunfei Shi](https://www.linkedin.com/in/shi-yunfei/), [Ning Wang*](https://www.linkedin.com/in/ning-wang-cmu/), [Jianmin Zheng*](https://www.linkedin.com/in/jianmimzheng/), [Yang Zhang*](https://www.linkedin.com/in/yang-zhang-cmu/), [Sha Yi](https://www.ri.cmu.edu/ri-people/yisha-sha-yi/), [Wenhao Luo](http://www.contrib.andrew.cmu.edu/~wenhaol/), and [Katia Sycara](https://www.ri.cmu.edu/ri-faculty/katia-sycara/)
6 | IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2020.
7 | The final submission version [pdf](docs/AdaptiveInformativeSamplingwithEnvironmentPartitioningforHeterogeneousMulti-RobotSystems.pdf), and also video [here](https://drive.google.com/file/d/1uPgnyV66UEWomSi_KKH2vZLEvRN9gDTj/view?usp=sharing). 8 | 9 | ## Introduction ## 10 | Multi-robot systems are widely used in environmental exploration and modeling, especially in hazardous environments. However, different types of robots are limited by different mobility, battery life, sensor type, etc. Heterogeneous robot systems are able to utilize various types of robots and provide solutions where robots are able to compensate each other with their different capabilities. In this paper, we consider the problem of sampling and modeling environmental characteristics with a heterogeneous team of robots. To utilize heterogeneity of the system while remaining computationally tractable, we propose an environmental partitioning approach that leverages various robot capabilities by forming a uniformly defined heterogeneity cost space. We combine with the mixture of Gaussian Processes model-learning framework to adaptively sample and model the environment in an efficient and scalable manner. We demonstrate our algorithm in field experiments with ground and aerial vehicles. 11 | 12 | ## Support Platform ## 13 | we present a system architecture for heterogeneous multi-robot informative samplingwith a modularized design that allows for flexible scale-ups and extensions in both robot characteristics and team size. The current robot platform we support includes: 14 | 15 | | Robot Platform | Description | Simulation | Physical Platform| 16 | | :-: | :-: | :-: | :-: | 17 | |[Clearpath Jackal UGV](https://clearpathrobotics.com/jackal-small-unmanned-ground-vehicle/#:~:text=UNMANNED%20GROUND%20VEHICLE,%2Dthe%2Dbox%20autonomous%20capability) | |:heavy_check_mark:|:heavy_check_mark:| 18 | |[Asctec Pelican UAV](https://mrsdprojects.ri.cmu.edu/2018teamg/documentation/asctec-pelican-uav-setup-guidance/) ||:interrobang:|:heavy_check_mark:| 19 | |[Hector Quadrator](http://wiki.ros.org/hector_quadrotor) | |:heavy_check_mark:|:interrobang:| 20 | 21 | Check [sampling_agent](src/sampling_agent/include/sampling_agent/sampling_agent.h) to add your own robot to this sampling framework. 22 | 23 | ## Installation ## 24 | First install **Full-Desktop** ROS Melodic by the [instructions](http://wiki.ros.org/melodic/Installation/Ubuntu).
25 | To install all additional ROS packages, dependencies and python packages. Please run the setup script. 26 | ```bat 27 | ./setup.sh 28 | ``` 29 | ## Build the package ## 30 | This work depends on multi ROS pacakgaes for robot simulation/deployment, it may take up to 5 minutes to build ths project. 31 | ``` 32 | catkin build 33 | ``` 34 | ## Run the simulation ## 35 | Don't forget to `source devel/setup.bash` before launch any files.
36 | First, launch the simulation environment in Gazebo in one terminal. Check or add customize simulation environments [here](src/sampling_gazebo_simulation/worlds). 37 | ```bat 38 | roslaunch sampling_gazebo_simulation two_ugv_one_uav_simulation.launch 39 | ``` 40 | Second, launch the heterogeneous multi-robot adaptive sampling algorithm in the second terminal. Please make sure the agent information fed to the sampling algorithim is consistent with the gazebo simulation by checking the [ros parameters](src/sampling_core/launch/heterogeneous_adaptive_sampling.launch). Users can also adjust or add heterogeneous primitives according to their robot systems [here](src/sampling_core/config)
41 | ```bat 42 | roslaunch sampling_core heterogeneous_adaptive_sampling.launch 43 | ``` 44 | You can also directly monitor the sampling performance by listenting to the `/sampling_performance` channel, which includes the number of samples collected, root mean square error for prediction, and average variance from prediction.
45 | ``` 46 | rostopic echo /sampling_performance 47 | ``` 48 | 49 |
50 | 51 | The left window is the simulation running in Gazebo. The right one is the visualization of heterogeneous multi-robot adaptive sampling in rviz. The leftmost grid is showing the heterogeneous environment partition and agents' locations, the middle one is showing the real-time model prediction, and the rightmost one is showing the real-time uncertainties from prediction. 52 | 53 | 54 | ## Results ## 55 | The robots in our simulation are different in speed, battery life and traversability. 56 | 57 | ### Environment Partition using Heterogeneity Primitives ### 58 | 59 | The red, blue and green dots represent the current locations of the ground robot 1, 2 and the aerial robot respectively. The corresponding shallow areas are their responsible regions. The black circle denotes an obstacle that the robot 1 and 2 need to avoid. The first partition from left uses normal Voronoi Diagram; the second adds speed heterogeneity; the third adds battery life, and the last adds traversability. 60 | 61 | ### Heterogeneous Informative Sampling ### 62 | 63 | Informative sampling performance comparison between heterogeneous and homogeneous multi-robot sampling algorithms. We run each algorithm on the same dataset 45 times with random robot initial locations and obstacle positions. The shallow areas represent the variance range. The red vertical line indicates the time when the aerial robot stopped operation. 64 | 65 | ## Citation ## 66 | If you find this work or code helpful, please cite: 67 | ``` 68 | @InProceedings{hetero_sampling, 69 | author = {Yunfei Shi, Ning Wang, Jianmin Zheng, Yang Zhang, Sha Yi, Wenhao Luo, and Katia Sycara}, 70 | title = {Adaptive Informative Sampling with Environment Partitioning for Heterogeneous Multi-Robot Systems}, 71 | booktitle = {IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, 72 | month = {Oct}, 73 | year = {2020} 74 | } 75 | ``` 76 | 77 | ## Acknowledgement ## 78 | This work is primarity from [Team SAMP](https://mrsdprojects.ri.cmu.edu/2018teamg/)'s (Yunfei Shi, Ning Wang, Jianmin Zheng and Yang Zhang) capstone project when they were pursing their masters' degrees in [Robotic Systems Development (MRSD)](https://mrsd.ri.cmu.edu/) at Carnegie Mellon University the Robotics Institute. Special thanks to [John M. Dolan](https://www.ri.cmu.edu/ri-faculty/john-m-dolan/), [Dimitrios (Dimi) Apostolopoulos](https://www.ri.cmu.edu/ri-faculty/dimitrios-dimi-apostolopoulos/) and [Sarah Conte](https://www.ri.cmu.edu/ri-people/sarah-conte/) for the support and advice. 79 | -------------------------------------------------------------------------------- /docs/AdaptiveInformativeSamplingwithEnvironmentPartitioningforHeterogeneousMulti-RobotSystems.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yangggzhang/Heterogeneous-Multi-Robot-Adaptive-Sampling/2d65a077f0bee63154c295f1540505132111ac42/docs/AdaptiveInformativeSamplingwithEnvironmentPartitioningforHeterogeneousMulti-RobotSystems.pdf -------------------------------------------------------------------------------- /docs/hector.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yangggzhang/Heterogeneous-Multi-Robot-Adaptive-Sampling/2d65a077f0bee63154c295f1540505132111ac42/docs/hector.png -------------------------------------------------------------------------------- /docs/heterogeneous_sampling_simulation.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yangggzhang/Heterogeneous-Multi-Robot-Adaptive-Sampling/2d65a077f0bee63154c295f1540505132111ac42/docs/heterogeneous_sampling_simulation.gif -------------------------------------------------------------------------------- /docs/jackal.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yangggzhang/Heterogeneous-Multi-Robot-Adaptive-Sampling/2d65a077f0bee63154c295f1540505132111ac42/docs/jackal.jpeg -------------------------------------------------------------------------------- /docs/partition.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yangggzhang/Heterogeneous-Multi-Robot-Adaptive-Sampling/2d65a077f0bee63154c295f1540505132111ac42/docs/partition.png -------------------------------------------------------------------------------- /docs/pelican.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yangggzhang/Heterogeneous-Multi-Robot-Adaptive-Sampling/2d65a077f0bee63154c295f1540505132111ac42/docs/pelican.jpg -------------------------------------------------------------------------------- /docs/result_fig4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yangggzhang/Heterogeneous-Multi-Robot-Adaptive-Sampling/2d65a077f0bee63154c295f1540505132111ac42/docs/result_fig4.png -------------------------------------------------------------------------------- /setup.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | git submodule update --init --recursive 3 | sudo apt-get install python-catkin-tools python-pip 4 | rosdep install --from-paths src --ignore-src -r -y --rosdistro melodic 5 | pip install numpy scipy sklearn --user -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/melodic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /src/sampling_agent/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(sampling_agent) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | roscpp 8 | rospy 9 | std_msgs 10 | visualization_msgs 11 | actionlib 12 | actionlib_msgs 13 | message_runtime 14 | sampling_measurement 15 | sampling_msgs 16 | sensor_msgs 17 | tf 18 | tf2 19 | tf2_ros 20 | hector_navigation 21 | hector_navigation_msgs 22 | hector_uav_msgs 23 | ) 24 | 25 | find_package(cmake_modules REQUIRED) 26 | find_package(Eigen3 REQUIRED) 27 | 28 | catkin_package( 29 | INCLUDE_DIRS include ${EIGEN3_INCLUDE_DIRS} 30 | CATKIN_DEPENDS message_runtime 31 | ) 32 | 33 | include_directories( 34 | include 35 | ${catkin_INCLUDE_DIRS} 36 | ${EIGEN3_INCLUDE_DIR} 37 | ) 38 | 39 | add_library(${PROJECT_NAME} 40 | src/sampling_agent.cpp 41 | src/sampling_agent_params.cpp 42 | src/jackal_agent.cpp 43 | src/jackal_agent_params.cpp 44 | src/pelican_agent.cpp 45 | src/pelican_agent_params.cpp 46 | src/hector_agent.cpp 47 | src/hector_agent_params.cpp 48 | ) 49 | 50 | add_executable(sampling_agent_node node/sampling_agent_node.cpp) 51 | target_link_libraries(sampling_agent_node ${PROJECT_NAME} ${catkin_LIBRARIES} ) 52 | 53 | install(TARGETS ${PROJECT_NAME} sampling_agent_node 54 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 55 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 56 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 57 | ) 58 | 59 | install(DIRECTORY include/${PROJECT_NAME}/ 60 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 61 | FILES_MATCHING PATTERN "*.h" 62 | PATTERN ".svn" EXCLUDE 63 | ) 64 | -------------------------------------------------------------------------------- /src/sampling_agent/include/sampling_agent/hector_agent.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "sampling_agent/hector_agent_params.h" 7 | #include "sampling_agent/sampling_agent.h" 8 | #include "sampling_agent/sampling_agent_params.h" 9 | 10 | namespace sampling { 11 | namespace agent { 12 | 13 | using Hector = hector::navigation::HectorQuadrotor; 14 | 15 | class HectorAgent : public SamplingAgent { 16 | public: 17 | HectorAgent() = delete; 18 | 19 | static std::unique_ptr MakeUniqueFromROSParam( 20 | ros::NodeHandle &nh, ros::NodeHandle &ph); 21 | 22 | private: 23 | HectorAgent(ros::NodeHandle &nh, const SamplingAgentParams &agent_params, 24 | const HectorAgentParams ¶ms); 25 | 26 | HectorAgentParams hector_params_; 27 | 28 | bool Navigate() override; 29 | 30 | ros::Subscriber odom_subscriber_; 31 | 32 | ros::ServiceClient hector_takeoff_client_; 33 | 34 | ros::ServiceClient hector_navigate_client_; 35 | 36 | void UpdatePositionFromOdom(const nav_msgs::Odometry &msg); 37 | 38 | bool taken_off_; 39 | }; 40 | } // namespace agent 41 | } // namespace sampling 42 | -------------------------------------------------------------------------------- /src/sampling_agent/include/sampling_agent/hector_agent_params.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | namespace sampling { 8 | namespace agent { 9 | 10 | const std::string KNavigationFrame = "world"; 11 | const double KNavigationHeight_m = 2.0; 12 | const double KNavigationSpeed_ms = 4.0; 13 | const double KTakeoffDistance_m = 0.5; 14 | 15 | class HectorAgentParams { 16 | public: 17 | HectorAgentParams(); 18 | 19 | bool LoadFromRosParams(ros::NodeHandle &ph); 20 | 21 | std::string navigation_frame; 22 | 23 | double navigation_height_m; 24 | 25 | double takeoff_distance_m; 26 | 27 | }; // namespace scene 28 | } // namespace agent 29 | } // namespace sampling -------------------------------------------------------------------------------- /src/sampling_agent/include/sampling_agent/jackal_agent.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "sampling_agent/jackal_agent_params.h" 11 | #include "sampling_agent/sampling_agent.h" 12 | 13 | namespace sampling { 14 | namespace agent { 15 | 16 | using JackalNavigator = 17 | actionlib::SimpleActionClient; 18 | 19 | class JackalAgent : public SamplingAgent { 20 | public: 21 | JackalAgent() = delete; 22 | 23 | static std::unique_ptr MakeUniqueFromROSParam( 24 | ros::NodeHandle &nh, ros::NodeHandle &ph); 25 | 26 | private: 27 | JackalAgent(ros::NodeHandle &nh, const SamplingAgentParams &agent_params, 28 | const JackalAgentParams &jackal_params, 29 | std::unique_ptr jackal_navigator); 30 | 31 | bool Navigate() override; 32 | 33 | JackalAgentParams jackal_params_; 34 | 35 | tf::TransformListener listener_; 36 | 37 | ros::Subscriber odom_subscriber_; 38 | 39 | void UpdatePositionFromOdom(const nav_msgs::Odometry &msg); 40 | 41 | bool GPStoOdom(const double &latitude, const double &longitude, 42 | geometry_msgs::PointStamped &odom_point); 43 | 44 | ros::Subscriber gps_subscriber_; 45 | 46 | void UpdatePositionFromGPS(const sensor_msgs::NavSatFix &msg); 47 | 48 | std::unique_ptr jackal_navigator_; 49 | }; 50 | } // namespace agent 51 | } // namespace sampling 52 | -------------------------------------------------------------------------------- /src/sampling_agent/include/sampling_agent/jackal_agent_params.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | namespace sampling { 8 | namespace agent { 9 | 10 | enum JackalNavigationMode { GPS, ODOM }; 11 | 12 | const std::string KJackalWorldFrame = "map"; 13 | const std::string KJackalNavigationMode = "ODOM"; 14 | const double KJackalMaxSpeed_ms = 1.5; 15 | const double KJackalExecuteTimeout_s = 30.0; 16 | const double KJackalPreemptTimeout_s = 15.0; 17 | 18 | class JackalAgentParams { 19 | public: 20 | JackalAgentParams(); 21 | 22 | bool LoadFromRosParams(ros::NodeHandle &ph); 23 | 24 | std::string navigation_frame; 25 | 26 | JackalNavigationMode navigation_mode; 27 | 28 | double execute_timeout_s; 29 | 30 | double preempt_timeout_s; 31 | 32 | }; // namespace scene 33 | } // namespace agent 34 | } // namespace sampling -------------------------------------------------------------------------------- /src/sampling_agent/include/sampling_agent/pelican_agent.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "sampling_agent/pelican_agent_params.h" 7 | #include "sampling_agent/sampling_agent.h" 8 | 9 | /// todo \Paul \Yunfei 10 | /// basic function of Pelican execution 11 | /// reference sampling_agent.h and jackal_agent.h 12 | 13 | namespace sampling { 14 | namespace agent { 15 | 16 | // Pelican only supports GPS navigation mode 17 | 18 | class PelicanAgent : public SamplingAgent { 19 | public: 20 | PelicanAgent() = delete; 21 | 22 | static std::unique_ptr MakeUniqueFromROSParam( 23 | ros::NodeHandle &nh, ros::NodeHandle &ph); 24 | 25 | private: 26 | PelicanAgent(ros::NodeHandle &nh, const SamplingAgentParams &agent_params, 27 | const PelicanAgentParams &pelican_params); 28 | 29 | ros::Publisher xb_command_publisher_; 30 | 31 | ros::Subscriber gps_subscriber_; 32 | 33 | void UpdatePositionFromGPS(const sensor_msgs::NavSatFix &msg); 34 | 35 | void CheckConvergence(); 36 | 37 | bool WaypointNavigate(const double &latitude, const double &longitude, 38 | const double &height, const double &converge_duration); 39 | 40 | bool Navigate() override; 41 | 42 | PelicanAgentParams pelican_params_; 43 | 44 | double cmd_latitude_; 45 | double cmd_longitude_; 46 | double last_cmd_latitude_; 47 | double last_cmd_longitude_; 48 | 49 | int converge_count_; 50 | 51 | bool gps_converge_flag_; // flag to detect the converge of GPS sensor 52 | 53 | boost::optional current_gps_position_; 54 | 55 | boost::optional last_gps_position_; 56 | }; 57 | } // namespace agent 58 | } // namespace sampling 59 | -------------------------------------------------------------------------------- /src/sampling_agent/include/sampling_agent/pelican_agent_params.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | namespace sampling { 9 | namespace agent { 10 | 11 | const double KHoverTime_s = 5.0; 12 | const double KNavigationWaitTime_s = 5.0; 13 | const double KNavigationTimeout_s = 20.0; 14 | const double KHoverHeight_mm = 5000.0; 15 | const double KMeasureHeight_mm = 3500.0; 16 | const double KGPSConvergeThreshold_mm = 1500.0; 17 | const int KGPSBufferSize = 5; 18 | const int KNavigateLoopRate_hz = 10; 19 | const double KGPSScaleFactor = 10e8; 20 | 21 | class PelicanAgentParams { 22 | public: 23 | PelicanAgentParams(); 24 | 25 | bool LoadFromRosParams(ros::NodeHandle &ph); 26 | 27 | double hover_time_s; 28 | 29 | double navigate_wait_time_s; // waiting time before detecting convergence 30 | 31 | double navigate_timeout_s; 32 | 33 | double hover_height_mm; 34 | 35 | double measure_height_mm; 36 | 37 | double gps_converge_threshold_mm; 38 | 39 | int gps_buffer_size; 40 | 41 | int navigate_loop_rate_hz; 42 | 43 | double navigation_latitude_offset; 44 | 45 | double navigation_longitude_offset; 46 | 47 | double rtk_latitude_offset; 48 | 49 | double rtk_longitude_offset; 50 | 51 | double latitude_offset; 52 | 53 | double longitude_offset; 54 | 55 | }; // namespace scene 56 | } // namespace agent 57 | } // namespace sampling -------------------------------------------------------------------------------- /src/sampling_agent/include/sampling_agent/sampling_agent.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | #include "sampling_agent/sampling_agent_params.h" 12 | 13 | namespace sampling { 14 | namespace agent { 15 | 16 | /// robot state machine 17 | /// Default : IDLE 18 | /// Workflow 19 | /// Request : Request next interest point from master computer 20 | /// Navigate : navigate to target location 21 | /// Measure: measure data 22 | /// Report : report sample to master computer 23 | enum SamplingState { IDLE, REQUEST, NAVIGATE, MEASURE, REPORT, DIED }; 24 | 25 | class SamplingAgent { 26 | public: 27 | SamplingAgent() = delete; 28 | 29 | static std::unique_ptr MakeUniqueFromROS(ros::NodeHandle &nh, 30 | ros::NodeHandle &ph); 31 | 32 | bool Run(); 33 | 34 | protected: 35 | SamplingAgent(ros::NodeHandle &nh, const SamplingAgentParams ¶ms); 36 | 37 | bool RequestTarget(); 38 | 39 | virtual void ReportLocationCallback(const ros::TimerEvent &); 40 | 41 | virtual bool Navigate(); 42 | 43 | virtual bool CollectMeasurement(); 44 | 45 | bool ReportSample(); 46 | 47 | bool StopAgentService(sampling_msgs::StopAgent::Request &req, 48 | sampling_msgs::StopAgent::Response &res); 49 | 50 | bool CheckService(std_srvs::Trigger::Request &req, 51 | std_srvs::Trigger::Response &res); 52 | 53 | ros::Timer event_timer_; 54 | 55 | SamplingState agent_state_; 56 | 57 | SamplingAgentParams params_; 58 | 59 | ros::ServiceClient sampling_goal_service_; 60 | 61 | ros::ServiceClient measurement_service_; 62 | 63 | ros::ServiceClient notify_died_agent_service_; 64 | 65 | ros::ServiceServer stop_agent_server_; 66 | 67 | ros::ServiceServer check_server_; 68 | 69 | ros::Publisher agent_location_publisher_; 70 | 71 | ros::Publisher sample_publisher_; 72 | 73 | boost::optional current_position_; 74 | 75 | boost::optional target_position_; 76 | 77 | boost::optional measurement_; 78 | 79 | boost::optional start_time_; 80 | 81 | bool IsAgentAlive(); 82 | 83 | bool ReportDiedAgent(); 84 | 85 | bool last_run_is_done_; 86 | 87 | bool active_in_master_; 88 | 89 | bool StartRetreat(); 90 | }; 91 | } // namespace agent 92 | } // namespace sampling -------------------------------------------------------------------------------- /src/sampling_agent/include/sampling_agent/sampling_agent_params.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | namespace sampling { 9 | namespace agent { 10 | 11 | const double KBatteryLife_ROS_sec = 5000.0; 12 | const double KMaxSpeed_ms = 2; 13 | const double KRetreatPositionX_m = -1.0; 14 | const double KRetreatPositionY_m = -1.0; 15 | 16 | class SamplingAgentParams { 17 | public: 18 | SamplingAgentParams(); 19 | 20 | bool LoadFromRosParams(ros::NodeHandle &ph); 21 | 22 | std::string agent_id; 23 | 24 | double max_speed_ms; 25 | 26 | double batterylife_ros_sec; 27 | 28 | geometry_msgs::Point retreat_position; 29 | 30 | }; // namespace scene 31 | } // namespace agent 32 | } // namespace sampling -------------------------------------------------------------------------------- /src/sampling_agent/launch/hector_simulation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | agent_type: "HECTOR" 7 | agent_id: "hector0" 8 | max_speed_ms: 4.0 9 | batterylife_ros_sec: 5000 10 | navigation_height_m: 3.0 11 | takeoff_distance_m: 0.5 12 | 13 | 14 | -------------------------------------------------------------------------------- /src/sampling_agent/launch/jackal_simulation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | agent_type: "JACKAL" 7 | agent_id: "jackal0" 8 | 9 | 10 | 11 | 12 | agent_type: "JACKAL" 13 | agent_id: "jackal1" 14 | 15 | 16 | -------------------------------------------------------------------------------- /src/sampling_agent/node/sampling_agent_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "sampling_agent/sampling_agent.h" 4 | 5 | int main(int argc, char **argv) { 6 | ros::init(argc, argv, "sampling_agent_node"); 7 | ros::NodeHandle nh, ph("~"); 8 | ros::Rate r(3); 9 | 10 | std::unique_ptr agent = 11 | sampling::agent::SamplingAgent::MakeUniqueFromROS(nh, ph); 12 | 13 | if (agent == nullptr) { 14 | ROS_ERROR("Failed to create a sampling agent!"); 15 | return -1; 16 | } 17 | 18 | ros::AsyncSpinner spinner(0); 19 | spinner.start(); 20 | 21 | while (ros::ok()) { 22 | agent->Run(); 23 | r.sleep(); 24 | } 25 | 26 | return 0; 27 | } -------------------------------------------------------------------------------- /src/sampling_agent/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sampling_agent 4 | 0.0.0 5 | The sampling_agent package 6 | 7 | Yang Zhang 8 | Yunfei Shi 9 | Ning Wang 10 | Jianmin Zheng 11 | 12 | BSD 13 | 14 | catkin 15 | roscpp 16 | rospy 17 | std_msgs 18 | message_runtime 19 | visualization_msgs 20 | actionlib 21 | actionlib_msgs 22 | sampling_measurement 23 | sampling_msgs 24 | tf 25 | cmake_modules 26 | sensor_msgs 27 | tf2 28 | tf2_ros 29 | hector_navigation 30 | hector_navigation_msgs 31 | hector_uav_msgs 32 | 33 | -------------------------------------------------------------------------------- /src/sampling_agent/src/hector_agent.cpp: -------------------------------------------------------------------------------- 1 | #include "sampling_agent/hector_agent.h" 2 | 3 | #include 4 | #include 5 | 6 | namespace sampling { 7 | namespace agent { 8 | 9 | HectorAgent::HectorAgent(ros::NodeHandle &nh, 10 | const SamplingAgentParams &agent_params, 11 | const HectorAgentParams &hector_params) 12 | : SamplingAgent(nh, agent_params), 13 | hector_params_(hector_params), 14 | taken_off_(false) { 15 | odom_subscriber_ = 16 | nh.subscribe(agent_params.agent_id + "/ground_truth/state", 1, 17 | &HectorAgent::UpdatePositionFromOdom, this); 18 | 19 | hector_takeoff_client_ = nh.serviceClient( 20 | agent_params.agent_id + "/hector_takeoff"); 21 | 22 | hector_navigate_client_ = 23 | nh.serviceClient( 24 | agent_params.agent_id + "/hector_navigation"); 25 | } 26 | 27 | std::unique_ptr HectorAgent::MakeUniqueFromROSParam( 28 | ros::NodeHandle &nh, ros::NodeHandle &ph) { 29 | SamplingAgentParams agent_params; 30 | if (!agent_params.LoadFromRosParams(ph)) { 31 | ROS_ERROR("Failed to load agent parameters for hector agent!"); 32 | return nullptr; 33 | } 34 | 35 | HectorAgentParams hector_params; 36 | if (!hector_params.LoadFromRosParams(ph)) { 37 | ROS_ERROR("Failed to load local parameters for hector agent!"); 38 | return nullptr; 39 | } 40 | 41 | return std::unique_ptr( 42 | new HectorAgent(nh, agent_params, hector_params)); 43 | } 44 | 45 | bool HectorAgent::Navigate() { 46 | if (!taken_off_) { 47 | hector_navigation_msgs::Takeoff take_off_srv; 48 | take_off_srv.request.takeoff_distance_m = hector_params_.takeoff_distance_m; 49 | if (hector_takeoff_client_.call(take_off_srv)) { 50 | if (take_off_srv.response.success) { 51 | ROS_INFO_STREAM("Hector" << params_.agent_id << "has taken off!"); 52 | taken_off_ = true; 53 | } else { 54 | ROS_INFO_STREAM("Hector" << params_.agent_id << " failed to take off!"); 55 | return false; 56 | } 57 | } else { 58 | ROS_INFO_STREAM("Hector" << params_.agent_id 59 | << " lost connect to take-off service!"); 60 | return false; 61 | } 62 | } 63 | 64 | if (!target_position_.is_initialized()) { 65 | ROS_INFO_STREAM("Hector" << params_.agent_id 66 | << " failed to receive sampling target!"); 67 | return false; 68 | } 69 | hector_navigation_msgs::Navigation navigation_srv; 70 | navigation_srv.request.goal = target_position_.get(); 71 | navigation_srv.request.goal.z = hector_params_.navigation_height_m; 72 | navigation_srv.request.speed = params_.max_speed_ms; 73 | 74 | if (!hector_navigate_client_.call(navigation_srv)) { 75 | ROS_INFO_STREAM("Hector" << params_.agent_id 76 | << " lost connect to navigation service!"); 77 | } else { 78 | if (navigation_srv.response.return_type == 0) { 79 | ROS_INFO_STREAM("Hector" << params_.agent_id 80 | << "succeeded in navigation!"); 81 | return true; 82 | } else { 83 | ROS_INFO_STREAM("Hector" << params_.agent_id << "failed navigation!"); 84 | return false; 85 | } 86 | } 87 | return false; 88 | } 89 | 90 | void HectorAgent::UpdatePositionFromOdom(const nav_msgs::Odometry &msg) { 91 | current_position_ = boost::make_optional(msg.pose.pose.position); 92 | } 93 | 94 | } // namespace agent 95 | } // namespace sampling 96 | -------------------------------------------------------------------------------- /src/sampling_agent/src/hector_agent_params.cpp: -------------------------------------------------------------------------------- 1 | #include "sampling_agent/hector_agent_params.h" 2 | 3 | namespace sampling { 4 | namespace agent { 5 | 6 | HectorAgentParams::HectorAgentParams() {} 7 | 8 | bool HectorAgentParams::LoadFromRosParams(ros::NodeHandle& ph) { 9 | ph.param("navigation_frame", navigation_frame, KNavigationFrame); 10 | 11 | ph.param("navigation_height_m", navigation_height_m, 12 | KNavigationHeight_m); 13 | 14 | ph.param("takeoff_distance_m", takeoff_distance_m, 15 | KTakeoffDistance_m); 16 | return true; 17 | } 18 | 19 | } // namespace agent 20 | } // namespace sampling -------------------------------------------------------------------------------- /src/sampling_agent/src/jackal_agent.cpp: -------------------------------------------------------------------------------- 1 | #include "sampling_agent/jackal_agent.h" 2 | 3 | #include 4 | 5 | namespace sampling { 6 | namespace agent { 7 | 8 | JackalAgent::JackalAgent(ros::NodeHandle &nh, 9 | const SamplingAgentParams &agent_params, 10 | const JackalAgentParams &jackal_params, 11 | std::unique_ptr jackal_navigator) 12 | : SamplingAgent(nh, agent_params), 13 | jackal_params_(jackal_params), 14 | jackal_navigator_(std::move(jackal_navigator)) { 15 | switch (jackal_params.navigation_mode) { 16 | case ODOM: { 17 | odom_subscriber_ = 18 | nh.subscribe(agent_params.agent_id + "/odometry/local_filtered", 1, 19 | &JackalAgent::UpdatePositionFromOdom, this); 20 | break; 21 | } 22 | case GPS: { 23 | gps_subscriber_ = nh.subscribe(agent_params.agent_id + "/navsat/fix", 1, 24 | &JackalAgent::UpdatePositionFromGPS, this); 25 | break; 26 | } 27 | default: 28 | break; 29 | } 30 | 31 | nh.setParam("/" + agent_params.agent_id + 32 | "/jackal_velocity_controller/linear/x/max_velocity", 33 | agent_params.max_speed_ms); 34 | 35 | nh.setParam( 36 | "/" + agent_params.agent_id + "/move_base/TrajectoryPlannerROS/max_vel_x", 37 | agent_params.max_speed_ms); 38 | } 39 | 40 | std::unique_ptr JackalAgent::MakeUniqueFromROSParam( 41 | ros::NodeHandle &nh, ros::NodeHandle &ph) { 42 | SamplingAgentParams agent_params; 43 | if (!agent_params.LoadFromRosParams(ph)) { 44 | ROS_ERROR("Failed to load agent parameters for hector agent!"); 45 | return nullptr; 46 | } 47 | 48 | JackalAgentParams jackal_params; 49 | if (!jackal_params.LoadFromRosParams(ph)) { 50 | ROS_ERROR_STREAM( 51 | "Failed to load parameters for agent : " << agent_params.agent_id); 52 | return nullptr; 53 | } 54 | std::unique_ptr jackal_navigator = 55 | std::unique_ptr( 56 | new JackalNavigator(agent_params.agent_id + "/move_base", true)); 57 | if (!jackal_navigator->waitForServer(ros::Duration(10.0))) { 58 | ROS_INFO_STREAM("Missing move base action for Jackal!"); 59 | return nullptr; 60 | } 61 | 62 | return std::unique_ptr(new JackalAgent( 63 | nh, agent_params, jackal_params, std::move(jackal_navigator))); 64 | } // namespace agent 65 | 66 | bool JackalAgent::GPStoOdom(const double &latitude, const double &longitude, 67 | geometry_msgs::PointStamped &odom_point) { 68 | double utm_x, utm_y; 69 | geometry_msgs::PointStamped UTM_point; 70 | std::string utm_zone; 71 | 72 | // convert lat/long to utm 73 | RobotLocalization::NavsatConversions::LLtoUTM(latitude, longitude, utm_y, 74 | utm_x, utm_zone); 75 | 76 | // Construct UTM_point and odom point geometry messages 77 | UTM_point.header.frame_id = "utm"; 78 | UTM_point.header.stamp = ros::Time::now(); 79 | UTM_point.point.x = utm_x; 80 | UTM_point.point.y = utm_y; 81 | UTM_point.point.z = 0; 82 | 83 | try { 84 | odom_point.header.stamp = ros::Time::now(); 85 | listener_.transformPoint(jackal_params_.navigation_frame, UTM_point, 86 | odom_point); 87 | } catch (tf::TransformException &ex) { 88 | ROS_WARN("%s", ex.what()); 89 | return false; 90 | } 91 | return true; 92 | } 93 | 94 | bool JackalAgent::Navigate() { 95 | move_base_msgs::MoveBaseGoal navigation_goal; 96 | const std::string navigation_frame = params_.agent_id + "/odom"; 97 | navigation_goal.target_pose.header.frame_id = navigation_frame; 98 | navigation_goal.target_pose.header.stamp = ros::Time::now(); 99 | navigation_goal.target_pose.pose.orientation.w = 1.0; 100 | 101 | switch (jackal_params_.navigation_mode) { 102 | case ODOM: { 103 | navigation_goal.target_pose.pose.position = target_position_.get(); 104 | 105 | geometry_msgs::PointStamped point_in, point_out; 106 | point_in.header.frame_id = jackal_params_.navigation_frame; 107 | point_in.point = target_position_.get(); 108 | 109 | try { 110 | // geometry_msgs::PointStamped base_point; 111 | listener_.transformPoint(navigation_frame, point_in, point_out); 112 | navigation_goal.target_pose.pose.position = point_out.point; 113 | 114 | } catch (tf::TransformException &ex) { 115 | ROS_ERROR_STREAM( 116 | "Received an exception trying to transform a point from " 117 | << jackal_params_.navigation_frame << "to " 118 | << point_in.header.frame_id << " " << ex.what()); 119 | return false; 120 | } 121 | break; 122 | } 123 | case GPS: { 124 | geometry_msgs::PointStamped odom_target_point; 125 | if (!GPStoOdom(target_position_.get().x, target_position_.get().y, 126 | odom_target_point)) { 127 | ROS_INFO_STREAM( 128 | "Failed to get local odometry target for jackal GPS navigation!"); 129 | return false; 130 | } 131 | navigation_goal.target_pose.pose.position = odom_target_point.point; 132 | break; 133 | } 134 | default: { 135 | ROS_INFO_STREAM("Unknown jackal navigation mode for jackal!"); 136 | return false; 137 | } 138 | } 139 | 140 | jackal_navigator_->sendGoalAndWait( 141 | navigation_goal, ros::Duration(jackal_params_.execute_timeout_s), 142 | ros::Duration(jackal_params_.preempt_timeout_s)); 143 | if (jackal_navigator_->getState() == 144 | actionlib::SimpleClientGoalState::SUCCEEDED) { 145 | ROS_INFO_STREAM("Jackal reached goal!"); 146 | 147 | return true; 148 | } else { 149 | ROS_INFO_STREAM("Robot " 150 | << params_.agent_id 151 | << " failed to reach the target location with state " 152 | << jackal_navigator_->getState().toString()); 153 | return false; 154 | } 155 | } 156 | 157 | void JackalAgent::UpdatePositionFromOdom(const nav_msgs::Odometry &msg) { 158 | geometry_msgs::PointStamped point_in, point_out; 159 | point_in.header = msg.header; 160 | point_in.point = msg.pose.pose.position; 161 | 162 | try { 163 | // geometry_msgs::PointStamped base_point; 164 | listener_.transformPoint(jackal_params_.navigation_frame, point_in, 165 | point_out); 166 | point_out.header.frame_id = jackal_params_.navigation_frame; 167 | current_position_ = boost::make_optional(point_out.point); 168 | 169 | } catch (tf::TransformException &ex) { 170 | ROS_ERROR_STREAM("Received an exception trying to transform a point from " 171 | << point_in.header.frame_id << "to " 172 | << jackal_params_.navigation_frame << " " << ex.what()); 173 | } 174 | } 175 | 176 | void JackalAgent::UpdatePositionFromGPS(const sensor_msgs::NavSatFix &msg) { 177 | current_position_ = boost::none; 178 | geometry_msgs::PointStamped odom_point; 179 | if (GPStoOdom(msg.latitude, msg.longitude, odom_point)) { 180 | current_position_ = boost::make_optional(odom_point.point); 181 | } 182 | } 183 | 184 | } // namespace agent 185 | } // namespace sampling 186 | -------------------------------------------------------------------------------- /src/sampling_agent/src/jackal_agent_params.cpp: -------------------------------------------------------------------------------- 1 | #include "sampling_agent/jackal_agent_params.h" 2 | 3 | namespace sampling { 4 | namespace agent { 5 | 6 | JackalAgentParams::JackalAgentParams() {} 7 | 8 | bool JackalAgentParams::LoadFromRosParams(ros::NodeHandle& ph) { 9 | ph.param("navigation_frame", navigation_frame, 10 | KJackalWorldFrame); 11 | 12 | std::string navigation_mode_str; 13 | 14 | ph.param("navigation_mode", navigation_mode_str, 15 | KJackalNavigationMode); 16 | if (navigation_mode_str.compare("ODOM") == 0) { 17 | navigation_mode = ODOM; 18 | } else if (navigation_mode_str.compare("GPS") == 0) { 19 | navigation_mode = GPS; 20 | } else { 21 | ROS_ERROR_STREAM("Unkown navigation mode for Jackal!"); 22 | return false; 23 | } 24 | 25 | ph.param("execute_timeout_s", execute_timeout_s, 26 | KJackalExecuteTimeout_s); 27 | 28 | ph.param("preempt_timeout_s", preempt_timeout_s, 29 | KJackalPreemptTimeout_s); 30 | return true; 31 | } 32 | 33 | } // namespace agent 34 | } // namespace sampling -------------------------------------------------------------------------------- /src/sampling_agent/src/pelican_agent_params.cpp: -------------------------------------------------------------------------------- 1 | #include "sampling_agent/pelican_agent_params.h" 2 | 3 | namespace sampling { 4 | namespace agent { 5 | 6 | PelicanAgentParams::PelicanAgentParams() {} 7 | 8 | bool PelicanAgentParams::LoadFromRosParams(ros::NodeHandle &ph) { 9 | ph.param("hover_time_s", hover_time_s, KHoverTime_s); 10 | 11 | ph.param("navigate_wait_time_s", navigate_wait_time_s, 12 | KNavigationWaitTime_s); 13 | 14 | ph.param("navigate_timeout_s", navigate_timeout_s, 15 | KNavigationTimeout_s); 16 | 17 | ph.param("hover_height_mm", hover_height_mm, KHoverHeight_mm); 18 | 19 | ph.param("measure_height_mm", measure_height_mm, KMeasureHeight_mm); 20 | 21 | ph.param("gps_converge_threshold_mm", gps_converge_threshold_mm, 22 | KGPSConvergeThreshold_mm); 23 | 24 | ph.param("gps_buffer_size", gps_buffer_size, KGPSBufferSize); 25 | 26 | ph.param("navigate_loop_rate_hz", navigate_loop_rate_hz, 27 | KNavigateLoopRate_hz); 28 | 29 | if (!ph.getParam("navigation_latitude_offset", navigation_latitude_offset)) { 30 | ROS_ERROR("Error! Missing pelican navigation latitude offset!"); 31 | return false; 32 | } 33 | 34 | if (!ph.getParam("navigation_longitude_offset", 35 | navigation_longitude_offset)) { 36 | ROS_ERROR("Error! Missing pelican navigation longitude offset!"); 37 | return false; 38 | } 39 | 40 | if (!ph.getParam("rtk_latitude_offset", rtk_latitude_offset)) { 41 | ROS_ERROR("Error! Missing rtk latitude offset!"); 42 | return false; 43 | } 44 | 45 | if (!ph.getParam("rtk_longitude_offset", rtk_longitude_offset)) { 46 | ROS_ERROR("Error! Missing rtk longitude offset!"); 47 | return false; 48 | } 49 | 50 | if (!ph.getParam("latitude_offset", latitude_offset)) { 51 | ROS_ERROR("Error! Missing latitude offset!"); 52 | return false; 53 | } 54 | 55 | if (!ph.getParam("longitude_offset", longitude_offset)) { 56 | ROS_ERROR("Error! Missing longitude offset!"); 57 | return false; 58 | } 59 | 60 | return true; 61 | } 62 | 63 | } // namespace agent 64 | } // namespace sampling -------------------------------------------------------------------------------- /src/sampling_agent/src/sampling_agent_params.cpp: -------------------------------------------------------------------------------- 1 | #include "sampling_agent/sampling_agent_params.h" 2 | 3 | #include 4 | 5 | namespace sampling { 6 | namespace agent { 7 | 8 | SamplingAgentParams::SamplingAgentParams() {} 9 | 10 | bool SamplingAgentParams::LoadFromRosParams(ros::NodeHandle& ph) { 11 | if (!ph.getParam("agent_id", agent_id)) { 12 | ROS_ERROR_STREAM("Missing agent id!"); 13 | return false; 14 | } 15 | ph.param("max_speed_ms", max_speed_ms, KMaxSpeed_ms); 16 | 17 | ph.param("batterylife_ros_sec", batterylife_ros_sec, 18 | KBatteryLife_ROS_sec); 19 | 20 | double retreat_position_x_m, retreat_position_y_m; 21 | 22 | ph.param("retreat_position_x_m", retreat_position_x_m, 23 | KRetreatPositionX_m); 24 | 25 | ph.param("retreat_position_y_m", retreat_position_y_m, 26 | KRetreatPositionY_m); 27 | 28 | retreat_position.x = retreat_position_x_m; 29 | retreat_position.y = retreat_position_y_m; 30 | retreat_position.z = 0.0; 31 | 32 | return true; 33 | } 34 | 35 | } // namespace agent 36 | } // namespace sampling -------------------------------------------------------------------------------- /src/sampling_core/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(sampling_core) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | roscpp 8 | rospy 9 | std_msgs 10 | visualization_msgs 11 | actionlib 12 | actionlib_msgs 13 | message_runtime 14 | sampling_measurement 15 | sampling_utils 16 | sampling_msgs 17 | sampling_agent 18 | sampling_partition 19 | sampling_online_learning 20 | sampling_visualization 21 | roslib 22 | std_srvs 23 | ) 24 | 25 | find_package(Eigen3 REQUIRED) 26 | find_package(Boost REQUIRED COMPONENTS thread) 27 | 28 | catkin_package( 29 | INCLUDE_DIRS include ${EIGEN3_INCLUDE_DIRS} 30 | CATKIN_DEPENDS message_runtime 31 | ) 32 | 33 | include_directories( 34 | ${catkin_INCLUDE_DIRS} 35 | ${EIGEN3_INCLUDE_DIR} 36 | ${Boost_INCLUDE_DIRS} 37 | include 38 | ) 39 | 40 | add_library(${PROJECT_NAME} 41 | src/sampling_core_params.cpp 42 | src/sampling_core.cpp 43 | src/sampling_core_performance_evaluation.cpp 44 | ) 45 | 46 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 47 | 48 | add_executable(heterogeneous_adaptive_sampling_node node/heterogeneous_adaptive_sampling_node.cpp) 49 | target_link_libraries(heterogeneous_adaptive_sampling_node ${PROJECT_NAME} ${catkin_LIBRARIES} ) -------------------------------------------------------------------------------- /src/sampling_core/config/scenario0_hetero.yaml: -------------------------------------------------------------------------------- 1 | #agents 2 | agent_ids: ["hector0", "jackal0", "jackal1"] 3 | 4 | # data 5 | random_initialization: true 6 | 7 | enable_performance_evaluation: True 8 | 9 | # Model Update 10 | model_update_frequency_count: 1 11 | 12 | # partition parameters 13 | HeterogeneousProperty: 14 | - heterogenities: ["DISTANCE", "SPEED", "BATTERY_LIFE", "TRAVERSABILITY"] 15 | weight_factor: [1.0, 2.0, 1.5, 1000.0] 16 | - agent_id: "hector0" 17 | heterogeneity_primitive: [1.0, 0.5, 1.0, 1.0] 18 | number_control_area: 0 19 | - agent_id: "jackal0" 20 | heterogeneity_primitive: [1.0, 0.75, 0.75, 1.0] 21 | number_control_area: 0 22 | - agent_id: "jackal1" 23 | heterogeneity_primitive: [1.0, 1.0, 0.75, 1.0] 24 | number_control_area: 0 25 | 26 | #Online Learning Handler 27 | learning_type: "GREEDY" 28 | learning_beta: 0.5 29 | 30 | #Visualization 31 | VisualizationProperty: 32 | - visualization_type: "LOCATION" 33 | name: "AgentLocation" 34 | offset: [0.0, 0.0] 35 | scale: [1.0, 1.0] 36 | - visualization_type: "PARTITION" 37 | name: "Partition" 38 | offset: [0.0, 0.0] 39 | scale: [1.0, 1.0] 40 | - visualization_type: "GRID" 41 | name: "PredictionMean" 42 | offset: [15.0, 0.0] 43 | scale: [1.0, 1.0] 44 | bounds: [-1.0, 7.5] 45 | - visualization_type: "GRID" 46 | name: "PredictionVariance" 47 | offset: [30.0, 0.0] 48 | scale: [1.0, 1.0] 49 | bounds: [0.0, 1.0] -------------------------------------------------------------------------------- /src/sampling_core/config/scenario0_homo.yaml: -------------------------------------------------------------------------------- 1 | #agents 2 | agent_ids: ["hector0", "jackal0", "jackal1"] 3 | 4 | # data 5 | random_initialization: true 6 | 7 | enable_performance_evaluation: True 8 | 9 | # Model Update 10 | model_update_frequency_count: 1 11 | 12 | # partition parameters 13 | HeterogeneousProperty: 14 | - heterogenities: ["DISTANCE"] 15 | weight_factor: [1.0] 16 | - agent_id: "hector0" 17 | heterogeneity_primitive: [1.0] 18 | number_control_area: 0 19 | - agent_id: "jackal0" 20 | heterogeneity_primitive: [1.0] 21 | number_control_area: 0 22 | - agent_id: "jackal1" 23 | heterogeneity_primitive: [1.0] 24 | number_control_area: 0 25 | 26 | #Online Learning Handler 27 | learning_type: "GREEDY" 28 | learning_beta: 0.5 29 | 30 | #Visualization 31 | VisualizationProperty: 32 | - visualization_type: "LOCATION" 33 | name: "AgentLocation" 34 | offset: [0.0, 0.0] 35 | scale: [1.0, 1.0] 36 | - visualization_type: "PARTITION" 37 | name: "Partition" 38 | offset: [0.0, 0.0] 39 | scale: [1.0, 1.0] 40 | - visualization_type: "GRID" 41 | name: "PredictionMean" 42 | offset: [15.0, 0.0] 43 | scale: [1.0, 1.0] 44 | bounds: [-1.0, 7.5] 45 | - visualization_type: "GRID" 46 | name: "PredictionVariance" 47 | offset: [30.0, 0.0] 48 | scale: [1.0, 1.0] 49 | bounds: [0.0, 1.0] -------------------------------------------------------------------------------- /src/sampling_core/config/scenario1_hetero.yaml: -------------------------------------------------------------------------------- 1 | #agents 2 | agent_ids: ["hector0", "jackal0", "jackal1"] 3 | 4 | # data 5 | random_initialization: true 6 | 7 | enable_performance_evaluation: True 8 | 9 | # Model Update 10 | model_update_frequency_count: 1 11 | 12 | # partition parameters 13 | HeterogeneousProperty: 14 | - heterogenities: ["DISTANCE", "SPEED", "BATTERY_LIFE", "TRAVERSABILITY"] 15 | weight_factor: [1.0, 2.0, 1.5, 1000.0] 16 | - agent_id: "hector0" 17 | heterogeneity_primitive: [1.0, 0.5, 1.0, 1.0] 18 | number_control_area: 0 19 | - agent_id: "jackal0" 20 | heterogeneity_primitive: [1.0, 0.75, 0.75, 1.0] 21 | number_control_area: 2 22 | control_area_center_0: [5.0, 10.0] 23 | control_area_radius_0: 1.5 24 | control_area_center_1: [7.0, 3.0] 25 | control_area_radius_1: 1.0 26 | - agent_id: "jackal1" 27 | heterogeneity_primitive: [1.0, 1.0, 0.75, 1.0] 28 | number_control_area: 2 29 | control_area_center_0: [5.0, 10.0] 30 | control_area_radius_0: 1.5 31 | control_area_center_1: [7.0, 3.0] 32 | control_area_radius_1: 1.0 33 | 34 | #Online Learning Handler 35 | learning_type: "GREEDY" 36 | learning_beta: 0.5 37 | 38 | #Visualization 39 | VisualizationProperty: 40 | - visualization_type: "LOCATION" 41 | name: "AgentLocation" 42 | offset: [0.0, 0.0] 43 | scale: [1.0, 1.0] 44 | - visualization_type: "PARTITION" 45 | name: "Partition" 46 | offset: [0.0, 0.0] 47 | scale: [1.0, 1.0] 48 | - visualization_type: "GRID" 49 | name: "PredictionMean" 50 | offset: [15.0, 0.0] 51 | scale: [1.0, 1.0] 52 | bounds: [-1.0, 7.5] 53 | - visualization_type: "GRID" 54 | name: "PredictionVariance" 55 | offset: [30.0, 0.0] 56 | scale: [1.0, 1.0] 57 | bounds: [0.0, 1.0] -------------------------------------------------------------------------------- /src/sampling_core/config/scenario1_homo.yaml: -------------------------------------------------------------------------------- 1 | #agents 2 | agent_ids: ["hector0", "jackal0", "jackal1"] 3 | 4 | # data 5 | random_initialization: true 6 | 7 | enable_performance_evaluation: True 8 | 9 | # Model Update 10 | model_update_frequency_count: 1 11 | 12 | # partition parameters 13 | HeterogeneousProperty: 14 | - heterogenities: ["DISTANCE"] 15 | weight_factor: [1.0] 16 | - agent_id: "hector0" 17 | heterogeneity_primitive: [1.0] 18 | number_control_area: 0 19 | - agent_id: "jackal0" 20 | heterogeneity_primitive: [1.0] 21 | number_control_area: 0 22 | - agent_id: "jackal1" 23 | heterogeneity_primitive: [1.0] 24 | number_control_area: 0 25 | 26 | #Online Learning Handler 27 | learning_type: "GREEDY" 28 | learning_beta: 0.5 29 | 30 | #Visualization 31 | VisualizationProperty: 32 | - visualization_type: "LOCATION" 33 | name: "AgentLocation" 34 | offset: [0.0, 0.0] 35 | scale: [1.0, 1.0] 36 | - visualization_type: "PARTITION" 37 | name: "Partition" 38 | offset: [0.0, 0.0] 39 | scale: [1.0, 1.0] 40 | - visualization_type: "GRID" 41 | name: "PredictionMean" 42 | offset: [15.0, 0.0] 43 | scale: [1.0, 1.0] 44 | bounds: [-1.0, 7.5] 45 | - visualization_type: "GRID" 46 | name: "PredictionVariance" 47 | offset: [30.0, 0.0] 48 | scale: [1.0, 1.0] 49 | bounds: [0.0, 1.0] -------------------------------------------------------------------------------- /src/sampling_core/config/scenario2_hetero.yaml: -------------------------------------------------------------------------------- 1 | #agents 2 | agent_ids: ["hector0", "jackal0", "jackal1"] 3 | 4 | # data 5 | random_initialization: true 6 | 7 | enable_performance_evaluation: True 8 | 9 | # Model Update 10 | model_update_frequency_count: 1 11 | 12 | # partition parameters 13 | HeterogeneousProperty: 14 | - heterogenities: ["DISTANCE", "SPEED", "BATTERY_LIFE", "TRAVERSABILITY"] 15 | weight_factor: [1.0, 2.0, 1.5, 1000.0] 16 | - agent_id: "hector0" 17 | heterogeneity_primitive: [1.0, 0.5, 1.0, 1.0] 18 | number_control_area: 0 19 | - agent_id: "jackal0" 20 | heterogeneity_primitive: [1.0, 0.75, 0.75, 1.0] 21 | number_control_area: 2 22 | control_area_center_0: [3.0, 3.0] 23 | control_area_radius_0: 1.5 24 | control_area_center_1: [9.0, 10.0] 25 | control_area_radius_1: 1.5 26 | - agent_id: "jackal1" 27 | heterogeneity_primitive: [1.0, 1.0, 0.75, 1.0] 28 | number_control_area: 2 29 | control_area_center_0: [3.0, 3.0] 30 | control_area_radius_0: 1.5 31 | control_area_center_1: [9.0, 10.0] 32 | control_area_radius_1: 1.5 33 | 34 | #Online Learning Handler 35 | learning_type: "GREEDY" 36 | learning_beta: 0.5 37 | 38 | #Visualization 39 | VisualizationProperty: 40 | - visualization_type: "LOCATION" 41 | name: "AgentLocation" 42 | offset: [0.0, 0.0] 43 | scale: [1.0, 1.0] 44 | - visualization_type: "PARTITION" 45 | name: "Partition" 46 | offset: [0.0, 0.0] 47 | scale: [1.0, 1.0] 48 | - visualization_type: "GRID" 49 | name: "PredictionMean" 50 | offset: [15.0, 0.0] 51 | scale: [1.0, 1.0] 52 | bounds: [-1.0, 7.5] 53 | - visualization_type: "GRID" 54 | name: "PredictionVariance" 55 | offset: [30.0, 0.0] 56 | scale: [1.0, 1.0] 57 | bounds: [0.0, 1.0] -------------------------------------------------------------------------------- /src/sampling_core/config/scenario2_homo.yaml: -------------------------------------------------------------------------------- 1 | #agents 2 | agent_ids: ["hector0", "jackal0", "jackal1"] 3 | 4 | # data 5 | random_initialization: true 6 | 7 | enable_performance_evaluation: True 8 | 9 | # Model Update 10 | model_update_frequency_count: 1 11 | 12 | # partition parameters 13 | HeterogeneousProperty: 14 | - heterogenities: ["DISTANCE"] 15 | weight_factor: [1.0] 16 | - agent_id: "hector0" 17 | heterogeneity_primitive: [1.0] 18 | number_control_area: 0 19 | - agent_id: "jackal0" 20 | heterogeneity_primitive: [1.0] 21 | number_control_area: 0 22 | - agent_id: "jackal1" 23 | heterogeneity_primitive: [1.0] 24 | number_control_area: 0 25 | 26 | 27 | #Online Learning Handler 28 | learning_type: "GREEDY" 29 | learning_beta: 0.5 30 | 31 | #Visualization 32 | VisualizationProperty: 33 | - visualization_type: "LOCATION" 34 | name: "AgentLocation" 35 | offset: [0.0, 0.0] 36 | scale: [1.0, 1.0] 37 | - visualization_type: "PARTITION" 38 | name: "Partition" 39 | offset: [0.0, 0.0] 40 | scale: [1.0, 1.0] 41 | - visualization_type: "GRID" 42 | name: "PredictionMean" 43 | offset: [15.0, 0.0] 44 | scale: [1.0, 1.0] 45 | bounds: [-1.0, 7.5] 46 | - visualization_type: "GRID" 47 | name: "PredictionVariance" 48 | offset: [30.0, 0.0] 49 | scale: [1.0, 1.0] 50 | bounds: [0.0, 1.0] -------------------------------------------------------------------------------- /src/sampling_core/config/scenario3_hetero.yaml: -------------------------------------------------------------------------------- 1 | #agents 2 | agent_ids: ["hector0", "jackal0", "jackal1"] 3 | 4 | # data 5 | random_initialization: true 6 | 7 | enable_performance_evaluation: True 8 | 9 | # Model Update 10 | model_update_frequency_count: 1 11 | 12 | # partition parameters 13 | HeterogeneousProperty: 14 | - heterogenities: ["DISTANCE", "SPEED", "BATTERY_LIFE", "TRAVERSABILITY"] 15 | weight_factor: [1.0, 2.0, 1.5, 1000.0] 16 | - agent_id: "hector0" 17 | heterogeneity_primitive: [1.0, 0.5, 1.0, 1.0] 18 | number_control_area: 0 19 | - agent_id: "jackal0" 20 | heterogeneity_primitive: [1.0, 0.75, 0.75, 1.0] 21 | number_control_area: 1 22 | control_area_center_0: [1.0, 7.0] 23 | control_area_radius_0: 2.5 24 | - agent_id: "jackal1" 25 | heterogeneity_primitive: [1.0, 1.0, 0.75, 1.0] 26 | number_control_area: 1 27 | control_area_center_0: [1.0, 7.0] 28 | control_area_radius_0: 2.5 29 | #Online Learning Handler 30 | learning_type: "GREEDY" 31 | learning_beta: 0.5 32 | 33 | #Visualization 34 | VisualizationProperty: 35 | - visualization_type: "LOCATION" 36 | name: "AgentLocation" 37 | offset: [0.0, 0.0] 38 | scale: [1.0, 1.0] 39 | - visualization_type: "PARTITION" 40 | name: "Partition" 41 | offset: [0.0, 0.0] 42 | scale: [1.0, 1.0] 43 | - visualization_type: "GRID" 44 | name: "PredictionMean" 45 | offset: [15.0, 0.0] 46 | scale: [1.0, 1.0] 47 | bounds: [-1.0, 7.5] 48 | - visualization_type: "GRID" 49 | name: "PredictionVariance" 50 | offset: [30.0, 0.0] 51 | scale: [1.0, 1.0] 52 | bounds: [0.0, 1.0] -------------------------------------------------------------------------------- /src/sampling_core/config/scenario3_homo.yaml: -------------------------------------------------------------------------------- 1 | #agents 2 | agent_ids: ["hector0", "jackal0", "jackal1"] 3 | 4 | # data 5 | random_initialization: true 6 | 7 | enable_performance_evaluation: True 8 | 9 | # Model Update 10 | model_update_frequency_count: 1 11 | 12 | # partition parameters 13 | HeterogeneousProperty: 14 | - heterogenities: ["DISTANCE"] 15 | weight_factor: [1.0] 16 | - agent_id: "hector0" 17 | heterogeneity_primitive: [1.0] 18 | number_control_area: 0 19 | - agent_id: "jackal0" 20 | heterogeneity_primitive: [1.0] 21 | number_control_area: 0 22 | - agent_id: "jackal1" 23 | heterogeneity_primitive: [1.0] 24 | number_control_area: 0 25 | 26 | #Online Learning Handler 27 | learning_type: "GREEDY" 28 | learning_beta: 0.5 29 | 30 | #Visualization 31 | VisualizationProperty: 32 | - visualization_type: "LOCATION" 33 | name: "AgentLocation" 34 | offset: [0.0, 0.0] 35 | scale: [1.0, 1.0] 36 | - visualization_type: "PARTITION" 37 | name: "Partition" 38 | offset: [0.0, 0.0] 39 | scale: [1.0, 1.0] 40 | - visualization_type: "GRID" 41 | name: "PredictionMean" 42 | offset: [15.0, 0.0] 43 | scale: [1.0, 1.0] 44 | bounds: [-1.0, 7.5] 45 | - visualization_type: "GRID" 46 | name: "PredictionVariance" 47 | offset: [30.0, 0.0] 48 | scale: [1.0, 1.0] 49 | bounds: [0.0, 1.0] -------------------------------------------------------------------------------- /src/sampling_core/include/sampling_core/sampling_core.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | #include "sampling_core/sampling_core_params.h" 8 | #include "sampling_core/sampling_core_performance_evaluation.h" 9 | #include "sampling_msgs/AddSampleToModel.h" 10 | #include "sampling_msgs/AgentLocation.h" 11 | #include "sampling_msgs/KillAgent.h" 12 | #include "sampling_msgs/Sample.h" 13 | #include "sampling_msgs/SamplingGoal.h" 14 | #include "sampling_online_learning/online_learning_handler.h" 15 | #include "sampling_partition/weighted_voronoi_partition.h" 16 | #include "sampling_visualization/agent_visualization_handler.h" 17 | #include "sampling_visualization/grid_visualization_handler.h" 18 | 19 | namespace sampling { 20 | namespace core { 21 | 22 | // todo agent die 23 | 24 | const std::string KModelingNamespace = "modeling/"; 25 | 26 | class SamplingCore { 27 | public: 28 | SamplingCore() = delete; 29 | 30 | static std::unique_ptr MakeUniqueFromRos(ros::NodeHandle &nh, 31 | ros::NodeHandle &ph); 32 | 33 | bool Loop(); 34 | 35 | private: 36 | SamplingCore( 37 | ros::NodeHandle &nh, const SamplingCoreParams ¶ms, 38 | std::unique_ptr partition_handler, 39 | std::unique_ptr learning_handler, 40 | std::unique_ptr 41 | agent_visualization_handler, 42 | std::vector> 43 | &grid_visualization_handlers, 44 | std::unique_ptr evaluation_handler); 45 | 46 | SamplingCoreParams params_; 47 | 48 | // Agent 49 | ros::Subscriber agent_location_subscriber_; 50 | 51 | ros::Subscriber sample_subscriber_; 52 | 53 | std::unordered_map 54 | agents_locations_; 55 | 56 | ros::ServiceClient modeling_add_test_location_client_; 57 | 58 | ros::ServiceClient modeling_add_sample_client_; 59 | 60 | ros::ServiceClient modeling_update_model_client_; 61 | 62 | ros::ServiceClient modeling_predict_client_; 63 | 64 | ros::ServiceServer kill_agent_server_; 65 | 66 | ros::ServiceServer sampling_goal_server_; 67 | 68 | std::vector agent_check_clients_; 69 | 70 | // Partition 71 | std::unique_ptr partition_handler_; 72 | 73 | // Online Learning 74 | std::unique_ptr learning_handler_; 75 | 76 | // Visualization 77 | std::unique_ptr 78 | agent_visualization_handler_; 79 | 80 | std::unique_ptr evaluation_handler_; 81 | 82 | std::unordered_map> 84 | grid_visualization_handlers_; 85 | 86 | void AgentLocationUpdateCallback( 87 | const sampling_msgs::AgentLocationConstPtr &msg); 88 | 89 | std::vector sample_buffer_; 90 | 91 | bool SampleToSrv(const std::vector &samples, 92 | sampling_msgs::AddSampleToModel &srv); 93 | 94 | void SampleUpdateCallback(const sampling_msgs::SampleConstPtr &msg); 95 | 96 | bool InitializeModelAndPrediction(); 97 | 98 | bool Initialize(); 99 | 100 | bool UpdateModel(); 101 | 102 | bool UpdatePrediction(); 103 | 104 | bool UpdateVisualization(); 105 | 106 | bool AssignSamplingGoal(sampling_msgs::SamplingGoal::Request &req, 107 | sampling_msgs::SamplingGoal::Response &res); 108 | 109 | bool KillAgent(sampling_msgs::KillAgent::Request &req, 110 | sampling_msgs::KillAgent::Response &res); 111 | 112 | std::vector updated_mean_prediction_; 113 | 114 | std::vector updated_var_prediction_; 115 | 116 | std::unordered_set died_agents_; 117 | 118 | int sample_count_; 119 | 120 | bool is_initialized_; 121 | }; 122 | } // namespace core 123 | } // namespace sampling 124 | -------------------------------------------------------------------------------- /src/sampling_core/include/sampling_core/sampling_core_params.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | namespace sampling { 11 | namespace core { 12 | 13 | const std::string KDataPackage = "sampling_data"; 14 | const int KModelUpdateFrequencyCount = 1; 15 | const int KInitSampleSize = 5; 16 | const double KInitSampleRatio = 0.05; 17 | 18 | class SamplingCoreParams { 19 | public: 20 | SamplingCoreParams(); 21 | 22 | bool LoadFromRosParams(ros::NodeHandle &ph); 23 | 24 | Eigen::MatrixXd test_locations; 25 | 26 | std::vector test_locations_msg; 27 | 28 | std::vector agent_ids; 29 | 30 | Eigen::VectorXd ground_truth_measurements; 31 | 32 | std::vector ground_truth_measurements_vec; 33 | 34 | bool enable_performance_evaluation; 35 | 36 | Eigen::MatrixXd initial_locations; 37 | 38 | std::vector initial_locations_msg; 39 | 40 | Eigen::VectorXd initial_measurements; 41 | 42 | bool LoadMatrix(const std::string &path, Eigen::MatrixXd &data); 43 | 44 | bool LoadVector(const std::string &path, Eigen::VectorXd &data); 45 | 46 | void MatrixToMsg(const Eigen::MatrixXd &data, 47 | std::vector &msg); 48 | 49 | int model_update_frequency_count; 50 | 51 | }; // namespace scene 52 | } // namespace core 53 | } // namespace sampling -------------------------------------------------------------------------------- /src/sampling_core/include/sampling_core/sampling_core_performance_evaluation.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | namespace sampling { 10 | namespace core { 11 | 12 | class SamplingCorePerformanceEvaluation { 13 | public: 14 | SamplingCorePerformanceEvaluation(); 15 | 16 | static std::unique_ptr MakeUniqueFromRos( 17 | ros::NodeHandle &nh, const std::vector &ground_truth_data); 18 | 19 | bool UpdatePerformance(const int &sample_count, 20 | const std::vector &prediction_data, 21 | const std::vector &prediction_variance); 22 | 23 | private: 24 | SamplingCorePerformanceEvaluation( 25 | ros::NodeHandle &nh, const std::vector &ground_truth_data); 26 | 27 | bool CalculateRMSE(const std::vector &ground_truth_data, 28 | const std::vector &prediction_data, double &rmse); 29 | 30 | bool CalculateMeanVariance(const std::vector &prediction_variance, 31 | double &mean_variance); 32 | 33 | void ReportPerformanceCallback(const ros::TimerEvent &); 34 | 35 | ros::Timer event_timer_; 36 | 37 | ros::Publisher performance_publisher_; 38 | 39 | std::vector ground_truth_data_; 40 | 41 | int sample_count_; 42 | 43 | double rmse_; 44 | 45 | double average_variance_; 46 | 47 | }; // namespace scene 48 | } // namespace core 49 | } // namespace sampling -------------------------------------------------------------------------------- /src/sampling_core/launch/heterogeneous_adaptive_sampling.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | num_gp: 3 8 | modeling_gp_0_kernel: [0.5, 0.5, 0.5] 9 | gating_gp_0_kernel: [0.75, 0.5, 0.05] 10 | modeling_gp_1_kernel: [0.65, 0.65, 0.5] 11 | gating_gp_1_kernel: [0.5, 0.5, 0.05] 12 | modeling_gp_2_kernel: [0.35, 0.35, 0.5] 13 | gating_gp_2_kernel: [1.25, 0.5, 0.05] 14 | EM_epsilon: 0.05 15 | EM_max_iteration: 100 16 | online_kernel_optimization: True 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | agent_type: "HECTOR" 25 | agent_id: "hector0" 26 | max_speed_ms: 5.0 27 | batterylife_ros_sec: 200 28 | navigation_height_m: 3.0 29 | takeoff_distance_m: 0.5 30 | 31 | 32 | 33 | 34 | 35 | agent_type: "JACKAL" 36 | agent_id: "jackal0" 37 | max_speed_ms: 2.0 38 | batterylife_ros_sec: 1000 39 | 40 | 41 | 42 | 43 | 44 | agent_type: "JACKAL" 45 | agent_id: "jackal1" 46 | max_speed_ms: 1.0 47 | batterylife_ros_sec: 1000 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /src/sampling_core/launch/homogeneous_adaptive_sampling.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | num_gp: 3 8 | modeling_gp_0_kernel: [0.5, 0.5, 0.5] 9 | gating_gp_0_kernel: [0.75, 0.5, 0.05] 10 | modeling_gp_1_kernel: [0.65, 0.65, 0.5] 11 | gating_gp_1_kernel: [0.5, 0.5, 0.05] 12 | modeling_gp_2_kernel: [0.35, 0.35, 0.5] 13 | gating_gp_2_kernel: [1.25, 0.5, 0.05] 14 | EM_epsilon: 0.05 15 | EM_max_iteration: 100 16 | online_kernel_optimization: True 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | agent_type: "HECTOR" 25 | agent_id: "hector0" 26 | max_speed_ms: 4.0 27 | batterylife_ros_sec: 200 28 | navigation_height_m: 3.0 29 | takeoff_distance_m: 0.5 30 | 31 | 32 | 33 | 34 | 35 | agent_type: "JACKAL" 36 | agent_id: "jackal0" 37 | max_speed_ms: 2.0 38 | batterylife_ros_sec: 1000 39 | 40 | 41 | 42 | 43 | 44 | agent_type: "JACKAL" 45 | agent_id: "jackal1" 46 | max_speed_ms: 1.0 47 | batterylife_ros_sec: 1000 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /src/sampling_core/node/heterogeneous_adaptive_sampling_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "sampling_core/sampling_core.h" 4 | 5 | int main(int argc, char **argv) { 6 | ros::init(argc, argv, "heteregoneous_adaptive_sampling"); 7 | ros::NodeHandle nh, ph("~"); 8 | std::unique_ptr sampling_core = 9 | sampling::core::SamplingCore::MakeUniqueFromRos(nh, ph); 10 | if (sampling_core == nullptr) { 11 | ROS_ERROR_STREAM("Failed to launch heterogeneous adaptive sampling!"); 12 | return -1; 13 | } 14 | 15 | ros::AsyncSpinner spinner(0); 16 | spinner.start(); 17 | 18 | ros::Rate loop_rate(3); 19 | while (ros::ok()) { 20 | sampling_core->Loop(); 21 | loop_rate.sleep(); 22 | } 23 | return 0; 24 | } 25 | -------------------------------------------------------------------------------- /src/sampling_core/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sampling_core 4 | 0.0.0 5 | The sampling_core package 6 | 7 | Yang Zhang 8 | Yunfei Shi 9 | Ning Wang 10 | Jianmin Zheng 11 | 12 | BSD 13 | 14 | catkin 15 | roscpp 16 | rospy 17 | std_msgs 18 | message_runtime 19 | visualization_msgs 20 | actionlib 21 | actionlib_msgs 22 | sampling_measurement 23 | sampling_msgs 24 | roslib 25 | sampling_partition 26 | sampling_online_learning 27 | sampling_visualization 28 | sampling_utils 29 | std_srvs 30 | sampling_agent 31 | 32 | -------------------------------------------------------------------------------- /src/sampling_core/src/sampling_core_params.cpp: -------------------------------------------------------------------------------- 1 | #include "sampling_core/sampling_core_params.h" 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include // std::random_shuffle 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include "sampling_msgs/Sample.h" 15 | #include "sampling_utils/utils.h" 16 | 17 | namespace sampling { 18 | namespace core { 19 | 20 | SamplingCoreParams::SamplingCoreParams() {} 21 | 22 | bool SamplingCoreParams::LoadFromRosParams(ros::NodeHandle &ph) { 23 | if (!ph.getParam("agent_ids", agent_ids)) { 24 | ROS_ERROR_STREAM("Please provide agent ids for sampling task!"); 25 | return false; 26 | } 27 | 28 | std::string test_location_file; 29 | if (!ph.getParam("test_location_file", test_location_file)) { 30 | ROS_ERROR_STREAM("Please provide test locations for sampling task!"); 31 | return false; 32 | } 33 | 34 | std::string pack_path = ros::package::getPath(KDataPackage); 35 | std::string test_location_dir = pack_path + "/location/" + test_location_file; 36 | 37 | if (!LoadMatrix(test_location_dir, test_locations)) { 38 | ROS_ERROR_STREAM("Failed to load test locations for sampling!"); 39 | return false; 40 | } 41 | 42 | MatrixToMsg(test_locations, test_locations_msg); 43 | bool has_groundtruth_measurement = false; 44 | std::string groundtruth_measurement_file; 45 | if (!ph.getParam("groundtruth_measurement_file", 46 | groundtruth_measurement_file)) { 47 | ROS_WARN_STREAM( 48 | "Ground truth meansurement file is NOT provided! Please provide " 49 | "samples for model initialization!"); 50 | has_groundtruth_measurement = false; 51 | } else { 52 | std::string groundtruth_measurement_dir = 53 | pack_path + "/measurement/" + test_location_file; 54 | 55 | if (!LoadVector(groundtruth_measurement_dir, ground_truth_measurements)) { 56 | ROS_ERROR_STREAM( 57 | "Failed to load ground truth measurements for sampling!"); 58 | return false; 59 | } 60 | ground_truth_measurements_vec.reserve(ground_truth_measurements.size()); 61 | for (int i = 0; i < ground_truth_measurements.size(); ++i) 62 | ground_truth_measurements_vec.push_back(ground_truth_measurements(i)); 63 | 64 | has_groundtruth_measurement = true; 65 | } 66 | 67 | if (has_groundtruth_measurement) { 68 | if (!ph.getParam("enable_performance_evaluation", 69 | enable_performance_evaluation)) { 70 | ROS_WARN_STREAM("Missing enable performance evaluation option!"); 71 | enable_performance_evaluation = false; 72 | } else { 73 | enable_performance_evaluation = true; 74 | } 75 | } 76 | bool random_initialization; 77 | ph.param("random_initialization", random_initialization, true); 78 | 79 | if (!has_groundtruth_measurement || !random_initialization) { 80 | std::string initial_measurement_file; 81 | if (!ph.getParam("initial_measurement_file", initial_measurement_file)) { 82 | ROS_ERROR_STREAM( 83 | "Please provide initial measurements for sampling task!"); 84 | return false; 85 | } 86 | std::string initial_measurement_dir = 87 | pack_path + "/measurement/" + initial_measurement_file; 88 | if (!LoadVector(initial_measurement_dir, initial_measurements)) { 89 | ROS_ERROR_STREAM("Failed to load initial measurements for sampling!"); 90 | return false; 91 | } 92 | std::string initial_location_file; 93 | if (!ph.getParam("initial_location_file", initial_location_file)) { 94 | ROS_ERROR_STREAM("Please provide initial locations for sampling task!"); 95 | return false; 96 | } 97 | std::string initial_location_dir = 98 | pack_path + "/location/" + initial_location_file; 99 | if (!LoadMatrix(initial_location_dir, initial_locations)) { 100 | ROS_ERROR_STREAM("Failed to load initial locations for sampling!"); 101 | return false; 102 | } 103 | MatrixToMsg(initial_locations, initial_locations_msg); 104 | 105 | if (initial_measurements.rows() != initial_measurements.size()) { 106 | ROS_ERROR_STREAM("Initial locations and measurements do NOT match!"); 107 | return false; 108 | } 109 | } else if (!has_groundtruth_measurement && random_initialization) { 110 | ROS_ERROR_STREAM( 111 | "Please provide ground truth data for random initialization!"); 112 | return false; 113 | } else { 114 | int initial_sample_size = 115 | std::max(KInitSampleSize, 116 | int(KInitSampleRatio * ground_truth_measurements.size())); 117 | std::vector index_vec; 118 | for (int i = 0; i < ground_truth_measurements.size(); ++i) 119 | index_vec.push_back(i); 120 | // using built-in random generator: 121 | std::random_shuffle(index_vec.begin(), index_vec.end()); 122 | std::vector random_initial_index; 123 | random_initial_index.reserve(initial_sample_size); 124 | for (int i = 0; i < initial_sample_size; ++i) { 125 | random_initial_index.push_back(index_vec[i]); 126 | } 127 | 128 | if (!utils::ExtractRows(test_locations, random_initial_index, 129 | initial_locations)) { 130 | ROS_ERROR_STREAM("Failed to generate initial locations for sampling!"); 131 | return false; 132 | } 133 | 134 | MatrixToMsg(initial_locations, initial_locations_msg); 135 | 136 | if (!utils::ExtractVec(ground_truth_measurements, random_initial_index, 137 | initial_measurements)) { 138 | ROS_ERROR_STREAM("Failed to generate initial measurements for sampling!"); 139 | return false; 140 | } 141 | } 142 | 143 | if (!ph.getParam("model_update_frequency_count", 144 | model_update_frequency_count)) { 145 | ROS_WARN_STREAM("Using default model update frequency (count) : " 146 | << KModelUpdateFrequencyCount); 147 | model_update_frequency_count = KModelUpdateFrequencyCount; 148 | return false; 149 | } 150 | 151 | return true; 152 | } // namespace core 153 | 154 | bool SamplingCoreParams::LoadMatrix(const std::string &path, 155 | Eigen::MatrixXd &data) { 156 | std::ifstream file(path.c_str(), std::ifstream::in); 157 | 158 | if (!file.is_open()) { 159 | ROS_INFO_STREAM("Error opening file " << path); 160 | return false; 161 | } 162 | 163 | std::vector> data_vec; 164 | 165 | std::string new_line; 166 | 167 | while (getline(file, new_line)) { 168 | std::stringstream ss(new_line); 169 | std::vector new_data; 170 | for (double k; ss >> k;) { 171 | new_data.push_back(k); 172 | if (ss.peek() == ',') ss.ignore(); 173 | } 174 | data_vec.push_back(new_data); 175 | } 176 | 177 | if (data_vec.empty()) { 178 | ROS_ERROR_STREAM("Empty data!"); 179 | return false; 180 | } 181 | 182 | data.resize(data_vec.size(), data_vec.front().size()); 183 | for (int i = 0; i < data_vec.size(); ++i) { 184 | for (int j = 0; j < data_vec[i].size(); ++j) { 185 | data(i, j) = data_vec[i][j]; 186 | } 187 | } 188 | return true; 189 | } 190 | 191 | bool SamplingCoreParams::LoadVector(const std::string &path, 192 | Eigen::VectorXd &data) { 193 | Eigen::MatrixXd data_mat; 194 | if (!LoadMatrix(path, data_mat)) { 195 | ROS_ERROR_STREAM("Failed to load " << path); 196 | return false; 197 | } 198 | if (data_mat.cols() != 1) { 199 | ROS_ERROR_STREAM("Loaded data has multiple dimensions!"); 200 | return false; 201 | } 202 | data = data_mat.col(0); 203 | return true; 204 | } 205 | 206 | void SamplingCoreParams::MatrixToMsg(const Eigen::MatrixXd &data, 207 | std::vector &msg) { 208 | msg.resize(data.rows()); 209 | for (int i = 0; i < data.rows(); ++i) { 210 | geometry_msgs::Point point; 211 | point.x = data(i, 0); 212 | point.y = data(i, 1); 213 | msg[i] = point; 214 | } 215 | } 216 | 217 | } // namespace core 218 | } // namespace sampling -------------------------------------------------------------------------------- /src/sampling_core/src/sampling_core_performance_evaluation.cpp: -------------------------------------------------------------------------------- 1 | #include "sampling_core/sampling_core_performance_evaluation.h" 2 | 3 | #include 4 | 5 | #include "sampling_msgs/SamplingPerformance.h" 6 | 7 | namespace sampling { 8 | namespace core { 9 | 10 | std::unique_ptr 11 | SamplingCorePerformanceEvaluation::MakeUniqueFromRos( 12 | ros::NodeHandle &nh, const std::vector &ground_truth_data) { 13 | if (ground_truth_data.empty()) { 14 | ROS_ERROR_STREAM("Empty ground truth data for performance evaluation"); 15 | return nullptr; 16 | } 17 | return std::unique_ptr( 18 | new SamplingCorePerformanceEvaluation(nh, ground_truth_data)); 19 | } 20 | 21 | SamplingCorePerformanceEvaluation::SamplingCorePerformanceEvaluation( 22 | ros::NodeHandle &nh, const std::vector &ground_truth_data) 23 | : ground_truth_data_(ground_truth_data), 24 | sample_count_(0), 25 | average_variance_(-1.0), 26 | rmse_(-1.0) { 27 | event_timer_ = nh.createTimer( 28 | ros::Duration(1.0), 29 | &SamplingCorePerformanceEvaluation::ReportPerformanceCallback, this); 30 | 31 | performance_publisher_ = nh.advertise( 32 | "sampling_performance", 1); 33 | } 34 | 35 | bool SamplingCorePerformanceEvaluation::UpdatePerformance( 36 | const int &sample_count, const std::vector &prediction_data, 37 | const std::vector &prediction_variance) { 38 | if (prediction_data.size() != ground_truth_data_.size()) { 39 | ROS_ERROR_STREAM("Evaluation data size does NOT match!"); 40 | return false; 41 | } 42 | sample_count_ = sample_count; 43 | if (!CalculateRMSE(ground_truth_data_, prediction_data, rmse_)) { 44 | return false; 45 | } 46 | if (!CalculateMeanVariance(prediction_variance, average_variance_)) { 47 | return false; 48 | } 49 | return true; 50 | } 51 | 52 | bool SamplingCorePerformanceEvaluation::CalculateRMSE( 53 | const std::vector &ground_truth_data, 54 | const std::vector &prediction_data, double &rmse) { 55 | if (ground_truth_data.size() != prediction_data.size() || 56 | ground_truth_data.empty()) 57 | return false; 58 | rmse = 0.0; 59 | for (int i = 0; i < ground_truth_data.size(); ++i) { 60 | double error = ground_truth_data[i] - prediction_data[i]; 61 | rmse += (error * error); 62 | } 63 | rmse = std::sqrt(rmse / double(ground_truth_data.size())); 64 | return true; 65 | } 66 | 67 | bool SamplingCorePerformanceEvaluation::CalculateMeanVariance( 68 | const std::vector &prediction_variance, double &mean_variance) { 69 | mean_variance = 0.0; 70 | double count = 0.0; 71 | for (const double &var : prediction_variance) { 72 | if (var > 0) { 73 | count += 1.0; 74 | mean_variance += var; 75 | } 76 | } 77 | if (count > 0) { 78 | mean_variance /= count; 79 | return true; 80 | } else { 81 | mean_variance = -1; 82 | return false; 83 | } 84 | return false; 85 | } 86 | 87 | void SamplingCorePerformanceEvaluation::ReportPerformanceCallback( 88 | const ros::TimerEvent &) { 89 | if (sample_count_ > 0) { 90 | sampling_msgs::SamplingPerformance msg; 91 | msg.header.stamp = ros::Time::now(); 92 | msg.sample_count = sample_count_; 93 | msg.rmse = rmse_; 94 | msg.average_variance = average_variance_; 95 | performance_publisher_.publish(msg); 96 | } 97 | } 98 | 99 | } // namespace core 100 | } // namespace sampling -------------------------------------------------------------------------------- /src/sampling_data/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(sampling_data) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | 8 | include_directories() -------------------------------------------------------------------------------- /src/sampling_data/location/wifi_2_routers.txt: -------------------------------------------------------------------------------- 1 | 0.000000,0.000000 2 | 1.000000,0.000000 3 | 2.000000,0.000000 4 | 3.000000,0.000000 5 | 4.000000,0.000000 6 | 5.000000,0.000000 7 | 6.000000,0.000000 8 | 7.000000,0.000000 9 | 8.000000,0.000000 10 | 9.000000,0.000000 11 | 9.000000,1.000000 12 | 8.000000,1.000000 13 | 7.000000,1.000000 14 | 6.000000,1.000000 15 | 5.000000,1.000000 16 | 4.000000,1.000000 17 | 3.000000,1.000000 18 | 2.000000,1.000000 19 | 1.000000,1.000000 20 | 0.000000,1.000000 21 | 0.000000,2.000000 22 | 1.000000,2.000000 23 | 2.000000,2.000000 24 | 3.000000,2.000000 25 | 4.000000,2.000000 26 | 5.000000,2.000000 27 | 6.000000,2.000000 28 | 7.000000,2.000000 29 | 8.000000,2.000000 30 | 9.000000,2.000000 31 | 9.000000,3.000000 32 | 8.000000,3.000000 33 | 7.000000,3.000000 34 | 6.000000,3.000000 35 | 5.000000,3.000000 36 | 4.000000,3.000000 37 | 3.000000,3.000000 38 | 2.000000,3.000000 39 | 1.000000,3.000000 40 | 0.000000,3.000000 41 | 0.000000,4.000000 42 | 1.000000,4.000000 43 | 2.000000,4.000000 44 | 3.000000,4.000000 45 | 4.000000,4.000000 46 | 5.000000,4.000000 47 | 6.000000,4.000000 48 | 7.000000,4.000000 49 | 8.000000,4.000000 50 | 9.000000,4.000000 51 | 9.000000,5.000000 52 | 8.000000,5.000000 53 | 7.000000,5.000000 54 | 6.000000,5.000000 55 | 5.000000,5.000000 56 | 4.000000,5.000000 57 | 3.000000,5.000000 58 | 2.000000,5.000000 59 | 1.000000,5.000000 60 | 0.000000,5.000000 61 | 0.000000,6.000000 62 | 1.000000,6.000000 63 | 2.000000,6.000000 64 | 3.000000,6.000000 65 | 4.000000,6.000000 66 | 5.000000,6.000000 67 | 6.000000,6.000000 68 | 7.000000,6.000000 69 | 8.000000,6.000000 70 | 9.000000,6.000000 71 | 9.000000,7.000000 72 | 8.000000,7.000000 73 | 7.000000,7.000000 74 | 6.000000,7.000000 75 | 5.000000,7.000000 76 | 4.000000,7.000000 77 | 3.000000,7.000000 78 | 2.000000,7.000000 79 | 1.000000,7.000000 80 | 0.000000,7.000000 81 | 0.000000,8.000000 82 | 1.000000,8.000000 83 | 2.000000,8.000000 84 | 3.000000,8.000000 85 | 4.000000,8.000000 86 | 5.000000,8.000000 87 | 6.000000,8.000000 88 | 7.000000,8.000000 89 | 8.000000,8.000000 90 | 9.000000,8.000000 91 | 9.000000,9.000000 92 | 8.000000,9.000000 93 | 7.000000,9.000000 94 | 6.000000,9.000000 95 | 5.000000,9.000000 96 | 4.000000,9.000000 97 | 3.000000,9.000000 98 | 2.000000,9.000000 99 | 1.000000,9.000000 100 | 0.000000,9.000000 101 | 0.000000,10.000000 102 | 1.000000,10.000000 103 | 2.000000,10.000000 104 | 3.000000,10.000000 105 | 4.000000,10.000000 106 | 5.000000,10.000000 107 | 6.000000,10.000000 108 | 7.000000,10.000000 109 | 8.000000,10.000000 110 | 9.000000,10.000000 111 | 9.000000,11.000000 112 | 8.000000,11.000000 113 | 7.000000,11.000000 114 | 6.000000,11.000000 115 | 5.000000,11.000000 116 | 4.000000,11.000000 117 | 3.000000,11.000000 118 | 2.000000,11.000000 119 | 1.000000,11.000000 120 | 0.000000,11.000000 121 | 0.000000,12.000000 122 | 1.000000,12.000000 123 | 2.000000,12.000000 124 | 3.000000,12.000000 125 | 4.000000,12.000000 126 | 5.000000,12.000000 127 | 6.000000,12.000000 128 | 7.000000,12.000000 129 | 8.000000,12.000000 130 | 9.000000,12.000000 131 | 9.000000,13.000000 132 | 8.000000,13.000000 133 | 7.000000,13.000000 134 | 6.000000,13.000000 135 | 5.000000,13.000000 136 | 4.000000,13.000000 137 | 3.000000,13.000000 138 | 2.000000,13.000000 139 | 1.000000,13.000000 140 | 0.000000,13.000000 141 | 0.000000,14.000000 142 | 1.000000,14.000000 143 | 2.000000,14.000000 144 | 3.000000,14.000000 145 | 4.000000,14.000000 146 | 5.000000,14.000000 147 | 6.000000,14.000000 148 | 7.000000,14.000000 149 | 8.000000,14.000000 150 | 9.000000,14.000000 151 | -------------------------------------------------------------------------------- /src/sampling_data/location/wifi_3_routers.txt: -------------------------------------------------------------------------------- 1 | 0.000000,0.000000 2 | 1.000000,0.000000 3 | 2.000000,0.000000 4 | 3.000000,0.000000 5 | 4.000000,0.000000 6 | 5.000000,0.000000 7 | 6.000000,0.000000 8 | 7.000000,0.000000 9 | 8.000000,0.000000 10 | 9.000000,0.000000 11 | 9.000000,1.000000 12 | 8.000000,1.000000 13 | 7.000000,1.000000 14 | 6.000000,1.000000 15 | 5.000000,1.000000 16 | 4.000000,1.000000 17 | 3.000000,1.000000 18 | 2.000000,1.000000 19 | 1.000000,1.000000 20 | 0.000000,1.000000 21 | 0.000000,2.000000 22 | 1.000000,2.000000 23 | 2.000000,2.000000 24 | 3.000000,2.000000 25 | 4.000000,2.000000 26 | 5.000000,2.000000 27 | 6.000000,2.000000 28 | 7.000000,2.000000 29 | 8.000000,2.000000 30 | 9.000000,2.000000 31 | 9.000000,3.000000 32 | 8.000000,3.000000 33 | 7.000000,3.000000 34 | 6.000000,3.000000 35 | 5.000000,3.000000 36 | 4.000000,3.000000 37 | 3.000000,3.000000 38 | 2.000000,3.000000 39 | 1.000000,3.000000 40 | 0.000000,3.000000 41 | 0.000000,4.000000 42 | 1.000000,4.000000 43 | 2.000000,4.000000 44 | 3.000000,4.000000 45 | 4.000000,4.000000 46 | 5.000000,4.000000 47 | 6.000000,4.000000 48 | 7.000000,4.000000 49 | 8.000000,4.000000 50 | 9.000000,4.000000 51 | 9.000000,5.000000 52 | 8.000000,5.000000 53 | 7.000000,5.000000 54 | 6.000000,5.000000 55 | 5.000000,5.000000 56 | 4.000000,5.000000 57 | 3.000000,5.000000 58 | 2.000000,5.000000 59 | 1.000000,5.000000 60 | 0.000000,5.000000 61 | 0.000000,6.000000 62 | 1.000000,6.000000 63 | 2.000000,6.000000 64 | 3.000000,6.000000 65 | 4.000000,6.000000 66 | 5.000000,6.000000 67 | 6.000000,6.000000 68 | 7.000000,6.000000 69 | 8.000000,6.000000 70 | 9.000000,6.000000 71 | 9.000000,7.000000 72 | 8.000000,7.000000 73 | 7.000000,7.000000 74 | 6.000000,7.000000 75 | 5.000000,7.000000 76 | 4.000000,7.000000 77 | 3.000000,7.000000 78 | 2.000000,7.000000 79 | 1.000000,7.000000 80 | 0.000000,7.000000 81 | 0.000000,8.000000 82 | 1.000000,8.000000 83 | 2.000000,8.000000 84 | 3.000000,8.000000 85 | 4.000000,8.000000 86 | 5.000000,8.000000 87 | 6.000000,8.000000 88 | 7.000000,8.000000 89 | 8.000000,8.000000 90 | 9.000000,8.000000 91 | 9.000000,9.000000 92 | 8.000000,9.000000 93 | 7.000000,9.000000 94 | 6.000000,9.000000 95 | 5.000000,9.000000 96 | 4.000000,9.000000 97 | 3.000000,9.000000 98 | 2.000000,9.000000 99 | 1.000000,9.000000 100 | 0.000000,9.000000 101 | 0.000000,10.000000 102 | 1.000000,10.000000 103 | 2.000000,10.000000 104 | 3.000000,10.000000 105 | 4.000000,10.000000 106 | 5.000000,10.000000 107 | 6.000000,10.000000 108 | 7.000000,10.000000 109 | 8.000000,10.000000 110 | 9.000000,10.000000 111 | 9.000000,11.000000 112 | 8.000000,11.000000 113 | 7.000000,11.000000 114 | 6.000000,11.000000 115 | 5.000000,11.000000 116 | 4.000000,11.000000 117 | 3.000000,11.000000 118 | 2.000000,11.000000 119 | 1.000000,11.000000 120 | 0.000000,11.000000 121 | 0.000000,12.000000 122 | 1.000000,12.000000 123 | 2.000000,12.000000 124 | 3.000000,12.000000 125 | 4.000000,12.000000 126 | 5.000000,12.000000 127 | 6.000000,12.000000 128 | 7.000000,12.000000 129 | 8.000000,12.000000 130 | 9.000000,12.000000 131 | 9.000000,13.000000 132 | 8.000000,13.000000 133 | 7.000000,13.000000 134 | 6.000000,13.000000 135 | 5.000000,13.000000 136 | 4.000000,13.000000 137 | 3.000000,13.000000 138 | 2.000000,13.000000 139 | 1.000000,13.000000 140 | 0.000000,13.000000 141 | 0.000000,14.000000 142 | 1.000000,14.000000 143 | 2.000000,14.000000 144 | 3.000000,14.000000 145 | 4.000000,14.000000 146 | 5.000000,14.000000 147 | 6.000000,14.000000 148 | 7.000000,14.000000 149 | 8.000000,14.000000 150 | 9.000000,14.000000 151 | -------------------------------------------------------------------------------- /src/sampling_data/measurement/artificial_wifi_3_routers.txt: -------------------------------------------------------------------------------- 1 | 6.101772 2 | 5.894644 3 | 7.147744 4 | 8.649234 5 | 9.367084 6 | 8.665424 7 | 6.520904 8 | 3.739052 9 | 2.170629 10 | 4.927989 11 | 4.593680 12 | 4.126586 13 | 5.552172 14 | 6.876832 15 | 7.304465 16 | 6.856291 17 | 5.990673 18 | 5.222937 19 | 4.745190 20 | 4.046137 21 | 3.966815 22 | 4.777320 23 | 4.985331 24 | 5.232959 25 | 5.585429 26 | 5.795319 27 | 5.566782 28 | 4.819783 29 | 3.954325 30 | 4.114684 31 | 3.916185 32 | 3.321520 33 | 3.395242 34 | 3.825516 35 | 4.386054 36 | 4.916656 37 | 5.303766 38 | 5.461019 39 | 5.309790 40 | 4.759742 41 | 5.705730 42 | 5.905243 43 | 6.030868 44 | 5.537937 45 | 4.480797 46 | 3.271612 47 | 2.439149 48 | 2.387587 49 | 3.155299 50 | 4.173659 51 | 4.868780 52 | 3.793306 53 | 2.299887 54 | 1.817086 55 | 2.589591 56 | 4.118113 57 | 5.599276 58 | 6.365512 59 | 6.324953 60 | 6.401325 61 | 6.689665 62 | 6.483560 63 | 6.360344 64 | 5.402353 65 | 3.819054 66 | 2.416029 67 | 2.063955 68 | 3.167593 69 | 5.134766 70 | 5.845345 71 | 6.865180 72 | 6.791547 73 | 4.696881 74 | 3.051546 75 | 2.761179 76 | 3.665029 77 | 5.034305 78 | 6.070514 79 | 6.403812 80 | 6.591343 81 | 6.235266 82 | 6.171299 83 | 5.645915 84 | 4.676630 85 | 3.769162 86 | 3.565295 87 | 4.490742 88 | 6.403001 89 | 8.239221 90 | 7.664060 91 | 8.007625 92 | 8.968126 93 | 7.748629 94 | 6.003457 95 | 4.694411 96 | 4.217052 97 | 4.526993 98 | 5.266183 99 | 5.889196 100 | 5.789516 101 | 5.392210 102 | 5.632998 103 | 5.075685 104 | 4.721023 105 | 5.007540 106 | 5.936119 107 | 7.194570 108 | 8.282209 109 | 8.634432 110 | 7.747292 111 | 6.876170 112 | 7.211198 113 | 7.776305 114 | 7.723861 115 | 6.995356 116 | 5.993467 117 | 5.254113 118 | 5.118516 119 | 5.405262 120 | 5.082357 121 | 4.730723 122 | 5.090342 123 | 5.273491 124 | 5.903220 125 | 6.822433 126 | 7.490178 127 | 7.377944 128 | 6.365950 129 | 5.139441 130 | 5.584979 131 | 4.317959 132 | 3.479193 133 | 4.686988 134 | 6.142204 135 | 6.947545 136 | 6.877563 137 | 6.148667 138 | 5.189138 139 | 4.409132 140 | 3.970688 141 | 2.129108 142 | 2.873798 143 | 4.218692 144 | 5.095940 145 | 5.218265 146 | 4.799103 147 | 4.272735 148 | 4.014428 149 | 4.060567 150 | 3.828793 151 | -------------------------------------------------------------------------------- /src/sampling_data/measurement/wifi_2_routers.txt: -------------------------------------------------------------------------------- 1 | 2.840157 2 | 3.659371 3 | 2.276979 4 | 1.821028 5 | 1.612725 6 | 1.967086 7 | 1.879824 8 | 1.826703 9 | 1.686633 10 | 2.335977 11 | 2.470828 12 | 1.149550 13 | 4.681508 14 | 6.265789 15 | 6.081180 16 | 4.337437 17 | 4.831246 18 | 3.757705 19 | 4.103869 20 | 4.259434 21 | 3.200172 22 | 3.729687 23 | 4.168079 24 | 3.862896 25 | 1.774762 26 | 2.109811 27 | 2.185976 28 | 2.538326 29 | 1.507583 30 | 2.622966 31 | 3.434864 32 | 1.677113 33 | 2.418755 34 | 2.157334 35 | 1.521162 36 | 0.659279 37 | 2.584866 38 | 5.521435 39 | 4.529461 40 | 4.158022 41 | 4.742912 42 | 3.900678 43 | 5.849465 44 | 2.511299 45 | 1.579897 46 | 2.275385 47 | 3.435502 48 | 4.224309 49 | 4.108504 50 | 5.279799 51 | 3.947429 52 | 3.806501 53 | 3.108060 54 | 2.607814 55 | 1.033741 56 | 1.429990 57 | 2.488836 58 | 2.772124 59 | 4.106119 60 | 5.333974 61 | 4.030457 62 | 4.312803 63 | 3.540187 64 | 1.733004 65 | 0.946416 66 | 0.511447 67 | 3.872446 68 | 3.822976 69 | 4.853666 70 | 5.091397 71 | 4.426556 72 | 1.668196 73 | 4.183363 74 | 1.462742 75 | 0.808078 76 | 0.659087 77 | 0.584589 78 | 1.072724 79 | 2.106240 80 | 3.018847 81 | 1.419233 82 | 0.558845 83 | 0.600892 84 | 0.675232 85 | 0.885126 86 | 0.873901 87 | 1.046136 88 | 1.344111 89 | 1.408546 90 | 2.104614 91 | 2.701374 92 | 2.245593 93 | 1.724246 94 | 1.630199 95 | 2.104301 96 | 2.406151 97 | 2.348043 98 | 2.533193 99 | 2.511454 100 | 2.066124 101 | 1.194792 102 | 2.884472 103 | 2.605646 104 | 1.985905 105 | 2.687772 106 | 3.856868 107 | 2.809193 108 | 3.140011 109 | 2.184795 110 | 2.215655 111 | 3.171606 112 | 2.810958 113 | 2.934877 114 | 3.402234 115 | 2.679398 116 | 3.990294 117 | 4.537022 118 | 2.200916 119 | 2.774910 120 | 1.634561 121 | 2.788487 122 | 3.357722 123 | 2.796834 124 | 3.905687 125 | 3.255709 126 | 4.526480 127 | 2.072965 128 | 2.296941 129 | 2.784508 130 | 2.860346 131 | 3.909766 132 | 5.647593 133 | 5.580100 134 | 4.612416 135 | 4.606940 136 | 4.517524 137 | 4.939298 138 | 4.742455 139 | 4.022533 140 | 3.783900 141 | 5.459080 142 | 4.141721 143 | 3.480737 144 | 3.603310 145 | 4.082546 146 | 1.945060 147 | 3.492488 148 | 3.002086 149 | 3.862822 150 | 3.450185 151 | -------------------------------------------------------------------------------- /src/sampling_data/measurement/wifi_3_routers.txt: -------------------------------------------------------------------------------- 1 | 5.717812 2 | 6.675318 3 | 5.819740 4 | 10.501844 5 | 6.965755 6 | 9.337100 7 | 6.759904 8 | 3.741251 9 | 2.418880 10 | 4.974487 11 | 4.297606 12 | 3.425991 13 | 4.954551 14 | 5.814352 15 | 7.576147 16 | 7.239139 17 | 6.892588 18 | 4.854006 19 | 4.448877 20 | 4.679075 21 | 3.883881 22 | 4.553717 23 | 4.471928 24 | 5.895271 25 | 7.638590 26 | 6.954682 27 | 6.660735 28 | 6.429425 29 | 4.882976 30 | 4.663666 31 | 3.415290 32 | 1.980902 33 | 2.825241 34 | 2.682390 35 | 1.820035 36 | 1.957012 37 | 2.935090 38 | 6.410582 39 | 4.863761 40 | 4.531166 41 | 6.369934 42 | 6.780565 43 | 7.454002 44 | 5.999964 45 | 7.226583 46 | 3.746797 47 | 2.863970 48 | 2.519711 49 | 2.885589 50 | 4.978404 51 | 5.389220 52 | 3.358925 53 | 3.230758 54 | 2.416099 55 | 0.936207 56 | 1.053968 57 | 5.734569 58 | 5.384471 59 | 5.816822 60 | 4.426200 61 | 8.106474 62 | 5.239688 63 | 7.386382 64 | 7.033713 65 | 3.289809 66 | 2.495507 67 | 3.798814 68 | 3.683974 69 | 6.606850 70 | 4.129078 71 | 6.620601 72 | 4.935234 73 | 4.974022 74 | 3.225257 75 | 5.424574 76 | 5.955295 77 | 4.520313 78 | 4.752821 79 | 6.873492 80 | 7.231472 81 | 7.129560 82 | 5.962017 83 | 5.653825 84 | 7.416312 85 | 3.225420 86 | 3.985822 87 | 1.359855 88 | 3.285310 89 | 8.762961 90 | 9.901160 91 | 7.510878 92 | 8.855445 93 | 9.221721 94 | 5.524737 95 | 6.819446 96 | 2.945692 97 | 1.631942 98 | 4.117596 99 | 6.379539 100 | 4.000038 101 | 5.357960 102 | 6.406187 103 | 8.137731 104 | 4.209441 105 | 3.270482 106 | 3.425088 107 | 5.313656 108 | 7.496498 109 | 12.382784 110 | 6.698008 111 | 6.052972 112 | 5.121832 113 | 6.392258 114 | 15.506139 115 | 6.584823 116 | 4.599164 117 | 5.381529 118 | 4.313770 119 | 3.452094 120 | 4.771280 121 | 6.457526 122 | 5.350965 123 | 6.389106 124 | 8.275804 125 | 9.587912 126 | 7.306162 127 | 8.814526 128 | 5.175586 129 | 4.766775 130 | 7.065134 131 | 3.897425 132 | 4.393989 133 | 4.306146 134 | 4.170660 135 | 5.332722 136 | 5.697017 137 | 4.972334 138 | 4.740890 139 | 3.610711 140 | 3.247071 141 | 2.002939 142 | 3.894049 143 | 3.431198 144 | 5.560505 145 | 5.316216 146 | 5.614523 147 | 4.856752 148 | 3.756946 149 | 4.327079 150 | 3.676400 151 | -------------------------------------------------------------------------------- /src/sampling_data/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sampling_data 4 | 0.0.0 5 | The sampling_data package 6 | 7 | Yang Zhang 8 | Yunfei Shi 9 | Ning Wang 10 | Jianmin Zheng 11 | 12 | BSD 13 | 14 | catkin 15 | 16 | 17 | -------------------------------------------------------------------------------- /src/sampling_gazebo_simulation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(sampling_gazebo_simulation) 3 | 4 | find_package(catkin REQUIRED 5 | global_planner 6 | ) 7 | 8 | catkin_package() 9 | 10 | include_directories() -------------------------------------------------------------------------------- /src/sampling_gazebo_simulation/launch/one_ugv_one_uav_simulation.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 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /src/sampling_gazebo_simulation/launch/sampling_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /src/sampling_gazebo_simulation/launch/spawn_hector.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | [/move_group/fake_controller_joint_states] 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /src/sampling_gazebo_simulation/launch/two_ugv_one_uav_simulation.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 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /src/sampling_gazebo_simulation/launch/two_ugv_simulation.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 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /src/sampling_gazebo_simulation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sampling_gazebo_simulation 4 | 0.0.0 5 | The sampling_gazebo_simulation package 6 | 7 | Yang Zhang 8 | Yunfei Shi 9 | Ning Wang 10 | Jianmin Zheng 11 | 12 | BSD 13 | 14 | catkin 15 | global_planner 16 | 17 | 18 | -------------------------------------------------------------------------------- /src/sampling_gazebo_simulation/rviz/samlping_visualization.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | Splitter Ratio: 0.5 9 | Tree Height: 1088 10 | - Class: rviz/Selection 11 | Name: Selection 12 | - Class: rviz/Tool Properties 13 | Expanded: 14 | - /2D Pose Estimate1 15 | - /2D Nav Goal1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.5886790156364441 19 | - Class: rviz/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: "" 29 | Preferences: 30 | PromptSaveOnExit: true 31 | Toolbars: 32 | toolButtonStyle: 2 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Class: rviz/Marker 37 | Enabled: true 38 | Marker Topic: /sampling_visualization/AgentLocation 39 | Name: Marker 40 | Namespaces: 41 | /sampling_visualization/: true 42 | Queue Size: 100 43 | Value: true 44 | - Class: rviz/Marker 45 | Enabled: true 46 | Marker Topic: /sampling_visualization/Partition 47 | Name: Marker 48 | Namespaces: 49 | /sampling_visualization/: true 50 | Queue Size: 100 51 | Value: true 52 | - Class: rviz/Marker 53 | Enabled: true 54 | Marker Topic: /sampling_visualization/PredictionMean 55 | Name: Marker 56 | Namespaces: 57 | /sampling_visualization/: true 58 | Queue Size: 100 59 | Value: true 60 | - Class: rviz/Marker 61 | Enabled: true 62 | Marker Topic: /sampling_visualization/PredictionVariance 63 | Name: Marker 64 | Namespaces: 65 | /sampling_visualization/: true 66 | Queue Size: 100 67 | Value: true 68 | Enabled: true 69 | Global Options: 70 | Background Color: 48; 48; 48 71 | Default Light: true 72 | Fixed Frame: sampling_visualization 73 | Frame Rate: 30 74 | Name: root 75 | Tools: 76 | - Class: rviz/Interact 77 | Hide Inactive Objects: true 78 | - Class: rviz/MoveCamera 79 | - Class: rviz/Select 80 | - Class: rviz/FocusCamera 81 | - Class: rviz/Measure 82 | - Class: rviz/SetInitialPose 83 | Theta std deviation: 0.2617993950843811 84 | Topic: /initialpose 85 | X std deviation: 0.5 86 | Y std deviation: 0.5 87 | - Class: rviz/SetGoal 88 | Topic: /move_base_simple/goal 89 | - Class: rviz/PublishPoint 90 | Single click: true 91 | Topic: /clicked_point 92 | Value: true 93 | Views: 94 | Current: 95 | Class: rviz/Orbit 96 | Distance: 44.15269088745117 97 | Enable Stereo Rendering: 98 | Stereo Eye Separation: 0.05999999865889549 99 | Stereo Focal Distance: 1 100 | Swap Stereo Eyes: false 101 | Value: false 102 | Focal Point: 103 | X: 26.513771057128906 104 | Y: -0.2309524118900299 105 | Z: 2.4089808464050293 106 | Focal Shape Fixed Size: true 107 | Focal Shape Size: 0.05000000074505806 108 | Invert Z Axis: false 109 | Name: Current View 110 | Near Clip Distance: 0.009999999776482582 111 | Pitch: 1.2097967863082886 112 | Target Frame: 113 | Value: Orbit (rviz) 114 | Yaw: 4.668572425842285 115 | Saved: ~ 116 | Window Geometry: 117 | Displays: 118 | collapsed: false 119 | Height: 1385 120 | Hide Left Dock: false 121 | Hide Right Dock: false 122 | QMainWindow State: 000000ff00000000fd000000040000000000000156000004cbfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004cb000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004cbfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000004cb000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000d2d0000003efc0100000002fb0000000800540069006d0065010000000000000d2d000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000abc000004cb00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 123 | Selection: 124 | collapsed: false 125 | Time: 126 | collapsed: false 127 | Tool Properties: 128 | collapsed: false 129 | Views: 130 | collapsed: false 131 | Width: 3373 132 | X: 67 133 | Y: 27 134 | -------------------------------------------------------------------------------- /src/sampling_gazebo_simulation/worlds/scenario0.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 1 5 | 0 0 10 0 -0 0 6 | 0.8 0.8 0.8 1 7 | 0.2 0.2 0.2 1 8 | 9 | 1000 10 | 0.9 11 | 0.01 12 | 0.001 13 | 14 | -0.5 0.1 -0.9 15 | 16 | 17 | 1 18 | 19 | 20 | 21 | 22 | 0 0 1 23 | 100 100 24 | 25 | 26 | 27 | 28 | 65535 29 | 30 | 31 | 32 | 33 | 100 34 | 50 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 10 43 | 44 | 45 | 0 46 | 47 | 48 | 0 0 1 49 | 100 100 50 | 51 | 52 | 53 | 57 | 58 | 59 | 0 60 | 0 61 | 0 62 | 63 | 64 | 0 0 -9.8 65 | 6e-06 2.3e-05 -4.2e-05 66 | 67 | 68 | 0.001 69 | 1 70 | 1000 71 | 72 | 73 | 0.4 0.4 0.4 1 74 | 0.7 0.7 0.7 1 75 | 1 76 | 77 | 80 | 81 | 82 | EARTH_WGS84 83 | 0 84 | 0 85 | 0 86 | 0 87 | 88 | 89 | 28 351000000 90 | 28 476593939 91 | 1596233988 470392538 92 | 28351 93 | 94 | 0 0 0 0 -0 0 95 | 1 1 1 96 | 97 | 0 0 0 0 -0 0 98 | 0 0 0 0 -0 0 99 | 0 0 0 0 -0 0 100 | 0 0 0 0 -0 0 101 | 102 | 103 | 104 | 0 0 10 0 -0 0 105 | 106 | 107 | 108 | 109 | 5 -5 2 0 0.275643 2.35619 110 | orbit 111 | perspective 112 | 113 | 114 | 115 | 116 | -------------------------------------------------------------------------------- /src/sampling_gazebo_simulation/worlds/scenario1.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 1 5 | 0 0 10 0 -0 0 6 | 0.8 0.8 0.8 1 7 | 0.2 0.2 0.2 1 8 | 9 | 1000 10 | 0.9 11 | 0.01 12 | 0.001 13 | 14 | -0.5 0.1 -0.9 15 | 16 | 17 | 1 18 | 19 | 20 | 21 | 22 | 0 0 1 23 | 100 100 24 | 25 | 26 | 27 | 28 | 65535 29 | 30 | 31 | 32 | 33 | 100 34 | 50 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 10 43 | 44 | 45 | 0 46 | 47 | 48 | 0 0 1 49 | 100 100 50 | 51 | 52 | 53 | 57 | 58 | 59 | 0 60 | 0 61 | 0 62 | 63 | 64 | 0 0 -9.8 65 | 6e-06 2.3e-05 -4.2e-05 66 | 67 | 68 | 0.001 69 | 1 70 | 1000 71 | 72 | 73 | 0.4 0.4 0.4 1 74 | 0.7 0.7 0.7 1 75 | 1 76 | 77 | 80 | 81 | 82 | EARTH_WGS84 83 | 0 84 | 0 85 | 0 86 | 0 87 | 88 | 89 | 208 551000000 90 | 143 149070150 91 | 1596266758 149889286 92 | 140836 93 | 94 | 0 0 0 0 -0 0 95 | 1 1 1 96 | 97 | 0 0 0 0 -0 0 98 | 0 0 0 0 -0 0 99 | 0 0 0 0 -0 0 100 | 0 0 0 0 -0 0 101 | 102 | 103 | 104 | 4.99995 10.0003 0.5 0 -1e-06 0.0034 105 | 1 1 1 106 | 107 | 4.99995 10.0003 0.5 0 -1e-06 0.0034 108 | 0 0 0 0 -0 0 109 | 5.8137 5.87949 12.9616 -0.711626 0.274778 0.033566 110 | 5.8137 5.87949 12.9616 0 -0 0 111 | 112 | 113 | 114 | 7.00001 2.99999 0.499994 6e-06 -6e-06 0.000778 115 | 1 1 1 116 | 117 | 7.00001 2.99999 0.499994 6e-06 -6e-06 0.000778 118 | 0 0 0 0 -0 0 119 | -8.1882 -8.17715 3.1583 0.647868 0.682401 -3.13771 120 | -8.1882 -8.17715 3.1583 0 -0 0 121 | 122 | 123 | 124 | 0 0 10 0 -0 0 125 | 126 | 127 | 128 | 5 10 0.5 0 -0 0 129 | 130 | 131 | 1 132 | 133 | 0.145833 134 | 0 135 | 0 136 | 0.145833 137 | 0 138 | 0.125 139 | 140 | 141 | 142 | 143 | 144 | 1.5 145 | 1 146 | 147 | 148 | 10 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 1.5 166 | 1 167 | 168 | 169 | 170 | 174 | 175 | 176 | 0 177 | 0 178 | 0 179 | 180 | 181 | 182 | 7 9 0.5 0 -0 0 183 | 184 | 185 | 1 186 | 187 | 0.145833 188 | 0 189 | 0 190 | 0.145833 191 | 0 192 | 0.125 193 | 194 | 195 | 196 | 197 | 198 | 1 199 | 1 200 | 201 | 202 | 10 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 1 220 | 1 221 | 222 | 223 | 224 | 228 | 229 | 230 | 0 231 | 0 232 | 0 233 | 234 | 235 | 236 | 237 | 21.0306 -7.61458 70.9921 0 1.2404 1.86697 238 | orbit 239 | perspective 240 | 241 | 242 | 243 | 244 | -------------------------------------------------------------------------------- /src/sampling_gazebo_simulation/worlds/scenario3.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 1 5 | 0 0 10 0 -0 0 6 | 0.8 0.8 0.8 1 7 | 0.2 0.2 0.2 1 8 | 9 | 1000 10 | 0.9 11 | 0.01 12 | 0.001 13 | 14 | -0.5 0.1 -0.9 15 | 16 | 17 | 1 18 | 19 | 20 | 21 | 22 | 0 0 1 23 | 100 100 24 | 25 | 26 | 27 | 28 | 65535 29 | 30 | 31 | 32 | 33 | 100 34 | 50 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 10 43 | 44 | 45 | 0 46 | 47 | 48 | 0 0 1 49 | 100 100 50 | 51 | 52 | 53 | 57 | 58 | 59 | 0 60 | 0 61 | 0 62 | 63 | 64 | 0 0 -9.8 65 | 6e-06 2.3e-05 -4.2e-05 66 | 67 | 68 | 0.001 69 | 1 70 | 1000 71 | 72 | 73 | 0.4 0.4 0.4 1 74 | 0.7 0.7 0.7 1 75 | 1 76 | 77 | 80 | 81 | 82 | EARTH_WGS84 83 | 0 84 | 0 85 | 0 86 | 0 87 | 88 | 89 | 161 310000000 90 | 63 356207750 91 | 1596267821 779180567 92 | 61037 93 | 94 | 0 0 0 0 -0 0 95 | 1 1 1 96 | 97 | 0 0 0 0 -0 0 98 | 0 0 0 0 -0 0 99 | 0 0 0 0 -0 0 100 | 0 0 0 0 -0 0 101 | 102 | 103 | 104 | 0.998611 7.00172 0.499988 7e-06 7e-06 -0.000204 105 | 1 1 1 106 | 107 | 0.998611 7.00172 0.499988 7e-06 7e-06 -0.000204 108 | 0 0 0 0 -0 0 109 | 6.04326 -6.03226 -3.1791 0.777384 0.788271 -0.075295 110 | 6.04326 -6.03226 -3.1791 0 -0 0 111 | 112 | 113 | 114 | 0 0 10 0 -0 0 115 | 116 | 117 | 118 | 4.5 2 0.5 0 -0 0 119 | 120 | 121 | 1 122 | 123 | 0.145833 124 | 0 125 | 0 126 | 0.145833 127 | 0 128 | 0.125 129 | 130 | 131 | 132 | 133 | 134 | 2.5 135 | 1 136 | 137 | 138 | 10 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 2.5 156 | 1 157 | 158 | 159 | 160 | 164 | 165 | 166 | 0 167 | 0 168 | 0 169 | 170 | 171 | 172 | 173 | 17.0895 -35.7368 62.9283 0 0.975644 1.66019 174 | orbit 175 | perspective 176 | 177 | 178 | 179 | 180 | -------------------------------------------------------------------------------- /src/sampling_measurement/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(sampling_measurement) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | roscpp 8 | rospy 9 | std_msgs 10 | message_runtime 11 | sampling_msgs 12 | ) 13 | 14 | catkin_package( 15 | CATKIN_DEPENDS rospy sampling_msgs message_runtime 16 | ) 17 | 18 | include_directories( 19 | ${catkin_INCLUDE_DIRS} 20 | ) -------------------------------------------------------------------------------- /src/sampling_measurement/launch/measurement_simulation_server.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /src/sampling_measurement/launch/simulation_measurement.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /src/sampling_measurement/launch/temperature_server.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /src/sampling_measurement/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sampling_measurement 4 | 0.0.0 5 | The sampling_measurement package 6 | 7 | Yang Zhang 8 | Yunfei Shi 9 | Ning Wang 10 | Jianmin Zheng 11 | 12 | BSD 13 | 14 | 15 | catkin 16 | roscpp 17 | rospy 18 | std_msgs 19 | message_runtime 20 | sampling_msgs 21 | 22 | 23 | -------------------------------------------------------------------------------- /src/sampling_measurement/script/measurement_simulation_server.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import itertools 3 | import numpy as np 4 | import rospkg 5 | import rospy 6 | from sampling_msgs.srv import RequestMeasurement, RequestMeasurementResponse 7 | 8 | class MeasurementSimulator(object): 9 | def __init__(self): 10 | rospy.init_node('measurement_simulation_node') 11 | measurement_trial = rospy.get_param("~measurement_trial") 12 | self.rospack = rospkg.RosPack() 13 | self.polyfit_order = rospy.get_param('~poly_order', 5) 14 | self.noise_stdev = rospy.get_param('~noise_stdev', 0.5) 15 | self.position_x, self.position_y = self.loadposition(measurement_trial) 16 | self.measurement = self.loadmeasurement(measurement_trial) 17 | self.polyfit_coef = self.polyfit2d(self.position_x, self.position_y, self.measurement, order=self.polyfit_order) 18 | self.generateGroundTruth(measurement_trial) 19 | self.measurement_simulation_server = rospy.Service('measurement_simulation', RequestMeasurement, self.simulatemeasurement) 20 | rospy.spin() 21 | 22 | def polyfit2d(self, x, y, z, order=3): 23 | ncols = (order + 1)**2 24 | G = np.zeros((x.size, ncols)) 25 | ij = itertools.product(range(order+1), range(order+1)) 26 | for k, (i,j) in enumerate(ij): 27 | G[:,k] = x**i * y**j 28 | m, _, _, _ = np.linalg.lstsq(G, z, rcond=None) 29 | return m 30 | 31 | def polyval2d(self, x, y, m): 32 | order = int(np.sqrt(len(m))) - 1 33 | ij = itertools.product(range(order+1), range(order+1)) 34 | z = 0.0 35 | for a, (i,j) in zip(m, ij): 36 | z += a * x**i * y**j 37 | return z 38 | 39 | def generateGroundTruth(self, measurement_trail): 40 | artifical_ground_truth = self.rospack.get_path('sampling_data') + "/measurement/artificial_" + measurement_trail + ".txt" 41 | data_file = open(artifical_ground_truth, "w") 42 | for x, y in zip(self.position_x, self.position_y): 43 | measurement = self.polyval2d(x, y, self.polyfit_coef) 44 | data_file.write("%f\n" %(measurement)) 45 | data_file.close() 46 | 47 | def loadposition(self, measurement_trial): 48 | position_x = [] 49 | position_y = [] 50 | position_file = self.rospack.get_path('sampling_data') + "/location/" + measurement_trial + ".txt" 51 | with open(position_file, "r") as filestream: 52 | for line in filestream: 53 | new_x, new_y = line.split(",") 54 | position_x.append(float(new_x)) 55 | position_y.append(float(new_y)) 56 | return np.array(position_x), np.array(position_y) 57 | 58 | def loadmeasurement(self, measurement_trail): 59 | measurement_file = self.rospack.get_path('sampling_data') + "/measurement/" + measurement_trail + ".txt" 60 | return np.loadtxt(measurement_file) 61 | 62 | def simulatemeasurement(self, req): 63 | simulated_measurement = self.polyval2d(req.position.x, req.position.y, self.polyfit_coef) 64 | simulated_measurement += np.random.normal(0, self.noise_stdev) 65 | return RequestMeasurementResponse(simulated_measurement) 66 | 67 | if __name__ == "__main__": 68 | measurement_simulation_server = MeasurementSimulator() 69 | -------------------------------------------------------------------------------- /src/sampling_measurement/script/temperature_server.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import sys 3 | import rospy 4 | import time 5 | import serial 6 | import re 7 | from collections import deque 8 | import numpy as np 9 | from sampling_msgs.msg import temperature_measurement 10 | from sampling_msgs.srv import RequestTemperatureMeasurement, RequestTemperatureMeasurementResponse 11 | 12 | class Temper(object): 13 | 14 | def __init__(self): 15 | self.raw_temp_window = deque() 16 | self.fused_temp_window = deque() 17 | self.fused_temp = 0.0 18 | self.raw_temp = 0.0 19 | self.converged = False 20 | self.ser = [] 21 | 22 | def read(self): 23 | try: 24 | line = self.ser.readline() 25 | m = re.search(r'[0-9.]+', line) 26 | if m is not None: 27 | degC = (float(m.group(0)) - 32) / 1.8 28 | # publish(degC, pub) 29 | return degC 30 | 31 | except KeyboardInterrupt: 32 | print('exiting') 33 | self.ser.close() 34 | return 35 | 36 | def is_converged(self, thre=0.1): 37 | # if len(self.fused_temp_window) >= 5: 38 | # self.fused_temp_window.popleft() 39 | # self.fused_temp_window.append(self.fused_temp) 40 | temp_std = np.std(list(self.fused_temp_window)) 41 | return (temp_std < thre) 42 | 43 | def collect_temperature_sample(self, req): 44 | while (not self.is_converged()): 45 | rospy.loginfo("Waiting for temperature measurement to converge") 46 | return RequestTemperatureMeasurementResponse(self.fused_temp) 47 | 48 | def main(self): 49 | # pub = rospy.Publisher('temp', Temperature, queue_size=1) 50 | converge_timer = time.time() 51 | rospy.init_node('node_temper') 52 | temperature_report_service_channel = rospy.get_param("~temperature_report_service_channel") 53 | temperature_report_service = rospy.Service(temperature_report_service_channel, RequestTemperatureMeasurement, self.collect_temperature_sample) 54 | temperature_publish_channel = rospy.get_param("~temperature_publish_channel") 55 | temperature_pub = rospy.Publisher(temperature_publish_channel, temperature_measurement, queue_size=10) 56 | usb_port = rospy.get_param("~USBPort") 57 | self.ser = serial.Serial(usb_port, 38400, timeout=1, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE, bytesize=serial.EIGHTBITS ) # open serial port 58 | r = rospy.Rate(50) 59 | while not rospy.is_shutdown(): 60 | degC = self.read() 61 | if not (degC is None): 62 | if len(self.raw_temp_window) >= 5: 63 | self.raw_temp_window.popleft() 64 | self.raw_temp_window.append(degC) 65 | self.raw_temp = degC 66 | 67 | if len(self.raw_temp_window) > 0: 68 | self.fused_temp = np.mean(list(self.raw_temp_window)) 69 | # check if converged 70 | now = time.time() 71 | self.fused_temp_window.append(self.fused_temp) 72 | if now - converge_timer > 0.2: 73 | self.converged = self.is_converged() 74 | converge_timer = time.time() 75 | self.fused_temp_window.clear() 76 | msg = temperature_measurement() 77 | msg.header.stamp = rospy.Time.now() 78 | msg.raw_temperature = self.raw_temp 79 | msg.filtered_temperature = self.fused_temp 80 | msg.converged = self.converged 81 | temperature_pub.publish(msg) 82 | r.sleep() 83 | return 0 84 | 85 | if __name__ == "__main__": 86 | temper = Temper() 87 | sys.exit(temper.main()) 88 | -------------------------------------------------------------------------------- /src/sampling_modeling/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(sampling_modeling) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | rospy 6 | sampling_msgs 7 | geometry_msgs 8 | std_srvs 9 | ) 10 | 11 | include_directories( 12 | ${catkin_INCLUDE_DIRS} 13 | ) 14 | 15 | -------------------------------------------------------------------------------- /src/sampling_modeling/launch/sampling_model.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | num_gp: 3 8 | gating: [1.0, 2.0, 3.0] 9 | modeling_gp_0_kernel: [5.0, 1.0, 0.1] 10 | gating_gp_0_kernel: [1.0, 0.5, 0.0] 11 | modeling_gp_1_kernel: [3.0, 2.0, 0.1] 12 | gating_gp_1_kernel: [2.0, 0.5, 0.0] 13 | modeling_gp_2_kernel: [1.0, 3.0, 0.1] 14 | gating_gp_2_kernel: [3.0, 0.5, 0.0] 15 | noise_stdev: 0.1 16 | EM_epsilon: 0.05 17 | EM_max_iteration: 100 18 | 19 | 20 | -------------------------------------------------------------------------------- /src/sampling_modeling/launch/sampling_modeling.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | num_gp: 3 10 | gating: [1.0, 2.0, 3.0] 11 | modeling_gp_0_kernel: [5.0, 1.0, 0.1] 12 | gating_gp_0_kernel: [1.0, 0.5, 0.0] 13 | modeling_gp_1_kernel: [3.0, 2.0, 0.1] 14 | gating_gp_1_kernel: [2.0, 0.5, 0.0] 15 | modeling_gp_2_kernel: [1.0, 3.0, 0.1] 16 | gating_gp_2_kernel: [3.0, 0.5, 0.0] 17 | noise_stdev: 0.1 18 | EM_epsilon: 0.05 19 | EM_max_iteration: 100 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /src/sampling_modeling/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sampling_modeling 4 | 0.0.0 5 | The sampling_modeling package 6 | 7 | Yang Zhang 8 | Yunfei Shi 9 | Ning Wang 10 | Jianmin Zheng 11 | 12 | BSD 13 | 14 | catkin 15 | 16 | rospy 17 | sampling_msgs 18 | geometry_msgs 19 | std_srvs 20 | 21 | 22 | -------------------------------------------------------------------------------- /src/sampling_modeling/script/gp.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # reference: http://krasserm.github.io/2018/03/19/gaussian-processes/ 4 | 5 | 6 | import numpy as np 7 | import rospkg 8 | import rospy 9 | from numpy.linalg import inv 10 | from numpy.linalg import cholesky, det, lstsq 11 | from scipy.optimize import minimize 12 | 13 | class RBF_kernel: 14 | def __init__(self, l=0.5, sigma_f=0.5): 15 | self.l = l 16 | self.sigma_f = sigma_f 17 | 18 | def Compute(self, X1, X2): 19 | sqdist = np.sum(X1**2, 1).reshape(-1, 1) + np.sum(X2**2, 1) - 2 * np.dot(X1, X2.T) 20 | return self.sigma_f ** 2 * np.exp(-0.5 / self.l ** 2 * sqdist) 21 | 22 | def UpdateKernel(self, l, sigma_f): 23 | self.l = l 24 | self.sigma_f = sigma_f 25 | 26 | def ComputeKernel(self, X1, X2, l, sigma_f): 27 | sqdist = np.sum(X1**2, 1).reshape(-1, 1) + np.sum(X2**2, 1) - 2 * np.dot(X1, X2.T) 28 | return sigma_f ** 2 * np.exp(-0.5 / l ** 2 * sqdist) 29 | 30 | def GetHyperparam(self): 31 | return self.l, self.sigma_f 32 | 33 | class GP: 34 | def __init__(self, l=0.5, sigma_f=0.5, sigma_y=0.1): 35 | self.kernel = RBF_kernel(l, sigma_f) 36 | self.sigma_y = sigma_y 37 | self.X_train = None 38 | self.Y_train = None 39 | 40 | def UpdateData(self, X_train, Y_train): 41 | self.X_train = X_train 42 | self.Y_train = Y_train 43 | 44 | def PosteriorPredict(self, X_test, X_train=None, Y_train=None, P= None): 45 | ''' 46 | Computes the suffifient statistics of the GP posterior predictive distribution 47 | from m training data X_train and Y_train and n new inputs X_s. 48 | 49 | Args: 50 | X_s: New input locations (n x d). 51 | X_train: Training locations (m x d). 52 | Y_train: Training targets (m x 1). 53 | l: Kernel length parameter. 54 | sigma_f: Kernel vertical variation parameter. 55 | sigma_y: Noise parameter. 56 | 57 | Returns: 58 | Posterior mean vector (n x d) and covariance matrix (n x n). 59 | ''' 60 | if X_train is not None: 61 | self.UpdateData(X_train, Y_train) 62 | 63 | K_train_test = self.kernel.Compute(self.X_train, X_test) 64 | K_train_train = self.kernel.Compute(self.X_train, self.X_train) 65 | K_test_test = self.kernel.Compute(X_test, X_test) + 1e-8 * np.eye(len(X_test)) 66 | if P is None: 67 | K_inv = inv(K_train_train + self.sigma_y ** 2 * np.eye(len(self.X_train))) 68 | else: 69 | K_inv = inv(K_train_train + self.sigma_y ** 2 * np.eye(len(self.X_train)) * np.diag(1.0/P)) 70 | 71 | mu_s = K_train_test.T.dot(K_inv).dot(self.Y_train) 72 | 73 | cov_s = K_test_test - K_train_test.T.dot(K_inv).dot(K_train_test) 74 | 75 | return mu_s, np.diag(cov_s) 76 | 77 | def OptimizeKernel(self, X_train=None, Y_train=None, p = None): 78 | ''' 79 | Returns a function that Computes the negative log marginal 80 | likelihood for training data X_train and Y_train and given 81 | noise level. 82 | 83 | Args: 84 | X_train: training locations (m x d). 85 | Y_train: training targets (m x 1). 86 | noise: known noise level of Y_train. 87 | ''' 88 | if X_train is not None: 89 | self.UpdateData(X_train, Y_train) 90 | def nnl_stable(theta): 91 | K = self.kernel.ComputeKernel(X_train, X_train, l=theta[0], sigma_f=theta[1]) + self.sigma_y**2 * np.eye(len(Y_train)) 92 | L = cholesky(K) 93 | return np.sum(np.log(np.diagonal(L))) + \ 94 | 0.5 * Y_train.T.dot(lstsq(L.T, lstsq(L, Y_train,rcond=None)[0],rcond=None)[0]) + \ 95 | 0.5 * len(X_train) * np.log(2*np.pi) 96 | l_init, sigma_f_init = self.kernel.GetHyperparam() 97 | 98 | res = minimize(nnl_stable, [l_init, sigma_f_init], method='L-BFGS-B') 99 | updated_l, updated_sigma_f = res.x 100 | self.kernel.UpdateKernel(updated_l, updated_sigma_f) 101 | -------------------------------------------------------------------------------- /src/sampling_modeling/script/gp_util.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | 4 | from matplotlib import cm 5 | from mpl_toolkits.mplot3d import Axes3D 6 | 7 | def plot_gp(mu, cov, X, X_train=None, Y_train=None, samples=[]): 8 | X = X.ravel() 9 | mu = mu.ravel() 10 | uncertainty = 1.96 * np.sqrt(np.diag(cov)) 11 | 12 | plt.fill_between(X, mu + uncertainty, mu - uncertainty, alpha=0.1) 13 | plt.plot(X, mu, label='Mean') 14 | for i, sample in enumerate(samples): 15 | plt.plot(X, sample) 16 | if X_train is not None: 17 | plt.plot(X_train, Y_train, 'rx') 18 | plt.legend() 19 | plt.show() 20 | 21 | def plot_gp_2D(gx, gy, mu, X_train, Y_train, title, i): 22 | ax = plt.gcf().add_subplot(1, 2, i, projection='3d') 23 | ax.plot_surface(gx, gy, mu.reshape(gx.shape), cmap=cm.coolwarm, linewidth=0, alpha=0.2, antialiased=False) 24 | ax.scatter(X_train[:,0], X_train[:,1], Y_train, c=Y_train, cmap=cm.coolwarm) 25 | ax.set_title(title) -------------------------------------------------------------------------------- /src/sampling_modeling/script/mixture_gp.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # reference: http://krasserm.github.io/2018/03/19/gaussian-processes/ 4 | 5 | import numpy as np 6 | from scipy.stats import norm 7 | from gp import RBF_kernel, GP 8 | from sklearn.preprocessing import normalize 9 | 10 | 11 | class MixtureGaussianProcess: 12 | def __init__(self, num_gp=3, gps = [GP() for i in range(3)], gating_gps = [GP() for i in range(3)], epsilon=0.05, max_iter=100): 13 | self.num_gp = num_gp 14 | self.gps = gps 15 | self.gating_gps = gating_gps 16 | self.X_train = None 17 | self.Y_train = None 18 | self.P = None 19 | self.epsilon= epsilon 20 | self.max_iter=max_iter 21 | 22 | def Expectation(self, pred_mean, pred_var, Y_train, P): 23 | R = np.zeros((len(Y_train), self.num_gp)) 24 | for i in range(self.num_gp): 25 | R[:, i] = norm(loc=pred_mean[:, i], scale=np.std(pred_var[:, i])).pdf(Y_train) 26 | P = P * R + 1e-6 27 | P = P / np.sum(P, axis = 1)[:,np.newaxis] 28 | return P 29 | 30 | def Maximization(self, X_train, Y_train, P): 31 | pred_mean = np.zeros((len(Y_train), self.num_gp)) 32 | pred_var = np.zeros_like(pred_mean) 33 | for i in range(self.num_gp): 34 | pred_mean[:, i], pred_var[:, i] = self.gps[i].PosteriorPredict(X_test=X_train, X_train=X_train, Y_train=Y_train, P=P[:, i]) 35 | return pred_mean, pred_var 36 | 37 | def Optimizate(self, X_train, Y_train, P): 38 | for i in range(self.num_gp): 39 | self.gps[i].OptimizeKernel(X_train=X_train, Y_train=Y_train, p=P[:,[i]]) 40 | 41 | def EMOptimize(self, optimize_kernel = False): 42 | for i in range(self.max_iter): 43 | prev_P = self.P 44 | pred_mean, pred_var = self.Maximization(self.X_train, self.Y_train, self.P) 45 | self.P = self.Expectation(pred_mean, pred_var, self.Y_train, self.P) 46 | diff_P = np.abs(self.P - prev_P) 47 | if (diff_P.max() <= self.epsilon): 48 | break 49 | if optimize_kernel == True: 50 | self.Optimizate(self.X_train, self.Y_train, self.P) 51 | pred_mean = pred_mean * self.P 52 | pred_var = pred_var * self.P 53 | return pred_mean.sum(axis=1), pred_var.sum(axis=1), self.P 54 | 55 | def AddSample(self, X_train, Y_train): 56 | if self.X_train is None: 57 | self.X_train = np.asarray(X_train) 58 | self.Y_train = np.asarray(Y_train) 59 | self.P = np.random.random((len(Y_train), self.num_gp)) 60 | self.P = self.P / np.sum(self.P, axis = 1)[:,np.newaxis] 61 | else: 62 | self.X_train = np.concatenate((self.X_train, X_train), axis=0) 63 | self.Y_train = np.concatenate((self.Y_train, Y_train)).reshape(-1) 64 | new_P = np.random.random((len(Y_train), self.num_gp)) 65 | new_P = new_P / np.sum(new_P, axis = 1)[:,np.newaxis] 66 | self.P = np.concatenate((self.P, new_P), axis=0) 67 | # self.X_train, I = np.unique(self.X_train, axis=0, return_index=True) 68 | # self.Y_train = self.Y_train[I] 69 | # self.P = self.P[I,:] 70 | 71 | def FitGatingFunction(self, X_train, P): 72 | for i in range(self.num_gp): 73 | self.gating_gps[i].OptimizeKernel( X_train=X_train, Y_train=P[:, [i]]) 74 | 75 | def PredictGatingFunction(self, X_test, X_train, P): 76 | P_prediction = np.zeros((X_test.shape[0], self.num_gp)) 77 | for i in range(self.num_gp): 78 | P_prediction[:, [i]], _ = self.gating_gps[i].PosteriorPredict(X_test, X_train=X_train, Y_train=P[:, [i]]) 79 | return P_prediction 80 | 81 | def OptimizeModel(self, optimize_kernel = False): 82 | _, _, P = self.EMOptimize(optimize_kernel) 83 | if optimize_kernel == True: 84 | self.FitGatingFunction(self.X_train, P) 85 | 86 | def Predict(self, X_test, X_train=None, Y_train=None): 87 | if X_train is None: 88 | X_train = self.X_train 89 | Y_train = self.Y_train 90 | P = self.PredictGatingFunction(X_test, self.X_train, self.P) 91 | pred_mean = np.zeros((X_test.shape[0], self.num_gp)) 92 | pred_var = np.zeros_like(pred_mean) 93 | for i in range(self.num_gp): 94 | pred_mean[:, i], pred_var[:, i] = self.gps[i].PosteriorPredict(X_test=X_test, X_train=X_train, Y_train=Y_train, P=self.P[:, i]) 95 | mean = pred_mean * P 96 | var = pred_var * P 97 | return mean.sum(axis=1), var.sum(axis=1) -------------------------------------------------------------------------------- /src/sampling_modeling/script/sampling_modeling_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import numpy as np 3 | import rospy 4 | from mixture_gp import MixtureGaussianProcess 5 | from gp import GP 6 | from sampling_msgs.srv import AddSampleToModel, AddSampleToModelResponse, AddTestPositionToModel, AddTestPositionToModelResponse, ModelPredict, ModelPredictResponse 7 | from std_srvs.srv import Trigger, TriggerResponse 8 | from geometry_msgs.msg import Point 9 | 10 | KModelingNameSpace = "modeling/" 11 | KOnlineOptimizationThreshold = 1000 12 | 13 | class SamplingModeling(object): 14 | def __init__(self): 15 | rospy.init_node('sampling_modeling_node') 16 | num_gp = rospy.get_param("~num_gp", 3) 17 | self.optimize_kernel = rospy.get_param("~online_kernel_optimization", True) 18 | modeling_gps = [] 19 | gating_gps = [] 20 | for i in range(num_gp): 21 | modeling_gp_param = rospy.get_param("~modeling_gp_" + str(i) + "_kernel", [0.5, 0.5, 0.1]) 22 | gating_gp_param = rospy.get_param("~gating_gp_" + str(i) + "_kernel", [0.5, 0.5, 0.1]) 23 | assert len(modeling_gp_param) == 3 24 | assert len(gating_gp_param) == 3 25 | modeling_gps.append(GP(modeling_gp_param[0], modeling_gp_param[1], modeling_gp_param[2])) 26 | gating_gps.append(GP(gating_gp_param[0], gating_gp_param[1], gating_gp_param[2])) 27 | EM_epsilon = rospy.get_param("~EM_epsilon", 0.03) 28 | EM_max_iteration = rospy.get_param("~EM_max_iteration", 100) 29 | self.model = MixtureGaussianProcess(num_gp=num_gp, gps=modeling_gps, gating_gps=gating_gps, epsilon=EM_epsilon, max_iter=EM_max_iteration) 30 | self.X_test = None 31 | self.add_test_position_server = rospy.Service(KModelingNameSpace + 'add_test_position', AddTestPositionToModel, self.AddTestPosition) 32 | self.add_sample_server = rospy.Service(KModelingNameSpace + 'add_samples_to_model', AddSampleToModel, self.AddSampleToModel) 33 | self.update_model_server = rospy.Service(KModelingNameSpace + 'update_model', Trigger, self.UpdateModel) 34 | self.model_predict_server = rospy.Service(KModelingNameSpace + 'model_predict', ModelPredict, self.ModelPredict) 35 | self.sample_count = 0 36 | rospy.spin() 37 | 38 | def AddTestPosition(self, req): 39 | self.X_test = np.zeros((len(req.positions), 2)) 40 | for i in range(len(req.positions)): 41 | self.X_test[i,0] = req.positions[i].x 42 | self.X_test[i,1] = req.positions[i].y 43 | return AddTestPositionToModelResponse(True) 44 | 45 | def AddSampleToModel(self, req): 46 | new_X = np.zeros((len(req.measurements), 2)) 47 | new_Y = np.asarray(req.measurements) 48 | for i in range(len(req.measurements)): 49 | new_X[i, 0] = req.positions[i].x 50 | new_X[i, 1] = req.positions[i].y 51 | self.model.AddSample(new_X, new_Y.reshape(-1)) 52 | self.sample_count = self.sample_count + len(new_Y) 53 | self.optimize_kernel = (self.sample_count <= KOnlineOptimizationThreshold) 54 | return AddSampleToModelResponse(True) 55 | 56 | def UpdateModel(self, req): 57 | self.model.OptimizeModel(optimize_kernel = self.optimize_kernel) 58 | return TriggerResponse( success=True, message="Successfully updated MGP model!") 59 | 60 | def ModelPredict(self, req): 61 | if self.X_test is None: 62 | return ModelPredictResponse(success=False) 63 | pred_mean, pred_var = self.model.Predict(self.X_test) 64 | return ModelPredictResponse(mean=pred_mean, var=pred_var, success=True) 65 | 66 | if __name__ == "__main__": 67 | sampling_modeling_server = SamplingModeling() 68 | -------------------------------------------------------------------------------- /src/sampling_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(sampling_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | std_msgs 6 | geometry_msgs 7 | message_generation 8 | ) 9 | 10 | add_message_files( 11 | FILES 12 | Sample.msg 13 | AgentLocation.msg 14 | SamplingPerformance.msg 15 | ) 16 | 17 | add_service_files( 18 | FILES 19 | RequestLocation.srv 20 | SamplingGoal.srv 21 | RequestTemperatureMeasurement.srv 22 | MeasurementService.srv 23 | ReportSampleService.srv 24 | StopAgent.srv 25 | RequestMeasurement.srv 26 | AddSampleToModel.srv 27 | AddTestPositionToModel.srv 28 | ModelPredict.srv 29 | KillAgent.srv 30 | ) 31 | 32 | generate_messages( 33 | DEPENDENCIES 34 | std_msgs 35 | geometry_msgs 36 | ) 37 | 38 | catkin_package( 39 | CATKIN_DEPENDS std_msgs message_runtime 40 | ) -------------------------------------------------------------------------------- /src/sampling_msgs/msg/AgentLocation.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | string agent_id 3 | geometry_msgs/Point position -------------------------------------------------------------------------------- /src/sampling_msgs/msg/Sample.msg: -------------------------------------------------------------------------------- 1 | bool valid 2 | string agent_id 3 | geometry_msgs/Point position 4 | float64 data -------------------------------------------------------------------------------- /src/sampling_msgs/msg/SamplingPerformance.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | int32 sample_count 3 | float64 rmse 4 | float64 average_variance -------------------------------------------------------------------------------- /src/sampling_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sampling_msgs 4 | 0.0.0 5 | The sampling_msgs package 6 | 7 | Yang Zhang 8 | Yunfei Shi 9 | Ning Wang 10 | Jianmin Zheng 11 | 12 | BSD 13 | 14 | catkin 15 | std_msgs 16 | message_runtime 17 | message_generation 18 | geometry_msgs 19 | 20 | 21 | -------------------------------------------------------------------------------- /src/sampling_msgs/srv/AddSampleToModel.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/Point[] positions 2 | float64[] measurements 3 | --- 4 | bool success -------------------------------------------------------------------------------- /src/sampling_msgs/srv/AddTestPositionToModel.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/Point[] positions 2 | --- 3 | bool success -------------------------------------------------------------------------------- /src/sampling_msgs/srv/KillAgent.srv: -------------------------------------------------------------------------------- 1 | string agent_id 2 | --- 3 | bool success -------------------------------------------------------------------------------- /src/sampling_msgs/srv/MeasurementService.srv: -------------------------------------------------------------------------------- 1 | string agent_id 2 | --- 3 | float64 data -------------------------------------------------------------------------------- /src/sampling_msgs/srv/ModelPredict.srv: -------------------------------------------------------------------------------- 1 | --- 2 | float64[] mean 3 | float64[] var 4 | bool success -------------------------------------------------------------------------------- /src/sampling_msgs/srv/ReportSampleService.srv: -------------------------------------------------------------------------------- 1 | string robot_id 2 | float64 data 3 | geometry_msgs/Point position 4 | --- 5 | bool success 6 | -------------------------------------------------------------------------------- /src/sampling_msgs/srv/RequestLocation.srv: -------------------------------------------------------------------------------- 1 | int32 robot_id 2 | --- 3 | float64 latitude 4 | float64 longitude -------------------------------------------------------------------------------- /src/sampling_msgs/srv/RequestMeasurement.srv: -------------------------------------------------------------------------------- 1 | string agent_id 2 | geometry_msgs/Point position 3 | --- 4 | float64 data -------------------------------------------------------------------------------- /src/sampling_msgs/srv/RequestTemperatureMeasurement.srv: -------------------------------------------------------------------------------- 1 | string robot_id 2 | --- 3 | float64 temperature -------------------------------------------------------------------------------- /src/sampling_msgs/srv/SamplingGoal.srv: -------------------------------------------------------------------------------- 1 | AgentLocation agent_location 2 | --- 3 | geometry_msgs/Point target_position -------------------------------------------------------------------------------- /src/sampling_msgs/srv/StopAgent.srv: -------------------------------------------------------------------------------- 1 | string agent_id 2 | --- 3 | bool success -------------------------------------------------------------------------------- /src/sampling_online_learning/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(sampling_online_learning) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | sampling_utils 7 | geometry_msgs 8 | ) 9 | 10 | find_package(Eigen3 REQUIRED) 11 | 12 | catkin_package( 13 | INCLUDE_DIRS include 14 | LIBRARIES sampling_online_learning 15 | DEPENDS EIGEN3 16 | ) 17 | 18 | include_directories( 19 | include 20 | ${catkin_INCLUDE_DIRS} 21 | ${EIGEN3_INCLUDE_DIR} 22 | ) 23 | 24 | add_library(${PROJECT_NAME} 25 | src/online_learning_handler.cpp 26 | ) 27 | 28 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 29 | 30 | install(TARGETS ${PROJECT_NAME} 31 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 32 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 33 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 34 | ) 35 | 36 | install(DIRECTORY include/${PROJECT_NAME}/ 37 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 38 | FILES_MATCHING PATTERN "*.h" 39 | PATTERN ".svn" EXCLUDE 40 | ) 41 | -------------------------------------------------------------------------------- /src/sampling_online_learning/include/sampling_online_learning/online_learning_handler.h: -------------------------------------------------------------------------------- 1 | /** 2 | * AUTHOR: Yang Zhang 3 | */ 4 | 5 | #pragma once 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include // std::pair, std::make_pair 15 | #include 16 | 17 | namespace sampling { 18 | namespace learning { 19 | 20 | const double KLearningBeta = 0.5; 21 | 22 | const std::string KLearningType_Greedy = "GREEDY"; 23 | const std::string KLearningType_UCB = "UCB"; 24 | const std::string KLearningType_Default = KLearningType_Greedy; 25 | 26 | class OnlineLearningHandler { 27 | public: 28 | OnlineLearningHandler() = delete; 29 | 30 | static std::unique_ptr MakeUniqueFromRosParam( 31 | ros::NodeHandle &ph); 32 | 33 | bool UpdateSampleCount(const geometry_msgs::Point &position); 34 | 35 | bool InformativeSelection(const Eigen::MatrixXd &locations, 36 | const std::vector &mean, 37 | const std::vector &variance, 38 | geometry_msgs::Point &informative_point); 39 | 40 | private: 41 | OnlineLearningHandler(const std::string &learning_type, 42 | const double &learning_beta); 43 | 44 | std::unordered_map, double, 45 | boost::hash>> 46 | count_map_; 47 | 48 | std::string learning_type_; 49 | 50 | double learning_beta_; 51 | }; 52 | } // namespace learning 53 | } // namespace sampling -------------------------------------------------------------------------------- /src/sampling_online_learning/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sampling_online_learning 4 | 0.0.0 5 | The sampling_online_learning package 6 | 7 | Yang Zhang 8 | Yunfei Shi 9 | Ning Wang 10 | Jianmin Zheng 11 | 12 | BSD 13 | 14 | catkin 15 | roscpp 16 | sampling_utils 17 | geometry_msgs 18 | 19 | 20 | -------------------------------------------------------------------------------- /src/sampling_online_learning/src/online_learning_handler.cpp: -------------------------------------------------------------------------------- 1 | #include "sampling_online_learning/online_learning_handler.h" 2 | 3 | #include "sampling_utils/utils.h" 4 | 5 | namespace sampling { 6 | namespace learning { 7 | 8 | std::unique_ptr 9 | OnlineLearningHandler::MakeUniqueFromRosParam(ros::NodeHandle &ph) { 10 | std::string learning_type; 11 | double learning_beta; 12 | ph.param("learning_type", learning_type, KLearningType_Default); 13 | ph.param("learning_beta", learning_beta, KLearningBeta); 14 | return std::unique_ptr( 15 | new OnlineLearningHandler(learning_type, learning_beta)); 16 | } 17 | 18 | bool OnlineLearningHandler::UpdateSampleCount( 19 | const geometry_msgs::Point &position) { 20 | count_map_[std::make_pair(position.x, position.y)] += 1.0; 21 | return true; 22 | } 23 | 24 | bool OnlineLearningHandler::InformativeSelection( 25 | const Eigen::MatrixXd &locations, const std::vector &mean, 26 | const std::vector &variance, 27 | geometry_msgs::Point &informative_point) { 28 | const size_t location_size = locations.rows(); 29 | if (location_size != mean.size() || location_size != variance.size()) { 30 | ROS_ERROR_STREAM("Informative point selection data does NOT match!"); 31 | return false; 32 | } 33 | 34 | if (KLearningType_Greedy.compare(learning_type_) == 0) { 35 | double max_variance = 0.0; 36 | int max_variance_index = 0; 37 | for (int i = 0; i < variance.size(); ++i) { 38 | if (variance[i] > max_variance) { 39 | max_variance = variance[i]; 40 | max_variance_index = i; 41 | } 42 | } 43 | 44 | informative_point.x = locations(max_variance_index, 0); 45 | informative_point.y = locations(max_variance_index, 1); 46 | 47 | return true; 48 | } else if (KLearningType_UCB.compare(learning_type_) == 0) { 49 | Eigen::VectorXd utility = Eigen::VectorXd(location_size); 50 | for (int i = 0; i < location_size; ++i) { 51 | utility(i) = 52 | mean[i] + 53 | variance[i] * learning_beta_ / 54 | (count_map_[std::make_pair(locations(i, 0), locations(i, 1))] + 55 | 1.0); 56 | } 57 | int max_utility_index = utility.maxCoeff(); 58 | informative_point.x = locations(max_utility_index, 0); 59 | informative_point.y = locations(max_utility_index, 1); 60 | return true; 61 | } else { 62 | ROS_ERROR_STREAM("Unknown informative point selection method!"); 63 | return false; 64 | } 65 | 66 | return false; 67 | } 68 | 69 | OnlineLearningHandler::OnlineLearningHandler(const std::string &learning_type, 70 | const double &learning_beta) 71 | : learning_type_(learning_type), learning_beta_(learning_beta) {} 72 | 73 | } // namespace learning 74 | } // namespace sampling -------------------------------------------------------------------------------- /src/sampling_partition/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(sampling_partition) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | roslib 7 | geometry_msgs 8 | sampling_msgs 9 | sampling_utils 10 | sampling_visualization 11 | ) 12 | 13 | find_package(Eigen3 REQUIRED) 14 | 15 | catkin_package( 16 | INCLUDE_DIRS include 17 | LIBRARIES sampling_partition 18 | DEPENDS EIGEN3 19 | ) 20 | 21 | include_directories( 22 | include 23 | ${catkin_INCLUDE_DIRS} 24 | ${EIGEN3_INCLUDE_DIR} 25 | ) 26 | 27 | add_library(${PROJECT_NAME} 28 | src/heterogeneity.cpp 29 | src/heterogeneity_distance.cpp 30 | src/heterogeneity_distance_dependent.cpp 31 | src/heterogeneity_topography_dependent.cpp 32 | src/weighted_voronoi_partition.cpp 33 | src/weighted_voronoi_partition_params.cpp 34 | ) 35 | 36 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 37 | 38 | 39 | add_executable(partition_node node/partition_node.cpp) 40 | target_link_libraries(partition_node ${PROJECT_NAME} ${catkin_LIBRARIES} ) 41 | 42 | install(TARGETS ${PROJECT_NAME} 43 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 44 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 45 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 46 | ) 47 | 48 | install(DIRECTORY include/${PROJECT_NAME}/ 49 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 50 | FILES_MATCHING PATTERN "*.h" 51 | PATTERN ".svn" EXCLUDE 52 | ) 53 | -------------------------------------------------------------------------------- /src/sampling_partition/config/heterogeneiry_property.config: -------------------------------------------------------------------------------- 1 | HeterogeneousProperty: 2 | - agent_ids: ["jackal0", "jackal1", "hector0"] 3 | heterogeneity: ["DISTANCE", "SPEED", "BATTERY_LIFE", "TRAVERSABILITY"] 4 | weight_factor: [0.5, 0.1, 0.2, 0.3] 5 | - agent_id: ["jackal0"] 6 | heterogeneity_primitive: [1.0, 0.1, 0.1, 0.2] 7 | number_control_area: 2 8 | control_center_0: [0, 0, 0] 9 | control_radius_0: 0.5 10 | control_center_1: [ 1, 1, 0] 11 | control_radius_1: 2 12 | - agent_id: ["jackal1"] 13 | heterogeneity_primitive: [1.0, 0.1, 0.1, 0.2] 14 | number_control_area: 2 15 | control_center_0: [0, 0, 0] 16 | control_radius_0: 0.5 17 | control_center_1: [ 1, 1, 0] 18 | control_radius_1: 2 19 | - agent_id: ["hector0"] 20 | heterogeneity_primitive: [0.1, 0.1, 0.2] 21 | number_control_area: 0 -------------------------------------------------------------------------------- /src/sampling_partition/include/sampling_partition/heterogeneity.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | #include "sampling_partition/heterogeneity_params.h" 8 | 9 | namespace sampling { 10 | namespace partition { 11 | 12 | const std::string KHomogeneityDistance = "DISTANCE"; 13 | const double KDistancePrimitive = 1.0; 14 | 15 | const std::string KHeterogeneitySpeed = "SPEED"; 16 | const std::string KHeterogeneityBatteryLife = "BATTERY_LIFE"; 17 | const std::string KHeterogeneityTraversability = "TRAVERSABILITY"; 18 | 19 | class Heterogeneity { 20 | public: 21 | Heterogeneity() = delete; 22 | 23 | virtual Eigen::VectorXd CalculateCost( 24 | const geometry_msgs::Point &agent_position, 25 | const Eigen::VectorXd &distance) = 0; 26 | 27 | std::string GetType(); 28 | 29 | protected: 30 | Heterogeneity(const HeterogeneityParams ¶ms, const Eigen::MatrixXd &map); 31 | 32 | HeterogeneityParams params_; 33 | 34 | Eigen::MatrixXd map_; 35 | }; 36 | } // namespace partition 37 | } // namespace sampling -------------------------------------------------------------------------------- /src/sampling_partition/include/sampling_partition/heterogeneity_distance.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "sampling_partition/heterogeneity.h" 7 | 8 | namespace sampling { 9 | namespace partition { 10 | 11 | class HeterogeneityDistance : public Heterogeneity { 12 | public: 13 | HeterogeneityDistance() = delete; 14 | 15 | HeterogeneityDistance(const HeterogeneityParams ¶ms, 16 | const Eigen::MatrixXd &map); 17 | 18 | Eigen::VectorXd CalculateCost(const geometry_msgs::Point &agent_position, 19 | const Eigen::VectorXd &distance) override; 20 | }; 21 | } // namespace partition 22 | } // namespace sampling -------------------------------------------------------------------------------- /src/sampling_partition/include/sampling_partition/heterogeneity_distance_dependent.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "sampling_partition/heterogeneity.h" 7 | 8 | namespace sampling { 9 | namespace partition { 10 | 11 | class HeterogeneityDistanceDepedent : public Heterogeneity { 12 | public: 13 | HeterogeneityDistanceDepedent() = delete; 14 | 15 | HeterogeneityDistanceDepedent(const HeterogeneityParams ¶ms, 16 | const Eigen::MatrixXd &map); 17 | 18 | Eigen::VectorXd CalculateCost(const geometry_msgs::Point &agent_position, 19 | const Eigen::VectorXd &distance) override; 20 | }; 21 | } // namespace partition 22 | } // namespace sampling -------------------------------------------------------------------------------- /src/sampling_partition/include/sampling_partition/heterogeneity_params.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | namespace sampling { 9 | namespace partition { 10 | 11 | class HeterogeneityParams { 12 | public: 13 | HeterogeneityParams(){}; 14 | 15 | std::string heterogeneity_type; 16 | 17 | double heterogeneity_primitive; 18 | 19 | std::vector control_area_center; 20 | 21 | std::vector control_area_radius; 22 | }; 23 | } // namespace partition 24 | } // namespace sampling -------------------------------------------------------------------------------- /src/sampling_partition/include/sampling_partition/heterogeneity_topography_dependent.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "sampling_partition/heterogeneity.h" 7 | 8 | namespace sampling { 9 | namespace partition { 10 | 11 | class HeterogeneityTopographyDepedent : public Heterogeneity { 12 | public: 13 | HeterogeneityTopographyDepedent() = delete; 14 | 15 | HeterogeneityTopographyDepedent(const HeterogeneityParams ¶ms, 16 | const Eigen::MatrixXd &map); 17 | 18 | Eigen::VectorXd CalculateCost(const geometry_msgs::Point &agent_position, 19 | const Eigen::VectorXd &distance) override; 20 | 21 | private: 22 | Eigen::VectorXd topography_cost_; 23 | }; 24 | } // namespace partition 25 | } // namespace sampling -------------------------------------------------------------------------------- /src/sampling_partition/include/sampling_partition/weighted_voronoi_partition.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Utility functions for high dimensio voronoi construction 3 | * AUTHOR: Yang Zhang 4 | */ 5 | 6 | #pragma once 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include "sampling_msgs/AgentLocation.h" 16 | #include "sampling_partition/heterogeneity.h" 17 | #include "sampling_partition/weighted_voronoi_partition_params.h" 18 | 19 | namespace sampling { 20 | namespace partition { 21 | 22 | class WeightedVoronoiPartition { 23 | public: 24 | WeightedVoronoiPartition() = delete; 25 | 26 | static std::unique_ptr MakeUniqueFromRosParam( 27 | const std::vector &agent_ids, const Eigen::MatrixXd &map, 28 | ros::NodeHandle &ph); 29 | 30 | bool ComputePartitionForAgent( 31 | const std::string &agent_id, 32 | const std::vector &location, 33 | std::vector &partition_index); 34 | 35 | bool ComputePartitionForMap( 36 | const std::vector &location, 37 | std::vector &index_for_map); 38 | 39 | private: 40 | WeightedVoronoiPartition( 41 | const WeightedVoronoiPartitionParam ¶ms, 42 | const std::unordered_map> 43 | &heterogeneity_param_map, 44 | const Eigen::MatrixXd &map); 45 | 46 | Eigen::VectorXd CalculateEuclideanDistance(const geometry_msgs::Point &point, 47 | const Eigen::MatrixXd &map); 48 | 49 | WeightedVoronoiPartitionParam params_; 50 | 51 | std::unordered_map>> 52 | heterogeneity_map_; 53 | 54 | Eigen::MatrixXd map_; 55 | }; 56 | } // namespace partition 57 | } // namespace sampling -------------------------------------------------------------------------------- /src/sampling_partition/include/sampling_partition/weighted_voronoi_partition_params.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | namespace sampling { 9 | namespace partition { 10 | 11 | const double KCutOffCost = 100.0; 12 | 13 | class WeightedVoronoiPartitionParam { 14 | public: 15 | WeightedVoronoiPartitionParam(); 16 | 17 | bool LoadFromXML(const XmlRpc::XmlRpcValue& param); 18 | 19 | std::unordered_set agent_ids; 20 | 21 | std::vector heterogenities; 22 | 23 | std::vector weight_factor; 24 | }; 25 | } // namespace partition 26 | } // namespace sampling -------------------------------------------------------------------------------- /src/sampling_partition/launch/partition.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | test_map_file: "test_map.txt" 5 | agent_ids: ["jackal0", "jackal1"] 6 | AgentLocations: 7 | - x: 2.0 8 | y: 2.0 9 | - x: 10.0 10 | y: 7.0 11 | HeterogeneousProperty: 12 | - heterogenities: ["DISTANCE","TRAVERSABILITY"] 13 | weight_factor: [1.0, 100.0] 14 | - agent_id: "jackal0" 15 | heterogeneity_primitive: [1.0, 1.0] 16 | number_control_area: 1 17 | control_area_center_0: [0.0, 0.0] 18 | control_area_radius_0: 3.0 19 | - agent_id: "jackal1" 20 | heterogeneity_primitive: [1.0, 1.0] 21 | number_control_area: 0 22 | VisualizationProperty: 23 | - visualization_type: "LOCATION" 24 | name: "AgentLocation" 25 | offset: [0.0, 0.0] 26 | scale: [1.0, 1.0] 27 | - visualization_type: "PARTITION" 28 | name: "Partition" 29 | offset: [0.0, 0.0] 30 | scale: [1.0, 1.0] 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /src/sampling_partition/map/test_map.txt: -------------------------------------------------------------------------------- 1 | 0.000000,0.000000 2 | 0.000000,1.000000 3 | 0.000000,2.000000 4 | 0.000000,3.000000 5 | 0.000000,4.000000 6 | 0.000000,5.000000 7 | 0.000000,6.000000 8 | 0.000000,7.000000 9 | 0.000000,8.000000 10 | 0.000000,9.000000 11 | 0.000000,10.000000 12 | 1.000000,0.000000 13 | 1.000000,1.000000 14 | 1.000000,2.000000 15 | 1.000000,3.000000 16 | 1.000000,4.000000 17 | 1.000000,5.000000 18 | 1.000000,6.000000 19 | 1.000000,7.000000 20 | 1.000000,8.000000 21 | 1.000000,9.000000 22 | 1.000000,10.000000 23 | 2.000000,0.000000 24 | 2.000000,1.000000 25 | 2.000000,2.000000 26 | 2.000000,3.000000 27 | 2.000000,4.000000 28 | 2.000000,5.000000 29 | 2.000000,6.000000 30 | 2.000000,7.000000 31 | 2.000000,8.000000 32 | 2.000000,9.000000 33 | 2.000000,10.000000 34 | 3.000000,0.000000 35 | 3.000000,1.000000 36 | 3.000000,2.000000 37 | 3.000000,3.000000 38 | 3.000000,4.000000 39 | 3.000000,5.000000 40 | 3.000000,6.000000 41 | 3.000000,7.000000 42 | 3.000000,8.000000 43 | 3.000000,9.000000 44 | 3.000000,10.000000 45 | 4.000000,0.000000 46 | 4.000000,1.000000 47 | 4.000000,2.000000 48 | 4.000000,3.000000 49 | 4.000000,4.000000 50 | 4.000000,5.000000 51 | 4.000000,6.000000 52 | 4.000000,7.000000 53 | 4.000000,8.000000 54 | 4.000000,9.000000 55 | 4.000000,10.000000 56 | 5.000000,0.000000 57 | 5.000000,1.000000 58 | 5.000000,2.000000 59 | 5.000000,3.000000 60 | 5.000000,4.000000 61 | 5.000000,5.000000 62 | 5.000000,6.000000 63 | 5.000000,7.000000 64 | 5.000000,8.000000 65 | 5.000000,9.000000 66 | 5.000000,10.000000 67 | 6.000000,0.000000 68 | 6.000000,1.000000 69 | 6.000000,2.000000 70 | 6.000000,3.000000 71 | 6.000000,4.000000 72 | 6.000000,5.000000 73 | 6.000000,6.000000 74 | 6.000000,7.000000 75 | 6.000000,8.000000 76 | 6.000000,9.000000 77 | 6.000000,10.000000 78 | 7.000000,0.000000 79 | 7.000000,1.000000 80 | 7.000000,2.000000 81 | 7.000000,3.000000 82 | 7.000000,4.000000 83 | 7.000000,5.000000 84 | 7.000000,6.000000 85 | 7.000000,7.000000 86 | 7.000000,8.000000 87 | 7.000000,9.000000 88 | 7.000000,10.000000 89 | 8.000000,0.000000 90 | 8.000000,1.000000 91 | 8.000000,2.000000 92 | 8.000000,3.000000 93 | 8.000000,4.000000 94 | 8.000000,5.000000 95 | 8.000000,6.000000 96 | 8.000000,7.000000 97 | 8.000000,8.000000 98 | 8.000000,9.000000 99 | 8.000000,10.000000 100 | 9.000000,0.000000 101 | 9.000000,1.000000 102 | 9.000000,2.000000 103 | 9.000000,3.000000 104 | 9.000000,4.000000 105 | 9.000000,5.000000 106 | 9.000000,6.000000 107 | 9.000000,7.000000 108 | 9.000000,8.000000 109 | 9.000000,9.000000 110 | 9.000000,10.000000 111 | 10.000000,0.000000 112 | 10.000000,1.000000 113 | 10.000000,2.000000 114 | 10.000000,3.000000 115 | 10.000000,4.000000 116 | 10.000000,5.000000 117 | 10.000000,6.000000 118 | 10.000000,7.000000 119 | 10.000000,8.000000 120 | 10.000000,9.000000 121 | 10.000000,10.000000 122 | -------------------------------------------------------------------------------- /src/sampling_partition/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sampling_partition 4 | 0.0.0 5 | The sampling_partition package 6 | 7 | Yang Zhang 8 | Yunfei Shi 9 | Ning Wang 10 | Jianmin Zheng 11 | 12 | BSD 13 | 14 | catkin 15 | 16 | roscpp 17 | sampling_msgs 18 | sampling_utils 19 | sampling_visualization 20 | geometry_msgs 21 | roslib 22 | 23 | 24 | -------------------------------------------------------------------------------- /src/sampling_partition/script/generate_map.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import sys 3 | # import rospy 4 | import time 5 | import numpy as np 6 | # import rospkg 7 | import random 8 | 9 | MIN_X = 0 10 | MAX_X = 10 11 | MIN_Y = 0 12 | MAX_Y = 10 13 | RESOLUTION = 1 14 | 15 | class MapGenerator(object): 16 | def __init__(self, min_x= MIN_X, max_x = MAX_X, min_y = MIN_Y, max_y = MAX_Y, resolution = RESOLUTION,file_name="test_map"): 17 | self.min_x = min_x 18 | self.max_x = max_x 19 | self.min_y = min_y 20 | self.max_y = max_y 21 | self.resolution = resolution 22 | self.file_name = file_name 23 | 24 | def generate_map(self): 25 | data_dir = "../map/" + self.file_name + ".txt" 26 | map_file = open(data_dir, "w") 27 | for x in np.arange(self.min_x, self.max_x + self.resolution, self.resolution): 28 | for y in np.arange(self.min_y, self.max_y + self.resolution, self.resolution): 29 | map_file.write("%f,%f \n" %(x, y)) 30 | map_file.close() 31 | print("Map data generation!") 32 | 33 | if __name__ == "__main__": 34 | generator = MapGenerator() 35 | generator.generate_map() -------------------------------------------------------------------------------- /src/sampling_partition/src/heterogeneity.cpp: -------------------------------------------------------------------------------- 1 | #include "sampling_partition/heterogeneity.h" 2 | 3 | #include /* sqrt */ 4 | 5 | #include "sampling_partition/heterogeneity_distance.h" 6 | #include "sampling_partition/heterogeneity_distance_dependent.h" 7 | #include "sampling_partition/heterogeneity_topography_dependent.h" 8 | 9 | namespace sampling { 10 | namespace partition { 11 | 12 | Heterogeneity::Heterogeneity(const HeterogeneityParams ¶ms, 13 | const Eigen::MatrixXd &map) 14 | : params_(params), map_(map) {} 15 | 16 | std::string Heterogeneity::GetType() { return params_.heterogeneity_type; } 17 | 18 | } // namespace partition 19 | } // namespace sampling -------------------------------------------------------------------------------- /src/sampling_partition/src/heterogeneity_distance.cpp: -------------------------------------------------------------------------------- 1 | #include "sampling_partition/heterogeneity_distance.h" 2 | 3 | #include 4 | 5 | namespace sampling { 6 | namespace partition { 7 | 8 | Eigen::VectorXd HeterogeneityDistance::CalculateCost( 9 | const geometry_msgs::Point &agent_position, 10 | const Eigen::VectorXd &distance) { 11 | Eigen::VectorXd cost = distance.array() * KDistancePrimitive; 12 | cost = cost.array().tanh(); 13 | return cost; 14 | } 15 | 16 | HeterogeneityDistance::HeterogeneityDistance(const HeterogeneityParams ¶ms, 17 | const Eigen::MatrixXd &map) 18 | : Heterogeneity(params, map) {} 19 | 20 | } // namespace partition 21 | } // namespace sampling -------------------------------------------------------------------------------- /src/sampling_partition/src/heterogeneity_distance_dependent.cpp: -------------------------------------------------------------------------------- 1 | #include "sampling_partition/heterogeneity_distance_dependent.h" 2 | 3 | #include 4 | 5 | namespace sampling { 6 | namespace partition { 7 | 8 | Eigen::VectorXd HeterogeneityDistanceDepedent::CalculateCost( 9 | const geometry_msgs::Point &agent_position, 10 | const Eigen::VectorXd &distance) { 11 | Eigen::VectorXd cost = distance.array() * params_.heterogeneity_primitive; 12 | cost = cost.array().tanh(); 13 | if (params_.heterogeneity_primitive >= 0) { 14 | return cost; 15 | } else { 16 | return cost.array() + 1.0; 17 | } 18 | } 19 | 20 | HeterogeneityDistanceDepedent::HeterogeneityDistanceDepedent( 21 | const HeterogeneityParams ¶ms, const Eigen::MatrixXd &map) 22 | : Heterogeneity(params, map) {} 23 | 24 | } // namespace partition 25 | } // namespace sampling -------------------------------------------------------------------------------- /src/sampling_partition/src/heterogeneity_topography_dependent.cpp: -------------------------------------------------------------------------------- 1 | #include "sampling_partition/heterogeneity_topography_dependent.h" 2 | 3 | #include 4 | 5 | namespace sampling { 6 | namespace partition { 7 | 8 | Eigen::VectorXd HeterogeneityTopographyDepedent::CalculateCost( 9 | const geometry_msgs::Point &agent_position, 10 | const Eigen::VectorXd &distance) { 11 | return topography_cost_; 12 | } 13 | 14 | HeterogeneityTopographyDepedent::HeterogeneityTopographyDepedent( 15 | const HeterogeneityParams ¶ms, const Eigen::MatrixXd &map) 16 | : Heterogeneity(params, map), 17 | topography_cost_(Eigen::VectorXd::Zero(map.rows())) { 18 | for (int i = 0; i < params_.control_area_center.size(); ++i) { 19 | geometry_msgs::Point center = params_.control_area_center[i]; 20 | Eigen::MatrixXd distance_map(map_.rows(), map_.cols()); 21 | distance_map.col(0).array() = map_.col(0).array() - center.x; 22 | distance_map.col(1).array() = map_.col(1).array() - center.y; 23 | Eigen::VectorXd distance = distance_map.rowwise().norm(); 24 | for (int j = 0; j < distance.size(); ++j) { 25 | if (distance(j) <= params_.control_area_radius[i]) { 26 | topography_cost_(j) = params_.heterogeneity_primitive; 27 | } 28 | } 29 | } 30 | } 31 | 32 | } // namespace partition 33 | } // namespace sampling -------------------------------------------------------------------------------- /src/sampling_partition/src/weighted_voronoi_partition_params.cpp: -------------------------------------------------------------------------------- 1 | #include "sampling_partition/weighted_voronoi_partition_params.h" 2 | 3 | #include 4 | 5 | #include "sampling_partition/heterogeneity.h" 6 | #include "sampling_utils/utils.h" 7 | 8 | namespace sampling { 9 | namespace partition { 10 | 11 | WeightedVoronoiPartitionParam::WeightedVoronoiPartitionParam() {} 12 | 13 | bool WeightedVoronoiPartitionParam::LoadFromXML( 14 | const XmlRpc::XmlRpcValue& param) { 15 | if (!utils::GetParam(param, "heterogenities", heterogenities)) { 16 | ROS_ERROR_STREAM( 17 | "Error loading heterogeneities for heterogeneous property!"); 18 | return false; 19 | } 20 | 21 | if (!utils::GetParam(param, "weight_factor", weight_factor) || 22 | weight_factor.size() != heterogenities.size()) { 23 | ROS_ERROR_STREAM( 24 | "Error loading weight factors for heterogeneous property!"); 25 | return false; 26 | } 27 | return true; 28 | } 29 | 30 | } // namespace partition 31 | } // namespace sampling -------------------------------------------------------------------------------- /src/sampling_utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(sampling_utils) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | ) 7 | 8 | catkin_package( 9 | INCLUDE_DIRS include 10 | ) 11 | 12 | include_directories( 13 | include 14 | ${catkin_INCLUDE_DIRS} 15 | ) -------------------------------------------------------------------------------- /src/sampling_utils/include/sampling_utils/utils.h: -------------------------------------------------------------------------------- 1 | 2 | /** 3 | * Utility functions for sampling project 4 | * AUTHOR: Yang Zhang 5 | */ 6 | 7 | #pragma once 8 | 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | namespace sampling { 15 | namespace utils { 16 | 17 | template 18 | void VectorInfo(const std::vector &data); 19 | 20 | template 21 | bool ExtractVec(const T &full, const std::vector &ind, T &target); 22 | 23 | template 24 | bool ExtractCols(const T &full, const std::vector &ind, T &target); 25 | 26 | template 27 | bool ExtractRows(const T &full, const std::vector &ind, T &target); 28 | 29 | template 30 | std::vector Extract(const std::vector &full, const std::vector &ind); 31 | 32 | template 33 | bool GetParam(const XmlRpc::XmlRpcValue &YamlNode, 34 | const std::string ¶m_name, T &data); 35 | 36 | template 37 | bool GetParam(const XmlRpc::XmlRpcValue &YamlNode, 38 | const std::string ¶m_name, std::vector &data); 39 | 40 | } // namespace utils 41 | } // namespace sampling 42 | #include "sampling_utils/utils_impl.h" -------------------------------------------------------------------------------- /src/sampling_utils/include/sampling_utils/utils_impl.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "utils.h" 4 | 5 | namespace sampling { 6 | namespace utils { 7 | 8 | template 9 | void VectorInfo(const std::vector &data) { 10 | std::string info; 11 | for (const T &value : data) { 12 | info += std::to_string(value); 13 | info += " "; 14 | } 15 | ROS_INFO_STREAM(info); 16 | } 17 | 18 | template 19 | bool ExtractVec(const T &full, const std::vector &ind, T &target) { 20 | int num_indices = ind.size(); 21 | target = T(num_indices); 22 | for (int i = 0; i < num_indices; i++) { 23 | if (ind[i] >= full.size()) return false; 24 | target[i] = full[ind[i]]; 25 | } 26 | return true; 27 | } 28 | 29 | template 30 | bool ExtractCols(const T &full, const std::vector &ind, T &target) { 31 | target = T(full.rows(), ind.size()); 32 | for (int i = 0; i < ind.size(); ++i) { 33 | if (ind[i] >= full.cols()) return false; 34 | target.col(i) = full.col(ind[i]); 35 | } 36 | return true; 37 | } 38 | 39 | template 40 | bool ExtractRows(const T &full, const std::vector &ind, T &target) { 41 | target = T(ind.size(), full.cols()); 42 | for (int i = 0; i < ind.size(); ++i) { 43 | if (ind[i] >= full.rows()) return false; 44 | target.row(i) = full.row(ind[i]); 45 | } 46 | return true; 47 | } 48 | 49 | template 50 | std::vector Extract(const std::vector &full, 51 | const std::vector &ind) { 52 | std::vector target; 53 | target.reserve(ind.size()); 54 | for (const T &i : ind) { 55 | target.push_back(full[i]); 56 | } 57 | return target; 58 | } 59 | 60 | template 61 | bool GetParam(const XmlRpc::XmlRpcValue &YamlNode, 62 | const std::string ¶m_name, T &data) { 63 | if (!YamlNode.hasMember(param_name)) { 64 | ROS_WARN_STREAM("Missing parameter : " << param_name); 65 | return false; 66 | } else { 67 | data = static_cast(YamlNode[param_name]); 68 | } 69 | return true; 70 | } 71 | 72 | template 73 | bool GetParam(const XmlRpc::XmlRpcValue &YamlNode, 74 | const std::string ¶m_name, std::vector &data) { 75 | if (!YamlNode.hasMember(param_name)) { 76 | ROS_WARN_STREAM("Missing parameter : " << param_name); 77 | return false; 78 | } 79 | data.reserve(YamlNode[param_name].size()); 80 | for (int i = 0; i < YamlNode[param_name].size(); ++i) { 81 | data.push_back(static_cast(YamlNode[param_name][i])); 82 | } 83 | return true; 84 | } 85 | } // namespace utils 86 | } // namespace sampling 87 | -------------------------------------------------------------------------------- /src/sampling_utils/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sampling_utils 4 | 0.0.0 5 | The sampling_utils package 6 | 7 | Yang Zhang 8 | Yunfei Shi 9 | Ning Wang 10 | Jianmin Zheng 11 | 12 | BSD 13 | 14 | catkin 15 | roscpp 16 | 17 | 18 | -------------------------------------------------------------------------------- /src/sampling_visualization/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(sampling_visualization) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | geometry_msgs 6 | roscpp 7 | sampling_msgs 8 | std_msgs 9 | visualization_msgs 10 | sampling_utils 11 | ) 12 | 13 | find_package(Eigen3 REQUIRED) 14 | 15 | catkin_package( 16 | INCLUDE_DIRS include 17 | LIBRARIES sampling_visualization 18 | DEPENDS EIGEN3 19 | ) 20 | 21 | include_directories( 22 | include 23 | ${catkin_INCLUDE_DIRS} 24 | ${EIGEN3_INCLUDE_DIR} 25 | ) 26 | 27 | add_library(${PROJECT_NAME} 28 | src/sampling_visualization_params.cpp 29 | src/grid_visualization_handler.cpp 30 | src/agent_visualization_handler 31 | src/sampling_visualization_utils.cpp 32 | ) 33 | 34 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 35 | 36 | install(TARGETS ${PROJECT_NAME} 37 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 38 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 39 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 40 | ) 41 | 42 | install(DIRECTORY include/${PROJECT_NAME}/ 43 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 44 | FILES_MATCHING PATTERN "*.h" 45 | PATTERN ".svn" EXCLUDE 46 | ) -------------------------------------------------------------------------------- /src/sampling_visualization/include/sampling_visualization/agent_visualization_handler.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include "sampling_msgs/AgentLocation.h" 10 | #include "sampling_visualization/sampling_visualization_params.h" 11 | 12 | namespace sampling { 13 | namespace visualization { 14 | 15 | const double KAgentVisualizationScale = 1.0; 16 | const double KAgentVisualizationHeight = 1.25; 17 | const double KVisualizationAgentUpdateRate_hz = 10; 18 | 19 | class AgentVisualizationHandler { 20 | public: 21 | AgentVisualizationHandler() = delete; 22 | 23 | static std::unique_ptr MakeUniqueFromXML( 24 | ros::NodeHandle &nh, const XmlRpc::XmlRpcValue &yaml_node, 25 | const int &number_of_agents, const Eigen::MatrixXd &map); 26 | 27 | bool UpdateMarker( 28 | const std::vector &agent_locations); 29 | 30 | std::string GetName(); 31 | 32 | private: 33 | AgentVisualizationHandler(ros::NodeHandle &nh, 34 | const visualization_msgs::Marker &marker, 35 | const double &map_center_x, 36 | const double &map_center_y, 37 | const SamplingVisualizationParams ¶ms); 38 | 39 | SamplingVisualizationParams params_; 40 | 41 | ros::Timer event_timer_; 42 | 43 | ros::Publisher Agent_publisher_; 44 | 45 | visualization_msgs::Marker marker_; 46 | 47 | double map_center_x_; 48 | 49 | double map_center_y_; 50 | 51 | void UpdateVisualizationCallback(const ros::TimerEvent &); 52 | }; 53 | } // namespace visualization 54 | } // namespace sampling -------------------------------------------------------------------------------- /src/sampling_visualization/include/sampling_visualization/grid_visualization_handler.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include "sampling_visualization/sampling_visualization_params.h" 9 | #include "sampling_visualization/sampling_visualization_utils.h" 10 | 11 | namespace sampling { 12 | namespace visualization { 13 | 14 | class GridVisualizationHandler { 15 | public: 16 | GridVisualizationHandler() = delete; 17 | 18 | static std::unique_ptr MakeUniqueFromXML( 19 | ros::NodeHandle &nh, const XmlRpc::XmlRpcValue &yaml_node, 20 | const Eigen::MatrixXd &map); 21 | 22 | bool UpdateMarker(const std::vector &marker_value); 23 | 24 | bool UpdateMarker(const std::vector &marker_value); 25 | 26 | std::string GetName(); 27 | 28 | private: 29 | GridVisualizationHandler(ros::NodeHandle &nh, 30 | const visualization_msgs::Marker &marker, 31 | const SamplingVisualizationParams ¶ms); 32 | 33 | SamplingVisualizationParams params_; 34 | 35 | SamplingVisualizationUtils utils_; 36 | 37 | ros::Timer event_timer_; 38 | 39 | ros::Publisher grid_publisher_; 40 | 41 | visualization_msgs::Marker marker_; 42 | 43 | void UpdateVisualizationCallback(const ros::TimerEvent &); 44 | }; 45 | } // namespace visualization 46 | } // namespace sampling -------------------------------------------------------------------------------- /src/sampling_visualization/include/sampling_visualization/sampling_visualization_params.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | namespace sampling { 11 | namespace visualization { 12 | 13 | const std::string KVisualizationType_Grid = "GRID"; 14 | const std::string KVisualizationType_Location = "LOCATION"; 15 | const std::string KVisualizationType_Partition = "PARTITION"; 16 | 17 | const std::string KAgentLocationMapName = "AgentLocation"; 18 | const std::string KPartitionMapName = "Partition"; 19 | const std::string KPredictionMeanMapName = "PredictionMean"; 20 | const std::string KPredictionVarianceMapName = "PredictionVariance"; 21 | 22 | const std::string KVisualizationNamespace = "/sampling_visualization/"; 23 | const std::string KVisualizationFrame = "sampling_visualization"; 24 | 25 | const double KVisualizationUpdateRate_hz = 1.0; 26 | 27 | const size_t KVisualizationDimension = 2; 28 | const double KVisualizationUpperBound = 5.0; 29 | const double KVisualizationLowerBound = -5.0; 30 | class SamplingVisualizationParams { 31 | public: 32 | SamplingVisualizationParams(); 33 | 34 | bool LoadFromXML(const XmlRpc::XmlRpcValue &yaml_node); 35 | 36 | std::string name; 37 | 38 | std::string visualization_type; 39 | 40 | std::vector offset; 41 | 42 | std::vector scale; 43 | 44 | // lower bound and upper bound 45 | std::vector bounds; 46 | }; 47 | 48 | } // namespace visualization 49 | } // namespace sampling -------------------------------------------------------------------------------- /src/sampling_visualization/include/sampling_visualization/sampling_visualization_utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | namespace sampling { 10 | namespace visualization { 11 | 12 | const double KHSVRange = 100.0; 13 | const double KPixelScale = 255.0; 14 | 15 | const int KRGBRedId = 0; 16 | const std::vector KRGBRed{255.0 / KPixelScale, 0.0 / KPixelScale, 17 | 0.0 / KPixelScale}; 18 | 19 | const int KRGBGreenId = 1; 20 | const std::vector KRGBGreen{0.0 / KPixelScale, 255.0 / KPixelScale, 21 | 0 / KPixelScale}; 22 | 23 | const int KRGBBlueId = 2; 24 | const std::vector KRGBBlue{0.0 / KPixelScale, 0 / KPixelScale, 25 | 255 / KPixelScale}; 26 | 27 | const int KRGBYellowId = 3; 28 | const std::vector KRGBYellow{255.0 / KPixelScale, 255.0 / KPixelScale, 29 | 0.0 / KPixelScale}; 30 | 31 | const int KRGBGreyId = 4; 32 | const std::vector KRGBGrey{128.0 / KPixelScale, 128.0 / KPixelScale, 33 | 128.0 / KPixelScale}; 34 | 35 | const int KRGBPinkId = 5; 36 | const std::vector KRGBPink{255.0 / KPixelScale, 102.0 / KPixelScale, 37 | 255.0 / KPixelScale}; 38 | 39 | class SamplingVisualizationUtils { 40 | public: 41 | SamplingVisualizationUtils(); 42 | 43 | void HSVtoRGB(const double &H, const double &S, const double &V, double &R, 44 | double &G, double &B); 45 | 46 | bool UpdateColor(const double &norm, std_msgs::ColorRGBA &color); 47 | 48 | bool UpdateColor(const int &agent_id, std_msgs::ColorRGBA &color); 49 | 50 | private: 51 | std::unordered_map> color_map_; 52 | }; 53 | 54 | } // namespace visualization 55 | } // namespace sampling 56 | -------------------------------------------------------------------------------- /src/sampling_visualization/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sampling_visualization 4 | 0.0.0 5 | The sampling_visualization package 6 | 7 | Yang Zhang 8 | Yunfei Shi 9 | Ning Wang 10 | Jianmin Zheng 11 | 12 | BSD 13 | 14 | catkin 15 | geometry_msgs 16 | roscpp 17 | sampling_msgs 18 | std_msgs 19 | visualization_msgs 20 | sampling_utils 21 | 22 | 23 | -------------------------------------------------------------------------------- /src/sampling_visualization/src/agent_visualization_handler.cpp: -------------------------------------------------------------------------------- 1 | #include "sampling_visualization/agent_visualization_handler.h" 2 | 3 | #include "sampling_visualization/sampling_visualization_utils.h" 4 | 5 | namespace sampling { 6 | namespace visualization { 7 | 8 | std::unique_ptr 9 | AgentVisualizationHandler::MakeUniqueFromXML( 10 | ros::NodeHandle &nh, const XmlRpc::XmlRpcValue &yaml_node, 11 | const int &number_of_agents, const Eigen::MatrixXd &map) { 12 | SamplingVisualizationParams params; 13 | if (!params.LoadFromXML(yaml_node)) { 14 | ROS_ERROR_STREAM("Failed to load parameters for visualization !"); 15 | return nullptr; 16 | } 17 | const int map_size = map.rows(); 18 | double map_center_x = map.col(0).mean(); 19 | double map_center_y = map.col(1).mean(); 20 | 21 | visualization_msgs::Marker marker; 22 | marker.header.frame_id = KVisualizationFrame; 23 | marker.header.stamp = ros::Time::now(); 24 | marker.ns = KVisualizationNamespace; 25 | marker.pose.orientation.w = 1.0; 26 | marker.action = visualization_msgs::Marker::ADD; 27 | marker.id = 0; 28 | marker.type = visualization_msgs::Marker::SPHERE_LIST; 29 | marker.scale.x = KAgentVisualizationScale; 30 | marker.scale.y = KAgentVisualizationScale; 31 | marker.scale.z = 1.0; 32 | marker.color.a = 1.0; 33 | 34 | SamplingVisualizationUtils utils; 35 | 36 | marker.points.resize(number_of_agents); 37 | marker.colors.resize(number_of_agents); 38 | for (int i = 0; i < number_of_agents; ++i) { 39 | geometry_msgs::Point waypoint; 40 | waypoint.x = map_center_x + params.offset[0]; 41 | waypoint.y = map_center_y + params.offset[1]; 42 | waypoint.z = KAgentVisualizationHeight; 43 | std_msgs::ColorRGBA color; 44 | if (!utils.UpdateColor(i, color)) { 45 | ROS_ERROR_STREAM("Failed to assign color for agent visualization!"); 46 | return nullptr; 47 | } 48 | marker.points[i] = waypoint; 49 | marker.colors[i] = color; 50 | } 51 | return std::unique_ptr( 52 | new AgentVisualizationHandler(nh, marker, map_center_x, map_center_y, 53 | params)); 54 | } 55 | 56 | AgentVisualizationHandler::AgentVisualizationHandler( 57 | ros::NodeHandle &nh, const visualization_msgs::Marker &marker, 58 | const double &map_center_x, const double &map_center_y, 59 | const SamplingVisualizationParams ¶ms) 60 | : marker_(marker), 61 | map_center_x_(map_center_x), 62 | map_center_y_(map_center_y), 63 | params_(params) { 64 | Agent_publisher_ = nh.advertise( 65 | KVisualizationNamespace + params.name, 1); 66 | 67 | event_timer_ = nh.createTimer( 68 | ros::Duration(1.0 / KVisualizationAgentUpdateRate_hz), 69 | &AgentVisualizationHandler::UpdateVisualizationCallback, this); 70 | } 71 | 72 | bool AgentVisualizationHandler::UpdateMarker( 73 | const std::vector &agent_locations) { 74 | if (agent_locations.size() != marker_.points.size()) { 75 | ROS_ERROR_STREAM("Number of agents for visualization does NOT match!"); 76 | return false; 77 | } 78 | for (int i = 0; i < agent_locations.size(); ++i) { 79 | marker_.points[i].x = 80 | (agent_locations[i].position.x - map_center_x_) * params_.scale[0] + 81 | params_.offset[0]; 82 | marker_.points[i].y = 83 | (agent_locations[i].position.y - map_center_y_) * params_.scale[1] + 84 | params_.offset[1]; 85 | } 86 | return true; 87 | } 88 | 89 | void AgentVisualizationHandler::UpdateVisualizationCallback( 90 | const ros::TimerEvent &) { 91 | Agent_publisher_.publish(marker_); 92 | } 93 | 94 | std::string AgentVisualizationHandler::GetName() { return params_.name; } 95 | } // namespace visualization 96 | } // namespace sampling -------------------------------------------------------------------------------- /src/sampling_visualization/src/grid_visualization_handler.cpp: -------------------------------------------------------------------------------- 1 | #include "sampling_visualization/grid_visualization_handler.h" 2 | 3 | namespace sampling { 4 | namespace visualization { 5 | 6 | std::unique_ptr 7 | GridVisualizationHandler::MakeUniqueFromXML( 8 | ros::NodeHandle &nh, const XmlRpc::XmlRpcValue &yaml_node, 9 | const Eigen::MatrixXd &map) { 10 | SamplingVisualizationParams params; 11 | if (!params.LoadFromXML(yaml_node)) { 12 | ROS_ERROR_STREAM("Failed to load parameters for visualization !"); 13 | return nullptr; 14 | } 15 | const int map_size = map.rows(); 16 | double map_center_x = map.col(0).mean(); 17 | double map_center_y = map.col(1).mean(); 18 | 19 | visualization_msgs::Marker marker; 20 | marker.header.frame_id = KVisualizationFrame; 21 | marker.header.stamp = ros::Time::now(); 22 | marker.ns = KVisualizationNamespace; 23 | marker.pose.orientation.w = 1.0; 24 | marker.action = visualization_msgs::Marker::ADD; 25 | marker.id = 0; 26 | marker.type = visualization_msgs::Marker::CUBE_LIST; 27 | marker.scale.x = 1.0; 28 | marker.scale.y = 1.0; 29 | marker.scale.z = 1.0; 30 | marker.points.resize(map_size); 31 | marker.colors.resize(map_size); 32 | std_msgs::ColorRGBA default_color = std_msgs::ColorRGBA(); 33 | default_color.r = 1.0; 34 | default_color.g = 1.0; 35 | default_color.b = 1.0; 36 | default_color.a = 1.0; 37 | for (int i = 0; i < map_size; ++i) { 38 | geometry_msgs::Point waypoint; 39 | waypoint.x = 40 | (map(i, 0) - map_center_x) * params.scale[0] + params.offset[0]; 41 | waypoint.y = 42 | (map(i, 1) - map_center_y) * params.scale[1] + params.offset[1]; 43 | waypoint.z = 0.0; 44 | marker.points[i] = waypoint; 45 | marker.colors[i] = default_color; 46 | } 47 | return std::unique_ptr( 48 | new GridVisualizationHandler(nh, marker, params)); 49 | } 50 | 51 | GridVisualizationHandler::GridVisualizationHandler( 52 | ros::NodeHandle &nh, const visualization_msgs::Marker &marker, 53 | const SamplingVisualizationParams ¶ms) 54 | : marker_(marker), params_(params) { 55 | grid_publisher_ = nh.advertise( 56 | KVisualizationNamespace + params.name, 1); 57 | 58 | event_timer_ = nh.createTimer( 59 | ros::Duration(1.0 / KVisualizationUpdateRate_hz), 60 | &GridVisualizationHandler::UpdateVisualizationCallback, this); 61 | } 62 | 63 | bool GridVisualizationHandler::UpdateMarker( 64 | const std::vector &marker_value) { 65 | if (KVisualizationType_Grid.compare(params_.visualization_type) != 0) { 66 | ROS_ERROR_STREAM("Wrong data type for visualization update! ?????????? " 67 | << params_.visualization_type); 68 | return false; 69 | } else if (marker_value.size() != marker_.points.size()) { 70 | ROS_ERROR_STREAM("Visualization data size does not match!"); 71 | return false; 72 | } 73 | for (const double &value : marker_value) { 74 | params_.bounds[0] = std::min(params_.bounds[0], value); 75 | params_.bounds[1] = std::max(params_.bounds[1], value); 76 | } 77 | const double value_range = params_.bounds[1] - params_.bounds[0]; 78 | for (int i = 0; i < marker_.points.size(); ++i) { 79 | double norm_value = (marker_value[i] - params_.bounds[0]) / value_range; 80 | if (!utils_.UpdateColor(norm_value, marker_.colors[i])) { 81 | ROS_ERROR_STREAM("Invalid visualization data point!" << norm_value); 82 | return false; 83 | } 84 | } 85 | return true; 86 | } 87 | 88 | bool GridVisualizationHandler::UpdateMarker( 89 | const std::vector &marker_value) { 90 | if (KVisualizationType_Partition.compare(params_.visualization_type) != 0) { 91 | ROS_ERROR_STREAM("Wrong data type for visualization update!" 92 | << params_.visualization_type); 93 | return false; 94 | } else if (marker_value.size() != marker_.points.size()) { 95 | ROS_ERROR_STREAM("Visualization data size does not match!"); 96 | return false; 97 | } 98 | for (int i = 0; i < marker_.points.size(); ++i) { 99 | if (!utils_.UpdateColor(marker_value[i], marker_.colors[i])) { 100 | ROS_ERROR_STREAM("Invalid visualization data point for partition!"); 101 | return false; 102 | } 103 | } 104 | return true; 105 | } 106 | 107 | void GridVisualizationHandler::UpdateVisualizationCallback( 108 | const ros::TimerEvent &) { 109 | grid_publisher_.publish(marker_); 110 | } 111 | 112 | std::string GridVisualizationHandler::GetName() { return params_.name; } 113 | 114 | } // namespace visualization 115 | } // namespace sampling -------------------------------------------------------------------------------- /src/sampling_visualization/src/sampling_visualization_params.cpp: -------------------------------------------------------------------------------- 1 | #include "sampling_visualization/sampling_visualization_params.h" 2 | 3 | #include "sampling_utils/utils.h" 4 | 5 | namespace sampling { 6 | namespace visualization { 7 | 8 | SamplingVisualizationParams::SamplingVisualizationParams() {} 9 | 10 | bool SamplingVisualizationParams::LoadFromXML( 11 | const XmlRpc::XmlRpcValue &yaml_node) { 12 | if (!utils::GetParam(yaml_node, "name", name)) { 13 | ROS_ERROR_STREAM("Error loading name for sampling visualization!"); 14 | return false; 15 | } 16 | 17 | if (!utils::GetParam(yaml_node, "visualization_type", visualization_type)) { 18 | ROS_ERROR_STREAM( 19 | "Error loading visualization type for sampling visualization!"); 20 | return false; 21 | } else if (KVisualizationType_Grid.compare(visualization_type) != 0 && 22 | KVisualizationType_Location.compare(visualization_type) != 0 && 23 | KVisualizationType_Partition.compare(visualization_type) != 0) { 24 | ROS_ERROR_STREAM("Unknow visualization type!"); 25 | return false; 26 | } 27 | if (!utils::GetParam(yaml_node, "offset", offset) || 28 | offset.size() != KVisualizationDimension) { 29 | ROS_ERROR_STREAM("Error loading offset for sampling visualization!"); 30 | return false; 31 | } 32 | 33 | if (!utils::GetParam(yaml_node, "scale", scale) || 34 | scale.size() != KVisualizationDimension) { 35 | ROS_ERROR_STREAM("Error loading scale for sampling visualization!"); 36 | return false; 37 | } 38 | 39 | if (!utils::GetParam(yaml_node, "bounds", bounds) || 40 | bounds.size() != KVisualizationDimension) { 41 | ROS_WARN_STREAM("Error loading bounds for sampling visualization " << name 42 | << " !"); 43 | bounds = 44 | std::vector{KVisualizationLowerBound, KVisualizationUpperBound}; 45 | } 46 | 47 | return true; 48 | } 49 | 50 | } // namespace visualization 51 | } // namespace sampling -------------------------------------------------------------------------------- /src/sampling_visualization/src/sampling_visualization_utils.cpp: -------------------------------------------------------------------------------- 1 | #include "sampling_visualization/sampling_visualization_utils.h" 2 | 3 | #include /* Mod */ 4 | #include 5 | #include 6 | 7 | namespace sampling { 8 | namespace visualization { 9 | 10 | SamplingVisualizationUtils::SamplingVisualizationUtils() { 11 | color_map_ = std::unordered_map>{ 12 | {KRGBRedId, KRGBRed}, {KRGBGreenId, KRGBGreen}, 13 | {KRGBBlueId, KRGBBlue}, {KRGBYellowId, KRGBYellow}, 14 | {KRGBGreyId, KRGBGrey}, {KRGBPinkId, KRGBPink}}; 15 | } 16 | 17 | void SamplingVisualizationUtils::HSVtoRGB(const double &H, const double &S, 18 | const double &V, double &R, double &G, 19 | double &B) { 20 | float C = V * S; // Chroma 21 | float HPrime = fmod(H / 60.0, 6); 22 | float X = C * (1 - fabs(fmod(HPrime, 2) - 1)); 23 | float M = V - C; 24 | 25 | if (0 <= HPrime && HPrime < 1) { 26 | R = C; 27 | G = X; 28 | B = 0; 29 | } else if (1 <= HPrime && HPrime < 2) { 30 | R = X; 31 | G = C; 32 | B = 0; 33 | } else if (2 <= HPrime && HPrime < 3) { 34 | R = 0; 35 | G = C; 36 | B = X; 37 | } else if (3 <= HPrime && HPrime < 4) { 38 | R = 0; 39 | G = X; 40 | B = C; 41 | } else if (4 <= HPrime && HPrime < 5) { 42 | R = X; 43 | G = 0; 44 | B = C; 45 | } else if (5 <= HPrime && HPrime < 6) { 46 | R = C; 47 | G = 0; 48 | B = X; 49 | } else { 50 | R = 0; 51 | G = 0; 52 | B = 0; 53 | } 54 | R += M; 55 | G += M; 56 | B += M; 57 | } 58 | 59 | bool SamplingVisualizationUtils::UpdateColor(const double &norm, 60 | std_msgs::ColorRGBA &color) { 61 | if (norm < 0 || norm > 1) return false; 62 | double r, g, b; 63 | HSVtoRGB((1 - norm) * KHSVRange, 1.0, 1.0, r, g, b); 64 | color.r = r; 65 | color.g = g; 66 | color.b = b; 67 | color.a = 1.0; 68 | return true; 69 | } 70 | 71 | bool SamplingVisualizationUtils::UpdateColor(const int &agent_id, 72 | std_msgs::ColorRGBA &color) { 73 | if (agent_id < 0) 74 | return false; 75 | else if (!color_map_.count(agent_id)) { 76 | srand(agent_id); 77 | double r = fmod((double)rand(), KPixelScale); 78 | double g = fmod((double)rand(), KPixelScale); 79 | double b = fmod((double)rand(), KPixelScale); 80 | color_map_[agent_id] = std::vector{r, g, b}; 81 | } 82 | color.r = color_map_[agent_id][0]; 83 | color.g = color_map_[agent_id][1]; 84 | color.b = color_map_[agent_id][2]; 85 | color.a = 1.0; 86 | return true; 87 | } 88 | } // namespace visualization 89 | } // namespace sampling 90 | --------------------------------------------------------------------------------