├── strands_navfn ├── srv │ ├── SetCostmap.srv │ └── MakeNavPlan.srv ├── test │ ├── willow_costmap.pgm │ ├── CMakeLists.txt │ └── strands_path_calc_test.cpp ├── bgp_plugin.xml ├── Makefile.orig ├── include │ └── strands_navfn │ │ ├── strands_navwin.h │ │ ├── strands_read_pgm_costmap.h │ │ ├── strands_potarr_point.h │ │ ├── strands_navfn_ros.h │ │ └── strands_navfn.h ├── CHANGELOG.rst ├── package.xml ├── CMakeLists.txt └── src │ ├── strands_read_pgm_costmap.cpp │ ├── strands_navfn_node.cpp │ ├── strands_navwin.cpp │ └── strands_navtest.cpp ├── calibrate_chest ├── data │ └── chest.png ├── action │ └── CalibrateCamera.action ├── src │ ├── chest_calibration_publisher.cpp │ ├── calibrate_chest.cpp │ └── calibration_server.cpp ├── README.md ├── CHANGELOG.rst ├── package.xml └── CMakeLists.txt ├── strands_movebase ├── strands_movebase_params │ ├── nav_fn_ros.yaml │ ├── trajectory_planner_ros.yaml │ ├── local_costmap_params.yaml │ ├── global_costmap_params.yaml │ ├── move_base_params.yaml │ ├── dwa_planner_ros_old.yaml │ ├── dwa_planner_ros.yaml │ └── costmap_common_params.yaml ├── plugins │ └── nodelet_plugins.xml ├── include │ └── strands_movebase │ │ ├── subsample_cloud_nodelet.h │ │ └── noise_approximate_voxel_grid.h ├── launch │ ├── amcl.launch │ ├── pointcloud_processing.launch │ └── movebase.launch ├── src │ ├── remove_edges_laser.cpp │ ├── subsample_cloud.cpp │ ├── subsample_cloud_nodelet.cpp │ ├── remove_edges_cloud.cpp │ ├── mirror_floor_points.cpp │ └── noise_approximate_voxel_grid.cpp ├── package.xml ├── CHANGELOG.rst └── CMakeLists.txt ├── movebase_state_service ├── srv │ └── MovebaseStateService.srv ├── CHANGELOG.rst ├── package.xml ├── CMakeLists.txt └── src │ └── movebase_state_service.cpp ├── .gitignore ├── strands_description ├── package.xml ├── launch │ └── strands_state_publisher.launch ├── CHANGELOG.rst ├── urdf │ ├── scitos_all_robot_model.xacro │ └── scitos_chest_camera.xacro └── CMakeLists.txt ├── param_loader ├── scripts │ └── param_loader.py ├── config │ ├── lincoln_dwa.yaml │ └── current_dwa.yaml ├── CHANGELOG.rst ├── package.xml └── CMakeLists.txt ├── LICENSE └── README.md /strands_navfn/srv/SetCostmap.srv: -------------------------------------------------------------------------------- 1 | uint8[] costs 2 | uint16 height 3 | uint16 width 4 | --- -------------------------------------------------------------------------------- /calibrate_chest/data/chest.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strands-project/strands_movebase/HEAD/calibrate_chest/data/chest.png -------------------------------------------------------------------------------- /strands_navfn/test/willow_costmap.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strands-project/strands_movebase/HEAD/strands_navfn/test/willow_costmap.pgm -------------------------------------------------------------------------------- /strands_movebase/strands_movebase_params/nav_fn_ros.yaml: -------------------------------------------------------------------------------- 1 | #see http://wiki.ros.org/navfn for param description 2 | NavfnROS: 3 | allow_unknown: false 4 | default_tolerance: 0.0 5 | -------------------------------------------------------------------------------- /movebase_state_service/srv/MovebaseStateService.srv: -------------------------------------------------------------------------------- 1 | bool save_to_disk 2 | --- 3 | sensor_msgs/Image global_costmap_image 4 | sensor_msgs/Image local_costmap_image 5 | sensor_msgs/Image camera_image 6 | -------------------------------------------------------------------------------- /strands_navfn/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | catkin_add_gtest(strands_path_calc_test strands_path_calc_test.cpp ../src/strands_read_pgm_costmap.cpp) 2 | target_link_libraries(strands_path_calc_test strands_navfn netpbm) 3 | -------------------------------------------------------------------------------- /calibrate_chest/action/CalibrateCamera.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | string command 3 | --- 4 | #result definition 5 | string status 6 | float32 angle 7 | float32 height 8 | --- 9 | #feedback definition 10 | string status 11 | float32 progress 12 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Compiled Dynamic libraries 8 | *.so 9 | *.dylib 10 | *.dll 11 | 12 | # Compiled Static libraries 13 | *.lai 14 | *.la 15 | *.a 16 | *.lib 17 | 18 | # Executables 19 | *.exe 20 | *.out 21 | *.app 22 | 23 | #invisible files 24 | *~ -------------------------------------------------------------------------------- /strands_movebase/strands_movebase_params/trajectory_planner_ros.yaml: -------------------------------------------------------------------------------- 1 | #config file for an alternative local planner to dwa planner. not being used 2 | TrajectoryPlannerROS: 3 | max_vel_x: 0.95 4 | min_vel_x: 0.1 5 | max_rotational_vel: 1.0 6 | min_in_place_rotational_vel: 0.4 7 | 8 | holonomic_robot: false 9 | -------------------------------------------------------------------------------- /strands_navfn/bgp_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A implementation of a grid based planner using Dijkstra 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /strands_movebase/plugins/nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | This is my nodelet. 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /strands_navfn/srv/MakeNavPlan.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/PoseStamped start 2 | geometry_msgs/PoseStamped goal 3 | --- 4 | 5 | uint8 plan_found 6 | string error_message 7 | 8 | # if plan_found is true, this is an array of waypoints from start to goal, where the first one equals start and the last one equals goal 9 | geometry_msgs/PoseStamped[] path 10 | -------------------------------------------------------------------------------- /strands_movebase/strands_movebase_params/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | update_frequency: 5.0 3 | publish_frequency: 2.0 4 | static_map: false 5 | rolling_window: true 6 | width: 4.0 7 | height: 4.0 8 | resolution: 0.05 9 | plugins: 10 | - {name: obstacle_layer, type: "costmap_2d::VoxelLayer"} 11 | - {name: local_inflation_layer, type: "costmap_2d::InflationLayer"} 12 | -------------------------------------------------------------------------------- /strands_movebase/strands_movebase_params/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | update_frequency: 5.0 3 | publish_frequency: 0.5 4 | static_map: true 5 | rolling_window: false 6 | plugins: 7 | - {name: map_layer, type: "costmap_2d::StaticLayer"} 8 | - {name: no_go_layer, type: "costmap_2d::StaticLayer"} 9 | - {name: obstacle_layer, type: "costmap_2d::VoxelLayer"} 10 | - {name: global_inflation_layer, type: "costmap_2d::InflationLayer"} 11 | 12 | -------------------------------------------------------------------------------- /strands_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | strands_description 4 | 0.0.23 5 | 6 | The strands_description package contains the description 7 | of the optional chest camera of the STRANDS robot. 8 | 9 | 10 | Nils Bore 11 | 12 | MIT 13 | 14 | http://github.com/strands-project/strands_movebase 15 | 16 | Nils Bore 17 | 18 | catkin 19 | calibrate_chest 20 | 21 | 22 | -------------------------------------------------------------------------------- /strands_movebase/strands_movebase_params/move_base_params.yaml: -------------------------------------------------------------------------------- 1 | base_global_planner: strands_navfn/NavfnROS 2 | base_local_planner: dwa_local_planner/DWAPlannerROS 3 | controller_frequency: 10.0 4 | controller_patience: 15.0 5 | planner_frequency: 0.5 6 | 7 | recovery_behaviors: 8 | - {name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery} 9 | - {name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery} 10 | 11 | conservative_reset: 12 | layer_names: ["obstacle_layer"] 13 | reset_distance: 2.0 14 | 15 | aggressive_reset: 16 | layer_names: ["obstacle_layer"] 17 | reset_distance: 0.38 #distance from rotation centre to tail (0.32m) + 6cm slack 18 | -------------------------------------------------------------------------------- /strands_movebase/include/strands_movebase/subsample_cloud_nodelet.h: -------------------------------------------------------------------------------- 1 | #ifndef SUBSAMPLE_CLOUD_NODELET_H 2 | #define SUBSAMPLE_CLOUD_NODELET_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace strands_movebase { 9 | 10 | class subsample_cloud_nodelet : public nodelet::Nodelet { 11 | private: 12 | ros::Subscriber sub; 13 | ros::Publisher pub; 14 | ros::Time time; 15 | double resolution; 16 | int min_points; 17 | int skip_points; 18 | public: 19 | virtual void onInit(); 20 | void callback(const sensor_msgs::PointCloud2::ConstPtr& msg); 21 | }; 22 | 23 | } // movebase_processing 24 | 25 | #endif // SUBSAMPLE_CLOUD_NODELET_H 26 | -------------------------------------------------------------------------------- /param_loader/scripts/param_loader.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | import sys 4 | import yaml 5 | import rospy 6 | import dynamic_reconfigure.client 7 | 8 | 9 | if __name__ == '__main__': 10 | rospy.init_node('param_loader') 11 | 12 | filtered_argv=rospy.myargv(argv=sys.argv) 13 | 14 | if len(filtered_argv)<2: 15 | rospy.logwarn("No config yaml file provided.") 16 | elif len(filtered_argv)>2: 17 | rospy.logwarn("Too many arguments.") 18 | else: 19 | file_name=filtered_argv[1] 20 | 21 | 22 | with open(file_name, 'r') as stream: 23 | yaml_config=yaml.load(stream) 24 | print yaml_config 25 | for key, val in yaml_config.iteritems(): 26 | print key 27 | print val 28 | client = dynamic_reconfigure.client.Client("/move_base/" + key) 29 | client.update_configuration(val) 30 | print "UPDATED" 31 | 32 | -------------------------------------------------------------------------------- /strands_navfn/Makefile.orig: -------------------------------------------------------------------------------- 1 | # 2 | # Makefile for navigation function planner 3 | # 4 | 5 | CC = g++ 6 | CXX = g++ 7 | LD = g++ 8 | CPPFLAGS = -Wall -g -O3 -Iinclude -I/usr/local/include -I/usr/local/include/opencv 9 | CFLAGS = -DGCC -msse2 -mpreferred-stack-boundary=4 -O3 -Wall -Iinclude -I/usr/local/include 10 | GCC = g++ 11 | LDFLAGS = -Lbin 12 | SHAREDFLAGS = -shared -Wl,-soname, 13 | LIBS = -lfltk -lnetpbm 14 | 15 | OBJECTS = navfn navwin 16 | 17 | all: bin/navtest 18 | 19 | bin/navtest: obj/navtest.o $(OBJECTS:%=obj/%.o) 20 | $(LD) $(LDFLAGS) -o $@ $(OBJECTS:%=obj/%.o) obj/navtest.o $(LIBS) 21 | 22 | # general cpp command from src->obj 23 | obj/%.o:src/%.cpp 24 | $(GCC) $(CPPFLAGS) -c $< -o $@ 25 | 26 | # general c command from src->obj 27 | obj/%.o:src/%.c 28 | $(GCC) $(CFLAGS) -c $< -o $@ 29 | 30 | obj/navfn.o: include/navfn/navfn.h 31 | 32 | clean: 33 | - rm obj/*.o 34 | - rm bin/navtest 35 | 36 | dist: 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /param_loader/config/lincoln_dwa.yaml: -------------------------------------------------------------------------------- 1 | DWAPlannerROS: 2 | acc_lim_theta: 3.2 3 | acc_lim_x: 1.0 4 | acc_lim_y: 0.0 5 | acc_limit_trans: 0.1 6 | angular_sim_granularity: 0.1 7 | forward_point_distance: 0.325 8 | goal_distance_bias: 9.0 9 | #holonomic_robot: false 10 | #latch_xy_goal_tolerance: true 11 | max_rot_vel: 1.0 12 | max_scaling_factor: 0.2 13 | max_trans_vel: 0.55 14 | max_vel_x: 0.55 15 | max_vel_y: 0.0 16 | min_rot_vel: 0.2 17 | min_trans_vel: 0.0 18 | min_vel_x: 0.0 19 | min_vel_y: 0.0 20 | occdist_scale: 0.01 21 | oscillation_reset_angle: 0.2 22 | oscillation_reset_dist: 0.05 23 | path_distance_bias: 5.0 24 | prune_plan: true 25 | restore_defaults: false 26 | rot_stopped_vel: 0.1 27 | scaling_speed: 0.25 28 | sim_granularity: 0.025 29 | sim_time: 1.7 30 | stop_time_buffer: 0.2 31 | trans_stopped_vel: 0.0 32 | use_dwa: true 33 | vth_samples: 20 34 | vx_samples: 3 35 | vy_samples: 10 36 | xy_goal_tolerance: 0.3 37 | yaw_goal_tolerance: 0.1 -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2014 STRANDS 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /strands_navfn/include/strands_navfn/strands_navwin.h: -------------------------------------------------------------------------------- 1 | // 2 | // drawing fns for nav fn 3 | // 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include "strands_navfn.h" 16 | 17 | namespace strands_navfn { 18 | class NavWin 19 | : public Fl_Double_Window 20 | { 21 | public: 22 | NavWin(int w, int h, const char *name); 23 | ~NavWin(); 24 | 25 | int nw,nh; // width and height of image 26 | int pw,ph; // width and height of pot field 27 | int dec, inc; // decimation or expansion for display 28 | 29 | float maxval; // max potential value 30 | void drawPot(NavFn *nav); // draw everything... 31 | 32 | void drawOverlay(); 33 | 34 | uchar *im; // image for drawing 35 | int *pc, *pn, *po; // priority buffers 36 | int pce, pne, poe; // buffer sizes 37 | int goal[2]; 38 | int start[2]; 39 | int *path; // path buffer, cell indices 40 | int pathlen; // how many we have 41 | int pathbuflen; // how big the path buffer is 42 | 43 | void draw(); // draw the image 44 | }; 45 | }; 46 | -------------------------------------------------------------------------------- /param_loader/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package param_loader 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.0.23 (2016-02-08) 6 | ------------------- 7 | 8 | 0.0.22 (2016-01-28) 9 | ------------------- 10 | 11 | 0.0.21 (2015-04-27) 12 | ------------------- 13 | 14 | 0.0.20 (2015-04-22) 15 | ------------------- 16 | * add simple package to dyn reconfigure move_base via yaml files 17 | * Contributors: Bruno Lacerda 18 | 19 | 0.0.19 (2015-04-10) 20 | ------------------- 21 | 22 | 0.0.18 (2015-03-28) 23 | ------------------- 24 | 25 | 0.0.17 (2015-03-16) 26 | ------------------- 27 | 28 | 0.0.16 (2015-02-13 13:58) 29 | ------------------------- 30 | 31 | 0.0.15 (2015-02-13 11:45) 32 | ------------------------- 33 | 34 | 0.0.14 (2014-12-17) 35 | ------------------- 36 | 37 | 0.0.13 (2014-11-21) 38 | ------------------- 39 | 40 | 0.0.12 (2014-11-20) 41 | ------------------- 42 | 43 | 0.0.11 (2014-11-19 19:25) 44 | ------------------------- 45 | 46 | 0.0.10 (2014-11-19 13:18) 47 | ------------------------- 48 | 49 | 0.0.9 (2014-11-19 08:08) 50 | ------------------------ 51 | 52 | 0.0.8 (2014-11-12) 53 | ------------------ 54 | 55 | 0.0.7 (2014-11-09) 56 | ------------------ 57 | 58 | 0.0.6 (2014-10-27) 59 | ------------------ 60 | 61 | 0.0.5 (2014-10-23) 62 | ------------------ 63 | -------------------------------------------------------------------------------- /strands_description/launch/strands_state_publisher.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | [/chest_calibration_publisher/state] 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /param_loader/config/current_dwa.yaml: -------------------------------------------------------------------------------- 1 | #see http://ros.org/wiki/dwa_local_planner?distro=groovy for param description 2 | DWAPlannerROS: 3 | #default 2.5 -> with this value the robot sometimes gets to close to the wall when leaving a goal position in narrow corridors, causing dwa planner to have problems 4 | acc_lim_x: 1.0 5 | 6 | acc_lim_y: 0.0 7 | 8 | #default 3.2 9 | acc_lim_theta: 2.0 10 | 11 | min_vel_y: 0.0 12 | max_vel_y: 0.0 13 | max_rot_vel: 1.0 14 | min_rot_vel: 0.4 15 | 16 | yaw_goal_tolerance: 0.1 17 | 18 | #default:0.1 -> with this value the robot sometimes has troubles achieving the goal, due to low tolerance 19 | xy_goal_tolerance: 0.1 20 | 21 | 22 | # if the robot ever reaches the goal xy location it will simply rotate in place, even if it ends up outside the goal tolerance while it is doing so. 23 | #latch_xy_goal_tolerance: true 24 | 25 | 26 | sim_time: 2.1 27 | 28 | 29 | #cost = path_distance_bias * (distance to path from the endpoint of the trajectory in meters) + goal_distance_bias * (distance to local goal from the endpoint of the trajectory in meters) + occdist_scale * (maximum obstacle cost along the trajectory in obstacle cost (0-254)) 30 | path_distance_bias: 32.0 #default:32, previous:5 31 | goal_distance_bias: 24.0 #default:24, previous:9 32 | occdist_scale: 0.01 #default:0.01 33 | 34 | 35 | oscillation_reset_dist: 0.05 36 | 37 | 38 | prune_plan: true 39 | 40 | #holonomic_robot: false 41 | 42 | -------------------------------------------------------------------------------- /strands_navfn/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package strands_navfn 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.0.23 (2016-02-08) 6 | ------------------- 7 | 8 | 0.0.22 (2016-01-28) 9 | ------------------- 10 | 11 | 0.0.21 (2015-04-27) 12 | ------------------- 13 | 14 | 0.0.19 (2015-04-10) 15 | ------------------- 16 | * prepare for release 17 | * removing navfn maintainers so they dont get spammed by our errors 18 | * added missing strands_navfn 19 | * reading default_tolerance param before generating global plans 20 | * adding copy of navfn package, with lots of strands_* prefixes to avoid conflicts with navfn 21 | * Contributors: Bruno Lacerda 22 | 23 | 0.0.18 (2015-03-28) 24 | ------------------- 25 | 26 | 0.0.17 (2015-03-16) 27 | ------------------- 28 | 29 | 0.0.16 (2015-02-13 13:58) 30 | ------------------------- 31 | 32 | 0.0.15 (2015-02-13 11:45) 33 | ------------------------- 34 | 35 | 0.0.14 (2014-12-17) 36 | ------------------- 37 | 38 | 0.0.13 (2014-11-21) 39 | ------------------- 40 | 41 | 0.0.12 (2014-11-20) 42 | ------------------- 43 | 44 | 0.0.11 (2014-11-19 19:25) 45 | ------------------------- 46 | 47 | 0.0.10 (2014-11-19 13:18) 48 | ------------------------- 49 | 50 | 0.0.9 (2014-11-19 08:08) 51 | ------------------------ 52 | 53 | 0.0.8 (2014-11-12) 54 | ------------------ 55 | 56 | 0.0.7 (2014-11-09) 57 | ------------------ 58 | 59 | 0.0.6 (2014-10-27) 60 | ------------------ 61 | 62 | 0.0.5 (2014-10-23) 63 | ------------------ 64 | -------------------------------------------------------------------------------- /strands_movebase/strands_movebase_params/dwa_planner_ros_old.yaml: -------------------------------------------------------------------------------- 1 | #see http://ros.org/wiki/dwa_local_planner?distro=groovy for param description 2 | DWAPlannerROS: 3 | #default 2.5 -> with this value the robot sometimes gets to close to the wall when leaving a goal position in narrow corridors, causing dwa planner to have problems 4 | acc_lim_x: 1.0 5 | 6 | acc_lim_y: 0.0 7 | 8 | #default 3.2 9 | acc_lim_th: 2.0 10 | 11 | min_vel_y: 0.0 12 | max_vel_y: 0.0 13 | max_rot_vel: 1.0 14 | min_rot_vel: 0.4 15 | 16 | yaw_goal_tolerance: 0.1 17 | 18 | #default:0.1 -> with this value the robot sometimes has troubles achieving the goal, due to low tolerance 19 | xy_goal_tolerance: 0.3 20 | 21 | 22 | # if the robot ever reaches the goal xy location it will simply rotate in place, even if it ends up outside the goal tolerance while it is doing so. 23 | latch_xy_goal_tolerance: true 24 | 25 | 26 | sim_time: 2.1 27 | 28 | 29 | #cost = path_distance_bias * (distance to path from the endpoint of the trajectory in meters) + goal_distance_bias * (distance to local goal from the endpoint of the trajectory in meters) + occdist_scale * (maximum obstacle cost along the trajectory in obstacle cost (0-254)) 30 | path_distance_bias: 32.0 #default:32, previous:5 31 | goal_distance_bias: 24.0 #default:24, previous:9 32 | occdist_scale: 0.01 #default:0.01 33 | 34 | 35 | oscillation_reset_dist: 0.05 36 | 37 | 38 | prune_plan: true 39 | 40 | holonomic_robot: false 41 | 42 | -------------------------------------------------------------------------------- /strands_description/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package strands_description 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.0.23 (2016-02-08) 6 | ------------------- 7 | 8 | 0.0.22 (2016-01-28) 9 | ------------------- 10 | * Adjusted chest camera x direction 11 | * Contributors: Nils Bore 12 | 13 | 0.0.21 (2015-04-27) 14 | ------------------- 15 | 16 | 0.0.18 (2015-03-28) 17 | ------------------- 18 | 19 | 0.0.17 (2015-03-16) 20 | ------------------- 21 | 22 | 0.0.16 (2015-02-13) 23 | ------------------- 24 | 25 | 0.0.14 (2014-12-17) 26 | ------------------- 27 | 28 | 0.0.13 (2014-11-21) 29 | ------------------- 30 | 31 | 0.0.12 (2014-11-20) 32 | ------------------- 33 | 34 | 0.0.11 (2014-11-19) 35 | ------------------- 36 | 37 | 0.0.10 (2014-11-19) 38 | ------------------- 39 | * Added a readme file for calibrate_chest and made the urdf for chest camera more accurate 40 | * Contributors: Nils Bore 41 | 42 | 0.0.9 (2014-11-19) 43 | ------------------ 44 | 45 | 0.0.8 (2014-11-12) 46 | ------------------ 47 | 48 | 0.0.7 (2014-11-09) 49 | ------------------ 50 | * final and tested version of loader 51 | * new machine tags 52 | * Contributors: Jaime Pulido Fentanes 53 | 54 | 0.0.6 (2014-10-27) 55 | ------------------ 56 | 57 | 0.0.5 (2014-10-23) 58 | ------------------ 59 | * added install targets 60 | * added changelogs 61 | * Cleared up package.xml 62 | * Split the chest camera links out of scitos_description, added them here instead 63 | * Contributors: Marc Hanheide, Nils Bore 64 | -------------------------------------------------------------------------------- /calibrate_chest/src/chest_calibration_publisher.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | int main(int argc, char** argv) { 5 | 6 | ros::init(argc, argv, "chest_calibration_publisher"); 7 | ros::NodeHandle n; 8 | 9 | // get calibration with respect to ground plane from the calibrate_chest node 10 | double height, angle; 11 | n.param("/chest_xtion_height", height, 1.10f); // get the height calibration 12 | n.param("/chest_xtion_angle", angle, 0.72f); // get the angle calibration 13 | 14 | //ROS_INFO("Setting height of chest camera: %f", height); 15 | //ROS_INFO("Setting angle of chest camera: %f", angle); 16 | ros::Rate rate(1.0f); 17 | 18 | ros::Publisher pub = n.advertise("/chest_calibration_publisher/state", 1); 19 | 20 | int counter = 0; // only publish for the first 10 secs, transforms will stay in tf 21 | while (n.ok() && counter < 10) { 22 | sensor_msgs::JointState joint_state; 23 | joint_state.header.stamp = ros::Time::now(); 24 | joint_state.name.resize(2); 25 | joint_state.position.resize(2); 26 | joint_state.velocity.resize(2); 27 | joint_state.name[0] = "chest_xtion_height_joint"; 28 | joint_state.name[1] = "chest_xtion_tilt_joint"; 29 | joint_state.position[0] = height; 30 | joint_state.position[1] = angle; 31 | joint_state.velocity[0] = 0; 32 | joint_state.velocity[1] = 0; 33 | pub.publish(joint_state); 34 | rate.sleep(); 35 | ++counter; 36 | } 37 | 38 | ROS_INFO("Stopping to publish chest transform after 10 seconds, quitting..."); 39 | 40 | return 0; 41 | } 42 | -------------------------------------------------------------------------------- /strands_movebase/strands_movebase_params/dwa_planner_ros.yaml: -------------------------------------------------------------------------------- 1 | #see http://ros.org/wiki/dwa_local_planner?distro=groovy for param description 2 | DWAPlannerROS: 3 | #default 2.5 -> with this value the robot sometimes gets to close to the wall when leaving a goal position in narrow corridors, causing dwa planner to have problems 4 | acc_lim_x: 0.8 5 | acc_limit_trans: 0.8 6 | acc_lim_y: 0.0 7 | acc_lim_theta: 3.14 8 | 9 | 10 | min_vel_x: 0.00 11 | max_vel_x: 0.55 12 | min_trans_vel: 0.05 13 | max_trans_vel: 0.55 14 | min_vel_y: 0.0 15 | max_vel_y: 0.0 16 | max_rot_vel: 1.0 17 | min_rot_vel: 0.4 18 | 19 | 20 | rot_stopped_vel: 0.01 #Below what maximum velocity we consider the robot to be stopped in translation", 0.1) 21 | trans_stopped_vel: 0.01 #"Below what maximum rotation velocity we consider the robot to be stopped in rotation", 0.1) 22 | 23 | 24 | vx_samples: 10 25 | vy_samples: 1 26 | vth_samples: 20 27 | 28 | 29 | yaw_goal_tolerance: 0.1 30 | #default:0.1 -> with this value the robot sometimes has troubles achieving the goal, due to low tolerance 31 | xy_goal_tolerance: 0.3 32 | # if the robot ever reaches the goal xy location it will simply rotate in place, even if it ends up outside the goal tolerance while it is doing so. 33 | latch_xy_goal_tolerance: true 34 | 35 | 36 | sim_time: 1.7 37 | 38 | #cost = path_distance_bias * (distance to path from the endpoint of the trajectory in meters) + goal_distance_bias * (distance to local goal from the endpoint of the trajectory in meters) + occdist_scale * (maximum obstacle cost along the trajectory in obstacle cost (0-254)) 39 | path_distance_bias: 32.0 #default:32, previous:5 40 | goal_distance_bias: 24.0 #default:24, previous:9 41 | occdist_scale: 0.01 #default:0.01 42 | 43 | oscillation_reset_dist: 0.05 44 | forward_point_distance: 0.0 45 | 46 | prune_plan: true 47 | holonomic_robot: false -------------------------------------------------------------------------------- /calibrate_chest/README.md: -------------------------------------------------------------------------------- 1 | calibrate_chest 2 | =============== 3 | 4 | The new way to use this package is through the `calibration_server` action server. To use it, run `rosrun calibrate_chest calibration_server` and, in another terminal, `rosrun actionlib axclient.py /calibrate_chest`. To compute a new transformation between the camera and the floor, enter command `calibrate`. If you want to publish an already saved calibration (the calibration is saved in mongodb between runs), enter command `publish`. When you run the calibrate command, it will check that you are closer than 3 degrees from the desired 46 degrees angle of the chest camera. Otherwise, no calibration will be stored. 5 | 6 | # calibrate_chest node (legacy) 7 | 8 | * Do `rosrun calibrate_chest calibrate_chest` with the datacentre running if you want to store the parameters there, make sure that the largest visible plane for the chest camera is the floor. Also, notice that you have to have the chest camera running and publishing on topic `chest_xtion`. 9 | * To use these parameters when you launch the bringup next time, be sure to have the datacentre running. 10 | * The urdf can be updated manually by doing `rosrun calibrate_chest chest_calibration_publisher` if you don't want to restart the larger system (e.g. `strands_movebase.launch`, which includes this). 11 | 12 | # Example 13 | 14 | When running `rosrun calibrate_chest calibrate_chest` the node tries to find the largest visible plane and determine the angle and height of the chest camera. It will display a window of the current point cloud, with the points belonging to the floor coloured in red. It should look something like the following, you might need a mouse to rotate: 15 | 16 | ![Calibration point cloud example](https://github.com/strands-project/strands_movebase/tree/hydro-devel/calibrate_chest/data/chest.png "Example of how the calibration should look.") 17 | 18 | Close the window to save the calibration. 19 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | strands_movebase 2 | ================ 3 | 4 | A repository for all the STRANDS-augmented movebase, including 3D obstacle avoidance, etc. Relies on scitos_2d_navigation if it is configured to only use laser scan input for navigation, https://github.com/strands-project/scitos_2d_navigation. 5 | 6 | # Usage 7 | 8 | * `roslaunch strands_movebase movebase.launch map:=/path/to/map.yaml` 9 | * To be able to use this package you have to create a map with gmapping. This can be run with `rosrun gmapping slam_gmapping`, save the map with `rosrun map_server map_saver`. 10 | * Each launch file takes the argument `map` which is the path to the map saved with gmapping. 11 | * Optionally, provide a `with_no_go_map:=true` and `no_go_map`, the path to a map annotated with no-go areas. 12 | * If you do not want to launch it with the 3d obstacle avoidance, provide the additional argument `with_camera:=false`. 13 | * Provide `camera:=` if you have an OpenNI camera publishing on another namespace than the default `chest_xtion`. 14 | * Optionally provide `z_obstacle_threshold:=`, where `` m is the distance from the floor above which we consider points as obstacles. Making this larger than the default 0.1 improves navigation robustness but may lead to missing small obstacles. 15 | * Same goes for `z_stair_threshold:=`, the distance below which points are considered negative obstacles. Again, increasing improves robustness, but make sure you don't have any stairs with smaller gaps than this. 16 | 17 | If you run with the camera option, be sure that you have a depth camera publishing on the `camera_namespace` topic. The camera also needs a valid TF transform connecting it to `base_link`. For more details on the Strands solution, see https://github.com/strands-project/strands_movebase/tree/hydro-devel/calibrate_chest and https://github.com/strands-project/strands_movebase/tree/hydro-devel/strands_description. 18 | -------------------------------------------------------------------------------- /movebase_state_service/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package movebase_state_service 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.0.23 (2016-02-08) 6 | ------------------- 7 | 8 | 0.0.22 (2016-01-28) 9 | ------------------- 10 | 11 | 0.0.21 (2015-04-27) 12 | ------------------- 13 | 14 | 0.0.18 (2015-03-28) 15 | ------------------- 16 | 17 | 0.0.17 (2015-03-16) 18 | ------------------- 19 | 20 | 0.0.16 (2015-02-13) 21 | ------------------- 22 | * Merge pull request `#38 `_ from nilsbore/state_snapshot 23 | [movebase_state_service] Added navfn as dependency in movebase_state_service 24 | * Forgot to add navfn as dependency in movebase_state_service 25 | * Contributors: Nils Bore 26 | 27 | 0.0.15 (2015-02-13) 28 | ------------------- 29 | * fixed version number 30 | * Merge remote-tracking branch 'origin/state_snapshot' into state_snapshot 31 | * Changed enum to be portable 32 | * Removed pcl_ros dependency and added installed the node 33 | * Update package.xml 34 | * Changed the plan topics 35 | * Removed old code 36 | * Fixed some bugs 37 | * Changed the default folder for storing the images 38 | * Added the state snapshot service for saving the current costmaps with the paths overlaid 39 | * Contributors: Marc Hanheide, Nils Bore, Rares Ambrus 40 | 41 | 0.0.14 (2014-12-17) 42 | ------------------- 43 | 44 | 0.0.13 (2014-11-21) 45 | ------------------- 46 | 47 | 0.0.12 (2014-11-20) 48 | ------------------- 49 | 50 | 0.0.11 (2014-11-19 19:25) 51 | ------------------------- 52 | 53 | 0.0.10 (2014-11-19 13:18) 54 | ------------------------- 55 | 56 | 0.0.9 (2014-11-19 08:08) 57 | ------------------------ 58 | 59 | 0.0.8 (2014-11-12) 60 | ------------------ 61 | 62 | 0.0.7 (2014-11-09) 63 | ------------------ 64 | 65 | 0.0.6 (2014-10-27) 66 | ------------------ 67 | 68 | 0.0.5 (2014-10-23) 69 | ------------------ 70 | -------------------------------------------------------------------------------- /strands_navfn/include/strands_navfn/strands_read_pgm_costmap.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | #ifndef READ_PGM_COSTMAP_H 30 | #define READ_PGM_COSTMAP_H 31 | 32 | // is true for ROS-generated raw cost maps 33 | COSTTYPE *readPGM(const char *fname, int *width, int *height, bool raw = false); 34 | 35 | #endif // READ_PGM_COSTMAP_H 36 | -------------------------------------------------------------------------------- /calibrate_chest/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package calibrate_chest 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.0.23 (2016-02-08) 6 | ------------------- 7 | 8 | 0.0.22 (2016-01-28) 9 | ------------------- 10 | * Fixed bug 11 | * Added an option to calibrate and publish at the same time 12 | * Added install target for calibration server 13 | * Contributors: Nils Bore 14 | 15 | 0.0.21 (2015-04-27) 16 | ------------------- 17 | 18 | 0.0.18 (2015-03-28) 19 | ------------------- 20 | 21 | 0.0.17 (2015-03-16) 22 | ------------------- 23 | 24 | 0.0.16 (2015-02-13) 25 | ------------------- 26 | 27 | 0.0.14 (2014-12-17) 28 | ------------------- 29 | * Update README.md 30 | * Added timeout if no point cloud received 31 | * Made desired angle configurable 32 | * Changed all the camera names to be parameters 33 | * Polishing 34 | * Added the publish option to the action server as well 35 | * Made it a bit nicer 36 | * Got it compiling 37 | * Added dep also in package.xml 38 | * Initial commit for making camera calibration an action server instead 39 | * Contributors: Nils Bore 40 | 41 | 0.0.13 (2014-11-21) 42 | ------------------- 43 | 44 | 0.0.12 (2014-11-20) 45 | ------------------- 46 | 47 | 0.0.11 (2014-11-19) 48 | ------------------- 49 | 50 | 0.0.10 (2014-11-19) 51 | ------------------- 52 | * Added example chest calibration 53 | * Update README.md 54 | * Update README.md 55 | * Update README.md 56 | * Update README.md 57 | * Update README.md 58 | * Added a readme file for calibrate_chest and made the urdf for chest camera more accurate 59 | * Contributors: Nils Bore 60 | 61 | 0.0.9 (2014-11-19) 62 | ------------------ 63 | 64 | 0.0.8 (2014-11-12) 65 | ------------------ 66 | 67 | 0.0.7 (2014-11-09) 68 | ------------------ 69 | 70 | 0.0.6 (2014-10-27) 71 | ------------------ 72 | 73 | 0.0.5 (2014-10-23) 74 | ------------------ 75 | * added changelogs 76 | * Listed pcl as a dependency in calibrate_chest package.xml 77 | * Removed everything but calibrate chest, preparing for move into strands_movebase 78 | * Contributors: Marc Hanheide, Nils Bore 79 | -------------------------------------------------------------------------------- /strands_movebase/launch/amcl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /param_loader/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | param_loader 4 | 0.0.23 5 | The param_loader package 6 | 7 | 8 | 9 | 10 | Bruno Lacerda 11 | 12 | 13 | 14 | 15 | 16 | MIT 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | dynamic_reconfigure 44 | rospy 45 | dynamic_reconfigure 46 | rospy 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /strands_navfn/include/strands_navfn/strands_potarr_point.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2008, (Simon) Ye Cheng 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the Willow Garage nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | *********************************************************************/ 36 | 37 | #ifndef POTARR_POINT_H_ 38 | #define POTARR_POINT_H_ 39 | 40 | #include 41 | 42 | namespace strands_navfn { 43 | struct PotarrPoint { 44 | float x; 45 | float y; 46 | float z; 47 | float pot_value; 48 | }; 49 | } 50 | 51 | POINT_CLOUD_REGISTER_POINT_STRUCT( 52 | strands_navfn::PotarrPoint, 53 | (float, x, x) 54 | (float, y, y) 55 | (float, z, z) 56 | (float, pot_value, pot_value)); 57 | 58 | #endif 59 | 60 | -------------------------------------------------------------------------------- /strands_movebase/src/remove_edges_laser.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | ros::Publisher pub; // publishes laser scan with removed edges 5 | float cutoff_angle; // how many angle to cut off in the laser 6 | 7 | void callback(const sensor_msgs::LaserScan::ConstPtr& msg) 8 | { 9 | int n_points = int(cutoff_angle / msg->angle_increment); 10 | float angle_min = msg->angle_min + float(n_points)*msg->angle_increment; 11 | float angle_max = msg->angle_max - float(n_points)*msg->angle_increment; 12 | int n_last = msg->ranges.size() - n_points; 13 | 14 | sensor_msgs::LaserScan msg_out; 15 | msg_out.ranges.insert(msg_out.ranges.begin(), msg->ranges.begin() + n_points, msg->ranges.begin() + n_last); 16 | if (!msg_out.intensities.empty()) { 17 | msg_out.intensities.insert(msg_out.intensities.begin(), msg->intensities.begin() + n_points, msg->intensities.begin() + n_last); 18 | } 19 | 20 | msg_out.angle_min = angle_min; 21 | msg_out.angle_max = angle_max; 22 | msg_out.angle_increment = msg->angle_increment; 23 | msg_out.time_increment = msg->time_increment; 24 | msg_out.scan_time = msg->scan_time; 25 | msg_out.range_min = msg->range_min; 26 | msg_out.range_max = msg->range_max; 27 | msg_out.header = msg->header; 28 | pub.publish(msg_out); 29 | } 30 | 31 | int main(int argc, char** argv) 32 | { 33 | ros::init(argc, argv, "remove_edges_laser"); 34 | ros::NodeHandle n; 35 | 36 | ros::NodeHandle pn("~"); 37 | // topic of input laser scan 38 | if (!pn.hasParam("input")) { 39 | ROS_ERROR("Could not find parameter input."); 40 | return -1; 41 | } 42 | std::string input; 43 | pn.getParam("input", input); 44 | 45 | // topic of output laser scan 46 | if (!pn.hasParam("output")) { 47 | ROS_ERROR("Could not find parameter output."); 48 | return -1; 49 | } 50 | std::string output; 51 | pn.getParam("output", output); 52 | 53 | // how wide angle to cut off at the edges 54 | if (!pn.hasParam("cutoff_angle")) { 55 | ROS_ERROR("Could not find parameter cutoff_angle."); 56 | return -1; 57 | } 58 | double temp; 59 | pn.getParam("cutoff_angle", temp); 60 | cutoff_angle = M_PI/180.0*temp; 61 | 62 | ros::Subscriber sub = n.subscribe(input, 1, callback); 63 | pub = n.advertise(output, 1); 64 | 65 | ros::spin(); 66 | 67 | return 0; 68 | } 69 | -------------------------------------------------------------------------------- /movebase_state_service/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | movebase_state_service 4 | 0.0.23 5 | 6 | This service returns the local and global costmaps as image with the global and local plans overlaid. 7 | It also returns the last image from the camera. 8 | 9 | 10 | 11 | nbore 12 | 13 | 14 | 15 | 16 | BSD 17 | 18 | 19 | 20 | http://github.com/strands-project/scitos_2d_navigation 21 | 22 | 23 | 24 | Nils Bore 25 | Bruno Lacerda 26 | 27 | 28 | 29 | catkin 30 | sensor_msgs 31 | roscpp 32 | std_msgs 33 | tf 34 | costmap_2d 35 | navfn 36 | nav_core 37 | cv_bridge 38 | nav_msgs 39 | geometry_msgs 40 | nav_core 41 | navfn 42 | message_runtime 43 | sensor_msgs 44 | roscpp 45 | std_msgs 46 | tf 47 | costmap_2d 48 | cv_bridge 49 | nav_msgs 50 | geometry_msgs 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /strands_movebase/src/subsample_cloud.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "strands_movebase/noise_approximate_voxel_grid.h" 9 | 10 | ros::Publisher pub; 11 | double resolution; 12 | int min_points; 13 | int skip_points; 14 | 15 | void callback(const sensor_msgs::PointCloud2::ConstPtr& msg) 16 | { 17 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); 18 | pcl::fromROSMsg(*msg, *cloud); 19 | pcl::PointCloud voxel_cloud; 20 | 21 | noise_approximate_voxel_grid sor(min_points, skip_points); // a bit faster than nvg, not as accurate 22 | sor.setInputCloud(cloud); 23 | sor.setLeafSize(resolution, resolution, resolution); 24 | sor.filter(voxel_cloud); 25 | 26 | /*pcl::StatisticalOutlierRemoval sor; // another possibility, slow 27 | sor.setInputCloud(new_cloud); 28 | sor.setMeanK(20); 29 | sor.setStddevMulThresh(1.0); 30 | sor.filter(voxel_cloud);*/ 31 | 32 | sensor_msgs::PointCloud2 msg_cloud; 33 | pcl::toROSMsg(voxel_cloud, msg_cloud); 34 | msg_cloud.header = msg->header; 35 | 36 | pub.publish(msg_cloud); 37 | } 38 | 39 | int main(int argc, char** argv) 40 | { 41 | ros::init(argc, argv, "subsample_cloud"); 42 | ros::NodeHandle n; 43 | 44 | ros::NodeHandle pn("~"); 45 | // topic of input cloud 46 | if (!pn.hasParam("input")) { 47 | ROS_ERROR("Could not find parameter input."); 48 | return -1; 49 | } 50 | std::string input; 51 | pn.getParam("input", input); 52 | 53 | // topic of output cloud 54 | if (!pn.hasParam("output")) { 55 | ROS_ERROR("Could not find parameter output."); 56 | return -1; 57 | } 58 | std::string output; 59 | pn.getParam("output", output); 60 | 61 | pn.param("resolution", resolution, 0.05); 62 | pn.param("min_points", min_points, 5); 63 | pn.param("skip_points", skip_points, 20); 64 | 65 | ros::Subscriber sub = n.subscribe(input, 1, callback); 66 | pub = n.advertise(output, 1); 67 | 68 | ros::Rate rate(5); // updating at 5 hz, slightly faster than move_base 69 | while (n.ok()) { 70 | rate.sleep(); 71 | ros::spinOnce(); 72 | } 73 | 74 | return 0; 75 | } 76 | -------------------------------------------------------------------------------- /strands_movebase/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | strands_movebase 4 | 0.0.23 5 | 6 | This package contains components for using the ROS move base together 7 | with the Scitos G5 robot. There is options for running obstacle avoidance 8 | both with only laser and with an additional depth-sensing camera 9 | mounted in front. The additional nodes in the package are for processing 10 | the incoming clouds from the camera for obstacle avoidance. 11 | 12 | 13 | 14 | nbore 15 | 16 | 17 | 18 | 19 | MIT 20 | 21 | 22 | 23 | http://github.com/strands-project/strands_movebase 24 | 25 | 26 | 27 | Nils Bore 28 | Bruno Lacerda 29 | 30 | 31 | 32 | catkin 33 | sensor_msgs 34 | roscpp 35 | std_msgs 36 | pcl_ros 37 | libpcl-all-dev 38 | tf 39 | nodelet 40 | sensor_msgs 41 | roscpp 42 | std_msgs 43 | pcl_ros 44 | libpcl-all 45 | tf 46 | amcl 47 | strands_description 48 | dwa_local_planner 49 | map_server 50 | movebase_state_service 51 | strands_navfn 52 | nodelet 53 | 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /strands_navfn/package.xml: -------------------------------------------------------------------------------- 1 | 2 | strands_navfn 3 | 0.0.23 4 | 5 | This package is a copy of the navfn package, with a small edit to allow for setting of the goal_tolerance param 6 | at runtime. The navfn package is currently maintained by David V. Lu!! davidvlu@gmail.com and 7 | Michael Ferguson mferguson@fetchrobotics.com 8 | 9 | navfn provides a fast interpolated navigation function that can be used to create plans for 10 | a mobile base. The planner assumes a circular robot and operates on a costmap to find a 11 | minimum cost plan from a start point to an end point in a grid. The navigation function is 12 | computed with Dijkstra's algorithm, but support for an A* heuristic may also be added in the 13 | near future. navfn also provides a ROS wrapper for the navfn planner that adheres to the 14 | nav_core::BaseGlobalPlanner interface specified in nav_core. 15 | 16 | 17 | Kurt Konolige 18 | Eitan Marder-Eppstein 19 | contradict@gmail.com 20 | Bruno Lacerda 21 | BSD 22 | http://wiki.ros.org/navfn 23 | 24 | catkin 25 | 26 | cmake_modules 27 | costmap_2d 28 | geometry_msgs 29 | nav_core 30 | nav_msgs 31 | netpbm 32 | pcl_conversions 33 | pcl_ros 34 | pluginlib 35 | rosconsole 36 | roscpp 37 | tf 38 | visualization_msgs 39 | 40 | costmap_2d 41 | geometry_msgs 42 | nav_core 43 | nav_msgs 44 | pcl_conversions 45 | pcl_ros 46 | pluginlib 47 | rosconsole 48 | roscpp 49 | tf 50 | visualization_msgs 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /strands_movebase/src/subsample_cloud_nodelet.cpp: -------------------------------------------------------------------------------- 1 | #include "strands_movebase/subsample_cloud_nodelet.h" 2 | 3 | // this should really be in the implementation (.cpp file) 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include "strands_movebase/noise_approximate_voxel_grid.h" 12 | 13 | // watch the capitalization carefully 14 | PLUGINLIB_EXPORT_CLASS(strands_movebase::subsample_cloud_nodelet, nodelet::Nodelet) 15 | 16 | namespace strands_movebase { 17 | 18 | void subsample_cloud_nodelet::onInit() 19 | { 20 | NODELET_INFO("Initializing subsample_cloud nodelet..."); 21 | 22 | ros::NodeHandle nh = getNodeHandle(); 23 | ros::NodeHandle pn = getPrivateNodeHandle(); 24 | 25 | // topic of input cloud 26 | if (!pn.hasParam("input")) { 27 | ROS_ERROR("Could not find parameter input."); 28 | } 29 | std::string input; 30 | pn.getParam("input", input); 31 | NODELET_INFO("With input %s", input.c_str()); 32 | 33 | // topic of output cloud 34 | if (!pn.hasParam("output")) { 35 | ROS_ERROR("Could not find parameter output."); 36 | } 37 | std::string output; 38 | pn.getParam("output", output); 39 | NODELET_INFO("And output %s", output.c_str()); 40 | 41 | pn.param("resolution", resolution, 0.05); 42 | pn.param("min_points", min_points, 5); 43 | pn.param("skip_points", skip_points, 20); 44 | 45 | time = ros::Time::now(); 46 | 47 | sub = nh.subscribe(input, 1, &subsample_cloud_nodelet::callback, this); 48 | pub = nh.advertise(output, 1); 49 | } 50 | 51 | void subsample_cloud_nodelet::callback(const sensor_msgs::PointCloud2::ConstPtr& msg) 52 | { 53 | // this will give us a rate of ~5hz 54 | ros::Time new_time = ros::Time::now(); 55 | if ((new_time - time).toSec() < 0.18) { 56 | return; 57 | } 58 | time = new_time; 59 | 60 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); 61 | pcl::fromROSMsg(*msg, *cloud); 62 | pcl::PointCloud voxel_cloud; 63 | 64 | noise_approximate_voxel_grid sor(min_points, skip_points); // a bit faster than nvg, not as accurate 65 | sor.setInputCloud(cloud); 66 | sor.setLeafSize(resolution, resolution, resolution); 67 | sor.filter(voxel_cloud); 68 | 69 | sensor_msgs::PointCloud2 msg_cloud; 70 | pcl::toROSMsg(voxel_cloud, msg_cloud); 71 | msg_cloud.header = msg->header; 72 | 73 | pub.publish(msg_cloud); 74 | } 75 | 76 | } // movebase_processing 77 | -------------------------------------------------------------------------------- /strands_movebase/src/remove_edges_cloud.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | ros::Publisher pub; // publishes pointcloud with removed edges 7 | double cutoff; // how many pixels to cut off in the depth image 8 | double cutoff_z; // how much to cut off along the z axis, meters 9 | 10 | void callback(const sensor_msgs::PointCloud2::ConstPtr& msg) 11 | { 12 | pcl::PointCloud cloud; 13 | pcl::fromROSMsg(*msg, cloud); 14 | 15 | Eigen::Matrix3f K; // camera matrix of short range primesense 16 | K << 570.34f, 0.0f, 314.5f, 0.0f, 570.34f, 235.5f, 0.0f, 0.0f, 1.0f; 17 | 18 | pcl::PointCloud new_cloud; 19 | new_cloud.resize(cloud.size()); 20 | Eigen::Vector3f p; 21 | int counter = 0; 22 | for (int i = 0; i < cloud.size(); ++i) { 23 | // have a threshold for closeness as well 24 | if (cloud.points[i].z < cutoff_z) { 25 | continue; 26 | } 27 | // transform points to image plane and make sure they are within bounds 28 | p = K*cloud.points[i].getVector3fMap(); 29 | p = p / p(2); // we don't have any points at z = 0 30 | if (p(0) > 0.0f + cutoff && p(0) < 629.0f - cutoff && 31 | p(1) > 0.0f + cutoff && p(1) < 471.0f - cutoff) { 32 | new_cloud.points[counter] = cloud.points[i]; 33 | ++counter; 34 | } 35 | } 36 | new_cloud.resize(counter); 37 | 38 | sensor_msgs::PointCloud2 msg_cloud; 39 | pcl::toROSMsg(new_cloud, msg_cloud); 40 | msg_cloud.header = msg->header; 41 | pub.publish(msg_cloud); 42 | } 43 | 44 | int main(int argc, char** argv) 45 | { 46 | ros::init(argc, argv, "remove_edges_cloud"); 47 | ros::NodeHandle n; 48 | 49 | ros::NodeHandle pn("~"); 50 | // topic of input cloud 51 | if (!pn.hasParam("input")) { 52 | ROS_ERROR("Could not find parameter input."); 53 | return -1; 54 | } 55 | std::string input; 56 | pn.getParam("input", input); 57 | 58 | // topic of output cloud 59 | if (!pn.hasParam("output")) { 60 | ROS_ERROR("Could not find parameter output."); 61 | return -1; 62 | } 63 | std::string output; 64 | pn.getParam("output", output); 65 | 66 | // how many pixels to cut off in the depth image 67 | if (!pn.hasParam("cutoff")) { 68 | ROS_ERROR("Could not find parameter cutoff."); 69 | return -1; 70 | } 71 | pn.getParam("cutoff", cutoff); 72 | 73 | if (!pn.hasParam("cutoff_z")) { 74 | ROS_ERROR("Could not find parameter cutoff_z."); 75 | return -1; 76 | } 77 | pn.getParam("cutoff_z", cutoff_z); 78 | 79 | ros::Subscriber sub = n.subscribe(input, 1, callback); 80 | pub = n.advertise(output, 1); 81 | 82 | ros::spin(); 83 | 84 | return 0; 85 | } 86 | -------------------------------------------------------------------------------- /calibrate_chest/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | calibrate_chest 4 | 0.0.23 5 | 6 | This package contains one node that calculates the angle and height 7 | between the camera publishing on topic /chest_xtion and the floor. 8 | This data is saved in the mongodb_store and published by another 9 | node when starting the robot with the scitos_bringup launch file. 10 | 11 | 12 | 13 | Nils Bore 14 | 15 | 16 | 17 | 18 | BSD 19 | 20 | 21 | 22 | http://github.com/strands-project/scitos_common 23 | 24 | 25 | 26 | Nils Bore 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | catkin 40 | sensor_msgs 41 | roscpp 42 | std_msgs 43 | mongodb_store 44 | pcl_ros 45 | libpcl-all-dev 46 | actionlib_msgs 47 | actionlib 48 | 49 | sensor_msgs 50 | roscpp 51 | std_msgs 52 | mongodb_store 53 | pcl_ros 54 | libpcl-all 55 | actionlib_msgs 56 | actionlib 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /strands_description/urdf/scitos_all_robot_model.xacro: -------------------------------------------------------------------------------- 1 | 2 | 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 | 56 | 57 | 58 | 59 | 60 | 61 | 63 | 64 | 65 | 66 | 67 | 68 | 70 | 71 | 72 | 73 | 74 | 75 | 77 | 78 | 79 | 80 | 81 | 82 | -------------------------------------------------------------------------------- /strands_description/urdf/scitos_chest_camera.xacro: -------------------------------------------------------------------------------- 1 | 2 | 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 | 57 | 58 | 59 | 60 | 61 | 62 | 64 | 65 | 66 | 67 | 68 | 69 | 71 | 72 | 73 | 74 | 75 | 76 | 78 | 79 | 80 | 81 | 82 | 83 | -------------------------------------------------------------------------------- /movebase_state_service/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(movebase_state_service) 3 | 4 | add_definitions(-std=c++0x) 5 | 6 | ## Find catkin macros and libraries 7 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 8 | ## is used, also find other catkin packages 9 | find_package(catkin REQUIRED COMPONENTS 10 | roscpp 11 | sensor_msgs 12 | std_msgs 13 | tf 14 | costmap_2d 15 | navfn 16 | nav_core 17 | cv_bridge 18 | nav_msgs 19 | geometry_msgs 20 | ) 21 | 22 | ## System dependencies are found with CMake's conventions 23 | # find_package(Boost REQUIRED COMPONENTS system) 24 | 25 | find_package(OpenCV REQUIRED) 26 | 27 | ## Generate messages in the 'msg' folder 28 | #~ add_message_files( 29 | #~ DEPENDENCIES actionlib_msgs std_msgs 30 | #~ ) 31 | 32 | ## Generate services in the 'srv' folder 33 | # add_service_files( 34 | # FILES 35 | # Service1.srv 36 | # Service2.srv 37 | # ) 38 | add_service_files( 39 | FILES 40 | MovebaseStateService.srv 41 | ) 42 | 43 | ## Generate added messages and services with any dependencies listed here 44 | generate_messages( 45 | DEPENDENCIES 46 | std_msgs # Or other packages containing msgs 47 | sensor_msgs 48 | # actionlib_msgs 49 | ) 50 | 51 | 52 | ################################### 53 | ## catkin specific configuration ## 54 | ################################### 55 | ## The catkin_package macro generates cmake config files for your package 56 | ## Declare things to be passed to dependent projects 57 | ## LIBRARIES: libraries you create in this project that dependent projects also need 58 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 59 | ## DEPENDS: system dependencies of this project that dependent projects also need 60 | catkin_package( 61 | # INCLUDE_DIRS include 62 | # LIBRARIES dynamic_layer overwrite_layer smart_obstacle_layer unleash_static_planner static_planner 63 | # CATKIN_DEPENDS costmap_2d dynamic_reconfigure roscpp nav_core actionlib_msgs 64 | # DEPENDS system_lib 65 | ) 66 | 67 | ########### 68 | ## Build ## 69 | ########### 70 | 71 | ## Specify additional locations of header files 72 | ## Your package locations should be listed before other locations 73 | # include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) 74 | include_directories( 75 | ${catkin_INCLUDE_DIRS} 76 | #include 77 | ) 78 | 79 | ## Declare a cpp library 80 | #add_library(noise_voxel_grid src/noise_voxel_grid.cpp) 81 | #add_library(noise_approximate_voxel_grid src/noise_approximate_voxel_grid.cpp) 82 | 83 | ## Declare a cpp executable 84 | add_executable(movebase_state_service src/movebase_state_service.cpp) 85 | 86 | ## Add cmake target dependencies of the executable/library 87 | ## as an example, message headers may need to be generated before nodes 88 | #add_dependencies(scitos_2d_navigation_node scitos_2d_navigation_generate_messages_cpp) 89 | add_dependencies(movebase_state_service movebase_state_service_generate_messages_cpp) 90 | target_link_libraries(movebase_state_service 91 | ${catkin_LIBRARIES} ${OpenCV_LIBS} 92 | ) 93 | 94 | ## Mark executables and/or libraries for installation 95 | install(TARGETS movebase_state_service 96 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 97 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 98 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 99 | ) 100 | -------------------------------------------------------------------------------- /strands_movebase/strands_movebase_params/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | global_frame: /map 2 | robot_base_frame: /base_link 3 | 4 | map_type: voxel 5 | 6 | footprint: [[-0.211159,-0.295000], 7 | [0.013709,-0.297036], 8 | [0.102681,-0.254027], 9 | [0.199937,-0.143212], 10 | [0.231037,-0.049411], 11 | [0.219249,0.097558], 12 | [0.140899,0.222458], 13 | [0.059921,0.279099], 14 | [-0.034768,0.307378], 15 | [-0.211159,0.295000], 16 | [-0.511159,0.100000], 17 | [-0.511159,-0.100000], 18 | [-0.311159,-0.250000]] 19 | 20 | #[[-0.30,-0.32], # Back side right 21 | #[-0.52,-0.11], # Back right 22 | #[-0.52, 0.11], # Back left 23 | #[-0.30, 0.32], # Back side left 24 | #[-0.05, 0.31], # Front side left 25 | #[ 0.05, 0.25], # Thick front left 26 | #[ 0.15, 0.13], # Thin front left 27 | #[ 0.20, 0.00], # Front 28 | #[ 0.15,-0.13], # Thin front right 29 | #[ 0.05,-0.25], # Thick front right 30 | #[-0.0 31 | #[-5,-0.31]] # Front side right 32 | #footprint: [[-0.211159,-0.295],[-0.0842764,-0.309861],[-0.0347691,-0.307378],[0.0137095,-0.297036],[0.0599199,-0.2791],[0.102681,-0.254027],[0.140898,-0.222459],[0.173596,-0.185203],[0.199937,-0.143212],[0.219249,-0.0975586],[0.231037,-0.0494112],[0.235,-3.89256e-07],[0.231037,0.0494104],[0.219249,0.0975578],[0.199938,0.143211],[0.173596,0.185202],[0.140899,0.222458],[0.102681,0.254026],[0.0599206,0.279099],[0.0137102,0.297036],[-0.0347683,0.307378],[-0.0842756,0.309861],[-0.211159,0.295],[-0.311159,0.25],[-0.411159,0.185],[-0.511159,0.10],[-0.515,0],[-0.511159,-0.10],[-0.411159,-0.185],[-0.311159,-0.25]] 33 | 34 | footprint_padding: 0.0001 35 | 36 | map_layer: 37 | map_topic: /map 38 | # unknown_cost_value: 100 39 | 40 | no_go_layer: 41 | map_topic: /no_go_map 42 | 43 | global_inflation_layer: 44 | inflation_radius: 0.8 45 | cost_scaling_factor: 5.0 46 | 47 | local_inflation_layer: 48 | inflation_radius: 0.2 49 | cost_scaling_factor: 5.0 50 | 51 | obstacle_layer: 52 | max_obstacle_height: 2.0 53 | obstacle_range: 2.5 54 | raytrace_range: 3.0 55 | 56 | origin_z: -0.08 57 | z_resolution: 0.2 58 | z_voxels: 6 59 | unknown_threshold: 6 60 | mark_threshold: 0 61 | 62 | track_unknown_space: true 63 | 64 | observation_sources: laser_obstacle laser_clearing point_cloud_sensor clear_sensor cliff_sensor head_cloud_sensor head_clear_sensor 65 | 66 | laser_obstacle: {sensor_frame: base_laser_link, data_type: LaserScan, topic: /move_base/scan_obstacle, marking: true, clearing: false} 67 | 68 | laser_clearing: {sensor_frame: base_laser_link, data_type: LaserScan, topic: scan, marking: false, clearing: true} 69 | 70 | point_cloud_sensor: {topic: /move_base/points_obstacle, data_type: PointCloud2, marking: true, clearing: false, min_obstacle_height: 0.1, max_obstacle_height: 0.9, observation_persistence: 0.0, obstacle_range: 2.0} 71 | 72 | clear_sensor: {topic: /move_base/points_clearing, data_type: PointCloud2, marking: false, clearing: true, min_obstacle_height: -0.1, max_obstacle_height: 1.1, observation_persistence: 0.0, raytrace_range: 4.0} 73 | cliff_sensor: {topic: /move_base/points_cliff, data_type: PointCloud2, marking: true, clearing: false, min_obstacle_height: 0.1, max_obstacle_height: 0.15, observation_persistence: 0.0, obstacle_range: 3.0} 74 | 75 | head_cloud_sensor: {topic: /move_base/head_points_obstacle, data_type: PointCloud2, marking: true, clearing: false, min_obstacle_height: 0.3, max_obstacle_height: 0.9, observation_persistence: 0.0, obstacle_range: 3.0} 76 | head_clear_sensor: {topic: /move_base/head_points_clearing, data_type: PointCloud2, marking: false, clearing: true, min_obstacle_height: -0.2, max_obstacle_height: 1.1, observation_persistence: 0.0, raytrace_range: 5.0} 77 | -------------------------------------------------------------------------------- /strands_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(strands_description) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED) 8 | 9 | ## System dependencies are found with CMake's conventions 10 | # find_package(Boost REQUIRED COMPONENTS system) 11 | 12 | 13 | ## Uncomment this if the package has a setup.py. This macro ensures 14 | ## modules and global scripts declared therein get installed 15 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 16 | # catkin_python_setup() 17 | 18 | ####################################### 19 | ## Declare ROS messages and services ## 20 | ####################################### 21 | 22 | ## Generate messages in the 'msg' folder 23 | # add_message_files( 24 | # FILES 25 | # Message1.msg 26 | # Message2.msg 27 | # ) 28 | 29 | ## Generate services in the 'srv' folder 30 | # add_service_files( 31 | # FILES 32 | # Service1.srv 33 | # Service2.srv 34 | # ) 35 | 36 | ## Generate added messages and services with any dependencies listed here 37 | # generate_messages( 38 | # DEPENDENCIES 39 | # std_msgs # Or other packages containing msgs 40 | # ) 41 | 42 | ################################### 43 | ## catkin specific configuration ## 44 | ################################### 45 | ## The catkin_package macro generates cmake config files for your package 46 | ## Declare things to be passed to dependent projects 47 | ## LIBRARIES: libraries you create in this project that dependent projects also need 48 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 49 | ## DEPENDS: system dependencies of this project that dependent projects also need 50 | catkin_package( 51 | # INCLUDE_DIRS include 52 | # LIBRARIES scitos_description 53 | # CATKIN_DEPENDS other_catkin_pkg 54 | # DEPENDS system_lib 55 | ) 56 | 57 | ########### 58 | ## Build ## 59 | ########### 60 | 61 | ## Specify additional locations of header files 62 | ## Your package locations should be listed before other locations 63 | # include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) 64 | 65 | ## Declare a cpp library 66 | # add_library(scitos_description 67 | # src/${PROJECT_NAME}/scitos_description.cpp 68 | # ) 69 | 70 | ## Declare a cpp executable 71 | # add_executable(scitos_description_node src/scitos_description_node.cpp) 72 | 73 | ## Add cmake target dependencies of the executable/library 74 | ## as an example, message headers may need to be generated before nodes 75 | # add_dependencies(scitos_description_node scitos_description_generate_messages_cpp) 76 | 77 | ## Specify libraries to link a library or executable target against 78 | # target_link_libraries(scitos_description_node 79 | # ${catkin_LIBRARIES} 80 | # ) 81 | 82 | ############# 83 | ## Install ## 84 | ############# 85 | 86 | # all install targets should use catkin DESTINATION variables 87 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 88 | 89 | ## Mark executable scripts (Python etc.) for installation 90 | ## in contrast to setup.py, you can choose the destination 91 | # install(PROGRAMS 92 | # scripts/my_python_script 93 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 94 | # ) 95 | 96 | ## Mark executables and/or libraries for installation 97 | # install(TARGETS scitos_description scitos_description_node 98 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 99 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 100 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 101 | # ) 102 | 103 | install(DIRECTORY urdf/ 104 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf 105 | ) 106 | 107 | 108 | 109 | install(DIRECTORY launch/ 110 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 111 | ) 112 | 113 | -------------------------------------------------------------------------------- /strands_navfn/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(strands_navfn) 3 | 4 | find_package(catkin REQUIRED 5 | COMPONENTS 6 | cmake_modules 7 | costmap_2d 8 | geometry_msgs 9 | nav_core 10 | nav_msgs 11 | pcl_conversions 12 | pcl_ros 13 | pluginlib 14 | roscpp 15 | tf 16 | visualization_msgs 17 | ) 18 | 19 | find_package(Eigen REQUIRED) 20 | find_package(PCL REQUIRED) 21 | include_directories( 22 | include 23 | ${catkin_INCLUDE_DIRS} 24 | SYSTEM 25 | ${EIGEN_INCLUDE_DIRS} 26 | ${PCL_INCLUDE_DIRS} 27 | ) 28 | add_definitions(${EIGEN_DEFINITIONS}) 29 | 30 | 31 | # services 32 | add_service_files( 33 | DIRECTORY srv 34 | FILES 35 | MakeNavPlan.srv 36 | SetCostmap.srv 37 | ) 38 | 39 | generate_messages( 40 | DEPENDENCIES 41 | std_msgs 42 | geometry_msgs 43 | nav_msgs 44 | ) 45 | 46 | catkin_package( 47 | INCLUDE_DIRS 48 | include 49 | LIBRARIES 50 | strands_navfn 51 | CATKIN_DEPENDS 52 | nav_core 53 | roscpp 54 | pluginlib 55 | ) 56 | 57 | add_library (strands_navfn src/strands_navfn.cpp src/strands_navfn_ros.cpp) 58 | target_link_libraries(strands_navfn 59 | ${catkin_LIBRARIES} 60 | ) 61 | add_dependencies(strands_navfn ${PROJECT_NAME}_generate_messages_cpp ${catkin_EXPORTED_TARGETS}) 62 | 63 | add_executable(strands_navfn_node src/strands_navfn_node.cpp) 64 | target_link_libraries(strands_navfn_node 65 | strands_navfn 66 | ) 67 | 68 | install(TARGETS strands_navfn strands_navfn_node 69 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 70 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 71 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 72 | ) 73 | 74 | install(DIRECTORY include/strands_navfn/ 75 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 76 | ) 77 | 78 | install(FILES bgp_plugin.xml 79 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 80 | ) 81 | 82 | ### The problem with FindFLTK is that it only reports success if *all* 83 | ### fltk components are installed, but we only need the core library. 84 | # include (FindFLTK) 85 | include (CheckIncludeFileCXX) 86 | check_include_file_cxx (FL/Fl.H NAVFN_HAVE_FLTK) 87 | check_include_file_cxx (pgm.h NAVFN_HAVE_NETPBM) 88 | message (STATUS "NAVFN_HAVE_FLTK: ${NAVFN_HAVE_FLTK}, NETPBM: ${NAVFN_HAVE_NETPBM}") 89 | # Just linking -lfltk is not sufficient on OS X 90 | if (NAVFN_HAVE_FLTK AND NAVFN_HAVE_NETPBM AND NOT APPLE) 91 | message (STATUS "FLTK found: adding navtest to build") 92 | add_executable (strands_navtest src/strands_navtest.cpp src/strands_navwin.cpp) 93 | set (FLTK_SKIP_FLUID 1) 94 | set (FLTK_SKIP_FORMS 1) 95 | set (FLTK_SKIP_IMAGES 1) 96 | find_package(FLTK) 97 | if(FLTK_FOUND) 98 | target_link_libraries (strands_navtest strands_navfn netpbm ${FLTK_LIBRARIES}) 99 | else (FLTK_FOUND) 100 | target_link_libraries (strands_navtest strands_navfn netpbm fltk) 101 | endif (FLTK_FOUND) 102 | else (NAVFN_HAVE_FLTK) 103 | message (STATUS "FLTK orf NETPBM not found: cannot build navtest") 104 | endif (NAVFN_HAVE_FLTK AND NAVFN_HAVE_NETPBM AND NOT APPLE) 105 | 106 | ### For some reason (on cmake-2.4.7 at least) the "check" for pgm.h 107 | ### always succeeds, even if pgm.h is not installed. It seems to be 108 | ### caused by a bug in the rule that attempts to build the C source: 109 | ### instead of directly calling e.g. 'gcc -c 110 | ### /CMakeFiles/CMakeTmp/CheckIncludeFile.c' it goes through some make 111 | ### infrastructure, which reports "Nothing to be done for 112 | ### `CMakeFiles/cmTryCompileExec.dir/build'" and calls that a success. 113 | ### 114 | ### As a workaround we simply force everyone to install libnetpbm 115 | # 116 | # include (CheckIncludeFile) 117 | # check_include_file (pgm.h NAVFN_HAVE_NETPBM) 118 | # message (STATUS "NAVFN_HAVE_NETPBM: ${NAVFN_HAVE_NETPBM}") 119 | # if (NAVFN_HAVE_NETPBM) 120 | # message (STATUS "found pgm.h") 121 | # add_definitions (-DNAVFN_HAVE_NETPBM) 122 | #target_link_libraries (navfn -lnetpbm) 123 | # else (NAVFN_HAVE_NETPBM) 124 | # message (STATUS "pgm.h not found (optional)") 125 | # endif (NAVFN_HAVE_NETPBM) 126 | 127 | if(CATKIN_ENABLE_TESTING) 128 | add_subdirectory(test) 129 | endif() 130 | -------------------------------------------------------------------------------- /strands_movebase/launch/pointcloud_processing.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 | 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 | -------------------------------------------------------------------------------- /strands_navfn/src/strands_read_pgm_costmap.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | #ifdef __APPLE__ 35 | # include 36 | #else 37 | extern "C" { 38 | #include 39 | // pgm.h is not very friendly with system headers... need to undef max() and min() afterwards 40 | #include 41 | #undef max 42 | #undef min 43 | } 44 | #endif 45 | 46 | void 47 | setcostobs(COSTTYPE *cmap, int n, int w) 48 | { 49 | int CS = 11; 50 | for (int i=-CS/2; i= 0; --jj) 107 | { 108 | int v = row[jj]; 109 | cmap[ii*ncols+jj] = v; 110 | if (v >= COST_OBS_ROS) 111 | otot++; 112 | if (v == 0) 113 | ftot++; 114 | } 115 | } 116 | else 117 | { 118 | ftot = ncols*nrows; 119 | for (int jj(ncols - 1); jj >= 0; --jj) 120 | { 121 | if (row[jj] < unknown_gray && ii < nrows-7 && ii > 7) 122 | { 123 | setcostobs(cmap,ii*ncols+jj,ncols); 124 | otot++; 125 | ftot--; 126 | } 127 | else if (row[jj] <= unknown_gray) 128 | { 129 | setcostunk(cmap,ii*ncols+jj,ncols); 130 | utot++; 131 | ftot--; 132 | } 133 | } 134 | } 135 | } 136 | printf("readPGM() Found %d obstacle cells, %d free cells, %d unknown cells\n", otot, ftot, utot); 137 | pgm_freerow(row); 138 | *width = ncols; 139 | *height = nrows; 140 | return cmap; 141 | } 142 | -------------------------------------------------------------------------------- /strands_movebase/src/mirror_floor_points.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | ros::Publisher obstacle_pub; 9 | ros::Publisher floor_pub; 10 | 11 | pcl::PointCloud::Ptr floor_cloud; 12 | pcl::PointCloud::Ptr obstacle_cloud; 13 | 14 | float below_threshold; 15 | tf::TransformListener* listener; 16 | 17 | void callback(const sensor_msgs::PointCloud2::ConstPtr& msg) 18 | { 19 | std::string base_frame("base_link"); 20 | 21 | geometry_msgs::PointStamped pout; 22 | geometry_msgs::PointStamped pin; 23 | pin.header.frame_id = msg->header.frame_id; 24 | pin.point.x = 0; pin.point.y = 0; pin.point.z = 0; 25 | geometry_msgs::Vector3Stamped vout; 26 | geometry_msgs::Vector3Stamped vin; 27 | vin.header.frame_id = base_frame; 28 | vin.vector.x = 0; vin.vector.y = 0; vin.vector.z = 1; 29 | 30 | float height; 31 | Eigen::Vector3f normal; 32 | try { 33 | listener->transformPoint(base_frame, ros::Time(0), pin, msg->header.frame_id, pout); 34 | height = pout.point.z; 35 | listener->transformVector(msg->header.frame_id, ros::Time(0), vin, base_frame, vout); 36 | normal = Eigen::Vector3f(vout.vector.x, vout.vector.y, vout.vector.z); 37 | } 38 | catch (tf::TransformException ex) { 39 | ROS_INFO("%s",ex.what()); 40 | return; 41 | } 42 | 43 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); 44 | pcl::fromROSMsg(*msg, *cloud); 45 | 46 | Eigen::Vector3f p = -height*normal; // a point in the floor plane 47 | float d = -p.dot(normal); // = height, d in the plane equation 48 | 49 | obstacle_cloud->points.clear(); 50 | obstacle_cloud->points.reserve(cloud->size()); 51 | floor_cloud->points.clear(); 52 | Eigen::Vector3f temp; 53 | float floor_dist; 54 | pcl::PointXYZ point; 55 | for (size_t i = 0; i < cloud->size(); ++i) { 56 | 57 | /*temp = cloud->points[i].getVector3fMap(); // DEBUG! 58 | 59 | if (i%640 < 300 && i%640 > 200 && i < 640*200 && i > 640*100) { 60 | temp -= 0.06*normal; 61 | }*/ 62 | 63 | // check signed distance to floor 64 | floor_dist = normal.dot(cloud->points[i].getVector3fMap()) + d; 65 | //floor_dist = normal.dot(temp) + d; // DEBUG 66 | 67 | // if enough below, consider a stair point 68 | if (floor_dist < below_threshold) { 69 | temp = cloud->points[i].getVector3fMap(); // RELEASE 70 | point.getVector3fMap() = -(d/normal.dot(temp))*temp + normal*0.11; 71 | floor_cloud->points.push_back(point); 72 | } 73 | else { // add as a normal obstacle or clearing point 74 | obstacle_cloud->points.push_back(cloud->points[i]); 75 | } 76 | } 77 | 78 | sensor_msgs::PointCloud2 floor_msg; 79 | pcl::toROSMsg(*floor_cloud, floor_msg); 80 | floor_msg.header = msg->header; 81 | 82 | floor_pub.publish(floor_msg); 83 | 84 | sensor_msgs::PointCloud2 obstacle_msg; 85 | pcl::toROSMsg(*obstacle_cloud, obstacle_msg); 86 | obstacle_msg.header = msg->header; 87 | 88 | obstacle_pub.publish(obstacle_msg); 89 | } 90 | 91 | int main(int argc, char** argv) 92 | { 93 | ros::init(argc, argv, "subsample_cloud"); 94 | ros::NodeHandle n; 95 | 96 | floor_cloud = pcl::PointCloud::Ptr(new pcl::PointCloud()); 97 | obstacle_cloud = pcl::PointCloud::Ptr(new pcl::PointCloud()); 98 | 99 | ros::NodeHandle pn("~"); 100 | // topic of input cloud 101 | if (!pn.hasParam("input")) { 102 | ROS_ERROR("Could not find parameter input."); 103 | return -1; 104 | } 105 | std::string input; 106 | pn.getParam("input", input); 107 | 108 | // topic of output cloud 109 | if (!pn.hasParam("obstacle_output")) { 110 | ROS_ERROR("Could not find parameter obstacle_output."); 111 | return -1; 112 | } 113 | std::string obstacle_output; 114 | pn.getParam("obstacle_output", obstacle_output); 115 | 116 | // topic of output cloud 117 | if (!pn.hasParam("floor_output")) { 118 | ROS_ERROR("Could not find parameter floor_output."); 119 | return -1; 120 | } 121 | std::string floor_output; 122 | pn.getParam("floor_output", floor_output); 123 | 124 | double bt; 125 | pn.param("below_threshold", bt, 0.05); 126 | below_threshold = -bt; 127 | 128 | listener = new tf::TransformListener(); 129 | 130 | ros::Subscriber sub = n.subscribe(input, 1, callback); 131 | obstacle_pub = n.advertise(obstacle_output, 1); 132 | floor_pub = n.advertise(floor_output, 1); 133 | 134 | ros::spin(); 135 | 136 | return 0; 137 | } 138 | -------------------------------------------------------------------------------- /strands_navfn/src/strands_navfn_node.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2008, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the Willow Garage nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: Bhaskara Marthi 36 | *********************************************************************/ 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | namespace cm=costmap_2d; 43 | namespace rm=geometry_msgs; 44 | 45 | using std::vector; 46 | using rm::PoseStamped; 47 | using std::string; 48 | using cm::Costmap2D; 49 | using cm::Costmap2DROS; 50 | 51 | namespace strands_navfn { 52 | 53 | class NavfnWithCostmap : public NavfnROS 54 | { 55 | public: 56 | NavfnWithCostmap(string name, Costmap2DROS* cmap); 57 | bool makePlanService(MakeNavPlan::Request& req, MakeNavPlan::Response& resp); 58 | 59 | private: 60 | void poseCallback(const rm::PoseStamped::ConstPtr& goal); 61 | Costmap2DROS* cmap_; 62 | ros::ServiceServer make_plan_service_; 63 | ros::Subscriber pose_sub_; 64 | }; 65 | 66 | 67 | bool NavfnWithCostmap::makePlanService(MakeNavPlan::Request& req, MakeNavPlan::Response& resp) 68 | { 69 | vector path; 70 | 71 | req.start.header.frame_id = "/map"; 72 | req.goal.header.frame_id = "/map"; 73 | bool success = makePlan(req.start, req.goal, path); 74 | resp.plan_found = success; 75 | if (success) { 76 | resp.path = path; 77 | } 78 | 79 | return true; 80 | } 81 | 82 | void NavfnWithCostmap::poseCallback(const rm::PoseStamped::ConstPtr& goal) { 83 | tf::Stamped global_pose; 84 | cmap_->getRobotPose(global_pose); 85 | vector path; 86 | rm::PoseStamped start; 87 | start.header.stamp = global_pose.stamp_; 88 | start.header.frame_id = global_pose.frame_id_; 89 | start.pose.position.x = global_pose.getOrigin().x(); 90 | start.pose.position.y = global_pose.getOrigin().y(); 91 | start.pose.position.z = global_pose.getOrigin().z(); 92 | start.pose.orientation.x = global_pose.getRotation().x(); 93 | start.pose.orientation.y = global_pose.getRotation().y(); 94 | start.pose.orientation.z = global_pose.getRotation().z(); 95 | start.pose.orientation.w = global_pose.getRotation().w(); 96 | makePlan(start, *goal, path); 97 | } 98 | 99 | 100 | NavfnWithCostmap::NavfnWithCostmap(string name, Costmap2DROS* cmap) : 101 | NavfnROS(name, cmap) 102 | { 103 | ros::NodeHandle private_nh("~"); 104 | cmap_ = cmap; 105 | make_plan_service_ = private_nh.advertiseService("make_plan", &NavfnWithCostmap::makePlanService, this); 106 | pose_sub_ = private_nh.subscribe("goal", 1, &NavfnWithCostmap::poseCallback, this); 107 | } 108 | 109 | } // namespace 110 | 111 | int main (int argc, char** argv) 112 | { 113 | ros::init(argc, argv, "global_planner"); 114 | 115 | tf::TransformListener tf(ros::Duration(10)); 116 | 117 | costmap_2d::Costmap2DROS lcr("costmap", tf); 118 | 119 | strands_navfn::NavfnWithCostmap strands_navfn("navfn_planner", &lcr); 120 | 121 | ros::spin(); 122 | return 0; 123 | } 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | -------------------------------------------------------------------------------- /param_loader/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(param_loader) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | dynamic_reconfigure 9 | rospy 10 | ) 11 | 12 | ## System dependencies are found with CMake's conventions 13 | # find_package(Boost REQUIRED COMPONENTS system) 14 | 15 | 16 | ## Uncomment this if the package has a setup.py. This macro ensures 17 | ## modules and global scripts declared therein get installed 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 19 | #catkin_python_setup() 20 | 21 | ################################################ 22 | ## Declare ROS messages, services and actions ## 23 | ################################################ 24 | 25 | ## To declare and build messages, services or actions from within this 26 | ## package, follow these steps: 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 28 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 29 | ## * In the file package.xml: 30 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 31 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 32 | ## pulled in transitively but can be declared for certainty nonetheless: 33 | ## * add a build_depend tag for "message_generation" 34 | ## * add a run_depend tag for "message_runtime" 35 | ## * In this file (CMakeLists.txt): 36 | ## * add "message_generation" and every package in MSG_DEP_SET to 37 | ## find_package(catkin REQUIRED COMPONENTS ...) 38 | ## * add "message_runtime" and every package in MSG_DEP_SET to 39 | ## catkin_package(CATKIN_DEPENDS ...) 40 | ## * uncomment the add_*_files sections below as needed 41 | ## and list every .msg/.srv/.action file to be processed 42 | ## * uncomment the generate_messages entry below 43 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 44 | 45 | ## Generate messages in the 'msg' folder 46 | # add_message_files( 47 | # FILES 48 | # Message1.msg 49 | # Message2.msg 50 | # ) 51 | 52 | ## Generate services in the 'srv' folder 53 | # add_service_files( 54 | # FILES 55 | # Service1.srv 56 | # Service2.srv 57 | # ) 58 | 59 | ## Generate actions in the 'action' folder 60 | # add_action_files( 61 | # FILES 62 | # Action1.action 63 | # Action2.action 64 | # ) 65 | 66 | ## Generate added messages and services with any dependencies listed here 67 | # generate_messages( 68 | # DEPENDENCIES 69 | # std_msgs # Or other packages containing msgs 70 | # ) 71 | 72 | ################################### 73 | ## catkin specific configuration ## 74 | ################################### 75 | ## The catkin_package macro generates cmake config files for your package 76 | ## Declare things to be passed to dependent projects 77 | ## INCLUDE_DIRS: uncomment this if you package contains header files 78 | ## LIBRARIES: libraries you create in this project that dependent projects also need 79 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 80 | ## DEPENDS: system dependencies of this project that dependent projects also need 81 | catkin_package( 82 | # INCLUDE_DIRS include 83 | # LIBRARIES param_loader 84 | # CATKIN_DEPENDS dynamic_reconfigure rospy 85 | # DEPENDS system_lib 86 | ) 87 | 88 | ########### 89 | ## Build ## 90 | ########### 91 | 92 | ## Specify additional locations of header files 93 | ## Your package locations should be listed before other locations 94 | # include_directories(include) 95 | include_directories( 96 | ${catkin_INCLUDE_DIRS} 97 | ) 98 | 99 | ## Declare a cpp library 100 | # add_library(param_loader 101 | # src/${PROJECT_NAME}/param_loader.cpp 102 | # ) 103 | 104 | ## Declare a cpp executable 105 | # add_executable(param_loader_node src/param_loader_node.cpp) 106 | 107 | ## Add cmake target dependencies of the executable/library 108 | ## as an example, message headers may need to be generated before nodes 109 | # add_dependencies(param_loader_node param_loader_generate_messages_cpp) 110 | 111 | ## Specify libraries to link a library or executable target against 112 | # target_link_libraries(param_loader_node 113 | # ${catkin_LIBRARIES} 114 | # ) 115 | 116 | ############# 117 | ## Install ## 118 | ############# 119 | 120 | install( 121 | DIRECTORY config 122 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 123 | ) 124 | 125 | install(PROGRAMS scripts/param_loader.py 126 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 127 | ) 128 | 129 | ############# 130 | ## Testing ## 131 | ############# 132 | 133 | ## Add gtest based cpp test target and link libraries 134 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_param_loader.cpp) 135 | # if(TARGET ${PROJECT_NAME}-test) 136 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 137 | # endif() 138 | 139 | ## Add folders to be run by python nosetests 140 | # catkin_add_nosetests(test) 141 | -------------------------------------------------------------------------------- /strands_navfn/test/strands_path_calc_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2012, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | // Load a willow garage costmap and return a NavFn instance using it. 37 | // If the costmap file fails to load, returns NULL. 38 | strands_navfn::NavFn* make_willow_nav() 39 | { 40 | int sx,sy; 41 | std::string path = ros::package::getPath( ROS_PACKAGE_NAME ) + "/test/willow_costmap.pgm"; 42 | 43 | COSTTYPE *cmap = readPGM( path.c_str(), &sx, &sy, true ); 44 | if( cmap == NULL ) 45 | { 46 | return NULL; 47 | } 48 | strands_navfn::NavFn* nav = new strands_navfn::NavFn(sx,sy); 49 | 50 | nav->priInc = 2*COST_NEUTRAL; // thin wavefront 51 | 52 | memcpy( nav->costarr, cmap, sx*sy ); 53 | 54 | return nav; 55 | } 56 | 57 | void print_neighborhood_of_last_path_entry( strands_navfn::NavFn* nav ) 58 | { 59 | printf("last path entries:\n"); 60 | for( int i = nav->npath - 4; i < nav->npath; i++ ) 61 | { 62 | printf("%.3f, %.3f\n", nav->pathx[ i ], nav->pathy[ i ]); 63 | } 64 | printf("potential field neighborhood of last entry:\n"); 65 | int xf = nav->pathx[ nav->npath-1 ]; 66 | int yf = nav->pathy[ nav->npath-1 ]; 67 | 68 | printf( " " ); 69 | for( int x = xf - 2; x <= xf + 2; x++ ) 70 | { 71 | printf( " %6d", x ); 72 | } 73 | printf( "\n" ); 74 | 75 | for( int y = yf - 2; y <= yf + 2; y++ ) 76 | { 77 | printf( "%5d:", y ); 78 | for( int x = xf - 2; x <= xf + 2; x++ ) 79 | { 80 | printf( " %5.1f", nav->potarr[ y * nav->nx + x ] ); 81 | } 82 | printf( "\n" ); 83 | } 84 | 85 | printf("gradient neighborhood of last entry:\n"); 86 | printf( " " ); 87 | for( int x = xf - 2; x <= xf + 2; x++ ) 88 | { 89 | printf( " %6d", x ); 90 | } 91 | printf( "\n" ); 92 | 93 | for( int y = yf - 2; y <= yf + 2; y++ ) 94 | { 95 | printf( "%5d x:", y ); 96 | for( int x = xf - 2; x <= xf + 2; x++ ) 97 | { 98 | printf( " %5.1f", nav->gradx[ y * nav->nx + x ] ); 99 | } 100 | printf( "\n" ); 101 | 102 | printf( " y:" ); 103 | for( int x = xf - 2; x <= xf + 2; x++ ) 104 | { 105 | printf( " %5.1f", nav->grady[ y * nav->nx + x ] ); 106 | } 107 | printf( "\n" ); 108 | } 109 | } 110 | 111 | TEST(PathCalc, oscillate_in_pinch_point) 112 | { 113 | strands_navfn::NavFn* nav = make_willow_nav(); 114 | ASSERT_TRUE( nav != NULL ); 115 | 116 | int goal[2]; 117 | int start[2]; 118 | 119 | start[0] = 428; 120 | start[1] = 746; 121 | 122 | goal[0] = 350; 123 | goal[1] = 450; 124 | 125 | nav->setGoal( goal ); 126 | nav->setStart( start ); 127 | 128 | bool plan_success = nav->calcNavFnDijkstra( true ); 129 | EXPECT_TRUE( plan_success ); 130 | if( !plan_success ) 131 | { 132 | print_neighborhood_of_last_path_entry( nav ); 133 | } 134 | } 135 | 136 | TEST(PathCalc, easy_nav_should_always_work) 137 | { 138 | strands_navfn::NavFn* nav = make_willow_nav(); 139 | ASSERT_TRUE( nav != NULL ); 140 | 141 | int goal[2]; 142 | int start[2]; 143 | 144 | start[0] = 350; 145 | start[1] = 400; 146 | 147 | goal[0] = 350; 148 | goal[1] = 450; 149 | 150 | nav->setGoal( goal ); 151 | nav->setStart( start ); 152 | 153 | EXPECT_TRUE( nav->calcNavFnDijkstra( true )); 154 | } 155 | 156 | int main(int argc, char **argv) 157 | { 158 | testing::InitGoogleTest(&argc, argv); 159 | return RUN_ALL_TESTS(); 160 | } 161 | -------------------------------------------------------------------------------- /calibrate_chest/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(calibrate_chest) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs std_msgs mongodb_store actionlib actionlib_msgs) 8 | 9 | ## System dependencies are found with CMake's conventions 10 | find_package(Boost REQUIRED COMPONENTS thread) 11 | 12 | find_package(PCL REQUIRED COMPONENTS common visualization) 13 | include_directories(${PCL_INCLUDE_DIRS}) 14 | link_directories(${PCL_LIBRARY_DIRS}) 15 | add_definitions(${PCL_DEFINITIONS}) 16 | 17 | 18 | ## Uncomment this if the package has a setup.py. This macro ensures 19 | ## modules and global scripts declared therein get installed 20 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 21 | # catkin_python_setup() 22 | 23 | ####################################### 24 | ## Declare ROS messages and services ## 25 | ####################################### 26 | 27 | ## Generate messages in the 'msg' folder 28 | # add_message_files( 29 | # FILES 30 | # Message1.msg 31 | # Message2.msg 32 | # ) 33 | 34 | ## Generate services in the 'srv' folder 35 | # add_service_files( 36 | # FILES 37 | # Service1.srv 38 | # Service2.srv 39 | # ) 40 | 41 | add_action_files( 42 | DIRECTORY action 43 | FILES CalibrateCamera.action 44 | ) 45 | 46 | ## Generate added messages and services with any dependencies listed here 47 | generate_messages( 48 | DEPENDENCIES 49 | std_msgs 50 | actionlib_msgs 51 | ) 52 | 53 | ################################### 54 | ## catkin specific configuration ## 55 | ################################### 56 | ## The catkin_package macro generates cmake config files for your package 57 | ## Declare things to be passed to dependent projects 58 | ## INCLUDE_DIRS: uncomment this if you package contains header files 59 | ## LIBRARIES: libraries you create in this project that dependent projects also need 60 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 61 | ## DEPENDS: system dependencies of this project that dependent projects also need 62 | catkin_package( 63 | # INCLUDE_DIRS include 64 | # LIBRARIES calibrate_chest 65 | # CATKIN_DEPENDS roscpp sensor_msgs std_msgs 66 | DEPENDS actionlib_msgs 67 | ) 68 | 69 | ########### 70 | ## Build ## 71 | ########### 72 | 73 | ## Specify additional locations of header files 74 | ## Your package locations should be listed before other locations 75 | # include_directories(include) 76 | include_directories( 77 | ${catkin_INCLUDE_DIRS} 78 | ) 79 | 80 | ## Declare a cpp library 81 | # add_library(calibrate_chest 82 | # src/${PROJECT_NAME}/calibrate_chest.cpp 83 | # ) 84 | 85 | ## Declare a cpp executable 86 | add_executable(calibrate_chest src/calibrate_chest.cpp) 87 | add_executable(chest_calibration_publisher src/chest_calibration_publisher.cpp) 88 | add_executable(calibration_server src/calibration_server.cpp) 89 | 90 | ## Add cmake target dependencies of the executable/library 91 | ## as an example, message headers may need to be generated before nodes 92 | add_dependencies(calibrate_chest mongodb_store_generate_messages_cpp) 93 | add_dependencies(calibration_server mongodb_store_generate_messages_cpp calibrate_chest_generate_messages_cpp) 94 | 95 | ## Specify libraries to link a library or executable target against 96 | target_link_libraries(calibrate_chest 97 | ${catkin_LIBRARIES} ${PCL_LIBRARIES} 98 | ) 99 | 100 | target_link_libraries(chest_calibration_publisher 101 | ${catkin_LIBRARIES} ${PCL_LIBRARIES} 102 | ) 103 | 104 | target_link_libraries(calibration_server 105 | ${catkin_LIBRARIES} ${PCL_LIBRARIES} 106 | ) 107 | 108 | ############# 109 | ## Install ## 110 | ############# 111 | 112 | # all install targets should use catkin DESTINATION variables 113 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 114 | 115 | ## Mark executable scripts (Python etc.) for installation 116 | ## in contrast to setup.py, you can choose the destination 117 | # install(PROGRAMS 118 | # scripts/my_python_script 119 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 120 | # ) 121 | 122 | ## Mark executables and/or libraries for installation 123 | install(TARGETS calibrate_chest chest_calibration_publisher calibration_server 124 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 125 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 126 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 127 | ) 128 | 129 | ## Mark cpp header files for installation 130 | # install(DIRECTORY include/${PROJECT_NAME}/ 131 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 132 | # FILES_MATCHING PATTERN "*.h" 133 | # PATTERN ".svn" EXCLUDE 134 | # ) 135 | 136 | ## Mark other files for installation (e.g. launch and bag files, etc.) 137 | # install(FILES 138 | # # myfile1 139 | # # myfile2 140 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 141 | # ) 142 | 143 | ############# 144 | ## Testing ## 145 | ############# 146 | 147 | ## Add gtest based cpp test target and link libraries 148 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_calibrate_chest.cpp) 149 | # if(TARGET ${PROJECT_NAME}-test) 150 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 151 | # endif() 152 | 153 | ## Add folders to be run by python nosetests 154 | # catkin_add_nosetests(test) 155 | -------------------------------------------------------------------------------- /calibrate_chest/src/calibrate_chest.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "mongodb_store/SetParam.h" 9 | 10 | ros::ServiceClient client; 11 | 12 | bool is_inlier(const Eigen::Vector3f& point, const Eigen::Vector4f plane, double threshold) 13 | { 14 | return fabs(point.dot(plane.segment<3>(0)) + plane(3)) < threshold; 15 | } 16 | 17 | void plot_best_plane(const pcl::PointCloud& points, const Eigen::Vector4f plane, double threshold) 18 | { 19 | pcl::PointCloud::Ptr inlier_cloud(new pcl::PointCloud()); 20 | pcl::PointCloud::Ptr outlier_cloud(new pcl::PointCloud()); 21 | 22 | for (int i = 0; i < points.size(); ++i) { 23 | if (is_inlier(points[i].getVector3fMap(), plane, threshold)) { 24 | inlier_cloud->push_back(points[i]); 25 | } 26 | else { 27 | outlier_cloud->push_back(points[i]); 28 | } 29 | } 30 | 31 | pcl::visualization::PCLVisualizer viewer("3D Viewer"); 32 | viewer.setBackgroundColor(0, 0, 0); 33 | viewer.addCoordinateSystem(1.0); 34 | viewer.initCameraParameters(); 35 | 36 | pcl::visualization::PointCloudColorHandlerCustom inlier_color_handler(inlier_cloud, 255, 0, 0); 37 | viewer.addPointCloud(inlier_cloud, inlier_color_handler, "inliers"); 38 | 39 | pcl::visualization::PointCloudColorHandlerCustom outlier_color_handler(outlier_cloud, 0, 0, 255); 40 | viewer.addPointCloud(outlier_cloud, outlier_color_handler, "outliers"); 41 | 42 | while (!viewer.wasStopped()) 43 | { 44 | viewer.spinOnce(100); 45 | boost::this_thread::sleep(boost::posix_time::microseconds(100000)); 46 | } 47 | 48 | } 49 | 50 | void compute_plane(Eigen::Vector4f& plane, const pcl::PointCloud& points, int* inds) 51 | { 52 | Eigen::Vector3f first = points[inds[1]].getVector3fMap() - points[inds[0]].getVector3fMap(); 53 | Eigen::Vector3f second = points[inds[2]].getVector3fMap() - points[inds[0]].getVector3fMap(); 54 | Eigen::Vector3f normal = first.cross(second); 55 | normal.normalize(); 56 | plane.segment<3>(0) = normal; 57 | plane(3) = -normal.dot(points[inds[0]].getVector3fMap()); 58 | } 59 | 60 | void extract_height_and_angle(const Eigen::Vector4f& plane) 61 | { 62 | ROS_INFO("Ground plane: %f, %f, %f, %f", plane(0), plane(1), plane(2), plane(3)); 63 | double dist = fabs(plane(3)/plane(2)); // distance along z axis 64 | double height = fabs(plane(3)/plane.segment<3>(0).squaredNorm()); // height 65 | ROS_INFO("Distance to plane along camera axis: %f", dist); 66 | ROS_INFO("Height above ground: %f", height); 67 | double angle = asin(height/dist); 68 | ROS_INFO("Angle radians: %f", angle); 69 | ROS_INFO("Angle degrees: %f", 180.0f*angle/M_PI); 70 | 71 | mongodb_store::SetParam srv; 72 | char buffer[250]; 73 | 74 | // store height above ground in datacentre 75 | ros::param::set("/chest_xtion_height", height); 76 | sprintf(buffer, "{\"path\":\"/chest_xtion_height\",\"value\":%f}", height); 77 | srv.request.param = buffer; 78 | if (!client.call(srv)) { 79 | ROS_ERROR("Failed to call set height, is config manager running?"); 80 | } 81 | 82 | // store angle between camera and horizontal plane 83 | ros::param::set("/chest_xtion_angle", angle); 84 | sprintf(buffer, "{\"path\":\"/chest_xtion_angle\",\"value\":%f}", angle); 85 | srv.request.param = buffer; 86 | if (!client.call(srv)) { 87 | ROS_ERROR("Failed to call set angle, is config manager running?"); 88 | } 89 | } 90 | 91 | void callback(const sensor_msgs::PointCloud2::ConstPtr& msg) 92 | { 93 | ROS_INFO("Got a pointcloud, calibrating..."); 94 | pcl::PointCloud cloud; 95 | pcl::fromROSMsg(*msg, cloud); 96 | 97 | int nbr = cloud.size(); 98 | 99 | int max = 1000; // ransac iterations 100 | double threshold = 0.02; // threshold for plane inliers 101 | 102 | Eigen::Vector4f best_plane; // best plane parameters found 103 | int best_inliers = -1; // best number of inliers 104 | 105 | int inds[3]; 106 | 107 | Eigen::Vector4f plane; 108 | int inliers; 109 | for (int i = 0; i < max; ++i) { 110 | 111 | for (int j = 0; j < 3; ++j) { 112 | inds[j] = rand() % nbr; // get a random point 113 | } 114 | 115 | // check that the points aren't the same 116 | if (inds[0] == inds[1] || inds[0] == inds[2] || inds[1] == inds[2]) { 117 | continue; 118 | } 119 | 120 | compute_plane(plane, cloud, inds); 121 | inliers = 0; 122 | for (int j = 0; j < nbr; j += 30) { // count number of inliers 123 | if (is_inlier(cloud[j].getVector3fMap(), plane, threshold)) { 124 | ++inliers; 125 | } 126 | } 127 | 128 | if (inliers > best_inliers) { 129 | best_plane = plane; 130 | best_inliers = inliers; 131 | } 132 | } 133 | 134 | extract_height_and_angle(best_plane); // find parameters and feed them to datacentre 135 | plot_best_plane(cloud, best_plane, threshold); // visually evaluate plane fit 136 | 137 | exit(0); 138 | } 139 | 140 | int main(int argc, char** argv) 141 | { 142 | ros::init(argc, argv, "calibrate_chest"); 143 | ros::NodeHandle n; 144 | 145 | std::string camera_topic = "chest_xtion"; 146 | client = n.serviceClient("/config_manager/set_param"); 147 | ros::Subscriber sub = n.subscribe(camera_topic + "/depth/points", 1, callback); 148 | 149 | ros::spin(); 150 | 151 | return 0; 152 | } 153 | -------------------------------------------------------------------------------- /strands_movebase/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package strands_movebase 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.0.23 (2016-02-08) 6 | ------------------- 7 | * Added proper install target for subsampling nodelet 8 | * Contributors: Nils Bore 9 | 10 | 0.0.22 (2016-01-28) 11 | ------------------- 12 | * remove scitos_2d_nav dependency. add amcl dependency 13 | * Merge branch 'indigo-devel' of https://github.com/bfalacerda/strands_movebase into indigo-devel 14 | * adding default="" for no_go_map 15 | * renaming obstacles.launch to pointcloud_processing.launch 16 | * new footprint + stop loading stuff from scitos_2d_nav 17 | * proper value for aggressive costmap reset distance 18 | * Switched to the nodelet version in the obstacles launch file 19 | * Added install targets 20 | * Added a new nodelet class to make the subsampling more efficient 21 | * old params 22 | * new params 23 | * param changes to dwa 24 | * Contributors: Bruno Lacerda, Nils Bore 25 | 26 | 0.0.21 (2015-04-27) 27 | ------------------- 28 | * Changed the machine tags to be able to launch the head camera processing nodes on the correct pc 29 | * Contributors: Nils Bore 30 | 31 | 0.0.18 (2015-03-28) 32 | ------------------- 33 | * Added dependency to state service since it is included in launch file 34 | * Contributors: Nils Bore 35 | 36 | 0.0.17 (2015-03-16) 37 | ------------------- 38 | * really correcting import 39 | * Merge branch 'hydro-devel' into indigo-devel 40 | * correcting imports 41 | * Avoiding planning through Unknown Areas by adding tracking of unknown obstacles and not allowing them 42 | * adding slight inflation to local costmap 43 | * Not copying the intensities vector in remove_edges_laser if there are no values in the input message 44 | * setting unknown_cost_value to 100 45 | * Contributors: Bruno Lacerda, Jaime Pulido Fentanes, Nils Bore 46 | 47 | 0.0.16 (2015-02-13) 48 | ------------------- 49 | 50 | 0.0.14 (2014-12-17) 51 | ------------------- 52 | * getting move base's own costmap clearance to clear the obstacle layer 53 | * reverting resolution as it is too much for the current depth cloud config and it creates a lot of lingering obstacles 54 | * new nav params 55 | * defaul user needs to be "" 56 | * Fixed indentation 57 | * Added remove_edges_laser to install targets 58 | * Made the calculation of the new points exact 59 | * Changed default arguments to make clear that they are floats 60 | * Added the new node in the launch file and included the decreased FOV obstacle adding laser in the sensor sources 61 | * Now it should work 62 | * Added a node that remove the edges of the laser 63 | * Contributors: Bruno Lacerda, Nils Bore 64 | 65 | 0.0.13 (2014-11-21) 66 | ------------------- 67 | * Made subsampling parameters configurable. 68 | * Whitespace only while I'm at it. 69 | * Contributors: Lucas Beyer 70 | 71 | 0.0.12 (2014-11-20) 72 | ------------------- 73 | * Set the obstacle adding thresholds to more reasonable values 74 | * Contributors: Nils Bore 75 | 76 | 0.0.11 (2014-11-19) 77 | ------------------- 78 | * Made the voxel map publish only for the local costmap as it might be demanding to publish the global 79 | * Fixed publishing of the occupied voxel map 80 | * Ability to include site-specific movebase parameters. 81 | * Contributors: Lucas Beyer, Nils Bore 82 | 83 | 0.0.10 (2014-11-19) 84 | ------------------- 85 | 86 | 0.0.9 (2014-11-19) 87 | ------------------ 88 | * fixing wrong machine tag 89 | * More precise footprint of the robot, including screen. 90 | * Cosmetic changes of `costmap_common_params.yaml`. 91 | * Renamed config.launch to obstacles.launch 92 | * Moved `move_base` to the main machine. 93 | Obstacle handling with the chest xtion still run on the chest xtion's pc. 94 | * Since *any* tag supports `if`, no need to dup nodes. 95 | * Purely syntactic changes to move_base/launch 96 | Because I'm allergic to working with inconsistent files. 97 | * Contributors: Jaime Pulido Fentanes, Lucas Beyer 98 | 99 | 0.0.8 (2014-11-12) 100 | ------------------ 101 | * Merge pull request `#13 `_ from nilsbore/backtrack 102 | [strands_movebase] Add option to use head_camera as well 103 | * Changed the unknown_threshold to 9 as well for consistency 104 | * Removed commented code 105 | * Removed unnecessary code and switched to the topics used in backtrack 106 | * Fixed the parameter settings, it actually works 107 | * Tidied up a bit and changed to the correct topic 108 | * Forgot to change the height 109 | * Added the option to have an extra head camera, with a higher voxelgrid 110 | * Contributors: Marc Hanheide, Nils Bore, Rares Ambrus 111 | 112 | 0.0.7 (2014-11-09) 113 | ------------------ 114 | * final and tested version of loader 115 | * new machine tags 116 | * Fixed typos in launch files 117 | * Added launch file options for changing the obstacle and stair heights to enable to tune the robustness 118 | * Contributors: Jaime Pulido Fentanes, Nils Bore, Rares Ambrus 119 | 120 | 0.0.6 (2014-10-27) 121 | ------------------ 122 | * Decreased the cost_scaling_factor, making the robot stay away from walls if possible. This made navigation more robust on Rosie, particularly through doors 123 | * Contributors: Nils Bore 124 | 125 | 0.0.5 (2014-10-23) 126 | ------------------ 127 | * added changelogs 128 | * Added launching of chest transform state publisher together with 3d movebase 129 | * Added the dependencies in catkin_package 130 | * Placed include files in include/strands_movebase and added install targets 131 | * Modified config.launch to use strands_movebase nodes and configs instead of scitos_2d_navigation 132 | * Renamed the launch files and made 3d obstacle avoidance the default 133 | * Corrected the homepage 134 | * Moved the headers to include folder 135 | * Removed move_base.launch since that will be in scitos_2d_navigation 136 | * Mad strands_movebase a package within the repo, to be able to put e.g. chest_calibration in another package 137 | * Contributors: Marc Hanheide, Nils Bore 138 | -------------------------------------------------------------------------------- /strands_movebase/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(strands_movebase) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED roscpp sensor_msgs std_msgs pcl_ros tf nodelet) 8 | 9 | ## System dependencies are found with CMake's conventions 10 | # find_package(Boost REQUIRED COMPONENTS system) 11 | 12 | find_package(PCL REQUIRED) 13 | include_directories(${PCL_INCLUDE_DIRS}) 14 | link_directories(${PCL_LIBRARY_DIRS}) 15 | add_definitions(${PCL_DEFINITIONS}) 16 | 17 | ## Uncomment this if the package has a setup.py. This macro ensures 18 | ## modules and global scripts declared therein get installed 19 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 20 | # catkin_python_setup() 21 | 22 | ####################################### 23 | ## Declare ROS messages and services ## 24 | ####################################### 25 | 26 | ## Generate messages in the 'msg' folder 27 | # add_message_files( 28 | # FILES 29 | # Message1.msg 30 | # Message2.msg 31 | # ) 32 | 33 | ## Generate services in the 'srv' folder 34 | # add_service_files( 35 | # FILES 36 | # Service1.srv 37 | # Service2.srv 38 | # ) 39 | 40 | ## Generate added messages and services with any dependencies listed here 41 | # generate_messages( 42 | # DEPENDENCIES 43 | # std_msgs # Or other packages containing msgs 44 | # ) 45 | 46 | ################################### 47 | ## catkin specific configuration ## 48 | ################################### 49 | ## The catkin_package macro generates cmake config files for your package 50 | ## Declare things to be passed to dependent projects 51 | ## LIBRARIES: libraries you create in this project that dependent projects also need 52 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 53 | ## DEPENDS: system dependencies of this project that dependent projects also need 54 | catkin_package( 55 | # INCLUDE_DIRS include 56 | # LIBRARIES subsample_cloud_nodelet 57 | CATKIN_DEPENDS roscpp sensor_msgs std_msgs pcl_ros tf 58 | DEPENDS libpcl-all-dev 59 | ) 60 | 61 | ########### 62 | ## Build ## 63 | ########### 64 | 65 | ## Specify additional locations of header files 66 | ## Your package locations should be listed before other locations 67 | # include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) 68 | include_directories( 69 | ${catkin_INCLUDE_DIRS} 70 | include 71 | ) 72 | 73 | ## Declare a cpp library 74 | #add_library(noise_voxel_grid src/noise_voxel_grid.cpp) 75 | add_library(noise_approximate_voxel_grid src/noise_approximate_voxel_grid.cpp include/strands_movebase/noise_approximate_voxel_grid.h) 76 | add_library(subsample_cloud_nodelet src/subsample_cloud_nodelet.cpp) 77 | 78 | ## Declare a cpp executable 79 | add_executable(subsample_cloud src/subsample_cloud.cpp) 80 | add_executable(remove_edges_cloud src/remove_edges_cloud.cpp) 81 | add_executable(remove_edges_laser src/remove_edges_laser.cpp) 82 | add_executable(mirror_floor_points src/mirror_floor_points.cpp) 83 | 84 | ## Add cmake target dependencies of the executable/library 85 | ## as an example, message headers may need to be generated before nodes 86 | # add_dependencies(scitos_2d_navigation_node scitos_2d_navigation_generate_messages_cpp) 87 | 88 | target_link_libraries(noise_approximate_voxel_grid 89 | ${PCL_LIBRARIES} 90 | ) 91 | 92 | target_link_libraries(subsample_cloud_nodelet 93 | ${catkin_LIBRARIES} ${PCL_LIBRARIES} noise_approximate_voxel_grid 94 | ) 95 | 96 | target_link_libraries(subsample_cloud 97 | ${catkin_LIBRARIES} ${PCL_LIBRARIES} noise_approximate_voxel_grid 98 | ) 99 | 100 | target_link_libraries(remove_edges_cloud 101 | ${catkin_LIBRARIES} ${PCL_LIBRARIES} 102 | ) 103 | 104 | target_link_libraries(remove_edges_laser 105 | ${catkin_LIBRARIES} 106 | ) 107 | 108 | target_link_libraries(mirror_floor_points 109 | ${catkin_LIBRARIES} ${PCL_LIBRARIES} 110 | ) 111 | 112 | 113 | ############# 114 | ## Install ## 115 | ############# 116 | 117 | # all install targets should use catkin DESTINATION variables 118 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 119 | 120 | ## Mark executable scripts (Python etc.) for installation 121 | ## in contrast to setup.py, you can choose the destination 122 | # install(PROGRAMS 123 | # scripts/my_python_script 124 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 125 | # ) 126 | 127 | ## Mark executables and/or libraries for installation 128 | install(TARGETS subsample_cloud remove_edges_cloud remove_edges_laser mirror_floor_points noise_approximate_voxel_grid 129 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 130 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 131 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 132 | ) 133 | 134 | install(TARGETS subsample_cloud_nodelet 135 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 136 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 137 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 138 | ) 139 | 140 | install(DIRECTORY launch/ 141 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 142 | ) 143 | 144 | install(DIRECTORY plugins/ 145 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/plugins 146 | ) 147 | 148 | install(DIRECTORY strands_movebase_params/ 149 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/strands_movebase_params 150 | ) 151 | 152 | ## Mark cpp header files for installation 153 | # install(DIRECTORY launch/${PROJECT_NAME}/ 154 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 155 | # FILES_MATCHING PATTERN "*.h" 156 | # PATTERN ".svn" EXCLUDE 157 | # ) 158 | 159 | ## Mark other files for installation (e.g. launch and bag files, etc.) 160 | install(FILES 161 | include/strands_movebase/noise_approximate_voxel_grid.h 162 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 163 | ) 164 | 165 | ############# 166 | ## Testing ## 167 | ############# 168 | 169 | ## Add gtest based cpp test target and link libraries 170 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_scitos_2d_navigation.cpp) 171 | # if(TARGET ${PROJECT_NAME}-test) 172 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 173 | # endif() 174 | 175 | ## Add folders to be run by python nosetests 176 | # catkin_add_nosetests(test) 177 | -------------------------------------------------------------------------------- /strands_movebase/src/noise_approximate_voxel_grid.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2009, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage, Inc. nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | * $Id$ 35 | * 36 | * Class more or less copied from the PCL approximate voxel grid class, 37 | * see www.pointclouds.org for details 38 | * 39 | */ 40 | 41 | #include "strands_movebase/noise_approximate_voxel_grid.h" 42 | 43 | #include 44 | #include 45 | #include 46 | 47 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 48 | void noise_approximate_voxel_grid::flush (PointCloud &output, size_t op, he *hhe, int rgba_index, int centroid_size) 49 | { 50 | hhe->centroid /= static_cast (hhe->count); 51 | pcl::for_each_type (pcl::xNdCopyEigenPointFunctor (hhe->centroid, output.points[op])); 52 | // ---[ RGB special case 53 | if (rgba_index >= 0) 54 | { 55 | // pack r/g/b into rgb 56 | float r = hhe->centroid[centroid_size-3], 57 | g = hhe->centroid[centroid_size-2], 58 | b = hhe->centroid[centroid_size-1]; 59 | int rgb = (static_cast (r)) << 16 | (static_cast (g)) << 8 | (static_cast (b)); 60 | memcpy (reinterpret_cast (&output.points[op]) + rgba_index, &rgb, sizeof (float)); 61 | } 62 | } 63 | 64 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 65 | void noise_approximate_voxel_grid::applyFilter (PointCloud &output) 66 | { 67 | int centroid_size = 4; 68 | if (downsample_all_data_) 69 | centroid_size = boost::mpl::size::value; 70 | 71 | // ---[ RGB special case 72 | std::vector fields; 73 | int rgba_index = -1; 74 | rgba_index = pcl::getFieldIndex (*input_, "rgb", fields); 75 | if (rgba_index == -1) 76 | rgba_index = pcl::getFieldIndex (*input_, "rgba", fields); 77 | if (rgba_index >= 0) 78 | { 79 | rgba_index = fields[rgba_index].offset; 80 | centroid_size += 3; 81 | } 82 | 83 | for (size_t i = 0; i < histsize_; i++) 84 | { 85 | history_[i].count = 0; 86 | history_[i].centroid = Eigen::VectorXf::Zero (centroid_size); 87 | } 88 | Eigen::VectorXf scratch = Eigen::VectorXf::Zero (centroid_size); 89 | 90 | unsigned int new_size = input_->points.size() / skip_points; 91 | output.points.resize (new_size); // size output for worst case 92 | size_t op = 0; // output pointer 93 | for (size_t cp = 0; cp < input_->points.size (); cp += skip_points) 94 | { 95 | int ix = static_cast (floor (input_->points[cp].x * inverse_leaf_size_[0])); 96 | int iy = static_cast (floor (input_->points[cp].y * inverse_leaf_size_[1])); 97 | int iz = static_cast (floor (input_->points[cp].z * inverse_leaf_size_[2])); 98 | unsigned int hash = static_cast ((ix * 7171 + iy * 3079 + iz * 4231) & (histsize_ - 1)); 99 | he *hhe = &history_[hash]; 100 | if ((ix != hhe->ix) || (iy != hhe->iy) || (iz != hhe->iz)) 101 | { 102 | if (hhe->count >= min_points) { 103 | flush (output, op++, hhe, rgba_index, centroid_size); 104 | } 105 | hhe->count = 0; 106 | hhe->centroid.setZero ();// = Eigen::VectorXf::Zero (centroid_size); 107 | } 108 | hhe->ix = ix; 109 | hhe->iy = iy; 110 | hhe->iz = iz; 111 | hhe->count++; 112 | 113 | // Unpack the point into scratch, then accumulate 114 | // ---[ RGB special case 115 | if (rgba_index >= 0) 116 | { 117 | // fill r/g/b data 118 | pcl::RGB rgb; 119 | memcpy (&rgb, (reinterpret_cast (&input_->points[cp])) + rgba_index, sizeof (pcl::RGB)); 120 | scratch[centroid_size-3] = rgb.r; 121 | scratch[centroid_size-2] = rgb.g; 122 | scratch[centroid_size-1] = rgb.b; 123 | } 124 | pcl::for_each_type (pcl::xNdCopyPointEigenFunctor (input_->points[cp], scratch)); 125 | hhe->centroid += scratch; 126 | } 127 | for (size_t i = 0; i < histsize_; i++) 128 | { 129 | he *hhe = &history_[i]; 130 | if (hhe->count >= min_points) { 131 | flush (output, op++, hhe, rgba_index, centroid_size); 132 | //std::cout << "Number of points: " << hhe->count << std::endl; 133 | } 134 | else { 135 | //std::cout << "Skipped that one, too few" << std::endl; 136 | } 137 | } 138 | output.points.resize (op); 139 | output.width = static_cast (output.points.size ()); 140 | output.height = 1; // downsampling breaks the organized structure 141 | output.is_dense = false; // we filter out invalid points 142 | } 143 | 144 | -------------------------------------------------------------------------------- /strands_navfn/src/strands_navwin.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // simple timing test of the nav fn planner 3 | // 4 | 5 | #include 6 | #include 7 | 8 | namespace strands_navfn { 9 | NavWin::NavWin(int w, int h, const char *name) 10 | : Fl_Double_Window(w,h,name) 11 | { 12 | nw = w; 13 | nh = h; 14 | dec = 1; 15 | maxval = 90*1000; 16 | im = NULL; 17 | pw = w; 18 | ph = h; 19 | pce = pne = poe = 0; 20 | pc = pn = po = NULL; 21 | pathlen = pathbuflen = 0; 22 | path = NULL; 23 | } 24 | 25 | NavWin::~NavWin() 26 | { 27 | } 28 | 29 | void 30 | NavWin::drawPot(NavFn *nav) 31 | { 32 | float *pot = nav->potarr; 33 | COSTTYPE *cst = nav->costarr; 34 | int width = nav->nx; 35 | int height = nav->ny; 36 | 37 | // params 38 | pw = width; 39 | ph = height; 40 | 41 | // figure out decimation or expansion to fit 42 | dec = 1; 43 | inc = 1; 44 | 45 | if (width >= nw/2) 46 | { 47 | int ww = width; 48 | while (ww > nw) 49 | { 50 | dec++; 51 | ww = width/dec; 52 | } 53 | 54 | int hh = height/dec; 55 | while (hh > nh) 56 | { 57 | dec++; 58 | hh = height/dec; 59 | } 60 | 61 | if (im == NULL) 62 | im = new uchar[nw*nh*3]; 63 | 64 | 65 | // draw potential 66 | for (int i=0; i= COST_OBS) 89 | { 90 | *ii = 120; 91 | *(ii+1) = 60; 92 | *(ii+2) = 60; 93 | } 94 | } 95 | } 96 | 97 | } 98 | 99 | else // expand 100 | { 101 | int ww = width; 102 | while (ww < nw/2) 103 | { 104 | inc++; 105 | ww = width*inc; 106 | } 107 | 108 | int hh = height*inc; 109 | while (hh < nh/2) 110 | { 111 | inc++; 112 | hh = height*inc; 113 | } 114 | 115 | if (im == NULL) 116 | im = new uchar[nw*nh*3]; 117 | 118 | for (int i=0; i maxval) 126 | v = 255; 127 | else 128 | v = (int)((*pp/maxval) * 255.0); 129 | for (int k=0; kcurPe; 150 | pc = new int[pce]; 151 | memcpy(pc, nav->curP, pce*sizeof(int)); 152 | 153 | if (pn) 154 | delete [] pn; 155 | pne = nav->nextPe; 156 | pn = new int[pne]; 157 | memcpy(pn, nav->nextP, pne*sizeof(int)); 158 | 159 | if (po) 160 | delete [] po; 161 | poe = nav->overPe; 162 | po = new int[poe]; 163 | memcpy(po, nav->overP, poe*sizeof(int)); 164 | 165 | // start and goal 166 | goal[0] = nav->goal[0]; 167 | goal[1] = nav->goal[1]; 168 | start[0] = nav->start[0]; 169 | start[1] = nav->start[1]; 170 | 171 | // path 172 | if (nav->npath > 0) 173 | { 174 | pathlen = nav->npath; 175 | if (pathbuflen < pathlen) 176 | { 177 | pathbuflen = pathlen; 178 | if (path) delete [] path; 179 | path = new int[pathbuflen]; 180 | } 181 | for (int i=0; ipathy[i])+(int)(nav->pathx[i]); 183 | } 184 | 185 | drawOverlay(); 186 | 187 | redraw(); 188 | } 189 | 190 | 191 | void 192 | NavWin::drawOverlay() 193 | { 194 | if (inc == 1) // decimation 195 | { 196 | fl_color(255,0,0); 197 | if (pce > 0) 198 | for (int i=0; i 0) 206 | for (int i=0; i 0) 214 | for (int i=0; i 0) 225 | for (int i=0; i 0) 233 | for (int i=0; i 0) 241 | for (int i=0; i 0) // decimation or equal pixels 270 | { 271 | int y = path[0]/pw; 272 | int x = path[0]%pw; 273 | for (int i=1; i 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 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 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | -------------------------------------------------------------------------------- /strands_movebase/include/strands_movebase/noise_approximate_voxel_grid.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2009, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Willow Garage, Inc. nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | * $Id$ 35 | * 36 | * Class more or less copied from the PCL approximate voxel grid class, 37 | * see www.pointclouds.org for details 38 | * 39 | */ 40 | 41 | #ifndef NOISE_APPROXIMATE_VOXEL_GRID_H 42 | #define NOISE_APPROXIMATE_VOXEL_GRID_H 43 | 44 | #include 45 | #include 46 | #include 47 | 48 | /** \brief ApproximateVoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data. 49 | * 50 | * \author James Bowman, Radu B. Rusu 51 | * \ingroup filters 52 | */ 53 | class noise_approximate_voxel_grid : public pcl::Filter 54 | { 55 | typedef pcl::PointXYZ PointT; 56 | 57 | using pcl::Filter::filter_name_; 58 | using pcl::Filter::getClassName; 59 | using pcl::Filter::input_; 60 | using pcl::Filter::indices_; 61 | 62 | typedef typename pcl::Filter::PointCloud PointCloud; 63 | typedef typename PointCloud::Ptr PointCloudPtr; 64 | typedef typename PointCloud::ConstPtr PointCloudConstPtr; 65 | 66 | private: 67 | struct he 68 | { 69 | he () : ix (), iy (), iz (), count (0), centroid () {} 70 | int ix, iy, iz; 71 | int count; 72 | Eigen::VectorXf centroid; 73 | }; 74 | 75 | public: 76 | /** \brief Empty constructor. */ 77 | noise_approximate_voxel_grid (int min_points, int skip_points) : 78 | pcl::Filter (), 79 | leaf_size_ (Eigen::Vector3f::Ones ()), 80 | inverse_leaf_size_ (Eigen::Array3f::Ones ()), 81 | downsample_all_data_ (true), 82 | histsize_ (512), 83 | history_ (new he[histsize_]), 84 | min_points(min_points), 85 | skip_points(skip_points) 86 | { 87 | filter_name_ = "ApproximateVoxelGrid"; 88 | } 89 | 90 | /** \brief Copy constructor. 91 | * \param[in] src the approximate voxel grid to copy into this. 92 | */ 93 | noise_approximate_voxel_grid (const noise_approximate_voxel_grid &src) : 94 | pcl::Filter (), 95 | leaf_size_ (src.leaf_size_), 96 | inverse_leaf_size_ (src.inverse_leaf_size_), 97 | downsample_all_data_ (src.downsample_all_data_), 98 | histsize_ (src.histsize_), 99 | history_ (new he[histsize_]), 100 | min_points(src.min_points), 101 | skip_points(src.skip_points) 102 | { 103 | for (size_t i = 0; i < histsize_; i++) 104 | history_[i] = src.history_[i]; 105 | } 106 | 107 | ~noise_approximate_voxel_grid () 108 | { 109 | delete[] history_; 110 | } 111 | 112 | /** \brief Copy operator. 113 | * \param[in] src the approximate voxel grid to copy into this. 114 | */ 115 | inline noise_approximate_voxel_grid& 116 | operator = (const noise_approximate_voxel_grid &src) 117 | { 118 | leaf_size_ = src.leaf_size_; 119 | inverse_leaf_size_ = src.inverse_leaf_size_; 120 | downsample_all_data_ = src.downsample_all_data_; 121 | histsize_ = src.histsize_; 122 | delete[] history_; 123 | history_ = new he[histsize_]; 124 | for (size_t i = 0; i < histsize_; i++) 125 | history_[i] = src.history_[i]; 126 | min_points = src.min_points; 127 | skip_points = src.skip_points; 128 | return (*this); 129 | } 130 | 131 | /** \brief Set the voxel grid leaf size. 132 | * \param[in] leaf_size the voxel grid leaf size 133 | */ 134 | inline void 135 | setLeafSize (const Eigen::Vector3f &leaf_size) 136 | { 137 | leaf_size_ = leaf_size; 138 | inverse_leaf_size_ = Eigen::Array3f::Ones () / leaf_size_.array (); 139 | } 140 | 141 | /** \brief Set the voxel grid leaf size. 142 | * \param[in] lx the leaf size for X 143 | * \param[in] ly the leaf size for Y 144 | * \param[in] lz the leaf size for Z 145 | */ 146 | inline void 147 | setLeafSize (float lx, float ly, float lz) 148 | { 149 | setLeafSize (Eigen::Vector3f (lx, ly, lz)); 150 | } 151 | 152 | /** \brief Get the voxel grid leaf size. */ 153 | inline Eigen::Vector3f 154 | getLeafSize () const { return (leaf_size_); } 155 | 156 | /** \brief Set to true if all fields need to be downsampled, or false if just XYZ. 157 | * \param downsample the new value (true/false) 158 | */ 159 | inline void 160 | setDownsampleAllData (bool downsample) { downsample_all_data_ = downsample; } 161 | 162 | /** \brief Get the state of the internal downsampling parameter (true if 163 | * all fields need to be downsampled, false if just XYZ). 164 | */ 165 | inline bool 166 | getDownsampleAllData () const { return (downsample_all_data_); } 167 | 168 | protected: 169 | /** \brief The size of a leaf. */ 170 | Eigen::Vector3f leaf_size_; 171 | 172 | /** \brief Compute 1/leaf_size_ to avoid division later */ 173 | Eigen::Array3f inverse_leaf_size_; 174 | 175 | /** \brief Set to true if all fields need to be downsampled, or false if just XYZ. */ 176 | bool downsample_all_data_; 177 | 178 | /** \brief history buffer size, power of 2 */ 179 | size_t histsize_; 180 | 181 | /** \brief history buffer */ 182 | struct he* history_; 183 | 184 | int min_points; 185 | 186 | int skip_points; 187 | 188 | typedef typename pcl::traits::fieldList::type FieldList; 189 | 190 | /** \brief Downsample a Point Cloud using a voxelized grid approach 191 | * \param output the resultant point cloud message 192 | */ 193 | void 194 | applyFilter (PointCloud &output); 195 | 196 | /** \brief Write a single point from the hash to the output cloud 197 | */ 198 | void 199 | flush(PointCloud &output, size_t op, he *hhe, int rgba_index, int centroid_size); 200 | }; 201 | #endif // NOISE_APPROXIMATE_VOXEL_GRID 202 | -------------------------------------------------------------------------------- /strands_navfn/include/strands_navfn/strands_navfn_ros.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2008, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the Willow Garage nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * Author: Eitan Marder-Eppstein 36 | *********************************************************************/ 37 | #ifndef NAVFN_NAVFN_ROS_H_ 38 | #define NAVFN_NAVFN_ROS_H_ 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | 53 | namespace strands_navfn { 54 | /** 55 | * @class NavfnROS 56 | * @brief Provides a ROS wrapper for the navfn planner which runs a fast, interpolated navigation function on a costmap. 57 | */ 58 | class NavfnROS : public nav_core::BaseGlobalPlanner { 59 | public: 60 | /** 61 | * @brief Default constructor for the NavFnROS object 62 | */ 63 | NavfnROS(); 64 | 65 | /** 66 | * @brief Constructor for the NavFnROS object 67 | * @param name The name of this planner 68 | * @param costmap A pointer to the ROS wrapper of the costmap to use 69 | */ 70 | NavfnROS(std::string name, costmap_2d::Costmap2DROS* costmap_ros); 71 | 72 | /** 73 | * @brief Initialization function for the NavFnROS object 74 | * @param name The name of this planner 75 | * @param costmap A pointer to the ROS wrapper of the costmap to use for planning 76 | */ 77 | void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros); 78 | 79 | /** 80 | * @brief Given a goal pose in the world, compute a plan 81 | * @param start The start pose 82 | * @param goal The goal pose 83 | * @param plan The plan... filled by the planner 84 | * @return True if a valid plan was found, false otherwise 85 | */ 86 | bool makePlan(const geometry_msgs::PoseStamped& start, 87 | const geometry_msgs::PoseStamped& goal, std::vector& plan); 88 | 89 | /** 90 | * @brief Given a goal pose in the world, compute a plan 91 | * @param start The start pose 92 | * @param goal The goal pose 93 | * @param tolerance The tolerance on the goal point for the planner 94 | * @param plan The plan... filled by the planner 95 | * @return True if a valid plan was found, false otherwise 96 | */ 97 | bool makePlan(const geometry_msgs::PoseStamped& start, 98 | const geometry_msgs::PoseStamped& goal, double tolerance, std::vector& plan); 99 | 100 | /** 101 | * @brief Computes the full navigation function for the map given a point in the world to start from 102 | * @param world_point The point to use for seeding the navigation function 103 | * @return True if the navigation function was computed successfully, false otherwise 104 | */ 105 | bool computePotential(const geometry_msgs::Point& world_point); 106 | 107 | /** 108 | * @brief Compute a plan to a goal after the potential for a start point has already been computed (Note: You should call computePotential first) 109 | * @param goal The goal pose to create a plan to 110 | * @param plan The plan... filled by the planner 111 | * @return True if a valid plan was found, false otherwise 112 | */ 113 | bool getPlanFromPotential(const geometry_msgs::PoseStamped& goal, std::vector& plan); 114 | 115 | /** 116 | * @brief Get the potential, or naviagation cost, at a given point in the world (Note: You should call computePotential first) 117 | * @param world_point The point to get the potential for 118 | * @return The navigation function's value at that point in the world 119 | */ 120 | double getPointPotential(const geometry_msgs::Point& world_point); 121 | 122 | /** 123 | * @brief Check for a valid potential value at a given point in the world (Note: You should call computePotential first) 124 | * @param world_point The point to get the potential for 125 | * @return True if the navigation function is valid at that point in the world, false otherwise 126 | */ 127 | bool validPointPotential(const geometry_msgs::Point& world_point); 128 | 129 | /** 130 | * @brief Check for a valid potential value at a given point in the world (Note: You should call computePotential first) 131 | * @param world_point The point to get the potential for 132 | * @param tolerance The tolerance on searching around the world_point specified 133 | * @return True if the navigation function is valid at that point in the world, false otherwise 134 | */ 135 | bool validPointPotential(const geometry_msgs::Point& world_point, double tolerance); 136 | 137 | /** 138 | * @brief Publish a path for visualization purposes 139 | */ 140 | void publishPlan(const std::vector& path, double r, double g, double b, double a); 141 | 142 | ~NavfnROS(){} 143 | 144 | bool makePlanService(nav_msgs::GetPlan::Request& req, nav_msgs::GetPlan::Response& resp); 145 | 146 | protected: 147 | 148 | /** 149 | * @brief Store a copy of the current costmap in \a costmap. Called by makePlan. 150 | */ 151 | costmap_2d::Costmap2DROS* costmap_ros_; 152 | boost::shared_ptr planner_; 153 | ros::Publisher plan_pub_; 154 | pcl_ros::Publisher potarr_pub_; 155 | bool initialized_, allow_unknown_, visualize_potential_; 156 | 157 | 158 | private: 159 | inline double sq_distance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2){ 160 | double dx = p1.pose.position.x - p2.pose.position.x; 161 | double dy = p1.pose.position.y - p2.pose.position.y; 162 | return dx*dx +dy*dy; 163 | } 164 | 165 | void mapToWorld(double mx, double my, double& wx, double& wy); 166 | void clearRobotCell(const tf::Stamped& global_pose, unsigned int mx, unsigned int my); 167 | std::string name_; 168 | double planner_window_x_, planner_window_y_, default_tolerance_; 169 | std::string tf_prefix_; 170 | boost::mutex mutex_; 171 | ros::ServiceServer make_plan_srv_; 172 | }; 173 | }; 174 | 175 | #endif 176 | -------------------------------------------------------------------------------- /strands_navfn/src/strands_navtest.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // simple timing test of the nav fn planner 3 | // expects a cost map in maps/willow-full-0.05.pgm 4 | // 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | using namespace strands_navfn; 17 | 18 | #ifdef __APPLE__ 19 | # include 20 | #else 21 | extern "C" { 22 | #include 23 | // pgm.h is not very friendly with system headers... need to undef max() and min() afterwards 24 | #include 25 | #undef max 26 | #undef min 27 | } 28 | #endif 29 | 30 | int goal[2]; 31 | int start[2]; 32 | 33 | double get_ms() 34 | { 35 | struct timeval t0; 36 | gettimeofday(&t0,NULL); 37 | double ret = t0.tv_sec * 1000.0; 38 | ret += ((double)t0.tv_usec)*0.001; 39 | return ret; 40 | } 41 | 42 | NavWin *nwin; 43 | 44 | void 45 | dispPot(NavFn *nav) 46 | { 47 | nwin->drawPot(nav); 48 | Fl::check(); 49 | } 50 | 51 | 52 | // is true for ROS-generated raw cost maps 53 | COSTTYPE *readPGM(const char *fname, int *width, int *height, bool raw = false); 54 | 55 | int main(int argc, char **argv) 56 | { 57 | int dispn = 0; 58 | 59 | int res = 50; // 50 mm resolution 60 | double size = 40.0; // 40 m on a side 61 | int inc = 2*COST_NEUTRAL; // thin wavefront 62 | bool got_start_goal = false; 63 | std::string pgm_file_name; 64 | 65 | start[0] = 420; 66 | start[1] = 420; 67 | 68 | goal[0] = 580; 69 | goal[1] = 400; 70 | 71 | if (argc > 1) 72 | { 73 | pgm_file_name = std::string( argv[ 1 ]) + ".pgm"; 74 | std::string txt_file_name = std::string( argv[ 1 ]) + ".txt"; 75 | 76 | std::ifstream txt_stream( txt_file_name.c_str() ); 77 | if( txt_stream ) 78 | { 79 | std::string name; 80 | int x, y; 81 | for( int i = 0; i < 2; i++ ) 82 | { 83 | txt_stream >> name >> x >> y; 84 | if( txt_stream && name == "Goal:" ) 85 | { 86 | goal[0] = x; 87 | goal[1] = y; 88 | } 89 | else if( txt_stream && name == "Start:" ) 90 | { 91 | start[0] = x; 92 | start[1] = y; 93 | } 94 | } 95 | got_start_goal = true; 96 | printf( "start is %d, %d, goal is %d, %d.\n", start[ 0 ], start[ 1 ], goal[ 0 ], goal[ 1 ]); 97 | } 98 | else 99 | { 100 | printf( "Failed to open file %s, assuming you didn't want to open a file.\n", txt_file_name.c_str() ); 101 | } 102 | } 103 | 104 | // get resolution (mm) and perhaps size (m) 105 | if( !got_start_goal ) 106 | { 107 | if (argc > 1) 108 | res = atoi(argv[1]); 109 | 110 | if (argc > 2) 111 | size = atoi(argv[2]); 112 | 113 | if (argc > 3) 114 | inc = atoi(argv[3]); 115 | 116 | if (argc > 4) 117 | dispn = atoi(argv[4]); 118 | } 119 | NavFn *nav; 120 | 121 | // try reading in a file 122 | int sx,sy; 123 | COSTTYPE *cmap = NULL; 124 | // cmap = readPGM("maps/willow-full-0.05.pgm",&sx,&sy); 125 | // cmap = readPGM("maps/navfn_test1.pgm",&sx,&sy,true); 126 | // cmap = readPGM("initial_costmap_1165_945.pgm",&sx,&sy,true); 127 | // cmap = readPGM("initial_costmap_2332_1825.pgm",&sx,&sy,true); 128 | cmap = readPGM( pgm_file_name.c_str(),&sx,&sy,true); 129 | // cmap = readPGM("navfn_pathlong.pgm",&sx,&sy,true); 130 | if (cmap) 131 | { 132 | nav = new NavFn(sx,sy); 133 | 134 | 135 | } 136 | else 137 | { 138 | sx = (int)((.001 + size) / (res*.001)); 139 | sy = sx; 140 | nav = new NavFn(sx,sy); // size in pixels 141 | goal[0] = sx-10; 142 | goal[1] = sy/2; 143 | start[0] = 20; 144 | start[1] = sy/2; 145 | } 146 | 147 | // display 148 | nwin = new NavWin(sx,sy,"Potential Field"); 149 | nwin->maxval = 2*sx*COST_NEUTRAL; 150 | Fl::visual(FL_RGB); 151 | nwin->show(); 152 | 153 | 154 | // set goal and robot poses 155 | int *gg = goal; 156 | nav->setGoal(gg); 157 | int *ss = start; 158 | nav->setStart(ss); 159 | 160 | // set display function 161 | nav->display(dispPot,dispn); 162 | 163 | 164 | nav->priInc = inc; 165 | printf("[NavTest] priority increment: %d\n", inc); 166 | 167 | #if 0 168 | // calculate the nav fn and path 169 | double t0 = get_ms(); 170 | // set up cost map from file, if it exists 171 | if (cmap) 172 | { 173 | nav->setCostmap(cmap); 174 | nav->setupNavFn(true); 175 | } 176 | else 177 | { 178 | nav->setupNavFn(false); 179 | nav->setObs(); // simple obstacles 180 | } 181 | //nav->propNavFnDijkstra(sx*sy/20); 182 | nav->propNavFnAstar(sx*sy/20); 183 | double t1 = get_ms(); 184 | 185 | printf("Time for plan calculation: %d ms\n", (int)(t1-t0)); 186 | 187 | // path 188 | nav->calcPath(4000); 189 | 190 | #else 191 | double t0 = get_ms(); 192 | // set up cost map from file, if it exists 193 | if (cmap) 194 | { 195 | // nav->setCostMap(cmap); 196 | memcpy(nav->costarr,cmap,sx*sy); 197 | nav->setupNavFn(true); 198 | } 199 | else 200 | { 201 | nav->setupNavFn(false); 202 | nav->setObs(); // simple obstacles 203 | } 204 | double t1 = get_ms(); 205 | // nav->calcNavFnAstar(); 206 | nav->calcNavFnDijkstra(true); 207 | double t2 = get_ms(); 208 | printf("Setup: %d ms Plan: %d ms Total: %d ms\n", 209 | (int)(t1-t0), (int)(t2-t1), (int)(t2-t0)); 210 | #endif 211 | 212 | // draw potential field 213 | float mmax = 0.0; 214 | float *pp = nav->potarr; 215 | int ntot = 0; 216 | for (int i=0; iny*nav->nx; i++, pp++) 217 | { 218 | if (*pp < 10e7 && *pp > mmax) 219 | mmax = *pp; 220 | if (*pp > 10e7) 221 | ntot++; // number of uncalculated cells 222 | } 223 | printf("[NavFn] Cells not touched: %d/%d\n", ntot, nav->nx*nav->ny); 224 | nwin->maxval = 4*mmax/3/15; 225 | dispPot(nav); 226 | while (Fl::check()) { 227 | if( Fl::event_key( 'q' )) 228 | { 229 | break; 230 | } 231 | } 232 | 233 | #if 0 234 | goal[1] = size-2; 235 | int k = nav->getCellIndex(gg); 236 | int st_nx = nav->st_nx; 237 | for (int i=0; i<900; i++, k--) 238 | { 239 | float npot = nav->potgrid[k]; 240 | printf("Pot: %0.1f\n", npot); 241 | printf("L: %0.1f R: %0.1f U: %0.1f D: %0.1f\n", 242 | nav->potgrid[k-1], nav->potgrid[k+1], nav->potgrid[k-st_nx], nav->potgrid[k+st_nx]); 243 | } 244 | #endif 245 | 246 | return 0; 247 | } 248 | 249 | 250 | // read in a PGM file for obstacles 251 | // no expansion yet... 252 | 253 | static int CS; 254 | 255 | void 256 | setcostobs(COSTTYPE *cmap, int n, int w) 257 | { 258 | CS = 11; 259 | for (int i=-CS/2; i= 0; --jj) 316 | { 317 | int v = row[jj]; 318 | cmap[ii*ncols+jj] = v; 319 | if (v >= COST_OBS_ROS) 320 | otot++; 321 | if (v == 0) 322 | ftot++; 323 | } 324 | } 325 | else 326 | { 327 | ftot = ncols*nrows; 328 | for (int jj(ncols - 1); jj >= 0; --jj) 329 | { 330 | if (row[jj] < unknown_gray && ii < nrows-7 && ii > 7) 331 | { 332 | setcostobs(cmap,ii*ncols+jj,ncols); 333 | otot++; 334 | ftot--; 335 | } 336 | #if 1 337 | else if (row[jj] <= unknown_gray) 338 | { 339 | setcostunk(cmap,ii*ncols+jj,ncols); 340 | utot++; 341 | ftot--; 342 | } 343 | #endif 344 | } 345 | } 346 | } 347 | printf("[NavTest] Found %d obstacle cells, %d free cells, %d unknown cells\n", otot, ftot, utot); 348 | pgm_freerow(row); 349 | *width = ncols; 350 | *height = nrows; 351 | return cmap; 352 | } 353 | -------------------------------------------------------------------------------- /movebase_state_service/src/movebase_state_service.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include "movebase_state_service/MovebaseStateService.h" 13 | 14 | class state_service { 15 | 16 | public: 17 | 18 | ros::NodeHandle n; 19 | ros::ServiceServer service; 20 | 21 | geometry_msgs::PoseStampedConstPtr last_goal; 22 | geometry_msgs::PoseConstPtr last_pose; 23 | nav_msgs::Path::ConstPtr global_last_path; 24 | nav_msgs::Path::ConstPtr local_last_path; 25 | std::string snapshot_folder; 26 | int counter; 27 | 28 | ros::Subscriber goal_sub; 29 | ros::Subscriber pose_sub; 30 | ros::Subscriber global_path_sub; 31 | ros::Subscriber local_path_sub; 32 | 33 | std::string image_input; 34 | std::string global_costmap_input; 35 | std::string local_costmap_input; 36 | std::string goal_input; 37 | std::string pose_input; 38 | std::string global_path_input; 39 | std::string local_path_input; 40 | 41 | state_service(const std::string& name) 42 | { 43 | ros::NodeHandle pn("~"); 44 | pn.param("image_input", image_input, std::string("/head_xtion/rgb/image_color")); 45 | pn.param("global_costmap_input", global_costmap_input, std::string("/move_base/global_costmap/costmap")); 46 | pn.param("local_costmap_input", local_costmap_input, std::string("/move_base/local_costmap/costmap")); 47 | pn.param("snapshot_folder", snapshot_folder, std::string(getenv("HOME")) + std::string("/.ros/movebase_state_service")); 48 | pn.param("goal_input", goal_input, std::string("/move_base/current_goal")); 49 | pn.param("pose_input", pose_input, std::string("/robot_pose")); 50 | pn.param("global_path_input", global_path_input, std::string("/move_base/DWAPlannerROS/global_plan")); 51 | pn.param("local_path_input", local_path_input, std::string("/move_base/DWAPlannerROS/local_plan")); 52 | 53 | goal_sub = n.subscribe(goal_input, 1, &state_service::goal_callback, this); 54 | pose_sub = n.subscribe(pose_input, 1, &state_service::pose_callback, this); 55 | global_path_sub = n.subscribe(global_path_input, 1, &state_service::global_path_callback, this); 56 | local_path_sub = n.subscribe(local_path_input, 1, &state_service::local_path_callback, this); 57 | counter = 0; 58 | 59 | service = n.advertiseService(name, &state_service::service_callback, this); 60 | } 61 | 62 | void goal_callback(const geometry_msgs::PoseStampedConstPtr& goal_msg) 63 | { 64 | last_goal = goal_msg; 65 | } 66 | 67 | void pose_callback(const geometry_msgs::PoseConstPtr& pose_msg) 68 | { 69 | last_pose = pose_msg; 70 | } 71 | 72 | void global_path_callback(const nav_msgs::Path::ConstPtr& path_msg) 73 | { 74 | global_last_path = path_msg; 75 | } 76 | 77 | void local_path_callback(const nav_msgs::Path::ConstPtr& path_msg) 78 | { 79 | local_last_path = path_msg; 80 | } 81 | 82 | void color_around_point(cv::Mat& map, int x, int y, int c) 83 | { 84 | for (int i = x - 2; i < x + 3; ++i) { 85 | for (int j = y - 2; j < y + 3; ++j) { 86 | // add out-of-bounds check 87 | if (i < 0 || i >= map.cols || j < 0 || j >= map.rows) { 88 | continue; 89 | } 90 | map.at(j, i)[0] = 0; 91 | map.at(j, i)[1] = 0; 92 | map.at(j, i)[2] = 0; 93 | map.at(j, i)[c] = 255; 94 | } 95 | } 96 | 97 | } 98 | 99 | void save_map(sensor_msgs::Image& image_msg, const std::string& map_file, const std::string& rgb_map_file, 100 | nav_msgs::OccupancyGrid::ConstPtr& last_map, nav_msgs::Path::ConstPtr& last_path, bool save_to_disk) 101 | { 102 | // save map 103 | double map_height = last_map->info.height; 104 | double map_width = last_map->info.width; 105 | double map_origin_x = last_map->info.origin.position.x; 106 | double map_origin_y = last_map->info.origin.position.y; 107 | double map_res = last_map->info.resolution; 108 | ROS_INFO_STREAM("Map width x height: " << map_width << " x " << map_height); 109 | costmap_2d::Costmap2D map(map_width, map_height, map_res, map_origin_x, map_origin_y); 110 | 111 | cv::Mat image = cv::Mat::zeros(map_height, map_width, CV_8UC3); 112 | 113 | // incoming dynamic map to Costmap2D 114 | for(int i = 0; i < map_width; i++) { 115 | for(int j = 0; j < map_height; j++) { 116 | map.setCost(i, j, last_map->data[map.getIndex(i, j)]); 117 | image.at(j, i)[0] = last_map->data[map.getIndex(i, j)]; 118 | image.at(j, i)[1] = last_map->data[map.getIndex(i, j)]; 119 | image.at(j, i)[2] = last_map->data[map.getIndex(i, j)]; 120 | } 121 | } 122 | 123 | double x, y; 124 | int map_x, map_y; 125 | 126 | if (last_path) { 127 | for (size_t i = 0; i < last_path->poses.size(); ++i) { 128 | x = last_path->poses[i].pose.position.x; 129 | y = last_path->poses[i].pose.position.y; 130 | map.worldToMapEnforceBounds(x, y, map_x, map_y); 131 | color_around_point(image, map_x, map_y, 1); 132 | } 133 | } 134 | 135 | if (last_goal) { 136 | x = last_goal->pose.position.x; 137 | y = last_goal->pose.position.y; 138 | map.worldToMapEnforceBounds(x, y, map_x, map_y); 139 | color_around_point(image, map_x, map_y, 2); 140 | } 141 | 142 | if (last_pose) { 143 | x = last_pose->position.x; 144 | y = last_pose->position.y; 145 | map.worldToMapEnforceBounds(x, y, map_x, map_y); 146 | color_around_point(image, map_x, map_y, 0); 147 | } 148 | 149 | if (save_to_disk) { 150 | map.saveMap(map_file); 151 | // use lossless png compression 152 | std::vector compression; 153 | compression.push_back(cv::IMWRITE_PNG_COMPRESSION); 154 | compression.push_back(0); // no compression 155 | cv::imwrite(rgb_map_file, image, compression); 156 | } 157 | 158 | std_msgs::Header header; 159 | header.seq = counter; 160 | header.stamp = ros::Time::now(); 161 | header.frame_id = "/map"; 162 | 163 | cv_bridge::CvImage global_costmap_tmp = cv_bridge::CvImage(header, "8UC3", image); 164 | global_costmap_tmp.toImageMsg(image_msg); 165 | } 166 | 167 | bool service_callback(movebase_state_service::MovebaseStateService::Request& req, 168 | movebase_state_service::MovebaseStateService::Response& res) 169 | { 170 | sensor_msgs::Image::ConstPtr last_img = ros::topic::waitForMessage(image_input, n, ros::Duration(5)); 171 | if (!last_img) { 172 | return false; 173 | } 174 | 175 | if (req.save_to_disk) { 176 | // save image 177 | boost::shared_ptr rgb_tracked_object; 178 | cv_bridge::CvImageConstPtr rgb_cv_img_boost_ptr; 179 | try { 180 | rgb_cv_img_boost_ptr = cv_bridge::toCvShare(*last_img, rgb_tracked_object); 181 | } 182 | catch (cv_bridge::Exception& e) { 183 | ROS_ERROR("cv_bridge exception: %s", e.what()); 184 | return false; 185 | } 186 | // use lossless png compression 187 | std::vector compression; 188 | compression.push_back(cv::IMWRITE_PNG_COMPRESSION); 189 | compression.push_back(0); // no compression 190 | 191 | std::string image_file = snapshot_folder + "/image" + std::to_string(counter) + ".png"; 192 | cv::imwrite(image_file, rgb_cv_img_boost_ptr->image, compression); 193 | } 194 | res.camera_image = *last_img; 195 | 196 | nav_msgs::OccupancyGrid::ConstPtr global_costmap_msg = ros::topic::waitForMessage(global_costmap_input, n, ros::Duration(5)); 197 | if (!global_costmap_msg) { 198 | return false; 199 | } 200 | std::string global_map_file = snapshot_folder + "/global_costmap" + std::to_string(counter) + ".pgm"; 201 | std::string global_rgb_map_file = snapshot_folder + "/global_costmap_path" + std::to_string(counter) + ".png"; 202 | save_map(res.global_costmap_image, global_map_file, global_rgb_map_file, global_costmap_msg, global_last_path, req.save_to_disk); 203 | 204 | nav_msgs::OccupancyGrid::ConstPtr local_costmap_msg = ros::topic::waitForMessage(local_costmap_input, n, ros::Duration(5)); 205 | if (!local_costmap_msg) { 206 | return false; 207 | } 208 | std::string local_map_file = snapshot_folder + "/local_costmap" + std::to_string(counter) + ".pgm"; 209 | std::string local_rgb_map_file = snapshot_folder + "/local_costmap_path" + std::to_string(counter) + ".png"; 210 | save_map(res.local_costmap_image, local_map_file, local_rgb_map_file, local_costmap_msg, local_last_path, req.save_to_disk); 211 | 212 | ++counter; 213 | 214 | return true; 215 | } 216 | 217 | }; 218 | 219 | int main(int argc, char** argv) 220 | { 221 | ros::init(argc, argv, "movebase_state_service"); 222 | 223 | state_service sc(ros::this_node::getName()); 224 | 225 | ros::spin(); 226 | 227 | return 0; 228 | } 229 | -------------------------------------------------------------------------------- /strands_navfn/include/strands_navfn/strands_navfn.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | // 36 | // Navigation function computation 37 | // Uses Dijkstra's method 38 | // Modified for Euclidean-distance computation 39 | // 40 | 41 | #ifndef _NAVFN_H 42 | #define _NAVFN_H 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | 49 | // cost defs 50 | #define COST_UNKNOWN_ROS 255 // 255 is unknown cost 51 | #define COST_OBS 254 // 254 for forbidden regions 52 | #define COST_OBS_ROS 253 // ROS values of 253 are obstacles 53 | 54 | // navfn cost values are set to 55 | // COST_NEUTRAL + COST_FACTOR * costmap_cost_value. 56 | // Incoming costmap cost values are in the range 0 to 252. 57 | // With COST_NEUTRAL of 50, the COST_FACTOR needs to be about 0.8 to 58 | // ensure the input values are spread evenly over the output range, 50 59 | // to 253. If COST_FACTOR is higher, cost values will have a plateau 60 | // around obstacles and the planner will then treat (for example) the 61 | // whole width of a narrow hallway as equally undesirable and thus 62 | // will not plan paths down the center. 63 | 64 | #define COST_NEUTRAL 50 // Set this to "open space" value 65 | #define COST_FACTOR 0.8 // Used for translating costs in NavFn::setCostmap() 66 | 67 | // Define the cost type in the case that it is not set. However, this allows 68 | // clients to modify it without changing the file. Arguably, it is better to require it to 69 | // be defined by a user explicitly 70 | #ifndef COSTTYPE 71 | #define COSTTYPE unsigned char // Whatever is used... 72 | #endif 73 | 74 | // potential defs 75 | #define POT_HIGH 1.0e10 // unassigned cell potential 76 | 77 | // priority buffers 78 | #define PRIORITYBUFSIZE 10000 79 | 80 | 81 | namespace strands_navfn { 82 | /** 83 | Navigation function call. 84 | \param costmap Cost map array, of type COSTTYPE; origin is upper left 85 | NOTE: will be modified to have a border of obstacle costs 86 | \param nx Width of map in cells 87 | \param ny Height of map in cells 88 | \param goal X,Y position of goal cell 89 | \param start X,Y position of start cell 90 | 91 | Returns length of plan if found, and fills an array with x,y interpolated 92 | positions at about 1/2 cell resolution; else returns 0. 93 | 94 | */ 95 | 96 | int create_nav_plan_astar(const COSTTYPE *costmap, int nx, int ny, 97 | int* goal, int* start, 98 | float *plan, int nplan); 99 | 100 | 101 | 102 | /** 103 | * @class NavFn 104 | * @brief Navigation function class. Holds buffers for costmap, navfn map. Maps are pixel-based. Origin is upper left, x is right, y is down. 105 | */ 106 | class NavFn 107 | { 108 | public: 109 | /** 110 | * @brief Constructs the planner 111 | * @param nx The x size of the map 112 | * @param ny The y size of the map 113 | */ 114 | NavFn(int nx, int ny); // size of map 115 | 116 | ~NavFn(); 117 | 118 | /** 119 | * @brief Sets or resets the size of the map 120 | * @param nx The x size of the map 121 | * @param ny The y size of the map 122 | */ 123 | void setNavArr(int nx, int ny); /**< sets or resets the size of the map */ 124 | int nx, ny, ns; /**< size of grid, in pixels */ 125 | 126 | /** 127 | * @brief Set up the cost array for the planner, usually from ROS 128 | * @param cmap The costmap 129 | * @param isROS Whether or not the costmap is coming in in ROS format 130 | * @param allow_unknown Whether or not the planner should be allowed to plan through unknown space 131 | */ 132 | void setCostmap(const COSTTYPE *cmap, bool isROS=true, bool allow_unknown = true); /**< sets up the cost map */ 133 | 134 | /** 135 | * @brief Calculates a plan using the A* heuristic, returns true if one is found 136 | * @return True if a plan is found, false otherwise 137 | */ 138 | bool calcNavFnAstar(); /**< calculates a plan, returns true if found */ 139 | 140 | /** 141 | * @brief Caclulates the full navigation function using Dijkstra 142 | */ 143 | bool calcNavFnDijkstra(bool atStart = false); /**< calculates the full navigation function */ 144 | 145 | /** 146 | * @brief Accessor for the x-coordinates of a path 147 | * @return The x-coordinates of a path 148 | */ 149 | float *getPathX(); /**< x-coordinates of path */ 150 | 151 | /** 152 | * @brief Accessor for the y-coordinates of a path 153 | * @return The y-coordinates of a path 154 | */ 155 | float *getPathY(); /**< y-coordinates of path */ 156 | 157 | /** 158 | * @brief Accessor for the length of a path 159 | * @return The length of a path 160 | */ 161 | int getPathLen(); /**< length of path, 0 if not found */ 162 | 163 | /** 164 | * @brief Gets the cost of the path found the last time a navigation function was computed 165 | * @return The cost of the last path found 166 | */ 167 | float getLastPathCost(); /**< Return cost of path found the last time A* was called */ 168 | 169 | /** cell arrays */ 170 | COSTTYPE *costarr; /**< cost array in 2D configuration space */ 171 | float *potarr; /**< potential array, navigation function potential */ 172 | bool *pending; /**< pending cells during propagation */ 173 | int nobs; /**< number of obstacle cells */ 174 | 175 | /** block priority buffers */ 176 | int *pb1, *pb2, *pb3; /**< storage buffers for priority blocks */ 177 | int *curP, *nextP, *overP; /**< priority buffer block ptrs */ 178 | int curPe, nextPe, overPe; /**< end points of arrays */ 179 | 180 | /** block priority thresholds */ 181 | float curT; /**< current threshold */ 182 | float priInc; /**< priority threshold increment */ 183 | 184 | /** goal and start positions */ 185 | /** 186 | * @brief Sets the goal position for the planner. Note: the navigation cost field computed gives the cost to get to a given point from the goal, not from the start. 187 | * @param goal the goal position 188 | */ 189 | void setGoal(int *goal); 190 | 191 | /** 192 | * @brief Sets the start position for the planner. Note: the navigation cost field computed gives the cost to get to a given point from the goal, not from the start. 193 | * @param start the start position 194 | */ 195 | void setStart(int *start); 196 | 197 | int goal[2]; 198 | int start[2]; 199 | /** 200 | * @brief Initialize cell k with cost v for propagation 201 | * @param k the cell to initialize 202 | * @param v the cost to give to the cell 203 | */ 204 | void initCost(int k, float v); /**< initialize cell with cost , for propagation */ 205 | 206 | /** simple obstacle for testing */ 207 | void setObs(); 208 | 209 | /** propagation */ 210 | 211 | /** 212 | * @brief Updates the cell at index n 213 | * @param n The index to update 214 | */ 215 | void updateCell(int n); /**< updates the cell at index */ 216 | 217 | /** 218 | * @brief Updates the cell at index n using the A* heuristic 219 | * @param n The index to update 220 | */ 221 | void updateCellAstar(int n); /**< updates the cell at index , uses A* heuristic */ 222 | 223 | void setupNavFn(bool keepit = false); /**< resets all nav fn arrays for propagation */ 224 | 225 | /** 226 | * @brief Run propagation for iterations, or until start is reached using breadth-first Dijkstra method 227 | * @param cycles The maximum number of iterations to run for 228 | * @param atStart Whether or not to stop when the start point is reached 229 | * @return true if the start point is reached 230 | */ 231 | bool propNavFnDijkstra(int cycles, bool atStart = false); /**< returns true if start point found or full prop */ 232 | /** 233 | * @brief Run propagation for iterations, or until start is reached using the best-first A* method with Euclidean distance heuristic 234 | * @param cycles The maximum number of iterations to run for 235 | * @return true if the start point is reached 236 | */ 237 | bool propNavFnAstar(int cycles); /**< returns true if start point found */ 238 | 239 | /** gradient and paths */ 240 | float *gradx, *grady; /**< gradient arrays, size of potential array */ 241 | float *pathx, *pathy; /**< path points, as subpixel cell coordinates */ 242 | int npath; /**< number of path points */ 243 | int npathbuf; /**< size of pathx, pathy buffers */ 244 | 245 | float last_path_cost_; /**< Holds the cost of the path found the last time A* was called */ 246 | 247 | 248 | /** 249 | * @brief Calculates the path for at mose cycles 250 | * @param n The maximum number of cycles to run for 251 | * @return The lenght of the path found 252 | */ 253 | int calcPath(int n, int *st = NULL); /**< calculates path for at most cycles, returns path length, 0 if none */ 254 | 255 | float gradCell(int n); /**< calculates gradient at cell , returns norm */ 256 | float pathStep; /**< step size for following gradient */ 257 | 258 | /** display callback */ 259 | void display(void fn(NavFn *nav), int n = 100); /**< is the number of cycles between updates */ 260 | int displayInt; /**< save second argument of display() above */ 261 | void (*displayFn)(NavFn *nav); /**< display function itself */ 262 | 263 | /** save costmap */ 264 | void savemap(const char *fname); /**< write out costmap and start/goal states as fname.pgm and fname.txt */ 265 | 266 | }; 267 | }; 268 | 269 | 270 | #endif // NAVFN 271 | -------------------------------------------------------------------------------- /calibrate_chest/src/calibration_server.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "mongodb_store/SetParam.h" 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | 16 | class CalibrateCameraServer { 17 | private: 18 | 19 | ros::NodeHandle n; 20 | actionlib::SimpleActionServer server; 21 | std::string action_name; 22 | ros::ServiceClient client; 23 | std::string camera_name; 24 | std::string camera_topic; 25 | double desired_angle; 26 | calibrate_chest::CalibrateCameraFeedback feedback; 27 | calibrate_chest::CalibrateCameraResult result; 28 | 29 | public: 30 | 31 | CalibrateCameraServer(const std::string& name, const std::string& camera_name, double angle) : 32 | server(n, name, boost::bind(&CalibrateCameraServer::execute_cb, this, _1), false), 33 | action_name(name), 34 | client(n.serviceClient("/config_manager/set_param")), 35 | camera_name(camera_name), 36 | camera_topic(camera_name + "/depth/points"), 37 | desired_angle(angle) 38 | { 39 | server.start(); 40 | } 41 | 42 | private: 43 | 44 | bool is_inlier(const Eigen::Vector3f& point, const Eigen::Vector4f plane, double threshold) const 45 | { 46 | return fabs(point.dot(plane.segment<3>(0)) + plane(3)) < threshold; 47 | } 48 | 49 | void plot_best_plane(const pcl::PointCloud& points, const Eigen::Vector4f plane, double threshold) const 50 | { 51 | pcl::PointCloud::Ptr inlier_cloud(new pcl::PointCloud()); 52 | pcl::PointCloud::Ptr outlier_cloud(new pcl::PointCloud()); 53 | 54 | for (int i = 0; i < points.size(); ++i) { 55 | if (is_inlier(points[i].getVector3fMap(), plane, threshold)) { 56 | inlier_cloud->push_back(points[i]); 57 | } 58 | else { 59 | outlier_cloud->push_back(points[i]); 60 | } 61 | } 62 | 63 | pcl::visualization::PCLVisualizer viewer("3D Viewer"); 64 | viewer.setBackgroundColor(0, 0, 0); 65 | viewer.addCoordinateSystem(1.0); 66 | viewer.initCameraParameters(); 67 | 68 | pcl::visualization::PointCloudColorHandlerCustom inlier_color_handler(inlier_cloud, 255, 0, 0); 69 | viewer.addPointCloud(inlier_cloud, inlier_color_handler, "inliers"); 70 | 71 | pcl::visualization::PointCloudColorHandlerCustom outlier_color_handler(outlier_cloud, 0, 0, 255); 72 | viewer.addPointCloud(outlier_cloud, outlier_color_handler, "outliers"); 73 | 74 | while (!viewer.wasStopped()) 75 | { 76 | viewer.spinOnce(100); 77 | boost::this_thread::sleep(boost::posix_time::microseconds(100000)); 78 | } 79 | 80 | } 81 | 82 | void compute_plane(Eigen::Vector4f& plane, const pcl::PointCloud& points, int* inds) const 83 | { 84 | Eigen::Vector3f first = points[inds[1]].getVector3fMap() - points[inds[0]].getVector3fMap(); 85 | Eigen::Vector3f second = points[inds[2]].getVector3fMap() - points[inds[0]].getVector3fMap(); 86 | Eigen::Vector3f normal = first.cross(second); 87 | normal.normalize(); 88 | plane.segment<3>(0) = normal; 89 | plane(3) = -normal.dot(points[inds[0]].getVector3fMap()); 90 | } 91 | 92 | void extract_height_and_angle(const Eigen::Vector4f& plane, bool only_calibrate) 93 | { 94 | feedback.status = "Checking and saving calibration..."; 95 | server.publishFeedback(feedback); 96 | 97 | ROS_INFO("Ground plane: %f, %f, %f, %f", plane(0), plane(1), plane(2), plane(3)); 98 | double dist = fabs(plane(3)/plane(2)); // distance along z axis 99 | double height = fabs(plane(3)/plane.segment<3>(0).squaredNorm()); // height 100 | ROS_INFO("Distance to plane along camera axis: %f", dist); 101 | ROS_INFO("Height above ground: %f", height); 102 | double angle = asin(height/dist); 103 | double angle_deg = 180.0f*angle/M_PI; 104 | ROS_INFO("Angle radians: %f", angle); 105 | ROS_INFO("Angle degrees: %f", angle_deg); 106 | 107 | result.angle = angle_deg; 108 | result.height = height; 109 | 110 | if (fabs(desired_angle - angle_deg) > 3.0f) { 111 | std::stringstream ss; 112 | ss << desired_angle; 113 | result.status = std::string("Angle not close enough to ") + ss.str() + " degrees."; 114 | server.setAborted(result); 115 | return; 116 | } 117 | 118 | mongodb_store::SetParam srv; 119 | char buffer[250]; 120 | 121 | // store height above ground in datacentre 122 | ros::param::set(std::string("/") + camera_name + "_height", height); 123 | sprintf(buffer, "{\"path\":\"/%s_height\",\"value\":%f}", camera_name.c_str(), height); 124 | srv.request.param = buffer; 125 | if (!client.call(srv)) { 126 | ROS_ERROR("Failed to call set height, is config manager running?"); 127 | } 128 | 129 | // store angle between camera and horizontal plane 130 | ros::param::set(std::string("/") + camera_name + "_angle", angle); 131 | sprintf(buffer, "{\"path\":\"/%s_angle\",\"value\":%f}", camera_name.c_str(), angle); 132 | srv.request.param = buffer; 133 | if (!client.call(srv)) { 134 | ROS_ERROR("Failed to call set angle, is config manager running?"); 135 | } 136 | 137 | result.status = "Successfully computed and saved calibration."; 138 | if (only_calibrate) { 139 | server.setSucceeded(result); 140 | } 141 | } 142 | 143 | void publish_calibration() 144 | { 145 | feedback.status = "Publishing calibration..."; 146 | feedback.progress = 0.0f; 147 | 148 | // get calibration with respect to ground plane from the calibrate_chest node 149 | double height, angle; 150 | n.param(std::string("/") + camera_name + "_height", height, 1.10f); // get the height calibration 151 | n.param(std::string("/") + camera_name + "_angle", angle, 0.72f); // get the angle calibration 152 | 153 | ros::Rate rate(1.0f); 154 | ros::Publisher pub = n.advertise("/chest_calibration_publisher/state", 1); 155 | 156 | int counter = 0; // only publish for the first 10 secs, transforms will stay in tf 157 | while (n.ok() && counter < 5) { 158 | sensor_msgs::JointState joint_state; 159 | joint_state.header.stamp = ros::Time::now(); 160 | joint_state.name.resize(2); 161 | joint_state.position.resize(2); 162 | joint_state.velocity.resize(2); 163 | joint_state.name[0] = camera_name + "_height_joint"; 164 | joint_state.name[1] = camera_name + "_tilt_joint"; 165 | joint_state.position[0] = height; 166 | joint_state.position[1] = angle; 167 | joint_state.velocity[0] = 0; 168 | joint_state.velocity[1] = 0; 169 | pub.publish(joint_state); 170 | 171 | feedback.progress = float(counter+1)/5.0f; 172 | server.publishFeedback(feedback); 173 | 174 | rate.sleep(); 175 | ++counter; 176 | } 177 | 178 | ROS_INFO("Stopping to publish chest transform after 5 seconds, quitting..."); 179 | 180 | result.status = "Published calibration."; 181 | result.angle = 180.0f/M_PI*angle; 182 | result.height = height; 183 | server.setSucceeded(result); 184 | } 185 | 186 | public: 187 | 188 | void msg_callback(const sensor_msgs::PointCloud2::ConstPtr& msg, bool only_calibrate = true) 189 | { 190 | //if (!do_calibrate) { 191 | // return; 192 | //} 193 | //do_calibrate = false; 194 | 195 | feedback.status = "Calibrating..."; 196 | feedback.progress = 0.0f; 197 | server.publishFeedback(feedback); 198 | 199 | ROS_INFO("Got a pointcloud, calibrating..."); 200 | pcl::PointCloud cloud; 201 | pcl::fromROSMsg(*msg, cloud); 202 | 203 | int nbr = cloud.size(); 204 | 205 | int max = 1000; // ransac iterations 206 | double threshold = 0.02; // threshold for plane inliers 207 | 208 | Eigen::Vector4f best_plane; // best plane parameters found 209 | int best_inliers = -1; // best number of inliers 210 | 211 | int inds[3]; 212 | 213 | Eigen::Vector4f plane; 214 | int inliers; 215 | for (int i = 0; i < max; ++i) { 216 | 217 | for (int j = 0; j < 3; ++j) { 218 | inds[j] = rand() % nbr; // get a random point 219 | } 220 | 221 | // check that the points aren't the same 222 | if (inds[0] == inds[1] || inds[0] == inds[2] || inds[1] == inds[2]) { 223 | continue; 224 | } 225 | 226 | compute_plane(plane, cloud, inds); 227 | inliers = 0; 228 | for (int j = 0; j < nbr; j += 30) { // count number of inliers 229 | if (is_inlier(cloud[j].getVector3fMap(), plane, threshold)) { 230 | ++inliers; 231 | } 232 | } 233 | 234 | if (inliers > best_inliers) { 235 | best_plane = plane; 236 | best_inliers = inliers; 237 | } 238 | 239 | if (i % 10 == 0 || i == max - 1) { 240 | feedback.progress = float(i+1)/float(max); 241 | server.publishFeedback(feedback); 242 | } 243 | } 244 | 245 | extract_height_and_angle(best_plane, only_calibrate); // find parameters and feed them to datacentre 246 | //plot_best_plane(cloud, best_plane, threshold); // visually evaluate plane fit 247 | } 248 | 249 | void execute_cb(const calibrate_chest::CalibrateCameraGoalConstPtr& goal) 250 | { 251 | if (goal->command == "calibrate") { 252 | sensor_msgs::PointCloud2::ConstPtr msg = ros::topic::waitForMessage(camera_topic, n, ros::Duration(5)); 253 | if (msg) { 254 | msg_callback(msg); 255 | } 256 | else { 257 | result.status = "Did not receive any point cloud."; 258 | server.setAborted(result); 259 | } 260 | } 261 | else if (goal->command == "publish") { 262 | publish_calibration(); 263 | } 264 | else if (goal->command == "calibrate_publish") { 265 | sensor_msgs::PointCloud2::ConstPtr msg = ros::topic::waitForMessage(camera_topic, n, ros::Duration(5)); 266 | if (msg) { 267 | msg_callback(msg, false); 268 | if (server.isActive()) { 269 | publish_calibration(); 270 | } 271 | } 272 | else { 273 | result.status = "Did not receive any point cloud."; 274 | server.setAborted(result); 275 | } 276 | } 277 | else { 278 | result.status = "Enter command \"calibrate\", \"publish\" or \"calibrate_publish\"."; 279 | server.setAborted(result); 280 | } 281 | } 282 | }; 283 | 284 | int main(int argc, char** argv) 285 | { 286 | ros::init(argc, argv, "calibrate_chest"); 287 | CalibrateCameraServer calibrate(ros::this_node::getName(), "chest_xtion", 46.0); 288 | ros::spin(); 289 | 290 | return 0; 291 | } 292 | --------------------------------------------------------------------------------