├── .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 | [](http://wiki.ros.org/melodic)
2 | [](./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