├── README.md ├── laser_scan_publisher_tutorial ├── CMakeLists.txt ├── package.xml └── src │ └── laser_scan_publisher.cpp ├── navigation_stage ├── CMakeLists.txt ├── launch │ ├── move_base_amcl_10cm.launch │ ├── move_base_amcl_2.5cm.launch │ ├── move_base_amcl_5cm.launch │ ├── move_base_fake_localization_10cm.launch │ ├── move_base_fake_localization_2.5cm.launch │ ├── move_base_fake_localization_5cm.launch │ ├── move_base_gmapping_5cm.launch │ └── move_base_multi_robot.launch ├── move_base_config │ ├── amcl_node.xml │ ├── base_local_planner_params.yaml │ ├── costmap_common_params.yaml │ ├── dwa_local_planner_params.yaml │ ├── global_costmap_params.yaml │ ├── local_costmap_params.yaml │ ├── move_base.xml │ └── slam_gmapping.xml ├── multi_robot.rviz ├── package.xml ├── single_robot.rviz └── stage_config │ ├── maps │ ├── willow-full-0.025.pgm │ ├── willow-full-0.05.pgm │ └── willow-full.pgm │ └── worlds │ ├── willow-pr2-2.5cm.world │ ├── willow-pr2-5cm.world │ ├── willow-pr2-multi.world │ └── willow-pr2.world ├── navigation_tutorials ├── CMakeLists.txt └── package.xml ├── odometry_publisher_tutorial ├── CMakeLists.txt ├── package.xml └── src │ └── odometry_publisher.cpp ├── point_cloud_publisher_tutorial ├── CMakeLists.txt ├── package.xml └── src │ └── point_cloud_publisher.cpp ├── robot_setup_tf_tutorial ├── CMakeLists.txt ├── manifest.xml ├── package.xml └── src │ ├── tf_broadcaster.cpp │ └── tf_listener.cpp ├── roomba_stage ├── CMakeLists.txt ├── manifest.xml ├── maps │ ├── lse_arena.pgm │ └── lse_arena.yaml ├── move_base_lse_arena.launch ├── package.xml ├── params │ ├── amcl_roomba.launch │ ├── base_local_planner_params.yaml │ ├── costmap_common_params.yaml │ ├── global_costmap_params.yaml │ ├── local_costmap_params.yaml │ └── local_costmap_params_2.yaml ├── roomba_lse_arena.world └── roomba_stage.rviz └── simple_navigation_goals_tutorial ├── CMakeLists.txt ├── package.xml └── src └── simple_navigation_goals.cpp /README.md: -------------------------------------------------------------------------------- 1 | # navigation_tutorials 2 | 3 | Tutorials about using the ROS Navigation stack. 4 | See: 5 | 6 | - http://wiki.ros.org/navigation_tutorials 7 | - http://wiki.ros.org/navigation/Tutorials 8 | -------------------------------------------------------------------------------- /laser_scan_publisher_tutorial/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(laser_scan_publisher_tutorial) 3 | 4 | # Find catkin dependencies 5 | find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs) 6 | 7 | # Call catkin_package 8 | catkin_package() 9 | 10 | # Add catkin_INCLUDE_DIRS to the include path 11 | include_directories(${catkin_INCLUDE_DIRS}) 12 | 13 | # Build the laser_scan_publisher executable 14 | add_executable(laser_scan_publisher src/laser_scan_publisher.cpp) 15 | # Add a build order dependency on sensor_msgs 16 | # This ensures that sensor_msgs' msg headers are built before your executable 17 | if(sensor_msgs_EXPORTED_TARGETS) 18 | add_dependencies(laser_scan_publisher ${sensor_msgs_EXPORTED_TARGETS}) 19 | endif() 20 | # Link against the catkin_LIBRARIES 21 | target_link_libraries(laser_scan_publisher ${catkin_LIBRARIES}) 22 | 23 | # Mark executables and/or libraries for installation 24 | install(TARGETS laser_scan_publisher 25 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 26 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 27 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 28 | -------------------------------------------------------------------------------- /laser_scan_publisher_tutorial/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | laser_scan_publisher_tutorial 4 | 0.2.4 5 | The laser_scan_publisher_tutorial package 6 | 7 | William Woodall 8 | 9 | BSD 10 | 11 | http://ros.org/wiki/laser_scan_publisher_tutorial 12 | https://github.com/ros-planning/navigation_tutorials 13 | https://github.com/ros-planning/navigation_tutorials/issues 14 | 15 | Eitan Marder-Eppstein 16 | 17 | catkin 18 | 19 | roscpp 20 | sensor_msgs 21 | 22 | roscpp 23 | sensor_msgs 24 | 25 | -------------------------------------------------------------------------------- /laser_scan_publisher_tutorial/src/laser_scan_publisher.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2009, 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 Willow Garage, Inc. 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 | #include 38 | #include 39 | 40 | int main(int argc, char** argv){ 41 | ros::init(argc, argv, "laser_scan_publisher"); 42 | 43 | ros::NodeHandle n; 44 | ros::Publisher scan_pub = n.advertise("scan", 50); 45 | 46 | unsigned int num_readings = 100; 47 | double laser_frequency = 40; 48 | double ranges[num_readings]; 49 | double intensities[num_readings]; 50 | 51 | int count = 0; 52 | ros::Rate r(1.0); 53 | while(n.ok()){ 54 | //generate some fake data for our laser scan 55 | for(unsigned int i = 0; i < num_readings; ++i){ 56 | ranges[i] = count; 57 | intensities[i] = 100 + count; 58 | } 59 | ros::Time scan_time = ros::Time::now(); 60 | 61 | //populate the LaserScan message 62 | sensor_msgs::LaserScan scan; 63 | scan.header.stamp = scan_time; 64 | scan.header.frame_id = "laser_frame"; 65 | scan.angle_min = -1.57; 66 | scan.angle_max = 1.57; 67 | scan.angle_increment = 3.14 / num_readings; 68 | scan.time_increment = (1 / laser_frequency) / (num_readings); 69 | scan.range_min = 0.0; 70 | scan.range_max = 100.0; 71 | 72 | scan.ranges.resize(num_readings); 73 | scan.intensities.resize(num_readings); 74 | for(unsigned int i = 0; i < num_readings; ++i){ 75 | scan.ranges[i] = ranges[i]; 76 | scan.intensities[i] = intensities[i]; 77 | } 78 | 79 | scan_pub.publish(scan); 80 | ++count; 81 | r.sleep(); 82 | } 83 | } 84 | -------------------------------------------------------------------------------- /navigation_stage/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(navigation_stage) 3 | 4 | # Find catkin 5 | find_package(catkin REQUIRED) 6 | 7 | catkin_package() 8 | 9 | # Install launch files 10 | install(DIRECTORY launch 11 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 12 | 13 | # Install move_base_config files 14 | install(DIRECTORY move_base_config 15 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 16 | 17 | # Install stage_config files 18 | install(DIRECTORY stage_config 19 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 20 | 21 | # Install the rviz files 22 | install(FILES multi_robot.rviz single_robot.rviz 23 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 24 | -------------------------------------------------------------------------------- /navigation_stage/launch/move_base_amcl_10cm.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /navigation_stage/launch/move_base_amcl_2.5cm.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /navigation_stage/launch/move_base_amcl_5cm.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /navigation_stage/launch/move_base_fake_localization_10cm.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /navigation_stage/launch/move_base_fake_localization_2.5cm.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /navigation_stage/launch/move_base_fake_localization_5cm.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /navigation_stage/launch/move_base_gmapping_5cm.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /navigation_stage/launch/move_base_multi_robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /navigation_stage/move_base_config/amcl_node.xml: -------------------------------------------------------------------------------- 1 | 2 | 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 | -------------------------------------------------------------------------------- /navigation_stage/move_base_config/base_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | #For full documentation of the parameters in this file, and a list of all the 2 | #parameters available for TrajectoryPlannerROS, please see 3 | #http://www.ros.org/wiki/base_local_planner 4 | TrajectoryPlannerROS: 5 | #Set the acceleration limits of the robot 6 | acc_lim_th: 3.2 7 | acc_lim_x: 2.5 8 | acc_lim_y: 2.5 9 | 10 | #Set the velocity limits of the robot 11 | max_vel_x: 0.65 12 | min_vel_x: 0.1 13 | max_rotational_vel: 1.0 14 | min_in_place_rotational_vel: 0.4 15 | 16 | #The velocity the robot will command when trying to escape from a stuck situation 17 | escape_vel: -0.1 18 | 19 | #For this example, we'll use a holonomic robot 20 | holonomic_robot: true 21 | 22 | #Since we're using a holonomic robot, we'll set the set of y velocities it will sample 23 | y_vels: [-0.3, -0.1, 0.1, -0.3] 24 | 25 | #Set the tolerance on achieving a goal 26 | xy_goal_tolerance: 0.1 27 | yaw_goal_tolerance: 0.05 28 | 29 | #We'll configure how long and with what granularity we'll forward simulate trajectories 30 | sim_time: 1.7 31 | sim_granularity: 0.025 32 | vx_samples: 3 33 | vtheta_samples: 20 34 | 35 | #Parameters for scoring trajectories 36 | goal_distance_bias: 0.8 37 | path_distance_bias: 0.6 38 | occdist_scale: 0.01 39 | heading_lookahead: 0.325 40 | 41 | #We'll use the Dynamic Window Approach to control instead of Trajectory Rollout for this example 42 | dwa: true 43 | 44 | #How far the robot must travel before oscillation flags are reset 45 | oscillation_reset_dist: 0.05 46 | 47 | #Eat up the plan as the robot moves along it 48 | prune_plan: true 49 | -------------------------------------------------------------------------------- /navigation_stage/move_base_config/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | #This file contains common configuration options for the two costmaps used in the navigation stack for more details on the parameters in this file, and a full list of the parameters used by the costmaps, please see http://www.ros.org/wiki/costmap_2d 2 | 3 | #For this example we'll configure the costmap in voxel-grid mode 4 | map_type: voxel 5 | 6 | #Voxel grid specific parameters 7 | origin_z: 0.0 8 | z_resolution: 0.2 9 | z_voxels: 10 10 | unknown_threshold: 9 11 | mark_threshold: 0 12 | 13 | #Set the tolerance we're willing to have for tf transforms 14 | transform_tolerance: 0.3 15 | 16 | #Obstacle marking parameters 17 | obstacle_range: 2.5 18 | max_obstacle_height: 2.0 19 | raytrace_range: 3.0 20 | 21 | #The footprint of the robot and associated padding 22 | footprint: [[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]] 23 | footprint_padding: 0.01 24 | 25 | #Cost function parameters 26 | inflation_radius: 0.55 27 | cost_scaling_factor: 10.0 28 | 29 | #The cost at which a cell is considered an obstacle when a map is read from the map_server 30 | lethal_cost_threshold: 100 31 | 32 | #Configuration for the sensors that the costmap will use to update a map 33 | observation_sources: base_scan 34 | base_scan: {data_type: LaserScan, expected_update_rate: 0.4, 35 | observation_persistence: 0.0, marking: true, clearing: true, max_obstacle_height: 0.4, min_obstacle_height: 0.08} 36 | -------------------------------------------------------------------------------- /navigation_stage/move_base_config/dwa_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | #For full documentation of the parameters in this file, and a list of all the 2 | #parameters available for DWAPlannerROS, please see 3 | #http://www.ros.org/wiki/dwa_local_planner 4 | DWAPlannerROS: 5 | acc_lim_th: 3.2 6 | acc_lim_x: 2.5 7 | acc_lim_y: 2.5 8 | 9 | max_vel_x: 0.65 10 | min_vel_x: 0.0 11 | 12 | max_vel_y: 0.1 13 | min_vel_y: -0.1 14 | 15 | max_trans_vel: 0.65 16 | min_trans_vel: 0.1 17 | 18 | max_rot_vel: 1.0 19 | min_rot_vel: 0.4 20 | 21 | sim_time: 1.7 22 | sim_granularity: 0.025 23 | 24 | goal_distance_bias: 32.0 25 | path_distance_bias: 24.0 26 | occdist_scale: 0.01 27 | 28 | stop_time_buffer: 0.2 29 | oscillation_reset_dist: 0.05 30 | 31 | forward_point_distance: 0.325 32 | 33 | scaling_speed: 0.25 34 | max_scaling_factor: 0.2 35 | 36 | vx_samples: 3 37 | vy_samples: 10 38 | vtheta_samples: 20 39 | 40 | sim_period: 0.1 41 | 42 | xy_goal_tolerance: 0.2 43 | yaw_goal_tolerance: 0.17 44 | 45 | rot_stopped_vel: 0.01 46 | trans_stopped_vel: 0.01 47 | -------------------------------------------------------------------------------- /navigation_stage/move_base_config/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | #Independent settings for the global planner's costmap. Detailed descriptions of these parameters can be found at http://www.ros.org/wiki/costmap_2d 2 | 3 | global_costmap: 4 | #Set the global and robot frames for the costmap 5 | global_frame: map 6 | robot_base_frame: base_link 7 | 8 | #Set the update and publish frequency of the costmap 9 | update_frequency: 5.0 10 | publish_frequency: 0.0 11 | 12 | #We'll use a map served by the map_server to initialize this costmap 13 | static_map: true 14 | rolling_window: false 15 | 16 | footprint_padding: 0.02 17 | -------------------------------------------------------------------------------- /navigation_stage/move_base_config/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | #Independent settings for the local planner's costmap. Detailed descriptions of these parameters can be found at http://www.ros.org/wiki/costmap_2d 2 | 3 | local_costmap: 4 | #We'll publish the voxel grid used by this costmap 5 | publish_voxel_map: true 6 | 7 | #Set the global and robot frames for the costmap 8 | global_frame: odom 9 | robot_base_frame: base_link 10 | 11 | #Set the update and publish frequency of the costmap 12 | update_frequency: 5.0 13 | publish_frequency: 2.0 14 | 15 | #We'll configure this costmap to be a rolling window... meaning it is always 16 | #centered at the robot 17 | static_map: false 18 | rolling_window: true 19 | width: 6.0 20 | height: 6.0 21 | resolution: 0.025 22 | origin_x: 0.0 23 | origin_y: 0.0 24 | -------------------------------------------------------------------------------- /navigation_stage/move_base_config/move_base.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /navigation_stage/move_base_config/slam_gmapping.xml: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /navigation_stage/multi_robot.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | Splitter Ratio: 0.5 9 | Tree Height: 510 10 | - Class: rviz/Selection 11 | Name: Selection 12 | - Class: rviz/Tool Properties 13 | Expanded: 14 | - /2D Pose Estimate1 15 | - /2D Nav Goal1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.588679 19 | - Class: rviz/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: "" 29 | Visualization Manager: 30 | Class: "" 31 | Displays: 32 | - Alpha: 0.7 33 | Class: rviz/Map 34 | Color Scheme: map 35 | Draw Behind: false 36 | Enabled: true 37 | Name: Map 38 | Topic: /map 39 | Value: true 40 | - Alpha: 1 41 | Buffer Length: 1 42 | Class: rviz/Path 43 | Color: 255; 149; 0 44 | Enabled: true 45 | Name: "Robot 0: Global Plan" 46 | Topic: /robot_0/move_base_node/TrajectoryPlannerROS/global_plan 47 | Value: true 48 | - Alpha: 1 49 | Buffer Length: 1 50 | Class: rviz/Path 51 | Color: 0; 0; 255 52 | Enabled: true 53 | Name: "Robot 0: Local Plan" 54 | Topic: /robot_0/move_base_node/TrajectoryPlannerROS/local_plan 55 | Value: true 56 | - Alpha: 1 57 | Buffer Length: 1 58 | Class: rviz/Path 59 | Color: 255; 0; 0 60 | Enabled: true 61 | Name: "Robot 0: NavFn Plan" 62 | Topic: /robot_0/move_base_node/NavfnROS/plan 63 | Value: true 64 | - Alpha: 1 65 | Class: rviz/GridCells 66 | Color: 0; 0; 255 67 | Enabled: true 68 | Name: "Robot 0: Inflated Obstacles" 69 | Topic: /robot_0/move_base_node/local_costmap/inflated_obstacles 70 | Value: true 71 | - Alpha: 1 72 | Class: rviz/GridCells 73 | Color: 255; 0; 0 74 | Enabled: true 75 | Name: "Robot 0: Obstacles" 76 | Topic: /robot_0/move_base_node/local_costmap/obstacles 77 | Value: true 78 | - Alpha: 1 79 | Class: rviz/GridCells 80 | Color: 0; 255; 0 81 | Enabled: true 82 | Name: "Robot 0: Unknown Space" 83 | Topic: /robot_0/move_base_node/local_costmap/unknown_space 84 | Value: true 85 | - Alpha: 1 86 | Class: rviz/Polygon 87 | Color: 255; 0; 0 88 | Enabled: true 89 | Name: "Robot 0: Robot Footprint" 90 | Topic: /robot_0/move_base_node/local_costmap/robot_footprint 91 | Value: true 92 | - Alpha: 1 93 | Buffer Length: 1 94 | Class: rviz/Path 95 | Color: 255; 149; 0 96 | Enabled: true 97 | Name: "Robot 1: Global Plan" 98 | Topic: /robot_1/move_base_node/TrajectoryPlannerROS/global_plan 99 | Value: true 100 | - Alpha: 1 101 | Buffer Length: 1 102 | Class: rviz/Path 103 | Color: 0; 0; 255 104 | Enabled: true 105 | Name: "Robot 1: Local Plan" 106 | Topic: /robot_1/move_base_node/TrajectoryPlannerROS/local_plan 107 | Value: true 108 | - Alpha: 1 109 | Buffer Length: 1 110 | Class: rviz/Path 111 | Color: 255; 0; 0 112 | Enabled: true 113 | Name: "Robot 1: NavFn Plan" 114 | Topic: /robot_1/move_base_node/NavfnROS/plan 115 | Value: true 116 | - Alpha: 1 117 | Class: rviz/GridCells 118 | Color: 0; 0; 255 119 | Enabled: true 120 | Name: "Robot 1: Inflated Obstacles" 121 | Topic: /robot_1/move_base_node/local_costmap/inflated_obstacles 122 | Value: true 123 | - Alpha: 1 124 | Class: rviz/GridCells 125 | Color: 255; 0; 0 126 | Enabled: true 127 | Name: "Robot 1: Obstacles" 128 | Topic: /robot_1/move_base_node/local_costmap/obstacles 129 | Value: true 130 | - Alpha: 1 131 | Class: rviz/GridCells 132 | Color: 0; 255; 0 133 | Enabled: true 134 | Name: "Robot 1: Unknown Space" 135 | Topic: /robot_1/move_base_node/local_costmap/unknown_space 136 | Value: true 137 | - Alpha: 1 138 | Class: rviz/Polygon 139 | Color: 255; 0; 0 140 | Enabled: true 141 | Name: "Robot 1: Robot Footprint" 142 | Topic: /robot_1/move_base_node/local_costmap/robot_footprint 143 | Value: true 144 | Enabled: true 145 | Global Options: 146 | Background Color: 48; 48; 48 147 | Fixed Frame: map 148 | Frame Rate: 30 149 | Name: root 150 | Tools: 151 | - Class: rviz/Interact 152 | Hide Inactive Objects: true 153 | - Class: rviz/MoveCamera 154 | - Class: rviz/Select 155 | - Class: rviz/FocusCamera 156 | - Class: rviz/Measure 157 | - Class: rviz/SetInitialPose 158 | Topic: /initialpose 159 | - Class: rviz/SetGoal 160 | Topic: /move_base_simple/goal 161 | - Class: rviz/PublishPoint 162 | Single click: true 163 | Topic: /clicked_point 164 | Value: true 165 | Views: 166 | Current: 167 | Class: rviz/Orbit 168 | Distance: 10 169 | Focal Point: 170 | X: 0 171 | Y: 0 172 | Z: 0 173 | Name: Current View 174 | Near Clip Distance: 0.01 175 | Pitch: 0.555398 176 | Target Frame: 177 | Value: Orbit (rviz) 178 | Yaw: 2.73721 179 | Saved: ~ 180 | Window Geometry: 181 | Displays: 182 | collapsed: false 183 | Height: 756 184 | Hide Left Dock: false 185 | Hide Right Dock: false 186 | QMainWindow State: 000000ff00000000fd0000000400000000000002090000028cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005600fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000002540000011a00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004c0000028c000000dc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000028cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004c0000028c000000a500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004fc0000003efc0100000002fb0000000800540069006d00650000000000000004fc0000020b00fffffffb0000000800540069006d00650100000000000004500000000000000000000001e20000028c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 187 | Selection: 188 | collapsed: false 189 | Time: 190 | collapsed: false 191 | Tool Properties: 192 | collapsed: false 193 | Views: 194 | collapsed: false 195 | Width: 1276 196 | X: 4 197 | Y: 22 198 | -------------------------------------------------------------------------------- /navigation_stage/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | navigation_stage 4 | 0.2.4 5 | 6 | This package holds example launch files for running the ROS navigation stack in stage. 7 | 8 | 9 | William Woodall 10 | 11 | BSD 12 | 13 | http://www.ros.org/wiki/navigation_stage 14 | https://github.com/ros-planning/navigation_tutorials 15 | https://github.com/ros-planning/navigation_tutorials/issues 16 | 17 | Eitan Marder-Eppstein 18 | 19 | catkin 20 | 21 | amcl 22 | fake_localization 23 | gmapping 24 | map_server 25 | move_base 26 | stage_ros 27 | 28 | 29 | -------------------------------------------------------------------------------- /navigation_stage/single_robot.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | Splitter Ratio: 0.5 9 | Tree Height: 485 10 | - Class: rviz/Selection 11 | Name: Selection 12 | - Class: rviz/Tool Properties 13 | Expanded: 14 | - /2D Pose Estimate1 15 | - /2D Nav Goal1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.588679016 19 | - Class: rviz/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: LaserScan 29 | Visualization Manager: 30 | Class: "" 31 | Displays: 32 | - Alpha: 0.699999988 33 | Class: rviz/Map 34 | Color Scheme: map 35 | Draw Behind: false 36 | Enabled: true 37 | Name: Map 38 | Topic: /map 39 | Value: true 40 | - Alpha: 1 41 | Class: rviz/Polygon 42 | Color: 0; 0; 255 43 | Enabled: true 44 | Name: Robot Footprint 45 | Topic: /move_base_node/local_costmap/footprint 46 | Value: true 47 | - Alpha: 1 48 | Buffer Length: 1 49 | Class: rviz/Path 50 | Color: 255; 149; 0 51 | Enabled: true 52 | Line Style: Lines 53 | Line Width: 0.0299999993 54 | Name: Global Plan 55 | Offset: 56 | X: 0 57 | Y: 0 58 | Z: 0 59 | Topic: /move_base_node/TrajectoryPlannerROS/global_plan 60 | Value: true 61 | - Alpha: 1 62 | Buffer Length: 1 63 | Class: rviz/Path 64 | Color: 0; 0; 255 65 | Enabled: true 66 | Line Style: Lines 67 | Line Width: 0.0299999993 68 | Name: Local Plan 69 | Offset: 70 | X: 0 71 | Y: 0 72 | Z: 0 73 | Topic: /move_base_node/TrajectoryPlannerROS/local_plan 74 | Value: true 75 | - Alpha: 1 76 | Buffer Length: 1 77 | Class: rviz/Path 78 | Color: 255; 0; 0 79 | Enabled: true 80 | Line Style: Lines 81 | Line Width: 0.0299999993 82 | Name: NavFn Plan 83 | Offset: 84 | X: 0 85 | Y: 0 86 | Z: 0 87 | Topic: /move_base_node/NavfnROS/plan 88 | Value: true 89 | - Arrow Length: 0.300000012 90 | Class: rviz/PoseArray 91 | Color: 255; 25; 0 92 | Enabled: true 93 | Name: PoseArray 94 | Topic: /particlecloud 95 | Value: true 96 | - Alpha: 0.699999988 97 | Class: rviz/Map 98 | Color Scheme: costmap 99 | Draw Behind: false 100 | Enabled: false 101 | Name: Global Costmap 102 | Topic: /move_base_node/global_costmap/costmap 103 | Value: false 104 | - Alpha: 0.699999988 105 | Class: rviz/Map 106 | Color Scheme: costmap 107 | Draw Behind: false 108 | Enabled: true 109 | Name: Local Costmap 110 | Topic: /move_base_node/local_costmap/costmap 111 | Value: true 112 | - Alpha: 1 113 | Autocompute Intensity Bounds: true 114 | Autocompute Value Bounds: 115 | Max Value: 10 116 | Min Value: -10 117 | Value: true 118 | Axis: Z 119 | Channel Name: intensity 120 | Class: rviz/LaserScan 121 | Color: 255; 255; 0 122 | Color Transformer: FlatColor 123 | Decay Time: 0 124 | Enabled: true 125 | Invert Rainbow: false 126 | Max Color: 255; 255; 255 127 | Max Intensity: 1 128 | Min Color: 0; 0; 0 129 | Min Intensity: 1 130 | Name: LaserScan 131 | Position Transformer: XYZ 132 | Queue Size: 10 133 | Selectable: true 134 | Size (Pixels): 3 135 | Size (m): 0.100000001 136 | Style: Flat Squares 137 | Topic: /base_scan 138 | Use Fixed Frame: true 139 | Use rainbow: true 140 | Value: true 141 | Enabled: true 142 | Global Options: 143 | Background Color: 48; 48; 48 144 | Fixed Frame: map 145 | Frame Rate: 30 146 | Name: root 147 | Tools: 148 | - Class: rviz/Interact 149 | Hide Inactive Objects: true 150 | - Class: rviz/MoveCamera 151 | - Class: rviz/Select 152 | - Class: rviz/FocusCamera 153 | - Class: rviz/Measure 154 | - Class: rviz/SetInitialPose 155 | Topic: /initialpose 156 | - Class: rviz/SetGoal 157 | Topic: /move_base_simple/goal 158 | - Class: rviz/PublishPoint 159 | Single click: true 160 | Topic: /clicked_point 161 | Value: true 162 | Views: 163 | Current: 164 | Class: rviz/ThirdPersonFollower 165 | Distance: 43.5168953 166 | Enable Stereo Rendering: 167 | Stereo Eye Separation: 0.0599999987 168 | Stereo Focal Distance: 1 169 | Swap Stereo Eyes: false 170 | Value: false 171 | Focal Point: 172 | X: 0 173 | Y: 0 174 | Z: 0 175 | Name: Current View 176 | Near Clip Distance: 0.00999999978 177 | Pitch: 0.710398257 178 | Target Frame: base_link 179 | Value: ThirdPersonFollower (rviz) 180 | Yaw: 2.82039738 181 | Saved: ~ 182 | Window Geometry: 183 | Displays: 184 | collapsed: false 185 | Height: 723 186 | Hide Left Dock: false 187 | Hide Right Dock: false 188 | QMainWindow State: 000000ff00000000fd00000004000000000000016400000274fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000002540000011a00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004100000274000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000274fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004100000274000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004fc0000003efc0100000002fb0000000800540069006d00650000000000000004fc0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000027d0000027400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 189 | Selection: 190 | collapsed: false 191 | Time: 192 | collapsed: false 193 | Tool Properties: 194 | collapsed: false 195 | Views: 196 | collapsed: false 197 | Width: 1276 198 | X: 2 199 | Y: 24 200 | -------------------------------------------------------------------------------- /navigation_stage/stage_config/maps/willow-full-0.025.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-planning/navigation_tutorials/115e46ec9e65d49756b5d2155326ad2c1694d1e6/navigation_stage/stage_config/maps/willow-full-0.025.pgm -------------------------------------------------------------------------------- /navigation_stage/stage_config/maps/willow-full-0.05.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-planning/navigation_tutorials/115e46ec9e65d49756b5d2155326ad2c1694d1e6/navigation_stage/stage_config/maps/willow-full-0.05.pgm -------------------------------------------------------------------------------- /navigation_stage/stage_config/maps/willow-full.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-planning/navigation_tutorials/115e46ec9e65d49756b5d2155326ad2c1694d1e6/navigation_stage/stage_config/maps/willow-full.pgm -------------------------------------------------------------------------------- /navigation_stage/stage_config/worlds/willow-pr2-2.5cm.world: -------------------------------------------------------------------------------- 1 | define block model 2 | ( 3 | size [0.500 0.500 0.750] 4 | gui_nose 0 5 | ) 6 | 7 | define topurg ranger 8 | ( 9 | sensor( 10 | range_max 30.0 11 | fov 270.25 12 | samples 1081 13 | ) 14 | # generic model properties 15 | color "black" 16 | size [ 0.050 0.050 0.100 ] 17 | ) 18 | 19 | define pr2 position 20 | ( 21 | size [0.650 0.650 0.250] 22 | origin [-0.050 0 0 0] 23 | gui_nose 1 24 | drive "omni" 25 | topurg(pose [ 0.275 0 0 0 ]) 26 | ) 27 | 28 | define floorplan model 29 | ( 30 | # sombre, sensible, artistic 31 | color "gray30" 32 | 33 | # most maps will need a bounding box 34 | boundary 1 35 | 36 | gui_nose 0 37 | gui_grid 0 38 | 39 | gui_outline 0 40 | gripper_return 0 41 | fiducial_return 0 42 | ranger_return 1 43 | ) 44 | 45 | # set the resolution of the underlying raytrace model in meters 46 | resolution 0.01 47 | 48 | interval_sim 100 # simulation timestep in milliseconds 49 | 50 | 51 | window 52 | ( 53 | size [ 745.000 448.000 ] 54 | 55 | rotate [ 0 -1.560 ] 56 | scale 30.287 57 | ) 58 | 59 | # load an environment bitmap 60 | floorplan 61 | ( 62 | name "willow" 63 | bitmap "../maps/willow-full-0.025.pgm" 64 | size [58.300 45.625 1.000] 65 | pose [ -22.812 29.150 0 90.000 ] 66 | ) 67 | 68 | # throw in a robot 69 | pr2( pose [ -26.068 12.140 0 87.363 ] name "pr2" color "blue") 70 | block( pose [ -25.251 10.586 0 180.000 ] color "red") 71 | -------------------------------------------------------------------------------- /navigation_stage/stage_config/worlds/willow-pr2-5cm.world: -------------------------------------------------------------------------------- 1 | define block model 2 | ( 3 | size [0.5 0.5 0.75] 4 | gui_nose 0 5 | ) 6 | 7 | define topurg ranger 8 | ( 9 | sensor( 10 | range_max 30.0 11 | fov 270.25 12 | samples 1081 13 | ) 14 | # generic model properties 15 | color "black" 16 | size [ 0.05 0.05 0.1 ] 17 | ) 18 | 19 | define pr2 position 20 | ( 21 | size [0.65 0.65 0.25] 22 | origin [-0.05 0 0 0] 23 | gui_nose 1 24 | drive "omni" 25 | topurg(pose [ 0.275 0.000 0 0.000 ]) 26 | ) 27 | 28 | define floorplan model 29 | ( 30 | # sombre, sensible, artistic 31 | color "gray30" 32 | 33 | # most maps will need a bounding box 34 | boundary 1 35 | 36 | gui_nose 0 37 | gui_grid 0 38 | 39 | gui_outline 0 40 | gripper_return 0 41 | fiducial_return 0 42 | ranger_return 1 43 | ) 44 | 45 | # set the resolution of the underlying raytrace model in meters 46 | resolution 0.02 47 | 48 | interval_sim 100 # simulation timestep in milliseconds 49 | 50 | 51 | window 52 | ( 53 | size [ 745.000 448.000 ] 54 | 55 | rotate [ 0.000 -1.560 ] 56 | scale 18.806 57 | ) 58 | 59 | # load an environment bitmap 60 | floorplan 61 | ( 62 | name "willow" 63 | bitmap "../maps/willow-full-0.05.pgm" 64 | size [58.25 47.25 1.0] 65 | pose [ -23.625 29.125 0 90.000 ] 66 | ) 67 | 68 | # throw in a robot 69 | pr2( pose [ -21.670 47.120 0 28.166 ] name "pr2" color "blue") 70 | block( pose [ -24.269 48.001 0 180.000 ] color "red") 71 | -------------------------------------------------------------------------------- /navigation_stage/stage_config/worlds/willow-pr2-multi.world: -------------------------------------------------------------------------------- 1 | define block model 2 | ( 3 | size [0.5 0.5 0.5] 4 | gui_nose 0 5 | ) 6 | 7 | define topurg ranger 8 | ( 9 | sensor( 10 | range_max 30.0 11 | fov 270.25 12 | samples 1081 13 | ) 14 | # generic model properties 15 | color "black" 16 | size [ 0.05 0.05 0.1 ] 17 | ) 18 | 19 | define pr2 position 20 | ( 21 | size [0.65 0.65 0.25] 22 | origin [-0.05 0 0 0] 23 | gui_nose 1 24 | drive "omni" 25 | topurg(pose [ 0.275 0.000 0 0.000 ]) 26 | ) 27 | 28 | define floorplan model 29 | ( 30 | # sombre, sensible, artistic 31 | color "gray30" 32 | 33 | # most maps will need a bounding box 34 | boundary 1 35 | 36 | gui_nose 0 37 | gui_grid 0 38 | 39 | gui_outline 0 40 | gripper_return 0 41 | fiducial_return 0 42 | ranger_return 1 43 | ) 44 | 45 | # set the resolution of the underlying raytrace model in meters 46 | resolution 0.02 47 | 48 | interval_sim 100 # simulation timestep in milliseconds 49 | 50 | 51 | window 52 | ( 53 | size [ 745.000 448.000 ] 54 | 55 | rotate [ 0.000 0.000 ] 56 | scale 28.806 57 | ) 58 | 59 | # load an environment bitmap 60 | floorplan 61 | ( 62 | name "willow" 63 | bitmap "../maps/willow-full.pgm" 64 | size [58.4 52.6 0.5] 65 | pose [ -26.300 29.200 0 90.000 ] 66 | ) 67 | 68 | # throw in a robot 69 | pr2( pose [ -21.670 47.120 0 28.166 ] name "pr2_0" color "blue") 70 | pr2( pose [ -21.670 48.120 0 28.166 ] name "pr2_1" color "green") 71 | block( pose [ -24.269 48.001 0 180.000 ] color "red") 72 | -------------------------------------------------------------------------------- /navigation_stage/stage_config/worlds/willow-pr2.world: -------------------------------------------------------------------------------- 1 | define block model 2 | ( 3 | size [0.5 0.5 0.5] 4 | gui_nose 0 5 | ) 6 | 7 | define topurg ranger 8 | ( 9 | sensor( 10 | range_max 30.0 11 | fov 270.25 12 | samples 1081 13 | ) 14 | # generic model properties 15 | color "black" 16 | size [ 0.05 0.05 0.1 ] 17 | ) 18 | 19 | define pr2 position 20 | ( 21 | size [0.65 0.65 0.25] 22 | origin [-0.05 0 0 0] 23 | gui_nose 1 24 | drive "omni" 25 | topurg(pose [ 0.275 0.000 0 0.000 ]) 26 | ) 27 | 28 | define floorplan model 29 | ( 30 | # sombre, sensible, artistic 31 | color "gray30" 32 | 33 | # most maps will need a bounding box 34 | boundary 1 35 | 36 | gui_nose 0 37 | gui_grid 0 38 | 39 | gui_outline 0 40 | gripper_return 0 41 | fiducial_return 0 42 | ranger_return 1 43 | ) 44 | 45 | # set the resolution of the underlying raytrace model in meters 46 | resolution 0.02 47 | 48 | interval_sim 100 # simulation timestep in milliseconds 49 | 50 | 51 | window 52 | ( 53 | size [ 745.000 448.000 ] 54 | 55 | rotate [ 0.000 0.000 ] 56 | scale 28.806 57 | ) 58 | 59 | # load an environment bitmap 60 | floorplan 61 | ( 62 | name "willow" 63 | bitmap "../maps/willow-full.pgm" 64 | size [58.4 52.6 0.5] 65 | pose [ -26.300 29.200 0 90.000 ] 66 | ) 67 | 68 | # throw in a robot 69 | pr2( pose [ -26.068 12.140 0 87.363 ] name "pr2" color "blue") 70 | block( pose [ -25.251 10.586 0 180.000 ] color "red") 71 | -------------------------------------------------------------------------------- /navigation_tutorials/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(navigation_tutorials) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /navigation_tutorials/package.xml: -------------------------------------------------------------------------------- 1 | 2 | navigation_tutorials 3 | 0.2.4 4 | 5 | Navigation related tutorials. 6 | 7 | William Woodall 8 | BSD 9 | 10 | http://www.ros.org/wiki/navigation_tutorials 11 | https://github.com/ros-planning/navigation_tutorials 12 | https://github.com/ros-planning/navigation_tutorials/issues 13 | 14 | catkin 15 | 16 | laser_scan_publisher_tutorial 17 | navigation_stage 18 | odometry_publisher_tutorial 19 | point_cloud_publisher_tutorial 20 | robot_setup_tf_tutorial 21 | roomba_stage 22 | simple_navigation_goals_tutorial 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /odometry_publisher_tutorial/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(odometry_publisher_tutorial) 3 | 4 | find_package(catkin REQUIRED COMPONENTS nav_msgs roscpp tf) 5 | 6 | catkin_package() 7 | 8 | include_directories(${catkin_INCLUDE_DIRS}) 9 | 10 | # Build the executable 11 | add_executable(odometry_publisher src/odometry_publisher.cpp) 12 | # Add a build order dependency on nav_msgs 13 | # This ensures that nav_msgs' msg headers are built before your executable 14 | if(nav_msgs_EXPORTED_TARGETS) 15 | add_dependencies(odometry_publisher ${nav_msgs_EXPORTED_TARGETS}) 16 | endif() 17 | # Link against the catkin libraries 18 | target_link_libraries(odometry_publisher ${catkin_LIBRARIES}) 19 | 20 | # Install the executable 21 | install(TARGETS odometry_publisher 22 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 23 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 24 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 25 | -------------------------------------------------------------------------------- /odometry_publisher_tutorial/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | odometry_publisher_tutorial 4 | 0.2.4 5 | The odometry_publisher_tutorial package 6 | 7 | William Woodall 8 | 9 | BSD 10 | 11 | http://ros.org/wiki/odometry_publisher_tutorial 12 | https://github.com/ros-planning/navigation_tutorials 13 | https://github.com/ros-planning/navigation_tutorials/issues 14 | 15 | Eitan Marder-Eppstein 16 | 17 | catkin 18 | 19 | nav_msgs 20 | roscpp 21 | tf 22 | 23 | nav_msgs 24 | roscpp 25 | tf 26 | 27 | -------------------------------------------------------------------------------- /odometry_publisher_tutorial/src/odometry_publisher.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2009, 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 Willow Garage, Inc. 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 | #include 38 | #include 39 | #include 40 | 41 | int main(int argc, char** argv){ 42 | ros::init(argc, argv, "odometry_publisher"); 43 | 44 | ros::NodeHandle n; 45 | ros::Publisher odom_pub = n.advertise("odom", 50); 46 | tf::TransformBroadcaster odom_broadcaster; 47 | 48 | double x = 0.0; 49 | double y = 0.0; 50 | double th = 0.0; 51 | 52 | double vx = 0.1; 53 | double vy = -0.1; 54 | double vth = 0.1; 55 | 56 | ros::Time current_time, last_time; 57 | current_time = ros::Time::now(); 58 | last_time = ros::Time::now(); 59 | 60 | ros::Rate r(1.0); 61 | while(n.ok()){ 62 | current_time = ros::Time::now(); 63 | 64 | //compute odometry in a typical way given the velocities of the robot 65 | double dt = (current_time - last_time).toSec(); 66 | double delta_x = (vx * cos(th) - vy * sin(th)) * dt; 67 | double delta_y = (vx * sin(th) + vy * cos(th)) * dt; 68 | double delta_th = vth * dt; 69 | 70 | x += delta_x; 71 | y += delta_y; 72 | th += delta_th; 73 | 74 | //since all odometry is 6DOF we'll need a quaternion created from yaw 75 | geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th); 76 | 77 | //first, we'll publish the transform over tf 78 | geometry_msgs::TransformStamped odom_trans; 79 | odom_trans.header.stamp = current_time; 80 | odom_trans.header.frame_id = "odom"; 81 | odom_trans.child_frame_id = "base_link"; 82 | 83 | odom_trans.transform.translation.x = x; 84 | odom_trans.transform.translation.y = y; 85 | odom_trans.transform.translation.z = 0.0; 86 | odom_trans.transform.rotation = odom_quat; 87 | 88 | //send the transform 89 | odom_broadcaster.sendTransform(odom_trans); 90 | 91 | //next, we'll publish the odometry message over ROS 92 | nav_msgs::Odometry odom; 93 | odom.header.stamp = current_time; 94 | odom.header.frame_id = "odom"; 95 | odom.child_frame_id = "base_link"; 96 | 97 | //set the position 98 | odom.pose.pose.position.x = x; 99 | odom.pose.pose.position.y = y; 100 | odom.pose.pose.position.z = 0.0; 101 | odom.pose.pose.orientation = odom_quat; 102 | 103 | //set the velocity 104 | odom.twist.twist.linear.x = vx; 105 | odom.twist.twist.linear.y = vy; 106 | odom.twist.twist.angular.z = vth; 107 | 108 | //publish the message 109 | odom_pub.publish(odom); 110 | 111 | last_time = current_time; 112 | r.sleep(); 113 | } 114 | } 115 | -------------------------------------------------------------------------------- /point_cloud_publisher_tutorial/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(point_cloud_publisher_tutorial) 3 | 4 | find_package(catkin REQUIRED COMPONENTS sensor_msgs roscpp) 5 | 6 | catkin_package() 7 | 8 | include_directories(${catkin_INCLUDE_DIRS}) 9 | 10 | # Build the executable 11 | add_executable(point_cloud_publisher src/point_cloud_publisher.cpp) 12 | # Add a build order dependency on sensor_msgs 13 | # This ensures that sensor_msgs' msg headers are built before your executable 14 | if(sensor_msgs_EXPORTED_TARGETS) 15 | add_dependencies(point_cloud_publisher ${sensor_msgs_EXPORTED_TARGETS}) 16 | endif() 17 | # Link against the catkin libraries 18 | target_link_libraries(point_cloud_publisher ${catkin_LIBRARIES}) 19 | 20 | # Install the executable 21 | install(TARGETS point_cloud_publisher 22 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 23 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 24 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 25 | -------------------------------------------------------------------------------- /point_cloud_publisher_tutorial/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | point_cloud_publisher_tutorial 4 | 0.2.4 5 | The point_cloud_publisher_tutorial package 6 | 7 | William Woodall 8 | 9 | BSD 10 | 11 | http://ros.org/wiki/point_cloud_publisher_tutorial 12 | https://github.com/ros-planning/navigation_tutorials 13 | https://github.com/ros-planning/navigation_tutorials/issues 14 | 15 | Eitan Marder-Eppstein 16 | 17 | catkin 18 | 19 | sensor_msgs 20 | roscpp 21 | 22 | sensor_msgs 23 | roscpp 24 | 25 | -------------------------------------------------------------------------------- /point_cloud_publisher_tutorial/src/point_cloud_publisher.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2009, 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 Willow Garage, Inc. 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 | #include 38 | #include 39 | 40 | int main(int argc, char** argv){ 41 | ros::init(argc, argv, "point_cloud_publisher"); 42 | 43 | ros::NodeHandle n; 44 | ros::Publisher cloud_pub = n.advertise("cloud", 50); 45 | 46 | unsigned int num_points = 100; 47 | 48 | int count = 0; 49 | ros::Rate r(1.0); 50 | while(n.ok()){ 51 | sensor_msgs::PointCloud cloud; 52 | cloud.header.stamp = ros::Time::now(); 53 | cloud.header.frame_id = "sensor_frame"; 54 | 55 | cloud.points.resize(num_points); 56 | 57 | //we'll also add an intensity channel to the cloud 58 | cloud.channels.resize(1); 59 | cloud.channels[0].name = "intensities"; 60 | cloud.channels[0].values.resize(num_points); 61 | 62 | //generate some fake data for our point cloud 63 | for(unsigned int i = 0; i < num_points; ++i){ 64 | cloud.points[i].x = 1 + count; 65 | cloud.points[i].y = 2 + count; 66 | cloud.points[i].z = 3 + count; 67 | cloud.channels[0].values[i] = 100 + count; 68 | } 69 | 70 | cloud_pub.publish(cloud); 71 | ++count; 72 | r.sleep(); 73 | } 74 | } 75 | -------------------------------------------------------------------------------- /robot_setup_tf_tutorial/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(robot_setup_tf_tutorial) 3 | 4 | find_package(catkin REQUIRED COMPONENTS geometry_msgs roscpp tf) 5 | 6 | catkin_package() 7 | 8 | include_directories(${catkin_INCLUDE_DIRS}) 9 | 10 | # Build the tf_broadcaster executable 11 | add_executable(tf_broadcaster src/tf_broadcaster.cpp) 12 | # Add a build order dependency on geometry_msgs 13 | # This ensures that geometry_msgs' msg headers are built before your executable 14 | if(geometry_msgs_EXPORTED_TARGETS) 15 | add_dependencies(tf_broadcaster ${geometry_msgs_EXPORTED_TARGETS}) 16 | endif() 17 | # Link against the catkin libraries 18 | target_link_libraries(tf_broadcaster ${catkin_LIBRARIES}) 19 | 20 | # Build the tf_listener executable 21 | add_executable(tf_listener src/tf_listener.cpp) 22 | # Add a build order dependency on geometry_msgs 23 | # This ensures that geometry_msgs' msg headers are built before your executable 24 | if(geometry_msgs_EXPORTED_TARGETS) 25 | add_dependencies(tf_listener ${geometry_msgs_EXPORTED_TARGETS}) 26 | endif() 27 | # Link against the catkin libraries 28 | target_link_libraries(tf_listener ${catkin_LIBRARIES}) 29 | 30 | # Install the executable 31 | install(TARGETS tf_broadcaster tf_listener 32 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 33 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 34 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 35 | -------------------------------------------------------------------------------- /robot_setup_tf_tutorial/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | robot_setup_tf 5 | 6 | 7 | Eitan Marder-Eppstein 8 | BSD 9 | 10 | http://pr.willowgarage.com/wiki/robot_tf_publisher 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /robot_setup_tf_tutorial/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | robot_setup_tf_tutorial 4 | 0.2.4 5 | The robot_setup_tf_tutorial package 6 | 7 | William Woodall 8 | 9 | BSD 10 | 11 | http://ros.org/wiki/robot_setup_tf_tutorial 12 | https://github.com/ros-planning/navigation_tutorials 13 | https://github.com/ros-planning/navigation_tutorials/issues 14 | 15 | Eitan Marder-Eppstein 16 | 17 | catkin 18 | 19 | geometry_msgs 20 | roscpp 21 | tf 22 | 23 | geometry_msgs 24 | roscpp 25 | tf 26 | 27 | -------------------------------------------------------------------------------- /robot_setup_tf_tutorial/src/tf_broadcaster.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 Willow Garage, Inc. 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 | #include 38 | #include 39 | 40 | int main(int argc, char** argv){ 41 | ros::init(argc, argv, "robot_tf_publisher"); 42 | ros::NodeHandle n; 43 | 44 | ros::Rate r(100); 45 | 46 | tf::TransformBroadcaster broadcaster; 47 | 48 | while(n.ok()){ 49 | broadcaster.sendTransform( 50 | tf::StampedTransform(tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)), 51 | ros::Time::now(), 52 | "base_link", 53 | "base_laser")); 54 | r.sleep(); 55 | } 56 | } 57 | -------------------------------------------------------------------------------- /robot_setup_tf_tutorial/src/tf_listener.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 Willow Garage, Inc. 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 | #include 38 | #include 39 | #include 40 | 41 | void transformPoint(const tf::TransformListener& listener){ 42 | //we'll create a point in the base_laser frame that we'd like to transform to the base_link frame 43 | geometry_msgs::PointStamped laser_point; 44 | laser_point.header.frame_id = "base_laser"; 45 | 46 | //we'll just use the most recent transform available for our simple example 47 | laser_point.header.stamp = ros::Time(); 48 | 49 | //just an arbitrary point in space 50 | laser_point.point.x = 1.0; 51 | laser_point.point.y = 0.2; 52 | laser_point.point.z = 0.0; 53 | 54 | try{ 55 | geometry_msgs::PointStamped base_point; 56 | listener.transformPoint("base_link", laser_point, base_point); 57 | 58 | ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f", 59 | laser_point.point.x, laser_point.point.y, laser_point.point.z, 60 | base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec()); 61 | } 62 | catch(tf::TransformException& ex){ 63 | ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what()); 64 | } 65 | } 66 | 67 | int main(int argc, char** argv){ 68 | ros::init(argc, argv, "robot_tf_listener"); 69 | ros::NodeHandle n; 70 | 71 | tf::TransformListener listener(ros::Duration(10)); 72 | 73 | //we'll transform a point once every second 74 | ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPoint, boost::ref(listener))); 75 | 76 | ros::spin(); 77 | 78 | } 79 | -------------------------------------------------------------------------------- /roomba_stage/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(roomba_stage) 3 | 4 | # Find catkin 5 | find_package(catkin REQUIRED) 6 | 7 | catkin_package() 8 | 9 | # Install maps files 10 | install(DIRECTORY maps 11 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 12 | 13 | # Install params files 14 | install(DIRECTORY params 15 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 16 | 17 | # Install other files 18 | install(FILES move_base_lse_arena.launch roomba_lse_arena.world roomba_stage.rviz 19 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 20 | -------------------------------------------------------------------------------- /roomba_stage/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | roomba_stage 5 | 6 | 7 | Gonçalo Cabrita 8 | BSD 9 | 10 | http://ros.org/wiki/roomba_stage 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /roomba_stage/maps/lse_arena.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-planning/navigation_tutorials/115e46ec9e65d49756b5d2155326ad2c1694d1e6/roomba_stage/maps/lse_arena.pgm -------------------------------------------------------------------------------- /roomba_stage/maps/lse_arena.yaml: -------------------------------------------------------------------------------- 1 | image: lse_arena.pgm 2 | resolution: 0.050000 3 | origin: [0.000000, 0.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /roomba_stage/move_base_lse_arena.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /roomba_stage/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | roomba_stage 4 | 0.2.4 5 | The roomba_stage package 6 | 7 | William Woodall 8 | 9 | BSD 10 | 11 | http://ros.org/wiki/roomba_stage 12 | https://github.com/ros-planning/navigation_tutorials 13 | https://github.com/ros-planning/navigation_tutorials/issues 14 | 15 | Gonçalo Cabrita 16 | 17 | catkin 18 | 19 | fake_localization 20 | map_server 21 | move_base 22 | stage_ros 23 | 24 | 25 | -------------------------------------------------------------------------------- /roomba_stage/params/amcl_roomba.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 | -------------------------------------------------------------------------------- /roomba_stage/params/base_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | controller_frequency: 5.0 2 | TrajectoryPlannerROS: 3 | max_vel_x: 0.20 4 | min_vel_x: 0.10 5 | max_rotational_vel: 1.5 6 | min_in_place_rotational_vel: 0.1 7 | acc_lim_th: 0.75 8 | acc_lim_x: 0.50 9 | acc_lim_y: 0.50 10 | 11 | holonomic_robot: false 12 | yaw_goal_tolerance: 0.05 13 | xy_goal_tolerance: 0.1 14 | goal_distance_bias: 0.8 15 | path_distance_bias: 0.6 16 | sim_time: 1.5 17 | heading_lookahead: 0.325 18 | oscillation_reset_dist: 0.05 19 | 20 | vx_samples: 6 21 | vtheta_samples: 20 22 | dwa: false 23 | 24 | -------------------------------------------------------------------------------- /roomba_stage/params/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | map_type: voxel 2 | z_voxels: 10 3 | unknown_threshold: 8 4 | 5 | obstacle_range: 2.5 6 | raytrace_range: 3.0 7 | robot_radius: 0.17 8 | #footprint: [[0.17, 0.17], [-0.17, 0.17], [-0.17, -0.17], [0.17, -0.17]] 9 | inflation_radius: 0.18 10 | observation_sources: laser_scan_sensor 11 | laser_scan_sensor: {sensor_frame: /base_laser_link, data_type: LaserScan, topic: /base_scan, marking: true, clearing: true} 12 | -------------------------------------------------------------------------------- /roomba_stage/params/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: /odom 3 | robot_base_frame: /base_link 4 | update_frequency: 3.0 5 | publish_frequency: 0.0 6 | #static_map: true 7 | -------------------------------------------------------------------------------- /roomba_stage/params/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: /odom 3 | robot_base_frame: /base_link 4 | update_frequency: 3.0 5 | publish_frequency: 2.0 6 | static_map: false 7 | rolling_window: true 8 | width: 4.0 9 | height: 4.0 10 | resolution: 0.05 11 | -------------------------------------------------------------------------------- /roomba_stage/params/local_costmap_params_2.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: /odom 3 | robot_base_frame: /base_link 4 | update_frequency: 3.0 5 | publish_frequency: 2.0 6 | static_map: false 7 | rolling_window: true 8 | width: 4.0 9 | height: 4.0 10 | resolution: 0.05 11 | -------------------------------------------------------------------------------- /roomba_stage/roomba_lse_arena.world: -------------------------------------------------------------------------------- 1 | # lse-roomba.world - basic world file for the roomba 2 | # Authors: Gonçalo Cabrita 3 | 4 | define hokuyo ranger 5 | ( 6 | sensor( 7 | range_min 0.0 8 | range_max 2.0 9 | fov 270.25 10 | samples 1081 11 | ) 12 | color "black" 13 | size [ 0.05 0.05 0.1 ] 14 | ) 15 | 16 | define roomba position 17 | ( 18 | size [0.33 0.33 0.1] 19 | 20 | # This block approximates the circular shape of a Roomba 21 | block 22 | ( 23 | points 16 24 | point[0] [ 0.225 0.000 ] 25 | point[1] [ 0.208 0.086 ] 26 | point[2] [ 0.159 0.159 ] 27 | point[3] [ 0.086 0.208 ] 28 | point[4] [ 0.000 0.225 ] 29 | point[5] [ -0.086 0.208 ] 30 | point[6] [ -0.159 0.159 ] 31 | point[7] [ -0.208 0.086 ] 32 | point[8] [ -0.225 0.000 ] 33 | point[9] [ -0.208 -0.086 ] 34 | point[10] [ -0.159 -0.159 ] 35 | point[11] [ -0.086 -0.208 ] 36 | point[12] [ -0.000 -0.225 ] 37 | point[13] [ 0.086 -0.208 ] 38 | point[14] [ 0.159 -0.159 ] 39 | point[15] [ 0.208 -0.086 ] 40 | 41 | color "gray50" 42 | ) 43 | 44 | hokuyo( pose [0 0 0.1 0] ) 45 | color "gray50" 46 | ) 47 | 48 | define floorplan model 49 | ( 50 | # Sombre, sensible, artistic 51 | color "gray30" 52 | 53 | # Most maps will need a bounding box 54 | boundary 1 55 | 56 | gui_nose 0 57 | gui_grid 0 58 | gui_move 0 59 | gui_outline 0 60 | gripper_return 0 61 | fiducial_return 0 62 | laser_return 1 63 | ) 64 | 65 | # Set the resolution of the underlying raytrace model in meters 66 | resolution 0.02 67 | 68 | interval_sim 100 # simulation timestep in milliseconds 69 | 70 | # Configure the GUI window 71 | window 72 | ( 73 | size [ 808.000 600.000 ] # in pixels 74 | scale 20 # pixels per meter 75 | center [ 2.0 1.5 ] 76 | rotate [ 0 0 ] 77 | 78 | show_data 1 # 1=on 0=off 79 | ) 80 | 81 | # load an environment bitmap 82 | floorplan 83 | ( 84 | name "lab_maze" 85 | size [4.0 3.0 1.00] 86 | pose [-1.5 2.0 0 90.000] 87 | bitmap "./maps/lse_arena.pgm" 88 | ) 89 | 90 | roomba 91 | ( 92 | # Can refer to the robot by this name 93 | name "roomba0" 94 | pose [ -1.0 1.0 0 0.0 ] 95 | 96 | drive "diff" 97 | 98 | # Report error-free position in world coordinates 99 | localization "gps" 100 | localization_origin [ 0 0 0 0 ] 101 | ) 102 | 103 | -------------------------------------------------------------------------------- /roomba_stage/roomba_stage.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | Splitter Ratio: 0.5 9 | Tree Height: 510 10 | - Class: rviz/Selection 11 | Name: Selection 12 | - Class: rviz/Tool Properties 13 | Expanded: 14 | - /2D Pose Estimate1 15 | - /2D Nav Goal1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.588679 19 | - Class: rviz/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: LaserScan 29 | Visualization Manager: 30 | Class: "" 31 | Displays: 32 | - Alpha: 0.7 33 | Class: rviz/Map 34 | Color Scheme: map 35 | Draw Behind: false 36 | Enabled: true 37 | Name: Map 38 | Topic: /map 39 | Value: true 40 | - Alpha: 1 41 | Class: rviz/Polygon 42 | Color: 0; 0; 255 43 | Enabled: true 44 | Name: Footprint 45 | Topic: /move_base/global_costmap/footprint_layer/footprint_stamped 46 | Value: true 47 | - Alpha: 1 48 | Class: rviz/GridCells 49 | Color: 0; 0; 255 50 | Enabled: true 51 | Name: Inflated Obstacles 52 | Topic: /move_base/local_costmap/inflated_obstacles 53 | Value: true 54 | - Alpha: 1 55 | Class: rviz/GridCells 56 | Color: 0; 255; 0 57 | Enabled: true 58 | Name: Obstacles 59 | Topic: /move_base/local_costmap/obstacles 60 | Value: true 61 | - Alpha: 1 62 | Buffer Length: 1 63 | Class: rviz/Path 64 | Color: 204; 0; 255 65 | Enabled: true 66 | Name: Local Plan 67 | Topic: /move_base/TrajectoryPlannerROS/local_plan 68 | Value: true 69 | - Alpha: 1 70 | Buffer Length: 1 71 | Class: rviz/Path 72 | Color: 0; 255; 0 73 | Enabled: true 74 | Name: Plan 75 | Topic: /move_base/NavfnROS/plan 76 | Value: true 77 | - Class: rviz/TF 78 | Enabled: false 79 | Frame Timeout: 15 80 | Frames: 81 | All Enabled: true 82 | Marker Scale: 1 83 | Name: TF 84 | Show Arrows: true 85 | Show Axes: true 86 | Show Names: true 87 | Tree: 88 | {} 89 | Update Interval: 0 90 | Value: false 91 | - Alpha: 1 92 | Autocompute Intensity Bounds: true 93 | Autocompute Value Bounds: 94 | Max Value: 10 95 | Min Value: -10 96 | Value: true 97 | Axis: Z 98 | Channel Name: intensity 99 | Class: rviz/LaserScan 100 | Color: 255; 255; 255 101 | Color Transformer: Intensity 102 | Decay Time: 0 103 | Enabled: true 104 | Max Color: 255; 255; 255 105 | Max Intensity: 1 106 | Min Color: 0; 0; 0 107 | Min Intensity: 1 108 | Name: LaserScan 109 | Position Transformer: XYZ 110 | Queue Size: 10 111 | Selectable: true 112 | Size (Pixels): 3 113 | Size (m): 0.01 114 | Style: Flat Squares 115 | Topic: /base_scan 116 | Use Fixed Frame: true 117 | Use rainbow: true 118 | Value: true 119 | - Alpha: 1 120 | Axes Length: 1 121 | Axes Radius: 0.1 122 | Class: rviz/Pose 123 | Color: 255; 25; 0 124 | Enabled: true 125 | Head Length: 0.1 126 | Head Radius: 0.05 127 | Name: Pose 128 | Shaft Length: 0.25 129 | Shaft Radius: 0.025 130 | Shape: Arrow 131 | Topic: /move_base/current_goal 132 | Value: true 133 | Enabled: true 134 | Global Options: 135 | Background Color: 48; 48; 48 136 | Fixed Frame: map 137 | Frame Rate: 30 138 | Name: root 139 | Tools: 140 | - Class: rviz/Interact 141 | Hide Inactive Objects: true 142 | - Class: rviz/MoveCamera 143 | - Class: rviz/Select 144 | - Class: rviz/FocusCamera 145 | - Class: rviz/Measure 146 | - Class: rviz/SetInitialPose 147 | Topic: /initialpose 148 | - Class: rviz/SetGoal 149 | Topic: /move_base_simple/goal 150 | - Class: rviz/PublishPoint 151 | Single click: true 152 | Topic: /clicked_point 153 | Value: true 154 | Views: 155 | Current: 156 | Class: rviz/Orbit 157 | Distance: 4.91901 158 | Focal Point: 159 | X: 0 160 | Y: 0 161 | Z: 0 162 | Name: Current View 163 | Near Clip Distance: 0.01 164 | Pitch: 0.645398 165 | Target Frame: base_footprint 166 | Value: Orbit (rviz) 167 | Yaw: 0.275398 168 | Saved: ~ 169 | Window Geometry: 170 | Displays: 171 | collapsed: false 172 | Height: 756 173 | Hide Left Dock: false 174 | Hide Right Dock: false 175 | QMainWindow State: 000000ff00000000fd0000000400000000000001640000028cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005600fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000002540000011a00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004c0000028c000000dc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000028cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004c0000028c000000a500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004fc0000003efc0100000002fb0000000800540069006d00650000000000000004fc0000020b00fffffffb0000000800540069006d00650100000000000004500000000000000000000002870000028c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 176 | Selection: 177 | collapsed: false 178 | Time: 179 | collapsed: false 180 | Tool Properties: 181 | collapsed: false 182 | Views: 183 | collapsed: false 184 | Width: 1276 185 | X: 4 186 | Y: 22 187 | -------------------------------------------------------------------------------- /simple_navigation_goals_tutorial/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(simple_navigation_goals_tutorial) 3 | 4 | find_package(catkin REQUIRED COMPONENTS actionlib move_base_msgs roscpp tf) 5 | 6 | catkin_package() 7 | 8 | include_directories(${catkin_INCLUDE_DIRS}) 9 | 10 | # Build the executable 11 | add_executable(simple_navigation_goals src/simple_navigation_goals.cpp) 12 | # Add a build order dependency on nav_msgs 13 | # This ensures that all msg headers are built before your executable 14 | if(catkin_EXPORTED_TARGETS) 15 | add_dependencies(simple_navigation_goals ${catkin_EXPORTED_TARGETS}) 16 | endif() 17 | # Link against the catkin libraries 18 | target_link_libraries(simple_navigation_goals ${catkin_LIBRARIES}) 19 | 20 | # Install the executable 21 | install(TARGETS simple_navigation_goals 22 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 23 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 24 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 25 | -------------------------------------------------------------------------------- /simple_navigation_goals_tutorial/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | simple_navigation_goals_tutorial 4 | 0.2.4 5 | The simple_navigation_goals_tutorial package 6 | 7 | William Woodall 8 | 9 | BSD 10 | 11 | http://ros.org/wiki/simple_navigation_goals_tutorial 12 | https://github.com/ros-planning/navigation_tutorials 13 | https://github.com/ros-planning/navigation_tutorials/issues 14 | 15 | Eitan Marder-Eppstein 16 | 17 | catkin 18 | 19 | actionlib 20 | move_base_msgs 21 | roscpp 22 | tf 23 | 24 | actionlib 25 | move_base_msgs 26 | roscpp 27 | tf 28 | 29 | -------------------------------------------------------------------------------- /simple_navigation_goals_tutorial/src/simple_navigation_goals.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 Willow Garage, Inc. 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 | * For a discussion of this tutorial, please see: 38 | * http://pr.willowgarage.com/wiki/navigation/Tutorials/SendingSimpleGoals 39 | *********************************************************************/ 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | #include 46 | 47 | typedef actionlib::SimpleActionClient MoveBaseClient; 48 | 49 | void spinThread(){ 50 | ros::spin(); 51 | } 52 | 53 | int main(int argc, char** argv){ 54 | ros::init(argc, argv, "simple_navigation_goals"); 55 | 56 | ros::NodeHandle n; 57 | 58 | boost::thread spin_thread = boost::thread(boost::bind(&spinThread)); 59 | 60 | MoveBaseClient ac("pose_base_controller"); 61 | 62 | //give some time for connections to register 63 | sleep(2.0); 64 | 65 | move_base_msgs::MoveBaseGoal goal; 66 | 67 | //we'll send a goal to the robot to move 2 meters forward 68 | goal.target_pose.header.frame_id = "base_link"; 69 | goal.target_pose.header.stamp = ros::Time::now(); 70 | 71 | goal.target_pose.pose.position.x = 2.0; 72 | goal.target_pose.pose.position.y = 0.2; 73 | goal.target_pose.pose.orientation = tf::createQuaternionMsgFromYaw(M_PI); 74 | 75 | ROS_INFO("Sending goal"); 76 | ac.sendGoal(goal); 77 | 78 | ac.waitForResult(); 79 | 80 | if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) 81 | ROS_INFO("Hooray, the base moved 2 meters forward"); 82 | else 83 | ROS_INFO("The base failed to move forward 2 meters for some reason"); 84 | 85 | return 0; 86 | } 87 | --------------------------------------------------------------------------------