├── max_min_lp_simulation ├── data │ ├── greedy │ │ ├── robots_0.txt │ │ ├── robots_1.txt │ │ ├── robots_2.txt │ │ ├── robots_3.txt │ │ ├── robots_4.txt │ │ ├── robots_5.txt │ │ ├── results.txt │ │ └── results_for_debug.txt │ └── local │ │ ├── robots_2.txt │ │ ├── robots_4.txt │ │ ├── results.txt │ │ ├── robots_1.txt │ │ ├── robots_3.txt │ │ ├── robots_5.txt │ │ └── targets.txt ├── srv │ ├── GetTotalNumTarget.srv │ ├── GetOdom.srv │ ├── MotionPrimitiveRequest.srv │ ├── MoveRobot.srv │ └── MessageRequest.srv ├── urdf │ ├── box.urdf.xacro │ ├── box_library.urdf.xacro │ └── kobuki.urdf.xacro ├── package.xml ├── include │ └── max_min_lp_simulation │ │ ├── get_odom.hpp │ │ ├── apply_motion_primitive.hpp │ │ ├── apply_motion_primitive_journal.hpp │ │ ├── kalman.hpp │ │ ├── RobotGreedyJournal.hpp │ │ ├── MaxMinLPRobotNode.hpp │ │ ├── MaxMinLPGreedyRobotNode.hpp │ │ ├── MaxMinLPGreedyCentralNode.hpp │ │ ├── MaxMinLPRobotNodeSimulation.hpp │ │ ├── MaxMinLPCentralNode.hpp │ │ └── MaxMinLPCentralNodeSimulation.hpp ├── launch │ ├── single_robot_case.launch │ ├── journal.launch │ ├── includes │ │ ├── moving_target.launch.xml │ │ └── kobuki.launch.xml │ ├── greedy_robots_journal.launch │ ├── three_robot_four_target_case_failure_case.launch │ ├── two_robot_three_target_case.launch │ ├── test_motion_primitive.launch │ ├── three_robot_four_target_case.launch │ ├── motion_primitive_test.launch │ └── server_to_robots.launch ├── src │ ├── kalman.cpp │ ├── get_odom.cpp │ ├── get_result_target_num.cpp │ ├── apply_motion_primitive_journal.cpp │ ├── get_target_odom_journal.cpp │ ├── animated_box_journal.cc │ └── apply_motion_primitive.cpp ├── gazebo_model │ └── world │ │ └── target_spawn_journal.world └── CMakeLists.txt ├── max_min_lp_experiment ├── README.md ├── srv │ ├── TargetRequest.srv │ └── RobotRequest.srv ├── yaml │ └── target_model.yaml ├── launch │ └── greedy_central.launch ├── package.xml ├── CMakeLists.txt ├── include │ └── max_min_lp_experiment │ │ ├── TargetNode.hpp │ │ ├── GreedyRobotNode.hpp │ │ └── GreedyCentralNode.hpp └── src │ ├── TargetNode.cpp │ ├── GreedyRobotNode.cpp │ └── GreedyCentralNode.cpp ├── .gitignore ├── CMakeLists.txt ├── max_min_lp_msgs ├── msg │ ├── general_node_array.msg │ ├── layered_node_array.msg │ ├── server_to_robots_array.msg │ ├── primitive_node.msg │ ├── target_node.msg │ ├── general_node.msg │ ├── server_to_robots.msg │ └── layered_node.msg ├── package.xml └── CMakeLists.txt ├── max_min_lp ├── CMakeLists.txt └── package.xml ├── max_min_lp_visualization ├── log │ ├── general_graph.png │ ├── layered_graph.png │ └── general_graph.dot ├── package.xml └── CMakeLists.txt ├── max_min_lp_core ├── package.xml ├── CMakeLists.txt └── include │ └── max_min_lp_core │ ├── MaxMinLPCore.hpp │ ├── MaxMinLPSequentialCore.hpp │ └── MaxMinLPDecentralizedCore.hpp ├── max_min_lp_demo ├── package.xml ├── yaml │ ├── test_example_2.yaml │ ├── test_example.yaml │ ├── test_paper_example.yaml │ ├── test_example_num_targets.yaml │ └── test_example_3.yaml ├── include │ └── max_min_lp_demo │ │ └── MaxMinLPDemo.hpp └── CMakeLists.txt └── README.md /max_min_lp_simulation/data/greedy/robots_0.txt: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /max_min_lp_simulation/data/greedy/robots_1.txt: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /max_min_lp_simulation/data/greedy/robots_2.txt: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /max_min_lp_simulation/data/greedy/robots_3.txt: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /max_min_lp_simulation/data/greedy/robots_4.txt: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /max_min_lp_simulation/data/greedy/robots_5.txt: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /max_min_lp_simulation/data/greedy/results.txt: -------------------------------------------------------------------------------- 1 | 0 2 | -------------------------------------------------------------------------------- /max_min_lp_simulation/data/greedy/results_for_debug.txt: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /max_min_lp_experiment/README.md: -------------------------------------------------------------------------------- 1 | This package is not available. -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | max_min_lp_demo/output/ 2 | max_min_lp_demo/data/ 3 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/kinetic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /max_min_lp_simulation/data/local/robots_2.txt: -------------------------------------------------------------------------------- 1 | 2 5 2 | 2 5 3 | 2 5 4 | -------------------------------------------------------------------------------- /max_min_lp_simulation/data/local/robots_4.txt: -------------------------------------------------------------------------------- 1 | 4 -1 2 | 4 -1 3 | 4 -1 4 | -------------------------------------------------------------------------------- /max_min_lp_simulation/data/local/results.txt: -------------------------------------------------------------------------------- 1 | 3.610000000 23 2 | 6.501000000 24 3 | -------------------------------------------------------------------------------- /max_min_lp_simulation/data/local/robots_1.txt: -------------------------------------------------------------------------------- 1 | -1 4 2 | -1.9 3.8 3 | -1.9 3.8 4 | -------------------------------------------------------------------------------- /max_min_lp_msgs/msg/general_node_array.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | general_node[] gen_nodes -------------------------------------------------------------------------------- /max_min_lp_msgs/msg/layered_node_array.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | layered_node[] lay_nodes -------------------------------------------------------------------------------- /max_min_lp_simulation/data/local/robots_3.txt: -------------------------------------------------------------------------------- 1 | 0 0 2 | 0 -2.8e-06 3 | 0.33 -0.52 4 | -------------------------------------------------------------------------------- /max_min_lp_simulation/data/local/robots_5.txt: -------------------------------------------------------------------------------- 1 | -4 -2 2 | -3.1 -2.3 3 | -3.1 -2.3 4 | -------------------------------------------------------------------------------- /max_min_lp_simulation/srv/GetTotalNumTarget.srv: -------------------------------------------------------------------------------- 1 | string request_odom 2 | 3 | --- 4 | 5 | int8 total_num_target -------------------------------------------------------------------------------- /max_min_lp_msgs/msg/server_to_robots_array.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | server_to_robots[] each_robot 3 | string[] robot_status -------------------------------------------------------------------------------- /max_min_lp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(max_min_lp) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() -------------------------------------------------------------------------------- /max_min_lp_simulation/srv/GetOdom.srv: -------------------------------------------------------------------------------- 1 | string request_odom 2 | 3 | --- 4 | 5 | geometry_msgs/Pose return_odom 6 | geometry_msgs/Pose[] return_target_odom -------------------------------------------------------------------------------- /max_min_lp_visualization/log/general_graph.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/raaslab/multi-robot_task_allocation/HEAD/max_min_lp_visualization/log/general_graph.png -------------------------------------------------------------------------------- /max_min_lp_visualization/log/layered_graph.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/raaslab/multi-robot_task_allocation/HEAD/max_min_lp_visualization/log/layered_graph.png -------------------------------------------------------------------------------- /max_min_lp_msgs/msg/primitive_node.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | int16[] neighbor_primitive_id 3 | float64[] neighbor_p_x_pos 4 | float64[] neighbor_p_y_pos 5 | float64[] neighbor_p_t_weight -------------------------------------------------------------------------------- /max_min_lp_simulation/srv/MotionPrimitiveRequest.srv: -------------------------------------------------------------------------------- 1 | int8 request_ROBOT_id 2 | int8 count_motion_primitive 3 | 4 | --- 5 | 6 | int8 return_count_motion_primitive 7 | int8 selected_primitive_id -------------------------------------------------------------------------------- /max_min_lp_experiment/srv/TargetRequest.srv: -------------------------------------------------------------------------------- 1 | string state_request 2 | 3 | --- 4 | 5 | string state_answer 6 | 7 | # Target info 8 | int8[] target_id 9 | geometry_msgs/Point[] target_pose 10 | -------------------------------------------------------------------------------- /max_min_lp_msgs/msg/target_node.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | int16[] target_id 3 | float64[] t_x_pos 4 | float64[] t_y_pos 5 | float64[] t_p_weight 6 | bool[] neighbor_primitive_exist 7 | 8 | primitive_node[] connect_primitive -------------------------------------------------------------------------------- /max_min_lp_msgs/msg/general_node.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | string type 3 | int16 id 4 | int16 loc_deg 5 | int16[] loc_neighbor 6 | float64[] loc_edge_weight 7 | float64 x_pos 8 | float64 y_pos 9 | float32 z_v # Used in the phase 4 -------------------------------------------------------------------------------- /max_min_lp_experiment/yaml/target_model.yaml: -------------------------------------------------------------------------------- 1 | num_of_targets: 1 2 | 3 | target_id: [0] # ID starts with 0. 4 | 5 | pose_x: [2] # In meters. 6 | pose_y: [0] 7 | 8 | velocity: [0.1] # In meters/second. 9 | orientation: [30] # In degree. -------------------------------------------------------------------------------- /max_min_lp_simulation/srv/MoveRobot.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/Pose goal_pos 2 | int8 rotation_direction 3 | int8 trans_duration 4 | int8 ang_duration 5 | float64 trans_speed 6 | float64 ang_speed 7 | 8 | --- 9 | 10 | string answer_msg -------------------------------------------------------------------------------- /max_min_lp_msgs/msg/server_to_robots.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | int16 robot_id 3 | float64 r_x_pos 4 | float64 r_y_pos 5 | 6 | int16[] primitive_id 7 | float64[] p_x_pos 8 | float64[] p_y_pos 9 | float64[] p_r_weight 10 | bool[] target_exist 11 | 12 | target_node[] connect_target -------------------------------------------------------------------------------- /max_min_lp_experiment/srv/RobotRequest.srv: -------------------------------------------------------------------------------- 1 | string state_request 2 | int8 robot_id 3 | geometry_msgs/Point robot_pose 4 | 5 | --- 6 | 7 | string state_answer 8 | 9 | # Target info 10 | int8[] target_id 11 | geometry_msgs/Point[] target_pose 12 | 13 | # Communicative last robot info 14 | int8 comm_robot_id -------------------------------------------------------------------------------- /max_min_lp_simulation/data/local/targets.txt: -------------------------------------------------------------------------------- 1 | -3 6 5 5 4 -3 6 1 -3 1 -1 -3 0 2 1 -2 -5 -3 1 7 -4 -4 -1 -4 -1 -1 6 -2 4 2 2 3 0 8.1 2 -3 -5 3 1 -5 3 6 -4 0 1 0 -1 2 -4 5 6 0 -5 1 -1 6 1 -3 8 1 2 | -3 6 5 5 4 -3 6 1 -3 1 -1 -3 0 2 1 -2 -5.1 -3.1 1 7 -4 -4 -1 -4 -1 -1 6.1 -1.9 4 2 2 3 0 8.1 2 -3 -5 3 1 -5 3 6 -4 0 1 0 -1.1 2 -4 5 6 0 -5 1 -1 6 1 -3 8 1 3 | -------------------------------------------------------------------------------- /max_min_lp_core/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | max_min_lp_core 4 | 0.0.0 5 | The max_min_lp_core package 6 | Yoonchang Sung 7 | TODO 8 | 9 | catkin 10 | max_min_lp_msgs 11 | max_min_lp_msgs 12 | -------------------------------------------------------------------------------- /max_min_lp_simulation/urdf/box.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /max_min_lp_experiment/launch/greedy_central.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /max_min_lp_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | max_min_lp_msgs 4 | 0.0.0 5 | The max_min_lp_msgs package 6 | Yoonchang Sung 7 | TODO 8 | 9 | catkin 10 | roscpp 11 | message_generation 12 | std_msgs 13 | geometry_msgs 14 | message_runtime 15 | -------------------------------------------------------------------------------- /max_min_lp_simulation/urdf/box_library.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /max_min_lp_core/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(max_min_lp_core) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | max_min_lp_msgs 6 | ) 7 | 8 | catkin_package( 9 | INCLUDE_DIRS include 10 | LIBRARIES max_min_lp_core 11 | CATKIN_DEPENDS max_min_lp_msgs 12 | DEPENDS 13 | ) 14 | 15 | include_directories( 16 | include 17 | ${catkin_INCLUDE_DIRS} 18 | ) 19 | 20 | add_library(${PROJECT_NAME} 21 | src/MaxMinLPCore.cpp 22 | src/MaxMinLPDecentralizedCore.cpp 23 | src/MaxMinLPSequentialCore.cpp 24 | ) 25 | 26 | target_link_libraries(${PROJECT_NAME} 27 | ${catkin_LIBRARIES} 28 | ) 29 | 30 | add_dependencies(${PROJECT_NAME} max_min_lp_msgs_generate_messages_cpp) 31 | -------------------------------------------------------------------------------- /max_min_lp/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | max_min_lp 4 | 0.0.0 5 | The max_min_lp package 6 | Yoonchang Sung 7 | TODO 8 | catkin 9 | max_min_lp_core 10 | max_min_lp_msgs 11 | max_min_lp_demo 12 | max_min_lp_simulation 13 | max_min_lp_experiment 14 | max_min_lp_visualization 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /max_min_lp_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(max_min_lp_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | std_msgs 7 | message_generation 8 | ) 9 | 10 | add_message_files( 11 | DIRECTORY msg 12 | FILES 13 | general_node.msg 14 | general_node_array.msg 15 | layered_node.msg 16 | layered_node_array.msg 17 | target_node.msg 18 | primitive_node.msg 19 | server_to_robots.msg 20 | server_to_robots_array.msg 21 | ) 22 | 23 | generate_messages( 24 | DEPENDENCIES 25 | std_msgs 26 | ) 27 | 28 | catkin_package( 29 | # INCLUDE_DIRS include 30 | # LIBRARIES 31 | CATKIN_DEPENDS roscpp std_msgs message_runtime 32 | # DEPENDS system_lib 33 | ) 34 | include_directories( 35 | ${catkin_INCLUDE_DIRS} 36 | ) -------------------------------------------------------------------------------- /max_min_lp_experiment/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | max_min_lp_experiment 4 | 0.0.0 5 | The max_min_lp_experiment package 6 | Yoonchang Sung 7 | TODO 8 | 9 | catkin 10 | roscpp 11 | rospy 12 | std_msgs 13 | tf 14 | message_generation 15 | roscpp 16 | rospy 17 | std_msgs 18 | tf 19 | message_runtime 20 | -------------------------------------------------------------------------------- /max_min_lp_demo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | max_min_lp_demo 4 | 0.0.0 5 | The local algorithm-based max min linear programs for the target allocation 6 | Yoonchang Sung 7 | TODO 8 | 9 | catkin 10 | roscpp 11 | std_msgs 12 | max_min_lp_msgs 13 | max_min_lp_core 14 | roscpp 15 | std_msgs 16 | max_min_lp_msgs 17 | max_min_lp_core 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /max_min_lp_msgs/msg/layered_node.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | int16 id 3 | int16 layer 4 | int16 connected_id 5 | string state 6 | bool red_tree_candidate 7 | 8 | float32 x_v # For red and blue nodes 9 | float32 f_r # For robot nodes 10 | float32 g_t # For target nodes 11 | float32 t_r # For red nodes in the first layer (S*(r)) 12 | 13 | float32 z_r # For red nodes in the recursive algorithm 14 | float32 z_b # For blue nodes in the recursive algorithm 15 | float32 s_b # For blue nodes in the phase 3 of the recursive algorithm 16 | float32 z_v # For red and blue nodes in the last of the phase 3 17 | 18 | float64[] edge_weight 19 | 20 | int16 loc_deg # Number of degress on the corresponding node 21 | int16[] loc_neighbor_id # ID of local neighbors 22 | int16[] loc_layer # Layer for the local neighbors 23 | int16[] loc_connected_id 24 | string[] loc_state -------------------------------------------------------------------------------- /max_min_lp_simulation/srv/MessageRequest.srv: -------------------------------------------------------------------------------- 1 | string state_check 2 | int8 robot_id 3 | geometry_msgs/Pose robot_info 4 | geometry_msgs/Pose[] motion_primitive_info 5 | 6 | --- 7 | 8 | string state_answer 9 | max_min_lp_msgs/server_to_robots neighbor_info 10 | max_min_lp_msgs/general_node[] gen_r_node 11 | max_min_lp_msgs/general_node[] gen_p_r_node 12 | max_min_lp_msgs/general_node[] gen_p_t_node 13 | max_min_lp_msgs/general_node[] gen_t_node 14 | 15 | int8 max_neighbor_hop 16 | int8 num_neighbors_at_each_hop 17 | int8 num_new_targets_at_each_hop 18 | 19 | ## ROBOT info 20 | # ROBOT to robot 21 | int8[] ROBOT_num_robot 22 | int8[] prev_accumulate_robot 23 | int8 num_survived_robot 24 | 25 | # ROBOT to motion primitive 26 | int8[] ROBOT_num_motion_primitive 27 | int8[] prev_accumulate_motion_primitive 28 | int8 num_survived_motion_primitive 29 | 30 | float64[] constraint_value -------------------------------------------------------------------------------- /max_min_lp_simulation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | max_min_lp_simulation 4 | 0.0.0 5 | The max_min_lp_simulate package 6 | Yoonchang Sung 7 | TODO 8 | 9 | catkin 10 | roscpp 11 | std_msgs 12 | max_min_lp_msgs 13 | max_min_lp_core 14 | message_generation 15 | tf 16 | gazebo_msgs 17 | eigen 18 | roscpp 19 | std_msgs 20 | max_min_lp_msgs 21 | max_min_lp_core 22 | message_runtime 23 | tf 24 | gazebo_msgs 25 | eigen 26 | -------------------------------------------------------------------------------- /max_min_lp_demo/yaml/test_example_2.yaml: -------------------------------------------------------------------------------- 1 | #Setting for the global graph 2 | num_r: 2 3 | num_p: 4 4 | num_t: 2 5 | 6 | #Setting for the robot nodes 7 | r1_deg: 2 8 | r1_neighbor: [1, 2] 9 | r1_edge_weight: [1, 1] 10 | r2_deg: 2 11 | r2_neighbor: [3, 4] 12 | r2_edge_weight: [1, 1] 13 | 14 | #Setting for the motion primitives to robot nodes 15 | p1_r_deg: 1 16 | p1_r_neighbor: [1] 17 | p1_r_edge_weight: [1] 18 | p2_r_deg: 1 19 | p2_r_neighbor: [1] 20 | p2_r_edge_weight: [1] 21 | p3_r_deg: 1 22 | p3_r_neighbor: [2] 23 | p3_r_edge_weight: [1] 24 | p4_r_deg: 1 25 | p4_r_neighbor: [2] 26 | p4_r_edge_weight: [1] 27 | 28 | #Setting for the motion primitives to target nodes 29 | p1_t_deg: 1 30 | p1_t_neighbor: [1] 31 | p1_t_edge_weight: [5] 32 | p2_t_deg: 2 33 | p2_t_neighbor: [1, 2] 34 | p2_t_edge_weight: [1, 7] 35 | p3_t_deg: 1 36 | p3_t_neighbor: [1] 37 | p3_t_edge_weight: [6] 38 | p4_t_deg: 1 39 | p4_t_neighbor: [2] 40 | p4_t_edge_weight: [1] 41 | 42 | #Setting for the targets nodes 43 | t1_deg: 3 44 | t1_neighbor: [1, 2, 3] 45 | t1_edge_weight: [5, 1, 6] 46 | t2_deg: 2 47 | t2_neighbor: [2, 4] 48 | t2_edge_weight: [7, 1] -------------------------------------------------------------------------------- /max_min_lp_experiment/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(max_min_lp_experiment) 3 | 4 | SET(CMAKE_CXX_FLAGS "-std=c++0x") 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | roscpp 8 | rospy 9 | std_msgs 10 | tf 11 | genmsg 12 | ) 13 | 14 | add_service_files( 15 | FILES 16 | RobotRequest.srv 17 | TargetRequest.srv 18 | ) 19 | 20 | generate_messages( 21 | DEPENDENCIES 22 | std_msgs 23 | geometry_msgs 24 | ) 25 | 26 | catkin_package( 27 | INCLUDE_DIRS include 28 | LIBRARIES max_min_lp_experiment 29 | CATKIN_DEPENDS roscpp rospy std_msgs tf message_runtime 30 | # DEPENDS system_lib 31 | ) 32 | 33 | include_directories( 34 | include 35 | ${catkin_INCLUDE_DIRS} 36 | ) 37 | 38 | add_executable(central_node src/GreedyCentralNode.cpp) 39 | target_link_libraries(central_node 40 | ${catkin_LIBRARIES} 41 | ) 42 | 43 | add_executable(robot_node src/GreedyRobotNode.cpp) 44 | target_link_libraries(robot_node 45 | ${catkin_LIBRARIES} 46 | ) 47 | 48 | add_executable(target_node src/TargetNode.cpp) 49 | target_link_libraries(target_node 50 | ${catkin_LIBRARIES} 51 | ) 52 | -------------------------------------------------------------------------------- /max_min_lp_experiment/include/max_min_lp_experiment/TargetNode.hpp: -------------------------------------------------------------------------------- 1 | #ifndef TARGETNODE_HPP_ 2 | #define TARGETNODE_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | using namespace std; 17 | 18 | class TargetNode { 19 | private: 20 | ros::NodeHandle m_nh; 21 | ros::NodeHandle m_private_nh; 22 | 23 | // Services 24 | ros::ServiceServer m_request_service; 25 | 26 | // Params from the launch file 27 | int m_time_period; 28 | 29 | // Target information 30 | int m_num_target; 31 | vector m_target_id; 32 | vector m_target_pose_x; 33 | vector m_target_pose_y; 34 | vector m_target_velocity; 35 | vector m_target_orientation; 36 | int m_time_step; 37 | 38 | public: 39 | TargetNode(); // Constructor 40 | 41 | bool applyTargetMotion(max_min_lp_experiment::TargetRequest::Request &req, max_min_lp_experiment::TargetRequest::Response &res); 42 | void targetInitialize(); 43 | }; 44 | 45 | #endif -------------------------------------------------------------------------------- /max_min_lp_simulation/include/max_min_lp_simulation/get_odom.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GETODOM_HPP_ 2 | #define GETODOM_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | using namespace std; 19 | 20 | class get_odom { 21 | private: 22 | ros::NodeHandle m_nh; 23 | ros::NodeHandle m_private_nh; 24 | 25 | // Subscribers 26 | ros::Subscriber m_odom_sub; 27 | 28 | // Publishers 29 | ros::Publisher m_general_node_pub; 30 | 31 | // Services 32 | ros::ServiceServer m_odom_service; 33 | 34 | string m_robot_name; 35 | int m_num_robot; 36 | int m_num_target; 37 | int m_robot_id; 38 | 39 | geometry_msgs::Pose m_pos; 40 | 41 | public: 42 | get_odom(); // Constructor 43 | 44 | void updateOdom(const gazebo_msgs::ModelStates::ConstPtr& msg); // Update odometry information by subscribing to /robot/odom 45 | bool return_odom(max_min_lp_simulation::GetOdom::Request &req, max_min_lp_simulation::GetOdom::Response &res); 46 | }; 47 | 48 | #endif -------------------------------------------------------------------------------- /max_min_lp_simulation/launch/single_robot_case.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 | -------------------------------------------------------------------------------- /max_min_lp_simulation/launch/journal.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 | -------------------------------------------------------------------------------- /max_min_lp_demo/yaml/test_example.yaml: -------------------------------------------------------------------------------- 1 | #Setting for the global graph 2 | num_r: 3 3 | num_p: 6 4 | num_t: 3 5 | 6 | #Setting for the robot nodes 7 | r1_deg: 2 8 | r1_neighbor: [1, 2] 9 | r1_edge_weight: [1, 1] 10 | r2_deg: 2 11 | r2_neighbor: [3, 4] 12 | r2_edge_weight: [1, 1] 13 | r3_deg: 2 14 | r3_neighbor: [5, 6] 15 | r3_edge_weight: [1, 1] 16 | 17 | #Setting for the motion primitives to robot nodes 18 | p1_r_deg: 1 19 | p1_r_neighbor: [1] 20 | p1_r_edge_weight: [1] 21 | p2_r_deg: 1 22 | p2_r_neighbor: [1] 23 | p2_r_edge_weight: [1] 24 | p3_r_deg: 1 25 | p3_r_neighbor: [2] 26 | p3_r_edge_weight: [1] 27 | p4_r_deg: 1 28 | p4_r_neighbor: [2] 29 | p4_r_edge_weight: [1] 30 | p5_r_deg: 1 31 | p5_r_neighbor: [3] 32 | p5_r_edge_weight: [1] 33 | p6_r_deg: 1 34 | p6_r_neighbor: [3] 35 | p6_r_edge_weight: [1] 36 | 37 | #Setting for the motion primitives to target nodes 38 | p1_t_deg: 1 39 | p1_t_neighbor: [1] 40 | p1_t_edge_weight: [5] 41 | p2_t_deg: 1 42 | p2_t_neighbor: [2] 43 | p2_t_edge_weight: [3] 44 | p3_t_deg: 2 45 | p3_t_neighbor: [2, 3] 46 | p3_t_edge_weight: [3, 3] 47 | p4_t_deg: 1 48 | p4_t_neighbor: [3] 49 | p4_t_edge_weight: [4] 50 | p5_t_deg: 1 51 | p5_t_neighbor: [3] 52 | p5_t_edge_weight: [2] 53 | p6_t_deg: 1 54 | p6_t_neighbor: [1] 55 | p6_t_edge_weight: [4] 56 | 57 | #Setting for the targets nodes 58 | t1_deg: 2 59 | t1_neighbor: [1, 6] 60 | t1_edge_weight: [5, 4] 61 | t2_deg: 2 62 | t2_neighbor: [2, 3] 63 | t2_edge_weight: [3, 3] 64 | t3_deg: 3 65 | t3_neighbor: [3, 4, 5] 66 | t3_edge_weight: [3, 4, 2] -------------------------------------------------------------------------------- /max_min_lp_demo/yaml/test_paper_example.yaml: -------------------------------------------------------------------------------- 1 | #Setting for the global graph 2 | num_r: 3 3 | num_p: 6 4 | num_t: 3 5 | 6 | #Setting for the robot nodes 7 | r1_deg: 2 8 | r1_neighbor: [1, 2] 9 | r1_edge_weight: [5, 4] 10 | r2_deg: 2 11 | r2_neighbor: [3, 4] 12 | r2_edge_weight: [3, 4] 13 | r3_deg: 2 14 | r3_neighbor: [5, 6] 15 | r3_edge_weight: [4, 5] 16 | 17 | #Setting for the motion primitives to robot nodes 18 | p1_r_deg: 1 19 | p1_r_neighbor: [1] 20 | p1_r_edge_weight: [5] 21 | p2_r_deg: 1 22 | p2_r_neighbor: [1] 23 | p2_r_edge_weight: [4] 24 | p3_r_deg: 1 25 | p3_r_neighbor: [2] 26 | p3_r_edge_weight: [3] 27 | p4_r_deg: 1 28 | p4_r_neighbor: [2] 29 | p4_r_edge_weight: [4] 30 | p5_r_deg: 1 31 | p5_r_neighbor: [3] 32 | p5_r_edge_weight: [4] 33 | p6_r_deg: 1 34 | p6_r_neighbor: [3] 35 | p6_r_edge_weight: [5] 36 | 37 | #Setting for the motion primitives to target nodes 38 | p1_t_deg: 1 39 | p1_t_neighbor: [1] 40 | p1_t_edge_weight: [4] 41 | p2_t_deg: 1 42 | p2_t_neighbor: [2] 43 | p2_t_edge_weight: [5] 44 | p3_t_deg: 1 45 | p3_t_neighbor: [1] 46 | p3_t_edge_weight: [5] 47 | p4_t_deg: 1 48 | p4_t_neighbor: [2] 49 | p4_t_edge_weight: [3] 50 | p5_t_deg: 2 51 | p5_t_neighbor: [2, 3] 52 | p5_t_edge_weight: [5, 4] 53 | p6_t_deg: 1 54 | p6_t_neighbor: [3] 55 | p6_t_edge_weight: [3] 56 | 57 | #Setting for the targets nodes 58 | t1_deg: 2 59 | t1_neighbor: [1, 3] 60 | t1_edge_weight: [4, 5] 61 | t2_deg: 3 62 | t2_neighbor: [2, 4, 5] 63 | t2_edge_weight: [5, 3, 5] 64 | t3_deg: 2 65 | t3_neighbor: [5, 6] 66 | t3_edge_weight: [4, 3] -------------------------------------------------------------------------------- /max_min_lp_simulation/include/max_min_lp_simulation/apply_motion_primitive.hpp: -------------------------------------------------------------------------------- 1 | #ifndef APPLYMOTIONPRIMITIVE_HPP_ 2 | #define APPLYMOTIONPRIMITIVE_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | using namespace std; 22 | 23 | class apply_motion_primitive { 24 | private: 25 | ros::NodeHandle m_nh; 26 | ros::NodeHandle m_private_nh; 27 | 28 | // Subscribers 29 | ros::Subscriber m_odom_sub; 30 | 31 | // Publishers 32 | ros::Publisher m_cmd_vel_robot_pub; 33 | 34 | // Services 35 | ros::ServiceServer m_move_service; 36 | 37 | // Clients 38 | ros::ServiceClient m_odom_client; 39 | 40 | string m_robot_name; 41 | int m_num_robot; 42 | int m_num_target; 43 | int m_robot_id; 44 | int m_check_rotation_direction; 45 | geometry_msgs::Pose m_goal_pos; 46 | 47 | geometry_msgs::Pose m_pos; 48 | 49 | public: 50 | apply_motion_primitive(); // Constructor 51 | 52 | bool move_robot(max_min_lp_simulation::MoveRobot::Request &req, max_min_lp_simulation::MoveRobot::Response &res); 53 | }; 54 | 55 | #endif -------------------------------------------------------------------------------- /max_min_lp_visualization/log/general_graph.dot: -------------------------------------------------------------------------------- 1 | strict graph { 2 | graph [bb="0,0,0,0"]; 3 | node [label="\N"]; 4 | robot1 [pos="-375.000000,75.000000)"]; 5 | primitive1 [pos="-225.000000,0.000000)"]; 6 | robot1 -- primitive1 [label=2.0, 7 | weight=2.0]; 8 | primitive2 [pos="-75.000000,0.000000)"]; 9 | robot1 -- primitive2 [label=2.0, 10 | weight=2.0]; 11 | robot2 [pos="-225.000000,75.000000)"]; 12 | robot2 -- primitive1 [label=2.0, 13 | weight=2.0]; 14 | primitive3 [pos="75.000000,0.000000)"]; 15 | robot2 -- primitive3 [label=2.0, 16 | weight=2.0]; 17 | robot3 [pos="-75.000000,75.000000)"]; 18 | robot3 -- primitive1 [label=2.0, 19 | weight=2.0]; 20 | primitive4 [pos="225.000000,0.000000)"]; 21 | robot3 -- primitive4 [label=2.0, 22 | weight=2.0]; 23 | robot4 [pos="75.000000,75.000000)"]; 24 | robot4 -- primitive2 [label=2.0, 25 | weight=2.0]; 26 | robot4 -- primitive3 [label=2.0, 27 | weight=2.0]; 28 | robot5 [pos="225.000000,75.000000)"]; 29 | robot5 -- primitive2 [label=2.0, 30 | weight=2.0]; 31 | robot5 -- primitive4 [label=2.0, 32 | weight=2.0]; 33 | robot6 [pos="375.000000,75.000000)"]; 34 | robot6 -- primitive3 [label=2.0, 35 | weight=2.0]; 36 | robot6 -- primitive4 [label=2.0, 37 | weight=2.0]; 38 | target1 [pos="0.000000,-75.000000)"]; 39 | target1 -- primitive1 [label=1.0, 40 | weight=1.0]; 41 | target1 -- primitive2 [label=1.0, 42 | weight=1.0]; 43 | target1 -- primitive3 [label=1.0, 44 | weight=1.0]; 45 | target1 -- primitive4 [label=1.0, 46 | weight=1.0]; 47 | } 48 | -------------------------------------------------------------------------------- /max_min_lp_simulation/include/max_min_lp_simulation/apply_motion_primitive_journal.hpp: -------------------------------------------------------------------------------- 1 | #ifndef APPLYMOTIONPRIMITIVEJOURNAL_HPP_ 2 | #define APPLYMOTIONPRIMITIVEJOURNAL_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | using namespace std; 22 | 23 | class apply_motion_primitive { 24 | private: 25 | ros::NodeHandle m_nh; 26 | ros::NodeHandle m_private_nh; 27 | 28 | // Subscribers 29 | ros::Subscriber m_odom_sub; 30 | 31 | // Publishers 32 | ros::Publisher m_cmd_vel_robot_pub; 33 | 34 | // Services 35 | ros::ServiceServer m_move_service; 36 | 37 | // Clients 38 | ros::ServiceClient m_odom_client; 39 | 40 | string m_robot_name; 41 | int m_num_robot; 42 | int m_num_target; 43 | int m_robot_id; 44 | int m_check_rotation_direction; 45 | int m_trans_duration; 46 | int m_ang_duration; 47 | double m_trans_speed; 48 | double m_ang_speed; 49 | 50 | geometry_msgs::Pose m_pos; 51 | 52 | public: 53 | apply_motion_primitive(); // Constructor 54 | 55 | bool move_robot(max_min_lp_simulation::MoveRobot::Request &req, max_min_lp_simulation::MoveRobot::Response &res); 56 | }; 57 | 58 | #endif -------------------------------------------------------------------------------- /max_min_lp_demo/yaml/test_example_num_targets.yaml: -------------------------------------------------------------------------------- 1 | #Setting for the global graph 2 | input_path: '/home/yoon/yoon/max_min_ws/src/max_min_lp_demo/data/t100/t100d8/30_100_8/' 3 | output_path: '/home/yoon/yoon/max_min_ws/src/max_min_lp_demo/outputs/t100/t100d8/30_100_8/' 4 | 5 | num_r: 3 6 | num_p: 6 7 | num_t: 3 8 | 9 | #Setting for the robot nodes 10 | r1_deg: 2 11 | r1_neighbor: [1, 2] 12 | r1_edge_weight: [1, 1] 13 | r2_deg: 2 14 | r2_neighbor: [3, 4] 15 | r2_edge_weight: [1, 1] 16 | r3_deg: 2 17 | r3_neighbor: [5, 6] 18 | r3_edge_weight: [1, 1] 19 | 20 | #Setting for the motion primitives to robot nodes 21 | p1_r_deg: 1 22 | p1_r_neighbor: [1] 23 | p1_r_edge_weight: [1] 24 | p2_r_deg: 1 25 | p2_r_neighbor: [1] 26 | p2_r_edge_weight: [1] 27 | p3_r_deg: 1 28 | p3_r_neighbor: [2] 29 | p3_r_edge_weight: [1] 30 | p4_r_deg: 1 31 | p4_r_neighbor: [2] 32 | p4_r_edge_weight: [1] 33 | p5_r_deg: 1 34 | p5_r_neighbor: [3] 35 | p5_r_edge_weight: [1] 36 | p6_r_deg: 1 37 | p6_r_neighbor: [3] 38 | p6_r_edge_weight: [1] 39 | 40 | #Setting for the motion primitives to target nodes 41 | p1_t_deg: 1 42 | p1_t_neighbor: [1] 43 | p1_t_edge_weight: [1] 44 | p2_t_deg: 1 45 | p2_t_neighbor: [2] 46 | p2_t_edge_weight: [1] 47 | p3_t_deg: 2 48 | p3_t_neighbor: [2, 3] 49 | p3_t_edge_weight: [1, 1] 50 | p4_t_deg: 1 51 | p4_t_neighbor: [3] 52 | p4_t_edge_weight: [1] 53 | p5_t_deg: 1 54 | p5_t_neighbor: [3] 55 | p5_t_edge_weight: [1] 56 | p6_t_deg: 1 57 | p6_t_neighbor: [1] 58 | p6_t_edge_weight: [1] 59 | 60 | #Setting for the targets nodes 61 | t1_deg: 2 62 | t1_neighbor: [1, 6] 63 | t1_edge_weight: [1, 1] 64 | t2_deg: 2 65 | t2_neighbor: [2, 3] 66 | t2_edge_weight: [1, 1] 67 | t3_deg: 3 68 | t3_neighbor: [3, 4, 5] 69 | t3_edge_weight: [1, 1, 1] -------------------------------------------------------------------------------- /max_min_lp_simulation/src/kalman.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Implementation of KalmanFilter class. 3 | * 4 | * @author: Hayk Martirosyan 5 | * @date: 2014.11.15 6 | */ 7 | 8 | #include 9 | #include 10 | 11 | #include "max_min_lp_simulation/kalman.hpp" 12 | 13 | KalmanFilter::KalmanFilter( 14 | double dt, 15 | const Eigen::MatrixXd& A, 16 | const Eigen::MatrixXd& C, 17 | const Eigen::MatrixXd& B, 18 | const Eigen::MatrixXd& Q, 19 | const Eigen::MatrixXd& R, 20 | const Eigen::MatrixXd& P) 21 | : A(A), C(C), B(B), Q(Q), R(R), P0(P), 22 | m(C.rows()), n(A.rows()), dt(dt), initialized(false), 23 | I(n, n), x_hat(n), x_hat_new(n) 24 | { 25 | I.setIdentity(); 26 | } 27 | 28 | KalmanFilter::KalmanFilter() {} 29 | 30 | void KalmanFilter::init(double t0, const Eigen::VectorXd& x0) { 31 | x_hat = x0; 32 | P = P0; 33 | this->t0 = t0; 34 | t = t0; 35 | initialized = true; 36 | } 37 | 38 | void KalmanFilter::init() { 39 | x_hat.setZero(); 40 | P = P0; 41 | t0 = 0; 42 | t = t0; 43 | initialized = true; 44 | } 45 | 46 | void KalmanFilter::update(const Eigen::VectorXd& y, const Eigen::VectorXd& u) { 47 | 48 | if(!initialized) 49 | throw std::runtime_error("Filter is not initialized!"); 50 | 51 | // std::cout<<"x_hat = "< 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | using namespace std; 21 | 22 | class GreedyRobotNode { 23 | private: 24 | ros::NodeHandle m_nh; 25 | ros::NodeHandle m_private_nh; 26 | 27 | // Publisher 28 | ros::Publisher m_after_motion_pub; 29 | ros::Publisher m_cmd_vel_pub; 30 | // Subscriber 31 | ros::Subscriber m_response_sub; 32 | ros::Subscriber m_pose_sub; 33 | // Clients 34 | ros::ServiceClient m_request_client; 35 | 36 | // Variables from the launch file 37 | int m_num_target; 38 | int m_robot_id; 39 | int m_num_motion_primitive; 40 | int m_time_period; 41 | double m_robot_velocity; 42 | double m_robot_angular_vel; 43 | 44 | // Robot info 45 | geometry_msgs::Point m_robot_pose; 46 | 47 | int m_count_robotInitialize_activate; 48 | 49 | // Optimal motion primitive that is applied at the end of algorithm at each time 50 | int m_selected_primitive_id; 51 | vector m_motion_case_rotation; 52 | vector m_check_rotation_direction; 53 | vector m_motion_primitive_pose; 54 | 55 | public: 56 | GreedyRobotNode(); // Constructor 57 | 58 | bool robotInitialize(); 59 | vector computeMotionPrimitives(); 60 | void applyMotionPrimitives(const std_msgs::String::ConstPtr& msg); 61 | void getRobotPose(const nav_msgs::Odometry::ConstPtr& msg); 62 | }; 63 | 64 | #endif -------------------------------------------------------------------------------- /max_min_lp_simulation/src/get_odom.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Central node for the simulation 3 | * Central node only links between a set of motion primitives and a set of targets. 4 | * Important note: in this code a robot is used to take into consideration of the reduction of the step 1 of local algorithm. 5 | * Thus, a robot here does not indicate a real robot but an element of a real robot. Instead, ROBOT is used for a real robot. 6 | * \Author Yoonchang Sung 7 | * \02/27/2017 8 | * Copyright 2017. All Rights Reserved. 9 | */ 10 | 11 | #include "max_min_lp_simulation/get_odom.hpp" 12 | 13 | get_odom::get_odom() : 14 | m_num_robot(1), m_num_target(1), m_robot_id(1), m_robot_name(string("robot_0")), m_private_nh("~") 15 | { 16 | m_private_nh.getParam("num_robot", m_num_robot); 17 | m_private_nh.getParam("num_target", m_num_target); 18 | m_private_nh.getParam("robot_id", m_robot_id); 19 | m_private_nh.getParam("robot_name", m_robot_name); 20 | 21 | m_odom_sub = m_nh.subscribe("/gazebo/model_states", 1000, &get_odom::updateOdom, this); 22 | m_odom_service = m_nh.advertiseService("/robot_"+boost::lexical_cast(m_robot_id)+"/odom_request", &get_odom::return_odom, this); 23 | } 24 | 25 | void get_odom::updateOdom(const gazebo_msgs::ModelStates::ConstPtr& msg) { 26 | int size_msg = m_num_robot + m_num_target + 1; // 1 is for 'ground plane' in gazebo. 27 | int id = -1; 28 | for (int i = 0; i < size_msg; i++) { 29 | if (strcmp(msg->name[i].c_str(), m_robot_name.c_str()) == 0) { 30 | id = i; 31 | } 32 | } 33 | if (id != -1) { 34 | m_pos = msg->pose[id]; 35 | } 36 | } 37 | 38 | bool get_odom::return_odom(max_min_lp_simulation::GetOdom::Request &req, max_min_lp_simulation::GetOdom::Response &res) { 39 | res.return_odom = m_pos; 40 | return true; 41 | } 42 | 43 | int main(int argc, char **argv) { 44 | ros::init(argc, argv, "get_odom"); 45 | 46 | get_odom gtom; 47 | 48 | ros::spin(); 49 | return 0; 50 | } -------------------------------------------------------------------------------- /max_min_lp_demo/include/max_min_lp_demo/MaxMinLPDemo.hpp: -------------------------------------------------------------------------------- 1 | #ifndef MAXMINLPDEMO_HPP_ 2 | #define MAXMINLPDEMO_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | using namespace std; 23 | 24 | class MaxMinLPDemo { 25 | private: 26 | ros::NodeHandle m_nh; 27 | 28 | // Publishers 29 | ros::Publisher m_general_node_pub; 30 | ros::Publisher m_layered_node_pub; 31 | 32 | // Subscribers 33 | ros::Subscriber m_test_sub; 34 | 35 | // General node values 36 | vector m_gen_r_node; 37 | vector m_gen_p_r_node; 38 | vector m_gen_p_t_node; 39 | vector m_gen_t_node; 40 | 41 | // Variables from the launch file 42 | string m_input_type; 43 | string m_output_type; 44 | string m_output_type_journal; 45 | int m_num_layer; // Number of layers in the layered model 46 | int m_num_text_files; 47 | bool m_verbal_flag; 48 | double m_epsilon; 49 | 50 | ofstream m_outputFile; 51 | ofstream m_outputFile_journal; 52 | 53 | int m_count; 54 | bool check_start; 55 | 56 | public: 57 | MaxMinLPDemo(); // Constructor 58 | 59 | void maxMinCallback(const std_msgs::String::ConstPtr& msg); 60 | void computeLocalAlgorithm(const std_msgs::String::ConstPtr& msg, int num_robot_node, int num_p_t_node, vector p_t_id, vector p_t_loc_deg, vector > p_t_neighbor); 61 | }; 62 | 63 | #endif -------------------------------------------------------------------------------- /max_min_lp_simulation/src/get_result_target_num.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Central node for the simulation 3 | * Central node only links between a set of motion primitives and a set of targets. 4 | * Important note: in this code a robot is used to take into consideration of the reduction of the step 1 of local algorithm. 5 | * Thus, a robot here does not indicate a real robot but an element of a real robot. Instead, ROBOT is used for a real robot. 6 | * \Author Yoonchang Sung 7 | * \02/27/2017 8 | * Copyright 2017. All Rights Reserved. 9 | */ 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | using namespace std; 25 | 26 | int main(int argc, char **argv) { 27 | ros::init(argc, argv, "get_total_num_target"); 28 | ros::NodeHandle nh; 29 | 30 | ros::ServiceClient total_num_target_client; 31 | ofstream outputFile; 32 | 33 | outputFile.open("/home/yoon/yoon/max_min_ws/src/max_min_lp_simulation/data/greedy/results.txt"); 34 | 35 | int return_total_num_target = 0; 36 | 37 | ros::Rate r(0.5); // 10 hz 38 | 39 | while (1) 40 | { 41 | clock_t current_time = clock(); 42 | 43 | cout<<"Time = "<("/total_num_target_request"); 46 | max_min_lp_simulation::GetTotalNumTarget srv; 47 | srv.request.request_odom = string("request"); 48 | 49 | if (total_num_target_client.call(srv)) { 50 | return_total_num_target = srv.response.total_num_target; 51 | outputFile< 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 15 | 16 | 17 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /max_min_lp_simulation/launch/includes/kobuki.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 15 | 16 | 17 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /max_min_lp_simulation/include/max_min_lp_simulation/kalman.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Kalman filter implementation using Eigen. Based on the following 3 | * introductory paper: 4 | * 5 | * http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf 6 | * 7 | * @author: Hayk Martirosyan 8 | * @date: 2014.11.15 9 | */ 10 | 11 | #include 12 | 13 | #pragma once 14 | 15 | class KalmanFilter { 16 | 17 | public: 18 | 19 | /** 20 | * Create a Kalman filter with the specified matrices. 21 | * A - System dynamics matrix 22 | * C - Output matrix 23 | * Q - Process noise covariance 24 | * R - Measurement noise covariance 25 | * P - Estimate error covariance 26 | */ 27 | KalmanFilter( 28 | double dt, 29 | const Eigen::MatrixXd& A, 30 | const Eigen::MatrixXd& C, 31 | const Eigen::MatrixXd& B, 32 | const Eigen::MatrixXd& Q, 33 | const Eigen::MatrixXd& R, 34 | const Eigen::MatrixXd& P 35 | ); 36 | 37 | /** 38 | * Create a blank estimator. 39 | */ 40 | KalmanFilter(); 41 | 42 | /** 43 | * Initialize the filter with initial states as zero. 44 | */ 45 | void init(); 46 | 47 | /** 48 | * Initialize the filter with a guess for initial states. 49 | */ 50 | void init(double t0, const Eigen::VectorXd& x0); 51 | 52 | /** 53 | * Update the estimated state based on measured values. The 54 | * time step is assumed to remain constant. 55 | */ 56 | void update(const Eigen::VectorXd& y, const Eigen::VectorXd& u); 57 | 58 | /** 59 | * Return the current state and time. 60 | */ 61 | Eigen::VectorXd state() { return x_hat; }; 62 | Eigen::MatrixXd covariance() { return P; }; 63 | double time() { return t; }; 64 | 65 | private: 66 | 67 | // Matrices for computation 68 | Eigen::MatrixXd A, C, B, Q, R, P, K, P0; 69 | 70 | // System dimensions 71 | int m, n; 72 | 73 | // Initial and current time 74 | double t0, t; 75 | 76 | // Discrete time step 77 | double dt; 78 | 79 | // Is the filter initialized? 80 | bool initialized; 81 | 82 | // n-size identity 83 | Eigen::MatrixXd I; 84 | 85 | // Estimated states 86 | Eigen::VectorXd x_hat, x_hat_new; 87 | }; -------------------------------------------------------------------------------- /max_min_lp_experiment/include/max_min_lp_experiment/GreedyCentralNode.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GREEDYCENTRALNODE_HPP_ 2 | #define GREEDYCENTRALNODE_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | using namespace std; 19 | 20 | class GreedyCentralNode { 21 | private: 22 | ros::NodeHandle m_nh; 23 | ros::NodeHandle m_private_nh; 24 | 25 | // Publishers 26 | ros::Publisher m_response_to_robot_pub; 27 | // Subscribers 28 | ros::Subscriber m_comm_graph_by_robots_sub; 29 | // Services 30 | ros::ServiceServer m_center_request_service; 31 | // Clients 32 | ros::ServiceClient m_center_request_client; 33 | 34 | // Params from the launch file 35 | int m_num_robot; 36 | int m_sensing_range; 37 | int m_time_period; 38 | 39 | int m_send_robot_id; 40 | int m_request_robot_id; 41 | bool m_check_request_send; 42 | 43 | // Robot information 44 | vector m_robot_id; 45 | vector m_robot_pose_x; 46 | vector m_robot_pose_y; 47 | 48 | // Target information 49 | int m_num_target; 50 | vector m_target_id; 51 | vector m_target_pose_x; 52 | vector m_target_pose_y; 53 | vector m_target_velocity; 54 | vector m_target_orientation; 55 | 56 | vector > m_primitives_to_targets; // Motion primitives to targets 57 | vector > m_targets_to_primitives; // Targets to motion primitives 58 | 59 | vector m_target_index_used; 60 | 61 | vector m_optimal_primitive_id; 62 | 63 | int m_check_finish_action_apply; 64 | int m_time_step; 65 | 66 | public: 67 | GreedyCentralNode(); // Constructor 68 | 69 | bool requestInitialize(max_min_lp_experiment::RobotRequest::Request &req, max_min_lp_experiment::RobotRequest::Response &res); 70 | void afterAppliedMotionPrimitive(const std_msgs::String::ConstPtr& msg); 71 | void targetInitialize(); 72 | }; 73 | 74 | #endif -------------------------------------------------------------------------------- /max_min_lp_simulation/gazebo_model/world/target_spawn_journal.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | model://ground_plane 8 | 9 | 10 | 11 | model://sun 12 | 13 | 14 | 15 | 16 | 0 0 0 0 0 0 17 | 18 | 19 | 20 | 21 | 0.00001 22 | 0.4 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 0.2 31 | 0.4 32 | 33 | 34 | 35 | 0 0 1 1 36 | 0 0 1 1 37 | 0.1 0.1 0.1 1 38 | 0 0 0 0 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 74 | 75 | 76 | 77 | 78 | -------------------------------------------------------------------------------- /max_min_lp_visualization/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | max_min_lp_visualization 4 | 0.0.0 5 | The max_min_lp_plot package 6 | 7 | 8 | 9 | 10 | yoon 11 | 12 | 13 | 14 | 15 | 16 | TODO 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 | catkin 43 | rospy 44 | std_msgs 45 | rospy 46 | std_msgs 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /max_min_lp_experiment/src/TargetNode.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Target node for the experiment. 3 | * \Author Yoonchang Sung 4 | * \05/08/2018 5 | * Copyright 2018. All Rights Reserved. 6 | */ 7 | 8 | #include "max_min_lp_experiment/TargetNode.hpp" 9 | 10 | #define PI 3.141592 11 | 12 | TargetNode::TargetNode() : 13 | m_time_period(5), m_private_nh("~") 14 | { 15 | m_private_nh.getParam("time_period", m_time_period); 16 | 17 | // Services 18 | m_request_service = m_nh.advertiseService("/target_request", &TargetNode::applyTargetMotion, this); 19 | 20 | targetInitialize(); 21 | m_time_step = 0; 22 | } 23 | 24 | bool TargetNode::applyTargetMotion(max_min_lp_experiment::TargetRequest::Request &req, max_min_lp_experiment::TargetRequest::Response &res) { 25 | if (strcmp(req.state_request.c_str(), "center_ready") == 0) { 26 | m_time_step += 1; 27 | 28 | for (int i = 0; i < m_num_target; i++) { 29 | m_target_pose_x[i] += (m_target_velocity[i]*cos(m_target_orientation[i]*PI/180)); 30 | m_target_pose_y[i] += (m_target_velocity[i]*sin(m_target_orientation[i]*PI/180)); 31 | 32 | res.target_id.push_back(m_target_id[i]); 33 | geometry_msgs::Point each_target_pose; 34 | each_target_pose.x = m_target_pose_x[i]; 35 | each_target_pose.y = m_target_pose_y[i]; 36 | each_target_pose.z = 0; 37 | res.target_pose.push_back(each_target_pose); 38 | } 39 | 40 | res.state_answer = "target_done"; 41 | } 42 | } 43 | 44 | void TargetNode::targetInitialize() { 45 | if (ros::param::get("target_node/num_of_targets", m_num_target)) {} 46 | else { 47 | ROS_WARN("Didn't find num_of_targets"); 48 | } 49 | if (ros::param::get("target_node/target_id", m_target_id)) {} 50 | else { 51 | ROS_WARN("Didn't find target_id"); 52 | } 53 | if (ros::param::get("target_node/pose_x", m_target_pose_x)) {} 54 | else { 55 | ROS_WARN("Didn't find pose_x"); 56 | } 57 | if (ros::param::get("target_node/pose_y", m_target_pose_y)) {} 58 | else { 59 | ROS_WARN("Didn't find pose_y"); 60 | } 61 | if (ros::param::get("target_node/velocity", m_target_velocity)) {} 62 | else { 63 | ROS_WARN("Didn't find velocity"); 64 | } 65 | if (ros::param::get("target_node/orientation", m_target_orientation)) {} 66 | else { 67 | ROS_WARN("Didn't find orientation"); 68 | } 69 | } 70 | 71 | int main(int argc, char **argv) { 72 | ros::init(argc, argv, "target_node"); 73 | TargetNode tn; 74 | 75 | ros::spin(); 76 | 77 | return 0; 78 | } 79 | -------------------------------------------------------------------------------- /max_min_lp_demo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(max_min_lp_demo) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | std_msgs 7 | max_min_lp_msgs 8 | max_min_lp_core 9 | ) 10 | 11 | catkin_package( 12 | INCLUDE_DIRS include 13 | LIBRARIES max_min_lp_demo_lib 14 | CATKIN_DEPENDS roscpp std_msgs 15 | # DEPENDS system_lib 16 | ) 17 | 18 | ########### 19 | ## Build ## 20 | ########### 21 | 22 | ## Specify additional locations of header files 23 | ## Your package locations should be listed before other locations 24 | # include_directories(include) 25 | include_directories( 26 | include 27 | ${catkin_INCLUDE_DIRS} 28 | ) 29 | 30 | # add_library(max_min_lp 31 | # src/MaxMinLP.cpp 32 | # ) 33 | 34 | # add_dependencies(max_min_lp ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 35 | 36 | add_executable(max_min_lp_demo_node src/MaxMinLPDemo.cpp) 37 | 38 | ## Add cmake target dependencies of the executable 39 | ## same as for the library above 40 | # add_dependencies(max_min_lp_demo_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 41 | 42 | ## Specify libraries to link a library or executable target against 43 | target_link_libraries(max_min_lp_demo_node 44 | ${catkin_LIBRARIES} 45 | ) 46 | 47 | ############# 48 | ## Install ## 49 | ############# 50 | 51 | # all install targets should use catkin DESTINATION variables 52 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 53 | 54 | ## Mark executable scripts (Python etc.) for installation 55 | ## in contrast to setup.py, you can choose the destination 56 | # install(PROGRAMS 57 | # scripts/my_python_script 58 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 59 | # ) 60 | 61 | ## Mark executables and/or libraries for installation 62 | # install(TARGETS max_min_lp max_min_lp_demo_node 63 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 64 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 65 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 66 | # ) 67 | 68 | ## Mark cpp header files for installation 69 | # install(DIRECTORY include/${PROJECT_NAME}/ 70 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 71 | # FILES_MATCHING PATTERN "*.h" 72 | # PATTERN ".svn" EXCLUDE 73 | # ) 74 | 75 | ## Mark other files for installation (e.g. launch and bag files, etc.) 76 | # install(FILES 77 | # # myfile1 78 | # # myfile2 79 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 80 | # ) -------------------------------------------------------------------------------- /max_min_lp_simulation/launch/greedy_robots_journal.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 | -------------------------------------------------------------------------------- /max_min_lp_demo/yaml/test_example_3.yaml: -------------------------------------------------------------------------------- 1 | #Setting for the global graph 2 | num_r: 6 3 | num_p: 12 4 | num_t: 5 5 | 6 | #Setting for the robot nodes 7 | r1_deg: 2 8 | r1_neighbor: [1, 2] 9 | r1_edge_weight: [1, 1] 10 | r2_deg: 2 11 | r2_neighbor: [3, 4] 12 | r2_edge_weight: [1, 1] 13 | r3_deg: 2 14 | r3_neighbor: [5, 6] 15 | r3_edge_weight: [1, 1] 16 | r4_deg: 2 17 | r4_neighbor: [7, 8] 18 | r4_edge_weight: [1, 1] 19 | r5_deg: 2 20 | r5_neighbor: [9, 10] 21 | r5_edge_weight: [1, 1] 22 | r6_deg: 2 23 | r6_neighbor: [11, 12] 24 | r6_edge_weight: [1, 1] 25 | 26 | #Setting for the motion primitives to robot nodes 27 | p1_r_deg: 1 28 | p1_r_neighbor: [1] 29 | p1_r_edge_weight: [1] 30 | p2_r_deg: 1 31 | p2_r_neighbor: [1] 32 | p2_r_edge_weight: [1] 33 | p3_r_deg: 1 34 | p3_r_neighbor: [2] 35 | p3_r_edge_weight: [1] 36 | p4_r_deg: 1 37 | p4_r_neighbor: [2] 38 | p4_r_edge_weight: [1] 39 | p5_r_deg: 1 40 | p5_r_neighbor: [3] 41 | p5_r_edge_weight: [1] 42 | p6_r_deg: 1 43 | p6_r_neighbor: [3] 44 | p6_r_edge_weight: [1] 45 | p7_r_deg: 1 46 | p7_r_neighbor: [4] 47 | p7_r_edge_weight: [1] 48 | p8_r_deg: 1 49 | p8_r_neighbor: [4] 50 | p8_r_edge_weight: [1] 51 | p9_r_deg: 1 52 | p9_r_neighbor: [5] 53 | p9_r_edge_weight: [1] 54 | p10_r_deg: 1 55 | p10_r_neighbor: [5] 56 | p10_r_edge_weight: [1] 57 | p11_r_deg: 1 58 | p11_r_neighbor: [6] 59 | p11_r_edge_weight: [1] 60 | p12_r_deg: 1 61 | p12_r_neighbor: [6] 62 | p12_r_edge_weight: [1] 63 | 64 | #Setting for the motion primitives to target nodes 65 | p1_t_deg: 1 66 | p1_t_neighbor: [1] 67 | p1_t_edge_weight: [1] 68 | p2_t_deg: 2 69 | p2_t_neighbor: [1, 2] 70 | p2_t_edge_weight: [1, 1] 71 | p3_t_deg: 1 72 | p3_t_neighbor: [2] 73 | p3_t_edge_weight: [1] 74 | p4_t_deg: 2 75 | p4_t_neighbor: [1, 2] 76 | p4_t_edge_weight: [1, 1] 77 | p5_t_deg: 1 78 | p5_t_neighbor: [1] 79 | p5_t_edge_weight: [1] 80 | p6_t_deg: 1 81 | p6_t_neighbor: [3] 82 | p6_t_edge_weight: [1] 83 | p7_t_deg: 1 84 | p7_t_neighbor: [3] 85 | p7_t_edge_weight: [1] 86 | p8_t_deg: 2 87 | p8_t_neighbor: [2, 4] 88 | p8_t_edge_weight: [1, 1] 89 | p9_t_deg: 1 90 | p9_t_neighbor: [4] 91 | p9_t_edge_weight: [1] 92 | p10_t_deg: 1 93 | p10_t_neighbor: [5] 94 | p10_t_edge_weight: [1] 95 | p11_t_deg: 1 96 | p11_t_neighbor: [4] 97 | p11_t_edge_weight: [1] 98 | p12_t_deg: 1 99 | p12_t_neighbor: [5] 100 | p12_t_edge_weight: [1] 101 | 102 | #Setting for the targets nodes 103 | t1_deg: 4 104 | t1_neighbor: [1, 2, 4, 5] 105 | t1_edge_weight: [1, 1, 1, 1] 106 | t2_deg: 4 107 | t2_neighbor: [2, 3, 4, 8] 108 | t2_edge_weight: [1, 1, 1, 1] 109 | t3_deg: 2 110 | t3_neighbor: [6, 7] 111 | t3_edge_weight: [1, 1] 112 | t4_deg: 3 113 | t4_neighbor: [8, 9, 11] 114 | t4_edge_weight: [1, 1, 1] 115 | t5_deg: 2 116 | t5_neighbor: [10, 12] 117 | t5_edge_weight: [1, 1] -------------------------------------------------------------------------------- /max_min_lp_simulation/include/max_min_lp_simulation/RobotGreedyJournal.hpp: -------------------------------------------------------------------------------- 1 | #ifndef RobotGreedyJournal_HPP_ 2 | #define RobotGreedyJournal_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | using namespace std; 29 | 30 | class RobotGreedyJournal { 31 | private: 32 | ros::NodeHandle m_nh; 33 | ros::NodeHandle m_private_nh; 34 | 35 | // Subscribers 36 | ros::Subscriber m_odom_sub; 37 | ros::Subscriber m_request_sub; 38 | 39 | // Publishers 40 | vector m_target_predicted_pose_pub; 41 | vector m_target_measurement_pose_pub; 42 | ros::Publisher m_cmd_vel_robot_pub; 43 | 44 | // Clients 45 | ros::ServiceClient m_target_odom_client; 46 | ros::ServiceClient m_move_client; 47 | 48 | // Variables from the launch file 49 | int m_num_robot; 50 | int m_num_target; 51 | int m_robot_id; 52 | int m_num_motion_primitive; 53 | int m_time_interval; 54 | int m_robot_time; 55 | double m_robot_trans_speed; 56 | double m_robot_ang_speed; 57 | double m_sensing_range; 58 | double m_comm_range; 59 | bool m_verbal_flag; 60 | string m_robot_name; 61 | 62 | // Variables for each robot 63 | vector > m_c; 64 | vector m_w; 65 | vector m_w_prime; 66 | vector m_kalman; 67 | bool m_firt_time_step; 68 | double m_moving_time; 69 | double m_cur_time; 70 | double m_prev_time; 71 | 72 | // Motion primitive-related 73 | vector m_motion_case_rotation; 74 | vector m_check_rotation_direction; 75 | vector m_motion_primitive_pose; 76 | vector m_motion_trans_duration; 77 | vector m_motion_ang_duration; 78 | 79 | // Robot info 80 | geometry_msgs::Pose m_pos; 81 | 82 | // Target info of each time-step 83 | vector m_measure_target_pos; 84 | vector m_predicted_target_pos; 85 | vector m_prev_target_pos; 86 | 87 | public: 88 | RobotGreedyJournal(); // Constructor 89 | 90 | void activateRobots(const std_msgs::String::ConstPtr& msg); 91 | void updateOdom(const gazebo_msgs::ModelStates::ConstPtr& msg); // Update odometry information by subscribing to /robot/odom 92 | vector computeMotionPrimitives(); 93 | void getMotionCaseRotation(); 94 | bool initialize(); 95 | }; 96 | 97 | #endif -------------------------------------------------------------------------------- /max_min_lp_simulation/include/max_min_lp_simulation/MaxMinLPRobotNode.hpp: -------------------------------------------------------------------------------- 1 | #ifndef MAXMINLPROBOTNODE_HPP_ 2 | #define MAXMINLPROBOTNODE_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | using namespace std; 28 | 29 | class MaxMinLPRobotNode { 30 | private: 31 | ros::NodeHandle m_nh; 32 | ros::NodeHandle m_private_nh; 33 | 34 | ros::Subscriber request_sub; 35 | 36 | // Publishers 37 | ros::Publisher m_general_node_pub; 38 | ros::Publisher m_layered_node_pub; 39 | ros::Publisher m_response_to_server_pub; 40 | ros::Publisher m_cmd_vel_robot_pub; 41 | 42 | // Subscriber 43 | ros::Subscriber m_odom_sub; 44 | 45 | // Clients 46 | ros::ServiceClient m_client; 47 | ros::ServiceClient m_primitive_client; 48 | 49 | // Variables from the launch file 50 | int m_num_robot; 51 | int m_num_target; 52 | int m_robot_id; 53 | int m_num_layer; // Number of layers in the layered model 54 | int m_num_motion_primitive; 55 | int m_time_interval; 56 | bool m_verbal_flag; 57 | double m_epsilon; 58 | string m_robot_name; 59 | 60 | // Optimal motion primitive that is applied at the end of algorithm at each time 61 | int m_count_time_interval; 62 | int m_selected_primitive_id; 63 | vector m_motion_case_rotation; 64 | 65 | // Robot info 66 | geometry_msgs::Pose m_pos; 67 | 68 | // For reduction 69 | vector m_constraint_value; 70 | 71 | // Local info 72 | int m_max_neighbor_hop; 73 | vector m_num_neighbors_at_each_hop; 74 | vector m_num_new_targets_at_each_hop; 75 | 76 | // ROBOT robot 77 | vector m_ROBOT_num_robot; 78 | vector m_prev_accumulate_robot; 79 | int m_num_survived_robot; 80 | 81 | // ROBOT motion primitives 82 | vector m_ROBOT_num_motion_primitive; 83 | vector m_prev_accumulate_motion_primitive; 84 | int m_num_survived_motion_primitive; 85 | 86 | vector m_gen_r_node; 87 | vector m_gen_p_r_node; 88 | vector m_gen_p_t_node; 89 | vector m_gen_t_node; 90 | 91 | public: 92 | MaxMinLPRobotNode(); // Constructor 93 | 94 | void updateOdom(const gazebo_msgs::ModelStates::ConstPtr& msg); // Update odometry information by subscribing to /robot/odom 95 | bool initialize(); 96 | bool getMotionPrimitive(); 97 | void applyMotionPrimitives(const std_msgs::String::ConstPtr& msg); 98 | vector computeMotionPrimitives(); 99 | }; 100 | 101 | #endif -------------------------------------------------------------------------------- /max_min_lp_simulation/include/max_min_lp_simulation/MaxMinLPGreedyRobotNode.hpp: -------------------------------------------------------------------------------- 1 | #ifndef MaxMinLPGREEDYRobotNode_HPP_ 2 | #define MaxMinLPGREEDYRobotNode_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | using namespace std; 32 | 33 | class MaxMinLPGreedyRobotNode { 34 | private: 35 | ros::NodeHandle m_nh; 36 | ros::NodeHandle m_private_nh; 37 | 38 | // Publisher 39 | ros::Publisher m_response_to_server_pub; 40 | 41 | // Subscriber 42 | ros::Subscriber m_request_sub; 43 | ros::Subscriber m_odom_sub; 44 | 45 | // Clients 46 | ros::ServiceClient m_client; 47 | ros::ServiceClient m_primitive_client; 48 | ros::ServiceClient m_move_client; 49 | 50 | // General node values 51 | vector m_gen_r_node; 52 | vector m_gen_p_r_node; 53 | vector m_gen_p_t_node; 54 | vector m_gen_t_node; 55 | 56 | max_min_lp_msgs::server_to_robots_array m_local_info; 57 | 58 | // Variables from the launch file 59 | int m_num_robot; 60 | int m_num_target; 61 | int m_robot_id; 62 | int m_num_layer; // Number of layers in the layered model 63 | int m_num_motion_primitive; 64 | int m_time_interval; 65 | bool m_verbal_flag; 66 | double m_epsilon; 67 | string m_robot_name; 68 | 69 | // Robot info 70 | geometry_msgs::Pose m_pos; 71 | 72 | int m_count_initialize_func; 73 | 74 | // int m_motion_case_rotation[]; 75 | 76 | // Optimal motion primitive that is applied at the end of algorithm at each time 77 | int m_selected_primitive_id; 78 | vector m_motion_case_rotation; 79 | vector m_check_rotation_direction; 80 | vector m_motion_primitive_pose; 81 | 82 | ofstream m_robot_outputFile; 83 | 84 | int count_computeMotionPrimitives; 85 | int * m_random_number_1; 86 | int * m_random_number_2; 87 | int * m_random_number_3; 88 | int * m_random_number_4; 89 | int * m_random_number_5; 90 | 91 | public: 92 | MaxMinLPGreedyRobotNode(); // Constructor 93 | ~MaxMinLPGreedyRobotNode() { 94 | m_robot_outputFile.close(); 95 | delete[] m_random_number_1; 96 | delete[] m_random_number_2; 97 | delete[] m_random_number_3; 98 | delete[] m_random_number_4; 99 | delete[] m_random_number_5; 100 | } 101 | 102 | void updateOdom(const gazebo_msgs::ModelStates::ConstPtr& msg); // Update odometry information by subscribing to /robot/odom 103 | bool initialize(); 104 | vector computeMotionPrimitives(); 105 | void applyMotionPrimitives(const std_msgs::String::ConstPtr& msg); 106 | }; 107 | 108 | #endif -------------------------------------------------------------------------------- /max_min_lp_simulation/include/max_min_lp_simulation/MaxMinLPGreedyCentralNode.hpp: -------------------------------------------------------------------------------- 1 | #ifndef MAXMINLPGREEDYCENTRALNODE_HPP_ 2 | #define MAXMINLPGREEDYCENTRALNODE_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | using namespace std; 26 | 27 | class MaxMinLPGreedyCentralNode { 28 | private: 29 | ros::NodeHandle m_nh; 30 | ros::NodeHandle m_private_nh; 31 | 32 | // Publishers 33 | // ros::Publisher m_request_pub; 34 | // ros::Publisher m_general_node_pub; 35 | // ros::Publisher m_layered_node_pub; 36 | ros::Publisher m_response_to_robot_pub; 37 | 38 | // Subscribers 39 | ros::Subscriber m_target_1_sub; 40 | ros::Subscriber m_target_2_sub; 41 | ros::Subscriber m_target_3_sub; 42 | 43 | ros::Subscriber m_comm_graph_by_robots_sub; 44 | 45 | // Services 46 | ros::ServiceServer m_service; 47 | ros::ServiceServer m_primitive_service; 48 | ros::ServiceServer m_total_num_target_service; 49 | 50 | // Clients 51 | ros::ServiceClient m_target_odom_client; 52 | 53 | // Variables from the launch file 54 | max_min_lp_msgs::server_to_robots_array m_robot_info; 55 | int m_num_robot; 56 | int m_num_target; 57 | int m_fov; 58 | int m_num_motion_primitive; 59 | 60 | int m_send_robot_id; 61 | int m_request_robot_id; 62 | bool m_check_request_send; 63 | 64 | // Motion primitives info 65 | vector m_primitive_id; 66 | vector m_primitive_x_pos; 67 | vector m_primitive_y_pos; 68 | 69 | // Target info of each time-step 70 | vector m_temp_target_pos; 71 | vector m_temp_target_name; 72 | 73 | // Target info when consensus of robots is fulfilled 74 | vector m_target_id; 75 | vector m_target_x_pos; 76 | vector m_target_y_pos; 77 | 78 | vector > m_primitives_to_targets; // Motion primitives to targets 79 | vector > m_targets_to_primitives; // Targets to motion primitives 80 | 81 | vector m_target_index_used; 82 | 83 | vector m_optimal_primitive_id; 84 | 85 | // For text outputs 86 | ofstream m_outputFile; 87 | 88 | int m_check_finish_action_apply; 89 | int m_time_step; 90 | 91 | int m_total_num_target_covered; 92 | 93 | public: 94 | MaxMinLPGreedyCentralNode(); // Constructor 95 | ~MaxMinLPGreedyCentralNode(){ 96 | m_outputFile.close(); 97 | } 98 | 99 | bool initialize(max_min_lp_simulation::MessageRequest::Request &req, max_min_lp_simulation::MessageRequest::Response &res); 100 | bool sendMotionPrimitive(max_min_lp_simulation::MotionPrimitiveRequest::Request &req, max_min_lp_simulation::MotionPrimitiveRequest::Response &res); 101 | void applySequentialLocalAlgorithm(const std_msgs::String::ConstPtr& msg); 102 | bool get_total_num_target(max_min_lp_simulation::GetTotalNumTarget::Request &req, max_min_lp_simulation::GetTotalNumTarget::Response &res); 103 | 104 | void updatePose(const gazebo_msgs::ModelStates::ConstPtr& msg, int target_id); // Update pose information 105 | }; 106 | 107 | #endif -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # multi-robot_task_allocation 2 | Algorithms of a multi-robot task allocation for multi-target tracking are provided, which have been developed in ROS Kinetic on Ubuntu 16.04. This package provides several features for a multi-robot task allocation when tracking multiple targets. A local algorithm plays a key role in this algorithm where it provides nice properties on communication limitation and was referred by Floréen, Patrik, et al. "Local approximability of max-min and min-max linear programs." Theory of Computing Systems 49.4 (2011): 672-697. 3 | 4 | ## Run a demo 5 | 6 | Demo is included in max_min_lp_demo. In max_min_lp_demo/data there are several instances that you can test. Local algorithm is implemented in a sequential way, not in a distributed since one machine is used to test the performance of the algorithm. 7 | 8 | ``` 9 | roslaunch max_min_lp_demo test_max_min_lp.launch 10 | ``` 11 | In case you want to visualize an original and layered graphs of instance that you are testing, run the following. 12 | 13 | ``` 14 | rosrun max_min_lp_visualization max_min_lp_plot.py 15 | ``` 16 | 17 | Then, the result of the graph will be in the max_min_lp_visualization/log folder. The following will activate the demo. 18 | 19 | ``` 20 | rostopic pub /robot_status std_msgs/String demo 21 | ``` 22 | 23 | ## Two main algorithms 24 | 25 | There are two versions of multi-robot multi-target tracking algorithms. The first algorithm is a local algorithm based one, which is explained in the beginning. The second algorithm is a greedy one where each robot greedily track targets without considering other robots. For both a central node exists to simulate sensing and communication graphs. Each robot has its own node for local computation and getting odometry information from the gazebo simulation. 26 | 27 | ### Local algorithm 28 | 29 | Run the gazebo simulation first. This will spawn five (turtlebot) robots with thirty stationary and moving targets. 30 | 31 | ``` 32 | roslaunch max_min_lp_simulation five_robot_twenty_target_case.launch 33 | ``` 34 | Then, run the main algorithm. 35 | 36 | ``` 37 | roslaunch max_min_lp_simulation simulation_server_to_robots.launch 38 | ``` 39 | 40 | To initiate the algorithm, 41 | 42 | ``` 43 | rostopic pub /robot_status std_msgs/String run 44 | ``` 45 | 46 | 47 | ### Greedy algorithm 48 | 49 | Again, run the gazebo simulation first. 50 | 51 | ``` 52 | roslaunch max_min_lp_simulation five_robot_twenty_target_case.launch 53 | ``` 54 | Then, run the main algorithm. 55 | 56 | ``` 57 | roslaunch max_min_lp_simulation greedy_server_to_robots.launch 58 | ``` 59 | 60 | To initiate the algorithm, 61 | ``` 62 | rostopic pub /robot_status std_msgs/String run 63 | ``` 64 | 65 | 66 | ## Generating and testing motion primitives of a robot 67 | 68 | In this package, which is max_min_lp_simulation, turtlebot is used as a mobile platform. This feature allows to generate and test different sets of motion primitives of a robot while changing `cmd_vel` values with different time intervals. In our setting time interval is defined by the number of publishers applied to. First, run the following command. 69 | 70 | ``` 71 | roslaunch max_min_lp_simulation single_robot_case.launch 72 | ``` 73 | 74 | This will run the gazebo simulator with a single robot. 75 | 76 | ``` 77 | rosrun max_min_lp_simulation motion_primitive_gerator 78 | ``` 79 | 80 | Now you can feed `/robot/status` rostopic in order to move the robot. In the following the first command moves the robot `x direction` and the second one rotates the robot in the counter-clockwise. In the gazebo simulator the positive values for `x` is the forward direction to the robot while the positive values for `z` rotates the robot in the counter-clockwise. 81 | 82 | ``` 83 | rostopic pub /robot_status std_msgs/String x 84 | rostopic pub /robot_status std_msgs/String z 85 | ``` 86 | 87 | You can also check the odometry information of the robot by using `rostopic echo /robot_1/odom`. 88 | -------------------------------------------------------------------------------- /max_min_lp_simulation/launch/three_robot_four_target_case_failure_case.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 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | -------------------------------------------------------------------------------- /max_min_lp_simulation/launch/two_robot_three_target_case.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 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | -------------------------------------------------------------------------------- /max_min_lp_simulation/include/max_min_lp_simulation/MaxMinLPRobotNodeSimulation.hpp: -------------------------------------------------------------------------------- 1 | #ifndef MAXMINLPROBOTNODESIMULATION_HPP_ 2 | #define MAXMINLPROBOTNODESIMULATION_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | using namespace std; 35 | 36 | class MaxMinLPRobotNodeSimulation { 37 | private: 38 | ros::NodeHandle m_nh; 39 | ros::NodeHandle m_private_nh; 40 | 41 | ros::Subscriber request_sub; 42 | 43 | // Publishers 44 | ros::Publisher m_general_node_pub; 45 | ros::Publisher m_layered_node_pub; 46 | ros::Publisher m_response_to_server_pub; 47 | ros::Publisher m_cmd_vel_robot_pub; 48 | 49 | // Subscriber 50 | ros::Subscriber m_odom_sub; 51 | 52 | // Clients 53 | ros::ServiceClient m_client; 54 | ros::ServiceClient m_primitive_client; 55 | ros::ServiceClient m_odom_client; 56 | ros::ServiceClient m_move_client; 57 | 58 | // Variables from the launch file 59 | int m_num_robot; 60 | int m_num_target; 61 | int m_robot_id; 62 | int m_num_layer; // Number of layers in the layered model 63 | int m_num_motion_primitive; 64 | int m_time_interval; 65 | bool m_verbal_flag; 66 | double m_epsilon; 67 | string m_robot_name; 68 | 69 | // Optimal motion primitive that is applied at the end of algorithm at each time 70 | int m_count_time_interval; 71 | int m_selected_primitive_id; 72 | vector m_motion_case_rotation; 73 | vector m_motion_primitive_pose; 74 | vector m_check_rotation_direction; 75 | 76 | // Robot info 77 | geometry_msgs::Pose m_pos; 78 | 79 | // For reduction 80 | vector m_constraint_value; 81 | 82 | // Local info 83 | int m_max_neighbor_hop; 84 | vector m_num_neighbors_at_each_hop; 85 | vector m_num_new_targets_at_each_hop; 86 | 87 | // ROBOT robot 88 | vector m_ROBOT_num_robot; 89 | vector m_prev_accumulate_robot; 90 | int m_num_survived_robot; 91 | 92 | // ROBOT motion primitives 93 | vector m_ROBOT_num_motion_primitive; 94 | vector m_prev_accumulate_motion_primitive; 95 | int m_num_survived_motion_primitive; 96 | 97 | vector m_gen_r_node; 98 | vector m_gen_p_r_node; 99 | vector m_gen_p_t_node; 100 | vector m_gen_t_node; 101 | 102 | // For debugging purpose 103 | int m_count_initialize_func; 104 | 105 | ofstream m_robot_outputFile; 106 | 107 | bool m_check_start; 108 | 109 | int count_computeMotionPrimitives; 110 | int * m_random_number_1; 111 | int * m_random_number_2; 112 | int * m_random_number_3; 113 | int * m_random_number_4; 114 | int * m_random_number_5; 115 | 116 | public: 117 | MaxMinLPRobotNodeSimulation(); // Constructor 118 | ~MaxMinLPRobotNodeSimulation() { 119 | m_robot_outputFile.close(); 120 | delete[] m_random_number_1; 121 | delete[] m_random_number_2; 122 | delete[] m_random_number_3; 123 | delete[] m_random_number_4; 124 | delete[] m_random_number_5; 125 | } 126 | 127 | void updateOdom(const gazebo_msgs::ModelStates::ConstPtr& msg); // Update odometry information by subscribing to /robot/odom 128 | bool initialize(); 129 | bool getMotionPrimitive(); 130 | void applyMotionPrimitives(const std_msgs::String::ConstPtr& msg); 131 | vector computeMotionPrimitives(); 132 | }; 133 | 134 | #endif -------------------------------------------------------------------------------- /max_min_lp_experiment/src/GreedyRobotNode.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Each robot node for the greedy algorithm in experiment. 3 | * \Author Yoonchang Sung 4 | * \04/14/2018 5 | * Copyright 2018. All Rights Reserved. 6 | */ 7 | 8 | #include "max_min_lp_experiment/GreedyRobotNode.hpp" 9 | 10 | GreedyRobotNode::GreedyRobotNode() : 11 | m_robot_id(1), m_num_motion_primitive(10), m_time_period(5), m_robot_velocity(0.1), 12 | m_robot_angular_vel(0.1), m_private_nh("~") 13 | { 14 | m_private_nh.getParam("robot_id", m_robot_id); 15 | m_private_nh.getParam("num_motion_primitive", m_num_motion_primitive); 16 | m_private_nh.getParam("time_period", m_time_period); 17 | m_private_nh.getParam("robot_velocity", m_robot_velocity); 18 | m_private_nh.getParam("robot_angular_vel", m_robot_angular_vel); 19 | 20 | // Publishers 21 | m_after_motion_pub = m_nh.advertise("/robot_after_motion", 1); 22 | m_cmd_vel_pub = m_nh.advertise("/RosAria/cmd_vel", 1); 23 | // Subscribers 24 | m_response_sub = m_nh.subscribe("/center_response", 10, &GreedyRobotNode::applyMotionPrimitives, this); 25 | m_pose_sub = m_nh.subscribe("/RosAria/pose", 10, &GreedyRobotNode::getRobotPose, this); 26 | 27 | m_count_robotInitialize_activate = 0; 28 | } 29 | 30 | void GreedyRobotNode::applyMotionPrimitives(const std_msgs::String::ConstPtr& msg) { 31 | // Orienteering. 32 | double init_time = ros::Time::now().toSec(); 33 | double cur_time = ros::Time::now().toSec(); 34 | int time_for_orient = 2; 35 | int time_for_translate = 2; 36 | 37 | while (cur_time-init_time > time_for_orient) { 38 | cur_time = ros::Time::now().toSec(); 39 | 40 | geometry_msgs::Twist cmd_vel; 41 | cmd_vel.linear.x = 0; cmd_vel.linear.y = 0; cmd_vel.linear.z = 0; 42 | cmd_vel.angular.x = 0; cmd_vel.angular.y = 0; cmd_vel.linear.z = m_robot_angular_vel; 43 | m_cmd_vel_pub.publish(cmd_vel); 44 | } 45 | 46 | // Translating. 47 | init_time = ros::Time::now().toSec(); 48 | cur_time = ros::Time::now().toSec(); 49 | 50 | while (cur_time-init_time > time_for_translate) { 51 | cur_time = ros::Time::now().toSec(); 52 | 53 | geometry_msgs::Twist cmd_vel; 54 | cmd_vel.linear.x = m_robot_velocity; cmd_vel.linear.y = 0; cmd_vel.linear.z = 0; 55 | cmd_vel.angular.x = 0; cmd_vel.angular.y = 0; cmd_vel.linear.z = 0; 56 | m_cmd_vel_pub.publish(cmd_vel); 57 | } 58 | 59 | // bool result_success = robotInitialize(); // Initialization through the central node. 60 | 61 | // if (result_success) { 62 | // std_msgs::String msg; 63 | // stringstream ss; 64 | // ss<<"action is applied"; 65 | // msg.data = ss.str(); 66 | // m_after_motion_pub.publish(msg); 67 | // } 68 | } 69 | 70 | bool GreedyRobotNode::robotInitialize() { 71 | m_request_client = m_nh.serviceClient("/robot_request"); 72 | max_min_lp_experiment::RobotRequest srv; 73 | srv.request.robot_id = m_robot_id; 74 | srv.request.robot_pose = m_robot_pose; 75 | srv.request.state_request = "ready"; 76 | 77 | // Compute motion primivites. 78 | if (m_count_robotInitialize_activate == 0) { 79 | m_motion_primitive_pose.clear(); 80 | m_motion_primitive_pose = computeMotionPrimitives(); 81 | } 82 | 83 | if (m_request_client.call(srv)) { 84 | if (strcmp(srv.response.state_answer.c_str(), "start") == 0) { 85 | // Obtain local information from the central node. 86 | srv.response.target_id; 87 | srv.response.target_pose; 88 | srv.response.comm_robot_id; 89 | 90 | m_count_robotInitialize_activate = 0; 91 | return true; 92 | } 93 | else { 94 | m_count_robotInitialize_activate += 1; 95 | return GreedyRobotNode::robotInitialize(); 96 | } 97 | } 98 | else { 99 | ROS_INFO("Fail to communicate with the server."); 100 | return false; 101 | } 102 | } 103 | 104 | vector GreedyRobotNode::computeMotionPrimitives() { 105 | vector motion_primitive; 106 | motion_primitive.push_back(m_robot_pose); // First motion primitive is to stay in the same position. 107 | 108 | // if ((m_num_motion_primitive-1) % 2 == 0) { // Even number. 109 | // (m_num_motion_primitive-1)/2 110 | // } 111 | // else { // Odd number. 112 | 113 | // } 114 | 115 | return motion_primitive; 116 | } 117 | 118 | void GreedyRobotNode::getRobotPose(const nav_msgs::Odometry::ConstPtr& msg) { 119 | m_robot_pose.x = msg->pose.pose.position.x; 120 | m_robot_pose.y = msg->pose.pose.position.y; 121 | m_robot_pose.z = msg->pose.pose.position.z; 122 | } 123 | 124 | int main(int argc, char **argv) { 125 | ros::init(argc, argv, "robot_node_greedy"); 126 | GreedyRobotNode rn; 127 | 128 | ros::spin(); 129 | return 0; 130 | } 131 | -------------------------------------------------------------------------------- /max_min_lp_simulation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(max_min_lp_simulation) 3 | 4 | SET(CMAKE_CXX_FLAGS "-std=c++0x") 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | roscpp 8 | std_msgs 9 | max_min_lp_msgs 10 | max_min_lp_core 11 | genmsg 12 | tf 13 | gazebo_msgs 14 | gazebo_ros 15 | ) 16 | 17 | find_package(gazebo REQUIRED) 18 | find_package(Eigen3 REQUIRED) 19 | 20 | add_service_files( 21 | FILES 22 | MessageRequest.srv 23 | MotionPrimitiveRequest.srv 24 | GetOdom.srv 25 | MoveRobot.srv 26 | GetTotalNumTarget.srv 27 | ) 28 | 29 | generate_messages( 30 | DEPENDENCIES 31 | std_msgs 32 | geometry_msgs 33 | max_min_lp_msgs 34 | ) 35 | 36 | catkin_package( 37 | INCLUDE_DIRS include 38 | LIBRARIES max_min_lp_simulation 39 | CATKIN_DEPENDS roscpp std_msgs max_min_lp_msgs max_min_lp_core message_runtime 40 | # DEPENDS Eigen3 41 | ) 42 | 43 | include_directories( 44 | include 45 | ${catkin_INCLUDE_DIRS} 46 | ${GAZEBO_INCLUDE_DIRS} 47 | ${EIGEN_INCLUDE_DIRS} 48 | ) 49 | 50 | link_directories( 51 | ${GAZEBO_LIBRARY_DIRS} 52 | ) 53 | 54 | add_library(kalman src/kalman.cpp) 55 | target_link_libraries(kalman ${catkin_LIBRARIES}) 56 | 57 | add_library(animated_box SHARED src/animated_box.cc) 58 | target_link_libraries(animated_box ${GAZEBO_LIBRARIES}) 59 | 60 | add_library(animated_box_journal SHARED src/animated_box_journal.cc) 61 | target_link_libraries(animated_box_journal ${GAZEBO_LIBRARIES}) 62 | 63 | add_executable(motion_primitive_generator src/motion_primitive_generator.cpp) 64 | target_link_libraries(motion_primitive_generator ${catkin_LIBRARIES}) 65 | 66 | add_executable(max_min_lp_simulation_central_node src/MaxMinLPCentralNode.cpp) 67 | target_link_libraries(max_min_lp_simulation_central_node ${catkin_LIBRARIES}) 68 | add_dependencies(max_min_lp_simulation_central_node max_min_lp_simulation_gencpp) 69 | add_executable(max_min_lp_simulation_robot_node src/MaxMinLPRobotNode.cpp) 70 | target_link_libraries(max_min_lp_simulation_robot_node ${catkin_LIBRARIES}) 71 | add_dependencies(max_min_lp_simulation_robot_node max_min_lp_simulation_gencpp) 72 | 73 | add_executable(max_min_lp_simulation_greedy_central_node src/MaxMinLPGreedyCentralNode.cpp) 74 | target_link_libraries(max_min_lp_simulation_greedy_central_node ${catkin_LIBRARIES}) 75 | add_dependencies(max_min_lp_simulation_greedy_central_node max_min_lp_simulation_gencpp) 76 | add_executable(max_min_lp_simulation_greedy_robot_node src/MaxMinLPGreedyRobotNode.cpp) 77 | target_link_libraries(max_min_lp_simulation_greedy_robot_node ${catkin_LIBRARIES}) 78 | add_dependencies(max_min_lp_simulation_greedy_robot_node max_min_lp_simulation_gencpp) 79 | 80 | add_executable(max_min_lp_simulation_central_node_simulation src/MaxMinLPCentralNodeSimulation.cpp) 81 | target_link_libraries(max_min_lp_simulation_central_node_simulation ${catkin_LIBRARIES}) 82 | add_dependencies(max_min_lp_simulation_central_node_simulation max_min_lp_simulation_gencpp) 83 | add_executable(max_min_lp_simulation_robot_node_simulation src/MaxMinLPRobotNodeSimulation.cpp) 84 | target_link_libraries(max_min_lp_simulation_robot_node_simulation ${catkin_LIBRARIES}) 85 | add_dependencies(max_min_lp_simulation_robot_node_simulation max_min_lp_simulation_gencpp) 86 | 87 | add_executable(apply_motion_primitive_node src/apply_motion_primitive.cpp) 88 | target_link_libraries(apply_motion_primitive_node ${catkin_LIBRARIES}) 89 | add_dependencies(apply_motion_primitive_node max_min_lp_simulation_gencpp) 90 | 91 | add_executable(get_odom_node src/get_odom.cpp) 92 | target_link_libraries(get_odom_node ${catkin_LIBRARIES}) 93 | add_dependencies(get_odom_node max_min_lp_simulation_gencpp) 94 | 95 | add_executable(get_target_odom_node src/get_target_odom.cpp) 96 | target_link_libraries(get_target_odom_node ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES} pthread) 97 | add_dependencies(get_target_odom_node max_min_lp_simulation_gencpp) 98 | 99 | add_executable(get_total_num_target_node src/get_result_target_num.cpp) 100 | target_link_libraries(get_total_num_target_node ${catkin_LIBRARIES}) 101 | add_dependencies(get_total_num_target_node max_min_lp_simulation_gencpp) 102 | 103 | # Journal-related 104 | add_executable(max_min_lp_simulation_robot_journal src/RobotGreedyJournal.cpp) 105 | target_link_libraries(max_min_lp_simulation_robot_journal kalman ${catkin_LIBRARIES}) 106 | add_executable(get_target_odom_journal_node src/get_target_odom_journal.cpp) 107 | target_link_libraries(get_target_odom_journal_node ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES} pthread) 108 | add_dependencies(get_target_odom_journal_node max_min_lp_simulation_gencpp) 109 | add_executable(apply_motion_primitive_journal src/apply_motion_primitive_journal.cpp) 110 | target_link_libraries(apply_motion_primitive_journal ${catkin_LIBRARIES}) 111 | add_dependencies(apply_motion_primitive_journal max_min_lp_simulation_gencpp) -------------------------------------------------------------------------------- /max_min_lp_core/include/max_min_lp_core/MaxMinLPCore.hpp: -------------------------------------------------------------------------------- 1 | #ifndef MAXMINLPCORE_HPP_ 2 | #define MAXMINLPCORE_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | using namespace std; 19 | 20 | namespace max_min_lp_core { 21 | 22 | struct TreeStruct { 23 | vector< vector > robot_node_id; 24 | vector< vector > red_node_id; 25 | vector< vector > blue_node_id; 26 | vector< vector > target_node_id; 27 | int tree_depth; 28 | }; 29 | 30 | class LayeredClass { 31 | private: 32 | string m_current; 33 | int m_layer; 34 | string m_state; 35 | public: 36 | LayeredClass(string _current, int _layer, string _state) : 37 | m_current(_current), m_layer(_layer), m_state(_state) {} 38 | 39 | bool operator<(const LayeredClass &right) const { 40 | if (m_current == right.m_current) { 41 | if (m_layer == right.m_layer) { 42 | return m_state < right.m_state; 43 | } 44 | else { 45 | return m_layer < right.m_layer; 46 | } 47 | } 48 | else { 49 | return m_current < right.m_current; 50 | } 51 | } 52 | }; 53 | 54 | class MaxMinLPCore { 55 | private: 56 | // General node values 57 | vector m_gen_r_node; 58 | vector m_gen_p_r_node; 59 | vector m_gen_p_t_node; 60 | vector m_gen_t_node; 61 | 62 | // Layered node values 63 | vector m_lay_robot_node; 64 | vector m_lay_red_node; 65 | vector m_lay_blue_node; 66 | vector m_lay_target_node; 67 | 68 | map m_layered_map; 69 | 70 | TreeStruct * m_red_tree; 71 | 72 | int m_num_red_layer_zero; 73 | 74 | // Variables from the launch file 75 | int m_num_layer; // Number of layers in the layered model 76 | bool m_verbal_flag; 77 | double m_epsilon; 78 | 79 | public: 80 | // Constructor 81 | MaxMinLPCore(); 82 | MaxMinLPCore(vector& _gen_r_node, vector& _gen_p_r_node, 83 | vector& _gen_p_t_node, vector& _gen_t_node, 84 | int _num_layer, bool _verbal_flag, double _epsilon); 85 | // Destructor 86 | ~MaxMinLPCore() { 87 | delete[] m_red_tree; 88 | } 89 | 90 | void convertLayeredMaxMinLP(); // Converting the general graph into the layered graph. Step 2 91 | void convertDecentralizedLayeredMaxMinLP(); // // Converting the general graph into the layered graph for the decentralized approach. 92 | vector applyLocalAlgorithm(); // Step 3 93 | 94 | map::iterator getMapPointer(string _current, int _layer, string _state); 95 | void getRedTreeStruct(TreeStruct * _red_tree, string _current, int _layer, string _state); 96 | bool computeRecursive(int _count_red_layer_zero, float _minimum_g_t ); 97 | 98 | int getNodeID(const string & s) { 99 | string return_str; 100 | if (isInteger(boost::lexical_cast(s.at(4)))) { // Greater than or equal to 100 101 | return_str = boost::lexical_cast(s.at(2))+boost::lexical_cast(s.at(3))+boost::lexical_cast(s.at(4)); 102 | } 103 | else { // Less than 100 104 | if (isInteger(boost::lexical_cast(s.at(3)))) { // Greater than or equal to 10 105 | return_str = boost::lexical_cast(s.at(2))+boost::lexical_cast(s.at(3)); 106 | } 107 | else { // Less than 10 108 | return_str = boost::lexical_cast(s.at(2)); 109 | } 110 | } 111 | 112 | return boost::lexical_cast(return_str); 113 | } 114 | inline bool isInteger(const string & s) { 115 | if(s.empty() || ((!isdigit(s[0])) && (s[0] != '-') && (s[0] != '+'))) return false ; 116 | 117 | char * p ; 118 | strtol(s.c_str(), &p, 10) ; 119 | 120 | return (*p == 0) ; 121 | } 122 | 123 | vector getRobotLayeredNode() { 124 | return m_lay_robot_node; 125 | } 126 | vector getRedLayeredNode() { 127 | return m_lay_red_node; 128 | } 129 | vector getBlueLayeredNode() { 130 | return m_lay_blue_node; 131 | } 132 | vector getTargetLayeredNode() { 133 | return m_lay_target_node; 134 | } 135 | }; 136 | 137 | } // namespace 138 | 139 | #endif -------------------------------------------------------------------------------- /max_min_lp_simulation/src/apply_motion_primitive_journal.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Central node for the simulation 3 | * Central node only links between a set of motion primitives and a set of targets. 4 | * Important note: in this code a robot is used to take into consideration of the reduction of the step 1 of local algorithm. 5 | * Thus, a robot here does not indicate a real robot but an element of a real robot. Instead, ROBOT is used for a real robot. 6 | * \Author Yoonchang Sung 7 | * \02/27/2017 8 | * Copyright 2017. All Rights Reserved. 9 | */ 10 | 11 | #include "max_min_lp_simulation/apply_motion_primitive_journal.hpp" 12 | 13 | #define PHI 3.141592 14 | 15 | apply_motion_primitive::apply_motion_primitive() : 16 | m_num_robot(1), m_num_target(1), m_robot_id(1), m_robot_name(string("robot_0")), m_private_nh("~") 17 | { 18 | m_private_nh.getParam("num_robot", m_num_robot); 19 | m_private_nh.getParam("num_target", m_num_target); 20 | m_private_nh.getParam("robot_id", m_robot_id); 21 | m_private_nh.getParam("robot_name", m_robot_name); 22 | 23 | m_cmd_vel_robot_pub = m_nh.advertise("/robot_"+boost::lexical_cast(m_robot_id)+"/cmd_vel_mux/input/teleop", 1); 24 | m_move_service = m_nh.advertiseService("/robot_"+boost::lexical_cast(m_robot_id)+"/move_request", &apply_motion_primitive::move_robot, this); 25 | } 26 | 27 | bool apply_motion_primitive::move_robot(max_min_lp_simulation::MoveRobot::Request &req, max_min_lp_simulation::MoveRobot::Response &res) { 28 | m_check_rotation_direction = req.rotation_direction; 29 | m_trans_duration = req.trans_duration; 30 | m_ang_duration = req.ang_duration; 31 | m_trans_speed = req.trans_speed; 32 | m_ang_speed = req.ang_speed; 33 | 34 | ROS_INFO("Robot %d trans_duration = %d, ang_duration = %d, trans_speed = %.2f, ang_speed = %.2f", m_robot_id, m_trans_duration, m_ang_duration, m_trans_speed, m_ang_speed); 35 | ROS_INFO("Robot %d rotation direction = %d", m_robot_id, m_check_rotation_direction); 36 | 37 | double cur_time =ros::Time::now().toSec(); 38 | double prev_time =ros::Time::now().toSec(); 39 | 40 | // m_goal_pos.orientation.w 41 | while (1) { 42 | cur_time =ros::Time::now().toSec(); 43 | geometry_msgs::Twist cmd_vel_msg; 44 | 45 | if (m_check_rotation_direction == 1) { // CW 46 | cmd_vel_msg.linear.x = 0; 47 | cmd_vel_msg.linear.y = 0; 48 | cmd_vel_msg.linear.z = 0; 49 | cmd_vel_msg.angular.x = 0; 50 | cmd_vel_msg.angular.y = 0; 51 | cmd_vel_msg.angular.z = m_ang_speed; 52 | 53 | m_cmd_vel_robot_pub.publish(cmd_vel_msg); 54 | } 55 | else if (m_check_rotation_direction == -1) { // CCW 56 | cmd_vel_msg.linear.x = 0; 57 | cmd_vel_msg.linear.y = 0; 58 | cmd_vel_msg.linear.z = 0; 59 | cmd_vel_msg.angular.x = 0; 60 | cmd_vel_msg.angular.y = 0; 61 | cmd_vel_msg.angular.z = -1*m_ang_speed; 62 | 63 | m_cmd_vel_robot_pub.publish(cmd_vel_msg); 64 | } 65 | else { // Remaining stationary 66 | break; 67 | } 68 | if (cur_time-prev_time > m_ang_duration) { 69 | break; 70 | } 71 | } 72 | 73 | cur_time =ros::Time::now().toSec(); 74 | prev_time =ros::Time::now().toSec(); 75 | 76 | // m_goal_pos.position.x, m_goal_pos.position.y 77 | while (1) { 78 | cur_time =ros::Time::now().toSec(); 79 | geometry_msgs::Twist cmd_vel_msg; 80 | 81 | cmd_vel_msg.linear.x = m_trans_speed; 82 | cmd_vel_msg.linear.y = 0; 83 | cmd_vel_msg.linear.z = 0; 84 | cmd_vel_msg.angular.x = 0; 85 | cmd_vel_msg.angular.y = 0; 86 | cmd_vel_msg.angular.z = 0; 87 | 88 | m_cmd_vel_robot_pub.publish(cmd_vel_msg); 89 | 90 | if (cur_time-prev_time > m_trans_duration) { 91 | break; 92 | } 93 | } 94 | 95 | geometry_msgs::Twist cmd_vel_msg; 96 | 97 | cmd_vel_msg.linear.x = 0; 98 | cmd_vel_msg.linear.y = 0; 99 | cmd_vel_msg.linear.z = 0; 100 | cmd_vel_msg.angular.x = 0; 101 | cmd_vel_msg.angular.y = 0; 102 | cmd_vel_msg.angular.z = 0; 103 | 104 | m_cmd_vel_robot_pub.publish(cmd_vel_msg); 105 | 106 | m_odom_client = m_nh.serviceClient("/robot_"+boost::lexical_cast(m_robot_id)+"/odom_request", true); 107 | max_min_lp_simulation::GetOdom srv; 108 | srv.request.request_odom = string("request"); 109 | 110 | if (m_odom_client.call(srv)) { 111 | geometry_msgs::Pose temp_pos; 112 | temp_pos = srv.response.return_odom; 113 | 114 | tf::Quaternion q(temp_pos.orientation.x, temp_pos.orientation.y, temp_pos.orientation.z, temp_pos.orientation.w); 115 | tf::Matrix3x3 m(q); 116 | double roll, pitch, yaw; 117 | m.getRPY(roll, pitch, yaw); 118 | yaw = yaw * 180 / PHI; 119 | ROS_INFO("Robot %d : (%.2f, %.2f, %.2f)", m_robot_id, temp_pos.position.x, temp_pos.position.y, yaw); 120 | res.answer_msg = string("success"); 121 | 122 | return true; 123 | } 124 | else { 125 | return false; 126 | } 127 | 128 | // ROS_INFO(" "); 129 | // ROS_INFO("(Apply_motion_primitive) ROBOT %d : (%.2f, %.2f)", m_robot_id, temp_pos.position.x, temp_pos.position.y); 130 | } 131 | 132 | int main(int argc, char **argv) { 133 | ros::init(argc, argv, "apply_motion_primitive_journal"); 134 | 135 | apply_motion_primitive amp; 136 | 137 | ros::spin(); 138 | return 0; 139 | } -------------------------------------------------------------------------------- /max_min_lp_core/include/max_min_lp_core/MaxMinLPSequentialCore.hpp: -------------------------------------------------------------------------------- 1 | #ifndef MAXMINLPSEQUENTIALCORE_HPP_ 2 | #define MAXMINLPSEQUENTIALCORE_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | using namespace std; 19 | 20 | namespace max_min_lp_core { 21 | 22 | struct TreeStruct { 23 | vector< vector > robot_node_id; 24 | vector< vector > red_node_id; 25 | vector< vector > blue_node_id; 26 | vector< vector > target_node_id; 27 | int tree_depth; 28 | }; 29 | 30 | class LayeredClass { 31 | private: 32 | string m_current; 33 | int m_layer; 34 | string m_state; 35 | public: 36 | LayeredClass(string _current, int _layer, string _state) : 37 | m_current(_current), m_layer(_layer), m_state(_state) {} 38 | 39 | bool operator<(const LayeredClass &right) const { 40 | if (m_current == right.m_current) { 41 | if (m_layer == right.m_layer) { 42 | return m_state < right.m_state; 43 | } 44 | else { 45 | return m_layer < right.m_layer; 46 | } 47 | } 48 | else { 49 | return m_current < right.m_current; 50 | } 51 | } 52 | }; 53 | 54 | class MaxMinLPSequentialCore { 55 | private: 56 | // General node values 57 | vector m_gen_r_node; 58 | vector m_gen_p_r_node; 59 | vector m_gen_p_t_node; 60 | vector m_gen_t_node; 61 | 62 | // Layered node values 63 | vector m_lay_robot_node; 64 | vector m_lay_red_node; 65 | vector m_lay_blue_node; 66 | vector m_lay_target_node; 67 | 68 | map m_layered_map; 69 | 70 | TreeStruct * m_red_tree; 71 | 72 | int m_num_red_layer_zero; 73 | 74 | //// Variables from the launch file 75 | int m_num_layer; // Number of layers in the layered model 76 | bool m_verbal_flag; 77 | double m_epsilon; 78 | int m_max_neighbor_hop; 79 | vector m_num_neighbors_at_each_hop; 80 | 81 | // ROBOT robot 82 | vector m_ROBOT_num_robot; 83 | vector m_prev_accumulate_robot; 84 | int m_num_survived_robot; 85 | 86 | // ROBOT motion primitives 87 | vector m_ROBOT_num_motion_primitive; 88 | vector m_prev_accumulate_motion_primitive; 89 | int m_num_survived_motion_primitive; 90 | 91 | vector m_constraint_value; 92 | 93 | public: 94 | // Constructor 95 | MaxMinLPSequentialCore(); 96 | MaxMinLPSequentialCore(vector& _gen_r_node, vector& _gen_p_r_node, 97 | vector& _gen_p_t_node, vector& _gen_t_node, int _num_layer, 98 | bool _verbal_flag, double _epsilon, vector _ROBOT_num_robot, vector _prev_accumulate_robot, int _num_survived_robot, 99 | vector _ROBOT_num_motion_primitive, vector _prev_accumulate_motion_primitive, int _num_survived_motion_primitive, 100 | vector _constraint_value); 101 | // Destructor 102 | ~MaxMinLPSequentialCore() { 103 | // delete[] m_red_tree; 104 | } 105 | 106 | void convertSequentialLayeredMaxMinLP(); // Converting the general graph into the layered graph. Step 2 107 | void applyLocalAlgorithm(); // Step 3 108 | 109 | map::iterator getMapPointer(string _current, int _layer, string _state); 110 | void getRedTreeStruct(TreeStruct * _red_tree, string _current, int _layer, string _state); 111 | bool computeRecursive(int _count_red_layer_zero, float _minimum_g_t ); 112 | 113 | int getNodeID(const string & s) { 114 | string return_str; 115 | if (isInteger(boost::lexical_cast(s.at(3)))) { // Greater than or equal to 10 116 | return_str = boost::lexical_cast(s.at(2))+boost::lexical_cast(s.at(3)); 117 | } 118 | else { // Less than 10 119 | return_str = boost::lexical_cast(s.at(2)); 120 | } 121 | return boost::lexical_cast(return_str); 122 | } 123 | inline bool isInteger(const string & s) { 124 | if(s.empty() || ((!isdigit(s[0])) && (s[0] != '-') && (s[0] != '+'))) return false ; 125 | 126 | char * p ; 127 | strtol(s.c_str(), &p, 10) ; 128 | 129 | return (*p == 0) ; 130 | } 131 | 132 | vector getXValues() { 133 | return m_gen_p_r_node; 134 | } 135 | 136 | vector getRobotLayeredNode() { 137 | return m_lay_robot_node; 138 | } 139 | vector getRedLayeredNode() { 140 | return m_lay_red_node; 141 | } 142 | vector getBlueLayeredNode() { 143 | return m_lay_blue_node; 144 | } 145 | vector getTargetLayeredNode() { 146 | return m_lay_target_node; 147 | } 148 | }; 149 | 150 | } // namespace 151 | 152 | #endif -------------------------------------------------------------------------------- /max_min_lp_core/include/max_min_lp_core/MaxMinLPDecentralizedCore.hpp: -------------------------------------------------------------------------------- 1 | #ifndef MAXMINLPDECENTRALIZEDCORE_HPP_ 2 | #define MAXMINLPDECENTRALIZEDCORE_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | using namespace std; 19 | 20 | namespace max_min_lp_core { 21 | 22 | struct TreeStruct { 23 | vector< vector > robot_node_id; 24 | vector< vector > red_node_id; 25 | vector< vector > blue_node_id; 26 | vector< vector > target_node_id; 27 | int tree_depth; 28 | }; 29 | 30 | class LayeredClass { 31 | private: 32 | string m_current; 33 | int m_layer; 34 | string m_state; 35 | public: 36 | LayeredClass(string _current, int _layer, string _state) : 37 | m_current(_current), m_layer(_layer), m_state(_state) {} 38 | 39 | bool operator<(const LayeredClass &right) const { 40 | if (m_current == right.m_current) { 41 | if (m_layer == right.m_layer) { 42 | return m_state < right.m_state; 43 | } 44 | else { 45 | return m_layer < right.m_layer; 46 | } 47 | } 48 | else { 49 | return m_current < right.m_current; 50 | } 51 | } 52 | }; 53 | 54 | class MaxMinLPDecentralizedCore { 55 | private: 56 | // General node values 57 | vector m_gen_r_node; 58 | vector m_gen_p_r_node; 59 | vector m_gen_p_t_node; 60 | vector m_gen_t_node; 61 | 62 | // Layered node values 63 | vector m_lay_robot_node; 64 | vector m_lay_red_node; 65 | vector m_lay_blue_node; 66 | vector m_lay_target_node; 67 | 68 | map m_layered_map; 69 | 70 | TreeStruct * m_red_tree; 71 | vector m_t_r; 72 | 73 | int m_num_red_layer_zero; 74 | 75 | //// Variables from the launch file 76 | int m_ROBOT_id; 77 | int m_num_layer; // Number of layers in the layered model 78 | bool m_verbal_flag; 79 | double m_epsilon; 80 | int m_max_neighbor_hop; 81 | vector m_num_neighbors_at_each_hop; 82 | 83 | // ROBOT robot 84 | vector m_ROBOT_num_robot; 85 | vector m_prev_accumulate_robot; 86 | int m_num_survived_robot; 87 | 88 | // ROBOT motion primitives 89 | vector m_ROBOT_num_motion_primitive; 90 | vector m_prev_accumulate_motion_primitive; 91 | int m_num_survived_motion_primitive; 92 | 93 | vector m_constraint_value; 94 | 95 | public: 96 | // Constructor 97 | MaxMinLPDecentralizedCore(); 98 | MaxMinLPDecentralizedCore(int _ROBOT_id, vector& _gen_r_node, vector& _gen_p_r_node, 99 | vector& _gen_p_t_node, vector& _gen_t_node, 100 | int _num_layer, bool _verbal_flag, double _epsilon, int _max_neighbor_hop, vector _num_neighbors_at_each_hop, vector _ROBOT_num_robot, 101 | vector _prev_accumulate_robot, int _num_survived_robot, vector _ROBOT_num_motion_primitive, vector _prev_accumulate_motion_primitive, 102 | int _num_survived_motion_primitive, vector _constraint_value); 103 | // Destructor 104 | ~MaxMinLPDecentralizedCore() { 105 | // delete[] m_red_tree; 106 | } 107 | 108 | void convertDecentralizedLayeredMaxMinLP(); // Converting the general graph into the layered graph for the decentralized approach. (Step 2) 109 | void applyLocalAlgorithmPhase1and2(); // Phase 1 and 2 of Step 3 110 | void applyLocalAlgorithmPhase3(); // Phase 3 of Step 3 111 | 112 | map::iterator getMapPointer(string _current, int _layer, string _state); 113 | void getRedTreeStruct(TreeStruct * _red_tree, string _current, int _layer, string _state); 114 | bool computeRecursive(int _count_red_layer_zero, float _minimum_g_t ); 115 | 116 | int getNodeID(const string & s) { 117 | string return_str; 118 | if (isInteger(boost::lexical_cast(s.at(3)))) { // Greater than or equal to 10 119 | return_str = boost::lexical_cast(s.at(2))+boost::lexical_cast(s.at(3)); 120 | } 121 | else { // Less than 10 122 | return_str = boost::lexical_cast(s.at(2)); 123 | } 124 | return boost::lexical_cast(return_str); 125 | } 126 | inline bool isInteger(const string & s) { 127 | if(s.empty() || ((!isdigit(s[0])) && (s[0] != '-') && (s[0] != '+'))) return false ; 128 | 129 | char * p ; 130 | strtol(s.c_str(), &p, 10) ; 131 | 132 | return (*p == 0) ; 133 | } 134 | 135 | vector getRobotLayeredNode() { 136 | return m_lay_robot_node; 137 | } 138 | vector getRedLayeredNode() { 139 | return m_lay_red_node; 140 | } 141 | vector getBlueLayeredNode() { 142 | return m_lay_blue_node; 143 | } 144 | vector getTargetLayeredNode() { 145 | return m_lay_target_node; 146 | } 147 | }; 148 | 149 | } // namespace 150 | 151 | #endif -------------------------------------------------------------------------------- /max_min_lp_simulation/include/max_min_lp_simulation/MaxMinLPCentralNode.hpp: -------------------------------------------------------------------------------- 1 | #ifndef MAXMINLPCENTRALNODE_HPP_ 2 | #define MAXMINLPCENTRALNODE_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | using namespace std; 26 | 27 | class MaxMinLPCentralNode { 28 | private: 29 | ros::NodeHandle m_nh; 30 | ros::NodeHandle m_private_nh; 31 | 32 | // Publishers 33 | // ros::Publisher m_request_pub; 34 | // ros::Publisher m_general_node_pub; 35 | // ros::Publisher m_layered_node_pub; 36 | 37 | // Subscribers 38 | ros::Subscriber m_target_1_sub; 39 | ros::Subscriber m_target_2_sub; 40 | ros::Subscriber m_target_3_sub; 41 | ros::Subscriber m_target_4_sub; 42 | ros::Subscriber m_target_5_sub; 43 | ros::Subscriber m_target_6_sub; 44 | ros::Subscriber m_target_7_sub; 45 | ros::Subscriber m_target_8_sub; 46 | ros::Subscriber m_target_9_sub; 47 | ros::Subscriber m_target_10_sub; 48 | ros::Subscriber m_target_11_sub; 49 | ros::Subscriber m_target_12_sub; 50 | ros::Subscriber m_target_13_sub; 51 | ros::Subscriber m_target_14_sub; 52 | ros::Subscriber m_target_15_sub; 53 | ros::Subscriber m_target_16_sub; 54 | ros::Subscriber m_target_17_sub; 55 | ros::Subscriber m_target_18_sub; 56 | ros::Subscriber m_target_19_sub; 57 | ros::Subscriber m_target_20_sub; 58 | 59 | ros::Subscriber m_comm_graph_by_robots_sub; 60 | 61 | // Publishers 62 | ros::Publisher m_general_node_pub; 63 | ros::Publisher m_layered_node_pub; 64 | 65 | // Services 66 | ros::ServiceServer m_service; 67 | ros::ServiceServer m_primitive_service; 68 | 69 | // Variables from the launch file 70 | max_min_lp_msgs::server_to_robots_array m_robot_info; 71 | int m_num_robot; 72 | int m_num_target; 73 | int m_num_motion_primitive; 74 | int m_num_layer; 75 | int m_fov; 76 | double m_epsilon; 77 | string m_objective_option; 78 | bool m_verbal_flag; 79 | 80 | int m_send_robot_id; 81 | int m_request_robot_id; 82 | bool m_check_request_send; 83 | bool m_check_apply_sequential_send; 84 | 85 | bool m_ready_to_send; 86 | 87 | // For reduction 88 | vector m_constraint_value; // This value is used for reduction i.e., 2/|V_i|. 89 | 90 | //// ROBOT //// 91 | // ROBOT robot 92 | vector m_ROBOT_num_robot; 93 | vector m_prev_accumulate_robot; 94 | int m_num_survived_robot; 95 | 96 | // ROBOT motion primitives 97 | vector m_ROBOT_num_motion_primitive; 98 | vector m_prev_accumulate_motion_primitive; 99 | int m_num_survived_motion_primitive; 100 | 101 | // ROBOT neighbor hop info 102 | vector > > m_ROBOT_neighbor; 103 | vector > m_ROBOT_assign_targets; 104 | vector m_max_neighbor_hop; 105 | 106 | //// robot //// 107 | // robot info 108 | vector m_robot_id; 109 | 110 | //// motion primitives //// 111 | vector m_primitive_id; 112 | vector m_primitive_original_id; 113 | vector m_primitive_x_pos; 114 | vector m_primitive_y_pos; 115 | vector m_dist_primitive_to_target; 116 | 117 | //// target //// 118 | // Target info of each time-step obtained from gazebo 119 | vector m_temp_target_pos; 120 | vector m_temp_target_name; 121 | 122 | // Target info when consensus of robots is fulfilled 123 | vector m_target_id; 124 | vector m_target_x_pos; 125 | vector m_target_y_pos; 126 | vector m_target_observed; 127 | 128 | vector > m_robots_to_primitives; // Robots to motion primitives 129 | vector > m_primitives_to_robots; // Motion primitives to robots 130 | vector > m_primitives_to_targets; // Motion primitives to targets 131 | vector > m_targets_to_primitives; // Targets to motion primitives 132 | 133 | vector > m_primitives_to_targets_weight; // Weights of motion primitives to targets 134 | vector > m_targets_to_primitives_weight; // Weights of targets to motion primitives 135 | 136 | // Send optimal motion primitives to each robot 137 | vector m_optimal_primitive_id; 138 | 139 | // General node values 140 | vector m_gen_r_node; 141 | vector m_gen_p_r_node; 142 | vector m_gen_p_t_node; 143 | vector m_gen_t_node; 144 | 145 | public: 146 | MaxMinLPCentralNode(); // Constructor 147 | 148 | bool initialize(max_min_lp_simulation::MessageRequest::Request &req, max_min_lp_simulation::MessageRequest::Response &res); 149 | bool sendMotionPrimitive(max_min_lp_simulation::MotionPrimitiveRequest::Request &req, max_min_lp_simulation::MotionPrimitiveRequest::Response &res); 150 | void updatePose(const gazebo_msgs::ModelStates::ConstPtr& msg, int target_id); // Update pose information 151 | vector buildGeneralNode(string option); 152 | void applySequentialLocalAlgorithm(const std_msgs::String::ConstPtr& msg); 153 | }; 154 | 155 | #endif -------------------------------------------------------------------------------- /max_min_lp_experiment/src/GreedyCentralNode.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Central node for the experiment. 3 | * Central node only links between a set of motion primitives and a set of targets. 4 | * \Author Yoonchang Sung 5 | * \04/14/2018 6 | * Copyright 2018. All Rights Reserved. 7 | */ 8 | 9 | #include "max_min_lp_experiment/GreedyCentralNode.hpp" 10 | 11 | GreedyCentralNode::GreedyCentralNode() : 12 | m_num_robot(1), m_sensing_range(10), m_time_period(5), m_private_nh("~") 13 | { 14 | m_private_nh.getParam("num_robot", m_num_robot); 15 | m_private_nh.getParam("sensing_range", m_sensing_range); 16 | m_private_nh.getParam("time_period", m_time_period); 17 | 18 | // Services 19 | m_center_request_service = m_nh.advertiseService("/robot_request", &GreedyCentralNode::requestInitialize, this); 20 | // Publishers 21 | m_response_to_robot_pub = m_nh.advertise("/center_response", 1); 22 | // Subscribers 23 | m_comm_graph_by_robots_sub = m_nh.subscribe("/robot_after_motion", 10, &GreedyCentralNode::afterAppliedMotionPrimitive, this); 24 | 25 | m_request_robot_id = 0; 26 | m_send_robot_id = 0; 27 | m_check_request_send = true; 28 | 29 | m_check_finish_action_apply = 0; 30 | m_time_step = 0; 31 | 32 | targetInitialize(); 33 | 34 | ROS_INFO("----------------------------- Time Step %d -----------------------------", m_time_step); 35 | ROS_INFO(" "); 36 | } 37 | 38 | bool GreedyCentralNode::requestInitialize(max_min_lp_experiment::RobotRequest::Request &req, max_min_lp_experiment::RobotRequest::Response &res) { 39 | if (strcmp(req.state_request.c_str(), "ready") == 0) { 40 | if (m_request_robot_id == req.robot_id) { 41 | res.state_answer = "wait"; 42 | m_request_robot_id += 1; 43 | m_send_robot_id = 0; 44 | 45 | m_robot_id.push_back(req.robot_id); 46 | m_robot_pose_x.push_back(req.robot_pose.x); 47 | m_robot_pose_y.push_back(req.robot_pose.y); 48 | } 49 | 50 | // Once all robots gave their local information to the central node, then do the following. 51 | if (m_request_robot_id == m_num_robot) { 52 | if (m_send_robot_id == req.robot_id) { // Here send local information to each robot. 53 | res.state_answer = "start"; 54 | 55 | // Compute observable targets and communicative last robot for each robot. 56 | vector observed_target_id; 57 | 58 | // Service of request to target_node. 59 | m_center_request_client = m_nh.serviceClient("/target_request"); 60 | max_min_lp_experiment::TargetRequest srv; 61 | srv.request.state_request = "center_ready"; 62 | 63 | for (int i = 0; i < m_num_target; i++) { 64 | double dist_target_robot = sqrt(pow(m_target_pose_x[i]-req.robot_pose.x,2) 65 | +pow(m_target_pose_y[i]-req.robot_pose.y,2)); 66 | geometry_msgs::Point target_pose; 67 | target_pose.x = m_target_pose_x[i]; 68 | target_pose.y = m_target_pose_y[i]; 69 | target_pose.z = 0; 70 | 71 | if (dist_target_robot < m_sensing_range) { 72 | observed_target_id.push_back(m_target_id[i]); 73 | res.target_id.push_back(m_target_id[i]); 74 | res.target_pose.push_back(target_pose); 75 | } 76 | } 77 | 78 | for (int i = m_send_robot_id-1; i == 0; i--) { 79 | bool found_comm_robot = false; 80 | for (vector::iterator it = observed_target_id.begin(); it != observed_target_id.end(); ++it) { 81 | double dist_target_robot = sqrt(pow(m_target_pose_x[*it]-m_robot_pose_x[i],2) 82 | +pow(m_target_pose_y[*it]-m_robot_pose_y[i],2)); 83 | if (dist_target_robot < m_sensing_range) { 84 | res.comm_robot_id = i; 85 | found_comm_robot = true; 86 | break; 87 | } 88 | } 89 | if (found_comm_robot) { 90 | break; 91 | } 92 | } 93 | 94 | if (m_send_robot_id+1 == m_num_robot) { 95 | m_request_robot_id = 0; 96 | } 97 | 98 | observed_target_id.clear(); 99 | m_send_robot_id += 1; 100 | } 101 | } 102 | } 103 | 104 | return true; 105 | } 106 | 107 | void GreedyCentralNode::afterAppliedMotionPrimitive(const std_msgs::String::ConstPtr& msg) { 108 | if (strcmp(msg->data.c_str(), "action is applied") == 0) { 109 | m_check_finish_action_apply += 1; 110 | 111 | if (m_check_finish_action_apply == m_num_robot) { // All robots finish applying their actions. 112 | ROS_INFO("Number of observed targets = %d", (int)m_target_index_used.size()); 113 | 114 | m_time_step += 1; 115 | ROS_INFO(" "); 116 | ROS_INFO("----------------------------- Step is successively complete -----------------------------"); 117 | ROS_INFO(" "); 118 | ROS_INFO("----------------------------- Time Step %d -----------------------------", m_time_step); 119 | ROS_INFO(" "); 120 | // Reinitiate and recuresively apply the same algorithm that was applied so far. 121 | // Reset all the values. 122 | m_request_robot_id = 1; 123 | m_send_robot_id = 1; 124 | m_check_request_send = true; 125 | m_check_finish_action_apply = 0; 126 | 127 | // Send a rostopic on /robot_status so that ROBOTs will compute max min LP again. 128 | // Publisher 129 | std_msgs::String msg; 130 | stringstream ss; 131 | ss<<"recompute"; 132 | msg.data = ss.str(); 133 | m_response_to_robot_pub.publish(msg); 134 | 135 | usleep(1000000); 136 | } 137 | } 138 | } 139 | 140 | void GreedyCentralNode::targetInitialize() { 141 | if (ros::param::get("central_node_greedy/num_of_targets", m_num_target)) {} 142 | else { 143 | ROS_WARN("Didn't find num_of_targets"); 144 | } 145 | if (ros::param::get("central_node_greedy/target_id", m_target_id)) {} 146 | else { 147 | ROS_WARN("Didn't find target_id"); 148 | } 149 | if (ros::param::get("central_node_greedy/pose_x", m_target_pose_x)) {} 150 | else { 151 | ROS_WARN("Didn't find pose_x"); 152 | } 153 | if (ros::param::get("central_node_greedy/pose_y", m_target_pose_y)) {} 154 | else { 155 | ROS_WARN("Didn't find pose_y"); 156 | } 157 | if (ros::param::get("central_node_greedy/velocity", m_target_velocity)) {} 158 | else { 159 | ROS_WARN("Didn't find velocity"); 160 | } 161 | if (ros::param::get("central_node_greedy/orientation", m_target_orientation)) {} 162 | else { 163 | ROS_WARN("Didn't find orientation"); 164 | } 165 | } 166 | 167 | int main(int argc, char **argv) { 168 | ros::init(argc, argv, "central_node_greedy"); 169 | GreedyCentralNode cn; 170 | 171 | ros::spin(); 172 | 173 | return 0; 174 | } 175 | -------------------------------------------------------------------------------- /max_min_lp_simulation/src/get_target_odom_journal.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Central node for the simulation 3 | * Central node only links between a set of motion primitives and a set of targets. 4 | * Important note: in this code a robot is used to take into consideration of the reduction of the step 1 of local algorithm. 5 | * Thus, a robot here does not indicate a real robot but an element of a real robot. Instead, ROBOT is used for a real robot. 6 | * \Author Yoonchang Sung 7 | * \07/17/2018 8 | * Copyright 2018. All Rights Reserved. 9 | */ 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | using namespace std; 29 | 30 | vector target_pos; 31 | 32 | bool return_odom(max_min_lp_simulation::GetOdom::Request &req, max_min_lp_simulation::GetOdom::Response &res) { 33 | res.return_target_odom = target_pos; 34 | return true; 35 | } 36 | 37 | void getTargetPos(double t_x, double t_y, double t_z, int t_id) { 38 | geometry_msgs::Pose temp_pos; 39 | temp_pos.position.x = t_x; 40 | temp_pos.position.y = t_y; 41 | temp_pos.position.z = t_z; 42 | 43 | target_pos.push_back(temp_pos); 44 | 45 | // ROS_INFO("target %d : (%.2f, %.2f, %.2f)", t_id, t_x, t_y, t_z); 46 | } 47 | 48 | void posesStampedCallback(ConstPosesStampedPtr &posesStamped) { 49 | // cout << posesStamped->DebugString(); 50 | 51 | // ::google::protobuf::int32 sec = posesStamped->time().sec(); 52 | // ::google::protobuf::int32 nsec = posesStamped->time().nsec(); 53 | // std::cout << "Read time: sec: " << sec << " nsec: " << nsec << std::endl; 54 | 55 | int target_id = 0; 56 | double temp_x = 0; 57 | double temp_y = 0; 58 | double temp_z = 0; 59 | 60 | bool t_0 = true;bool t_1 = true;bool t_2 = true;bool t_3 = true;bool t_4 = true;bool t_5 = true;bool t_6 = true;bool t_7 = true;bool t_8 = true;bool t_9 = true; 61 | bool t_10 = true;bool t_11 = true;bool t_12 = true;bool t_13 = true;bool t_14 = true;bool t_15 = true;bool t_16 = true;bool t_17 = true; 62 | bool t_18 = true;bool t_19 = true;bool t_20 = true;bool t_21 = true;bool t_22 = true;bool t_23 = true;bool t_24 = true;bool t_25 = true;bool t_26 = true; 63 | bool t_27 = true;bool t_28 = true;bool t_29 = true;bool t_30 = true; 64 | 65 | target_pos.clear(); 66 | 67 | // ROS_INFO("posesStampedCallback() is called!!"); 68 | for (int i = 0; i < posesStamped->pose_size(); ++i) { 69 | const ::gazebo::msgs::Pose &pose = posesStamped->pose(i); 70 | string name = pose.name(); 71 | if (name == std::string("target_0") && t_0) { 72 | const ::gazebo::msgs::Vector3d &position = pose.position(); 73 | temp_x = position.x();temp_y = position.y();temp_z = position.z(); 74 | t_0 = false;getTargetPos(temp_x, temp_y, temp_z, target_id);target_id += 1; 75 | } 76 | // else if (name == std::string("target_1") && t_1) { 77 | // const ::gazebo::msgs::Vector3d &position = pose.position(); 78 | // temp_x = position.x();temp_y = position.y();temp_z = position.z(); 79 | // t_1 = false;getTargetPos(temp_x, temp_y, temp_z, target_id);target_id += 1; 80 | // } 81 | // else if (name == std::string("target_2") && t_2) { 82 | // const ::gazebo::msgs::Vector3d &position = pose.position(); 83 | // temp_x = position.x();temp_y = position.y();temp_z = position.z(); 84 | // t_2 = false;getTargetPos(temp_x, temp_y, temp_z, target_id);target_id += 1; 85 | // } 86 | // else if (name == std::string("target_3") && t_3) { 87 | // const ::gazebo::msgs::Vector3d &position = pose.position(); 88 | // temp_x = position.x();temp_y = position.y();temp_z = position.z(); 89 | // t_3 = false;getTargetPos(temp_x, temp_y, temp_z, target_id);target_id += 1; 90 | // } 91 | // else if (name == std::string("target_4") && t_4) { 92 | // const ::gazebo::msgs::Vector3d &position = pose.position(); 93 | // temp_x = position.x();temp_y = position.y();temp_z = position.z(); 94 | // t_4 = false;getTargetPos(temp_x, temp_y, temp_z, target_id);target_id += 1; 95 | // } 96 | // else if (name == std::string("target_5") && t_5) { 97 | // const ::gazebo::msgs::Vector3d &position = pose.position(); 98 | // temp_x = position.x();temp_y = position.y();temp_z = position.z(); 99 | // t_5 = false;getTargetPos(temp_x, temp_y, temp_z, target_id);target_id += 1; 100 | // } 101 | // else if (name == std::string("target_6") && t_6) { 102 | // const ::gazebo::msgs::Vector3d &position = pose.position(); 103 | // temp_x = position.x();temp_y = position.y();temp_z = position.z(); 104 | // t_6 = false;getTargetPos(temp_x, temp_y, temp_z, target_id);target_id += 1; 105 | // } 106 | // else if (name == std::string("target_7") && t_7) { 107 | // const ::gazebo::msgs::Vector3d &position = pose.position(); 108 | // temp_x = position.x();temp_y = position.y();temp_z = position.z(); 109 | // t_7 = false;getTargetPos(temp_x, temp_y, temp_z, target_id);target_id += 1; 110 | // } 111 | // else if (name == std::string("target_8") && t_8) { 112 | // const ::gazebo::msgs::Vector3d &position = pose.position(); 113 | // temp_x = position.x();temp_y = position.y();temp_z = position.z(); 114 | // t_8 = false;getTargetPos(temp_x, temp_y, temp_z, target_id);target_id += 1; 115 | // } 116 | // else if (name == std::string("target_9") && t_9) { 117 | // const ::gazebo::msgs::Vector3d &position = pose.position(); 118 | // temp_x = position.x();temp_y = position.y();temp_z = position.z(); 119 | // t_9 = false;getTargetPos(temp_x, temp_y, temp_z, target_id);target_id += 1; 120 | // } 121 | } 122 | } 123 | 124 | int main(int argc, char **argv) { 125 | ros::init(argc, argv, "get_target_odom_journal_node"); 126 | ros::NodeHandle nh; 127 | 128 | ros::ServiceServer odom_service; 129 | 130 | // Load gazebo 131 | gazebo::client::setup(argc, argv); 132 | 133 | // Create our node for communication 134 | gazebo::transport::NodePtr node(new gazebo::transport::Node()); 135 | node->Init(); 136 | 137 | // Listen to Gazebo pose info topic 138 | gazebo::transport::SubscriberPtr sub = node->Subscribe("~/pose/info", posesStampedCallback); 139 | 140 | odom_service = nh.advertiseService("/target_odom_request", return_odom); 141 | 142 | // while (true) { 143 | // gazebo::common::Time::MSleep(10); 144 | // } 145 | 146 | // // Make sure to shut everything down. 147 | // gazebo::client::shutdown(); 148 | 149 | ros::spin(); 150 | return 0; 151 | } -------------------------------------------------------------------------------- /max_min_lp_simulation/include/max_min_lp_simulation/MaxMinLPCentralNodeSimulation.hpp: -------------------------------------------------------------------------------- 1 | #ifndef MAXMINLPCENTRALNODESIMULATION_HPP_ 2 | #define MAXMINLPCENTRALNODESIMULATION_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | using namespace std; 29 | 30 | class MaxMinLPCentralNodeSimulation { 31 | private: 32 | ros::NodeHandle m_nh; 33 | ros::NodeHandle m_private_nh; 34 | 35 | // Subscribers 36 | ros::Subscriber m_target_1_sub; 37 | ros::Subscriber m_target_2_sub; 38 | ros::Subscriber m_target_3_sub; 39 | ros::Subscriber m_target_4_sub; 40 | ros::Subscriber m_target_5_sub; 41 | ros::Subscriber m_target_6_sub; 42 | ros::Subscriber m_target_7_sub; 43 | ros::Subscriber m_target_8_sub; 44 | ros::Subscriber m_target_9_sub; 45 | ros::Subscriber m_target_10_sub; 46 | ros::Subscriber m_target_11_sub; 47 | ros::Subscriber m_target_12_sub; 48 | ros::Subscriber m_target_13_sub; 49 | ros::Subscriber m_target_14_sub; 50 | ros::Subscriber m_target_15_sub; 51 | ros::Subscriber m_target_16_sub; 52 | ros::Subscriber m_target_17_sub; 53 | ros::Subscriber m_target_18_sub; 54 | ros::Subscriber m_target_19_sub; 55 | ros::Subscriber m_target_20_sub; 56 | ros::Subscriber m_target_21_sub; 57 | ros::Subscriber m_target_22_sub; 58 | ros::Subscriber m_target_23_sub; 59 | ros::Subscriber m_target_24_sub; 60 | ros::Subscriber m_target_25_sub; 61 | ros::Subscriber m_target_26_sub; 62 | ros::Subscriber m_target_27_sub; 63 | ros::Subscriber m_target_28_sub; 64 | ros::Subscriber m_target_29_sub; 65 | ros::Subscriber m_target_30_sub; 66 | 67 | ros::Subscriber m_comm_graph_by_robots_sub; 68 | 69 | // Publishers 70 | ros::Publisher m_general_node_pub; 71 | ros::Publisher m_layered_node_pub; 72 | ros::Publisher m_response_to_robot_pub; 73 | 74 | // Services 75 | ros::ServiceServer m_service; 76 | ros::ServiceServer m_primitive_service; 77 | 78 | // Clients 79 | ros::ServiceClient m_target_odom_client; 80 | 81 | // Variables from the launch file 82 | max_min_lp_msgs::server_to_robots_array m_robot_info; 83 | int m_num_robot; 84 | int m_num_target; 85 | int m_num_motion_primitive; 86 | int m_num_layer; 87 | float m_fov; 88 | double m_epsilon; 89 | string m_objective_option; 90 | bool m_verbal_flag; 91 | bool m_verbal_local_flag; 92 | 93 | int m_send_robot_id; 94 | int m_request_robot_id; 95 | bool m_check_request_send; 96 | bool m_check_apply_sequential_send; 97 | int m_check_finish_action_apply; 98 | 99 | bool m_ready_to_send; 100 | 101 | int m_time_step; 102 | 103 | // For reduction 104 | vector m_constraint_value; // This value is used for reduction i.e., 2/|V_i|. 105 | 106 | //// ROBOT //// 107 | // ROBOT robot 108 | vector m_ROBOT_num_robot; 109 | vector m_prev_accumulate_robot; 110 | int m_num_survived_robot; 111 | vector m_ROBOT_x_pos; 112 | vector m_ROBOT_y_pos; 113 | 114 | // ROBOT motion primitives 115 | vector m_ROBOT_num_motion_primitive; 116 | vector m_prev_accumulate_motion_primitive; 117 | int m_num_survived_motion_primitive; 118 | 119 | // ROBOT neighbor hop info 120 | vector > > m_ROBOT_neighbor; 121 | vector > m_ROBOT_assign_targets; 122 | vector m_max_neighbor_hop; 123 | 124 | //// robot //// 125 | // robot info 126 | vector m_robot_id; 127 | 128 | //// motion primitives //// 129 | vector m_primitive_id; 130 | vector m_primitive_original_id; 131 | vector m_primitive_x_pos; 132 | vector m_primitive_y_pos; 133 | vector m_dist_primitive_to_target; 134 | 135 | //// target //// 136 | // Target info of each time-step obtained from gazebo 137 | vector m_temp_target_pos; 138 | vector m_temp_target_name; 139 | 140 | // Target info when consensus of robots is fulfilled 141 | vector m_target_id; 142 | vector m_target_x_pos; 143 | vector m_target_y_pos; 144 | vector m_target_observed; 145 | 146 | vector > m_ROBOTs_to_targets; // ROBOTs to targets 147 | vector > m_robots_to_primitives; // Robots to motion primitives 148 | vector > m_primitives_to_robots; // Motion primitives to robots 149 | vector > m_primitives_to_targets; // Motion primitives to targets 150 | vector > m_targets_to_primitives; // Targets to motion primitives 151 | 152 | vector > m_primitives_to_targets_weight; // Weights of motion primitives to targets 153 | vector > m_targets_to_primitives_weight; // Weights of targets to motion primitives 154 | 155 | // Send optimal motion primitives to each robot 156 | vector m_optimal_primitive_id; 157 | vector m_optimal_primitive_id_for_plot; 158 | 159 | // General node values 160 | vector m_gen_r_node; 161 | vector m_gen_p_r_node; 162 | vector m_gen_p_t_node; 163 | vector m_gen_t_node; 164 | 165 | // For text outputs 166 | ofstream m_target_outputFile; 167 | ofstream m_outputFile; 168 | 169 | public: 170 | MaxMinLPCentralNodeSimulation(); // Constructor 171 | ~MaxMinLPCentralNodeSimulation() { 172 | m_target_outputFile.close(); 173 | m_outputFile.close(); 174 | } 175 | 176 | bool initialize(max_min_lp_simulation::MessageRequest::Request &req, max_min_lp_simulation::MessageRequest::Response &res); 177 | bool sendMotionPrimitive(max_min_lp_simulation::MotionPrimitiveRequest::Request &req, max_min_lp_simulation::MotionPrimitiveRequest::Response &res); 178 | void updatePose(const gazebo_msgs::ModelStates::ConstPtr& msg, int target_id); // Update pose information 179 | vector buildGeneralNode(string option); 180 | void applySequentialLocalAlgorithm(const std_msgs::String::ConstPtr& msg); 181 | }; 182 | 183 | #endif -------------------------------------------------------------------------------- /max_min_lp_simulation/src/animated_box_journal.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2012 Open Source Robotics Foundation 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #define PI 3.141592 26 | 27 | namespace gazebo 28 | { 29 | class AnimatedBox : public ModelPlugin 30 | { 31 | public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/) 32 | { 33 | // Store the pointer to the model 34 | this->model = _parent; 35 | 36 | // Next position of target. 37 | bool flag_next = false; 38 | 39 | // Parameters 40 | int time_duration = 10; // time duration (seconds) for each step 41 | int tot_num_steps = 30; // Total number of steps 42 | int num_targets = 1; // Number of targets 43 | time_t t; 44 | srand((unsigned) time(&t)); 45 | int env_size = 20; // Size of the bounded environment 46 | int num_next_pose = 6; // Number of possible next positions 47 | int degree_between_poses = 360 / num_next_pose; 48 | int moving_dist = 5; // Distance that target moves at each step 49 | std::vector next_pose_x; 50 | std::vector next_pose_y; 51 | int temp_pose_id; // Temporal next position IDs 52 | 53 | int each_degree = 0; 54 | for (int i = 0; i < num_next_pose; i++) { 55 | next_pose_x.push_back(moving_dist * cos(each_degree*PI/180)); 56 | next_pose_y.push_back(moving_dist * sin(each_degree*PI/180)); 57 | // printf("\n%d pose\n", i+1); 58 | // printf("each_degree = %d\n", each_degree); 59 | // printf("next_pose_x = %.2f, next_pose_y = %.2f\n", cos(each_degree*PI/180), sin(each_degree*PI/180)); 60 | each_degree += degree_between_poses; 61 | } 62 | 63 | // Variables for targets 64 | std::vector target_pos_x; std::vector target_pos_y; 65 | std::vector prev_target_pos_x; std::vector prev_target_pos_y; 66 | std::vector anim; 67 | std::vector anim_next; 68 | 69 | // Targets 70 | for (int i = 0; i < num_targets; i++) { 71 | target_pos_x.push_back(rand()%env_size-env_size/2); 72 | target_pos_y.push_back(rand()%env_size-env_size/2); 73 | std::string temp_name = "target_"+boost::lexical_cast(i); 74 | gazebo::common::PoseAnimationPtr temp_anim( 75 | new gazebo::common::PoseAnimation(temp_name, 1000.0, false)); 76 | anim.push_back(temp_anim); 77 | if (flag_next) { 78 | std::string temp_name_next = "target_"+boost::lexical_cast(i)+"_next_pose"; 79 | gazebo::common::PoseAnimationPtr temp_anim_next( 80 | new gazebo::common::PoseAnimation(temp_name_next, 1000.0, false)); 81 | anim_next.push_back(temp_anim_next); 82 | } 83 | 84 | gazebo::common::PoseKeyFrame *temp_key = anim[i]->CreateKeyFrame(0); 85 | temp_key->Translation(ignition::math::Vector3d(target_pos_x[i], target_pos_y[i], 0)); 86 | temp_key->Rotation(ignition::math::Quaterniond(0, 0, 0)); 87 | 88 | for (int j = 0; j < tot_num_steps; j++) { 89 | prev_target_pos_x.push_back(target_pos_x[i]); 90 | prev_target_pos_y.push_back(target_pos_y[i]); 91 | 92 | while (1) { 93 | double temp_target_pos_x = target_pos_x[i]; 94 | double temp_target_pos_y = target_pos_y[i]; 95 | temp_pose_id = rand()%num_next_pose; 96 | temp_target_pos_x += next_pose_x[temp_pose_id]; 97 | temp_target_pos_y += next_pose_y[temp_pose_id]; 98 | 99 | if ((temp_target_pos_x <= env_size/2)&&(temp_target_pos_x >= (-1)*env_size/2)&& 100 | (temp_target_pos_y <= env_size/2)&&(temp_target_pos_y >= (-1)*env_size/2)) { 101 | break; 102 | } 103 | } 104 | 105 | target_pos_x[i] += next_pose_x[temp_pose_id]; 106 | target_pos_y[i] += next_pose_y[temp_pose_id]; 107 | // printf("Target_%d pose: (%.2f, %.2f)\n", i+1, target_pos_x[i], target_pos_y[i]); 108 | // printf("Distance traveled: %.2f\n", sqrt((prev_target_pos_x[i]-target_pos_x[i])*(prev_target_pos_x[i]-target_pos_x[i]) 109 | // +(prev_target_pos_y[i]-target_pos_y[i])*(prev_target_pos_y[i]-target_pos_y[i]))); 110 | if (flag_next) { 111 | gazebo::common::PoseKeyFrame *temp_key_next = anim_next[i]->CreateKeyFrame(time_duration*j); 112 | temp_key_next->Translation(ignition::math::Vector3d(target_pos_x[i], target_pos_y[i], 0)); 113 | temp_key_next->Rotation(ignition::math::Quaterniond(0, 0, 0)); 114 | } 115 | temp_key = anim[i]->CreateKeyFrame(time_duration*(j+1)); 116 | temp_key->Translation(ignition::math::Vector3d(target_pos_x[i], target_pos_y[i], 0)); 117 | temp_key->Rotation(ignition::math::Quaterniond(0, 0, 0)); 118 | if (flag_next) { 119 | gazebo::common::PoseKeyFrame *temp_key_next = anim_next[i]->CreateKeyFrame(time_duration*j); 120 | temp_key_next = anim_next[i]->CreateKeyFrame(time_duration*(j+1)-1); 121 | temp_key_next->Translation(ignition::math::Vector3d(target_pos_x[i], target_pos_y[i], 0)); 122 | temp_key_next->Rotation(ignition::math::Quaterniond(0, 0, 0)); 123 | } 124 | } 125 | 126 | if (std::strcmp(_parent->GetName().c_str(), temp_name.c_str()) == 0) { 127 | _parent->SetAnimation(anim[i]); 128 | } 129 | if (flag_next) { 130 | std::string temp_name_next = "target_"+boost::lexical_cast(i)+"_next_pose"; 131 | if (std::strcmp(_parent->GetName().c_str(), temp_name_next.c_str()) == 0) { 132 | _parent->SetAnimation(anim_next[i]); 133 | } 134 | } 135 | } 136 | } 137 | 138 | // Pointer to the model 139 | private: physics::ModelPtr model; 140 | 141 | // Pointer to the update event connection 142 | private: event::ConnectionPtr updateConnection; 143 | }; 144 | 145 | // Register this plugin with the simulator 146 | GZ_REGISTER_MODEL_PLUGIN(AnimatedBox) 147 | } 148 | -------------------------------------------------------------------------------- /max_min_lp_visualization/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(max_min_lp_visualization) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | rospy 9 | std_msgs 10 | ) 11 | 12 | ## System dependencies are found with CMake's conventions 13 | # find_package(Boost REQUIRED COMPONENTS system) 14 | 15 | 16 | ## Uncomment this if the package has a setup.py. This macro ensures 17 | ## modules and global scripts declared therein get installed 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 19 | # catkin_python_setup() 20 | 21 | ################################################ 22 | ## Declare ROS messages, services and actions ## 23 | ################################################ 24 | 25 | ## To declare and build messages, services or actions from within this 26 | ## package, follow these steps: 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 28 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 29 | ## * In the file package.xml: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 32 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 33 | ## but can be declared for certainty nonetheless: 34 | ## * add a run_depend tag for "message_runtime" 35 | ## * In this file (CMakeLists.txt): 36 | ## * add "message_generation" and every package in MSG_DEP_SET to 37 | ## find_package(catkin REQUIRED COMPONENTS ...) 38 | ## * add "message_runtime" and every package in MSG_DEP_SET to 39 | ## catkin_package(CATKIN_DEPENDS ...) 40 | ## * uncomment the add_*_files sections below as needed 41 | ## and list every .msg/.srv/.action file to be processed 42 | ## * uncomment the generate_messages entry below 43 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 44 | 45 | ## Generate messages in the 'msg' folder 46 | # add_message_files( 47 | # FILES 48 | # Message1.msg 49 | # Message2.msg 50 | # ) 51 | 52 | ## Generate services in the 'srv' folder 53 | # add_service_files( 54 | # FILES 55 | # Service1.srv 56 | # Service2.srv 57 | # ) 58 | 59 | ## Generate actions in the 'action' folder 60 | # add_action_files( 61 | # FILES 62 | # Action1.action 63 | # Action2.action 64 | # ) 65 | 66 | ## Generate added messages and services with any dependencies listed here 67 | # generate_messages( 68 | # DEPENDENCIES 69 | # std_msgs 70 | # ) 71 | 72 | ################################################ 73 | ## Declare ROS dynamic reconfigure parameters ## 74 | ################################################ 75 | 76 | ## To declare and build dynamic reconfigure parameters within this 77 | ## package, follow these steps: 78 | ## * In the file package.xml: 79 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 80 | ## * In this file (CMakeLists.txt): 81 | ## * add "dynamic_reconfigure" to 82 | ## find_package(catkin REQUIRED COMPONENTS ...) 83 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 84 | ## and list every .cfg file to be processed 85 | 86 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 87 | # generate_dynamic_reconfigure_options( 88 | # cfg/DynReconf1.cfg 89 | # cfg/DynReconf2.cfg 90 | # ) 91 | 92 | ################################### 93 | ## catkin specific configuration ## 94 | ################################### 95 | ## The catkin_package macro generates cmake config files for your package 96 | ## Declare things to be passed to dependent projects 97 | ## INCLUDE_DIRS: uncomment this if you package contains header files 98 | ## LIBRARIES: libraries you create in this project that dependent projects also need 99 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 100 | ## DEPENDS: system dependencies of this project that dependent projects also need 101 | catkin_package( 102 | # INCLUDE_DIRS include 103 | # LIBRARIES max_min_lp_visualization 104 | # CATKIN_DEPENDS rospy std_msgs 105 | # DEPENDS system_lib 106 | ) 107 | 108 | ########### 109 | ## Build ## 110 | ########### 111 | 112 | ## Specify additional locations of header files 113 | ## Your package locations should be listed before other locations 114 | # include_directories(include) 115 | include_directories( 116 | ${catkin_INCLUDE_DIRS} 117 | ) 118 | 119 | ## Declare a C++ library 120 | # add_library(max_min_lp_visualization 121 | # src/${PROJECT_NAME}/max_min_lp_visualization.cpp 122 | # ) 123 | 124 | ## Add cmake target dependencies of the library 125 | ## as an example, code may need to be generated before libraries 126 | ## either from message generation or dynamic reconfigure 127 | # add_dependencies(max_min_lp_visualization ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 128 | 129 | ## Declare a C++ executable 130 | # add_executable(max_min_lp_visualization_node src/max_min_lp_visualization_node.cpp) 131 | 132 | ## Add cmake target dependencies of the executable 133 | ## same as for the library above 134 | # add_dependencies(max_min_lp_visualization_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 135 | 136 | ## Specify libraries to link a library or executable target against 137 | # target_link_libraries(max_min_lp_visualization_node 138 | # ${catkin_LIBRARIES} 139 | # ) 140 | 141 | ############# 142 | ## Install ## 143 | ############# 144 | 145 | # all install targets should use catkin DESTINATION variables 146 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 147 | 148 | ## Mark executable scripts (Python etc.) for installation 149 | ## in contrast to setup.py, you can choose the destination 150 | # install(PROGRAMS 151 | # scripts/my_python_script 152 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 153 | # ) 154 | 155 | ## Mark executables and/or libraries for installation 156 | # install(TARGETS max_min_lp_visualization max_min_lp_visualization_node 157 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 158 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 159 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 160 | # ) 161 | 162 | ## Mark cpp header files for installation 163 | # install(DIRECTORY include/${PROJECT_NAME}/ 164 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 165 | # FILES_MATCHING PATTERN "*.h" 166 | # PATTERN ".svn" EXCLUDE 167 | # ) 168 | 169 | ## Mark other files for installation (e.g. launch and bag files, etc.) 170 | # install(FILES 171 | # # myfile1 172 | # # myfile2 173 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 174 | # ) 175 | 176 | ############# 177 | ## Testing ## 178 | ############# 179 | 180 | ## Add gtest based cpp test target and link libraries 181 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_max_min_lp_visualization.cpp) 182 | # if(TARGET ${PROJECT_NAME}-test) 183 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 184 | # endif() 185 | 186 | ## Add folders to be run by python nosetests 187 | # catkin_add_nosetests(test) 188 | -------------------------------------------------------------------------------- /max_min_lp_simulation/src/apply_motion_primitive.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Central node for the simulation 3 | * Central node only links between a set of motion primitives and a set of targets. 4 | * Important note: in this code a robot is used to take into consideration of the reduction of the step 1 of local algorithm. 5 | * Thus, a robot here does not indicate a real robot but an element of a real robot. Instead, ROBOT is used for a real robot. 6 | * \Author Yoonchang Sung 7 | * \02/27/2017 8 | * Copyright 2017. All Rights Reserved. 9 | */ 10 | 11 | #include "max_min_lp_simulation/apply_motion_primitive.hpp" 12 | 13 | #define PHI 3.141592 14 | 15 | apply_motion_primitive::apply_motion_primitive() : 16 | m_num_robot(1), m_num_target(1), m_robot_id(1), m_robot_name(string("robot_1")), m_private_nh("~") 17 | { 18 | m_private_nh.getParam("num_robot", m_num_robot); 19 | m_private_nh.getParam("num_target", m_num_target); 20 | m_private_nh.getParam("robot_id", m_robot_id); 21 | m_private_nh.getParam("robot_name", m_robot_name); 22 | 23 | m_cmd_vel_robot_pub = m_nh.advertise("/robot_"+boost::lexical_cast(m_robot_id)+"/cmd_vel_mux/input/teleop", 1); 24 | m_move_service = m_nh.advertiseService("/robot_"+boost::lexical_cast(m_robot_id)+"/move_request", &apply_motion_primitive::move_robot, this); 25 | } 26 | 27 | bool apply_motion_primitive::move_robot(max_min_lp_simulation::MoveRobot::Request &req, max_min_lp_simulation::MoveRobot::Response &res) { 28 | m_goal_pos = req.goal_pos; 29 | m_check_rotation_direction = req.rotation_direction; 30 | 31 | ROS_INFO("Robot %d goal pos (%.2f, %.2f, %.2f)", m_robot_id, m_goal_pos.position.x, m_goal_pos.position.y, m_goal_pos.orientation.w); 32 | ROS_INFO("Robot %d rotation direction = %d", m_robot_id, m_check_rotation_direction); 33 | 34 | // m_goal_pos.orientation.w 35 | float ang_vel = 0.5; 36 | while (1) { 37 | geometry_msgs::Twist cmd_vel_msg; 38 | 39 | if (m_check_rotation_direction == 1) { // CW 40 | cmd_vel_msg.linear.x = 0; 41 | cmd_vel_msg.linear.y = 0; 42 | cmd_vel_msg.linear.z = 0; 43 | cmd_vel_msg.angular.x = 0; 44 | cmd_vel_msg.angular.y = 0; 45 | cmd_vel_msg.angular.z = ang_vel; 46 | 47 | m_cmd_vel_robot_pub.publish(cmd_vel_msg); 48 | } 49 | else if (m_check_rotation_direction == -1) { // CCW 50 | cmd_vel_msg.linear.x = 0; 51 | cmd_vel_msg.linear.y = 0; 52 | cmd_vel_msg.linear.z = 0; 53 | cmd_vel_msg.angular.x = 0; 54 | cmd_vel_msg.angular.y = 0; 55 | cmd_vel_msg.angular.z = -1*ang_vel; 56 | 57 | m_cmd_vel_robot_pub.publish(cmd_vel_msg); 58 | } 59 | else { // Remaining stationary 60 | break; 61 | } 62 | 63 | m_odom_client = m_nh.serviceClient("/robot_"+boost::lexical_cast(m_robot_id)+"/odom_request"); 64 | max_min_lp_simulation::GetOdom srv; 65 | srv.request.request_odom = string("request"); 66 | 67 | if (m_odom_client.call(srv)) { 68 | geometry_msgs::Pose temp_pos; 69 | temp_pos = srv.response.return_odom; 70 | 71 | tf::Quaternion q(temp_pos.orientation.x, temp_pos.orientation.y, temp_pos.orientation.z, temp_pos.orientation.w); 72 | tf::Matrix3x3 m(q); 73 | double roll, pitch, yaw; 74 | m.getRPY(roll, pitch, yaw); 75 | 76 | yaw = yaw * 180 / PHI; 77 | 78 | // ROS_INFO("Robot %d : (%.2f, %.2f, %.2f)", m_robot_id, temp_pos.position.x, temp_pos.position.y, yaw); 79 | 80 | if (yaw > (m_goal_pos.orientation.w-5) && yaw < (m_goal_pos.orientation.w+5)) { 81 | ang_vel = 0.3; 82 | } 83 | 84 | if (yaw > (m_goal_pos.orientation.w-1) && yaw < (m_goal_pos.orientation.w+1)) { 85 | ROS_INFO("Robot %d angular velocity was applied.", m_robot_id); 86 | ROS_INFO("Robot %d yaw = %.2f, goal_orient. = %.2f", m_robot_id, yaw, m_goal_pos.orientation.w); 87 | break; 88 | } 89 | } 90 | else { 91 | ROS_INFO("ERROR: Robot %d's move is failed when giving an angular velocity.", m_robot_id); 92 | } 93 | } 94 | 95 | // m_goal_pos.position.x, m_goal_pos.position.y 96 | float linear_vel = 0.5; 97 | while (1) { 98 | geometry_msgs::Twist cmd_vel_msg; 99 | 100 | cmd_vel_msg.linear.x = linear_vel; 101 | cmd_vel_msg.linear.y = 0; 102 | cmd_vel_msg.linear.z = 0; 103 | cmd_vel_msg.angular.x = 0; 104 | cmd_vel_msg.angular.y = 0; 105 | cmd_vel_msg.angular.z = 0; 106 | 107 | m_cmd_vel_robot_pub.publish(cmd_vel_msg); 108 | 109 | m_odom_client = m_nh.serviceClient("/robot_"+boost::lexical_cast(m_robot_id)+"/odom_request"); 110 | max_min_lp_simulation::GetOdom srv; 111 | srv.request.request_odom = string("request"); 112 | 113 | if (m_odom_client.call(srv)) { 114 | geometry_msgs::Pose temp_pos; 115 | temp_pos = srv.response.return_odom; 116 | 117 | tf::Quaternion q(temp_pos.orientation.x, temp_pos.orientation.y, temp_pos.orientation.z, temp_pos.orientation.w); 118 | tf::Matrix3x3 m(q); 119 | double roll, pitch, yaw; 120 | m.getRPY(roll, pitch, yaw); 121 | 122 | yaw = yaw * 180 / PHI; 123 | 124 | // ROS_INFO("ROBOT %d : (%.2f, %.2f, %.2f)", m_robot_id, temp_pos.position.x, temp_pos.position.y, yaw); 125 | if (temp_pos.position.x > m_goal_pos.position.x - 0.2 && temp_pos.position.x < m_goal_pos.position.x + 0.2 && 126 | temp_pos.position.y > m_goal_pos.position.y - 0.2 && temp_pos.position.y < m_goal_pos.position.y + 0.2) { 127 | linear_vel = 0.3; 128 | } 129 | 130 | if (temp_pos.position.x > m_goal_pos.position.x - 0.05 && temp_pos.position.x < m_goal_pos.position.x + 0.05 && 131 | temp_pos.position.y > m_goal_pos.position.y - 0.05 && temp_pos.position.y < m_goal_pos.position.y + 0.05) { 132 | ROS_INFO("Robot %d linear velocity was applied.", m_robot_id); 133 | ROS_INFO("Robot %d pos = (%.2f, %.2f), goal_pos = (%.2f, %.2f)", m_robot_id, temp_pos.position.x, temp_pos.position.y, m_goal_pos.position.x, m_goal_pos.position.y); 134 | break; 135 | } 136 | } 137 | else { 138 | ROS_INFO("ERROR: Robot %d's move is failed when giving a linear velocity.", m_robot_id); 139 | } 140 | } 141 | 142 | geometry_msgs::Twist cmd_vel_msg; 143 | 144 | cmd_vel_msg.linear.x = 0; 145 | cmd_vel_msg.linear.y = 0; 146 | cmd_vel_msg.linear.z = 0; 147 | cmd_vel_msg.angular.x = 0; 148 | cmd_vel_msg.angular.y = 0; 149 | cmd_vel_msg.angular.z = 0; 150 | 151 | m_cmd_vel_robot_pub.publish(cmd_vel_msg); 152 | 153 | m_odom_client = m_nh.serviceClient("/robot_"+boost::lexical_cast(m_robot_id)+"/odom_request", true); 154 | max_min_lp_simulation::GetOdom srv; 155 | srv.request.request_odom = string("request"); 156 | 157 | if (m_odom_client.call(srv)) { 158 | geometry_msgs::Pose temp_pos; 159 | temp_pos = srv.response.return_odom; 160 | 161 | tf::Quaternion q(temp_pos.orientation.x, temp_pos.orientation.y, temp_pos.orientation.z, temp_pos.orientation.w); 162 | tf::Matrix3x3 m(q); 163 | double roll, pitch, yaw; 164 | m.getRPY(roll, pitch, yaw); 165 | 166 | yaw = yaw * 180 / PHI; 167 | 168 | ROS_INFO("Robot %d : (%.2f, %.2f, %.2f)", m_robot_id, temp_pos.position.x, temp_pos.position.y, yaw); 169 | 170 | res.answer_msg = string("success"); 171 | 172 | return true; 173 | } 174 | else { 175 | return false; 176 | } 177 | 178 | // ROS_INFO(" "); 179 | // ROS_INFO("(Apply_motion_primitive) ROBOT %d : (%.2f, %.2f)", m_robot_id, temp_pos.position.x, temp_pos.position.y); 180 | } 181 | 182 | int main(int argc, char **argv) { 183 | ros::init(argc, argv, "apply_motion_primitive"); 184 | 185 | apply_motion_primitive amp; 186 | 187 | ros::spin(); 188 | return 0; 189 | } -------------------------------------------------------------------------------- /max_min_lp_simulation/urdf/kobuki.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 56 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | -------------------------------------------------------------------------------- /max_min_lp_simulation/launch/test_motion_primitive.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 | 140 | 141 | -------------------------------------------------------------------------------- /max_min_lp_simulation/launch/three_robot_four_target_case.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 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 180 | 181 | -------------------------------------------------------------------------------- /max_min_lp_simulation/launch/motion_primitive_test.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 | 142 | 143 | -------------------------------------------------------------------------------- /max_min_lp_simulation/launch/server_to_robots.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 | 86 | 151 | 152 | --------------------------------------------------------------------------------