├── CMakeLists.txt ├── README.md ├── commander ├── CMakeLists.txt ├── launch │ ├── config │ │ ├── costMap.rviz │ │ ├── costmap_common_params.yaml │ │ ├── global_costmap_params.yaml │ │ ├── local_costmap_params.yaml │ │ ├── planner_global.yaml │ │ └── planner_teb.yaml │ ├── detection.launch │ ├── localization.launch │ ├── mapping.launch │ └── navigation.launch ├── maps │ ├── myMapName.pgm │ └── myMapName.yaml ├── package.xml └── src │ └── example_node_kalman_filter.cpp ├── images ├── GazeboEnvironment.png └── OccupancyGrid.png ├── pal_person_detector_opencv ├── CMakeLists.txt ├── MODULE ├── README.md ├── launch │ └── detector.launch ├── msg │ ├── Detection2d.msg │ └── Detections2d.msg ├── package.xml └── src │ └── person_detector.cpp └── simulation_environment ├── CMakeLists.txt ├── config └── config.yaml ├── launch ├── apartment.launch └── temperature.launch ├── models ├── apartment │ ├── model.config │ └── model.sdf ├── diff_drive │ ├── diff_drive.gazebo │ ├── diff_drive.xacro │ ├── macros.xacro │ ├── materials.xacro │ └── parameter.xacro └── test_human │ ├── meshes │ ├── brown_eye.png │ ├── female_casualsuit01_diffuse.png │ ├── female_casualsuit01_normal.png │ ├── ponytail01_diffuse.png │ ├── test_human.dae │ └── test_human.png │ ├── model.config │ └── model.sdf ├── package.xml ├── src └── temp_plugin.cpp └── worlds ├── apartment.world └── temperature.world /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/kinetic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Tutorial for ROS and Gazebo 2 | This is the code basis for ROS and Gazebo Tutorials and Assignments. We use a simulated differential drive robot equipped with different sensors, such as an IMU, an Odometer, a LiDAR or a Camera. Check out the video tutorials for getting a detailed introduction into ROS and Gazebo. 3 | 4 | 5 | ## Table of Contents 6 | [Requirements](#requirements)
7 | [Additional Required Packages](#packages)
8 | [Get Started](#getstarted)
9 | [Video Tutorials](#video)
10 | [Important Topics](#topics)
11 | [Assignment 01](#assignment01)
12 | [Assignment 02](#assignment02)
13 | [Performance Improvements for Virtual Machines](#improvement)
14 | [License](#license)
15 | 16 | ## Requirements 17 | * ROS Kinetic (Ubuntu 16.04) or Melodic (Ubuntu 18.04), Installation Instructions can be found [here](http://wiki.ros.org/kinetic/Installation/Ubuntu) 18 | * Gazebo (recommended: Version 7.0), comes with the ROS full desktop version, otherwise Installation Instructions can be found [here](http://gazebosim.org/tutorials?tut=install_ubuntu&ver=7.0) 19 | * (optional) ROS-QTC-Plugin for using QT Creator as IDE, Installation Instructions can be found [here](https://ros-qtc-plugin.readthedocs.io/en/latest/_source/Improve-ROS-Qt-Creator-Plugin-Developers-ONLY.html) 20 | 21 | ## Additional Required Packages (installation instructions are given for ROS Melodic) 22 | * [gmapping](http://wiki.ros.org/gmapping) 23 | ```bash 24 | sudo apt-get install ros-melodic-gmapping 25 | ``` 26 | * [teleop_twist_keyboard](http://wiki.ros.org/teleop_twist_keyboard) 27 | ```bash 28 | sudo apt-get install ros-melodic-teleop-twist-keyboard 29 | ``` 30 | * [map_server](http://wiki.ros.org/map_server) 31 | ```bash 32 | sudo apt-get install ros-melodic-map-server 33 | ``` 34 | * [amcl](http://wiki.ros.org/amcl) 35 | ```bash 36 | sudo apt-get install ros-melodic-amcl 37 | ``` 38 | * [move_base](http://wiki.ros.org/move_base) 39 | ```bash 40 | sudo apt-get install ros-melodic-move-base 41 | ``` 42 | * [global_planner](http://wiki.ros.org/global_planner) 43 | ```bash 44 | sudo apt-get install ros-melodic-global-planner 45 | ``` 46 | * [teb_local_planner](http://wiki.ros.org/teb_local_planner) 47 | ```bash 48 | sudo apt-get install ros-melodic-teb-local-planner 49 | ``` 50 | 51 | ## Get Started 52 | * Start by creating a catkin workspace folder, downloading the git repository and compiling the code 53 | ```bash 54 | mkdir -p ~/tutorial_ws/src 55 | cd ~/tutorial_ws/src 56 | git clone https://github.com/NRottmann/ROS_Gazebo_Tutorial.git 57 | cd .. 58 | catkin_make 59 | ``` 60 | * Start the simulation 61 | ```bash 62 | cd ~/tutorial_ws 63 | source devel/setup.bash 64 | roslaunch simulation_environment apartment.launch 65 | ``` 66 | * The Gazebo environment should have opened and something similar to the image below should have appeared: 67 | ![Image of Gazebo](images/GazeboEnvironment.png "Gazebo Simulation Environment") 68 | 69 | * Now you can move the robot around by opening a new terminal and typing 70 | ```bash 71 | rosrun teleop_twist_keyboard teleop_twist_keyboard.py 72 | ``` 73 | 74 | ## Video Tutorials 75 | [![Introduction Video](http://img.youtube.com/vi/brDKAweq_IM/0.jpg)](http://www.youtube.com/watch?v=brDKAweq_IM&list=PLlcq6PMeufrApLSWZR73ivGDjTxayA_Ss) 76 | 77 | ## Important Topics (Message Types) provided by the Simulation Environment 78 | * Published 79 | * /camera/image_raw (sensor_msgs/Image) 80 | * /imu (sensor_msgs/Imu) 81 | * /odom (nav_msgs/Odometry) 82 | * /scan (sensor_msgs/LaserScan) 83 | * Subsrcibed 84 | * /cmd_vel (geoemtry_msgs/Twist) 85 | 86 | ## Assignment 01: 87 | 1. Generate a Map of the Environment 88 | * Start the simulation environment and gampping (hint: have a look into the mapping.launch file) 89 | * Drive the robot around using the teleop_twist_keyboard until you the map is sufficient accurate (hint: you can have a look onto the map by using rviz) 90 | ```bash 91 | rosrun teleop_twist_keyboard teleop_twist_keyboard.py 92 | ``` 93 | * Save the map using map_server 94 | ```bash 95 | rosrun map_server map_saver -f myMapName 96 | ``` 97 | 98 | 2. Find the Person in the Building (the robot will start at a random position outside the building) 99 | * Create a ROS Node which enables the robot to search in the apartment environment for the missing person (The person will be somewhere placed). After finding the missing person, mark the position of the missing person using visualization_msgs::Marker which can be displayed in rviz. The topic has to be name missing_person. An example on how to do such a visualization message can be found [here](http://wiki.ros.org/rviz/Tutorials/Markers%3A%20Points%20and%20Lines). 100 | * For localization, you can use the amcl (Particle Filter) package (example: localization.launch) 101 | * For the navigation, you can use move_base, together with local and global path planner (example: navigation.launch). The navigation requires of course localization provided by tthe amcl package. 102 | * For moving the robot around, you can publish poses to the robot to the topic /move_base_simple/goal. The move_base package will do thee planning (if correctly configured) using a global and a local path planner. 103 | * The missing person can be detected by using the person_detector (example: detection.launch). A new topic /person_detector will appear which publishes the message pal_person_detector_opencv/Detections2d. This message contains information about detected person in the camera image. For more information about the person detector, we refer to the [ros wiki](http://wiki.ros.org/Robots/TIAGo/Tutorials/PersonDetection). For simplicity, we included the required parts of the pal repository into our tutorial repository. 104 | 105 | #### Hint 106 | You can either design an autonomous search for the robot or pre-define points for navigation (based on your map) to search all rooms. 107 | 108 | ### Submission 109 | 110 | * Please fork the repository to your own GitHub profile, duplicate it and set the duplicate to private. Add me, NRottmann, as a contributor such that I can donwload it later. Then start editing your duplicat repository such that the assignment is full filled. Finally, send an email to Nils.Rottmann (at) rob.uni-luebeck.de with the concern Rescue_Assignment_01 with simply the link to your forked GitHub repository. 111 | * Optional (if you do not have an own GitHub Account): Check out a new branch and solve the assignment in this branch. Afterwards, push the branch to this repository. Finally, send an email to Nils.Rottmann (at) rob.uni-luebeck.de with the concern Rescue_Assignment_01 with simply the name of the pushed branch. 112 | * Add instructions for installation and usage of your repository below under the headline Participant Instructions. 113 | 114 | ### Remarks 115 | 116 | * There are a lot of good manuals on how ROS and Gazebo work, besides the provided video tutorials. 117 | * The [ROS wiki](http://wiki.ros.org/) is quite extensive and provides good explanations for all here used packages. 118 | * If you have a questions with regard to ROS or Gazebo, remember: You are probably not the first one who asked this. Thus, a simple google search might already solve your problem. 119 | * There are a lot of good communities which provide excellent help with regard to ROS and Gazebo, e.g. Q&A at the ROS wiki, Q&A at the Gazebo wiki, stackoverflow, etc. Do not hesitate to ask your own question but first check whether there exists already a similar thread. 120 | * Finally, if you tried the above and you still have an unanswered question, then feel free to ask us via Mail, Nils.Rottmann (at) rob.uni-luebeck.de 121 | 122 | ### Participant Instructions 123 | 124 | ... 125 | 126 | 127 | 128 | ## Assignment 02: 129 | 130 | In this assignment, you have to let the robot automatically detect two fire sources. Therefore, the robot has a temperature sensor on board which publishes to the 131 | 132 | ``` 133 | /temperature (sensor_msgs/Temperature) 134 | ``` 135 | 136 | topic. For running the scenario enter 137 | 138 | ``` 139 | roslaunch simulation_environment temperature.launch 140 | ``` 141 | 142 | The scenario is a free world scenario, thus with only a ground plane and no other boundaries at all. This enables fast simulations. To successfully complete the assignment, you have to: 143 | 144 | * let the robot autonomously find both fire sources 145 | 146 | * after finding each fire source, publish a self-designed message consisting of the ROS messages 147 | 148 | * sensor_msgs/Temeperature 149 | * geometry_msgs/Pose 150 | 151 | to give out the fire source location together with the temperature. 152 | 153 | #### Hint 154 | For getting the pose of the robot you can simply subscribe to the odometry topic (it is published without failure, thus exact). 155 | 156 | ### Submission 157 | 158 | * Please fork the repository to your own GitHub profile, duplicate it and set the duplicate to private. Add me, NRottmann, as a contributor such that I can donwload it later. Then start editing your duplicat repository such that the assignment is full filled. Finally, send an email to Nils.Rottmann (at) rob.uni-luebeck.de with the concern Rescue_Assignment_02 with simply the link to your forked GitHub repository. 159 | * Optional (if you do not have an own GitHub Account): Check out a new branch and solve the assignment in this branch. Afterwards, push the branch to this repository. Finally, send an email to Nils.Rottmann (at) rob.uni-luebeck.de with the concern Rescue_Assignment_02 with simply the name of the pushed branch. 160 | * Add instructions for installation and usage of your repository below under the headline Participant Instructions. 161 | 162 | ### Participant Instructions 163 | 164 | ... 165 | 166 | 167 | ## Performance Improvements for Virtual Machines 168 | 169 | If you are using a virtual machine to run Ubuntu 18.04, you might suffer from bad performance. Don't worry, there are multiple options to increase the performance of your VM: 170 | 171 | * If you have HDD and SSD hard disks, then make sure your VM is stored on the SSD. 172 | * Make sure to allocate enough RAM to the VM. If your PC has 8GB you can allocate 4GB to the VM. 173 | * Install the guest additions in VirtualBox. With that better graphics driver can be used. 174 | * Increase the video memory of the VM. 175 | * Use a better graphic processor if you are running VirtualBox on a notebook. 176 | * Allocate more CPU cores to the VM. 177 | * Enable 3D acceleration. 178 | 179 | ## License 180 | 181 | The MIT License (MIT) 182 | Copyright (c) 2020 Nils Rottmann 183 | 184 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 185 | 186 | The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 187 | 188 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 189 | -------------------------------------------------------------------------------- /commander/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(commander) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | rospy 7 | std_msgs 8 | geometry_msgs 9 | sensor_msgs 10 | nav_msgs 11 | ) 12 | 13 | find_package(Eigen3 REQUIRED) 14 | 15 | catkin_package( 16 | CATKIN_DEPENDS 17 | roscpp 18 | rospy 19 | std_msgs 20 | geometry_msgs 21 | sensor_msgs 22 | nav_msgs 23 | DEPENDS 24 | EIGEN3 25 | ) 26 | 27 | include_directories( 28 | ${catkin_INCLUDE_DIRS} 29 | ${src} 30 | ${EIGEN3_INCLUDE_DIR} 31 | ) 32 | 33 | 34 | add_executable(example_node_kalman_filter src/example_node_kalman_filter.cpp) 35 | target_link_libraries(example_node_kalman_filter ${catkin_LIBRARIES}) 36 | add_dependencies(example_node_kalman_filter ${catkin_EXPORTED_TARGETS}) 37 | -------------------------------------------------------------------------------- /commander/launch/config/costMap.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /TF1/Frames1 9 | - /LaserScan1 10 | - /Global Plan1 11 | Splitter Ratio: 0.27833572030067444 12 | Tree Height: 809 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.5886790156364441 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: LaserScan 32 | Preferences: 33 | PromptSaveOnExit: true 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Class: rviz/TF 38 | Enabled: true 39 | Frame Timeout: 15 40 | Frames: 41 | All Enabled: true 42 | base_link: 43 | Value: true 44 | map: 45 | Value: true 46 | odom_link: 47 | Value: true 48 | chassis_link: 49 | Value: true 50 | laser_link: 51 | Value: true 52 | Marker Scale: 1 53 | Name: TF 54 | Show Arrows: true 55 | Show Axes: true 56 | Show Names: false 57 | Tree: 58 | map: 59 | odom: 60 | base_link: 61 | {} 62 | Update Interval: 0 63 | Value: true 64 | - Alpha: 1 65 | Autocompute Intensity Bounds: true 66 | Autocompute Value Bounds: 67 | Max Value: 10 68 | Min Value: -10 69 | Value: true 70 | Axis: Z 71 | Channel Name: intensity 72 | Class: rviz/LaserScan 73 | Color: 239; 41; 41 74 | Color Transformer: FlatColor 75 | Decay Time: 0 76 | Enabled: true 77 | Invert Rainbow: false 78 | Max Color: 255; 255; 255 79 | Max Intensity: 4096 80 | Min Color: 0; 0; 0 81 | Min Intensity: 0 82 | Name: LaserScan 83 | Position Transformer: XYZ 84 | Queue Size: 10 85 | Selectable: true 86 | Size (Pixels): 3 87 | Size (m): 0.029999999329447746 88 | Style: Flat Squares 89 | Topic: /scan 90 | Unreliable: false 91 | Use Fixed Frame: true 92 | Use rainbow: true 93 | Value: true 94 | - Alpha: 1 95 | Class: rviz/Polygon 96 | Color: 25; 255; 0 97 | Enabled: true 98 | Name: Footprint 99 | Topic: /move_base/global_costmap/footprint 100 | Unreliable: false 101 | Value: true 102 | - Alpha: 0.699999988079071 103 | Class: rviz/Map 104 | Color Scheme: map 105 | Draw Behind: false 106 | Enabled: true 107 | Name: Occupancy 108 | Topic: /map 109 | Unreliable: false 110 | Use Timestamp: false 111 | Value: true 112 | - Alpha: 0.4000000059604645 113 | Class: rviz/Map 114 | Color Scheme: costmap 115 | Draw Behind: false 116 | Enabled: true 117 | Name: Global Cost 118 | Topic: /move_base/global_costmap/costmap 119 | Unreliable: false 120 | Use Timestamp: false 121 | Value: true 122 | - Alpha: 0.699999988079071 123 | Class: rviz/Map 124 | Color Scheme: costmap 125 | Draw Behind: false 126 | Enabled: true 127 | Name: Local Cost 128 | Topic: /move_base/local_costmap/costmap 129 | Unreliable: false 130 | Use Timestamp: false 131 | Value: true 132 | - Alpha: 1 133 | Axes Length: 1 134 | Axes Radius: 0.10000000149011612 135 | Class: rviz/Pose 136 | Color: 255; 25; 0 137 | Enabled: true 138 | Head Length: 0.30000001192092896 139 | Head Radius: 0.10000000149011612 140 | Name: Goal 141 | Shaft Length: 1 142 | Shaft Radius: 0.05000000074505806 143 | Shape: Arrow 144 | Topic: /move_base/current_goal 145 | Unreliable: false 146 | Value: true 147 | - Alpha: 1 148 | Buffer Length: 1 149 | Class: rviz/Path 150 | Color: 78; 154; 6 151 | Enabled: true 152 | Head Diameter: 0.30000001192092896 153 | Head Length: 0.20000000298023224 154 | Length: 0.30000001192092896 155 | Line Style: Lines 156 | Line Width: 0.029999999329447746 157 | Name: Global Plan 158 | Offset: 159 | X: 0 160 | Y: 0 161 | Z: 0 162 | Pose Color: 255; 85; 255 163 | Pose Style: None 164 | Radius: 0.029999999329447746 165 | Shaft Diameter: 0.10000000149011612 166 | Shaft Length: 0.10000000149011612 167 | Topic: /move_base/TrajectoryPlannerROS/global_plan 168 | Unreliable: false 169 | Value: true 170 | - Alpha: 1 171 | Buffer Length: 1 172 | Class: rviz/Path 173 | Color: 25; 255; 0 174 | Enabled: true 175 | Head Diameter: 0.30000001192092896 176 | Head Length: 0.20000000298023224 177 | Length: 0.30000001192092896 178 | Line Style: Lines 179 | Line Width: 0.029999999329447746 180 | Name: LocalPlan 181 | Offset: 182 | X: 0 183 | Y: 0 184 | Z: 0 185 | Pose Color: 255; 85; 255 186 | Pose Style: None 187 | Radius: 0.029999999329447746 188 | Shaft Diameter: 0.10000000149011612 189 | Shaft Length: 0.10000000149011612 190 | Topic: /move_base/TrajectoryPlannerROS/local_plan 191 | Unreliable: false 192 | Value: true 193 | Enabled: true 194 | Global Options: 195 | Background Color: 43; 43; 43 196 | Default Light: true 197 | Fixed Frame: map 198 | Frame Rate: 30 199 | Name: root 200 | Tools: 201 | - Class: rviz/Interact 202 | Hide Inactive Objects: true 203 | - Class: rviz/MoveCamera 204 | - Class: rviz/Select 205 | - Class: rviz/FocusCamera 206 | - Class: rviz/Measure 207 | - Class: rviz/SetInitialPose 208 | Topic: /initialpose 209 | - Class: rviz/SetGoal 210 | Topic: /move_base_simple/goal 211 | - Class: rviz/PublishPoint 212 | Single click: true 213 | Topic: /clicked_point 214 | Value: true 215 | Views: 216 | Current: 217 | Class: rviz/Orbit 218 | Distance: 10.694085121154785 219 | Enable Stereo Rendering: 220 | Stereo Eye Separation: 0.05999999865889549 221 | Stereo Focal Distance: 1 222 | Swap Stereo Eyes: false 223 | Value: false 224 | Focal Point: 225 | X: 0 226 | Y: 0 227 | Z: 0 228 | Focal Shape Fixed Size: true 229 | Focal Shape Size: 0.05000000074505806 230 | Invert Z Axis: false 231 | Name: Current View 232 | Near Clip Distance: 0.009999999776482582 233 | Pitch: 1.0503977537155151 234 | Target Frame: 235 | Value: Orbit (rviz) 236 | Yaw: 0.6553946733474731 237 | Saved: ~ 238 | Window Geometry: 239 | Displays: 240 | collapsed: false 241 | Height: 1023 242 | Hide Left Dock: false 243 | Hide Right Dock: false 244 | QMainWindow State: 000000ff00000000fd00000004000000000000022c00000366fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000366000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000281000001240000000000000000000000010000022a00000368fc0200000006fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0049006d006100670065000000003d000003680000000000000000fb0000000a0049006d006100670065010000003d000001b10000000000000000fb0000000a0049006d00610067006501000001f4000001b10000000000000000fc000002dd000000c80000000000fffffffa000000000100000002fb0000000a0049006d0061006700650000000000ffffffff0000000000000000fb0000000a00560069006500770073000000062e0000010f0000010000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000073d00000299fc0100000002fb0000000a0049006d00610067006500000000000000073d0000000000000000fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d00000039fc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000050b0000036600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 245 | Selection: 246 | collapsed: false 247 | Time: 248 | collapsed: false 249 | Tool Properties: 250 | collapsed: false 251 | Views: 252 | collapsed: false 253 | Width: 1853 254 | X: 67 255 | Y: 27 256 | -------------------------------------------------------------------------------- /commander/launch/config/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | obstacle_range: 10 2 | raytrace_range: 10 3 | max_obstacle_height: 2.0 4 | inf_is_valid: true 5 | footprint: [[0.01, 0.01], [-0.01, 0.01], [-0.01, -0.01], [0.01, -0.01]] 6 | 7 | inflation_radius: 0.0 8 | costscalingfactor: 1.3 9 | 10 | map_topic: /map 11 | subscribe_to_updates: true 12 | observation_sources: laser_scan_sensor 13 | laser_scan_sensor: {sensor_frame: laser_link, data_type: LaserScan, topic: scan, marking: true, clearing: true} 14 | global_frame: map 15 | robot_base_frame: base_link 16 | always_send_full_costmap: true 17 | -------------------------------------------------------------------------------- /commander/launch/config/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | update_frequency: 1 3 | publish_frequency: 1 4 | transform_tolerance: 1 5 | width: 40 6 | height: 40 7 | origin_x: 0 8 | origin_y: 0 9 | static_map: true 10 | rolling_window: true 11 | resolution: 0.1 12 | -------------------------------------------------------------------------------- /commander/launch/config/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | update_frequency: 5 3 | publish_frequency: 5 4 | transform_tolerance: 1 5 | width: 6 6 | height: 6 7 | origin_x: -3 8 | origin_y: -3 9 | static_map: false 10 | rolling_window: true 11 | resolution: 0.1 12 | -------------------------------------------------------------------------------- /commander/launch/config/planner_global.yaml: -------------------------------------------------------------------------------- 1 | GlobalPlanner: 2 | allow_unknown: false 3 | default_tolerance: 0.0 4 | 5 | use_dijkstra: true 6 | use_quadratic: true 7 | use_grid_path: false 8 | 9 | old_navfn_behavior: false 10 | 11 | lethal_cost: 253 12 | neutral_cost: 66 13 | cost_factor: 0.55 14 | 15 | visualize_potential: true 16 | publish_potential: true 17 | 18 | orientation_mode : 0 19 | orientation_window_size: 1 20 | -------------------------------------------------------------------------------- /commander/launch/config/planner_teb.yaml: -------------------------------------------------------------------------------- 1 | TebLocalPlannerROS: 2 | 3 | odom_topic: /odom 4 | map_frame: map 5 | 6 | # Trajectory 7 | teb_autosize: True 8 | dt_ref: 0.3 9 | dt_hysteresis: 0.03 10 | global_plan_overwrite_orientation: True 11 | max_global_plan_lookahead_dist: 0 12 | feasibility_check_no_poses: 10 13 | 14 | # Robot 15 | 16 | max_vel_x: 1 17 | max_vel_x_backwards: 0.1 18 | max_vel_y: 0.0 19 | max_vel_theta: 1 20 | acc_lim_x: 0.5 21 | acc_lim_theta: 0.5 22 | min_turning_radius: 0.0 23 | 24 | footprint_model: 25 | type: "circular" 26 | radius: 0.2 27 | 28 | # GoalTolerance 29 | 30 | xy_goal_tolerance: 0.4 31 | yaw_goal_tolerance: 3.14 32 | free_goal_vel: False 33 | 34 | # Obstacles 35 | 36 | inflation_dist: 1.0 37 | min_obstacle_dist: 0.4 38 | include_costmap_obstacles: True 39 | costmap_obstacles_behind_robot_dist: 1.0 40 | obstacle_poses_affected: 30 41 | costmap_converter_plugin: "" 42 | costmap_converter_spin_thread: True 43 | costmap_converter_rate: 5 44 | 45 | # Optimization 46 | 47 | no_inner_iterations: 5 48 | no_outer_iterations: 4 49 | optimization_activate: True 50 | optimization_verbose: False 51 | penalty_epsilon: 0.05 52 | weight_max_vel_x: 4 53 | weight_max_vel_theta: 1 54 | weight_acc_lim_x: 1 55 | weight_acc_lim_theta: 1 56 | weight_kinematics_nh: 100 57 | weight_kinematics_forward_drive: 1 58 | weight_kinematics_turning_radius: 1 59 | weight_optimaltime: 1 60 | weight_obstacle: 50 61 | weight_dynamic_obstacle: 10 # not in use yet 62 | global_plan_viapoint_sep: 0.3 63 | 64 | # Homotopy Class Planner 65 | 66 | enable_homotopy_class_planning: False 67 | enable_multithreading: True 68 | simple_exploration: False 69 | max_number_classes: 2 70 | roadmap_graph_no_samples: 15 71 | roadmap_graph_area_width: 5 72 | h_signature_prescaler: 0.5 73 | h_signature_threshold: 0.1 74 | obstacle_keypoint_offset: 0.1 75 | obstacle_heading_threshold: 0.45 76 | visualize_hc_graph: True 77 | -------------------------------------------------------------------------------- /commander/launch/detection.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /commander/launch/localization.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /commander/launch/mapping.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /commander/launch/navigation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /commander/maps/myMapName.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NRottmann/ROS_Gazebo_Tutorial/9847886f7145f0f5d2b38425e62a1029e7b210df/commander/maps/myMapName.pgm -------------------------------------------------------------------------------- /commander/maps/myMapName.yaml: -------------------------------------------------------------------------------- 1 | image: myMapName.pgm 2 | resolution: 0.050000 3 | origin: [-100.000000, -100.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /commander/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | commander 4 | 0.0.0 5 | commander package for the simulation environment 6 | 7 | sanu 8 | 9 | BSD 10 | 11 | catkin 12 | 13 | roscpp 14 | rospy 15 | std_msgs 16 | geometry_msgs 17 | sensor_msgs 18 | nav_msgs 19 | 20 | gazebo_ros 21 | roscpp 22 | rospy 23 | std_msgs 24 | geometry_msgs 25 | sensor_msgs 26 | nav_msgs 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /commander/src/example_node_kalman_filter.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #define PI 3.14159265f 8 | 9 | // Define a callback class for the Kalman Filter 10 | class KFListener 11 | { 12 | public: 13 | // The Callback function to subscribe to the odomtery topic (/odom) 14 | void callbackOdom(const nav_msgs::Odometry::ConstPtr& msgIn); 15 | // The Callback function to subscribe to the imu topic (/imu) 16 | void callbackImu(const sensor_msgs::Imu::ConstPtr& msgIn); 17 | // The Kalman Filter function 18 | geometry_msgs::PoseWithCovarianceStamped kalmanFilter(); 19 | // The Constructor of the class, here we will initialize the subscriber and publisher among other things 20 | KFListener(ros::NodeHandle nh, ros::NodeHandle nhp); 21 | private: 22 | // Here we have to define instances of our publisher and subscriber 23 | ros::Subscriber subOdom; 24 | ros::Subscriber subImu; 25 | ros::Publisher pubPose; 26 | // Here we have to define other variables, for example to store the recent measurements 27 | nav_msgs::Odometry msgOdom; 28 | sensor_msgs::Imu msgImu; 29 | 30 | }; 31 | 32 | // Define the constructor of the class, thus we have to initialize all subscriber and publisher 33 | KFListener::KFListener(ros::NodeHandle nh, ros::NodeHandle nhp) { 34 | // Initialize subscriber 35 | subOdom = nh.subscribe("/odom", 20, &KFListener::callbackOdom, this); 36 | subImu = nh.subscribe("/imu", 20, &KFListener::callbackImu, this); 37 | // Initialize publisher 38 | pubPose = nh.advertise("/pose", 20); 39 | } 40 | 41 | // Definition of the callback functions 42 | void KFListener::callbackOdom(const nav_msgs::Odometry::ConstPtr& msgIn) 43 | { 44 | msgOdom = *msgIn; 45 | } 46 | void KFListener::callbackImu(const sensor_msgs::Imu::ConstPtr& msgIn) 47 | { 48 | msgImu = *msgIn; 49 | } 50 | 51 | // Here we would define a Kalman Filter function 52 | geometry_msgs::PoseWithCovarianceStamped KFListener::kalmanFilter() { 53 | geometry_msgs::PoseWithCovarianceStamped msgOut; 54 | /* 55 | * Some Code for the Kalman Filter 56 | */ 57 | pubPose.publish(msgOut); 58 | } 59 | 60 | int main(int argc, char **argv) 61 | { 62 | // Initialize the ROS System 63 | ros::init(argc, argv, "kalmanFilter"); 64 | // Initialize node handle 65 | ros::NodeHandle nh; 66 | ros::NodeHandle nhp("~"); 67 | // Get instance of the class 68 | KFListener kflistener(nh, nhp); 69 | // Start the loop 70 | ros::spin(); 71 | return 0; 72 | } 73 | -------------------------------------------------------------------------------- /images/GazeboEnvironment.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NRottmann/ROS_Gazebo_Tutorial/9847886f7145f0f5d2b38425e62a1029e7b210df/images/GazeboEnvironment.png -------------------------------------------------------------------------------- /images/OccupancyGrid.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NRottmann/ROS_Gazebo_Tutorial/9847886f7145f0f5d2b38425e62a1029e7b210df/images/OccupancyGrid.png -------------------------------------------------------------------------------- /pal_person_detector_opencv/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(pal_person_detector_opencv) 3 | 4 | find_package(catkin REQUIRED COMPONENTS image_transport 5 | message_generation 6 | geometry_msgs 7 | cv_bridge 8 | roscpp) 9 | 10 | find_package(OpenCV REQUIRED) 11 | 12 | add_message_files( 13 | DIRECTORY msg 14 | FILES 15 | Detection2d.msg 16 | Detections2d.msg 17 | ) 18 | 19 | generate_messages(DEPENDENCIES geometry_msgs) 20 | 21 | catkin_package( 22 | CATKIN_DEPENDS geometry_msgs message_runtime image_transport cv_bridge roscpp 23 | ) 24 | 25 | include_directories(SYSTEM ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}) 26 | 27 | add_executable(pal_person_detector_opencv src/person_detector.cpp) 28 | target_link_libraries(pal_person_detector_opencv ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) 29 | add_dependencies(pal_person_detector_opencv ${catkin_EXPORTED_TARGETS} pal_person_detector_opencv_generate_messages_cpp) 30 | 31 | install(TARGETS pal_person_detector_opencv 32 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 33 | -------------------------------------------------------------------------------- /pal_person_detector_opencv/MODULE: -------------------------------------------------------------------------------- 1 | VISION 2 | -------------------------------------------------------------------------------- /pal_person_detector_opencv/README.md: -------------------------------------------------------------------------------- 1 | # pal_person_detector_opencv 2 | ROS node integrating the person detector for RGB images based on OpenCV's HoG Adaboost cascade 3 | 4 | ## How to launch 5 | 6 | `roslaunch pal_person_detector_opencv detector.launch image:=my_image max_rate:=10 scale:=0.5` 7 | 8 | where 9 | 10 | `mmy_image` is the image topic name that the node will subscribe to 11 | 12 | `max_rate` is the maximum frequency at which the node will publish detections 13 | 14 | `scale` is the scaling factor belonging to (0, 1] that will be applied to the images before running the detector. It is useful to downsample the images in order to speed up the detector. 15 | 16 | If no arguments are provided the node will subscribe to `/xtion/rgb/image_raw' and will downscale the images by a factor of 0.5 and the maximum publication rate will be 5 Hz. 17 | 18 | ## Detections topic 19 | 20 | The detections are published in the topic `/person_detector/detections` and the message type is [pal_detection_msgs::Detections2d](https://github.com/pal-robotics/pal_msgs) 21 | 22 | ## Debug image 23 | 24 | The image `/person_detector/debug` shows the processed images with the ROIs corresponding to detected persons. In order to visualize the debug image you may do as follows: 25 | 26 | `rosrun image_view image_view image:=/person_detector/debug` 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /pal_person_detector_opencv/launch/detector.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /pal_person_detector_opencv/msg/Detection2d.msg: -------------------------------------------------------------------------------- 1 | ## Rectangle defined by a point, its width and height 2 | # corresponds to the openCV struct : CvRect 3 | 4 | # coordinates of the top left corner of the box 5 | int64 x 6 | int64 y 7 | 8 | # width of the box 9 | int64 width 10 | 11 | # height of the box 12 | int64 height 13 | 14 | -------------------------------------------------------------------------------- /pal_person_detector_opencv/msg/Detections2d.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | pal_person_detector_opencv/Detection2d[] detections 4 | 5 | # Optional transformation between the camera frame and a certain parent frame 6 | geometry_msgs/TransformStamped camera_pose 7 | 8 | 9 | -------------------------------------------------------------------------------- /pal_person_detector_opencv/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | pal_person_detector_opencv 4 | 0.1.0 5 | Fullbody person detector on RGB images based on OpenCV HoG Adaboost cascade 6 | 7 | jordi pages 8 | 9 | BSD 10 | 11 | catkin 12 | message_generation 13 | roscpp 14 | sensor_msgs 15 | geometry_msgs 16 | cv_bridge 17 | image_transport 18 | 19 | message_runtime 20 | roscpp 21 | sensor_msgs 22 | geometry_msgs 23 | cv_bridge 24 | image_transport 25 | 26 | 27 | -------------------------------------------------------------------------------- /pal_person_detector_opencv/src/person_detector.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (Modified BSD License) 3 | * 4 | * Copyright (c) 2013, PAL Robotics, S.L. 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 PAL Robotics, S.L. 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 | /** \author Jordi Pages */ 36 | 37 | // PAL headers 38 | #include 39 | 40 | // ROS headers 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | // OpenCV headers 49 | #include 50 | #include 51 | #include 52 | 53 | // Boost headers 54 | #include 55 | #include 56 | 57 | // Std C++ headers 58 | #include 59 | 60 | /** 61 | * @brief The PersonDetector class encapsulating an image subscriber and the OpenCV's CPU HOG person detector 62 | * 63 | * @example rosrun person_detector_opencv person_detector image:=/camera/image _rate:=5 _scale:=0.5 64 | * 65 | */ 66 | class PersonDetector 67 | { 68 | public: 69 | 70 | PersonDetector(ros::NodeHandle& nh, 71 | ros::NodeHandle& pnh, 72 | double imageScaling = 1.0); 73 | virtual ~PersonDetector(); 74 | 75 | protected: 76 | 77 | ros::NodeHandle _nh, _pnh; 78 | 79 | void imageCallback(const sensor_msgs::ImageConstPtr& msg); 80 | 81 | void detectPersons(const cv::Mat& img, 82 | std::vector& detections); 83 | 84 | void scaleDetections(std::vector& detections, 85 | double scaleX, double scaleY) const; 86 | 87 | void publishDetections(const std::vector& detections) const; 88 | 89 | void publishDebugImage(cv::Mat& img, 90 | const std::vector& detections) const; 91 | 92 | double _imageScaling; 93 | mutable cv_bridge::CvImage _cvImgDebug; 94 | 95 | boost::scoped_ptr _hogCPU; 96 | 97 | image_transport::ImageTransport _imageTransport, _privateImageTransport; 98 | image_transport::Subscriber _imageSub; 99 | ros::Time _imgTimeStamp; 100 | 101 | ros::Publisher _detectionPub; 102 | image_transport::Publisher _imDebugPub; 103 | 104 | }; 105 | 106 | PersonDetector::PersonDetector(ros::NodeHandle& nh, 107 | ros::NodeHandle& pnh, 108 | double imageScaling): 109 | _nh(nh), 110 | _pnh(pnh), 111 | _imageScaling(imageScaling), 112 | _imageTransport(nh), 113 | _privateImageTransport(pnh) 114 | { 115 | 116 | _hogCPU.reset( new cv::HOGDescriptor ); 117 | _hogCPU->setSVMDetector( cv::HOGDescriptor::getDefaultPeopleDetector() ); 118 | 119 | image_transport::TransportHints transportHint("raw"); 120 | 121 | _imageSub = _imageTransport.subscribe("image", 1, &PersonDetector::imageCallback, this, transportHint); 122 | _imDebugPub = _privateImageTransport.advertise("debug", 1); 123 | 124 | _detectionPub = _pnh.advertise("detections", 1); 125 | 126 | cv::namedWindow("person detections"); 127 | } 128 | 129 | PersonDetector::~PersonDetector() 130 | { 131 | cv::destroyWindow("person detections"); 132 | } 133 | 134 | void PersonDetector::imageCallback(const sensor_msgs::ImageConstPtr& msg) 135 | { 136 | cv_bridge::CvImageConstPtr cvImgPtr; 137 | cvImgPtr = cv_bridge::toCvShare(msg); 138 | 139 | _imgTimeStamp = msg->header.stamp; 140 | 141 | cv::Mat img(static_cast(_imageScaling*cvImgPtr->image.rows), 142 | static_cast(_imageScaling*cvImgPtr->image.cols), 143 | cvImgPtr->image.type()); 144 | 145 | if ( _imageScaling == 1.0 ) 146 | cvImgPtr->image.copyTo(img); 147 | else 148 | { 149 | cv::resize(cvImgPtr->image, img, img.size()); 150 | } 151 | 152 | std::vector detections; 153 | 154 | detectPersons(img, detections); 155 | 156 | if ( _imageScaling != 1.0 ) 157 | { 158 | scaleDetections(detections, 159 | static_cast(cvImgPtr->image.cols)/static_cast(img.cols), 160 | static_cast(cvImgPtr->image.rows)/static_cast(img.rows)); 161 | } 162 | 163 | publishDetections(detections); 164 | 165 | cv::Mat imDebug = cvImgPtr->image.clone(); 166 | publishDebugImage(imDebug, detections); 167 | } 168 | 169 | void PersonDetector::scaleDetections(std::vector& detections, 170 | double scaleX, double scaleY) const 171 | { 172 | BOOST_FOREACH(cv::Rect& detection, detections) 173 | { 174 | cv::Rect roi(detection); 175 | detection.x = static_cast(roi.x * scaleX); 176 | detection.y = static_cast(roi.y * scaleY); 177 | detection.width = static_cast(roi.width * scaleX); 178 | detection.height = static_cast(roi.height * scaleY); 179 | } 180 | } 181 | 182 | 183 | void PersonDetector::detectPersons(const cv::Mat& img, 184 | std::vector& detections) 185 | { 186 | double start = static_cast(cv::getTickCount()); 187 | 188 | _hogCPU->detectMultiScale(img, 189 | detections, 190 | 0, //hit threshold: decrease in order to increase number of detections but also false alarms 191 | cv::Size(8,8), //win stride 192 | cv::Size(0,0), //padding 24,16 193 | 1.02, //scaling 194 | 1, //final threshold 195 | false); //use mean-shift to fuse detections 196 | 197 | double stop = static_cast(cv::getTickCount()); 198 | ROS_DEBUG_STREAM("Elapsed time in detectMultiScale: " << 1000.0*(stop-start)/cv::getTickFrequency() << " ms"); 199 | } 200 | 201 | void PersonDetector::publishDetections(const std::vector& detections) const 202 | { 203 | pal_person_detector_opencv::Detections2d msg; 204 | pal_person_detector_opencv::Detection2d detection; 205 | 206 | msg.header.frame_id = ""; 207 | msg.header.stamp = _imgTimeStamp; 208 | 209 | BOOST_FOREACH(const cv::Rect& roi, detections) 210 | { 211 | detection.x = roi.x; 212 | detection.y = roi.y; 213 | detection.width = roi.width; 214 | detection.height = roi.height; 215 | 216 | msg.detections.push_back(detection); 217 | } 218 | 219 | _detectionPub.publish(msg); 220 | } 221 | 222 | void PersonDetector::publishDebugImage(cv::Mat& img, 223 | const std::vector& detections) const 224 | { 225 | //draw detections 226 | BOOST_FOREACH(const cv::Rect& roi, detections) 227 | { 228 | cv::rectangle(img, roi, CV_RGB(0,255,0), 2); 229 | } 230 | 231 | if ( img.channels() == 3 && img.depth() == CV_8U ) 232 | _cvImgDebug.encoding = sensor_msgs::image_encodings::BGR8; 233 | 234 | else if ( img.channels() == 1 && img.depth() == CV_8U ) 235 | _cvImgDebug.encoding = sensor_msgs::image_encodings::MONO8; 236 | else 237 | throw std::runtime_error("Error in Detector2dNode::publishDebug: only 24-bit BGR or 8-bit MONO images are currently supported"); 238 | 239 | _cvImgDebug.image = img; 240 | sensor_msgs::Image imgMsg; 241 | imgMsg.header.stamp = _imgTimeStamp; 242 | _cvImgDebug.toImageMsg(imgMsg); //copy image data to ROS message 243 | 244 | _imDebugPub.publish(imgMsg); 245 | } 246 | 247 | int main(int argc, char **argv) 248 | { 249 | ros::init(argc,argv,"pal_person_detector_opencv"); // Create and name the Node 250 | ros::NodeHandle nh, pnh("~"); 251 | 252 | ros::CallbackQueue cbQueue; 253 | nh.setCallbackQueue(&cbQueue); 254 | 255 | double scale = 1.0; 256 | pnh.param("scale", scale, scale); 257 | 258 | double freq = 10; 259 | pnh.param("rate", freq, freq); 260 | 261 | ROS_INFO_STREAM("Setting image scale factor to: " << scale); 262 | ROS_INFO_STREAM("Setting detector max rate to: " << freq); 263 | ROS_INFO(" "); 264 | 265 | ROS_INFO_STREAM("Creating person detector ..."); 266 | 267 | PersonDetector detector(nh, pnh, scale); 268 | 269 | ROS_INFO_STREAM("Spinning to serve callbacks ..."); 270 | 271 | ros::Rate rate(freq); 272 | while ( ros::ok() ) 273 | { 274 | cbQueue.callAvailable(); 275 | rate.sleep(); 276 | } 277 | 278 | return 0; 279 | } 280 | -------------------------------------------------------------------------------- /simulation_environment/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(simulation_environment) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | rospy 7 | gazebo_ros 8 | std_msgs 9 | geometry_msgs 10 | sensor_msgs 11 | nav_msgs 12 | ) 13 | 14 | # Depending on system install of Gazebo 15 | find_package(gazebo REQUIRED) 16 | 17 | catkin_package( 18 | CATKIN_DEPENDS 19 | roscpp 20 | rospy 21 | std_msgs 22 | geometry_msgs 23 | sensor_msgs 24 | nav_msgs 25 | ) 26 | 27 | link_directories(${GAZEBO_LIBRARY_DIRS}) 28 | 29 | include_directories( 30 | ${catkin_INCLUDE_DIRS} 31 | ${src} 32 | ${GAZEBO_INCLUDE_DIRS} 33 | ) 34 | 35 | # Gazebo Plugins 36 | add_library(temp_plugin SHARED src/temp_plugin.cpp) 37 | target_link_libraries(temp_plugin ${GAZEBO_LIBRARIES}) 38 | add_dependencies(temp_plugin ${catkin_EXPORTED_TARGETS}) 39 | 40 | # Gazebo set flags 41 | list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}") 42 | -------------------------------------------------------------------------------- /simulation_environment/config/config.yaml: -------------------------------------------------------------------------------- 1 | # Robot specs 2 | wheel_radius: 0.1075 # diameter of a wheel, in m (float) 3 | axis_distance: 0.1788 # half of the wheelbase, in m (float) 4 | v0: 1.0 # maximum linear and angular velocity 5 | w0: 1.0 6 | 7 | # Controller specs 8 | Pv: 1 # Proportional control value for the velocity 9 | Pw: 1 10 | Dv: 1 # Derivative control value for the velocity 11 | Dw: 1 12 | 13 | # Odometry specs 14 | noise_vel: 0.1 # noise for velocity and odometry 15 | noise_odom: 0.1 16 | -------------------------------------------------------------------------------- /simulation_environment/launch/apartment.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 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /simulation_environment/launch/temperature.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 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /simulation_environment/models/apartment/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | apartment 4 | 1.0 5 | model.sdf 6 | 7 | Oscar Lima 8 | olima@isr.tecnico.ulisboa.pt 9 | 10 | ISR testbed walls and doors 11 | 12 | -------------------------------------------------------------------------------- /simulation_environment/models/apartment/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0.070483 0.106017 0 0 -0 0 6 | 7 | 8 | 9 | 10 | 11 | 1.9 0.05 2.0 12 | 13 | 14 | 0 0 1.0 0 -0 0 15 | 16 | 17 | 0 0 1.0 0 -0 0 18 | 19 | 20 | 1.9 0.05 2.0 21 | 22 | 23 | 24 | 28 | 0.666667 0.666667 0.498039 1 29 | 30 | 31 | -2.73248 1.48798 0 0 -0 0 32 | 33 | 34 | 35 | 36 | 37 | 38 | 2.15 0.05 2.0 39 | 40 | 41 | 0 0 1.0 0 -0 0 42 | 43 | 44 | 0 0 1.0 0 -0 0 45 | 46 | 47 | 2.15 0.05 2.0 48 | 49 | 50 | 51 | 55 | 0.666667 0.666667 0.498039 1 56 | 57 | 58 | -0.092483 -4.98802 0 0 -0 0 59 | 60 | 61 | 62 | 63 | 64 | 65 | 1.15 0.05 2.0 66 | 67 | 68 | 0 0 1.0 0 -0 0 69 | 70 | 71 | 0 0 1.0 0 -0 0 72 | 73 | 74 | 1.15 0.05 2.0 75 | 76 | 77 | 78 | 82 | 0.666667 0.666667 0.498039 1 83 | 84 | 85 | 0.957517 -4.43802 0 0 -0 1.5708 86 | 87 | 88 | 89 | 90 | 91 | 92 | 2.15 0.05 2.0 93 | 94 | 95 | 0 0 1.0 0 -0 0 96 | 97 | 98 | 0 0 1.0 0 -0 0 99 | 100 | 101 | 2.15 0.05 2.0 102 | 103 | 104 | 105 | 109 | 0.666667 0.666667 0.498039 1 110 | 111 | 112 | 2.00752 -3.88802 0 0 -0 0 113 | 114 | 115 | 116 | 117 | 118 | 119 | 1.15 0.05 2.0 120 | 121 | 122 | 0 0 1.0 0 -0 0 123 | 124 | 125 | 0 0 1.0 0 -0 0 126 | 127 | 128 | 1.15 0.05 2.0 129 | 130 | 131 | 132 | 136 | 0.666667 0.666667 0.498039 1 137 | 138 | 139 | 3.05752 -3.33802 0 0 -0 1.5708 140 | 141 | 142 | 143 | 144 | 145 | 146 | 0.65 0.05 2.0 147 | 148 | 149 | 0 0 1.0 0 -0 0 150 | 151 | 152 | 0 0 1.0 0 -0 0 153 | 154 | 155 | 0.65 0.05 2.0 156 | 157 | 158 | 159 | 163 | 0.666667 0.666667 0.498039 1 164 | 165 | 166 | 3.35752 -2.78802 0 0 -0 0 167 | 168 | 169 | 170 | 171 | 172 | 173 | 1.9 0.05 2.0 174 | 175 | 176 | 0 0 1.0 0 -0 0 177 | 178 | 179 | 0 0 1.0 0 -0 0 180 | 181 | 182 | 1.9 0.05 2.0 183 | 184 | 185 | 186 | 190 | 0.666667 0.666667 0.498039 1 191 | 192 | 193 | 3.65752 -1.86302 0 0 -0 1.5708 194 | 195 | 196 | 197 | 198 | 199 | 200 | 0.9 0.05 2.0 201 | 202 | 203 | 0 0 1.0 0 -0 0 204 | 205 | 206 | 0 0 1.0 0 -0 0 207 | 208 | 209 | 0.9 0.05 2.0 210 | 211 | 212 | 213 | 217 | 0.666667 0.666667 0.498039 1 218 | 219 | 220 | 3.23252 -0.938017 0 0 -0 3.14159 221 | 222 | 223 | 224 | 225 | 226 | 227 | 1.9 0.05 2.0 228 | 229 | 230 | 0 0 1.0 0 -0 0 231 | 232 | 233 | 0 0 1.0 0 -0 0 234 | 235 | 236 | 1.9 0.05 2.0 237 | 238 | 239 | 240 | 244 | 0.666667 0.666667 0.498039 1 245 | 246 | 247 | 1.04652 1.55798 0 0 -0 0 248 | 249 | 250 | 251 | 252 | 253 | 254 | 8.90001 0.05 2.0 255 | 256 | 257 | 0 0 1.0 0 -0 0 258 | 259 | 260 | 0 0 1.0 0 -0 0 261 | 262 | 263 | 8.90001 0.05 2.0 264 | 265 | 266 | 267 | 271 | 0.666667 0.666667 0.498039 1 272 | 273 | 274 | -3.65148 0.562983 0 0 -0 1.57215 275 | 276 | 277 | 278 | 279 | 280 | 281 | 4.65 0.05 2.0 282 | 283 | 284 | 0 0 1.0 0 -0 0 285 | 286 | 287 | 0 0 1.0 0 -0 0 288 | 289 | 290 | 4.65 0.05 2.0 291 | 292 | 293 | 294 | 298 | 0.666667 0.666667 0.498039 1 299 | 300 | 301 | -1.35748 4.98798 0 0 -0 0 302 | 303 | 304 | 305 | 306 | 307 | 308 | 2.4 0.05 2.0 309 | 310 | 311 | 0 0 1.0 0 -0 0 312 | 313 | 314 | 0 0 1.0 0 -0 0 315 | 316 | 317 | 2.4 0.05 2.0 318 | 319 | 320 | 321 | 325 | 0.666667 0.666667 0.498039 1 326 | 327 | 328 | 0.942517 3.81298 0 0 0 -1.5708 329 | 330 | 331 | 332 | 333 | 334 | 0.9 0.05 2.0 335 | 336 | 337 | 0 0 1.0 0 -0 0 338 | 339 | 340 | 0 0 1.0 0 -0 0 341 | 342 | 343 | 0.9 0.05 2.0 344 | 345 | 346 | 347 | 351 | 0.666667 0.666667 0.498039 1 352 | 353 | 354 | 0.517517 2.63798 0 0 -0 3.14159 355 | 356 | 357 | 358 | 359 | 360 | 361 | 3.4 0.05 2.0 362 | 363 | 364 | 0 0 1.0 0 -0 0 365 | 366 | 367 | 0 0 1.0 0 -0 0 368 | 369 | 370 | 3.4 0.05 2.0 371 | 372 | 373 | 374 | 378 | 0.666667 0.666667 0.498039 1 379 | 380 | 381 | -0.835483 3.14198 0 0 -0 1.5708 382 | 383 | 384 | 385 | 386 | 387 | 388 | 3.4 0.05 2.0 389 | 390 | 391 | 0 0 1.0 0 -0 0 392 | 393 | 394 | 0 0 1.0 0 -0 0 395 | 396 | 397 | 3.4 0.05 2.0 398 | 399 | 400 | 401 | 405 | 0.666667 0.666667 0.498039 1 406 | 407 | 408 | 1.97152 3.23298 0 0 -0 1.5708 409 | 410 | 411 | 412 | 413 | 414 | 415 | 2.65 0.05 2.0 416 | 417 | 418 | 0 0 1.0 0 -0 0 419 | 420 | 421 | 0 0 1.0 0 -0 0 422 | 423 | 424 | 2.65 0.05 2.0 425 | 426 | 427 | 428 | 432 | 0.666667 0.666667 0.498039 1 433 | 434 | 435 | -2.34548 -3.86202 0 0 -0 0 436 | 437 | 438 | 439 | 440 | 441 | 442 | 3.4 0.05 2.0 443 | 444 | 445 | 0 0 1.0 0 -0 0 446 | 447 | 448 | 0 0 1.0 0 -0 0 449 | 450 | 451 | 3.4 0.05 2.0 452 | 453 | 454 | 455 | 459 | 0.666667 0.666667 0.498039 1 460 | 461 | 462 | 2.80752 0.736983 0 0 -0 1.5708 463 | 464 | 1 465 | 466 | 467 | 468 | -------------------------------------------------------------------------------- /simulation_environment/models/diff_drive/diff_drive.gazebo: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | true 10 | 20 11 | left_wheel_hinge 12 | right_wheel_hinge 13 | ${L_wheel*2} 14 | ${r_wheel*2} 15 | ${torque_wheel} 16 | cmd_vel 17 | odom 18 | odom_link 19 | base_link 20 | false 21 | 22 | 23 | 24 | 25 | 26 | 27 | true 28 | base_link 29 | imu 30 | imu_service 31 | 0.1 32 | 20.0 33 | 34 | 35 | 36 | 37 | 38 | 39 | 1 40 | 10.0 41 | base_link 42 | test 43 | fix_velocity 44 | 5.0 5.0 5.0 45 | 0.1 0.1 0.1 46 | 0 0 0 47 | 0.1 0.1 0.1 48 | 49 | 50 | 51 | 52 | 53 | 54 | 0 0 0 0 0 0 55 | true 56 | 20 57 | 58 | 59 | 60 | 720 61 | 1 62 | -1.570796 63 | 1.570796 64 | 65 | 66 | 67 | 0.10 68 | 10.0 69 | 0.01 70 | 71 | 72 | gaussian 73 | 0.0 74 | 0.01 75 | 76 | 77 | 78 | /scan 79 | /laser_link 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 20.0 88 | 89 | 1.3962634 90 | 91 | 800 92 | 800 93 | R8G8B8 94 | 95 | 96 | 0.02 97 | 300 98 | 99 | 100 | gaussian 101 | 0.0 102 | 0.007 103 | 104 | 105 | 106 | true 107 | 0.0 108 | camera 109 | image_raw 110 | camera_info 111 | camera_link 112 | 0.07 113 | 0.0 114 | 0.0 115 | 0.0 116 | 0.0 117 | 0.0 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | Gazebo/Orange 130 | 131 | 132 | -------------------------------------------------------------------------------- /simulation_environment/models/diff_drive/diff_drive.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 0 0 0 0 0 0 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 0 0 0 0 0 0 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 | 0 54 | 0 55 | 1.0 56 | 1.0 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 1.0 80 | 1.0 81 | 0.0 82 | 0.0 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 1.0 111 | 1.0 112 | 0.0 113 | 0.0 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | -------------------------------------------------------------------------------- /simulation_environment/models/diff_drive/macros.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 10 | 11 | 12 | 13 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /simulation_environment/models/diff_drive/materials.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /simulation_environment/models/diff_drive/parameter.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /simulation_environment/models/test_human/meshes/brown_eye.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NRottmann/ROS_Gazebo_Tutorial/9847886f7145f0f5d2b38425e62a1029e7b210df/simulation_environment/models/test_human/meshes/brown_eye.png -------------------------------------------------------------------------------- /simulation_environment/models/test_human/meshes/female_casualsuit01_diffuse.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NRottmann/ROS_Gazebo_Tutorial/9847886f7145f0f5d2b38425e62a1029e7b210df/simulation_environment/models/test_human/meshes/female_casualsuit01_diffuse.png -------------------------------------------------------------------------------- /simulation_environment/models/test_human/meshes/female_casualsuit01_normal.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NRottmann/ROS_Gazebo_Tutorial/9847886f7145f0f5d2b38425e62a1029e7b210df/simulation_environment/models/test_human/meshes/female_casualsuit01_normal.png -------------------------------------------------------------------------------- /simulation_environment/models/test_human/meshes/ponytail01_diffuse.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NRottmann/ROS_Gazebo_Tutorial/9847886f7145f0f5d2b38425e62a1029e7b210df/simulation_environment/models/test_human/meshes/ponytail01_diffuse.png -------------------------------------------------------------------------------- /simulation_environment/models/test_human/meshes/test_human.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NRottmann/ROS_Gazebo_Tutorial/9847886f7145f0f5d2b38425e62a1029e7b210df/simulation_environment/models/test_human/meshes/test_human.png -------------------------------------------------------------------------------- /simulation_environment/models/test_human/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | test_human 5 | 1.0 6 | model.sdf 7 | 8 | 9 | My name 10 | name@email.address 11 | 12 | 13 | 14 | A description of the model 15 | 16 | 17 | -------------------------------------------------------------------------------- /simulation_environment/models/test_human/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 0 -0.1 0.95 0 0 0 8 | 60.0 9 | 10 | 24.88 11 | 0.0 12 | 0.0 13 | 25.73 14 | 0.0 15 | 2.48 16 | 17 | 18 | 19 | 20 | 0 0 0.01 0 0 1.57 21 | 22 | 23 | 0.5 0.35 0.02 24 | 25 | 26 | 27 | 28 | 29 | 0 0 0.02 0 0 1.57 30 | 31 | 32 | model://test_human/meshes/test_human.dae 33 | 34 | 35 | 36 | 37 | 38 | 0 0 0.02 0 0 1.57 39 | 40 | 41 | model://test_human/meshes/test_human.dae 42 | 43 | 44 | 45 | 46 | 47 | 1 48 | 49 | 50 | -------------------------------------------------------------------------------- /simulation_environment/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | simulation_environment 4 | 0.0.0 5 | The simulation_environment package for the differential drive robot 6 | 7 | sanu 8 | 9 | BSD 10 | 11 | catkin 12 | gazebo_ros 13 | 14 | gazebo_ros 15 | roscpp 16 | rospy 17 | std_msgs 18 | geometry_msgs 19 | sensor_msgs 20 | nav_msgs 21 | 22 | gazebo_ros 23 | roscpp 24 | rospy 25 | std_msgs 26 | geometry_msgs 27 | sensor_msgs 28 | nav_msgs 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /simulation_environment/src/temp_plugin.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include "sensor_msgs/Temperature.h" 7 | #include 8 | 9 | namespace gazebo 10 | { 11 | class TempSensor : public ModelPlugin 12 | { 13 | 14 | // Pointer to the model 15 | private: physics::ModelPtr model; 16 | 17 | // Pointer to the update event connection 18 | private: event::ConnectionPtr updateConnection; 19 | 20 | // Handle for the gazebo ros node 21 | private: std::unique_ptr rosNode; 22 | 23 | // ROS publisher 24 | ros::Publisher rosPub; 25 | 26 | public: void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) 27 | { 28 | // Store the pointer to the model 29 | this->model = _parent; 30 | 31 | // Make sure the ROS node for Gazebo has already been initialized 32 | if (!ros::isInitialized()) 33 | { 34 | ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. " 35 | << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); 36 | return; 37 | } 38 | 39 | // Reset the ros node name and initialize subscriber and publisher 40 | this->rosNode.reset(new ros::NodeHandle("")); 41 | rosPub = this->rosNode->advertise("temperature", 10); 42 | 43 | // Listen to the update event. This event is broadcast every 44 | // simulation iteration. 45 | this->updateConnection = event::Events::ConnectWorldUpdateBegin( 46 | std::bind(&TempSensor::OnUpdate, this)); 47 | } 48 | 49 | // Called by the world update start event 50 | public: void OnUpdate() 51 | { 52 | // Get pose 53 | ignition::math::Pose3d pose = model->WorldPose(); 54 | // Publish to rostopic temperature 55 | sensor_msgs::Temperature msg; 56 | msg.temperature = getTemperature(pose.Pos().X(), pose.Pos().Y()); 57 | msg.variance = 0; 58 | this->rosPub.publish(msg); 59 | } 60 | 61 | // The temperature field function 62 | private: double getTemperature(double x, double y) { 63 | double Ta = 20; 64 | double Tc1 = 1000; 65 | double Tc2 = 500; 66 | 67 | double xc1 = -5; double yc1 = -5; 68 | double xc2 = 8; double yc2 = -3; 69 | 70 | double r1 = 0.5 * sqrt((x-xc1)*(x-xc1) + (y-yc1)*(y-yc1)); 71 | double r2 = 0.5 * sqrt((x-xc2)*(x-xc2) + (y-yc2)*(y-yc2)); 72 | 73 | double T = Ta + (Tc1 - Ta) * exp(-r1) + (Tc2 - Ta) * exp(-r2); 74 | return T; 75 | } 76 | }; 77 | 78 | // Register this plugin with the simulator 79 | GZ_REGISTER_MODEL_PLUGIN(TempSensor) 80 | } 81 | -------------------------------------------------------------------------------- /simulation_environment/worlds/apartment.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | model://ground_plane 10 | 11 | 14 | 15 | 16 | 17 | 18 | 19 | model://sun 20 | 21 | 22 | 23 | 24 | model://apartment 25 | 0.491830 0.433522 0 0 0 0 26 | 27 | 28 | 29 | 30 | model://test_human 31 | -2.5 4.75 0 0 1.57 -0.8 32 | 33 | 34 | 35 | 36 | 1500 37 | 38 | 39 | quick 40 | 70 41 | 42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /simulation_environment/worlds/temperature.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | model://ground_plane 10 | 11 | 14 | 15 | 16 | 17 | 18 | 19 | model://sun 20 | 21 | 22 | 23 | 24 | 1500 25 | 26 | 27 | quick 28 | 70 29 | 30 | 31 | 32 | 33 | 34 | 35 | --------------------------------------------------------------------------------