├── rvo_move ├── .gitignore ├── maps │ ├── map.png │ ├── levine_towne_nav.pgm │ ├── levine_towne_nav.yaml │ ├── map.yaml │ └── stage.world ├── action │ └── Move.action ├── src │ ├── move_server.cpp │ ├── move_client.cpp │ ├── rvo_wrapper.hpp │ ├── bot_client.cpp │ └── rvo_wrapper.cpp ├── matlab │ ├── plot_poly.m │ ├── delauncy_decomp.m │ ├── plot_delaunay.m │ ├── plot_graph.m │ ├── make_graph.m │ ├── save_outline.m │ ├── poly_smooth.m │ ├── fit_polygons.m │ └── Map.m ├── launch │ ├── move.launch │ └── recursive_robots.launch ├── CMakeLists.txt ├── package.xml ├── example │ └── test.cpp ├── include │ └── rvo_move │ │ └── bot_client.hpp ├── config │ └── scarab_move.yaml └── urdf │ ├── robot.xacro │ └── robot.urdf ├── rvo2 ├── AUTHORS ├── package.xml ├── README ├── COPYING ├── src │ ├── Obstacle.cpp │ ├── Obstacle.h │ ├── Definitions.h │ ├── Agent.h │ ├── KdTree.h │ ├── RVOSimulator.cpp │ ├── KdTree.cpp │ └── Agent.cpp ├── CMakeLists.txt └── include │ └── RVO │ └── Vector2.h ├── player_map ├── srv │ └── GetMap.srv ├── package.xml ├── CMakeLists.txt ├── launch │ └── test.launch ├── include │ └── player_map │ │ ├── rosmap.hpp │ │ └── map.h └── src │ ├── draw_paths.cpp │ ├── map.c │ ├── rosmap_test.cpp │ └── rosmap.cpp └── Readme.md /rvo_move/.gitignore: -------------------------------------------------------------------------------- 1 | msg/Move*.msg 2 | .vscode -------------------------------------------------------------------------------- /rvo2/AUTHORS: -------------------------------------------------------------------------------- 1 | Jur van den Berg 2 | Stephen J. Guy 3 | Jamie Snape 4 | Ming C. Lin 5 | Dinesh Manocha 6 | -------------------------------------------------------------------------------- /rvo_move/maps/map.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Osovey/rvo-ros-wrapper/HEAD/rvo_move/maps/map.png -------------------------------------------------------------------------------- /rvo_move/maps/levine_towne_nav.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Osovey/rvo-ros-wrapper/HEAD/rvo_move/maps/levine_towne_nav.pgm -------------------------------------------------------------------------------- /rvo_move/action/Move.action: -------------------------------------------------------------------------------- 1 | geometry_msgs/PoseStamped target_pose 2 | --- 3 | --- 4 | geometry_msgs/PoseStamped base_position 5 | -------------------------------------------------------------------------------- /player_map/srv/GetMap.srv: -------------------------------------------------------------------------------- 1 | # Get the map as a nav_msgs/OccupancyGrid + include Cspace information 2 | --- 3 | nav_msgs/OccupancyGrid map 4 | float32[] occ_dist 5 | float64 max_occ_dist 6 | -------------------------------------------------------------------------------- /rvo_move/maps/levine_towne_nav.yaml: -------------------------------------------------------------------------------- 1 | image: levine_towne_nav.pgm 2 | resolution: 0.050000 3 | origin: [-12.55, -17.63, 0.0000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /rvo_move/maps/map.yaml: -------------------------------------------------------------------------------- 1 | # 缩小了5倍 2 | image: map.png 3 | resolution: 0.05 4 | origin: [-116.0, -113.0, 0.0] 5 | # origin: [0.0, 0.0, 0.0] 6 | negate: 0 7 | occupied_thresh: 0.65 8 | free_thresh: 0.235 # 0.196 9 | mode: trinary # trinary, raw, scale 10 | -------------------------------------------------------------------------------- /rvo_move/src/move_server.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "rvo_wrapper.hpp" 4 | 5 | int main(int argc, char **argv) { 6 | ros::init(argc, argv, "move_server"); 7 | rvo::MoveServer ms(ros::this_node::getName()); 8 | ms.start(); 9 | ros::spin(); 10 | } 11 | -------------------------------------------------------------------------------- /rvo_move/matlab/plot_poly.m: -------------------------------------------------------------------------------- 1 | function [] = plot_poly(polygons) 2 | hold all 3 | for k = 1:length(polygons) 4 | xys = polygons{k}; 5 | xys = [xys; xys(1, :)]; 6 | x = xys(:, 1); 7 | y = xys(:, 2); 8 | plot(x, y, '.-') 9 | end 10 | hold off; 11 | end 12 | -------------------------------------------------------------------------------- /rvo2/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rvo2 4 | 0.0.0 5 | The rvo2 package 6 | Ben Charrow 7 | Jur van den Berg 8 | Stephen J. Guy 9 | Jamie Snape 10 | Ming C. Lin 11 | Dinesh Manocha 12 | 13 | TODO 14 | catkin 15 | 16 | -------------------------------------------------------------------------------- /rvo_move/matlab/delauncy_decomp.m: -------------------------------------------------------------------------------- 1 | function [dt] = delauncy_decomp(polys) 2 | 3 | profile = cell2mat(polys); 4 | constraints = []; 5 | start = 1; 6 | for k = 1:length(polys) 7 | if k == 1 8 | xys = flipud(polys{k}); 9 | else 10 | xys = polys{k}; 11 | end 12 | n = size(xys, 1); 13 | inc = (start):(start+n-2); 14 | constraints(start:(start+n-1), :) = [inc.' (inc + 1).'; start + n - 1, start]; 15 | start = size(constraints, 1) + 1; 16 | end 17 | 18 | dt = DelaunayTri(profile, constraints); 19 | end 20 | -------------------------------------------------------------------------------- /rvo_move/matlab/plot_delaunay.m: -------------------------------------------------------------------------------- 1 | function [] = plot_delaunay(map, dt, xy, neighbors) 2 | clf(gcf); 3 | figure(gcf); 4 | inside = inOutStatus(dt); 5 | plot(map); 6 | hold on; 7 | centers = dt(inside, :); 8 | triplot(centers, dt.X(:,1), dt.X(:,2)); 9 | plot(xy(:, 1), xy(:, 2), 'r.', 'MarkerSize', 22); 10 | for k = 1:length(neighbors) 11 | ns = neighbors{k}; 12 | for l = 1:length(ns) 13 | xs = [xy(k, 1); xy(ns(l), 1)]; 14 | ys = [xy(k, 2); xy(ns(l), 2)]; 15 | plot(xs, ys, 'k-'); 16 | end 17 | end 18 | hold off; 19 | end 20 | -------------------------------------------------------------------------------- /rvo_move/launch/move.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /player_map/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | player_map 4 | 0.0.0 5 | The player_map package 6 | Ben Charrow 7 | Ben Charrow 8 | 9 | TODO 10 | catkin 11 | 12 | roscpp 13 | nav_msgs 14 | map_server 15 | 16 | roscpp 17 | nav_msgs 18 | map_server 19 | 20 | -------------------------------------------------------------------------------- /rvo_move/matlab/plot_graph.m: -------------------------------------------------------------------------------- 1 | function [] = plot_graph(m, xys, neighbors) 2 | 3 | hold on; 4 | plot(xys(:, 1), xys(:, 2), '.') 5 | 6 | for k = 1:size(xys, 1) 7 | xy = xys(k, :); 8 | 9 | for n = 1:length(neighbors{k}) 10 | oxys = xys(neighbors{k}, :); 11 | for l = 1:n 12 | plot([xy(1), oxys(l, 1)], [xy(2); oxys(l, 2)], '-'); 13 | end 14 | end 15 | end 16 | xl = xlim; 17 | yl = ylim; 18 | for k = 1:size(xys, 1) 19 | if (xl(1) < xys(k, 1) && xys(k, 1) < xl(2) && ... 20 | yl(1) < xys(k, 2) && xys(k, 2) < yl(2)) 21 | text(xys(k, 1) + 0.2, xys(k, 2) + 0.2, sprintf('%i', k)); 22 | end 23 | end 24 | 25 | 26 | end 27 | -------------------------------------------------------------------------------- /rvo2/README: -------------------------------------------------------------------------------- 1 | RVO2 Library is an easy-to-use C++ implementation of the optimal reciprocal 2 | collision avoidance (ORCA) formulation for 3 | multi-agent simulation. RVO2 Library automatically uses parallelism for 4 | computing the motion of the agents on machines with multiple processors and a 5 | compiler supporting OpenMP . 6 | 7 | Please send all bug reports to . 8 | 9 | The authors may be contacted via: 10 | 11 | Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha 12 | Dept. of Computer Science 13 | 201 S. Columbia St. 14 | Frederick P. Brooks, Jr. Computer Science Bldg. 15 | Chapel Hill, N.C. 27599-3175 16 | United States of America 17 | 18 | 19 | -------------------------------------------------------------------------------- /player_map/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(player_map) 3 | 4 | find_package(Eigen3 REQUIRED) 5 | include_directories(${EIGEN_INCLUDE_DIRS}) 6 | 7 | find_package(catkin REQUIRED COMPONENTS roscpp nav_msgs map_server) 8 | 9 | add_service_files(FILES GetMap.srv) 10 | generate_messages(DEPENDENCIES nav_msgs) 11 | 12 | catkin_package( 13 | INCLUDE_DIRS include 14 | LIBRARIES rvoplayermap 15 | CATKIN_DEPENDS roscpp nav_msgs map_server) 16 | 17 | include_directories(include ${catkin_INCLUDE_DIRS}) 18 | 19 | add_library(rvoplayermap src/map.c src/rosmap.cpp) 20 | target_link_libraries(rvoplayermap ${catkin_LIBRARIES}) 21 | add_dependencies(rvoplayermap ${PROJECT_NAME}_gencpp) 22 | 23 | add_executable(rvo_draw_paths src/draw_paths.cpp) 24 | target_link_libraries(rvo_draw_paths rvoplayermap) 25 | 26 | catkin_add_gtest(test_rosmap src/rosmap_test.cpp) 27 | target_link_libraries(test_rosmap rvoplayermap) 28 | -------------------------------------------------------------------------------- /player_map/launch/test.launch: -------------------------------------------------------------------------------- 1 | 2 | 5 | 7 | 8 | 9 | 10 | 11 | 14 | 15 | 16 | 17 | 18 | 20 | 21 | -------------------------------------------------------------------------------- /rvo_move/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rvo_move) 3 | 4 | find_package(Eigen3 REQUIRED) 5 | 6 | find_package(catkin REQUIRED COMPONENTS roscpp visualization_msgs nav_msgs rvo2 7 | actionlib_msgs actionlib tf player_map) 8 | 9 | add_action_files(DIRECTORY action FILES Move.action) 10 | generate_messages(DEPENDENCIES actionlib_msgs geometry_msgs) 11 | 12 | catkin_package( 13 | INCLUDE_DIRS include 14 | LIBRARIES botclient 15 | CATKIN_DEPENDS roscpp visualization_msgs nav_msgs rvo2 16 | actionlib_msgs actionlib tf player_map) 17 | 18 | include_directories(include ${EIGEN_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) 19 | 20 | add_library(botclient src/bot_client.cpp) 21 | target_link_libraries(botclient ${catkin_LIBRARIES}) 22 | 23 | add_executable(move_server src/move_server.cpp src/rvo_wrapper.cpp) 24 | target_link_libraries(move_server botclient) 25 | 26 | add_executable(move_client src/move_client.cpp) 27 | target_link_libraries(move_client botclient) 28 | 29 | add_executable(test_node example/test.cpp) 30 | target_link_libraries(test_node ${catkin_LIBRARIES}) -------------------------------------------------------------------------------- /rvo_move/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rvo_move 4 | 0.0.0 5 | The rvo_move package 6 | Ben Charrow 7 | Ben Charrow 8 | 9 | TODO 10 | catkin 11 | 12 | roscpp 13 | visualization_msgs 14 | nav_msgs 15 | rvo2 16 | actionlib_msgs 17 | actionlib 18 | tf 19 | player_map 20 | 21 | roscpp 22 | visualization_msgs 23 | nav_msgs 24 | rvo2 25 | actionlib_msgs 26 | actionlib 27 | tf 28 | player_map 29 | 30 | -------------------------------------------------------------------------------- /rvo_move/matlab/make_graph.m: -------------------------------------------------------------------------------- 1 | function [xy, neighbors] = make_graph(map, dt) 2 | inside = inOutStatus(dt); 3 | incent = dt.incenters; 4 | neighbors = cell(size(incent, 1), 1); 5 | for k = 1:size(dt.Triangulation) 6 | ns = dt.neighbors(k); 7 | n = ns(~isnan(ns)); 8 | neighbors{k} = n(inside(n)); 9 | end 10 | 11 | second_neighbors = cell(size(neighbors)); 12 | for k = 1:size(neighbors) 13 | ns = neighbors{k}; 14 | for l = 1:length(ns) 15 | if ~inside(k) || isnan(ns(l)) || ~inside(ns(l)) 16 | continue 17 | end 18 | others = ns(ns ~= ns(l)); 19 | second_neighbors{ns(l)} = unique([others, second_neighbors{ns(l)}]); 20 | end 21 | end 22 | 23 | neighbors = cellfun(@(a, b) unique([a, b]), neighbors, second_neighbors, ... 24 | 'UniformOutput', false); 25 | xy = incent; 26 | 27 | % Remove invalid vertices and calculate new neighbor indices 28 | valid = inside & ~map.collide(xy); 29 | new_inds = cumsum(valid); 30 | xy = xy(valid, :); 31 | neighbors = neighbors(valid); 32 | neighbors = cellfun(@(a) new_inds(a), neighbors, 'UniformOutput', false); 33 | 34 | end 35 | -------------------------------------------------------------------------------- /rvo_move/matlab/save_outline.m: -------------------------------------------------------------------------------- 1 | close all; 2 | clear all; 3 | map = Map.FromYAML('~/git/scarab/upenn_maps/maps/levine_towne_nav.yaml'); 4 | 5 | %% 6 | inflated_map = map.InflateObstacles(1.0); 7 | plot(inflated_map) 8 | polygons = fit_polygons(inflated_map, 120, 100); 9 | % Change coordinates of outside boundary to be counterclockwise 10 | polygons{1} = flipud(polygons{1}); 11 | 12 | %% 13 | min_sep = 0.3; 14 | max_sep = 2.0; 15 | smoothed = cellfun(@(x) poly_smooth(x, min_sep, max_sep), polygons, 'UniformOutput', false); 16 | clf(gcf); 17 | figure(gcf); 18 | plot_poly(smoothed) 19 | %% 20 | dt = delauncy_decomp(smoothed); 21 | [xy, neighbors] = make_graph(map, dt); 22 | assert(~any(map.collide(xy))); 23 | %% Visualize results 24 | plot_delaunay(map, dt, xy, neighbors) 25 | 26 | %% Save the neighbors to disk 27 | fid = fopen('levine_towne_waypoints.txt', 'w'); 28 | strs = cellfun(@(a) sprintf(' %i', a), neighbors, 'UniformOutput', false); 29 | for k = 1:size(xy, 1) 30 | fprintf(fid, '%0.5f %0.5f%s\n', xy(k, :), strs{k}); 31 | end 32 | fclose(fid); 33 | 34 | %% Save the polygons to disk 35 | fid = fopen('levine_towne_polygons.txt', 'w'); 36 | for k = 1:length(polygons) 37 | fprintf(fid, '===\n'); 38 | fprintf(fid, '% 7.3f % 7.3f\n', polygons{k}.'); 39 | end 40 | fprintf(fid, '==='); 41 | fclose(fid); 42 | -------------------------------------------------------------------------------- /rvo_move/launch/recursive_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 | -------------------------------------------------------------------------------- /rvo_move/example/test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include "geometry_msgs/PoseStamped.h" 7 | 8 | int main(int argc, char **argv) { 9 | ros::init(argc, argv, "test_node"); 10 | ros::NodeHandle nh; 11 | 12 | int n; 13 | nh.param("robot_num", n, 16); 14 | 15 | std::vector goal_publishers; 16 | 17 | std::string prefix = "robot_"; 18 | std::string topic_name = "/goal"; 19 | 20 | for (int i = 0; i < n; i++) { 21 | std::ostringstream topic; 22 | topic << prefix << i << topic_name; 23 | goal_publishers.push_back( 24 | nh.advertise(topic.str(), 10)); 25 | } 26 | 27 | ros::Duration(1).sleep(); 28 | 29 | int group_num = 4; 30 | int group_size = 4; 31 | 32 | std::vector> goals{ 33 | {2.21, 0.15}, 34 | {0.63, -2.7}, 35 | {-1.72, -0.02}, 36 | {0.25, 2.75} 37 | }; 38 | 39 | for (int i = 0; i < group_num; i++) { 40 | geometry_msgs::PoseStamped goal; 41 | goal.header.frame_id = "/map"; 42 | goal.header.stamp = ros::Time::now(); 43 | goal.pose.position.x = goals[i].first; 44 | goal.pose.position.y = goals[i].second; 45 | for (int j = 0; j < group_size; j++) { 46 | goal_publishers[i * group_size + j].publish(goal); 47 | } 48 | } 49 | 50 | return 0; 51 | } 52 | -------------------------------------------------------------------------------- /rvo_move/include/rvo_move/bot_client.hpp: -------------------------------------------------------------------------------- 1 | #ifndef BOT_COMM_HPP 2 | #define BOT_COMM_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include "nav_msgs/Odometry.h" 10 | 11 | #include "geometry_msgs/PoseStamped.h" 12 | #include "geometry_msgs/PoseWithCovarianceStamped.h" 13 | 14 | namespace rvo { 15 | class BotClient { 16 | public: 17 | BotClient(std::string prefix); 18 | 19 | void odomCallback(const nav_msgs::Odometry &msg); 20 | void poseCallback(const geometry_msgs::PoseWithCovarianceStamped &msg); 21 | void velCallback(const geometry_msgs::Twist &msg); 22 | 23 | bool haveOdom(); 24 | bool haveOdom(const ros::Duration &d); 25 | bool havePose(); 26 | bool havePose(const ros::Duration &d); 27 | 28 | std::string getName() const { return name_; } 29 | nav_msgs::Odometry getOdom(); 30 | geometry_msgs::Pose getPose(); 31 | 32 | void pubVel(double v, double w); 33 | 34 | static std::vector MakeBots(const ros::NodeHandle& nh); 35 | static void FreeBots(std::vector bots); 36 | protected: 37 | ros::NodeHandle nh_; 38 | ros::Subscriber pose_sub_, odom_sub_, vel_sub_; 39 | ros::Publisher vel_pub_; 40 | bool got_odom_, got_pose_; 41 | std::string name_; 42 | tf::TransformListener tf_listener_; 43 | 44 | boost::mutex mutex_; 45 | 46 | geometry_msgs::PoseStamped pose_; 47 | nav_msgs::Odometry odom_; 48 | geometry_msgs::Twist vel_; 49 | }; 50 | } 51 | #endif 52 | -------------------------------------------------------------------------------- /rvo_move/matlab/poly_smooth.m: -------------------------------------------------------------------------------- 1 | function [smoothed] = poly_smooth(xy, min_sep, max_sep) 2 | xy = [xy; xy(1, :)]; 3 | 4 | smoothed = xy(1, :); 5 | for k = 2:(size(xy, 1)-1) 6 | prev = smoothed(end, :); 7 | curr = xy(k, :); 8 | next = xy(k + 1, :); 9 | prev_dist = norm(curr - prev); 10 | next_dist = norm(prev - next); 11 | % plot(gca, smoothed(:, 1), smoothed(:, 2), '.'); 12 | % title(gca, k); 13 | % hold on; 14 | % text(0.2 + smoothed(end, 1), smoothed(end, 2), sprintf('%i', k)); 15 | % hold off; 16 | % pause(0.25); 17 | if next_dist < max_sep 18 | smoothed(end + 1, :) = curr; 19 | elseif prev_dist < min_sep 20 | continue 21 | else 22 | while norm(curr - prev) > max_sep 23 | dir = (curr - prev) / norm(curr - prev); 24 | prev = prev + dir * min(max_sep, norm(curr - prev)); 25 | smoothed(end + 1, :) = prev; 26 | end 27 | if (norm(smoothed(end, :) - curr) > min_sep) 28 | dir = (curr - prev) / norm(curr - prev); 29 | prev = prev + dir * min(max_sep, norm(curr - prev)); 30 | smoothed(end + 1, :) = prev; 31 | end 32 | end 33 | end 34 | 35 | if norm(smoothed(end, :) - smoothed(1, :)) < min_sep 36 | smoothed(end, :) = []; 37 | elseif norm(smoothed(end, :) - smoothed(1, :)) > max_sep 38 | prev = smoothed(end, :); 39 | curr = smoothed(1, :); 40 | while norm(curr - prev) > max_sep 41 | dir = (curr - prev) / norm(curr - prev); 42 | prev = prev + dir * min(max_sep, norm(curr - prev)); 43 | smoothed(end + 1, :) = prev; 44 | end 45 | end 46 | end 47 | -------------------------------------------------------------------------------- /rvo_move/matlab/fit_polygons.m: -------------------------------------------------------------------------------- 1 | function [polygons] = fit_polygons(map, occ_cutoff, pixel_cutoff) 2 | % FIT_POLYGONS Fit polygons to obstacles in occupancy grid 3 | % Finds boundary pixels of connected components of pixels that represent 4 | % occupied space and then fits straight line segments to them. polygons is 5 | % a cell array of xy-coordinates arranged in counterclockwise order. 6 | % 7 | % map: Map() object. 8 | % occ_cutoff: Pixels in map.occgrid above this value are considered as occupied. 9 | % pixel_cutoff: Minimum number of boundary pixels for a polygon to be returned. 10 | occgrid = map.occgrid; 11 | plot(map); 12 | hold on; 13 | bin_occ = occgrid > occ_cutoff; 14 | % Coordinates are ordered clockwise, but because occgrid is flipped along 15 | % y-axis, the result will be clockwise in xy coordinates. 16 | bound = bwboundaries(bin_occ); 17 | valid = cellfun(@length, bound) > pixel_cutoff; 18 | bound = bound(valid); 19 | vc = varycolor(length(bound)); 20 | set(gca, 'ColorOrder', vc); 21 | hold all; 22 | polygons = cell(length(bound), 1); 23 | for k = 1:length(bound) 24 | pixels = bound{k}; 25 | xys = map.subToXY(pixels); 26 | polygons{k} = polyOutline(xys, inf); 27 | plot(polygons{k}(:, 1), polygons{k}(:, 2), '.-', 'LineWidth', 1); 28 | end 29 | hold off; 30 | end 31 | 32 | function [xys] = polyOutline(xys, dist) 33 | prev_xy1 = xys(1, :); 34 | prev_xy2 = xys(2, :); 35 | new_xys = prev_xy1; 36 | for k = 3:size(xys, 1) 37 | curr = xys(k, :); 38 | diff1 = sign(prev_xy1 - prev_xy2); 39 | diff2 = sign(prev_xy2 - curr); 40 | same_dir = all(diff1 == diff2) || (norm(diff1)^2 == 2 && norm(diff2)^2 == 2); 41 | if ~same_dir || norm(prev_xy2 - new_xys(end, :)) > dist 42 | new_xys = [new_xys; prev_xy2]; 43 | else 44 | ; 45 | end 46 | 47 | prev_xy1 = prev_xy2; 48 | prev_xy2 = curr; 49 | end 50 | xys = new_xys; 51 | end -------------------------------------------------------------------------------- /rvo_move/config/scarab_move.yaml: -------------------------------------------------------------------------------- 1 | neighborDist: 5.0 2 | maxNeighbors: 10 3 | timeHorizon: 10.0 4 | timeHorizonObst: 3.0 5 | # radius: 0.08 6 | maxSpeed: 0.4 7 | timestep: 0.05 # 1 / controller rate 8 | axel_width: 0.255 9 | los_margin: 0.1 10 | 11 | waypoint_spacing: 0.1 12 | path_margin: 0.6 13 | goal_tolerance: 0.1 14 | 15 | # http://gamma.cs.unc.edu/RVO2/documentation/2.0/params.html 16 | # timeStep 17 | # float (time) 18 | # The time step of the simulation. Must be positive. 19 | # maxNeighbors 20 | # size_t 21 | # The maximal number of other agents the agent takes into account in the 22 | # navigation. The larger this number, the longer the running time of the 23 | # simulation. If the number is too low, the simulation will not be safe. 24 | # maxSpeed 25 | # float (distance/time) 26 | # The maximum speed of the agent. Must be non-negative. 27 | # neighborDist 28 | # float (distance) 29 | # The maximal distance (center point to center point) to other agents the 30 | # agent takes into account in the navigation. The larger this number, the 31 | # longer the running time of the simulation. If the number is too low, 32 | # the simulation will not be safe. Must be non-negative. 33 | # radius 34 | # float (distance) 35 | # The radius of the agent. Must be non-negative. 36 | # timeHorizon 37 | # float (time) 38 | # The minimal amount of time for which the agent's velocities that are 39 | # computed by the simulation are safe with respect to other agents. The 40 | # larger this number, the sooner this agent will respond to the presence 41 | # of other agents, but the less freedom the agent has in choosing its 42 | # velocities. Must be positive. 43 | # timeHorizonObst 44 | # float (time) 45 | # The minimal amount of time for which the agent's velocities that are 46 | # computed by the simulation are safe with respect to obstacles. The 47 | # larger this number, the sooner this agent will respond to the presence 48 | # of obstacles, but the less freedom the agent has in choosing its 49 | # velocities. Must be positive. 50 | -------------------------------------------------------------------------------- /rvo_move/src/move_client.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include 6 | 7 | #include 8 | 9 | using namespace std; 10 | 11 | class MoveClient { 12 | public: 13 | MoveClient(std::string name = "move_server") : 14 | ac_(name, true), action_name_(name), waiting_(false) { 15 | ROS_INFO("Waiting for action server '%s'", 16 | ros::names::resolve(name).c_str()); 17 | ac_.waitForServer(); 18 | if (!ac_.isServerConnected()) { 19 | ROS_ERROR("Couldn't connect to server"); 20 | ros::shutdown(); 21 | } else { 22 | ROS_INFO("Action server started"); 23 | } 24 | my_name_ = ros::names::resolve(ros::NodeHandle("~").getNamespace()); 25 | sub_ = nh_.subscribe("goal", 1, &MoveClient::sendGoal, this); 26 | } 27 | 28 | void sendGoal(const geometry_msgs::PoseStamped& goal_msg) { 29 | boost::mutex::scoped_lock lock(mutex_); 30 | rvo_move::MoveGoal goal; 31 | goal.target_pose.pose.position.x = goal_msg.pose.position.x; 32 | goal.target_pose.pose.position.y = goal_msg.pose.position.y; 33 | if (waiting_) { 34 | ac_.stopTrackingGoal(); 35 | } 36 | waiting_ = true; 37 | ac_.sendGoal(goal, boost::bind(&MoveClient::doneCb, this, _1, _2)); 38 | } 39 | 40 | void doneCb(const actionlib::SimpleClientGoalState& state, 41 | const rvo_move::MoveResultConstPtr& result) { 42 | boost::mutex::scoped_lock lock(mutex_); 43 | waiting_ = false; 44 | ROS_INFO_STREAM("" << my_name_ << ": Done. State = " << 45 | state.toString() << " " << *result); 46 | } 47 | 48 | private: 49 | boost::mutex mutex_; 50 | ros::NodeHandle nh_; 51 | ros::Subscriber sub_; 52 | actionlib::SimpleActionClient ac_; 53 | std::string action_name_, my_name_; 54 | bool waiting_; 55 | }; 56 | 57 | int main(int argc, char **argv) { 58 | ros::init(argc, argv, "rvo_move"); 59 | MoveClient mc("move_server"); 60 | ros::spin(); 61 | return 0; 62 | } 63 | -------------------------------------------------------------------------------- /rvo2/COPYING: -------------------------------------------------------------------------------- 1 | Copyright (c) 2008-2010 University of North Carolina at Chapel Hill. 2 | All rights reserved. 3 | 4 | Permission to use, copy, modify, and distribute this software and its 5 | documentation for educational, research, and non-profit purposes, without fee, 6 | and without a written agreement is hereby granted, provided that the above 7 | copyright notice, this paragraph, and the following four paragraphs appear in 8 | all copies. 9 | 10 | Permission to incorporate this software into commercial products may be obtained 11 | by contacting the authors or the Office of Technology 12 | Development at the University of North Carolina at Chapel Hill . 13 | 14 | This software program and documentation are copyrighted by the University of 15 | North Carolina at Chapel Hill. The software program and documentation are 16 | supplied "as is," without any accompanying services from the University of North 17 | Carolina at Chapel Hill or the authors. The University of North Carolina at 18 | Chapel Hill and the authors do not warrant that the operation of the program 19 | will be uninterrupted or error-free. The end-user understands that the program 20 | was developed for research purposes and is advised not to rely exclusively on 21 | the program for any reason. 22 | 23 | IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE AUTHORS 24 | BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR 25 | CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS 26 | SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF NORTH CAROLINA AT 27 | CHAPEL HILL OR THE AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS SPECIFICALLY 30 | DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 31 | OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AND ANY STATUTORY 32 | WARRANTY OF NON-INFRINGEMENT. THE SOFTWARE PROVIDED HEREUNDER IS ON AN "AS IS" 33 | BASIS, AND THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS HAVE 34 | NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR 35 | MODIFICATIONS. 36 | -------------------------------------------------------------------------------- /rvo_move/urdf/robot.xacro: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /rvo_move/src/rvo_wrapper.hpp: -------------------------------------------------------------------------------- 1 | #ifndef RVO_WRAPPER_HPP 2 | #define RVO_WRAPPER_HPP 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | #include 12 | 13 | #include 14 | 15 | #include 16 | 17 | #include 18 | 19 | namespace rvo { 20 | class RVOWrapper { 21 | public: 22 | RVOWrapper(std::vector bots, size_t id, OccupancyMap *map, 23 | const std::vector > &obstacles); 24 | ~RVOWrapper(); 25 | 26 | static RVOWrapper* ROSInit(const ros::NodeHandle& nh, OccupancyMap *map, 27 | std::vector bots); 28 | 29 | bool step(); 30 | std::vector setGoal(const geometry_msgs::Pose& p); 31 | 32 | size_t getID() const { return id_; } 33 | bool syncState(); 34 | bool setVelocities(); 35 | 36 | void setAgentDefaults(float neighborDist, size_t maxNeighbors, float timeHorizon, 37 | float timeHorizonObst, float radius, float maxSpeed); 38 | void setTimestep(float ts) { timestep_ = ts; } 39 | void setAxelWidth(float width) { axel_width_ = width; } 40 | void setWaypointSpacing(float spacing) { way_spacing_ = spacing; } 41 | void setPathMargin(float margin) { path_margin_ = margin; } 42 | void setGoalTolerance(float tol) { goal_tol_ = tol; } 43 | void setLOSMargin(float marg) { los_margin_ = marg; } 44 | void setMaxSpeed(float marg) { max_speed_ = marg; } 45 | void addAgents(); 46 | private: 47 | bool getLeadGoal(RVO::Vector2 *goal); 48 | bool getOtherGoal(int bot_id, RVO::Vector2 *goal); 49 | 50 | ros::NodeHandle nh_; 51 | ros::Publisher vis_pub_, vis_pub_throt_; 52 | std::string namespace_; 53 | 54 | RVO::RVOSimulator *sim_; 55 | std::vector bots_; 56 | RVO::Vector2 goal_; 57 | std::vector waypoints_; 58 | boost::scoped_ptr map_; 59 | size_t id_; 60 | 61 | float max_dist_; 62 | float axel_width_; 63 | float goal_tol_; 64 | float path_margin_; 65 | float way_spacing_; 66 | float timestep_; 67 | float los_margin_; 68 | double max_speed_; 69 | }; 70 | 71 | class MoveServer { 72 | public: 73 | MoveServer(const std::string &server_name); 74 | ~MoveServer(); 75 | void executeCB(const rvo_move::MoveGoalConstPtr &goal); 76 | void start(); 77 | private: 78 | ros::NodeHandle nh_, pnh_; 79 | ros::Publisher path_pub_, vis_pub_; 80 | std::string namespace_; 81 | actionlib::SimpleActionServer as_; 82 | std::string action_name_; 83 | std::string tf_frame_; 84 | std::vector bots_; 85 | RVOWrapper *wrapper_; 86 | double timestep_; 87 | }; 88 | } 89 | #endif 90 | -------------------------------------------------------------------------------- /rvo2/src/Obstacle.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Obstacle.cpp 3 | * RVO2 Library 4 | * 5 | * Copyright (c) 2008-2010 University of North Carolina at Chapel Hill. 6 | * All rights reserved. 7 | * 8 | * Permission to use, copy, modify, and distribute this software and its 9 | * documentation for educational, research, and non-profit purposes, without 10 | * fee, and without a written agreement is hereby granted, provided that the 11 | * above copyright notice, this paragraph, and the following four paragraphs 12 | * appear in all copies. 13 | * 14 | * Permission to incorporate this software into commercial products may be 15 | * obtained by contacting the authors or the Office of 16 | * Technology Development at the University of North Carolina at Chapel Hill 17 | * . 18 | * 19 | * This software program and documentation are copyrighted by the University of 20 | * North Carolina at Chapel Hill. The software program and documentation are 21 | * supplied "as is," without any accompanying services from the University of 22 | * North Carolina at Chapel Hill or the authors. The University of North 23 | * Carolina at Chapel Hill and the authors do not warrant that the operation of 24 | * the program will be uninterrupted or error-free. The end-user understands 25 | * that the program was developed for research purposes and is advised not to 26 | * rely exclusively on the program for any reason. 27 | * 28 | * IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE 29 | * AUTHORS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR 30 | * CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS 31 | * SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF NORTH CAROLINA AT 32 | * CHAPEL HILL OR THE AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH 33 | * DAMAGE. 34 | * 35 | * THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS SPECIFICALLY 36 | * DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 37 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AND ANY 38 | * STATUTORY WARRANTY OF NON-INFRINGEMENT. THE SOFTWARE PROVIDED HEREUNDER IS ON 39 | * AN "AS IS" BASIS, AND THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE 40 | * AUTHORS HAVE NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, 41 | * ENHANCEMENTS, OR MODIFICATIONS. 42 | * 43 | * Please send all bug reports to . 44 | * 45 | * The authors may be contacted via: 46 | * 47 | * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha 48 | * Dept. of Computer Science 49 | * 201 S. Columbia St. 50 | * Frederick P. Brooks, Jr. Computer Science Bldg. 51 | * Chapel Hill, N.C. 27599-3175 52 | * United States of America 53 | * 54 | * 55 | */ 56 | 57 | #include "Obstacle.h" 58 | #include "RVOSimulator.h" 59 | 60 | namespace RVO { 61 | Obstacle::Obstacle() : isConvex_(false), nextObstacle_(NULL), prevObstacle_(NULL), id_(0) { } 62 | } 63 | -------------------------------------------------------------------------------- /rvo2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # CMakeLists.txt 3 | # RVO2 Library 4 | # 5 | # Copyright (c) 2008-2010 University of North Carolina at Chapel Hill. 6 | # All rights reserved. 7 | # 8 | # Permission to use, copy, modify, and distribute this software and its 9 | # documentation for educational, research, and non-profit purposes, without fee, 10 | # and without a written agreement is hereby granted, provided that the above 11 | # copyright notice, this paragraph, and the following four paragraphs appear in 12 | # all copies. 13 | # 14 | # Permission to incorporate this software into commercial products may be 15 | # obtained by contacting the authors or the Office of 16 | # Technology Development at the University of North Carolina at Chapel Hill 17 | # . 18 | # 19 | # This software program and documentation are copyrighted by the University of 20 | # North Carolina at Chapel Hill. The software program and documentation are 21 | # supplied "as is," without any accompanying services from the University of 22 | # North Carolina at Chapel Hill or the authors. The University of North Carolina 23 | # at Chapel Hill and the authors do not warrant that the operation of the 24 | # program will be uninterrupted or error-free. The end-user understands that the 25 | # program was developed for research purposes and is advised not to rely 26 | # exclusively on the program for any reason. 27 | # 28 | # IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE 29 | # AUTHORS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR 30 | # CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS 31 | # SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF NORTH CAROLINA AT 32 | # CHAPEL HILL OR THE AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH 33 | # DAMAGE. 34 | # 35 | # THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS SPECIFICALLY 36 | # DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 37 | # OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AND ANY STATUTORY 38 | # WARRANTY OF NON-INFRINGEMENT. THE SOFTWARE PROVIDED HEREUNDER IS ON AN "AS IS" 39 | # BASIS, AND THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS 40 | # HAVE NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR 41 | # MODIFICATIONS. 42 | # 43 | # Please send all bug reports to . 44 | # 45 | # The authors may be contacted via: 46 | # 47 | # Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha 48 | # Dept. of Computer Science 49 | # 201 S. Columbia St. 50 | # Frederick P. Brooks, Jr. Computer Science Bldg. 51 | # Chapel Hill, N.C. 27599-3175 52 | # United States of America 53 | # 54 | # 55 | # 56 | 57 | cmake_minimum_required(VERSION 2.8.3) 58 | project(rvo2) 59 | 60 | find_package(catkin) 61 | find_package(OpenMP REQUIRED) 62 | catkin_package(INCLUDE_DIRS include LIBRARIES rvo2) 63 | include_directories(include/RVO) 64 | 65 | add_library(rvo2 66 | src/Agent.cpp src/KdTree.cpp src/Obstacle.cpp src/RVOSimulator.cpp) 67 | 68 | target_link_libraries(rvo2 OpenMP::OpenMP_CXX) 69 | -------------------------------------------------------------------------------- /player_map/include/player_map/rosmap.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ROSMAP_HPP 2 | #define ROSMAP_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | namespace rvo { 17 | typedef std::vector > PointVector; 18 | 19 | double pathLength(const rvo::PointVector &path); 20 | 21 | class OccupancyMap { 22 | public: 23 | OccupancyMap(map_t *map); 24 | ~OccupancyMap(); 25 | 26 | static OccupancyMap* FromMapServer(const char *srv_name); 27 | 28 | void setMap(const nav_msgs::OccupancyGrid &grid); 29 | void updateCSpace(double max_occ_dist); 30 | 31 | double minX() { return MAP_WXGX(map_, 0); } 32 | double minY() { return MAP_WYGY(map_, 0); } 33 | double maxX() { return MAP_WXGX(map_, map_->size_x); } 34 | double maxY() { return MAP_WYGY(map_, map_->size_y); } 35 | 36 | const map_cell_t* getCell(double x, double y) { 37 | return map_get_cell(map_, x, y, 0); 38 | } 39 | 40 | bool lineOfSight(double x1, double y1, double x2, double y2, 41 | double max_occ_dist = 0.0) const; 42 | PointVector astar(double x1, double y1, double x2, double y2, 43 | double max_occ_dist = 0.0); 44 | 45 | // TODO: Unify these two APIs 46 | 47 | // Get a list of endpoints 48 | const PointVector& prepareShortestPaths(double x, double y, 49 | double max_occ_dist, 50 | double min_dist, double max_dist); 51 | // Get the path whose endpoint is ind from last call to prepareShortestPaths() 52 | PointVector buildShortestPath(int ind); 53 | 54 | // Calculate single source shortest paths to all endpoints 55 | // Returns a vector, element at index i is true if path from dests[i] exists 56 | // Use buildShortestPath to construct path; index matches point in dests 57 | void prepareAllShortestPaths(double x, double y, double max_occ_dist); 58 | // Get shortest path 59 | PointVector shortestPath(double x, double y); 60 | 61 | private: 62 | struct Node { 63 | Node() {} 64 | Node(const std::pair &c, float d, float h) : 65 | coord(c), true_dist(d), heuristic(h) { } 66 | std::pair coord; 67 | float true_dist; 68 | float heuristic; 69 | }; 70 | 71 | struct NodeCompare { 72 | bool operator()(const Node &lnode, const Node &rnode) { 73 | return make_pair(lnode.heuristic, lnode.coord) < 74 | make_pair(rnode.heuristic, rnode.coord); 75 | } 76 | }; 77 | 78 | void initializeSearch(double startx, double starty); 79 | bool nextNode(double max_occ_dist, Node *curr_node); 80 | void addNeighbors(const Node &node, double max_occ_dist); 81 | void buildPath(int i, int j, PointVector *path); 82 | 83 | map_t *map_; 84 | int ncells_; 85 | int starti_, startj_; 86 | int stopi_, stopj_; 87 | boost::scoped_array costs_; 88 | boost::scoped_array prev_i_; 89 | boost::scoped_array prev_j_; 90 | // Priority queue mapping cost to index 91 | boost::scoped_ptr > Q_; 92 | PointVector endpoints_; 93 | }; 94 | 95 | } 96 | #endif 97 | -------------------------------------------------------------------------------- /rvo2/src/Obstacle.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Obstacle.h 3 | * RVO2 Library 4 | * 5 | * Copyright (c) 2008-2010 University of North Carolina at Chapel Hill. 6 | * All rights reserved. 7 | * 8 | * Permission to use, copy, modify, and distribute this software and its 9 | * documentation for educational, research, and non-profit purposes, without 10 | * fee, and without a written agreement is hereby granted, provided that the 11 | * above copyright notice, this paragraph, and the following four paragraphs 12 | * appear in all copies. 13 | * 14 | * Permission to incorporate this software into commercial products may be 15 | * obtained by contacting the authors or the Office of 16 | * Technology Development at the University of North Carolina at Chapel Hill 17 | * . 18 | * 19 | * This software program and documentation are copyrighted by the University of 20 | * North Carolina at Chapel Hill. The software program and documentation are 21 | * supplied "as is," without any accompanying services from the University of 22 | * North Carolina at Chapel Hill or the authors. The University of North 23 | * Carolina at Chapel Hill and the authors do not warrant that the operation of 24 | * the program will be uninterrupted or error-free. The end-user understands 25 | * that the program was developed for research purposes and is advised not to 26 | * rely exclusively on the program for any reason. 27 | * 28 | * IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE 29 | * AUTHORS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR 30 | * CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS 31 | * SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF NORTH CAROLINA AT 32 | * CHAPEL HILL OR THE AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH 33 | * DAMAGE. 34 | * 35 | * THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS SPECIFICALLY 36 | * DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 37 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AND ANY 38 | * STATUTORY WARRANTY OF NON-INFRINGEMENT. THE SOFTWARE PROVIDED HEREUNDER IS ON 39 | * AN "AS IS" BASIS, AND THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE 40 | * AUTHORS HAVE NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, 41 | * ENHANCEMENTS, OR MODIFICATIONS. 42 | * 43 | * Please send all bug reports to . 44 | * 45 | * The authors may be contacted via: 46 | * 47 | * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha 48 | * Dept. of Computer Science 49 | * 201 S. Columbia St. 50 | * Frederick P. Brooks, Jr. Computer Science Bldg. 51 | * Chapel Hill, N.C. 27599-3175 52 | * United States of America 53 | * 54 | * 55 | */ 56 | 57 | #ifndef RVO_OBSTACLE_H_ 58 | #define RVO_OBSTACLE_H_ 59 | 60 | /** 61 | * \file Obstacle.h 62 | * \brief Contains the Obstacle class. 63 | */ 64 | 65 | #include "Definitions.h" 66 | 67 | namespace RVO { 68 | /** 69 | * \brief Defines static obstacles in the simulation. 70 | */ 71 | class Obstacle { 72 | private: 73 | /** 74 | * \brief Constructs a static obstacle instance. 75 | */ 76 | Obstacle(); 77 | 78 | bool isConvex_; 79 | Obstacle *nextObstacle_; 80 | Vector2 point_; 81 | Obstacle *prevObstacle_; 82 | Vector2 unitDir_; 83 | 84 | size_t id_; 85 | 86 | friend class Agent; 87 | friend class KdTree; 88 | friend class RVOSimulator; 89 | }; 90 | } 91 | 92 | #endif /* RVO_OBSTACLE_H_ */ 93 | -------------------------------------------------------------------------------- /rvo_move/urdf/robot.urdf: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /player_map/src/draw_paths.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | using namespace rvo; 9 | 10 | int main(int argc, char **argv) { 11 | ros::init(argc, argv, "draw_paths"); 12 | 13 | ros::NodeHandle nh; 14 | ros::Publisher pub = nh.advertise("/marker_array", 10, true); 15 | 16 | boost::scoped_ptr map; 17 | map.reset(OccupancyMap::FromMapServer("/static_map")); 18 | map->updateCSpace(1.0); 19 | double start_x = 9.0; 20 | double start_y = 12.5; 21 | Eigen::Vector2f start(start_x, start_y); 22 | double dest_x = 19.0; 23 | double dest_y = 23.0; 24 | 25 | visualization_msgs::MarkerArray ma; 26 | if (false) { 27 | ROS_INFO("Planning"); 28 | PointVector path = map->astar(start_x, start_y, dest_x, dest_y, 29 | 0.25); 30 | ROS_INFO("Done"); 31 | 32 | ma.markers.resize(1); 33 | visualization_msgs::Marker &m = ma.markers.at(0); 34 | m.action = visualization_msgs::Marker::ADD; 35 | m.type = visualization_msgs::Marker::LINE_STRIP; 36 | m.header.stamp = ros::Time(); 37 | m.header.frame_id = "/map"; 38 | m.id = 1; 39 | m.ns = "/draw_paths"; 40 | m.pose.orientation.w = 1.0; 41 | m.scale.x = 0.05; 42 | m.color.r = 1.0; 43 | m.color.a = 1.0; 44 | m.points.resize(path.size()); 45 | for (size_t i = 0; i < path.size(); ++i) { 46 | m.points.at(i).x = path.at(i)(0); 47 | m.points.at(i).y = path.at(i)(1); 48 | } 49 | } else { 50 | ROS_INFO("Planning"); 51 | const PointVector& endpoints = map->prepareShortestPaths(start_x, start_y, 52 | 6.0, 2.0, 0.3); 53 | ROS_INFO("Done; got %zu", endpoints.size()); 54 | 55 | double sep = 2.0; 56 | double los_margin = 0.1; 57 | std::vector inds; 58 | inds.push_back(0); 59 | for (size_t curr = 1; curr < endpoints.size(); ++curr) { 60 | // Check if within threshold to any point 61 | bool too_close = false; 62 | for (size_t prev = 0; prev < inds.size(); ++prev) { 63 | Eigen::Vector2f first, second; 64 | first = endpoints.at(curr); 65 | second = endpoints.at(inds.at(prev)); 66 | double distance = (first - second).norm(); 67 | if (distance < sep || (first - start).norm() < 4.0) { 68 | too_close = true; 69 | break; 70 | } 71 | } 72 | if (too_close) { 73 | continue; 74 | } else { 75 | inds.push_back(curr); 76 | } 77 | } 78 | ROS_INFO("inds.size() = %zu", inds.size()); 79 | 80 | visualization_msgs::Marker base; 81 | base.action = visualization_msgs::Marker::ADD; 82 | base.type = visualization_msgs::Marker::LINE_STRIP; 83 | base.header.stamp = ros::Time(); 84 | base.header.frame_id = "/map"; 85 | base.id = 0; 86 | base.ns = "/draw_paths"; 87 | base.pose.orientation.w = 1.0; 88 | base.scale.x = 0.05; 89 | base.color.r = 1.0; 90 | base.color.a = 1.0; 91 | 92 | ma.markers.resize(inds.size()); 93 | for (size_t i = 0; i < inds.size(); ++i) { 94 | visualization_msgs::Marker &m = ma.markers.at(i); 95 | m = base; 96 | m.id = i; 97 | PointVector path = map->buildShortestPath(inds.at(i)); 98 | m.points.resize(path.size()); 99 | for (size_t i = 0; i < path.size(); ++i) { 100 | m.points.at(i).x = path.at(i)(0); 101 | m.points.at(i).y = path.at(i)(1); 102 | } 103 | } 104 | } 105 | 106 | pub.publish(ma); 107 | ros::spin(); 108 | } 109 | -------------------------------------------------------------------------------- /player_map/src/map.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Player - One Hell of a Robot Server 3 | * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 4 | * gerkey@usc.edu kaspers@robotics.usc.edu 5 | * 6 | * This library is free software; you can redistribute it and/or 7 | * modify it under the terms of the GNU Lesser General Public 8 | * License as published by the Free Software Foundation; either 9 | * version 2.1 of the License, or (at your option) any later version. 10 | * 11 | * This library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | * Lesser General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU Lesser General Public 17 | * License along with this library; if not, write to the Free Software 18 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 19 | * 20 | */ 21 | /************************************************************************** 22 | * Desc: Global map (grid-based) 23 | * Author: Andrew Howard 24 | * Date: 6 Feb 2003 25 | * CVS: $Id: map.c 1713 2003-08-23 04:03:43Z inspectorg $ 26 | **************************************************************************/ 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #include "player_map/map.h" 35 | 36 | // Create a new map 37 | map_t *map_alloc(void) 38 | { 39 | map_t *map; 40 | 41 | map = (map_t*) malloc(sizeof(map_t)); 42 | 43 | // Assume we start at (0, 0) 44 | map->origin_x = 0; 45 | map->origin_y = 0; 46 | 47 | // Make the size odd 48 | map->size_x = 0; 49 | map->size_y = 0; 50 | map->scale = 0; 51 | 52 | // Allocate storage for main map 53 | map->cells = (map_cell_t*) NULL; 54 | 55 | return map; 56 | } 57 | 58 | 59 | // Destroy a map 60 | void map_free(map_t *map) 61 | { 62 | free(map->cells); 63 | free(map); 64 | return; 65 | } 66 | 67 | 68 | // Get the cell at the given point 69 | map_cell_t *map_get_cell(map_t *map, double ox, double oy, double oa) 70 | { 71 | int i, j; 72 | map_cell_t *cell; 73 | 74 | i = MAP_GXWX(map, ox); 75 | j = MAP_GYWY(map, oy); 76 | 77 | if (!MAP_VALID(map, i, j)) 78 | return NULL; 79 | 80 | cell = map->cells + MAP_INDEX(map, i, j); 81 | return cell; 82 | } 83 | 84 | 85 | // Update the cspace distance values 86 | // TODO: replace this with a more efficient implementation. Not crucial, 87 | // because we only do it once, at startup. 88 | void map_update_cspace(map_t *map, double max_occ_dist) 89 | { 90 | int i, j; 91 | int ni, nj; 92 | int s; 93 | double d; 94 | map_cell_t *cell, *ncell; 95 | 96 | map->max_occ_dist = max_occ_dist; 97 | s = (int) ceil(map->max_occ_dist / map->scale); 98 | 99 | // Reset the distance values 100 | for (j = 0; j < map->size_y; j++) 101 | { 102 | for (i = 0; i < map->size_x; i++) 103 | { 104 | cell = map->cells + MAP_INDEX(map, i, j); 105 | cell->occ_dist = map->max_occ_dist; 106 | } 107 | } 108 | 109 | // Find all the occupied cells and update their neighbours 110 | for (j = 0; j < map->size_y; j++) 111 | { 112 | for (i = 0; i < map->size_x; i++) 113 | { 114 | cell = map->cells + MAP_INDEX(map, i, j); 115 | if (cell->occ_state != +1) 116 | continue; 117 | 118 | cell->occ_dist = 0; 119 | 120 | // Update adjacent cells 121 | for (nj = -s; nj <= +s; nj++) 122 | { 123 | for (ni = -s; ni <= +s; ni++) 124 | { 125 | if (!MAP_VALID(map, i + ni, j + nj)) 126 | continue; 127 | 128 | ncell = map->cells + MAP_INDEX(map, i + ni, j + nj); 129 | d = map->scale * sqrt(ni * ni + nj * nj); 130 | 131 | if (d < ncell->occ_dist) 132 | ncell->occ_dist = d; 133 | } 134 | } 135 | } 136 | } 137 | 138 | return; 139 | } 140 | -------------------------------------------------------------------------------- /rvo_move/src/bot_client.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | using namespace std; 9 | 10 | namespace rvo { 11 | BotClient::BotClient(string prefix) : 12 | got_odom_(false), got_pose_(false) { 13 | name_ = prefix; 14 | odom_sub_ = nh_.subscribe("odom", 5, &BotClient::odomCallback, this); 15 | 16 | tf::StampedTransform transform; 17 | if (tf_listener_.waitForTransform("/map", name_ + "/odom", ros::Time(0), ros::Duration(0.5))) { 18 | tf_listener_.lookupTransform("/map", name_ + "/odom", ros::Time(0), transform); 19 | got_pose_ = true; 20 | pose_.pose.position.x = transform.getOrigin().x(); 21 | pose_.pose.position.y = transform.getOrigin().y(); 22 | pose_.pose.orientation.w = transform.getRotation().getW(); 23 | pose_.pose.orientation.x = transform.getRotation().getX(); 24 | pose_.pose.orientation.y = transform.getRotation().getY(); 25 | pose_.pose.orientation.z = transform.getRotation().getZ(); 26 | pose_.header.stamp = ros::Time::now(); 27 | } 28 | assert(got_pose_); 29 | 30 | vel_pub_ = nh_.advertise("cmd_vel", 5); 31 | } 32 | 33 | void BotClient::odomCallback(const nav_msgs::Odometry &msg) { 34 | boost::mutex::scoped_lock lock(mutex_); 35 | got_odom_ = true; 36 | odom_ = msg; 37 | // ROS_WARN_STREAM_THROTTLE(2, "" << getName() << ": odom = " << msg.twist.twist.linear.x << 38 | // ", " << msg.twist.twist.angular.z); 39 | } 40 | 41 | void BotClient::poseCallback(const geometry_msgs::PoseWithCovarianceStamped &msg) { 42 | boost::mutex::scoped_lock lock(mutex_); 43 | got_pose_ = true; 44 | pose_.pose = msg.pose.pose; 45 | pose_.header = msg.header; 46 | // ROS_WARN_STREAM_THROTTLE(2, "" << getName() << ": pose = " << 47 | // pose_.pose.position.x << ", " << pose_.pose.position.y); 48 | } 49 | 50 | nav_msgs::Odometry BotClient::getOdom() { 51 | boost::mutex::scoped_lock lock(mutex_); 52 | return odom_; 53 | } 54 | 55 | geometry_msgs::Pose BotClient::getPose() { 56 | boost::mutex::scoped_lock lock(mutex_); 57 | 58 | tf::StampedTransform transform; 59 | ros::Time now = ros::Time::now(); 60 | 61 | bool succeed = false; 62 | if (tf_listener_.waitForTransform("/map", name_ + "/base_link", now, ros::Duration(0.5))) { 63 | succeed = true; 64 | tf_listener_.lookupTransform("/map", name_ + "/base_link", now, transform); 65 | pose_.pose.position.x = transform.getOrigin().x(); 66 | pose_.pose.position.y = transform.getOrigin().y(); 67 | pose_.pose.orientation.w = transform.getRotation().getW(); 68 | pose_.pose.orientation.x = transform.getRotation().getX(); 69 | pose_.pose.orientation.y = transform.getRotation().getY(); 70 | pose_.pose.orientation.z = transform.getRotation().getZ(); 71 | pose_.header.stamp = ros::Time::now(); 72 | } 73 | 74 | assert(succeed); 75 | 76 | return pose_.pose; 77 | } 78 | 79 | bool BotClient::haveOdom() { 80 | boost::mutex::scoped_lock lock(mutex_); 81 | return got_odom_; 82 | } 83 | 84 | bool BotClient::haveOdom(const ros::Duration &d) { 85 | boost::mutex::scoped_lock lock(mutex_); 86 | return got_odom_ && (ros::Time::now() - odom_.header.stamp) < d; 87 | } 88 | 89 | bool BotClient::havePose() { 90 | boost::mutex::scoped_lock lock(mutex_); 91 | return got_pose_; 92 | } 93 | 94 | bool BotClient::havePose(const ros::Duration &d) { 95 | boost::mutex::scoped_lock lock(mutex_); 96 | return got_pose_ && (ros::Time::now() - pose_.header.stamp) < d; 97 | } 98 | 99 | vector BotClient::MakeBots(const ros::NodeHandle& nh) { 100 | vector bots; 101 | int nbots; 102 | nh.param("nbots", nbots, 0); 103 | // instantiate listeners 104 | for (int id = 0; id < nbots; id++) { 105 | stringstream ss; 106 | ss.str(); 107 | ss << "/robot_" << id; 108 | bots.push_back(new BotClient(ss.str())); 109 | } 110 | return bots; 111 | } 112 | 113 | void BotClient::FreeBots(vector bots) { 114 | for (size_t i = 0; i < bots.size(); ++i) { 115 | delete bots[i]; 116 | } 117 | } 118 | 119 | void BotClient::pubVel(double v, double w) { 120 | geometry_msgs::Twist t; 121 | t.linear.x = v; 122 | t.angular.z = w; 123 | vel_pub_.publish(t); 124 | } 125 | } 126 | -------------------------------------------------------------------------------- /player_map/include/player_map/map.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Player - One Hell of a Robot Server 3 | * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 4 | * gerkey@usc.edu kaspers@robotics.usc.edu 5 | * 6 | * This library is free software; you can redistribute it and/or 7 | * modify it under the terms of the GNU Lesser General Public 8 | * License as published by the Free Software Foundation; either 9 | * version 2.1 of the License, or (at your option) any later version. 10 | * 11 | * This library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | * Lesser General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU Lesser General Public 17 | * License along with this library; if not, write to the Free Software 18 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 19 | * 20 | */ 21 | /************************************************************************** 22 | * Desc: Global map (grid-based) 23 | * Author: Andrew Howard 24 | * Date: 6 Feb 2003 25 | * CVS: $Id: map.h 1713 2003-08-23 04:03:43Z inspectorg $ 26 | **************************************************************************/ 27 | 28 | #ifndef MAP_H 29 | #define MAP_H 30 | 31 | #include 32 | 33 | #ifdef __cplusplus 34 | extern "C" { 35 | #endif 36 | 37 | // Forward declarations 38 | struct _rtk_fig_t; 39 | 40 | 41 | // Limits 42 | #define MAP_WIFI_MAX_LEVELS 8 43 | 44 | 45 | // Description for a single map cell. 46 | typedef struct 47 | { 48 | // Occupancy state (-1 = free, 0 = unknown, +1 = occ) 49 | int occ_state; 50 | 51 | // Distance to the nearest occupied cell 52 | double occ_dist; 53 | 54 | // Wifi levels 55 | //int wifi_levels[MAP_WIFI_MAX_LEVELS]; 56 | 57 | } map_cell_t; 58 | 59 | 60 | // Description for a map 61 | typedef struct 62 | { 63 | // Map origin; the map is a viewport onto a conceptual larger map. 64 | double origin_x, origin_y; 65 | 66 | // Map scale (m/cell) 67 | double scale; 68 | 69 | // Map dimensions (number of cells) 70 | int size_x, size_y; 71 | 72 | // The map data, stored as a grid 73 | map_cell_t *cells; 74 | 75 | // Max distance at which we care about obstacles, for constructing 76 | // likelihood field 77 | double max_occ_dist; 78 | 79 | } map_t; 80 | 81 | 82 | 83 | /************************************************************************** 84 | * Basic map functions 85 | **************************************************************************/ 86 | 87 | // Create a new (empty) map 88 | map_t *map_alloc(void); 89 | 90 | // Destroy a map 91 | void map_free(map_t *map); 92 | 93 | // Get the cell at the given point 94 | map_cell_t *map_get_cell(map_t *map, double ox, double oy, double oa); 95 | 96 | // Load an occupancy map 97 | int map_load_occ(map_t *map, const char *filename, double scale, int negate); 98 | 99 | // Load a wifi signal strength map 100 | //int map_load_wifi(map_t *map, const char *filename, int index); 101 | 102 | // Update the cspace distances 103 | void map_update_cspace(map_t *map, double max_occ_dist); 104 | 105 | 106 | /************************************************************************** 107 | * Range functions 108 | **************************************************************************/ 109 | 110 | // Extract a single range reading from the map 111 | double map_calc_range(map_t *map, double ox, double oy, double oa, double max_range); 112 | 113 | 114 | /************************************************************************** 115 | * GUI/diagnostic functions 116 | **************************************************************************/ 117 | 118 | // Draw the occupancy grid 119 | void map_draw_occ(map_t *map, struct _rtk_fig_t *fig); 120 | 121 | // Draw the cspace map 122 | void map_draw_cspace(map_t *map, struct _rtk_fig_t *fig); 123 | 124 | // Draw a wifi map 125 | void map_draw_wifi(map_t *map, struct _rtk_fig_t *fig, int index); 126 | 127 | 128 | /************************************************************************** 129 | * Map manipulation macros 130 | **************************************************************************/ 131 | 132 | // Convert from map index to world coords 133 | #define MAP_WXGX(map, i) (map->origin_x + ((i) - map->size_x / 2) * map->scale) 134 | #define MAP_WYGY(map, j) (map->origin_y + ((j) - map->size_y / 2) * map->scale) 135 | 136 | // Convert from world coords to map coords 137 | #define MAP_GXWX(map, x) (floor((x - map->origin_x) / map->scale + 0.5) + map->size_x / 2) 138 | #define MAP_GYWY(map, y) (floor((y - map->origin_y) / map->scale + 0.5) + map->size_y / 2) 139 | 140 | // Test to see if the given map coords lie within the absolute map bounds. 141 | #define MAP_VALID(map, i, j) ((i >= 0) && (i < map->size_x) && (j >= 0) && (j < map->size_y)) 142 | 143 | // Compute the cell index for the given map coords. 144 | #define MAP_INDEX(map, i, j) ((i) + (j) * map->size_x) 145 | 146 | #ifdef __cplusplus 147 | } 148 | #endif 149 | 150 | #endif 151 | -------------------------------------------------------------------------------- /rvo2/src/Definitions.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Definitions.h 3 | * RVO2 Library 4 | * 5 | * Copyright (c) 2008-2010 University of North Carolina at Chapel Hill. 6 | * All rights reserved. 7 | * 8 | * Permission to use, copy, modify, and distribute this software and its 9 | * documentation for educational, research, and non-profit purposes, without 10 | * fee, and without a written agreement is hereby granted, provided that the 11 | * above copyright notice, this paragraph, and the following four paragraphs 12 | * appear in all copies. 13 | * 14 | * Permission to incorporate this software into commercial products may be 15 | * obtained by contacting the authors or the Office of 16 | * Technology Development at the University of North Carolina at Chapel Hill 17 | * . 18 | * 19 | * This software program and documentation are copyrighted by the University of 20 | * North Carolina at Chapel Hill. The software program and documentation are 21 | * supplied "as is," without any accompanying services from the University of 22 | * North Carolina at Chapel Hill or the authors. The University of North 23 | * Carolina at Chapel Hill and the authors do not warrant that the operation of 24 | * the program will be uninterrupted or error-free. The end-user understands 25 | * that the program was developed for research purposes and is advised not to 26 | * rely exclusively on the program for any reason. 27 | * 28 | * IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE 29 | * AUTHORS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR 30 | * CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS 31 | * SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF NORTH CAROLINA AT 32 | * CHAPEL HILL OR THE AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH 33 | * DAMAGE. 34 | * 35 | * THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS SPECIFICALLY 36 | * DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 37 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AND ANY 38 | * STATUTORY WARRANTY OF NON-INFRINGEMENT. THE SOFTWARE PROVIDED HEREUNDER IS ON 39 | * AN "AS IS" BASIS, AND THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE 40 | * AUTHORS HAVE NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, 41 | * ENHANCEMENTS, OR MODIFICATIONS. 42 | * 43 | * Please send all bug reports to . 44 | * 45 | * The authors may be contacted via: 46 | * 47 | * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha 48 | * Dept. of Computer Science 49 | * 201 S. Columbia St. 50 | * Frederick P. Brooks, Jr. Computer Science Bldg. 51 | * Chapel Hill, N.C. 27599-3175 52 | * United States of America 53 | * 54 | * 55 | */ 56 | 57 | #ifndef RVO_DEFINITIONS_H_ 58 | #define RVO_DEFINITIONS_H_ 59 | 60 | /** 61 | * \file Definitions.h 62 | * \brief Contains functions and constants used in multiple classes. 63 | */ 64 | 65 | #include 66 | #include 67 | #include 68 | #include 69 | 70 | #include "Vector2.h" 71 | 72 | /** 73 | * \brief A sufficiently small positive number. 74 | */ 75 | const float RVO_EPSILON = 0.00001f; 76 | 77 | namespace RVO { 78 | class Agent; 79 | class Obstacle; 80 | class RVOSimulator; 81 | 82 | /** 83 | * \brief Computes the squared distance from a line segment with the 84 | * specified endpoints to a specified point. 85 | * \param a The first endpoint of the line segment. 86 | * \param b The second endpoint of the line segment. 87 | * \param c The point to which the squared distance is to 88 | * be calculated. 89 | * \return The squared distance from the line segment to the point. 90 | */ 91 | inline float distSqPointLineSegment(const Vector2 &a, const Vector2 &b, 92 | const Vector2 &c) 93 | { 94 | const float r = ((c - a) * (b - a)) / absSq(b - a); 95 | 96 | if (r < 0.0f) { 97 | return absSq(c - a); 98 | } 99 | else if (r > 1.0f) { 100 | return absSq(c - b); 101 | } 102 | else { 103 | return absSq(c - (a + r * (b - a))); 104 | } 105 | } 106 | 107 | /** 108 | * \brief Computes the signed distance from a line connecting the 109 | * specified points to a specified point. 110 | * \param a The first point on the line. 111 | * \param b The second point on the line. 112 | * \param c The point to which the signed distance is to 113 | * be calculated. 114 | * \return Positive when the point c lies to the left of the line ab. 115 | */ 116 | inline float leftOf(const Vector2 &a, const Vector2 &b, const Vector2 &c) 117 | { 118 | return det(a - c, b - a); 119 | } 120 | 121 | /** 122 | * \brief Computes the square of a float. 123 | * \param a The float to be squared. 124 | * \return The square of the float. 125 | */ 126 | inline float sqr(float a) 127 | { 128 | return a * a; 129 | } 130 | } 131 | 132 | #endif /* RVO_DEFINITIONS_H_ */ 133 | -------------------------------------------------------------------------------- /Readme.md: -------------------------------------------------------------------------------- 1 | # RVO2 ROS WRAPPER 2 | 3 | ## RVO2 LIBRARY 4 | 5 | https://gamma.cs.unc.edu/RVO2/ 6 | 7 | ## Wrapper 8 | 9 | https://github.com/bcharrow/rvo 10 | 11 | ## 介绍 12 | 13 | 基于**OCRA**算法，分布式的运行在每个机器人上。 14 | 15 | ### 包名 16 | 17 | ​ rvo_move 18 | 19 | ### 节点组成 20 | 21 | - **move_client**(src/move_client.cpp) 22 | - **订阅话题** 23 | - **名称**：\goal 24 | - **类型**：geometry_msgs::PoseStamped 25 | - **介绍**：接收目标点 26 | - **发布话题**：- 27 | - **Action**： 28 | - Move.action(action/Move.action) 29 | - **介绍**：action客户端，在接收到\goal话题后，通过action向move_server发送请求，move_server会计算agent的路线，并在行进过程中进行碰撞避免。 30 | 31 | - **move_server**(src/move_server.cpp) 32 | 33 | - **订阅话题** 34 | 35 | - **名称**：\odom 36 | 37 | - **类型**：nav_msgs::Odometry 38 | 39 | - **介绍**：每个机器人的move_sever节点都会订阅系统中所有机器人的\odom话题来获取各机器人的位置。 40 | 41 | ------ 42 | 43 | - **名称**：\map 44 | 45 | - **类型**：- 46 | 47 | - **介绍**：栅格地图 48 | 49 | - **发布话题**： 50 | 51 | - **名称**：\cmd_vel 52 | - **类型**：geometry_msgs::Twist 53 | - **介绍**： 向move_server节点所属的机器人发送速度话题来控制机器人的行进速度和方向。 54 | 55 | - **Action**： 56 | 57 | - Move.action(action/Move.action) 58 | - **介绍**：action服务端，接受action客户端的请求，action客户端会发送一个目标位置，move_server节点会根据A*算法计算路线，并实时的进行碰撞避免。 59 | 60 | ## 配置 61 | 62 | 部分参数的解释： 63 | 64 | - radius：机器人的半径 65 | - maxSpeed：机器人可以行进的最大速度 66 | - goal_tolerance：机器人距离终点的最大容忍距离（停在终点附近即可看作是导航完成）。 67 | 68 | ```yaml 69 | #config/scarab_move.yaml 70 | neighborDist: 10.0 71 | maxNeighbors: 10 72 | timeHorizon: 10.0 73 | timeHorizonObst: 3.0 74 | radius: 0.08 75 | maxSpeed: 1 76 | timestep: 0.2 # 1 / controller rate 77 | axel_width: 0.255 78 | los_margin: 0.1 79 | 80 | waypoint_spacing: 0.1 81 | path_margin: 0.6 82 | goal_tolerance: 0.1 83 | 84 | # http://gamma.cs.unc.edu/RVO2/documentation/2.0/params.html 85 | # timeStep 86 | # float (time) 87 | # The time step of the simulation. Must be positive. 88 | # maxNeighbors 89 | # size_t 90 | # The maximal number of other agents the agent takes into account in the 91 | # navigation. The larger this number, the longer the running time of the 92 | # simulation. If the number is too low, the simulation will not be safe. 93 | # maxSpeed 94 | # float (distance/time) 95 | # The maximum speed of the agent. Must be non-negative. 96 | # neighborDist 97 | # float (distance) 98 | # The maximal distance (center point to center point) to other agents the 99 | # agent takes into account in the navigation. The larger this number, the 100 | # longer the running time of the simulation. If the number is too low, 101 | # the simulation will not be safe. Must be non-negative. 102 | # radius 103 | # float (distance) 104 | # The radius of the agent. Must be non-negative. 105 | # timeHorizon 106 | # float (time) 107 | # The minimal amount of time for which the agent's velocities that are 108 | # computed by the simulation are safe with respect to other agents. The 109 | # larger this number, the sooner this agent will respond to the presence 110 | # of other agents, but the less freedom the agent has in choosing its 111 | # velocities. Must be positive. 112 | # timeHorizonObst 113 | # float (time) 114 | # The minimal amount of time for which the agent's velocities that are 115 | # computed by the simulation are safe with respect to obstacles. The 116 | # larger this number, the sooner this agent will respond to the presence 117 | # of obstacles, but the less freedom the agent has in choosing its 118 | # velocities. Must be positive. 119 | 120 | ``` 121 | 122 | ## Example 123 | 124 | 用launch文件，启动16个机器人。 125 | 126 | 每个机器人launch文件（递归启动多个机器人）(recursive_robots.launch) 127 | 128 | ```xml 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | ``` 155 | 156 | 总的launch文件(move_launch) 157 | 158 | ```xml 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | ``` 176 | 177 | -------------------------------------------------------------------------------- /player_map/src/rosmap_test.cpp: -------------------------------------------------------------------------------- 1 | #include "gtest/gtest.h" 2 | 3 | #include 4 | 5 | #include 6 | 7 | using namespace Eigen; 8 | using namespace rvo; 9 | enum { 10 | FREE = -1, 11 | UNKNOWN = 0, 12 | OCCUPIED = 1 13 | }; 14 | 15 | class AstarTest : public testing::Test { 16 | protected: 17 | void SetUp() { 18 | // Make 3x3 completely free map centered at origin 19 | Map3x3_ = map_alloc(); 20 | int width = 3, height = 3; 21 | Map3x3_->max_occ_dist = 0.0; 22 | Map3x3_->origin_x = 0.0; 23 | Map3x3_->origin_y = 0.0; 24 | Map3x3_->size_x = width; 25 | Map3x3_->size_y = height; 26 | Map3x3_->scale = 1.0; 27 | Map3x3_->cells = (map_cell_t*) calloc(width * height, sizeof(Map3x3_->cells[0])); 28 | for (int x = -1; x < 2; ++x) { 29 | for (int y = -1; y < 2; ++y) { 30 | map_get_cell(Map3x3_, x, y, 0)->occ_state = FREE; 31 | } 32 | } 33 | OccMap3x3_.reset(new OccupancyMap(Map3x3_)); 34 | 35 | // Make 100x100 completely free map centered at origin 36 | Map100x100_ = map_alloc(); 37 | width = 100, height = 100; 38 | Map100x100_->max_occ_dist = 0.0; 39 | Map100x100_->origin_x = 0.0; 40 | Map100x100_->origin_y = 0.0; 41 | Map100x100_->size_x = width; 42 | Map100x100_->size_y = height; 43 | Map100x100_->scale = 1.0; 44 | Map100x100_->cells = (map_cell_t*) calloc(width * height, sizeof(Map100x100_->cells[0])); 45 | for (int x = -50; x < 50; ++x) { 46 | for (int y = -50; y < 50; ++y) { 47 | map_get_cell(Map100x100_, x, y, 0)->occ_state = FREE; 48 | } 49 | } 50 | OccMap100x100_.reset(new OccupancyMap(Map100x100_)); 51 | } 52 | 53 | boost::scoped_ptr OccMap3x3_, OccMap100x100_; 54 | map_t *Map3x3_, *Map100x100_; 55 | }; 56 | 57 | TEST_F(AstarTest, OneStepRight) { 58 | Vector2f start(0, 0), stop(0, 1); 59 | PointVector path = OccMap3x3_->astar(start(0), start(1), stop(0), stop(1)); 60 | ASSERT_EQ(path.size(), (unsigned)2); 61 | EXPECT_EQ(path[0], start); 62 | EXPECT_EQ(path[1], stop); 63 | } 64 | 65 | TEST_F(AstarTest, OneStepDiag) { 66 | Vector2f start(0, 0), stop(1, 1); 67 | PointVector path = OccMap3x3_->astar(start(0), start(1), stop(0), stop(1)); 68 | ASSERT_EQ(path.size(), (unsigned)2); 69 | EXPECT_EQ(path[0], start); 70 | EXPECT_EQ(path[1], stop); 71 | } 72 | 73 | TEST_F(AstarTest, TwoSteps) { 74 | Vector2f start(-1, -1), stop(1, 1); 75 | PointVector path = OccMap3x3_->astar(start(0), start(1), stop(0), stop(1)); 76 | ASSERT_EQ(path.size(), (unsigned)3); 77 | EXPECT_EQ(path[0], start); 78 | EXPECT_EQ(path[1], Vector2f(0, 0)); 79 | EXPECT_EQ(path[2], stop); 80 | } 81 | 82 | TEST_F(AstarTest, Obstacles) { 83 | Vector2f start(-1, -1), stop(1, -1); 84 | map_get_cell(Map3x3_, 0.0, 0.0, 0)->occ_state = OCCUPIED; 85 | map_get_cell(Map3x3_, 0.0, -1.0, 0)->occ_state = OCCUPIED; 86 | 87 | PointVector path = OccMap3x3_->astar(start(0), start(1), stop(0), stop(1)); 88 | ASSERT_EQ(path.size(), (unsigned)5); 89 | EXPECT_EQ(path[0], start); 90 | EXPECT_EQ(path[1], Vector2f(-1, 0)); 91 | EXPECT_EQ(path[2], Vector2f(0, 1)); 92 | EXPECT_EQ(path[3], Vector2f(1, 0)); 93 | EXPECT_EQ(path[4], Vector2f(1, -1)); 94 | } 95 | 96 | TEST_F(AstarTest, BiggerMap) { 97 | Vector2f start(-50, -50), stop(49, 49); 98 | 99 | PointVector path = OccMap100x100_->astar(start(0), start(1), 100 | stop(0), stop(1)); 101 | ASSERT_EQ(path.size(), (unsigned)100); 102 | } 103 | 104 | class SightTest : public testing::Test { 105 | protected: 106 | void SetUp() { 107 | // Make 3x3 completely free map centered at origin 108 | Map3x3_ = map_alloc(); 109 | Map3x3_->max_occ_dist = 0.0; 110 | int width = 3, height = 3; 111 | Map3x3_->origin_x = 0.0; 112 | Map3x3_->origin_y = 0.0; 113 | Map3x3_->size_x = width; 114 | Map3x3_->size_y = height; 115 | Map3x3_->scale = 1.0; 116 | Map3x3_->cells = (map_cell_t*) calloc(width * height, sizeof(Map3x3_->cells[0])); 117 | for (int x = -1; x < 2; ++x) { 118 | for (int y = -1; y < 2; ++y) { 119 | map_get_cell(Map3x3_, x, y, 0)->occ_state = FREE; 120 | } 121 | } 122 | OccMap3x3_.reset(new OccupancyMap(Map3x3_)); 123 | 124 | // Make 100x100 completely free map centered at origin 125 | Map100x100_ = map_alloc(); 126 | Map100x100_->max_occ_dist = 0.0; 127 | width = 100, height = 100; 128 | Map100x100_->origin_x = 0.0; 129 | Map100x100_->origin_y = 0.0; 130 | Map100x100_->size_x = width; 131 | Map100x100_->size_y = height; 132 | Map100x100_->scale = 1.0; 133 | Map100x100_->cells = (map_cell_t*) calloc(width * height, sizeof(Map100x100_->cells[0])); 134 | for (int x = -50; x < 50; ++x) { 135 | for (int y = -50; y < 50; ++y) { 136 | map_get_cell(Map100x100_, x, y, 0)->occ_state = FREE; 137 | } 138 | } 139 | OccMap100x100_.reset(new OccupancyMap(Map100x100_)); 140 | } 141 | 142 | boost::scoped_ptr OccMap3x3_, OccMap100x100_; 143 | map_t *Map3x3_, *Map100x100_; 144 | }; 145 | 146 | TEST_F(SightTest, AllLOS) { 147 | for (double x1 = -1.4; x1 < 1.5; x1 += 0.25) { 148 | for (double y1 = -1.4; y1 < 1.5; y1 += 0.25) { 149 | for (double x2 = -1.4; x2 < 1.5; x2 += 0.25) { 150 | for (double y2 = -1.4; y2 < 1.5; y2 += 0.25) { 151 | // fprintf(stderr, "%f %f %f %f\n", x1, y1, x2, y2); 152 | ASSERT_TRUE(OccMap100x100_->lineOfSight(x1, y1, x2, y2)); 153 | } 154 | } 155 | } 156 | } 157 | } 158 | 159 | TEST_F(SightTest, MiddleBlocked) { 160 | map_get_cell(Map3x3_, 0, 0, 0)->occ_state = OCCUPIED; 161 | ASSERT_TRUE(OccMap3x3_->lineOfSight(-1, 1, 0, 1)); 162 | ASSERT_TRUE(OccMap3x3_->lineOfSight(-1, 1, 1, 1)); 163 | 164 | ASSERT_FALSE(OccMap3x3_->lineOfSight(-1, 0, 1, 1)); 165 | ASSERT_FALSE(OccMap3x3_->lineOfSight(-1, 0, 1, 0)); 166 | ASSERT_FALSE(OccMap3x3_->lineOfSight(-1, 0, 1, -1)); 167 | 168 | ASSERT_TRUE(OccMap3x3_->lineOfSight(0.6, -0.2, 0.6, 0.6)); 169 | ASSERT_FALSE(OccMap3x3_->lineOfSight(0.6, -0.2, 0, 0.6)); 170 | } 171 | 172 | TEST_F(SightTest, BigMap) { 173 | for (double y = -50; y < 49.5; y += 0.25) { 174 | map_get_cell(Map100x100_, 0, y, 0)->occ_state = OCCUPIED; 175 | } 176 | 177 | for (double x = -50.0; x < -1.0; x += 1.0) { 178 | for (double y = -50; y < 49.5; y += 0.25) { 179 | ASSERT_FALSE(OccMap100x100_->lineOfSight(x, y, 21.25, 3.25)); 180 | } 181 | } 182 | } 183 | 184 | int main(int argc, char **argv){ 185 | testing::InitGoogleTest(&argc, argv); 186 | return RUN_ALL_TESTS(); 187 | } 188 | -------------------------------------------------------------------------------- /rvo_move/maps/stage.world: -------------------------------------------------------------------------------- 1 | define hokuyolaser ranger 2 | ( 3 | sensor( 4 | # laser-specific properties 5 | # factory settings for LMS200 6 | range [ 0.0 5.0 ] 7 | fov 270.0 8 | samples 270 9 | ) 10 | model 11 | ( 12 | # generic model properties 13 | size [ 0.07 0.07 0.05 ] # dimensions from LMS200 data sheet 14 | color "blue" 15 | obstacle_return 0 16 | ) 17 | ) 18 | 19 | 20 | define floorplan model 21 | ( 22 | # sombre, sensible, artistic 23 | color "gray30" 24 | 25 | # most maps will need a bounding box 26 | boundary 1 27 | 28 | gui_nose 0 29 | gui_grid 0 30 | 31 | obstacle_return 1 32 | gui_outline 0 33 | gripper_return 0 34 | fiducial_return 0 35 | laser_return 1 36 | ) 37 | 38 | 39 | define background model 40 | ( 41 | # sombre, sensible, artistic 42 | color "red" 43 | 44 | # most maps will need a bounding box 45 | boundary 1 46 | 47 | gui_nose 0 48 | gui_grid 0 49 | 50 | obstacle_return 0 51 | gui_outline 0 52 | gripper_return 0 53 | fiducial_return 0 54 | laser_return 1 55 | ) 56 | 57 | 58 | # set the resolution of the underlying raytrace model in meters 59 | resolution 0.02 60 | 61 | interval_sim 100 # simulation timestep in milliseconds 62 | 63 | 64 | window 65 | ( 66 | size [ 635.000 666.000 ] # in pixels 67 | scale 36.995 # pixels per meter 68 | center [ 0 0 ] 69 | rotate [ 0 0 ] 70 | 71 | show_data 1 # 1=on 0=off 72 | ) 73 | 74 | # load an environment bitmap 75 | 76 | floorplan 77 | ( 78 | name "cave" 79 | bitmap "map.png" 80 | size [16.0 16.0 0] 81 | pose [ 0 0 0 0 ] 82 | ) 83 | 84 | define pioneer_base position 85 | ( 86 | color "red" # Default color. 87 | drive "diff" # Differential steering model. 88 | gui_nose 0 # Draw a nose on the robot so we can see which way it points 89 | obstacle_return 0 # Can hit things. 90 | ranger_return 0.5 # reflects sonar beams 91 | blob_return 1 # Seen by blobfinders 92 | fiducial_return 1 # Seen as "1" fiducial finders 93 | 94 | localization "gps" 95 | localization_origin [0 0 0 0] # Start odometry at (0, 0, 0). 96 | 97 | # alternative odometric localization with simple error model 98 | # localization "odom" # Change to "gps" to have impossibly perfect, global odometry 99 | # odom_error [ 0.05 0.05 0.1 ] # Odometry error or slip in X, Y and Theta 100 | # (Uniform random distribution) 101 | 102 | # four DOF kinematics limits 103 | # [ xmin xmax ymin ymax zmin zmax amin amax ] 104 | velocity_bounds [-1 1 0 0 0 0 -45.0 45.0 ] 105 | acceleration_bounds [-1.0 1.0 0 0 0 0 -45 45.0 ] 106 | ) 107 | 108 | define pioneer2dx_base_no_sonar pioneer_base 109 | ( 110 | # actual size 111 | size [0.44 0.38 0.22] # sizes from MobileRobots' web site 112 | 113 | # the pioneer's center of rotation is offset from its center of area 114 | origin [-0.04 0 0 0] 115 | 116 | # draw a nose on the robot so we can see which way it points 117 | gui_nose 0 118 | 119 | # estimated mass in KG 120 | mass 23.0 121 | ) 122 | 123 | # as above, but with front sonar only 124 | define pioneer2dx_no_sonar pioneer2dx_base_no_sonar 125 | ( 126 | # simplified Body shape: 127 | block( 128 | points 8 129 | point[0] [-0.2 0.12] 130 | point[1] [-0.2 -0.12] 131 | point[2] [-0.12 -0.2555] 132 | point[3] [0.12 -0.2555] 133 | point[4] [0.2 -0.12] 134 | point[5] [0.2 0.12] 135 | point[6] [0.12 0.2555] 136 | point[7] [-0.12 0.2555] 137 | z [0 0.22] 138 | ) 139 | ) 140 | 141 | 142 | pioneer2dx_no_sonar 143 | ( 144 | # can refer to the robot by this name 145 | name "robot_0" 146 | color "red" 147 | pose [-0.25 -0.35 0 0] 148 | hokuyolaser(pose [ 0.225 0.000 -0.15 0.000 ]) 149 | ) 150 | 151 | pioneer2dx_no_sonar 152 | ( 153 | # can refer to the robot by this name 154 | name "robot_1" 155 | color "red" 156 | pose [-0.31 -0.14 0 0] 157 | hokuyolaser(pose [ 0.225 0.000 -0.15 0.000 ]) 158 | ) 159 | 160 | pioneer2dx_no_sonar 161 | ( 162 | # can refer to the robot by this name 163 | name "robot_2" 164 | color "red" 165 | pose [-0.35 0.19 0 0] 166 | hokuyolaser(pose [ 0.225 0.000 -0.15 0.000 ]) 167 | ) 168 | 169 | pioneer2dx_no_sonar 170 | ( 171 | # can refer to the robot by this name 172 | name "robot_3" 173 | color "red" 174 | pose [-0.28 0.5 0 0] 175 | hokuyolaser(pose [ 0.225 0.000 -0.15 0.000 ]) 176 | ) 177 | 178 | pioneer2dx_no_sonar 179 | ( 180 | # can refer to the robot by this name 181 | name "robot_4" 182 | color "yellow" 183 | pose [0.07 1.07 0 0] 184 | hokuyolaser(pose [ 0.225 0.000 -0.15 0.000 ]) 185 | ) 186 | 187 | 188 | pioneer2dx_no_sonar 189 | ( 190 | # can refer to the robot by this name 191 | name "robot_5" 192 | color "yellow" 193 | pose [0.3 1.04 0 0] 194 | hokuyolaser(pose [ 0.225 0.000 -0.15 0.000 ]) 195 | ) 196 | 197 | 198 | pioneer2dx_no_sonar 199 | ( 200 | # can refer to the robot by this name 201 | name "robot_6" 202 | color "yellow" 203 | pose [0.6 1.03 0 0] 204 | hokuyolaser(pose [ 0.225 0.000 -0.15 0.000 ]) 205 | ) 206 | 207 | 208 | pioneer2dx_no_sonar 209 | ( 210 | # can refer to the robot by this name 211 | name "robot_7" 212 | color "yellow" 213 | pose [0.37 1.32 0 0] 214 | hokuyolaser(pose [ 0.225 0.000 -0.15 0.000 ]) 215 | ) 216 | 217 | pioneer2dx_no_sonar 218 | ( 219 | # can refer to the robot by this name 220 | name "robot_8" 221 | color "blue" 222 | pose [1.34 -0.2 0 0] 223 | hokuyolaser(pose [ 0.225 0.000 -0.15 0.000 ]) 224 | ) 225 | 226 | 227 | pioneer2dx_no_sonar 228 | ( 229 | # can refer to the robot by this name 230 | name "robot_9" 231 | color "blue" 232 | pose [1.3 0.035.32 0 0] 233 | hokuyolaser(pose [ 0.225 0.000 -0.15 0.000 ]) 234 | ) 235 | 236 | 237 | pioneer2dx_no_sonar 238 | ( 239 | # can refer to the robot by this name 240 | name "robot_10" 241 | color "blue" 242 | pose [1.21 0.38 0 0] 243 | hokuyolaser(pose [ 0.225 0.000 -0.15 0.000 ]) 244 | ) 245 | 246 | 247 | pioneer2dx_no_sonar 248 | ( 249 | # can refer to the robot by this name 250 | name "robot_11" 251 | color "blue" 252 | pose [1.25 0.66 0 0] 253 | hokuyolaser(pose [ 0.225 0.000 -0.15 0.000 ]) 254 | ) 255 | 256 | pioneer2dx_no_sonar 257 | ( 258 | # can refer to the robot by this name 259 | name "robot_12" 260 | color "white" 261 | pose [0.11 -0.83 0 0] 262 | hokuyolaser(pose [ 0.225 0.000 -0.15 0.000 ]) 263 | ) 264 | 265 | pioneer2dx_no_sonar 266 | ( 267 | # can refer to the robot by this name 268 | name "robot_13" 269 | color "white" 270 | pose [0.38 -0.89 0 0] 271 | hokuyolaser(pose [ 0.225 0.000 -0.15 0.000 ]) 272 | ) 273 | 274 | pioneer2dx_no_sonar 275 | ( 276 | # can refer to the robot by this name 277 | name "robot_14" 278 | color "white" 279 | pose [0.70 -0.84 0 0] 280 | hokuyolaser(pose [ 0.225 0.000 -0.15 0.000 ]) 281 | ) 282 | 283 | pioneer2dx_no_sonar 284 | ( 285 | # can refer to the robot by this name 286 | name "robot_15" 287 | color "white" 288 | pose [0.98 -0.62 0 0] 289 | hokuyolaser(pose [ 0.225 0.000 -0.15 0.000 ]) 290 | ) -------------------------------------------------------------------------------- /rvo_move/matlab/Map.m: -------------------------------------------------------------------------------- 1 | classdef Map < handle 2 | properties 3 | % 2D occupancy grid; 0 if free, 1 if occupied 4 | occgrid = []; 5 | res_xy = 1.0; 6 | % 4 element vector of environment lims: [xy]_min, [xz]_max 7 | bound_xy = []; 8 | 9 | wall_ij; 10 | end 11 | 12 | methods(Static) 13 | function [new_map] = FromYAML(path, pad_xy) 14 | if nargin < 2 15 | pad_xy = [0 0]; 16 | end 17 | dict = read_yaml(path); 18 | base = fileparts(path); 19 | if base 20 | map_path = [base filesep dict('image')]; 21 | else 22 | map_path = dict('image'); 23 | end 24 | img = imread(map_path); 25 | origin = dict('origin'); 26 | new_map = Map(img, dict('resolution'), origin(1:2), pad_xy); 27 | end 28 | end 29 | 30 | methods 31 | function [obj] = Map(grid, res_xy, offset, pad_xy) 32 | if nargin < 4 33 | pad_xy = [0 0]; 34 | end 35 | obj.res_xy = res_xy; 36 | obj.bound_xy = [offset, offset + res_xy * [size(grid, 2), size(grid, 1)]]; 37 | 38 | grid_sz = size(grid); 39 | pad_ij = fliplr(round(pad_xy / obj.res_xy)); 40 | obj.occgrid = 255 * ones(size(grid) + pad_ij * 2, 'uint8'); 41 | obj.occgrid(pad_ij(1) + (1:grid_sz(1)), pad_ij(2) + (1:grid_sz(2))) = flipud(grid); 42 | 43 | obj.bound_xy(1:2) = obj.bound_xy(1:2) - pad_xy; 44 | obj.bound_xy(3:4) = obj.bound_xy(3:4) + pad_xy; 45 | 46 | [wall_i, wall_j] = find(obj.occgrid < 76); 47 | obj.wall_ij = [wall_i, wall_j]; 48 | end 49 | 50 | function [new_map] = InflateObstacles(map, amount) 51 | grid = double(map.occgrid < 120); 52 | grid = conv2(grid, ones(round(amount / map.res_xy)), 'same'); 53 | grid = 255 * (grid < 1); 54 | new_map = Map(flipud(grid), map.res_xy, map.bound_xy(1:2), [0 0]); 55 | end 56 | 57 | 58 | function [] = wipe(obj, x_lim, y_lim) 59 | xys = obj.indToXY(find(double(obj.occgrid) < 78)); 60 | valid = xys(:, 1) >= x_lim(1) & xys(:, 1) <= x_lim(2) ... 61 | & xys(:, 2) >= y_lim(1) & xys(:, 2) <= y_lim(2); 62 | obj.occgrid(obj.xyToInd(xys(valid, :))) = 255; 63 | end 64 | 65 | function [] = fill(obj, x_lim, y_lim) 66 | xys = obj.indToXY(find(double(obj.occgrid) > 78)); 67 | valid = xys(:, 1) >= x_lim(1) & xys(:, 1) <= x_lim(2) ... 68 | & xys(:, 2) >= y_lim(1) & xys(:, 2) <= y_lim(2); 69 | obj.occgrid(obj.xyToInd(xys(valid, :))) = 77; 70 | end 71 | 72 | % function [xy] = wallCoords(obj, x_lim, y_lim) 73 | % % Get all xy coordinates of walls within givin box 74 | % upleft_ij = obj.xyToSub([x_lim(1), y_lim(2)]); 75 | % lowright_ij = obj.xyToSub([x_lim(2), y_lim(1)]); 76 | % 77 | % max_i = upleft_ij(1); 78 | % min_j = upleft_ij(2); 79 | % min_i = lowright_ij(1); 80 | % max_j = lowright_ij(2); 81 | % 82 | % % valid = (min_i <= obj.wall_ij(:, 1) & obj.wall_ij(:, 1) <= max_i & ... 83 | % % min_j <= obj.wall_ij(:, 2) & obj.wall_ij(:, 2) <= max_j); 84 | % xy = obj.subToXY(obj.wall_ij(:, :)); 85 | % end 86 | 87 | function [xy] = subToXY(map, ij) 88 | ji = [ij(:, 2), ij(:, 1)]; 89 | scaled = bsxfun(@(a, b) a .* b, ji - 1, map.res_xy); 90 | xy = bsxfun(@plus, scaled, map.bound_xy(1:2) + map.res_xy / 2); 91 | end 92 | 93 | function [ij] = xyToSub(map, xy) 94 | diff = bsxfun(@minus, xy, map.bound_xy(1:2)); 95 | 96 | % Make origin of map (i.e. bound_xy(1:2)) have valid subscript 97 | diff(bsxfun(@lt, diff, map.res_xy)) = eps; 98 | 99 | jik = bsxfun(@(a, b) a ./ b, diff, map.res_xy); 100 | inds = jik ~= floor(jik); 101 | jik(inds) = floor(jik(inds)) + 1; 102 | 103 | % Points outside boundary get negative indicies 104 | valid = all(bsxfun(@le, map.bound_xy(1:2), xy) & ... 105 | bsxfun(@ge, map.bound_xy(3:4), xy), 2); 106 | jik(~valid, :) = -1; 107 | ij = [jik(:, 2), jik(:, 1)]; 108 | end 109 | 110 | function [inds] = xyToInd(map, xy) 111 | ij = map.xyToSub(xy); 112 | inds = round(sub2ind(size(map.occgrid), ij(:, 1), ij(:, 2))); 113 | end 114 | 115 | function [xy] = indToXY(map, ind) 116 | [i, j] = ind2sub(size(map.occgrid), ind); 117 | xy = map.subToXY([i, j]); 118 | end 119 | 120 | function [collide] = collide(map, xys) 121 | inds = map.xyToInd(xys); 122 | collide = map.occgrid(inds) < 100; 123 | end 124 | 125 | 126 | function plot(obj, fh, min_xy, max_xy) 127 | if nargin < 2 128 | fh = gcf; 129 | clf(fh); 130 | end 131 | if nargin < 4 132 | min_xy = obj.bound_xy(1:2); 133 | max_xy = obj.bound_xy(3:4); 134 | end 135 | if isempty(min_xy) 136 | min_xy = obj.bound_xy(1:2); 137 | end 138 | if isempty(max_xy) 139 | max_xy = obj.bound_xy(3:4); 140 | end 141 | 142 | min_ij = obj.xyToSub(min_xy); 143 | max_ij = obj.xyToSub(max_xy); 144 | if any(max_ij == -1 | min_ij == -1) 145 | warning('map:limits', 'Requested x_lim or y_lim is bigger than map'); 146 | end 147 | map = obj.occgrid(min_ij(1):max_ij(1), min_ij(2):max_ij(2)); 148 | 149 | % Make figure pretty + informative 150 | imshow(map, 'XData', [min_xy(1), max_xy(1)], ... 151 | 'YData', [min_xy(2), max_xy(2)], ... 152 | 'Parent', gca); 153 | ax = get(fh, 'CurrentAxes'); 154 | hold(ax, 'on'); 155 | axis(ax, 'on'); 156 | axis(ax, 'xy'); 157 | xlabel(ax, 'X (m)'); 158 | ylabel(ax, 'Y (m)'); 159 | axis(ax, 'equal'); 160 | xlim(ax, [min_xy(1), max_xy(1)]); 161 | ylim(ax, [min_xy(2), max_xy(2)]); 162 | hold(ax, 'off'); 163 | end 164 | 165 | function display(map) 166 | s = char('', sprintf('Map:')); 167 | [i, j] = size(map.occgrid); 168 | s = char(s, sprintf(' %i-by-%i', i, j)); 169 | s = char(s, ... 170 | sprintf(' X:(% 5.1f,% 5.1f) Y:(% 5.1f,% 5.1f)', ... 171 | map.bound_xy([1, 3, 2, 4]))); 172 | s = char(s, sprintf(' XY-Res: %6.2f', map.res_xy)); 173 | disp(s); 174 | end 175 | end 176 | end 177 | -------------------------------------------------------------------------------- /rvo2/src/Agent.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Agent.h 3 | * RVO2 Library 4 | * 5 | * Copyright (c) 2008-2010 University of North Carolina at Chapel Hill. 6 | * All rights reserved. 7 | * 8 | * Permission to use, copy, modify, and distribute this software and its 9 | * documentation for educational, research, and non-profit purposes, without 10 | * fee, and without a written agreement is hereby granted, provided that the 11 | * above copyright notice, this paragraph, and the following four paragraphs 12 | * appear in all copies. 13 | * 14 | * Permission to incorporate this software into commercial products may be 15 | * obtained by contacting the authors or the Office of 16 | * Technology Development at the University of North Carolina at Chapel Hill 17 | * . 18 | * 19 | * This software program and documentation are copyrighted by the University of 20 | * North Carolina at Chapel Hill. The software program and documentation are 21 | * supplied "as is," without any accompanying services from the University of 22 | * North Carolina at Chapel Hill or the authors. The University of North 23 | * Carolina at Chapel Hill and the authors do not warrant that the operation of 24 | * the program will be uninterrupted or error-free. The end-user understands 25 | * that the program was developed for research purposes and is advised not to 26 | * rely exclusively on the program for any reason. 27 | * 28 | * IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE 29 | * AUTHORS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR 30 | * CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS 31 | * SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF NORTH CAROLINA AT 32 | * CHAPEL HILL OR THE AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH 33 | * DAMAGE. 34 | * 35 | * THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS SPECIFICALLY 36 | * DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 37 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AND ANY 38 | * STATUTORY WARRANTY OF NON-INFRINGEMENT. THE SOFTWARE PROVIDED HEREUNDER IS ON 39 | * AN "AS IS" BASIS, AND THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE 40 | * AUTHORS HAVE NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, 41 | * ENHANCEMENTS, OR MODIFICATIONS. 42 | * 43 | * Please send all bug reports to . 44 | * 45 | * The authors may be contacted via: 46 | * 47 | * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha 48 | * Dept. of Computer Science 49 | * 201 S. Columbia St. 50 | * Frederick P. Brooks, Jr. Computer Science Bldg. 51 | * Chapel Hill, N.C. 27599-3175 52 | * United States of America 53 | * 54 | * 55 | */ 56 | 57 | #ifndef RVO_AGENT_H_ 58 | #define RVO_AGENT_H_ 59 | 60 | /** 61 | * \file Agent.h 62 | * \brief Contains the Agent class. 63 | */ 64 | 65 | #include "Definitions.h" 66 | #include "RVOSimulator.h" 67 | 68 | namespace RVO { 69 | /** 70 | * \brief Defines an agent in the simulation. 71 | */ 72 | class Agent { 73 | private: 74 | /** 75 | * \brief Constructs an agent instance. 76 | * \param sim The simulator instance. 77 | */ 78 | explicit Agent(RVOSimulator *sim); 79 | 80 | /** 81 | * \brief Computes the neighbors of this agent. 82 | */ 83 | void computeNeighbors(); 84 | 85 | /** 86 | * \brief Computes the new velocity of this agent. 87 | */ 88 | void computeNewVelocity(); 89 | 90 | /** 91 | * \brief Inserts an agent neighbor into the set of neighbors of 92 | * this agent. 93 | * \param agent A pointer to the agent to be inserted. 94 | * \param rangeSq The squared range around this agent. 95 | */ 96 | void insertAgentNeighbor(const Agent *agent, float &rangeSq); 97 | 98 | /** 99 | * \brief Inserts a static obstacle neighbor into the set of neighbors 100 | * of this agent. 101 | * \param obstacle The number of the static obstacle to be 102 | * inserted. 103 | * \param rangeSq The squared range around this agent. 104 | */ 105 | void insertObstacleNeighbor(const Obstacle *obstacle, float rangeSq); 106 | 107 | /** 108 | * \brief Updates the two-dimensional position and two-dimensional 109 | * velocity of this agent. 110 | */ 111 | void update(); 112 | 113 | std::vector > agentNeighbors_; 114 | size_t maxNeighbors_; 115 | float maxSpeed_; 116 | float neighborDist_; 117 | Vector2 newVelocity_; 118 | std::vector > obstacleNeighbors_; 119 | std::vector orcaLines_; 120 | Vector2 position_; 121 | Vector2 prefVelocity_; 122 | float radius_; 123 | RVOSimulator *sim_; 124 | float timeHorizon_; 125 | float timeHorizonObst_; 126 | Vector2 velocity_; 127 | 128 | size_t id_; 129 | 130 | friend class KdTree; 131 | friend class RVOSimulator; 132 | }; 133 | 134 | /** 135 | * \relates Agent 136 | * \brief Solves a one-dimensional linear program on a specified line 137 | * subject to linear constraints defined by lines and a circular 138 | * constraint. 139 | * \param lines Lines defining the linear constraints. 140 | * \param lineNo The specified line constraint. 141 | * \param radius The radius of the circular constraint. 142 | * \param optVelocity The optimization velocity. 143 | * \param directionOpt True if the direction should be optimized. 144 | * \param result A reference to the result of the linear program. 145 | * \return True if successful. 146 | */ 147 | bool linearProgram1(const std::vector &lines, size_t lineNo, 148 | float radius, const Vector2 &optVelocity, 149 | bool directionOpt, Vector2 &result); 150 | 151 | /** 152 | * \relates Agent 153 | * \brief Solves a two-dimensional linear program subject to linear 154 | * constraints defined by lines and a circular constraint. 155 | * \param lines Lines defining the linear constraints. 156 | * \param radius The radius of the circular constraint. 157 | * \param optVelocity The optimization velocity. 158 | * \param directionOpt True if the direction should be optimized. 159 | * \param result A reference to the result of the linear program. 160 | * \return The number of the line it fails on, and the number of lines if successful. 161 | */ 162 | size_t linearProgram2(const std::vector &lines, float radius, 163 | const Vector2 &optVelocity, bool directionOpt, 164 | Vector2 &result); 165 | 166 | /** 167 | * \relates Agent 168 | * \brief Solves a two-dimensional linear program subject to linear 169 | * constraints defined by lines and a circular constraint. 170 | * \param lines Lines defining the linear constraints. 171 | * \param numObstLines Count of obstacle lines. 172 | * \param beginLine The line on which the 2-d linear program failed. 173 | * \param radius The radius of the circular constraint. 174 | * \param result A reference to the result of the linear program. 175 | */ 176 | void linearProgram3(const std::vector &lines, size_t numObstLines, size_t beginLine, 177 | float radius, Vector2 &result); 178 | } 179 | 180 | #endif /* RVO_AGENT_H_ */ 181 | -------------------------------------------------------------------------------- /rvo2/src/KdTree.h: -------------------------------------------------------------------------------- 1 | /* 2 | * KdTree.h 3 | * RVO2 Library 4 | * 5 | * Copyright (c) 2008-2010 University of North Carolina at Chapel Hill. 6 | * All rights reserved. 7 | * 8 | * Permission to use, copy, modify, and distribute this software and its 9 | * documentation for educational, research, and non-profit purposes, without 10 | * fee, and without a written agreement is hereby granted, provided that the 11 | * above copyright notice, this paragraph, and the following four paragraphs 12 | * appear in all copies. 13 | * 14 | * Permission to incorporate this software into commercial products may be 15 | * obtained by contacting the authors or the Office of 16 | * Technology Development at the University of North Carolina at Chapel Hill 17 | * . 18 | * 19 | * This software program and documentation are copyrighted by the University of 20 | * North Carolina at Chapel Hill. The software program and documentation are 21 | * supplied "as is," without any accompanying services from the University of 22 | * North Carolina at Chapel Hill or the authors. The University of North 23 | * Carolina at Chapel Hill and the authors do not warrant that the operation of 24 | * the program will be uninterrupted or error-free. The end-user understands 25 | * that the program was developed for research purposes and is advised not to 26 | * rely exclusively on the program for any reason. 27 | * 28 | * IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE 29 | * AUTHORS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR 30 | * CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS 31 | * SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF NORTH CAROLINA AT 32 | * CHAPEL HILL OR THE AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH 33 | * DAMAGE. 34 | * 35 | * THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS SPECIFICALLY 36 | * DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 37 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AND ANY 38 | * STATUTORY WARRANTY OF NON-INFRINGEMENT. THE SOFTWARE PROVIDED HEREUNDER IS ON 39 | * AN "AS IS" BASIS, AND THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE 40 | * AUTHORS HAVE NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, 41 | * ENHANCEMENTS, OR MODIFICATIONS. 42 | * 43 | * Please send all bug reports to . 44 | * 45 | * The authors may be contacted via: 46 | * 47 | * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha 48 | * Dept. of Computer Science 49 | * 201 S. Columbia St. 50 | * Frederick P. Brooks, Jr. Computer Science Bldg. 51 | * Chapel Hill, N.C. 27599-3175 52 | * United States of America 53 | * 54 | * 55 | */ 56 | 57 | #ifndef RVO_KD_TREE_H_ 58 | #define RVO_KD_TREE_H_ 59 | 60 | /** 61 | * \file KdTree.h 62 | * \brief Contains the KdTree class. 63 | */ 64 | 65 | #include "Definitions.h" 66 | 67 | namespace RVO { 68 | /** 69 | * \brief Defines kd-trees for agents and static obstacles in the 70 | * simulation. 71 | */ 72 | class KdTree { 73 | private: 74 | /** 75 | * \brief Defines an agent kd-tree node. 76 | */ 77 | class AgentTreeNode { 78 | public: 79 | /** 80 | * \brief The beginning node number. 81 | */ 82 | size_t begin; 83 | 84 | /** 85 | * \brief The ending node number. 86 | */ 87 | size_t end; 88 | 89 | /** 90 | * \brief The left node number. 91 | */ 92 | size_t left; 93 | 94 | /** 95 | * \brief The maximum x-coordinate. 96 | */ 97 | float maxX; 98 | 99 | /** 100 | * \brief The maximum y-coordinate. 101 | */ 102 | float maxY; 103 | 104 | /** 105 | * \brief The minimum x-coordinate. 106 | */ 107 | float minX; 108 | 109 | /** 110 | * \brief The minimum y-coordinate. 111 | */ 112 | float minY; 113 | 114 | /** 115 | * \brief The right node number. 116 | */ 117 | size_t right; 118 | }; 119 | 120 | /** 121 | * \brief Defines an obstacle kd-tree node. 122 | */ 123 | class ObstacleTreeNode { 124 | public: 125 | /** 126 | * \brief The left obstacle tree node. 127 | */ 128 | ObstacleTreeNode *left; 129 | 130 | /** 131 | * \brief The obstacle number. 132 | */ 133 | const Obstacle *obstacle; 134 | 135 | /** 136 | * \brief The right obstacle tree node. 137 | */ 138 | ObstacleTreeNode *right; 139 | }; 140 | 141 | /** 142 | * \brief Constructs a kd-tree instance. 143 | * \param sim The simulator instance. 144 | */ 145 | explicit KdTree(RVOSimulator *sim); 146 | 147 | /** 148 | * \brief Destroys this kd-tree instance. 149 | */ 150 | ~KdTree(); 151 | 152 | /** 153 | * \brief Builds an agent kd-tree. 154 | */ 155 | void buildAgentTree(); 156 | 157 | void buildAgentTreeRecursive(size_t begin, size_t end, size_t node); 158 | 159 | /** 160 | * \brief Builds an obstacle kd-tree. 161 | */ 162 | void buildObstacleTree(); 163 | 164 | ObstacleTreeNode *buildObstacleTreeRecursive(const std::vector & 165 | obstacles); 166 | 167 | /** 168 | * \brief Computes the agent neighbors of the specified agent. 169 | * \param agent A pointer to the agent for which agent 170 | * neighbors are to be computed. 171 | * \param rangeSq The squared range around the agent. 172 | */ 173 | void computeAgentNeighbors(Agent *agent, float &rangeSq) const; 174 | 175 | /** 176 | * \brief Computes the obstacle neighbors of the specified agent. 177 | * \param agent A pointer to the agent for which obstacle 178 | * neighbors are to be computed. 179 | * \param rangeSq The squared range around the agent. 180 | */ 181 | void computeObstacleNeighbors(Agent *agent, float rangeSq) const; 182 | 183 | /** 184 | * \brief Deletes the specified obstacle tree node. 185 | * \param node A pointer to the obstacle tree node to be 186 | * deleted. 187 | */ 188 | void deleteObstacleTree(ObstacleTreeNode *node); 189 | 190 | void queryAgentTreeRecursive(Agent *agent, float &rangeSq, 191 | size_t node) const; 192 | 193 | void queryObstacleTreeRecursive(Agent *agent, float rangeSq, 194 | const ObstacleTreeNode *node) const; 195 | 196 | /** 197 | * \brief Queries the visibility between two points within a 198 | * specified radius. 199 | * \param q1 The first point between which visibility is 200 | * to be tested. 201 | * \param q2 The second point between which visibility is 202 | * to be tested. 203 | * \param radius The radius within which visibility is to be 204 | * tested. 205 | * \return True if q1 and q2 are mutually visible within the radius; 206 | * false otherwise. 207 | */ 208 | bool queryVisibility(const Vector2 &q1, const Vector2 &q2, 209 | float radius) const; 210 | 211 | bool queryVisibilityRecursive(const Vector2 &q1, const Vector2 &q2, 212 | float radius, 213 | const ObstacleTreeNode *node) const; 214 | 215 | std::vector agents_; 216 | std::vector agentTree_; 217 | ObstacleTreeNode *obstacleTree_; 218 | RVOSimulator *sim_; 219 | 220 | static const size_t MAX_LEAF_SIZE = 10; 221 | 222 | friend class Agent; 223 | friend class RVOSimulator; 224 | }; 225 | } 226 | 227 | #endif /* RVO_KD_TREE_H_ */ 228 | -------------------------------------------------------------------------------- /rvo2/src/RVOSimulator.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * RVOSimulator.cpp 3 | * RVO2 Library 4 | * 5 | * Copyright (c) 2008-2010 University of North Carolina at Chapel Hill. 6 | * All rights reserved. 7 | * 8 | * Permission to use, copy, modify, and distribute this software and its 9 | * documentation for educational, research, and non-profit purposes, without 10 | * fee, and without a written agreement is hereby granted, provided that the 11 | * above copyright notice, this paragraph, and the following four paragraphs 12 | * appear in all copies. 13 | * 14 | * Permission to incorporate this software into commercial products may be 15 | * obtained by contacting the authors or the Office of 16 | * Technology Development at the University of North Carolina at Chapel Hill 17 | * . 18 | * 19 | * This software program and documentation are copyrighted by the University of 20 | * North Carolina at Chapel Hill. The software program and documentation are 21 | * supplied "as is," without any accompanying services from the University of 22 | * North Carolina at Chapel Hill or the authors. The University of North 23 | * Carolina at Chapel Hill and the authors do not warrant that the operation of 24 | * the program will be uninterrupted or error-free. The end-user understands 25 | * that the program was developed for research purposes and is advised not to 26 | * rely exclusively on the program for any reason. 27 | * 28 | * IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE 29 | * AUTHORS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR 30 | * CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS 31 | * SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF NORTH CAROLINA AT 32 | * CHAPEL HILL OR THE AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH 33 | * DAMAGE. 34 | * 35 | * THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS SPECIFICALLY 36 | * DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 37 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AND ANY 38 | * STATUTORY WARRANTY OF NON-INFRINGEMENT. THE SOFTWARE PROVIDED HEREUNDER IS ON 39 | * AN "AS IS" BASIS, AND THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE 40 | * AUTHORS HAVE NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, 41 | * ENHANCEMENTS, OR MODIFICATIONS. 42 | * 43 | * Please send all bug reports to . 44 | * 45 | * The authors may be contacted via: 46 | * 47 | * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha 48 | * Dept. of Computer Science 49 | * 201 S. Columbia St. 50 | * Frederick P. Brooks, Jr. Computer Science Bldg. 51 | * Chapel Hill, N.C. 27599-3175 52 | * United States of America 53 | * 54 | * 55 | */ 56 | 57 | #include "RVOSimulator.h" 58 | 59 | #include "Agent.h" 60 | #include "KdTree.h" 61 | #include "Obstacle.h" 62 | 63 | #ifdef _OPENMP 64 | #include 65 | #endif 66 | 67 | namespace RVO { 68 | RVOSimulator::RVOSimulator() : defaultAgent_(NULL), globalTime_(0.0f), kdTree_(NULL), timeStep_(0.0f) 69 | { 70 | kdTree_ = new KdTree(this); 71 | } 72 | 73 | RVOSimulator::RVOSimulator(float timeStep, float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, const Vector2 &velocity) : defaultAgent_(NULL), globalTime_(0.0f), kdTree_(NULL), timeStep_(timeStep) 74 | { 75 | kdTree_ = new KdTree(this); 76 | defaultAgent_ = new Agent(this); 77 | 78 | defaultAgent_->maxNeighbors_ = maxNeighbors; 79 | defaultAgent_->maxSpeed_ = maxSpeed; 80 | defaultAgent_->neighborDist_ = neighborDist; 81 | defaultAgent_->radius_ = radius; 82 | defaultAgent_->timeHorizon_ = timeHorizon; 83 | defaultAgent_->timeHorizonObst_ = timeHorizonObst; 84 | defaultAgent_->velocity_ = velocity; 85 | } 86 | 87 | RVOSimulator::~RVOSimulator() 88 | { 89 | if (defaultAgent_ != NULL) { 90 | delete defaultAgent_; 91 | } 92 | 93 | for (size_t i = 0; i < agents_.size(); ++i) { 94 | delete agents_[i]; 95 | } 96 | 97 | for (size_t i = 0; i < obstacles_.size(); ++i) { 98 | delete obstacles_[i]; 99 | } 100 | 101 | delete kdTree_; 102 | } 103 | 104 | size_t RVOSimulator::addAgent(const Vector2 &position) 105 | { 106 | if (defaultAgent_ == NULL) { 107 | return RVO_ERROR; 108 | } 109 | 110 | Agent *agent = new Agent(this); 111 | 112 | agent->position_ = position; 113 | agent->maxNeighbors_ = defaultAgent_->maxNeighbors_; 114 | agent->maxSpeed_ = defaultAgent_->maxSpeed_; 115 | agent->neighborDist_ = defaultAgent_->neighborDist_; 116 | agent->radius_ = defaultAgent_->radius_; 117 | agent->timeHorizon_ = defaultAgent_->timeHorizon_; 118 | agent->timeHorizonObst_ = defaultAgent_->timeHorizonObst_; 119 | agent->velocity_ = defaultAgent_->velocity_; 120 | 121 | agent->id_ = agents_.size(); 122 | 123 | agents_.push_back(agent); 124 | 125 | return agents_.size() - 1; 126 | } 127 | 128 | size_t RVOSimulator::addAgent(const Vector2 &position, float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, const Vector2 &velocity) 129 | { 130 | Agent *agent = new Agent(this); 131 | 132 | agent->position_ = position; 133 | agent->maxNeighbors_ = maxNeighbors; 134 | agent->maxSpeed_ = maxSpeed; 135 | agent->neighborDist_ = neighborDist; 136 | agent->radius_ = radius; 137 | agent->timeHorizon_ = timeHorizon; 138 | agent->timeHorizonObst_ = timeHorizonObst; 139 | agent->velocity_ = velocity; 140 | 141 | agent->id_ = agents_.size(); 142 | 143 | agents_.push_back(agent); 144 | 145 | return agents_.size() - 1; 146 | } 147 | 148 | size_t RVOSimulator::addObstacle(const std::vector &vertices) 149 | { 150 | if (vertices.size() < 2) { 151 | return RVO_ERROR; 152 | } 153 | 154 | const size_t obstacleNo = obstacles_.size(); 155 | 156 | for (size_t i = 0; i < vertices.size(); ++i) { 157 | Obstacle *obstacle = new Obstacle(); 158 | obstacle->point_ = vertices[i]; 159 | 160 | if (i != 0) { 161 | obstacle->prevObstacle_ = obstacles_.back(); 162 | obstacle->prevObstacle_->nextObstacle_ = obstacle; 163 | } 164 | 165 | if (i == vertices.size() - 1) { 166 | obstacle->nextObstacle_ = obstacles_[obstacleNo]; 167 | obstacle->nextObstacle_->prevObstacle_ = obstacle; 168 | } 169 | 170 | obstacle->unitDir_ = normalize(vertices[(i == vertices.size() - 1 ? 0 : i + 1)] - vertices[i]); 171 | 172 | if (vertices.size() == 2) { 173 | obstacle->isConvex_ = true; 174 | } 175 | else { 176 | obstacle->isConvex_ = (leftOf(vertices[(i == 0 ? vertices.size() - 1 : i - 1)], vertices[i], vertices[(i == vertices.size() - 1 ? 0 : i + 1)]) >= 0.0f); 177 | } 178 | 179 | obstacle->id_ = obstacles_.size(); 180 | 181 | obstacles_.push_back(obstacle); 182 | } 183 | 184 | return obstacleNo; 185 | } 186 | 187 | void RVOSimulator::doStep() 188 | { 189 | kdTree_->buildAgentTree(); 190 | 191 | #ifdef _OPENMP 192 | #pragma omp parallel for 193 | #endif 194 | for (int i = 0; i < static_cast(agents_.size()); ++i) { 195 | agents_[i]->computeNeighbors(); 196 | agents_[i]->computeNewVelocity(); 197 | } 198 | 199 | #ifdef _OPENMP 200 | #pragma omp parallel for 201 | #endif 202 | for (int i = 0; i < static_cast(agents_.size()); ++i) { 203 | agents_[i]->update(); 204 | } 205 | 206 | globalTime_ += timeStep_; 207 | } 208 | 209 | size_t RVOSimulator::getAgentAgentNeighbor(size_t agentNo, size_t neighborNo) const 210 | { 211 | return agents_[agentNo]->agentNeighbors_[neighborNo].second->id_; 212 | } 213 | 214 | size_t RVOSimulator::getAgentMaxNeighbors(size_t agentNo) const 215 | { 216 | return agents_[agentNo]->maxNeighbors_; 217 | } 218 | 219 | float RVOSimulator::getAgentMaxSpeed(size_t agentNo) const 220 | { 221 | return agents_[agentNo]->maxSpeed_; 222 | } 223 | 224 | float RVOSimulator::getAgentNeighborDist(size_t agentNo) const 225 | { 226 | return agents_[agentNo]->neighborDist_; 227 | } 228 | 229 | size_t RVOSimulator::getAgentNumAgentNeighbors(size_t agentNo) const 230 | { 231 | return agents_[agentNo]->agentNeighbors_.size(); 232 | } 233 | 234 | size_t RVOSimulator::getAgentNumObstacleNeighbors(size_t agentNo) const 235 | { 236 | return agents_[agentNo]->obstacleNeighbors_.size(); 237 | } 238 | 239 | size_t RVOSimulator::getAgentNumORCALines(size_t agentNo) const 240 | { 241 | return agents_[agentNo]->orcaLines_.size(); 242 | } 243 | 244 | size_t RVOSimulator::getAgentObstacleNeighbor(size_t agentNo, size_t neighborNo) const 245 | { 246 | return agents_[agentNo]->obstacleNeighbors_[neighborNo].second->id_; 247 | } 248 | 249 | const Line &RVOSimulator::getAgentORCALine(size_t agentNo, size_t lineNo) const 250 | { 251 | return agents_[agentNo]->orcaLines_[lineNo]; 252 | } 253 | 254 | const Vector2 &RVOSimulator::getAgentPosition(size_t agentNo) const 255 | { 256 | return agents_[agentNo]->position_; 257 | } 258 | 259 | const Vector2 &RVOSimulator::getAgentPrefVelocity(size_t agentNo) const 260 | { 261 | return agents_[agentNo]->prefVelocity_; 262 | } 263 | 264 | float RVOSimulator::getAgentRadius(size_t agentNo) const 265 | { 266 | return agents_[agentNo]->radius_; 267 | } 268 | 269 | float RVOSimulator::getAgentTimeHorizon(size_t agentNo) const 270 | { 271 | return agents_[agentNo]->timeHorizon_; 272 | } 273 | 274 | float RVOSimulator::getAgentTimeHorizonObst(size_t agentNo) const 275 | { 276 | return agents_[agentNo]->timeHorizonObst_; 277 | } 278 | 279 | const Vector2 &RVOSimulator::getAgentVelocity(size_t agentNo) const 280 | { 281 | return agents_[agentNo]->velocity_; 282 | } 283 | 284 | float RVOSimulator::getGlobalTime() const 285 | { 286 | return globalTime_; 287 | } 288 | 289 | size_t RVOSimulator::getNumAgents() const 290 | { 291 | return agents_.size(); 292 | } 293 | 294 | size_t RVOSimulator::getNumObstacleVertices() const 295 | { 296 | return obstacles_.size(); 297 | } 298 | 299 | const Vector2 &RVOSimulator::getObstacleVertex(size_t vertexNo) const 300 | { 301 | return obstacles_[vertexNo]->point_; 302 | } 303 | 304 | size_t RVOSimulator::getNextObstacleVertexNo(size_t vertexNo) const 305 | { 306 | return obstacles_[vertexNo]->nextObstacle_->id_; 307 | } 308 | 309 | size_t RVOSimulator::getPrevObstacleVertexNo(size_t vertexNo) const 310 | { 311 | return obstacles_[vertexNo]->prevObstacle_->id_; 312 | } 313 | 314 | float RVOSimulator::getTimeStep() const 315 | { 316 | return timeStep_; 317 | } 318 | 319 | void RVOSimulator::processObstacles() 320 | { 321 | kdTree_->buildObstacleTree(); 322 | } 323 | 324 | bool RVOSimulator::queryVisibility(const Vector2 &point1, const Vector2 &point2, float radius) const 325 | { 326 | return kdTree_->queryVisibility(point1, point2, radius); 327 | } 328 | 329 | void RVOSimulator::setAgentDefaults(float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, const Vector2 &velocity) 330 | { 331 | if (defaultAgent_ == NULL) { 332 | defaultAgent_ = new Agent(this); 333 | } 334 | 335 | defaultAgent_->maxNeighbors_ = maxNeighbors; 336 | defaultAgent_->maxSpeed_ = maxSpeed; 337 | defaultAgent_->neighborDist_ = neighborDist; 338 | defaultAgent_->radius_ = radius; 339 | defaultAgent_->timeHorizon_ = timeHorizon; 340 | defaultAgent_->timeHorizonObst_ = timeHorizonObst; 341 | defaultAgent_->velocity_ = velocity; 342 | } 343 | 344 | void RVOSimulator::setAgentMaxNeighbors(size_t agentNo, size_t maxNeighbors) 345 | { 346 | agents_[agentNo]->maxNeighbors_ = maxNeighbors; 347 | } 348 | 349 | void RVOSimulator::setAgentMaxSpeed(size_t agentNo, float maxSpeed) 350 | { 351 | agents_[agentNo]->maxSpeed_ = maxSpeed; 352 | } 353 | 354 | void RVOSimulator::setAgentNeighborDist(size_t agentNo, float neighborDist) 355 | { 356 | agents_[agentNo]->neighborDist_ = neighborDist; 357 | } 358 | 359 | void RVOSimulator::setAgentPosition(size_t agentNo, const Vector2 &position) 360 | { 361 | agents_[agentNo]->position_ = position; 362 | } 363 | 364 | void RVOSimulator::setAgentPrefVelocity(size_t agentNo, const Vector2 &prefVelocity) 365 | { 366 | agents_[agentNo]->prefVelocity_ = prefVelocity; 367 | } 368 | 369 | void RVOSimulator::setAgentRadius(size_t agentNo, float radius) 370 | { 371 | agents_[agentNo]->radius_ = radius; 372 | } 373 | 374 | void RVOSimulator::setAgentTimeHorizon(size_t agentNo, float timeHorizon) 375 | { 376 | agents_[agentNo]->timeHorizon_ = timeHorizon; 377 | } 378 | 379 | void RVOSimulator::setAgentTimeHorizonObst(size_t agentNo, float timeHorizonObst) 380 | { 381 | agents_[agentNo]->timeHorizonObst_ = timeHorizonObst; 382 | } 383 | 384 | void RVOSimulator::setAgentVelocity(size_t agentNo, const Vector2 &velocity) 385 | { 386 | agents_[agentNo]->velocity_ = velocity; 387 | } 388 | 389 | void RVOSimulator::setTimeStep(float timeStep) 390 | { 391 | timeStep_ = timeStep; 392 | } 393 | } 394 | -------------------------------------------------------------------------------- /rvo2/include/RVO/Vector2.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Vector2.h 3 | * RVO2 Library 4 | * 5 | * Copyright (c) 2008-2010 University of North Carolina at Chapel Hill. 6 | * All rights reserved. 7 | * 8 | * Permission to use, copy, modify, and distribute this software and its 9 | * documentation for educational, research, and non-profit purposes, without 10 | * fee, and without a written agreement is hereby granted, provided that the 11 | * above copyright notice, this paragraph, and the following four paragraphs 12 | * appear in all copies. 13 | * 14 | * Permission to incorporate this software into commercial products may be 15 | * obtained by contacting the authors or the Office of 16 | * Technology Development at the University of North Carolina at Chapel Hill 17 | * . 18 | * 19 | * This software program and documentation are copyrighted by the University of 20 | * North Carolina at Chapel Hill. The software program and documentation are 21 | * supplied "as is," without any accompanying services from the University of 22 | * North Carolina at Chapel Hill or the authors. The University of North 23 | * Carolina at Chapel Hill and the authors do not warrant that the operation of 24 | * the program will be uninterrupted or error-free. The end-user understands 25 | * that the program was developed for research purposes and is advised not to 26 | * rely exclusively on the program for any reason. 27 | * 28 | * IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE 29 | * AUTHORS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR 30 | * CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS 31 | * SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF NORTH CAROLINA AT 32 | * CHAPEL HILL OR THE AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH 33 | * DAMAGE. 34 | * 35 | * THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS SPECIFICALLY 36 | * DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 37 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AND ANY 38 | * STATUTORY WARRANTY OF NON-INFRINGEMENT. THE SOFTWARE PROVIDED HEREUNDER IS ON 39 | * AN "AS IS" BASIS, AND THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE 40 | * AUTHORS HAVE NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, 41 | * ENHANCEMENTS, OR MODIFICATIONS. 42 | * 43 | * Please send all bug reports to . 44 | * 45 | * The authors may be contacted via: 46 | * 47 | * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha 48 | * Dept. of Computer Science 49 | * 201 S. Columbia St. 50 | * Frederick P. Brooks, Jr. Computer Science Bldg. 51 | * Chapel Hill, N.C. 27599-3175 52 | * United States of America 53 | * 54 | * 55 | */ 56 | 57 | #ifndef RVO_VECTOR2_H_ 58 | #define RVO_VECTOR2_H_ 59 | 60 | /** 61 | * \file Vector2.h 62 | * \brief Contains the Vector2 class. 63 | */ 64 | 65 | #include 66 | #include 67 | 68 | namespace RVO { 69 | /** 70 | * \brief Defines a two-dimensional vector. 71 | */ 72 | class Vector2 { 73 | public: 74 | /** 75 | * \brief Constructs and initializes a two-dimensional vector instance 76 | * to (0.0, 0.0). 77 | */ 78 | inline Vector2() : x_(0.0f), y_(0.0f) { } 79 | 80 | /** 81 | * \brief Constructs and initializes a two-dimensional vector from 82 | * the specified xy-coordinates. 83 | * \param x The x-coordinate of the two-dimensional 84 | * vector. 85 | * \param y The y-coordinate of the two-dimensional 86 | * vector. 87 | */ 88 | inline Vector2(float x, float y) : x_(x), y_(y) { } 89 | 90 | /** 91 | * \brief Returns the x-coordinate of this two-dimensional vector. 92 | * \return The x-coordinate of the two-dimensional vector. 93 | */ 94 | inline float x() const { return x_; } 95 | 96 | /** 97 | * \brief Returns the y-coordinate of this two-dimensional vector. 98 | * \return The y-coordinate of the two-dimensional vector. 99 | */ 100 | inline float y() const { return y_; } 101 | 102 | /** 103 | * \brief Computes the negation of this two-dimensional vector. 104 | * \return The negation of this two-dimensional vector. 105 | */ 106 | inline Vector2 operator-() const 107 | { 108 | return Vector2(-x_, -y_); 109 | } 110 | 111 | /** 112 | * \brief Computes the dot product of this two-dimensional vector with 113 | * the specified two-dimensional vector. 114 | * \param vector The two-dimensional vector with which the 115 | * dot product should be computed. 116 | * \return The dot product of this two-dimensional vector with a 117 | * specified two-dimensional vector. 118 | */ 119 | inline float operator*(const Vector2 &vector) const 120 | { 121 | return x_ * vector.x() + y_ * vector.y(); 122 | } 123 | 124 | /** 125 | * \brief Computes the scalar multiplication of this 126 | * two-dimensional vector with the specified scalar value. 127 | * \param s The scalar value with which the scalar 128 | * multiplication should be computed. 129 | * \return The scalar multiplication of this two-dimensional vector 130 | * with a specified scalar value. 131 | */ 132 | inline Vector2 operator*(float s) const 133 | { 134 | return Vector2(x_ * s, y_ * s); 135 | } 136 | 137 | /** 138 | * \brief Computes the scalar division of this two-dimensional vector 139 | * with the specified scalar value. 140 | * \param s The scalar value with which the scalar 141 | * division should be computed. 142 | * \return The scalar division of this two-dimensional vector with a 143 | * specified scalar value. 144 | */ 145 | inline Vector2 operator/(float s) const 146 | { 147 | const float invS = 1.0f / s; 148 | 149 | return Vector2(x_ * invS, y_ * invS); 150 | } 151 | 152 | /** 153 | * \brief Computes the vector sum of this two-dimensional vector with 154 | * the specified two-dimensional vector. 155 | * \param vector The two-dimensional vector with which the 156 | * vector sum should be computed. 157 | * \return The vector sum of this two-dimensional vector with a 158 | * specified two-dimensional vector. 159 | */ 160 | inline Vector2 operator+(const Vector2 &vector) const 161 | { 162 | return Vector2(x_ + vector.x(), y_ + vector.y()); 163 | } 164 | 165 | /** 166 | * \brief Computes the vector difference of this two-dimensional 167 | * vector with the specified two-dimensional vector. 168 | * \param vector The two-dimensional vector with which the 169 | * vector difference should be computed. 170 | * \return The vector difference of this two-dimensional vector with a 171 | * specified two-dimensional vector. 172 | */ 173 | inline Vector2 operator-(const Vector2 &vector) const 174 | { 175 | return Vector2(x_ - vector.x(), y_ - vector.y()); 176 | } 177 | 178 | /** 179 | * \brief Tests this two-dimensional vector for equality with the 180 | * specified two-dimensional vector. 181 | * \param vector The two-dimensional vector with which to 182 | * test for equality. 183 | * \return True if the two-dimensional vectors are equal. 184 | */ 185 | inline bool operator==(const Vector2 &vector) const 186 | { 187 | return x_ == vector.x() && y_ == vector.y(); 188 | } 189 | 190 | /** 191 | * \brief Tests this two-dimensional vector for inequality with the 192 | * specified two-dimensional vector. 193 | * \param vector The two-dimensional vector with which to 194 | * test for inequality. 195 | * \return True if the two-dimensional vectors are not equal. 196 | */ 197 | inline bool operator!=(const Vector2 &vector) const 198 | { 199 | return x_ != vector.x() || y_ != vector.y(); 200 | } 201 | 202 | /** 203 | * \brief Sets the value of this two-dimensional vector to the scalar 204 | * multiplication of itself with the specified scalar value. 205 | * \param s The scalar value with which the scalar 206 | * multiplication should be computed. 207 | * \return A reference to this two-dimensional vector. 208 | */ 209 | inline Vector2 &operator*=(float s) 210 | { 211 | x_ *= s; 212 | y_ *= s; 213 | 214 | return *this; 215 | } 216 | 217 | /** 218 | * \brief Sets the value of this two-dimensional vector to the scalar 219 | * division of itself with the specified scalar value. 220 | * \param s The scalar value with which the scalar 221 | * division should be computed. 222 | * \return A reference to this two-dimensional vector. 223 | */ 224 | inline Vector2 &operator/=(float s) 225 | { 226 | const float invS = 1.0f / s; 227 | x_ *= invS; 228 | y_ *= invS; 229 | 230 | return *this; 231 | } 232 | 233 | /** 234 | * \brief Sets the value of this two-dimensional vector to the vector 235 | * sum of itself with the specified two-dimensional vector. 236 | * \param vector The two-dimensional vector with which the 237 | * vector sum should be computed. 238 | * \return A reference to this two-dimensional vector. 239 | */ 240 | inline Vector2 &operator+=(const Vector2 &vector) 241 | { 242 | x_ += vector.x(); 243 | y_ += vector.y(); 244 | 245 | return *this; 246 | } 247 | 248 | /** 249 | * \brief Sets the value of this two-dimensional vector to the vector 250 | * difference of itself with the specified two-dimensional 251 | * vector. 252 | * \param vector The two-dimensional vector with which the 253 | * vector difference should be computed. 254 | * \return A reference to this two-dimensional vector. 255 | */ 256 | inline Vector2 &operator-=(const Vector2 &vector) 257 | { 258 | x_ -= vector.x(); 259 | y_ -= vector.y(); 260 | 261 | return *this; 262 | } 263 | 264 | private: 265 | float x_; 266 | float y_; 267 | }; 268 | 269 | /** 270 | * \relates Vector2 271 | * \brief Computes the scalar multiplication of the specified 272 | * two-dimensional vector with the specified scalar value. 273 | * \param s The scalar value with which the scalar 274 | * multiplication should be computed. 275 | * \param vector The two-dimensional vector with which the scalar 276 | * multiplication should be computed. 277 | * \return The scalar multiplication of the two-dimensional vector with the 278 | * scalar value. 279 | */ 280 | inline Vector2 operator*(float s, const Vector2 &vector) 281 | { 282 | return Vector2(s * vector.x(), s * vector.y()); 283 | } 284 | 285 | /** 286 | * \relates Vector2 287 | * \brief Inserts the specified two-dimensional vector into the specified 288 | * output stream. 289 | * \param os The output stream into which the two-dimensional 290 | * vector should be inserted. 291 | * \param vector The two-dimensional vector which to insert into 292 | * the output stream. 293 | * \return A reference to the output stream. 294 | */ 295 | inline std::ostream &operator<<(std::ostream &os, const Vector2 &vector) 296 | { 297 | os << "(" << vector.x() << "," << vector.y() << ")"; 298 | 299 | return os; 300 | } 301 | 302 | /** 303 | * \relates Vector2 304 | * \brief Computes the length of a specified two-dimensional vector. 305 | * \param vector The two-dimensional vector whose length is to be 306 | * computed. 307 | * \return The length of the two-dimensional vector. 308 | */ 309 | inline float abs(const Vector2 &vector) 310 | { 311 | return std::sqrt(vector * vector); 312 | } 313 | 314 | /** 315 | * \relates Vector2 316 | * \brief Computes the squared length of a specified two-dimensional 317 | * vector. 318 | * \param vector The two-dimensional vector whose squared length 319 | * is to be computed. 320 | * \return The squared length of the two-dimensional vector. 321 | */ 322 | inline float absSq(const Vector2 &vector) 323 | { 324 | return vector * vector; 325 | } 326 | 327 | /** 328 | * \relates Vector2 329 | * \brief Computes the determinant of a two-dimensional square matrix with 330 | * rows consisting of the specified two-dimensional vectors. 331 | * \param vector1 The top row of the two-dimensional square 332 | * matrix. 333 | * \param vector2 The bottom row of the two-dimensional square 334 | * matrix. 335 | * \return The determinant of the two-dimensional square matrix. 336 | */ 337 | inline float det(const Vector2 &vector1, const Vector2 &vector2) 338 | { 339 | return vector1.x() * vector2.y() - vector1.y() * vector2.x(); 340 | } 341 | 342 | /** 343 | * \relates Vector2 344 | * \brief Computes the normalization of the specified two-dimensional 345 | * vector. 346 | * \param vector The two-dimensional vector whose normalization 347 | * is to be computed. 348 | * \return The normalization of the two-dimensional vector. 349 | */ 350 | inline Vector2 normalize(const Vector2 &vector) 351 | { 352 | return vector / abs(vector); 353 | } 354 | } 355 | 356 | #endif /* RVO_VECTOR2_H_ */ 357 | -------------------------------------------------------------------------------- /player_map/src/rosmap.cpp: -------------------------------------------------------------------------------- 1 | #include "player_map/rosmap.hpp" 2 | 3 | #include 4 | 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | using namespace std; 11 | 12 | namespace rvo { 13 | 14 | double pathLength(const rvo::PointVector &path) { 15 | double dist = 0.0; 16 | for (size_t i = 0; i < path.size() - 1; ++i) { 17 | dist += (path[i] - path[i+1]).norm(); 18 | } 19 | return dist; 20 | } 21 | 22 | void convertMap(const nav_msgs::OccupancyGrid &map, map_t *pmap) { 23 | pmap->size_x = map.info.width; 24 | pmap->size_y = map.info.height; 25 | pmap->scale = map.info.resolution; 26 | pmap->origin_x = map.info.origin.position.x + (pmap->size_x / 2) * pmap->scale; 27 | pmap->origin_y = map.info.origin.position.y + (pmap->size_y / 2) * pmap->scale; 28 | pmap->max_occ_dist = 0.0; 29 | // Convert to player format 30 | pmap->cells = (map_cell_t*)malloc(sizeof(map_cell_t)*pmap->size_x*pmap->size_y); 31 | ROS_ASSERT(pmap->cells); 32 | for(int i=0;isize_x * pmap->size_y;i++) { 33 | if(map.data[i] == 0) 34 | pmap->cells[i].occ_state = -1; 35 | else if(map.data[i] == 100) 36 | pmap->cells[i].occ_state = +1; 37 | else 38 | pmap->cells[i].occ_state = 0; 39 | 40 | pmap->cells[i].occ_dist = 0; 41 | } 42 | } 43 | 44 | map_t * requestCSpaceMap(const char *srv_name) { 45 | ros::NodeHandle nh; 46 | // Taken from PAMCL 47 | map_t* map = map_alloc(); 48 | ROS_ASSERT(map); 49 | 50 | // get map via RPC 51 | player_map::GetMap::Request req; 52 | player_map::GetMap::Response resp; 53 | ROS_INFO_ONCE("Requesting c-space + map..."); 54 | while(!ros::service::call(srv_name, req, resp)) { 55 | ROS_WARN("Request for map '%s' failed; trying again...", 56 | ros::names::resolve(string(srv_name)).c_str()); 57 | ros::Duration d(4.0); 58 | d.sleep(); 59 | if (!nh.ok()) 60 | return NULL; 61 | } 62 | ROS_INFO_ONCE("Received a %d X %d map @ %.3f m/pix max_occ_dist= %.3f\n", 63 | resp.map.info.width, resp.map.info.height, 64 | resp.map.info.resolution, resp.max_occ_dist); 65 | convertMap(resp.map, map); 66 | 67 | map->max_occ_dist = resp.max_occ_dist; 68 | for (int j = 0; j < map->size_y; j++) { 69 | for (int i = 0; i < map->size_x; i++) { 70 | int ind = MAP_INDEX(map, i, j); 71 | (map->cells + ind)->occ_dist = resp.occ_dist[ind]; 72 | } 73 | } 74 | return map; 75 | } 76 | 77 | map_t * requestMap(const char *srv_name) { 78 | ros::NodeHandle nh; 79 | // Taken from PAMCL 80 | map_t* map = map_alloc(); 81 | ROS_ASSERT(map); 82 | 83 | // get map via RPC 84 | nav_msgs::GetMap::Request req; 85 | nav_msgs::GetMap::Response resp; 86 | ROS_INFO("Requesting the map..."); 87 | while(!ros::service::call(srv_name, req, resp)) { 88 | ROS_WARN("Request for map '%s' failed; trying again...", 89 | ros::names::resolve(string(srv_name)).c_str()); 90 | ros::Duration d(4.0); 91 | d.sleep(); 92 | if (!nh.ok()) 93 | return NULL; 94 | } 95 | ROS_INFO_ONCE("Received a %d X %d map @ %.3f m/pix\n", 96 | resp.map.info.width, resp.map.info.height, 97 | resp.map.info.resolution); 98 | 99 | convertMap(resp.map, map); 100 | return map; 101 | } 102 | 103 | 104 | OccupancyMap::OccupancyMap(map_t *map) : map_(map), ncells_(0) { 105 | 106 | } 107 | 108 | OccupancyMap::~OccupancyMap() { 109 | if (map_ != NULL) { 110 | map_free(map_); 111 | } 112 | } 113 | 114 | OccupancyMap* OccupancyMap::FromMapServer(const char *srv_name) { 115 | map_t *map = requestMap(srv_name); 116 | return new OccupancyMap(map); 117 | } 118 | 119 | void OccupancyMap::setMap(const nav_msgs::OccupancyGrid &grid) { 120 | if (map_ != NULL) { 121 | map_free(map_); 122 | } 123 | map_ = map_alloc(); 124 | ROS_ASSERT(map_); 125 | convertMap(grid, map_); 126 | } 127 | 128 | void OccupancyMap::updateCSpace(double max_occ_dist) { 129 | // check if cspace needs to be updated 130 | if (map_ == NULL || map_->max_occ_dist > max_occ_dist) { 131 | return; 132 | } 133 | map_update_cspace(map_, max_occ_dist); 134 | } 135 | 136 | bool OccupancyMap::lineOfSight(double x1, double y1, double x2, double y2, 137 | double max_occ_dist) const { 138 | if (map_ == NULL) { 139 | return true; 140 | } 141 | if (map_->max_occ_dist < max_occ_dist) { 142 | ROS_ERROR("OccupancyMap::lineOfSight() CSpace has been calculated up to %f, " 143 | "but max_occ_dist=%.2f", 144 | map_->max_occ_dist, max_occ_dist); 145 | ROS_BREAK(); 146 | } 147 | // March along the line between (x1, y1) and (x2, y2) until the point passes 148 | // beyond (x2, y2). 149 | double step_size = map_->scale / 2.0; 150 | 151 | double dy = y2 - y1; 152 | double dx = x2 - x1; 153 | double t = atan2(dy, dx); 154 | double ct = cos(t); 155 | double st = sin(t); 156 | 157 | double mag_sq = dy * dy + dx * dx; 158 | // fprintf(stderr, "t: %f ct: %f st: %f\n", t, ct, st); 159 | double line_x = x1, line_y = y1; 160 | // Terminate when current point defines a vector longer than original vector 161 | while (pow(line_x - x1, 2) + pow(line_y - y1, 2) < mag_sq) { 162 | // fprintf(stderr, "%f %f\n", line_x, line_y); 163 | map_cell_t *cell = map_get_cell(map_, line_x, line_y, 0); 164 | if (cell == NULL) { 165 | // ROS_WARN_THROTTLE(5, "lineOfSight() Beyond map edge"); 166 | return false; 167 | } else if (cell->occ_state != -1 || cell->occ_dist < max_occ_dist) { 168 | return false; 169 | } 170 | line_x += step_size * ct; 171 | line_y += step_size * st; 172 | } 173 | return true; 174 | } 175 | 176 | void OccupancyMap::initializeSearch(double startx, double starty) { 177 | starti_ = MAP_GXWX(map_, startx); 178 | startj_ = MAP_GYWY(map_, starty); 179 | 180 | if (!MAP_VALID(map_, starti_, startj_)) { 181 | ROS_ERROR("OccupancyMap::initializeSearch() Invalid starting position"); 182 | ROS_BREAK(); 183 | } 184 | 185 | int ncells = map_->size_x * map_->size_y; 186 | if (ncells_ != ncells) { 187 | ncells_ = ncells; 188 | costs_.reset(new float[ncells]); 189 | prev_i_.reset(new int[ncells]); 190 | prev_j_.reset(new int[ncells]); 191 | } 192 | 193 | // TODO: Return to more efficient lazy-initialization 194 | // // Map is large and initializing costs_ takes a while. To speedup, 195 | // // partially initialize costs_ in a rectangle surrounding start and stop 196 | // // positions + margin. If you run up against boundary, initialize the rest. 197 | // int margin = 120; 198 | // init_ul_ = make_pair(max(0, min(starti_, stopi) - margin), 199 | // max(0, min(startj_, stopj) - margin)); 200 | // init_lr_ = make_pair(min(map_->size_x, max(starti_, stopi) + margin), 201 | // min(map_->size_y, max(startj_, stopj) + margin)); 202 | // for (int j = init_ul.second; j < init_lr.second; ++j) { 203 | // for (int i = init_ul.first; i < init_lr.first; ++i) { 204 | // int ind = MAP_INDEX(map_, i, j); 205 | // costs_[ind] = std::numeric_limits::infinity(); 206 | // } 207 | // } 208 | // full_init_ = false; 209 | 210 | for (int i = 0; i < ncells_; ++i) { 211 | costs_[i] = std::numeric_limits::infinity(); 212 | prev_i_[i] = -1; 213 | prev_j_[i] = -1; 214 | } 215 | 216 | int start_ind = MAP_INDEX(map_, starti_, startj_); 217 | costs_[start_ind] = 0.0; 218 | prev_i_[starti_] = starti_; 219 | prev_j_[startj_] = startj_; 220 | 221 | Q_.reset(new set()); 222 | Q_->insert(Node(make_pair(starti_, startj_), 0.0, 0.0)); 223 | 224 | stopi_ = -1; 225 | stopj_ = -1; 226 | } 227 | 228 | void OccupancyMap::addNeighbors(const Node &node, double max_occ_dist) { 229 | // TODO: Return to more efficient lazy-initialization 230 | // // Check if we're neighboring nodes whose costs_ are uninitialized. 231 | // // if (!full_init && 232 | // // ((ci + 1 >= init_lr.first) || (ci - 1 <= init_ul.first) || 233 | // // (cj + 1 >= init_lr.second) || (cj - 1 <= init_ul.second))) { 234 | // // full_init = true; 235 | // // for (int j = 0; j < map_->size_y; ++j) { 236 | // // for (int i = 0; i < map_->size_x; ++i) { 237 | // // // Only initialize costs_ that are outside original rectangle 238 | // // if (!(init_ul.first <= i && i < init_lr.first && 239 | // // init_ul.second <= j && j < init_lr.second)) { 240 | // // int ind = MAP_INDEX(map_, i, j); 241 | // // costs_[ind] = std::numeric_limits::infinity(); 242 | // // } 243 | // // } 244 | // // } 245 | // // } 246 | 247 | int ci = node.coord.first; 248 | int cj = node.coord.second; 249 | 250 | // Iterate over neighbors 251 | for (int newj = cj - 1; newj <= cj + 1; ++newj) { 252 | for (int newi = ci - 1; newi <= ci + 1; ++newi) { 253 | // Skip self edges 254 | if ((newi == ci && newj == cj) || !MAP_VALID(map_, newi, newj)) { 255 | continue; 256 | } 257 | // fprintf(stderr, " Examining %i %i ", newi, newj); 258 | int index = MAP_INDEX(map_, newi, newj); 259 | map_cell_t *cell = map_->cells + index; 260 | // If cell is occupied or too close to occupied cell, continue 261 | 262 | if (cell->occ_state != -1 || cell->occ_dist < max_occ_dist) { 263 | // fprintf(stderr, "occupado\n"); 264 | continue; 265 | } 266 | // fprintf(stderr, "free\n"); 267 | double edge_cost = ci == newi || cj == newj ? 1 : sqrt(2); 268 | double heur_cost = 0.0; 269 | if (stopi_ != -1 && stopj_ != -1) { 270 | heur_cost = hypot(newi - stopi_, newj - stopj_); 271 | } 272 | double ttl_cost = edge_cost + node.true_dist + heur_cost; 273 | if (ttl_cost < costs_[index]) { 274 | // fprintf(stderr, " Better path: new cost= % 6.2f\n", ttl_cost); 275 | // If node has finite cost, it's in queue and needs to be removed 276 | if (!isinf(costs_[index])) { 277 | Q_->erase(Node(make_pair(newi, newj), costs_[index], 0.0)); 278 | } 279 | costs_[index] = ttl_cost; 280 | prev_i_[index] = ci; 281 | prev_j_[index] = cj; 282 | Q_->insert(Node(make_pair(newi, newj), 283 | edge_cost + node.true_dist, 284 | ttl_cost)); 285 | } 286 | } 287 | } 288 | } 289 | 290 | void OccupancyMap::buildPath(int i, int j, PointVector *path) { 291 | while (!(i == starti_ && j == startj_)) { 292 | int index = MAP_INDEX(map_, i, j); 293 | float x = MAP_WXGX(map_, i); 294 | float y = MAP_WYGY(map_, j); 295 | path->push_back(Eigen::Vector2f(x, y)); 296 | 297 | i = prev_i_[index]; 298 | j = prev_j_[index]; 299 | } 300 | float x = MAP_WXGX(map_, i); 301 | float y = MAP_WYGY(map_, j); 302 | path->push_back(Eigen::Vector2f(x, y)); 303 | } 304 | 305 | bool OccupancyMap::nextNode(double max_occ_dist, Node *curr_node) { 306 | if (!Q_->empty()) { 307 | // Copy node and then erase it 308 | *curr_node = *Q_->begin(); 309 | int ci = curr_node->coord.first, cj = curr_node->coord.second; 310 | // fprintf(stderr, "At %i %i (cost = %6.2f) % 7.2f % 7.2f \n", 311 | // ci, cj, curr_node.true_dist, MAP_WXGX(map_, ci), MAP_WYGY(map_, cj)); 312 | costs_[MAP_INDEX(map_, ci, cj)] = curr_node->true_dist; 313 | Q_->erase(Q_->begin()); 314 | addNeighbors(*curr_node, max_occ_dist); 315 | return true; 316 | } else { 317 | return false; 318 | } 319 | } 320 | 321 | PointVector OccupancyMap::astar(double startx, double starty, 322 | double stopx, double stopy, 323 | double max_occ_dist /* = 0.0 */) { 324 | int stopi = MAP_GXWX(map_, stopx), stopj = MAP_GYWY(map_, stopy); 325 | if (!MAP_VALID(map_, stopi ,stopj)) { 326 | ROS_ERROR("OccupancyMap::astar() Invalid stopping position"); 327 | ROS_BREAK(); 328 | } 329 | if (map_->max_occ_dist < max_occ_dist) { 330 | ROS_ERROR("OccupancyMap::astar() CSpace has been calculated up to %f, " 331 | "but max_occ_dist=%.2f", 332 | map_->max_occ_dist, max_occ_dist); 333 | ROS_BREAK(); 334 | } 335 | 336 | initializeSearch(startx, starty); 337 | // Set stop to use heuristic 338 | stopi_ = stopi; 339 | stopj_ = stopj; 340 | 341 | bool found = false; 342 | Node curr_node; 343 | while (nextNode(max_occ_dist, &curr_node)) { 344 | if (curr_node.coord.first == stopi && curr_node.coord.second == stopj) { 345 | found = true; 346 | break; 347 | } 348 | } 349 | 350 | // Recreate path 351 | PointVector path; 352 | if (found) { 353 | buildPath(stopi, stopj, &path); 354 | } 355 | return PointVector(path.rbegin(), path.rend()); 356 | } 357 | 358 | const PointVector& 359 | OccupancyMap::prepareShortestPaths(double x, double y, double max_occ_dist, 360 | double min_dist, double max_dist) { 361 | if (map_->max_occ_dist < max_occ_dist) { 362 | ROS_ERROR("OccupancyMap::prepareShortestPaths() CSpace has been calculated " 363 | "up to %f, but max_occ_dist=%.2f", 364 | map_->max_occ_dist, max_occ_dist); 365 | ROS_BREAK(); 366 | } 367 | 368 | initializeSearch(x, y); 369 | 370 | Node curr_node; 371 | endpoints_.clear(); 372 | while (nextNode(max_occ_dist, &curr_node)) { 373 | double node_dist = curr_node.true_dist * map_->scale; 374 | if (min_dist < node_dist && node_dist < max_dist) { 375 | float x = MAP_WXGX(map_, curr_node.coord.first); 376 | float y = MAP_WYGY(map_, curr_node.coord.second); 377 | endpoints_.push_back(Eigen::Vector2f(x, y)); 378 | } else if (max_dist < node_dist) { 379 | break; 380 | } 381 | } 382 | return endpoints_; 383 | } 384 | 385 | PointVector OccupancyMap::buildShortestPath(int ind) { 386 | // Recreate path 387 | const Eigen::Vector2f &stop = endpoints_.at(ind); 388 | PointVector path; 389 | int stopi = MAP_GXWX(map_, stop(0)), stopj = MAP_GYWY(map_, stop(1)); 390 | buildPath(stopi, stopj, &path); 391 | return PointVector(path.rbegin(), path.rend()); 392 | } 393 | 394 | void OccupancyMap::prepareAllShortestPaths(double x, double y, 395 | double max_occ_dist) { 396 | if (map_->max_occ_dist < max_occ_dist) { 397 | ROS_ERROR("OccupancyMap::shortestToDests() CSpace has been calculated " 398 | "up to %f, but max_occ_dist=%.2f", 399 | map_->max_occ_dist, max_occ_dist); 400 | ROS_BREAK(); 401 | } 402 | 403 | initializeSearch(x, y); 404 | 405 | Node curr_node; 406 | while (nextNode(max_occ_dist, &curr_node)) { 407 | ; 408 | } 409 | } 410 | 411 | PointVector OccupancyMap::shortestPath(double stopx, double stopy) { 412 | int i = MAP_GXWX(map_, stopx); 413 | int j = MAP_GYWY(map_, stopy); 414 | int ind = MAP_INDEX(map_, i, j); 415 | PointVector path; 416 | 417 | if (!MAP_VALID(map_, i, j)) { 418 | ROS_ERROR("OccMap::shortestPath() Invalid destination: x=%f y=%f", 419 | stopx, stopy); 420 | ROS_BREAK(); 421 | return path; // return to prevent compiler warning 422 | } else if ( prev_i_[ind] == -1 || prev_j_[ind] == -1) { 423 | return path; 424 | } else { 425 | buildPath(i, j, &path); 426 | return PointVector(path.rbegin(), path.rend()); 427 | } 428 | } 429 | 430 | } 431 | -------------------------------------------------------------------------------- /rvo2/src/KdTree.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * KdTree.cpp 3 | * RVO2 Library 4 | * 5 | * Copyright (c) 2008-2010 University of North Carolina at Chapel Hill. 6 | * All rights reserved. 7 | * 8 | * Permission to use, copy, modify, and distribute this software and its 9 | * documentation for educational, research, and non-profit purposes, without 10 | * fee, and without a written agreement is hereby granted, provided that the 11 | * above copyright notice, this paragraph, and the following four paragraphs 12 | * appear in all copies. 13 | * 14 | * Permission to incorporate this software into commercial products may be 15 | * obtained by contacting the authors or the Office of 16 | * Technology Development at the University of North Carolina at Chapel Hill 17 | * . 18 | * 19 | * This software program and documentation are copyrighted by the University of 20 | * North Carolina at Chapel Hill. The software program and documentation are 21 | * supplied "as is," without any accompanying services from the University of 22 | * North Carolina at Chapel Hill or the authors. The University of North 23 | * Carolina at Chapel Hill and the authors do not warrant that the operation of 24 | * the program will be uninterrupted or error-free. The end-user understands 25 | * that the program was developed for research purposes and is advised not to 26 | * rely exclusively on the program for any reason. 27 | * 28 | * IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE 29 | * AUTHORS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR 30 | * CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS 31 | * SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF NORTH CAROLINA AT 32 | * CHAPEL HILL OR THE AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH 33 | * DAMAGE. 34 | * 35 | * THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS SPECIFICALLY 36 | * DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 37 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AND ANY 38 | * STATUTORY WARRANTY OF NON-INFRINGEMENT. THE SOFTWARE PROVIDED HEREUNDER IS ON 39 | * AN "AS IS" BASIS, AND THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE 40 | * AUTHORS HAVE NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, 41 | * ENHANCEMENTS, OR MODIFICATIONS. 42 | * 43 | * Please send all bug reports to . 44 | * 45 | * The authors may be contacted via: 46 | * 47 | * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha 48 | * Dept. of Computer Science 49 | * 201 S. Columbia St. 50 | * Frederick P. Brooks, Jr. Computer Science Bldg. 51 | * Chapel Hill, N.C. 27599-3175 52 | * United States of America 53 | * 54 | * 55 | */ 56 | 57 | #include "KdTree.h" 58 | 59 | #include "Agent.h" 60 | #include "RVOSimulator.h" 61 | #include "Obstacle.h" 62 | 63 | namespace RVO { 64 | KdTree::KdTree(RVOSimulator *sim) : obstacleTree_(NULL), sim_(sim) { } 65 | 66 | KdTree::~KdTree() 67 | { 68 | deleteObstacleTree(obstacleTree_); 69 | } 70 | 71 | void KdTree::buildAgentTree() 72 | { 73 | if (agents_.size() < sim_->agents_.size()) { 74 | for (size_t i = agents_.size(); i < sim_->agents_.size(); ++i) { 75 | agents_.push_back(sim_->agents_[i]); 76 | } 77 | 78 | agentTree_.resize(2 * agents_.size() - 1); 79 | } 80 | 81 | if (!agents_.empty()) { 82 | buildAgentTreeRecursive(0, agents_.size(), 0); 83 | } 84 | } 85 | 86 | void KdTree::buildAgentTreeRecursive(size_t begin, size_t end, size_t node) 87 | { 88 | agentTree_[node].begin = begin; 89 | agentTree_[node].end = end; 90 | agentTree_[node].minX = agentTree_[node].maxX = agents_[begin]->position_.x(); 91 | agentTree_[node].minY = agentTree_[node].maxY = agents_[begin]->position_.y(); 92 | 93 | for (size_t i = begin + 1; i < end; ++i) { 94 | agentTree_[node].maxX = std::max(agentTree_[node].maxX, agents_[i]->position_.x()); 95 | agentTree_[node].minX = std::min(agentTree_[node].minX, agents_[i]->position_.x()); 96 | agentTree_[node].maxY = std::max(agentTree_[node].maxY, agents_[i]->position_.y()); 97 | agentTree_[node].minY = std::min(agentTree_[node].minY, agents_[i]->position_.y()); 98 | } 99 | 100 | if (end - begin > MAX_LEAF_SIZE) { 101 | /* No leaf node. */ 102 | const bool isVertical = (agentTree_[node].maxX - agentTree_[node].minX > agentTree_[node].maxY - agentTree_[node].minY); 103 | const float splitValue = (isVertical ? 0.5f * (agentTree_[node].maxX + agentTree_[node].minX) : 0.5f * (agentTree_[node].maxY + agentTree_[node].minY)); 104 | 105 | size_t left = begin; 106 | size_t right = end; 107 | 108 | while (left < right) { 109 | while (left < right && (isVertical ? agents_[left]->position_.x() : agents_[left]->position_.y()) < splitValue) { 110 | ++left; 111 | } 112 | 113 | while (right > left && (isVertical ? agents_[right - 1]->position_.x() : agents_[right - 1]->position_.y()) >= splitValue) { 114 | --right; 115 | } 116 | 117 | if (left < right) { 118 | std::swap(agents_[left], agents_[right - 1]); 119 | ++left; 120 | --right; 121 | } 122 | } 123 | 124 | if (left == begin) { 125 | ++left; 126 | ++right; 127 | } 128 | 129 | agentTree_[node].left = node + 1; 130 | agentTree_[node].right = node + 2 * (left - begin); 131 | 132 | buildAgentTreeRecursive(begin, left, agentTree_[node].left); 133 | buildAgentTreeRecursive(left, end, agentTree_[node].right); 134 | } 135 | } 136 | 137 | void KdTree::buildObstacleTree() 138 | { 139 | deleteObstacleTree(obstacleTree_); 140 | 141 | std::vector obstacles(sim_->obstacles_.size()); 142 | 143 | for (size_t i = 0; i < sim_->obstacles_.size(); ++i) { 144 | obstacles[i] = sim_->obstacles_[i]; 145 | } 146 | 147 | obstacleTree_ = buildObstacleTreeRecursive(obstacles); 148 | } 149 | 150 | 151 | KdTree::ObstacleTreeNode *KdTree::buildObstacleTreeRecursive(const std::vector &obstacles) 152 | { 153 | if (obstacles.empty()) { 154 | return NULL; 155 | } 156 | else { 157 | ObstacleTreeNode *const node = new ObstacleTreeNode; 158 | 159 | size_t optimalSplit = 0; 160 | size_t minLeft = obstacles.size(); 161 | size_t minRight = obstacles.size(); 162 | 163 | for (size_t i = 0; i < obstacles.size(); ++i) { 164 | size_t leftSize = 0; 165 | size_t rightSize = 0; 166 | 167 | const Obstacle *const obstacleI1 = obstacles[i]; 168 | const Obstacle *const obstacleI2 = obstacleI1->nextObstacle_; 169 | 170 | /* Compute optimal split node. */ 171 | for (size_t j = 0; j < obstacles.size(); ++j) { 172 | if (i == j) { 173 | continue; 174 | } 175 | 176 | const Obstacle *const obstacleJ1 = obstacles[j]; 177 | const Obstacle *const obstacleJ2 = obstacleJ1->nextObstacle_; 178 | 179 | const float j1LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ1->point_); 180 | const float j2LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ2->point_); 181 | 182 | if (j1LeftOfI >= -RVO_EPSILON && j2LeftOfI >= -RVO_EPSILON) { 183 | ++leftSize; 184 | } 185 | else if (j1LeftOfI <= RVO_EPSILON && j2LeftOfI <= RVO_EPSILON) { 186 | ++rightSize; 187 | } 188 | else { 189 | ++leftSize; 190 | ++rightSize; 191 | } 192 | 193 | if (std::make_pair(std::max(leftSize, rightSize), std::min(leftSize, rightSize)) >= std::make_pair(std::max(minLeft, minRight), std::min(minLeft, minRight))) { 194 | break; 195 | } 196 | } 197 | 198 | if (std::make_pair(std::max(leftSize, rightSize), std::min(leftSize, rightSize)) < std::make_pair(std::max(minLeft, minRight), std::min(minLeft, minRight))) { 199 | minLeft = leftSize; 200 | minRight = rightSize; 201 | optimalSplit = i; 202 | } 203 | } 204 | 205 | /* Build split node. */ 206 | std::vector leftObstacles(minLeft); 207 | std::vector rightObstacles(minRight); 208 | 209 | size_t leftCounter = 0; 210 | size_t rightCounter = 0; 211 | const size_t i = optimalSplit; 212 | 213 | const Obstacle *const obstacleI1 = obstacles[i]; 214 | const Obstacle *const obstacleI2 = obstacleI1->nextObstacle_; 215 | 216 | for (size_t j = 0; j < obstacles.size(); ++j) { 217 | if (i == j) { 218 | continue; 219 | } 220 | 221 | Obstacle *const obstacleJ1 = obstacles[j]; 222 | Obstacle *const obstacleJ2 = obstacleJ1->nextObstacle_; 223 | 224 | const float j1LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ1->point_); 225 | const float j2LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ2->point_); 226 | 227 | if (j1LeftOfI >= -RVO_EPSILON && j2LeftOfI >= -RVO_EPSILON) { 228 | leftObstacles[leftCounter++] = obstacles[j]; 229 | } 230 | else if (j1LeftOfI <= RVO_EPSILON && j2LeftOfI <= RVO_EPSILON) { 231 | rightObstacles[rightCounter++] = obstacles[j]; 232 | } 233 | else { 234 | /* Split obstacle j. */ 235 | const float t = det(obstacleI2->point_ - obstacleI1->point_, obstacleJ1->point_ - obstacleI1->point_) / det(obstacleI2->point_ - obstacleI1->point_, obstacleJ1->point_ - obstacleJ2->point_); 236 | 237 | const Vector2 splitpoint = obstacleJ1->point_ + t * (obstacleJ2->point_ - obstacleJ1->point_); 238 | 239 | Obstacle *const newObstacle = new Obstacle(); 240 | newObstacle->point_ = splitpoint; 241 | newObstacle->prevObstacle_ = obstacleJ1; 242 | newObstacle->nextObstacle_ = obstacleJ2; 243 | newObstacle->isConvex_ = true; 244 | newObstacle->unitDir_ = obstacleJ1->unitDir_; 245 | 246 | newObstacle->id_ = sim_->obstacles_.size(); 247 | 248 | sim_->obstacles_.push_back(newObstacle); 249 | 250 | obstacleJ1->nextObstacle_ = newObstacle; 251 | obstacleJ2->prevObstacle_ = newObstacle; 252 | 253 | if (j1LeftOfI > 0.0f) { 254 | leftObstacles[leftCounter++] = obstacleJ1; 255 | rightObstacles[rightCounter++] = newObstacle; 256 | } 257 | else { 258 | rightObstacles[rightCounter++] = obstacleJ1; 259 | leftObstacles[leftCounter++] = newObstacle; 260 | } 261 | } 262 | } 263 | 264 | node->obstacle = obstacleI1; 265 | node->left = buildObstacleTreeRecursive(leftObstacles); 266 | node->right = buildObstacleTreeRecursive(rightObstacles); 267 | return node; 268 | } 269 | } 270 | 271 | void KdTree::computeAgentNeighbors(Agent *agent, float &rangeSq) const 272 | { 273 | queryAgentTreeRecursive(agent, rangeSq, 0); 274 | } 275 | 276 | void KdTree::computeObstacleNeighbors(Agent *agent, float rangeSq) const 277 | { 278 | queryObstacleTreeRecursive(agent, rangeSq, obstacleTree_); 279 | } 280 | 281 | void KdTree::deleteObstacleTree(ObstacleTreeNode *node) 282 | { 283 | if (node != NULL) { 284 | deleteObstacleTree(node->left); 285 | deleteObstacleTree(node->right); 286 | delete node; 287 | } 288 | } 289 | 290 | void KdTree::queryAgentTreeRecursive(Agent *agent, float &rangeSq, size_t node) const 291 | { 292 | if (agentTree_[node].end - agentTree_[node].begin <= MAX_LEAF_SIZE) { 293 | for (size_t i = agentTree_[node].begin; i < agentTree_[node].end; ++i) { 294 | agent->insertAgentNeighbor(agents_[i], rangeSq); 295 | } 296 | } 297 | else { 298 | const float distSqLeft = sqr(std::max(0.0f, agentTree_[agentTree_[node].left].minX - agent->position_.x())) + sqr(std::max(0.0f, agent->position_.x() - agentTree_[agentTree_[node].left].maxX)) + sqr(std::max(0.0f, agentTree_[agentTree_[node].left].minY - agent->position_.y())) + sqr(std::max(0.0f, agent->position_.y() - agentTree_[agentTree_[node].left].maxY)); 299 | 300 | const float distSqRight = sqr(std::max(0.0f, agentTree_[agentTree_[node].right].minX - agent->position_.x())) + sqr(std::max(0.0f, agent->position_.x() - agentTree_[agentTree_[node].right].maxX)) + sqr(std::max(0.0f, agentTree_[agentTree_[node].right].minY - agent->position_.y())) + sqr(std::max(0.0f, agent->position_.y() - agentTree_[agentTree_[node].right].maxY)); 301 | 302 | if (distSqLeft < distSqRight) { 303 | if (distSqLeft < rangeSq) { 304 | queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left); 305 | 306 | if (distSqRight < rangeSq) { 307 | queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right); 308 | } 309 | } 310 | } 311 | else { 312 | if (distSqRight < rangeSq) { 313 | queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right); 314 | 315 | if (distSqLeft < rangeSq) { 316 | queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left); 317 | } 318 | } 319 | } 320 | 321 | } 322 | } 323 | 324 | void KdTree::queryObstacleTreeRecursive(Agent *agent, float rangeSq, const ObstacleTreeNode *node) const 325 | { 326 | if (node == NULL) { 327 | return; 328 | } 329 | else { 330 | const Obstacle *const obstacle1 = node->obstacle; 331 | const Obstacle *const obstacle2 = obstacle1->nextObstacle_; 332 | 333 | const float agentLeftOfLine = leftOf(obstacle1->point_, obstacle2->point_, agent->position_); 334 | 335 | queryObstacleTreeRecursive(agent, rangeSq, (agentLeftOfLine >= 0.0f ? node->left : node->right)); 336 | 337 | const float distSqLine = sqr(agentLeftOfLine) / absSq(obstacle2->point_ - obstacle1->point_); 338 | 339 | if (distSqLine < rangeSq) { 340 | if (agentLeftOfLine < 0.0f) { 341 | /* 342 | * Try obstacle at this node only if agent is on right side of 343 | * obstacle (and can see obstacle). 344 | */ 345 | agent->insertObstacleNeighbor(node->obstacle, rangeSq); 346 | } 347 | 348 | /* Try other side of line. */ 349 | queryObstacleTreeRecursive(agent, rangeSq, (agentLeftOfLine >= 0.0f ? node->right : node->left)); 350 | 351 | } 352 | } 353 | } 354 | 355 | bool KdTree::queryVisibility(const Vector2 &q1, const Vector2 &q2, float radius) const 356 | { 357 | return queryVisibilityRecursive(q1, q2, radius, obstacleTree_); 358 | } 359 | 360 | bool KdTree::queryVisibilityRecursive(const Vector2 &q1, const Vector2 &q2, float radius, const ObstacleTreeNode *node) const 361 | { 362 | if (node == NULL) { 363 | return true; 364 | } 365 | else { 366 | const Obstacle *const obstacle1 = node->obstacle; 367 | const Obstacle *const obstacle2 = obstacle1->nextObstacle_; 368 | 369 | const float q1LeftOfI = leftOf(obstacle1->point_, obstacle2->point_, q1); 370 | const float q2LeftOfI = leftOf(obstacle1->point_, obstacle2->point_, q2); 371 | const float invLengthI = 1.0f / absSq(obstacle2->point_ - obstacle1->point_); 372 | 373 | if (q1LeftOfI >= 0.0f && q2LeftOfI >= 0.0f) { 374 | return queryVisibilityRecursive(q1, q2, radius, node->left) && ((sqr(q1LeftOfI) * invLengthI >= sqr(radius) && sqr(q2LeftOfI) * invLengthI >= sqr(radius)) || queryVisibilityRecursive(q1, q2, radius, node->right)); 375 | } 376 | else if (q1LeftOfI <= 0.0f && q2LeftOfI <= 0.0f) { 377 | return queryVisibilityRecursive(q1, q2, radius, node->right) && ((sqr(q1LeftOfI) * invLengthI >= sqr(radius) && sqr(q2LeftOfI) * invLengthI >= sqr(radius)) || queryVisibilityRecursive(q1, q2, radius, node->left)); 378 | } 379 | else if (q1LeftOfI >= 0.0f && q2LeftOfI <= 0.0f) { 380 | /* One can see through obstacle from left to right. */ 381 | return queryVisibilityRecursive(q1, q2, radius, node->left) && queryVisibilityRecursive(q1, q2, radius, node->right); 382 | } 383 | else { 384 | const float point1LeftOfQ = leftOf(q1, q2, obstacle1->point_); 385 | const float point2LeftOfQ = leftOf(q1, q2, obstacle2->point_); 386 | const float invLengthQ = 1.0f / absSq(q2 - q1); 387 | 388 | return (point1LeftOfQ * point2LeftOfQ >= 0.0f && sqr(point1LeftOfQ) * invLengthQ > sqr(radius) && sqr(point2LeftOfQ) * invLengthQ > sqr(radius) && queryVisibilityRecursive(q1, q2, radius, node->left) && queryVisibilityRecursive(q1, q2, radius, node->right)); 389 | } 390 | } 391 | } 392 | } 393 | -------------------------------------------------------------------------------- /rvo_move/src/rvo_wrapper.cpp: -------------------------------------------------------------------------------- 1 | #include "rvo_wrapper.hpp" 2 | #include 3 | #include 4 | 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | using namespace std; 13 | //================================ Utilities ================================// 14 | 15 | RVO::Vector2 pose_to_rvo(const geometry_msgs::Pose& p) { 16 | return RVO::Vector2(p.position.x, p.position.y); 17 | } 18 | 19 | bool quat_angle(const geometry_msgs::Quaternion& msg, double *angle) { 20 | tf::Quaternion q = tf::Quaternion(msg.x, msg.y, msg.z, msg.w); 21 | if (fabs(q.length2() - 1) > 1e-4) { 22 | ROS_DEBUG_THROTTLE(1.0, "Quaternion not properly normalized"); 23 | return false; 24 | } else { 25 | *angle = tf::getYaw(q); 26 | return true; 27 | } 28 | } 29 | 30 | RVO::Vector2 odom_to_rvo(const nav_msgs::Odometry& odom, 31 | const geometry_msgs::Pose& p) { 32 | double vx = odom.twist.twist.linear.x; 33 | double vw = odom.twist.twist.linear.z; 34 | 35 | double theta; 36 | if (!quat_angle(p.orientation, &theta)) { 37 | ROS_DEBUG_THROTTLE(5.0, "odom_to_rvo()"); 38 | return RVO::Vector2(0.0, 0.0); 39 | } 40 | // Rotate to get velocity in world frame 41 | double xdot = vx * cos(theta) - vw * sin(theta); 42 | double ydot = vx * sin(theta) + vw * cos(theta); 43 | return RVO::Vector2(xdot, ydot); 44 | } 45 | 46 | geometry_msgs::Twist vel_to_twist(const RVO::Vector2 vel) { 47 | geometry_msgs::Twist t; 48 | t.linear.x = vel.x(); 49 | t.angular.z = vel.y(); 50 | return t; 51 | } 52 | 53 | RVO::Vector2 eig_to_rvo(const Eigen::Vector2f& vec) { 54 | return RVO::Vector2(vec(0), vec(1)); 55 | } 56 | 57 | Eigen::Vector2f rvo_to_eig(const RVO::Vector2& vec) { 58 | return Eigen::Vector2f(vec.x(), vec.y()); 59 | } 60 | 61 | //================================= Wrapper =================================// 62 | namespace rvo { 63 | RVOWrapper::RVOWrapper(vector bots, size_t id, OccupancyMap *map, 64 | const vector > &obstacles) : 65 | bots_(bots), map_(map), id_(id), axel_width_(0.23), goal_tol_(0.1), 66 | path_margin_(0.2), timestep_(0.1), los_margin_(0.1), max_speed_(0.3) { 67 | if (id >= bots_.size()) { 68 | stringstream ss; 69 | ss << "RVOWrapper::RVOWrapper(): Bot id=" << id << " >= number of bots=" << bots_.size(); 70 | throw std::runtime_error(ss.str()); 71 | } 72 | 73 | RVO::Vector2 velocity(0.0, 0.0); 74 | float neighborDist = 3.0, timeHorizon = 2.0, timeHorizonObst = 2.0; 75 | float radius = 0.25; 76 | size_t maxNeighbors = 10; 77 | namespace_ = nh_.getNamespace(); 78 | sim_ = new RVO::RVOSimulator(); 79 | sim_->setAgentDefaults(neighborDist, maxNeighbors, timeHorizon, 80 | timeHorizonObst, radius, max_speed_, velocity); 81 | sim_->setTimeStep(timestep_); 82 | 83 | vis_pub_ = nh_.advertise("marker_array", 10, true); 84 | vis_pub_throt_ = nh_.advertise("marker_array_throttle", 10, true); 85 | 86 | // TODO: RVO should be able to use occupancy grid information. 87 | 88 | visualization_msgs::MarkerArray ma; 89 | visualization_msgs::Marker m; 90 | m.action = visualization_msgs::Marker::ADD; 91 | m.type = visualization_msgs::Marker::LINE_STRIP; 92 | m.header.stamp = ros::Time(); 93 | m.header.frame_id = "/map"; 94 | m.id = 1; 95 | m.ns = namespace_ + "/rvo"; 96 | m.pose.orientation.w = 1.0; 97 | m.scale.x = 0.2; 98 | m.color.r = 1.0; 99 | m.color.a = 1.0; 100 | m.points.resize(0); 101 | for (size_t i = 0; i < obstacles.size(); ++i) { 102 | const vector &obs = obstacles[i]; 103 | m.points.clear(); 104 | m.points.resize(obs.size() + 1); 105 | for (size_t j = 0; j < obs.size(); ++j) { 106 | m.points[j].x = obs[j].x(); 107 | m.points[j].y = obs[j].y(); 108 | } 109 | m.points.back().x = obs[0].x(); 110 | m.points.back().y = obs[0].y(); 111 | 112 | sim_->addObstacle(obs); 113 | ma.markers.push_back(m); 114 | m.id++; 115 | } 116 | vis_pub_.publish(ma); 117 | vis_pub_throt_.publish(ma); 118 | sim_->processObstacles(); 119 | 120 | } 121 | 122 | RVOWrapper::~RVOWrapper() { 123 | delete sim_; 124 | } 125 | 126 | RVOWrapper* RVOWrapper::ROSInit(const ros::NodeHandle& nh, OccupancyMap *map, vector bots) { 127 | // Setup simulation 128 | int id; 129 | double neighborDist, timeHorizon, timeHorizonObst, radius, maxSpeed, los_margin; 130 | double axel_width, waypoint_spacing, path_margin, timestep, goal_tol; 131 | int maxNeighbors; 132 | nh.param("neighborDist", neighborDist, 3.0); 133 | nh.param("maxNeighbors", maxNeighbors, 10); 134 | nh.param("timeHorizon", timeHorizon, 2.0); 135 | nh.param("timeHorizonObst", timeHorizonObst, 2.0); 136 | nh.param("radius", radius, 0.05); 137 | nh.param("maxSpeed", maxSpeed, 3.0); 138 | nh.param("id", id, -1); 139 | nh.param("timestep", timestep, 0.1); 140 | nh.param("axel_width", axel_width, 0.23); 141 | nh.param("waypoint_spacing", waypoint_spacing, 0.75); 142 | nh.param("path_margin", path_margin, 0.1); 143 | nh.param("goal_tolerance", goal_tol, 0.1); 144 | nh.param("los_margin", los_margin, 0.1); 145 | 146 | string my_namespace = getenv("ROS_NAMESPACE"); 147 | if (id == -1) { 148 | for (size_t i = 0; i < bots.size(); ++i) { 149 | if (my_namespace == bots[i]->getName()) { 150 | ROS_INFO("My id is %zu", i); 151 | id = i; 152 | break; 153 | } 154 | } 155 | if (id == -1) { 156 | ROS_ERROR("Coudln't find namespace among robots and id param not set"); 157 | ROS_ERROR("my_namespace = %s", my_namespace.c_str()); 158 | ROS_BREAK(); 159 | } 160 | } 161 | vector > obstacles; 162 | if (nh.hasParam("obstacle_file")) { 163 | std::string fname; 164 | nh.getParam("obstacle_file", fname); 165 | ifstream ifs(fname.c_str(), ios::in); 166 | std::string line; 167 | vector obstacle; 168 | 169 | while (getline(ifs, line)) { 170 | if (line == "===") { 171 | if (obstacle.size() > 0) { 172 | ROS_DEBUG("Adding obstacle with %zu vertices", obstacle.size()); 173 | obstacles.push_back(obstacle); 174 | obstacle.clear(); 175 | } 176 | } else { 177 | float x, y; 178 | sscanf(line.c_str(), "%f %f", &x, &y); 179 | obstacle.push_back(RVO::Vector2(x, y)); 180 | } 181 | } 182 | } 183 | if (obstacles.size() == 0) { 184 | ROS_WARN("NO OBSTACLES LOADED"); 185 | } else { 186 | ROS_INFO("Loaded %zu obstacles", obstacles.size()); 187 | } 188 | RVOWrapper *wrapper = new RVOWrapper(bots, id, map, obstacles); 189 | wrapper->setAgentDefaults(neighborDist, maxNeighbors, timeHorizon, 190 | timeHorizonObst, radius, maxSpeed); 191 | wrapper->setTimestep(timestep); 192 | wrapper->setAxelWidth(axel_width); 193 | wrapper->setWaypointSpacing(waypoint_spacing); 194 | wrapper->setPathMargin(path_margin); 195 | wrapper->setGoalTolerance(goal_tol); 196 | wrapper->setMaxSpeed(maxSpeed); 197 | wrapper->addAgents(); 198 | return wrapper; 199 | } 200 | 201 | void RVOWrapper::setAgentDefaults(float neighborDist, size_t maxNeighbors, 202 | float timeHorizon, float timeHorizonObst, 203 | float radius, float maxSpeed) { 204 | sim_->setAgentDefaults(neighborDist, maxNeighbors, timeHorizon, 205 | timeHorizonObst, radius, maxSpeed); 206 | } 207 | 208 | 209 | void RVOWrapper::addAgents() { 210 | for (size_t i = 0; i < bots_.size(); ++i) { 211 | sim_->addAgent(RVO::Vector2(-1000, -1000)); 212 | } 213 | } 214 | 215 | bool RVOWrapper::step() { 216 | // TODO: synchornize ROS clock and sim clock. 217 | sim_->doStep(); 218 | RVO::Vector2 vel = sim_->getAgentVelocity(id_); 219 | // ROS_INFO("%s's actual xy_vel: % 6.2f % 6.2f", 220 | // bots_[id_]->getName().c_str(), vel.x(), vel.y()); 221 | 222 | // ROS_INFO("Sim vel: % 6.2f % 6.2f", vel.x(), vel.y()); 223 | 224 | double theta; 225 | if (!quat_angle(bots_[id_]->getPose().orientation, &theta)) { 226 | ROS_WARN_THROTTLE(5.0, "Problem with quaternion in step()"); 227 | return false; 228 | } 229 | double desired_theta = atan2(vel.y(), vel.x()); 230 | bool at_dest = RVO::abs(sim_->getAgentPosition(id_) - goal_) < goal_tol_; 231 | double vx; 232 | double vw; 233 | 234 | double norm_theta_diff = angles::normalize_angle(desired_theta - theta); 235 | bool straight_forward = fabs(norm_theta_diff) < M_PI / 8.0; 236 | bool straight_backward = fabs(norm_theta_diff) > M_PI - M_PI/16; 237 | if (at_dest) { 238 | // We're here, stop 239 | vx = 0.0; 240 | vw = 0.0; 241 | } else if (!straight_forward && !straight_backward) { 242 | // We're not oriented well, rotate to correct that 243 | vx = 0.0; 244 | vw = copysign(0.6, norm_theta_diff); 245 | } else { 246 | // We're correctly oriented and not at the goal, let RVO do its thing 247 | // 248 | // Solve for control inputs for standard kinematic model with feedback 249 | // linearization; see (eqn 2) in "Smooth and Collision-Free Navigation 250 | // for Multiple Robots Under Differential-Drive Constraints" 251 | double ct = cos(theta); 252 | double st = sin(theta); 253 | double L = sim_->getAgentRadius(id_) / 2.0; 254 | Eigen::Matrix2d m; 255 | m(0, 0) = ct / 2.0 + axel_width_ * st / L; 256 | m(0, 1) = ct / 2.0 - axel_width_ * st / L; 257 | m(1, 0) = st / 2.0 - axel_width_ * ct / L; 258 | m(1, 1) = st / 2.0 + axel_width_ * ct / L; 259 | 260 | Eigen::Vector2d v(vel.x(), vel.y()); 261 | Eigen::Vector2d u = m.inverse() * v; 262 | // ROS_DEBUG("L: % 6.2f", L); 263 | // ROS_DEBUG_STREAM("m: " << m); 264 | // ROS_DEBUG_STREAM("m_inv: " << m.inverse()); 265 | // ROS_DEBUG_STREAM("vel: " << setprecision(2) << v.transpose()); 266 | // ROS_DEBUG_STREAM("wheel vel: " << setprecision(2) << u.transpose()); 267 | 268 | vx = (u(0) + u(1)) / 2.0; 269 | vw = (u(1) - u(0)) / L; 270 | vx = min(max(vx, -max_speed_), max_speed_); 271 | vw = min(max(vw, -max_speed_), max_speed_); 272 | } 273 | 274 | bots_[id_]->pubVel(vx, vw); 275 | // ROS_DEBUG("Theta: % 6.2f", theta); 276 | // ROS_DEBUG("sim pose: % 6.2f % 6.2f", sim_->getAgentPosition(id_).x(), 277 | // sim_->getAgentPosition(id_).y()); 278 | // ROS_DEBUG("command: % 6.2f % 6.2f\n", vx, vw); 279 | 280 | return at_dest; 281 | } 282 | 283 | std::vector RVOWrapper::setGoal(const geometry_msgs::Pose& p) { 284 | RVO::Vector2 start = pose_to_rvo(bots_[id_]->getPose()); 285 | goal_ = pose_to_rvo(p); 286 | waypoints_.clear(); 287 | 288 | PointVector path; 289 | double margin = path_margin_; 290 | while (true) { 291 | path = map_->astar(start.x(), start.y(), goal_.x(), goal_.y(), margin); 292 | if (path.size() != 0 || margin < 0.05) { 293 | break; 294 | } else { 295 | ROS_DEBUG("Shrinking path"); 296 | margin *= 0.9; 297 | } 298 | } 299 | 300 | if (path.size() != 0) { 301 | waypoints_.push_back(eig_to_rvo(path[0])); 302 | for (size_t i = 0; i < path.size() - 1; ++i) { 303 | const RVO::Vector2& prev = waypoints_.back(); 304 | const RVO::Vector2& curr = eig_to_rvo(path[i]); 305 | if (RVO::abs(prev - curr) > way_spacing_) { 306 | waypoints_.push_back(eig_to_rvo(path[i-1])); 307 | } 308 | } 309 | waypoints_.push_back(eig_to_rvo(path.back())); 310 | ROS_DEBUG("Waypoint path has %zu vertices", waypoints_.size()); 311 | } 312 | 313 | std::vector ros_path; 314 | ros_path.resize(waypoints_.size()); 315 | for (size_t i = 0; i < waypoints_.size(); ++i) { 316 | tf::poseTFToMsg(tf::Pose(tf::createIdentityQuaternion(), 317 | tf::Vector3(waypoints_[i].x(), waypoints_[i].y(), 0)), 318 | ros_path[i]); 319 | } 320 | return ros_path; 321 | } 322 | 323 | bool RVOWrapper::getLeadGoal(RVO::Vector2 *goal) { 324 | // Direct robot towards successor of point that it is closest to 325 | float min_dist = std::numeric_limits::infinity(); 326 | int min_ind = -1; 327 | // Get closest visible waypoint 328 | RVO::Vector2 pos = sim_->getAgentPosition(id_); 329 | for (size_t wayind = 0; wayind < waypoints_.size(); ++wayind) { 330 | if (!sim_->queryVisibility(pos, waypoints_[wayind], los_margin_)) { 331 | continue; 332 | } 333 | float dist = RVO::absSq(pos - waypoints_[wayind]); 334 | if (dist < min_dist) { 335 | min_dist = dist; 336 | min_ind = wayind; 337 | } 338 | } 339 | 340 | if (min_ind == -1) { 341 | ROS_WARN("No waypoints are visible!"); 342 | *goal = RVO::Vector2(); 343 | return false; 344 | } else { 345 | int orig_min = min_ind; 346 | // Find waypoint furthest along the path that robot can see 347 | while (static_cast(min_ind + 1) < waypoints_.size() && 348 | sim_->queryVisibility(pos, waypoints_[min_ind+1], los_margin_) && 349 | sqrt(RVO::absSq(waypoints_[orig_min] - waypoints_[min_ind+1])) < 1.0) { 350 | ++min_ind; 351 | } 352 | if (!sim_->queryVisibility(pos, waypoints_[min_ind])) { 353 | ROS_WARN("RVOWrapper::getLeadGoal() %s's next waypoint is not visible", 354 | bots_[id_]->getName().c_str()); 355 | } 356 | 357 | *goal = RVO::normalize(waypoints_[min_ind] - pos); 358 | // if (RVO::absSq(*goal) > 1.0f) { 359 | // *goal = RVO::normalize(*goal); 360 | // } 361 | 362 | visualization_msgs::MarkerArray ma; 363 | ma.markers.resize(1); 364 | visualization_msgs::Marker &m = ma.markers.at(0); 365 | m.header.stamp = ros::Time(); 366 | m.header.frame_id = "/map"; 367 | m.action = visualization_msgs::Marker::ADD; 368 | m.type = visualization_msgs::Marker::SPHERE; 369 | m.id = 100; 370 | m.ns = namespace_ + "/rvo"; 371 | m.pose.orientation.w = 1.0; 372 | m.scale.x = 0.1; 373 | m.scale.y = 0.1; 374 | m.scale.z = 0.1; 375 | m.color.a = 1.0; 376 | m.color.r = 1.0; 377 | m.pose.position.x = waypoints_[min_ind].x(); 378 | m.pose.position.y = waypoints_[min_ind].y(); 379 | vis_pub_.publish(ma); 380 | 381 | // ROS_DEBUG("Advancing towards goal"); 382 | // ROS_DEBUG("occ dist: % 6.2f", 383 | // map_get_cell(map_, pos.x(), pos.y(), 0.0)->occ_dist); 384 | // ROS_DEBUG("min_ind: %i", min_ind); 385 | // ROS_DEBUG("position: % 6.2f % 6.2f", pos.x(), pos.y()); 386 | // ROS_DEBUG("waypoint: % 6.2f % 6.2f", 387 | // waypoints_[min_ind].x(), waypoints_[min_ind].y()); 388 | // ROS_DEBUG("%s's actual goalVec: % 6.2f % 6.2f", 389 | // bots_[id_]->getName().c_str(), goal->x(), goal->y()); 390 | // ROS_DEBUG("Num obs: %zu", sim_->getAgentNumObstacleNeighbors(id_)); 391 | return true; 392 | } 393 | } 394 | 395 | bool RVOWrapper::syncState() { 396 | // Update RVO simulator with latest position & velocitys from ROS 397 | bool have_everything = true; 398 | ros::Duration d(60.0); 399 | 400 | for (size_t i = 0; i < sim_->getNumAgents(); ++i) { 401 | BotClient *bot = bots_[i]; 402 | if (bot->havePose(d)) { 403 | RVO::Vector2 position = pose_to_rvo(bot->getPose()); 404 | sim_->setAgentPosition(i, position); 405 | } else { 406 | ROS_WARN_THROTTLE(5.0, "No pose info for %s (id: %zu)", 407 | bot->getName().c_str(), i); 408 | have_everything = false; 409 | } 410 | 411 | if (bot->haveOdom(d)) { 412 | RVO::Vector2 vel = odom_to_rvo(bot->getOdom(), bot->getPose()); 413 | sim_->setAgentVelocity(i, vel); 414 | } else { 415 | ROS_WARN_THROTTLE(5.0, "No odom info for %s (id: %zu)", 416 | bot->getName().c_str(), i); 417 | have_everything = false; 418 | } 419 | } 420 | 421 | return have_everything; 422 | } 423 | 424 | bool RVOWrapper::setVelocities() { 425 | // Set preferred velocities 426 | bool ok = true; 427 | for (size_t i = 0; i < sim_->getNumAgents(); ++i) { 428 | if (i == id_) { 429 | RVO::Vector2 goalVector; 430 | ok = getLeadGoal(&goalVector); 431 | if (!ok) { 432 | ROS_WARN("%s problem getting goal", bots_[id_]->getName().c_str()); 433 | } else { 434 | // Perturb a little to avoid deadlocks due to perfect symmetry. 435 | float angle = std::rand() * 2.0f * M_PI / RAND_MAX; 436 | float dist = std::rand() * 0.0001f / RAND_MAX; 437 | sim_->setAgentPrefVelocity(i, goalVector + 438 | dist * RVO::Vector2(std::cos(angle), std::sin(angle))); 439 | } 440 | } else { 441 | BotClient *bot = bots_[i]; 442 | RVO::Vector2 goal = odom_to_rvo(bot->getOdom(), bot->getPose()); 443 | // ROS_DEBUG("%s thinks %s's xy_vel: % 6.2f % 6.2f", bots_[id_]->getName().c_str(), 444 | // bot->getName().c_str(), goal.x(), goal.y()); 445 | sim_->setAgentPrefVelocity(i, goal); 446 | } 447 | } 448 | return ok; 449 | } 450 | 451 | //============================== MoveServer ===============================// 452 | 453 | MoveServer::MoveServer(const std::string &server_name) : 454 | pnh_("~"), as_(nh_, server_name, boost::bind(&MoveServer::executeCB, this, _1), false), 455 | action_name_(ros::names::resolve(server_name)) { 456 | 457 | ROS_INFO("Setting up action server '%s'", action_name_.c_str()); 458 | // Get the map 459 | OccupancyMap *map = OccupancyMap::FromMapServer("/static_map"); 460 | map->updateCSpace(1.0); 461 | 462 | // Get bots 463 | namespace_ = nh_.getNamespace(); 464 | bots_ = BotClient::MakeBots(pnh_); 465 | ROS_INFO("Listening to %zu bots", bots_.size()); 466 | for (size_t i = 0; i < bots_.size(); ++i) { 467 | ROS_INFO(" Bot %zu: %s", i, bots_[i]->getName().c_str()); 468 | } 469 | 470 | wrapper_ = RVOWrapper::ROSInit(pnh_, map, bots_); 471 | pnh_.param("timestep", timestep_, 0.1); 472 | wrapper_->setTimestep(timestep_); 473 | 474 | path_pub_ = nh_.advertise("path", 5, true); 475 | pnh_.param("map_frame_id", tf_frame_, std::string("/map")); 476 | nav_msgs::Path path; 477 | path.header.stamp = ros::Time::now(); 478 | path.header.frame_id = tf_frame_; 479 | path.poses.resize(0); 480 | path_pub_.publish(path); 481 | ROS_INFO("Done setting up %s", action_name_.c_str()); 482 | } 483 | 484 | MoveServer::~MoveServer() { 485 | delete wrapper_; 486 | for (size_t i = 0; i < bots_.size(); ++i) { 487 | delete bots_[i]; 488 | } 489 | } 490 | 491 | void MoveServer::executeCB(const rvo_move::MoveGoalConstPtr &goal) { 492 | std::string prefix = action_name_; 493 | const char *pref = prefix.c_str(); 494 | ROS_INFO("%s got request: (%.2f, %.2f)", pref, 495 | goal->target_pose.pose.position.x, 496 | goal->target_pose.pose.position.y); 497 | 498 | std::vector path = wrapper_->setGoal(goal->target_pose.pose); 499 | // Publish the path 500 | nav_msgs::Path nav_path; 501 | nav_path.header.stamp = ros::Time::now(); 502 | nav_path.header.frame_id = tf_frame_; 503 | nav_path.poses.resize(path.size()); 504 | for (size_t i = 0; i < path.size(); ++i) { 505 | nav_path.poses[i].header.stamp = ros::Time::now(); 506 | nav_path.poses[i].header.frame_id = tf_frame_; 507 | nav_path.poses[i].pose = path[i]; 508 | } 509 | path_pub_.publish(nav_path); 510 | 511 | if (path.size() == 0) { 512 | ROS_WARN("No path found"); 513 | bots_[wrapper_->getID()]->pubVel(0.0, 0.0); 514 | as_.setAborted(); 515 | return; 516 | } 517 | for (ros::Rate rate(1.0 / timestep_); ; rate.sleep()) { 518 | if (!ros::ok()) { 519 | ROS_INFO("%s Ros shutdown", action_name_.c_str()); 520 | as_.setPreempted(); 521 | break; 522 | } 523 | 524 | if (as_.isPreemptRequested()) { 525 | ROS_INFO("%s Preempted", action_name_.c_str()); 526 | // set the action state to preempted 527 | as_.setPreempted(); 528 | break; 529 | } 530 | 531 | if (!wrapper_->syncState()) { 532 | ROS_WARN_THROTTLE(5.0, "%s Problem synchronizing state", pref); 533 | } 534 | 535 | if (!wrapper_->setVelocities()) { 536 | ROS_WARN("%s Problem while setting agent velocities", pref); 537 | as_.setAborted(); 538 | break; 539 | } 540 | 541 | if (wrapper_->step()) { 542 | ROS_INFO("%s Reached destination", pref); 543 | as_.setSucceeded(); 544 | break; 545 | } 546 | } 547 | nav_path.poses.resize(0); 548 | path_pub_.publish(nav_path); 549 | if (!as_.isNewGoalAvailable()) { 550 | bots_[wrapper_->getID()]->pubVel(0.0, 0.0); 551 | } 552 | } 553 | 554 | void MoveServer::start() { 555 | as_.start(); 556 | } 557 | } 558 | -------------------------------------------------------------------------------- /rvo2/src/Agent.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Agent.cpp 3 | * RVO2 Library 4 | * 5 | * Copyright (c) 2008-2010 University of North Carolina at Chapel Hill. 6 | * All rights reserved. 7 | * 8 | * Permission to use, copy, modify, and distribute this software and its 9 | * documentation for educational, research, and non-profit purposes, without 10 | * fee, and without a written agreement is hereby granted, provided that the 11 | * above copyright notice, this paragraph, and the following four paragraphs 12 | * appear in all copies. 13 | * 14 | * Permission to incorporate this software into commercial products may be 15 | * obtained by contacting the authors or the Office of 16 | * Technology Development at the University of North Carolina at Chapel Hill 17 | * . 18 | * 19 | * This software program and documentation are copyrighted by the University of 20 | * North Carolina at Chapel Hill. The software program and documentation are 21 | * supplied "as is," without any accompanying services from the University of 22 | * North Carolina at Chapel Hill or the authors. The University of North 23 | * Carolina at Chapel Hill and the authors do not warrant that the operation of 24 | * the program will be uninterrupted or error-free. The end-user understands 25 | * that the program was developed for research purposes and is advised not to 26 | * rely exclusively on the program for any reason. 27 | * 28 | * IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE 29 | * AUTHORS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR 30 | * CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS 31 | * SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF NORTH CAROLINA AT 32 | * CHAPEL HILL OR THE AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH 33 | * DAMAGE. 34 | * 35 | * THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS SPECIFICALLY 36 | * DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 37 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AND ANY 38 | * STATUTORY WARRANTY OF NON-INFRINGEMENT. THE SOFTWARE PROVIDED HEREUNDER IS ON 39 | * AN "AS IS" BASIS, AND THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE 40 | * AUTHORS HAVE NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, 41 | * ENHANCEMENTS, OR MODIFICATIONS. 42 | * 43 | * Please send all bug reports to . 44 | * 45 | * The authors may be contacted via: 46 | * 47 | * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha 48 | * Dept. of Computer Science 49 | * 201 S. Columbia St. 50 | * Frederick P. Brooks, Jr. Computer Science Bldg. 51 | * Chapel Hill, N.C. 27599-3175 52 | * United States of America 53 | * 54 | * 55 | */ 56 | 57 | #include "Agent.h" 58 | 59 | #include "KdTree.h" 60 | #include "Obstacle.h" 61 | 62 | namespace RVO { 63 | Agent::Agent(RVOSimulator *sim) : maxNeighbors_(0), maxSpeed_(0.0f), neighborDist_(0.0f), radius_(0.0f), sim_(sim), timeHorizon_(0.0f), timeHorizonObst_(0.0f), id_(0) { } 64 | 65 | void Agent::computeNeighbors() 66 | { 67 | obstacleNeighbors_.clear(); 68 | float rangeSq = sqr(timeHorizonObst_ * maxSpeed_ + radius_); 69 | sim_->kdTree_->computeObstacleNeighbors(this, rangeSq); 70 | 71 | agentNeighbors_.clear(); 72 | 73 | if (maxNeighbors_ > 0) { 74 | rangeSq = sqr(neighborDist_); 75 | sim_->kdTree_->computeAgentNeighbors(this, rangeSq); 76 | } 77 | } 78 | 79 | /* Search for the best new velocity. */ 80 | void Agent::computeNewVelocity() 81 | { 82 | orcaLines_.clear(); 83 | 84 | const float invTimeHorizonObst = 1.0f / timeHorizonObst_; 85 | 86 | /* Create obstacle ORCA lines. */ 87 | for (size_t i = 0; i < obstacleNeighbors_.size(); ++i) { 88 | 89 | const Obstacle *obstacle1 = obstacleNeighbors_[i].second; 90 | const Obstacle *obstacle2 = obstacle1->nextObstacle_; 91 | 92 | const Vector2 relativePosition1 = obstacle1->point_ - position_; 93 | const Vector2 relativePosition2 = obstacle2->point_ - position_; 94 | 95 | /* 96 | * Check if velocity obstacle of obstacle is already taken care of by 97 | * previously constructed obstacle ORCA lines. 98 | */ 99 | bool alreadyCovered = false; 100 | 101 | for (size_t j = 0; j < orcaLines_.size(); ++j) { 102 | if (det(invTimeHorizonObst * relativePosition1 - orcaLines_[j].point, orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVO_EPSILON && det(invTimeHorizonObst * relativePosition2 - orcaLines_[j].point, orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVO_EPSILON) { 103 | alreadyCovered = true; 104 | break; 105 | } 106 | } 107 | 108 | if (alreadyCovered) { 109 | continue; 110 | } 111 | 112 | /* Not yet covered. Check for collisions. */ 113 | 114 | const float distSq1 = absSq(relativePosition1); 115 | const float distSq2 = absSq(relativePosition2); 116 | 117 | const float radiusSq = sqr(radius_); 118 | 119 | const Vector2 obstacleVector = obstacle2->point_ - obstacle1->point_; 120 | const float s = (-relativePosition1 * obstacleVector) / absSq(obstacleVector); 121 | const float distSqLine = absSq(-relativePosition1 - s * obstacleVector); 122 | 123 | Line line; 124 | 125 | if (s < 0.0f && distSq1 <= radiusSq) { 126 | /* Collision with left vertex. Ignore if non-convex. */ 127 | if (obstacle1->isConvex_) { 128 | line.point = Vector2(0.0f, 0.0f); 129 | line.direction = normalize(Vector2(-relativePosition1.y(), relativePosition1.x())); 130 | orcaLines_.push_back(line); 131 | } 132 | 133 | continue; 134 | } 135 | else if (s > 1.0f && distSq2 <= radiusSq) { 136 | /* Collision with right vertex. Ignore if non-convex 137 | * or if it will be taken care of by neighoring obstace */ 138 | if (obstacle2->isConvex_ && det(relativePosition2, obstacle2->unitDir_) >= 0.0f) { 139 | line.point = Vector2(0.0f, 0.0f); 140 | line.direction = normalize(Vector2(-relativePosition2.y(), relativePosition2.x())); 141 | orcaLines_.push_back(line); 142 | } 143 | 144 | continue; 145 | } 146 | else if (s >= 0.0f && s < 1.0f && distSqLine <= radiusSq) { 147 | /* Collision with obstacle segment. */ 148 | line.point = Vector2(0.0f, 0.0f); 149 | line.direction = -obstacle1->unitDir_; 150 | orcaLines_.push_back(line); 151 | continue; 152 | } 153 | 154 | /* 155 | * No collision. 156 | * Compute legs. When obliquely viewed, both legs can come from a single 157 | * vertex. Legs extend cut-off line when nonconvex vertex. 158 | */ 159 | 160 | Vector2 leftLegDirection, rightLegDirection; 161 | 162 | if (s < 0.0f && distSqLine <= radiusSq) { 163 | /* 164 | * Obstacle viewed obliquely so that left vertex 165 | * defines velocity obstacle. 166 | */ 167 | if (!obstacle1->isConvex_) { 168 | /* Ignore obstacle. */ 169 | continue; 170 | } 171 | 172 | obstacle2 = obstacle1; 173 | 174 | const float leg1 = std::sqrt(distSq1 - radiusSq); 175 | leftLegDirection = Vector2(relativePosition1.x() * leg1 - relativePosition1.y() * radius_, relativePosition1.x() * radius_ + relativePosition1.y() * leg1) / distSq1; 176 | rightLegDirection = Vector2(relativePosition1.x() * leg1 + relativePosition1.y() * radius_, -relativePosition1.x() * radius_ + relativePosition1.y() * leg1) / distSq1; 177 | } 178 | else if (s > 1.0f && distSqLine <= radiusSq) { 179 | /* 180 | * Obstacle viewed obliquely so that 181 | * right vertex defines velocity obstacle. 182 | */ 183 | if (!obstacle2->isConvex_) { 184 | /* Ignore obstacle. */ 185 | continue; 186 | } 187 | 188 | obstacle1 = obstacle2; 189 | 190 | const float leg2 = std::sqrt(distSq2 - radiusSq); 191 | leftLegDirection = Vector2(relativePosition2.x() * leg2 - relativePosition2.y() * radius_, relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2; 192 | rightLegDirection = Vector2(relativePosition2.x() * leg2 + relativePosition2.y() * radius_, -relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2; 193 | } 194 | else { 195 | /* Usual situation. */ 196 | if (obstacle1->isConvex_) { 197 | const float leg1 = std::sqrt(distSq1 - radiusSq); 198 | leftLegDirection = Vector2(relativePosition1.x() * leg1 - relativePosition1.y() * radius_, relativePosition1.x() * radius_ + relativePosition1.y() * leg1) / distSq1; 199 | } 200 | else { 201 | /* Left vertex non-convex; left leg extends cut-off line. */ 202 | leftLegDirection = -obstacle1->unitDir_; 203 | } 204 | 205 | if (obstacle2->isConvex_) { 206 | const float leg2 = std::sqrt(distSq2 - radiusSq); 207 | rightLegDirection = Vector2(relativePosition2.x() * leg2 + relativePosition2.y() * radius_, -relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2; 208 | } 209 | else { 210 | /* Right vertex non-convex; right leg extends cut-off line. */ 211 | rightLegDirection = obstacle1->unitDir_; 212 | } 213 | } 214 | 215 | /* 216 | * Legs can never point into neighboring edge when convex vertex, 217 | * take cutoff-line of neighboring edge instead. If velocity projected on 218 | * "foreign" leg, no constraint is added. 219 | */ 220 | 221 | const Obstacle *const leftNeighbor = obstacle1->prevObstacle_; 222 | 223 | bool isLeftLegForeign = false; 224 | bool isRightLegForeign = false; 225 | 226 | if (obstacle1->isConvex_ && det(leftLegDirection, -leftNeighbor->unitDir_) >= 0.0f) { 227 | /* Left leg points into obstacle. */ 228 | leftLegDirection = -leftNeighbor->unitDir_; 229 | isLeftLegForeign = true; 230 | } 231 | 232 | if (obstacle2->isConvex_ && det(rightLegDirection, obstacle2->unitDir_) <= 0.0f) { 233 | /* Right leg points into obstacle. */ 234 | rightLegDirection = obstacle2->unitDir_; 235 | isRightLegForeign = true; 236 | } 237 | 238 | /* Compute cut-off centers. */ 239 | const Vector2 leftCutoff = invTimeHorizonObst * (obstacle1->point_ - position_); 240 | const Vector2 rightCutoff = invTimeHorizonObst * (obstacle2->point_ - position_); 241 | const Vector2 cutoffVec = rightCutoff - leftCutoff; 242 | 243 | /* Project current velocity on velocity obstacle. */ 244 | 245 | /* Check if current velocity is projected on cutoff circles. */ 246 | const float t = (obstacle1 == obstacle2 ? 0.5f : ((velocity_ - leftCutoff) * cutoffVec) / absSq(cutoffVec)); 247 | const float tLeft = ((velocity_ - leftCutoff) * leftLegDirection); 248 | const float tRight = ((velocity_ - rightCutoff) * rightLegDirection); 249 | 250 | if ((t < 0.0f && tLeft < 0.0f) || (obstacle1 == obstacle2 && tLeft < 0.0f && tRight < 0.0f)) { 251 | /* Project on left cut-off circle. */ 252 | const Vector2 unitW = normalize(velocity_ - leftCutoff); 253 | 254 | line.direction = Vector2(unitW.y(), -unitW.x()); 255 | line.point = leftCutoff + radius_ * invTimeHorizonObst * unitW; 256 | orcaLines_.push_back(line); 257 | continue; 258 | } 259 | else if (t > 1.0f && tRight < 0.0f) { 260 | /* Project on right cut-off circle. */ 261 | const Vector2 unitW = normalize(velocity_ - rightCutoff); 262 | 263 | line.direction = Vector2(unitW.y(), -unitW.x()); 264 | line.point = rightCutoff + radius_ * invTimeHorizonObst * unitW; 265 | orcaLines_.push_back(line); 266 | continue; 267 | } 268 | 269 | /* 270 | * Project on left leg, right leg, or cut-off line, whichever is closest 271 | * to velocity. 272 | */ 273 | const float distSqCutoff = ((t < 0.0f || t > 1.0f || obstacle1 == obstacle2) ? std::numeric_limits::infinity() : absSq(velocity_ - (leftCutoff + t * cutoffVec))); 274 | const float distSqLeft = ((tLeft < 0.0f) ? std::numeric_limits::infinity() : absSq(velocity_ - (leftCutoff + tLeft * leftLegDirection))); 275 | const float distSqRight = ((tRight < 0.0f) ? std::numeric_limits::infinity() : absSq(velocity_ - (rightCutoff + tRight * rightLegDirection))); 276 | 277 | if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight) { 278 | /* Project on cut-off line. */ 279 | line.direction = -obstacle1->unitDir_; 280 | line.point = leftCutoff + radius_ * invTimeHorizonObst * Vector2(-line.direction.y(), line.direction.x()); 281 | orcaLines_.push_back(line); 282 | continue; 283 | } 284 | else if (distSqLeft <= distSqRight) { 285 | /* Project on left leg. */ 286 | if (isLeftLegForeign) { 287 | continue; 288 | } 289 | 290 | line.direction = leftLegDirection; 291 | line.point = leftCutoff + radius_ * invTimeHorizonObst * Vector2(-line.direction.y(), line.direction.x()); 292 | orcaLines_.push_back(line); 293 | continue; 294 | } 295 | else { 296 | /* Project on right leg. */ 297 | if (isRightLegForeign) { 298 | continue; 299 | } 300 | 301 | line.direction = -rightLegDirection; 302 | line.point = rightCutoff + radius_ * invTimeHorizonObst * Vector2(-line.direction.y(), line.direction.x()); 303 | orcaLines_.push_back(line); 304 | continue; 305 | } 306 | } 307 | 308 | const size_t numObstLines = orcaLines_.size(); 309 | 310 | const float invTimeHorizon = 1.0f / timeHorizon_; 311 | 312 | /* Create agent ORCA lines. */ 313 | for (size_t i = 0; i < agentNeighbors_.size(); ++i) { 314 | const Agent *const other = agentNeighbors_[i].second; 315 | 316 | const Vector2 relativePosition = other->position_ - position_; 317 | const Vector2 relativeVelocity = velocity_ - other->velocity_; 318 | const float distSq = absSq(relativePosition); 319 | const float combinedRadius = radius_ + other->radius_; 320 | const float combinedRadiusSq = sqr(combinedRadius); 321 | 322 | Line line; 323 | Vector2 u; 324 | 325 | if (distSq > combinedRadiusSq) { 326 | /* No collision. */ 327 | const Vector2 w = relativeVelocity - invTimeHorizon * relativePosition; 328 | /* Vector from cutoff center to relative velocity. */ 329 | const float wLengthSq = absSq(w); 330 | 331 | const float dotProduct1 = w * relativePosition; 332 | 333 | if (dotProduct1 < 0.0f && sqr(dotProduct1) > combinedRadiusSq * wLengthSq) { 334 | /* Project on cut-off circle. */ 335 | const float wLength = std::sqrt(wLengthSq); 336 | const Vector2 unitW = w / wLength; 337 | 338 | line.direction = Vector2(unitW.y(), -unitW.x()); 339 | u = (combinedRadius * invTimeHorizon - wLength) * unitW; 340 | } 341 | else { 342 | /* Project on legs. */ 343 | const float leg = std::sqrt(distSq - combinedRadiusSq); 344 | 345 | if (det(relativePosition, w) > 0.0f) { 346 | /* Project on left leg. */ 347 | line.direction = Vector2(relativePosition.x() * leg - relativePosition.y() * combinedRadius, relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq; 348 | } 349 | else { 350 | /* Project on right leg. */ 351 | line.direction = -Vector2(relativePosition.x() * leg + relativePosition.y() * combinedRadius, -relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq; 352 | } 353 | 354 | const float dotProduct2 = relativeVelocity * line.direction; 355 | 356 | u = dotProduct2 * line.direction - relativeVelocity; 357 | } 358 | } 359 | else { 360 | /* Collision. Project on cut-off circle of time timeStep. */ 361 | const float invTimeStep = 1.0f / sim_->timeStep_; 362 | 363 | /* Vector from cutoff center to relative velocity. */ 364 | const Vector2 w = relativeVelocity - invTimeStep * relativePosition; 365 | 366 | const float wLength = abs(w); 367 | const Vector2 unitW = w / wLength; 368 | 369 | line.direction = Vector2(unitW.y(), -unitW.x()); 370 | u = (combinedRadius * invTimeStep - wLength) * unitW; 371 | } 372 | 373 | line.point = velocity_ + 0.5f * u; 374 | orcaLines_.push_back(line); 375 | } 376 | 377 | size_t lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, newVelocity_); 378 | 379 | if (lineFail < orcaLines_.size()) { 380 | linearProgram3(orcaLines_, numObstLines, lineFail, maxSpeed_, newVelocity_); 381 | } 382 | } 383 | 384 | void Agent::insertAgentNeighbor(const Agent *agent, float &rangeSq) 385 | { 386 | if (this != agent) { 387 | const float distSq = absSq(position_ - agent->position_); 388 | 389 | if (distSq < rangeSq) { 390 | if (agentNeighbors_.size() < maxNeighbors_) { 391 | agentNeighbors_.push_back(std::make_pair(distSq, agent)); 392 | } 393 | 394 | size_t i = agentNeighbors_.size() - 1; 395 | 396 | while (i != 0 && distSq < agentNeighbors_[i - 1].first) { 397 | agentNeighbors_[i] = agentNeighbors_[i - 1]; 398 | --i; 399 | } 400 | 401 | agentNeighbors_[i] = std::make_pair(distSq, agent); 402 | 403 | if (agentNeighbors_.size() == maxNeighbors_) { 404 | rangeSq = agentNeighbors_.back().first; 405 | } 406 | } 407 | } 408 | } 409 | 410 | void Agent::insertObstacleNeighbor(const Obstacle *obstacle, float rangeSq) 411 | { 412 | const Obstacle *const nextObstacle = obstacle->nextObstacle_; 413 | 414 | const float distSq = distSqPointLineSegment(obstacle->point_, nextObstacle->point_, position_); 415 | 416 | if (distSq < rangeSq) { 417 | obstacleNeighbors_.push_back(std::make_pair(distSq, obstacle)); 418 | 419 | size_t i = obstacleNeighbors_.size() - 1; 420 | 421 | while (i != 0 && distSq < obstacleNeighbors_[i - 1].first) { 422 | obstacleNeighbors_[i] = obstacleNeighbors_[i - 1]; 423 | --i; 424 | } 425 | 426 | obstacleNeighbors_[i] = std::make_pair(distSq, obstacle); 427 | } 428 | } 429 | 430 | void Agent::update() 431 | { 432 | velocity_ = newVelocity_; 433 | position_ += velocity_ * sim_->timeStep_; 434 | } 435 | 436 | bool linearProgram1(const std::vector &lines, size_t lineNo, float radius, const Vector2 &optVelocity, bool directionOpt, Vector2 &result) 437 | { 438 | const float dotProduct = lines[lineNo].point * lines[lineNo].direction; 439 | const float discriminant = sqr(dotProduct) + sqr(radius) - absSq(lines[lineNo].point); 440 | 441 | if (discriminant < 0.0f) { 442 | /* Max speed circle fully invalidates line lineNo. */ 443 | return false; 444 | } 445 | 446 | const float sqrtDiscriminant = std::sqrt(discriminant); 447 | float tLeft = -dotProduct - sqrtDiscriminant; 448 | float tRight = -dotProduct + sqrtDiscriminant; 449 | 450 | for (size_t i = 0; i < lineNo; ++i) { 451 | const float denominator = det(lines[lineNo].direction, lines[i].direction); 452 | const float numerator = det(lines[i].direction, lines[lineNo].point - lines[i].point); 453 | 454 | if (std::fabs(denominator) <= RVO_EPSILON) { 455 | /* Lines lineNo and i are (almost) parallel. */ 456 | if (numerator < 0.0f) { 457 | return false; 458 | } 459 | else { 460 | continue; 461 | } 462 | } 463 | 464 | const float t = numerator / denominator; 465 | 466 | if (denominator >= 0.0f) { 467 | /* Line i bounds line lineNo on the right. */ 468 | tRight = std::min(tRight, t); 469 | } 470 | else { 471 | /* Line i bounds line lineNo on the left. */ 472 | tLeft = std::max(tLeft, t); 473 | } 474 | 475 | if (tLeft > tRight) { 476 | return false; 477 | } 478 | } 479 | 480 | if (directionOpt) { 481 | /* Optimize direction. */ 482 | if (optVelocity * lines[lineNo].direction > 0.0f) { 483 | /* Take right extreme. */ 484 | result = lines[lineNo].point + tRight * lines[lineNo].direction; 485 | } 486 | else { 487 | /* Take left extreme. */ 488 | result = lines[lineNo].point + tLeft * lines[lineNo].direction; 489 | } 490 | } 491 | else { 492 | /* Optimize closest point. */ 493 | const float t = lines[lineNo].direction * (optVelocity - lines[lineNo].point); 494 | 495 | if (t < tLeft) { 496 | result = lines[lineNo].point + tLeft * lines[lineNo].direction; 497 | } 498 | else if (t > tRight) { 499 | result = lines[lineNo].point + tRight * lines[lineNo].direction; 500 | } 501 | else { 502 | result = lines[lineNo].point + t * lines[lineNo].direction; 503 | } 504 | } 505 | 506 | return true; 507 | } 508 | 509 | size_t linearProgram2(const std::vector &lines, float radius, const Vector2 &optVelocity, bool directionOpt, Vector2 &result) 510 | { 511 | if (directionOpt) { 512 | /* 513 | * Optimize direction. Note that the optimization velocity is of unit 514 | * length in this case. 515 | */ 516 | result = optVelocity * radius; 517 | } 518 | else if (absSq(optVelocity) > sqr(radius)) { 519 | /* Optimize closest point and outside circle. */ 520 | result = normalize(optVelocity) * radius; 521 | } 522 | else { 523 | /* Optimize closest point and inside circle. */ 524 | result = optVelocity; 525 | } 526 | 527 | for (size_t i = 0; i < lines.size(); ++i) { 528 | if (det(lines[i].direction, lines[i].point - result) > 0.0f) { 529 | /* Result does not satisfy constraint i. Compute new optimal result. */ 530 | const Vector2 tempResult = result; 531 | 532 | if (!linearProgram1(lines, i, radius, optVelocity, directionOpt, result)) { 533 | result = tempResult; 534 | return i; 535 | } 536 | } 537 | } 538 | 539 | return lines.size(); 540 | } 541 | 542 | void linearProgram3(const std::vector &lines, size_t numObstLines, size_t beginLine, float radius, Vector2 &result) 543 | { 544 | float distance = 0.0f; 545 | 546 | for (size_t i = beginLine; i < lines.size(); ++i) { 547 | if (det(lines[i].direction, lines[i].point - result) > distance) { 548 | /* Result does not satisfy constraint of line i. */ 549 | std::vector projLines(lines.begin(), lines.begin() + static_cast(numObstLines)); 550 | 551 | for (size_t j = numObstLines; j < i; ++j) { 552 | Line line; 553 | 554 | float determinant = det(lines[i].direction, lines[j].direction); 555 | 556 | if (std::fabs(determinant) <= RVO_EPSILON) { 557 | /* Line i and line j are parallel. */ 558 | if (lines[i].direction * lines[j].direction > 0.0f) { 559 | /* Line i and line j point in the same direction. */ 560 | continue; 561 | } 562 | else { 563 | /* Line i and line j point in opposite direction. */ 564 | line.point = 0.5f * (lines[i].point + lines[j].point); 565 | } 566 | } 567 | else { 568 | line.point = lines[i].point + (det(lines[j].direction, lines[i].point - lines[j].point) / determinant) * lines[i].direction; 569 | } 570 | 571 | line.direction = normalize(lines[j].direction - lines[i].direction); 572 | projLines.push_back(line); 573 | } 574 | 575 | const Vector2 tempResult = result; 576 | 577 | if (linearProgram2(projLines, radius, Vector2(-lines[i].direction.y(), lines[i].direction.x()), true, result) < projLines.size()) { 578 | /* This should in principle not happen. The result is by definition 579 | * already in the feasible region of this linear program. If it fails, 580 | * it is due to small floating point error, and the current result is 581 | * kept. 582 | */ 583 | result = tempResult; 584 | } 585 | 586 | distance = det(lines[i].direction, lines[i].point - result); 587 | } 588 | } 589 | } 590 | } 591 | --------------------------------------------------------------------------------