├── README.md ├── chapter10_tutorials ├── LICENSE ├── README.md ├── chapter10_tutorials │ ├── CMakeLists.txt │ └── package.xml ├── images │ ├── rosbook_arm_moveit.png │ └── rosbook_arm_moveit_no_controllers.png ├── rosbook_arm_bringup │ ├── CMakeLists.txt │ ├── config │ │ ├── approach_planner.yaml │ │ └── rosbook_arm_motions.yaml │ ├── launch │ │ ├── play_motion.launch │ │ └── rosbook_arm_bringup.launch │ └── package.xml ├── rosbook_arm_controller_configuration │ ├── CMakeLists.txt │ ├── launch │ │ └── default_controllers.launch │ └── package.xml ├── rosbook_arm_controller_configuration_gazebo │ ├── CMakeLists.txt │ ├── config │ │ ├── joint_trajectory_controllers.yaml │ │ └── pids.yaml │ └── package.xml ├── rosbook_arm_description │ ├── CMakeLists.txt │ ├── config │ │ └── joint_limits.yaml │ ├── gazebo │ │ └── gazebo.urdf.xacro │ ├── meshes │ │ └── sensors │ │ │ └── xtion_pro_live │ │ │ ├── xtion_pro_live.dae │ │ │ └── xtion_pro_live.png │ ├── package.xml │ ├── robots │ │ ├── rosbook_arm_base.urdf.xacro │ │ ├── rosbook_arm_rgbd.urdf.xacro │ │ └── upload_rosbook_arm.launch │ └── urdf │ │ ├── arm │ │ ├── arm.gazebo.xacro │ │ ├── arm.transmission.xacro │ │ └── arm.urdf.xacro │ │ ├── base │ │ ├── base.gazebo.xacro │ │ └── base.urdf.xacro │ │ ├── deg_to_rad.xacro │ │ ├── gripper │ │ ├── gripper.gazebo.xacro │ │ ├── gripper.transmission.xacro │ │ └── gripper.urdf.xacro │ │ ├── inertia.xacro │ │ ├── materials.urdf.xacro │ │ └── sensors │ │ ├── xtion_pro_live.gazebo.xacro │ │ └── xtion_pro_live.urdf.xacro ├── rosbook_arm_gazebo │ ├── CMakeLists.txt │ ├── launch │ │ ├── rosbook_arm_empty_world.launch │ │ ├── rosbook_arm_gazebo.launch │ │ ├── rosbook_arm_grasping_world.launch │ │ └── rosbook_arm_spawn.launch │ ├── package.xml │ └── worlds │ │ ├── empty.world │ │ └── grasping.world ├── rosbook_arm_hardware_gazebo │ ├── CMakeLists.txt │ ├── include │ │ └── rosbook_arm_hardware_gazebo │ │ │ └── rosbook_arm_hardware_gazebo.h │ ├── package.xml │ ├── rosbook_arm_hardware_gazebo_plugins.xml │ └── src │ │ └── rosbook_arm_hardware_gazebo.cpp ├── rosbook_arm_moveit_config │ ├── .setup_assistant │ ├── CMakeLists.txt │ ├── config │ │ ├── controllers.yaml │ │ ├── fake_controllers.yaml │ │ ├── joint_limits.yaml │ │ ├── kinematics.yaml │ │ ├── moveit.rviz │ │ ├── ompl_planning.yaml │ │ ├── rosbook_arm.srdf │ │ └── sensors_rgbd.yaml │ ├── launch │ │ ├── default_warehouse_db.launch │ │ ├── demo.launch │ │ ├── fake_moveit_controller_manager.launch.xml │ │ ├── move_group.launch │ │ ├── moveit.rviz │ │ ├── moveit_planning_execution.launch │ │ ├── moveit_rviz.launch │ │ ├── ompl_planning_pipeline.launch.xml │ │ ├── planning_context.launch │ │ ├── planning_pipeline.launch.xml │ │ ├── rosbook_arm_moveit_controller_manager.launch.xml │ │ ├── rosbook_arm_moveit_sensor_manager.launch.xml │ │ ├── run_benchmark_ompl.launch │ │ ├── sensor_manager.launch.xml │ │ ├── setup_assistant.launch │ │ ├── trajectory_execution.launch.xml │ │ ├── warehouse.launch │ │ └── warehouse_settings.launch.xml │ └── package.xml ├── rosbook_arm_pick_and_place │ ├── CMakeLists.txt │ ├── config │ │ └── rosbook_arm_grasp_data.yaml │ ├── launch │ │ └── grasp_generator_server.launch │ ├── package.xml │ └── scripts │ │ └── pick_and_place.py └── rosbook_arm_snippets │ ├── CMakeLists.txt │ ├── package.xml │ └── src │ ├── move_group_add_object.cpp │ ├── move_group_plan_group_state.cpp │ ├── move_group_plan_random_target.cpp │ ├── move_group_plan_single_target.cpp │ └── move_group_remove_object.cpp ├── chapter2_tutorials ├── CMakeLists.txt ├── cfg │ └── chapter2.cfg ├── launch │ └── chapter2.launch ├── msg │ └── chapter2_msg1.msg ├── package.xml ├── src │ ├── example1_a.cpp │ ├── example1_b.cpp │ ├── example2_a.cpp │ ├── example2_b.cpp │ ├── example3_a.cpp │ ├── example3_b.cpp │ └── example4.cpp └── srv │ └── chapter2_srv1.srv ├── chapter3_tutorials ├── CMakeLists.txt ├── bag │ └── 2014-07-01-22-54-34.bag ├── cfg │ └── DynamicParam.cfg ├── config │ ├── bag_plot.perspective │ ├── chapter3_tutorials.config │ ├── diagnostic_aggregator.yaml │ ├── example10.rviz │ ├── example9.rviz │ └── example_tf.rviz ├── launch │ ├── example1.launch │ ├── example10.launch │ ├── example1_gdb.launch │ ├── example1_valgrind.launch │ ├── example2.launch │ ├── example3.launch │ ├── example4_5.launch │ ├── example4_record.launch │ ├── example6.launch │ ├── example7.launch │ ├── example8.launch │ └── example9.launch ├── output │ └── gdb_run_node_example1.txt ├── package.xml ├── src │ ├── example1.cpp │ ├── example10.cpp │ ├── example2.cpp │ ├── example3.cpp │ ├── example4.cpp │ ├── example5.cpp │ ├── example6.cpp │ ├── example7.cpp │ ├── example8.cpp │ └── example9.cpp └── srv │ └── SetSpeed.srv ├── chapter4_tutorials ├── CMakeLists.txt ├── launch │ ├── example1.launch │ ├── example2.launch │ └── example6.launch ├── package.xml ├── rviz │ └── laser.rviz └── src │ ├── c4_example1.cpp │ ├── c4_example2.cpp │ ├── c4_example3.cpp │ ├── c4_example4.cpp │ ├── c4_example5_1.ino │ ├── c4_example5_2.cpp │ ├── c4_example5_2.ino │ ├── c4_example6.cpp │ ├── c4_example7.cpp │ ├── c4_example8.cpp │ └── c4_example9.cpp ├── chapter5_tutorials ├── CMakeLists.txt ├── bag │ └── viso2_demo │ │ └── download_amphoras_pool_bag_files.sh ├── calibration │ ├── camera │ │ ├── calibrationdata.tar.gz │ │ ├── cameracalibrator-4-stdout.log │ │ └── cameracheck-stdout.log │ ├── camera_stereo │ │ ├── calibrationdata.tar.gz │ │ ├── cameracalibrator-4-stdout.log │ │ └── cameracheck-stdout.log │ ├── firewire_camera │ │ ├── calibrationdata.tar.gz │ │ └── cameracalibrator-4-stdout.log │ ├── gscam │ │ ├── calibrationdata.tar.gz │ │ └── stdout.log │ └── pattern │ │ └── calibration_pattern_chessboard.pdf ├── cfg │ ├── .gitignore │ ├── Camera.cfg │ ├── CameraStereo.cfg │ └── Homography.cfg ├── config │ ├── camera │ │ ├── calibration_webcam.yaml │ │ └── webcam.yaml │ ├── camera_stereo │ │ ├── disparity.yaml │ │ ├── logitech_c120.yaml │ │ ├── logitech_c120_left.yaml │ │ ├── logitech_c120_right.yaml │ │ ├── rviz.vcg │ │ └── rviz_odometry.vcg │ ├── chapter5_tutorials.config │ ├── firewire_camera │ │ ├── calibration_firewire_camera.yaml │ │ └── format7_mode0.yaml │ ├── fovis_demo │ │ ├── openni_kinect_hw_registered.yaml │ │ ├── openni_kinect_no_registered.yaml │ │ ├── openni_kinect_sw_registered.yaml │ │ ├── rviz_hw_registered.rviz │ │ ├── rviz_no_registered.rviz │ │ └── rviz_sw_registered.rviz │ ├── gscam │ │ ├── calibration_gscam.yaml │ │ └── logitech.yaml │ ├── usb_cam │ │ └── logitech.yaml │ └── viso2_demo │ │ ├── disparity.yaml │ │ └── rviz.vcg ├── launch │ ├── calibration │ │ ├── calibration_camera_chessboard.launch │ │ ├── calibration_camera_stereo_chessboard.launch │ │ ├── calibration_firewire_camera_acircles.launch │ │ ├── calibration_firewire_camera_chessboard.launch │ │ ├── calibration_firewire_camera_circles.launch │ │ └── calibration_gscam_chessboard.launch │ ├── camera.launch │ ├── camera_polling.launch │ ├── camera_stereo.launch │ ├── camera_timer.launch │ ├── firewire_camera.launch │ ├── fovis_demo.launch │ ├── fovis_hw_registered.launch │ ├── fovis_no_registered.launch │ ├── fovis_sw_registered.launch │ ├── frames │ │ └── stereo_frames.launch │ ├── gscam.launch │ ├── homography.launch │ ├── openni_kinect_hw_registered.launch │ ├── openni_kinect_no_registered.launch │ ├── openni_kinect_sw_registered.launch │ ├── usb_cam.launch │ ├── viso2_demo.launch │ └── visual_odometry │ │ ├── viso2_mono_odometer.launch │ │ └── viso2_stereo_odometer.launch ├── package.xml └── src │ ├── camera.cpp │ ├── camera_polling.cpp │ ├── camera_stereo.cpp │ ├── camera_timer.cpp │ └── homography.cpp ├── chapter6_tutorials ├── CMakeLists.txt ├── data │ ├── test_pcd.pcd │ └── write_pcd_test.pcd ├── package.xml └── src │ ├── pcl_create.cpp │ ├── pcl_downsampling.cpp │ ├── pcl_filter.cpp │ ├── pcl_matching.cpp │ ├── pcl_model_estimation.cpp │ ├── pcl_partitioning.cpp │ ├── pcl_planar_segmentation.cpp │ ├── pcl_read.cpp │ ├── pcl_visualize.cpp │ ├── pcl_visualize2.cpp │ └── pcl_write.cpp ├── chapter7_tutorials ├── chapter7_tutorials │ ├── CMakeLists.txt │ └── package.xml ├── robot1_description │ ├── CMakeLists.txt │ ├── launch │ │ ├── display.launch │ │ ├── display_state_pub.launch │ │ └── state_xacro.launch │ ├── meshes │ │ ├── bot.dae │ │ └── hokuyo.dae │ ├── package.xml │ ├── src │ │ └── state_publisher.cpp │ ├── urdf.rviz │ └── urdf │ │ ├── dae.urdf │ │ ├── origins.gv │ │ ├── origins.pdf │ │ ├── robot.gazebo │ │ ├── robot1.urdf │ │ ├── robot1.xacro │ │ ├── robot1_arm.xacro │ │ ├── robot1_base.xacro │ │ ├── robot1_base_01.xacro │ │ ├── robot1_base_01_cam.xacro │ │ ├── robot1_base_01_cam_hok.xacro │ │ ├── robot1_base_02.xacro │ │ ├── robot1_base_03.xacro │ │ ├── robot1_base_03_hydro.xacro │ │ ├── robot1_base_04.xacro │ │ ├── robot1_base_laser.xacro │ │ ├── robot1_physics.urdf │ │ └── robot1_processed.urdf └── robot1_gazebo │ ├── CMakeLists.txt │ ├── launch │ ├── gazebo.launch │ └── gazebo_wg.launch │ ├── package.xml │ └── worlds │ └── robot.world ├── chapter8_tutorials ├── .comandos.swp ├── CMakeLists.txt ├── launch │ ├── gazebo.launch │ ├── gazebo_map_robot.launch │ ├── gazebo_mapping_robot.launch │ ├── gazebo_xacro.launch │ ├── mapping.rviz │ └── mapping.vcg ├── maps │ ├── map.pgm │ └── map.yaml ├── package.xml ├── src │ ├── base_controller.cpp │ ├── laser.cpp │ ├── odometry.cpp │ ├── tf_broadcaster.cpp │ └── tf_listener.cpp └── urdf │ ├── frames.gv │ ├── robot1_base_01.xacro │ └── robot1_base_04.xacro ├── chapter9_tutorials ├── CMakeLists.txt ├── launch │ ├── base_local_planner_params.yaml │ ├── chapter9_configuration_gazebo.launch │ ├── costmap_common_params.yaml │ ├── display.vcg │ ├── global_costmap_params.yaml │ ├── local_costmap_params.yaml │ ├── move_base.launch │ ├── navigation.rviz │ └── navigation.vcg ├── maps │ ├── map.pgm │ └── map.yaml ├── package.xml └── src │ └── sendGoals.cpp └── contributors.txt /chapter10_tutorials/LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2015, Enrique Fernandez, Anil Mahtani 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | 1. Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 2. Redistributions in binary form must reproduce the above copyright notice, 10 | this list of conditions and the following disclaimer in the documentation 11 | and/or other materials provided with the distribution. 12 | 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | 24 | The views and conclusions contained in the software and documentation are those 25 | of the authors and should not be interpreted as representing official policies, 26 | either expressed or implied, of the project. 27 | -------------------------------------------------------------------------------- /chapter10_tutorials/README.md: -------------------------------------------------------------------------------- 1 | # ROS Book Hydro - Chapter 10 tutorials : ROS arm in MoveIt! # 2 | 3 | ## Launch MoveIt! ## 4 | 5 | * For a kinematic version (very fast) without controllers: 6 | 7 | ``` bash 8 | roslaunch rosbook_arm_moveit_config demo.launch 9 | ``` 10 | 11 | [![rosbook_arm_moveit_no_controllers.png](https://raw.githubusercontent.com/AaronMR/ROS_Book_Hydro/master/chapter10_tutorials/images/rosbook_arm_moveit_no_controllers.png)](http://youtu.be/aAihbFjSwBo) 12 | 13 | * For a version using the simulated model of the arm: 14 | 15 | ``` bash 16 | roslaunch rosbook_arm_gazebo rosbook_arm_empty_world.launch 17 | roslaunch rosbook_arm_moveit_config moveit_rviz.launch config:=true 18 | ``` 19 | 20 | [![rosbook_arm_moveit.png](https://raw.githubusercontent.com/AaronMR/ROS_Book_Hydro/master/chapter10_tutorials/images/rosbook_arm_moveit.png)](https://youtu.be/gZJDvElwqg0) 21 | 22 | # Pick & Place # 23 | 24 | Run the simulated model of the arm on the grasping world: 25 | 26 | ``` bash 27 | roslaunch rosbook_arm_gazebo rosbook_arm_grasping_world.launch 28 | roslaunch rosbook_arm_moveit_config moveit_rviz.launch config:=true 29 | roslaunch rosbook_arm_pick_and_place grasp_generator_server.launch 30 | rosrun rosbook_arm_pick_and_place pick_and_place.py 31 | ``` 32 | 33 | This [video](http://youtu.be/GR0pmhgVq70) shows the grasping world, which also 34 | spawns the arm with an RGB-D sensor on top of it. 35 | 36 | Note that the objects in the planning scene has been created manually, without 37 | perception, by using their dimensions from the SDF files in `~/.gazebo/models` 38 | or the `meshes/.dae` in meshlab as with the coke_can model. 39 | 40 | ## Demo mode ## 41 | 42 | Run the following instead: 43 | 44 | ``` bash 45 | roslaunch rosbook_arm_moveit_config demo.launch 46 | roslaunch rosbook_arm_pick_and_place grasp_generator_server.launch 47 | rosrun rosbook_arm_pick_and_place pick_and_place.py 48 | ``` 49 | 50 | This [video](http://youtu.be/q2YBhHWuJS0) shows the arm doing a Pick and Place 51 | in demo mode. 52 | -------------------------------------------------------------------------------- /chapter10_tutorials/chapter10_tutorials/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(chapter10_tutorials) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /chapter10_tutorials/chapter10_tutorials/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | chapter10_tutorials 4 | 0.0.0 5 | ROS book Chapter 10 tutorials - MoveIt! 6 | 7 | Enrique Fernandez 8 | Enrique Fernandez 9 | 10 | BSD 11 | 12 | catkin 13 | 14 | rosbook_arm_gazebo 15 | rosbook_arm_pick_and_place 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /chapter10_tutorials/images/rosbook_arm_moveit.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AaronMR/Learning_ROS_for_Robotics_Programming_2nd_edition/afc45a0a207f36ced489876abce4414afa2f11ca/chapter10_tutorials/images/rosbook_arm_moveit.png -------------------------------------------------------------------------------- /chapter10_tutorials/images/rosbook_arm_moveit_no_controllers.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AaronMR/Learning_ROS_for_Robotics_Programming_2nd_edition/afc45a0a207f36ced489876abce4414afa2f11ca/chapter10_tutorials/images/rosbook_arm_moveit_no_controllers.png -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rosbook_arm_bringup) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | 8 | foreach(dir config launch) 9 | install(DIRECTORY ${dir}/ 10 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) 11 | endforeach() 12 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_bringup/config/approach_planner.yaml: -------------------------------------------------------------------------------- 1 | approach_planner: 2 | planning_groups: # Sorted by order of preference 3 | - arm 4 | 5 | exclude_from_planning_joints: 6 | - finger_1_joint 7 | - finger_2_joint 8 | 9 | joint_tolerance: 0.01 10 | skip_planning_approach_vel: 0.5 11 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_bringup/config/rosbook_arm_motions.yaml: -------------------------------------------------------------------------------- 1 | play_motion: 2 | motions: 3 | grasp: 4 | joints: [shoulder_joint, rotor_0_joint, upper_arm_joint, rotor_1_joint, forearm_joint, rotor_2_joint, tool_joint, 5 | finger_1_joint, finger_2_joint] 6 | points: 7 | - positions: [0.0, 0.0, 0.0, 0.0, 1.57, 0.0, 0.0, 0.0, 0.0] 8 | time_from_start: 0.0 9 | meta: 10 | name: Grasp 11 | usage: posture 12 | description: Arm in grasping position, with elbow at 90 degrees. 13 | home: 14 | joints: [shoulder_joint, rotor_0_joint, upper_arm_joint, rotor_1_joint, forearm_joint, rotor_2_joint, tool_joint, 15 | finger_1_joint, finger_2_joint] 16 | points: 17 | - positions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 18 | time_from_start: 0.0 19 | meta: 20 | name: Home 21 | usage: posture 22 | description: Arm up and completely stretched. 23 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_bringup/launch/play_motion.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_bringup/launch/rosbook_arm_bringup.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rosbook_arm_bringup 4 | 0.0.0 5 | Bring up ROS book arm 6 | 7 | Enrique Fernandez 8 | Enrique Fernandez 9 | 10 | BSD 11 | 12 | catkin 13 | 14 | robot_state_publisher 15 | rosbook_arm_moveit_config 16 | rosbook_arm_controller_configuration 17 | rosbook_arm_description 18 | play_motion 19 | 20 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_controller_configuration/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rosbook_arm_controller_configuration) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | 8 | foreach(dir launch) 9 | install(DIRECTORY ${dir}/ 10 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) 11 | endforeach() 12 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_controller_configuration/launch/default_controllers.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_controller_configuration/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rosbook_arm_controller_configuration 4 | 0.0.0 5 | ROS book arm controller configuration 6 | 7 | Enrique Fernandez 8 | Enrique Fernandez 9 | 10 | BSD 11 | 12 | catkin 13 | 14 | controller_manager 15 | joint_state_controller 16 | joint_trajectory_controller 17 | 18 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_controller_configuration_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rosbook_arm_controller_configuration_gazebo) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | 8 | foreach(dir config) 9 | install(DIRECTORY ${dir}/ 10 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) 11 | endforeach() 12 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_controller_configuration_gazebo/config/joint_trajectory_controllers.yaml: -------------------------------------------------------------------------------- 1 | arm_controller: 2 | type: position_controllers/JointTrajectoryController 3 | joints: 4 | - shoulder_joint 5 | - rotor_0_joint 6 | - upper_arm_joint 7 | - rotor_1_joint 8 | - forearm_joint 9 | - rotor_2_joint 10 | - tool_joint 11 | 12 | constraints: 13 | goal_time: &goal_time_constraint 2.0 14 | shoulder_joint: 15 | goal: &goal_pos_constraint 0.3 16 | rotor_0_joint: 17 | goal: *goal_pos_constraint 18 | upper_arm_joint: 19 | goal: *goal_pos_constraint 20 | rotor_1_joint: 21 | goal: *goal_pos_constraint 22 | forearm_joint: 23 | goal: *goal_pos_constraint 24 | rotor_2_joint: 25 | goal: *goal_pos_constraint 26 | tool_joint: 27 | goal: *goal_pos_constraint 28 | 29 | gripper_controller: 30 | type: position_controllers/JointTrajectoryController 31 | joints: 32 | - finger_1_joint 33 | - finger_2_joint 34 | 35 | constraints: 36 | goal_time: *goal_time_constraint 37 | finger_1_joint: 38 | goal: *goal_pos_constraint 39 | finger_2_joint: 40 | goal: *goal_pos_constraint 41 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_controller_configuration_gazebo/config/pids.yaml: -------------------------------------------------------------------------------- 1 | gains: 2 | shoulder_joint: {p: 1000.0, d: 10.0, i: 0.01, i_clamp: 3.0} 3 | rotor_0_joint: {p: 1000.0, d: 10.0, i: 0.01, i_clamp: 3.0} 4 | upper_arm_joint: {p: 1000.0, d: 10.0, i: 0.01, i_clamp: 3.0} 5 | rotor_1_joint: {p: 1000.0, d: 10.0, i: 0.01, i_clamp: 3.0} 6 | forearm_joint: {p: 1000.0, d: 10.0, i: 0.01, i_clamp: 3.0} 7 | rotor_2_joint: {p: 1000.0, d: 10.0, i: 0.01, i_clamp: 3.0} 8 | tool_joint: {p: 1000.0, d: 10.0, i: 0.01, i_clamp: 3.0} 9 | 10 | finger_1_joint: {p: 50.0, d: 1.0, i: 0.01, i_clamp: 1.0} 11 | finger_2_joint: {p: 50.0, d: 1.0, i: 0.01, i_clamp: 1.0} 12 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_controller_configuration_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rosbook_arm_controller_configuration_gazebo 4 | 0.0.0 5 | ROS book arm controller configuration for gazebo simulation 6 | 7 | Enrique Fernandez 8 | Enrique Fernandez 9 | 10 | BSD 11 | 12 | catkin 13 | 14 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rosbook_arm_description) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | 8 | foreach(dir config gazebo meshes robots urdf) 9 | install(DIRECTORY ${dir}/ 10 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) 11 | endforeach() 12 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_description/config/joint_limits.yaml: -------------------------------------------------------------------------------- 1 | joint_limits: 2 | shoulder_joint: 3 | has_velocity_limits: true 4 | max_velocity: &max_arm_vel 5.0 5 | has_acceleration_limits: false 6 | max_acceleration: &max_arm_acc 0 7 | rotor_0_joint: 8 | has_velocity_limits: true 9 | max_velocity: *max_arm_vel 10 | has_acceleration_limits: false 11 | max_acceleration: *max_arm_acc 12 | upper_arm_joint: 13 | has_velocity_limits: true 14 | max_velocity: *max_arm_vel 15 | has_acceleration_limits: false 16 | max_acceleration: *max_arm_acc 17 | rotor_1_joint: 18 | has_velocity_limits: true 19 | max_velocity: *max_arm_vel 20 | has_acceleration_limits: false 21 | max_acceleration: *max_arm_acc 22 | forearm_joint: 23 | has_velocity_limits: true 24 | max_velocity: *max_arm_vel 25 | has_acceleration_limits: false 26 | max_acceleration: *max_arm_acc 27 | rotor_2_joint: 28 | has_velocity_limits: true 29 | max_velocity: *max_arm_vel 30 | has_acceleration_limits: false 31 | max_acceleration: *max_arm_acc 32 | tool_joint: 33 | has_velocity_limits: true 34 | max_velocity: *max_arm_vel 35 | has_acceleration_limits: false 36 | max_acceleration: *max_arm_acc 37 | 38 | finger_1_joint: 39 | has_velocity_limits: true 40 | max_velocity: &max_finger_vel 5.0 41 | has_acceleration_limits: false 42 | max_acceleration: &max_finger_acc 0 43 | finger_2_joint: 44 | has_velocity_limits: true 45 | max_velocity: *max_finger_vel 46 | has_acceleration_limits: false 47 | max_acceleration: *max_finger_acc 48 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_description/gazebo/gazebo.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | rosbook_arm_hardware_gazebo/ROSBookArmHardwareGazebo 7 | 0.001 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_description/meshes/sensors/xtion_pro_live/xtion_pro_live.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AaronMR/Learning_ROS_for_Robotics_Programming_2nd_edition/afc45a0a207f36ced489876abce4414afa2f11ca/chapter10_tutorials/rosbook_arm_description/meshes/sensors/xtion_pro_live/xtion_pro_live.png -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rosbook_arm_description 4 | 0.0.0 5 | ROS book arm URDF description 6 | 7 | anil 8 | anil 9 | 10 | BSD 11 | 12 | catkin 13 | 14 | xacro 15 | 16 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_description/robots/rosbook_arm_base.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_description/robots/rosbook_arm_rgbd.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 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 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_description/robots/upload_rosbook_arm.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_description/urdf/arm/arm.transmission.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | transmission_interface/SimpleTransmission 5 | 6 | ${reduction} 7 | hardware_interface/PositionJointInterface 8 | 9 | 10 | hardware_interface/PositionJointInterface 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_description/urdf/base/base.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 1000000.0 6 | 100.0 7 | 100.0 8 | 100.0 9 | 0.0 10 | 0.0 11 | 12 | 13 | 14 | true 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_description/urdf/base/base.urdf.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 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_description/urdf/deg_to_rad.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_description/urdf/gripper/gripper.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0.9 6 | 0.9 7 | 8 | 9 | 0.9 10 | 0.9 11 | 12 | 13 | 14 | 15 | true 16 | 17 | 18 | true 19 | 20 | 21 | 22 | 23 | Gazebo/Black 24 | 25 | 26 | Gazebo/Black 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_description/urdf/gripper/gripper.transmission.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | transmission_interface/SimpleTransmission 5 | 6 | ${reduction} 7 | hardware_interface/PositionJointInterface 8 | 9 | 10 | hardware_interface/PositionJointInterface 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_description/urdf/inertia.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | 10 | 11 | 12 | 15 | 16 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_description/urdf/materials.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rosbook_arm_gazebo) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | 8 | foreach(dir launch worlds) 9 | install(DIRECTORY ${dir}/ 10 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) 11 | endforeach() 12 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_gazebo/launch/rosbook_arm_empty_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_gazebo/launch/rosbook_arm_gazebo.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 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_gazebo/launch/rosbook_arm_grasping_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_gazebo/launch/rosbook_arm_spawn.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 19 | 20 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rosbook_arm_gazebo 4 | 0.0.0 5 | ROS book arm gazebo simulation 6 | 7 | Enrique Fernandez 8 | Enrique Fernandez 9 | 10 | BSD 11 | 12 | catkin 13 | 14 | gazebo_ros 15 | rosbook_arm_bringup 16 | rosbook_arm_controller_configuration_gazebo 17 | rosbook_arm_description 18 | rosbook_arm_hardware_gazebo 19 | 20 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_gazebo/worlds/empty.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | model://sun 7 | 8 | 9 | 10 | 11 | model://ground_plane 12 | 13 | 14 | 15 | 16 | 0.001 17 | 1 18 | 1000 19 | 0 0 -9.81 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_gazebo/worlds/grasping.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | model://sun 7 | 8 | 9 | 10 | 11 | model://ground_plane 12 | 13 | 14 | 15 | 16 | model://table 17 | 1 0 0 0 0 1.5708 18 | 0 19 | 20 | 21 | 22 | 23 | 27 | 28 | 29 | 30 | model://coke_can 31 | 0.75 0.25 1.05 0 0 0 32 | 33 | 34 | 35 | 36 | 0.001 37 | 1 38 | 1000 39 | 0 0 -9.81 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_hardware_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rosbook_arm_hardware_gazebo) 3 | 4 | find_package(catkin REQUIRED 5 | COMPONENTS 6 | roscpp 7 | angles 8 | control_toolbox 9 | gazebo_ros_control 10 | hardware_interface 11 | joint_limits_interface 12 | ) 13 | 14 | find_package(gazebo REQUIRED) 15 | 16 | catkin_package( 17 | INCLUDE_DIRS include 18 | LIBRARIES ${PROJECT_NAME} 19 | CATKIN_DEPENDS 20 | roscpp 21 | angles 22 | control_toolbox 23 | gazebo_ros_control 24 | hardware_interface 25 | joint_limits_interface 26 | ) 27 | 28 | include_directories(include ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS}) 29 | 30 | add_library(${PROJECT_NAME} src/rosbook_arm_hardware_gazebo.cpp) 31 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}) 32 | 33 | install(TARGETS ${PROJECT_NAME} 34 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 35 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 36 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 37 | ) 38 | 39 | install(DIRECTORY include/ 40 | DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} 41 | FILES_MATCHING PATTERN "*.h" 42 | ) 43 | install (FILES rosbook_arm_hardware_gazebo_plugins.xml 44 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 45 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_hardware_gazebo/include/rosbook_arm_hardware_gazebo/rosbook_arm_hardware_gazebo.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef ROSBOOK_ARM_HARDWARE_GAZEBO_H 3 | #define ROSBOOK_ARM_HARDWARE_GAZEBO_H 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | namespace rosbook_arm_hardware_gazebo 22 | { 23 | 24 | class ROSBookArmHardwareGazebo : public gazebo_ros_control::RobotHWSim 25 | { 26 | public: 27 | 28 | ROSBookArmHardwareGazebo(); 29 | 30 | bool initSim(const std::string& robot_namespace, 31 | ros::NodeHandle model_nh, 32 | gazebo::physics::ModelPtr parent_model, 33 | const urdf::Model* const urdf_model, 34 | std::vector transmissions); 35 | 36 | void readSim(ros::Time time, ros::Duration period); 37 | 38 | void writeSim(ros::Time time, ros::Duration period); 39 | 40 | private: 41 | // Raw data 42 | unsigned int n_dof_; 43 | 44 | std::vector transmission_names_; 45 | 46 | std::vector jnt_pos_; 47 | std::vector jnt_vel_; 48 | std::vector jnt_eff_; 49 | 50 | std::vector jnt_pos_cmd_; 51 | 52 | std::vector sim_joints_; 53 | 54 | // Hardware interface: joints 55 | hardware_interface::JointStateInterface jnt_state_interface_; 56 | hardware_interface::PositionJointInterface jnt_pos_cmd_interface_; 57 | 58 | // Joint limits interface 59 | joint_limits_interface::PositionJointSoftLimitsInterface jnt_limits_interface_; 60 | 61 | // PID controllers 62 | std::vector pids_; 63 | 64 | template 65 | std::string containerToString(const T& cont, const std::string& prefix) 66 | { 67 | std::stringstream ss; 68 | ss << prefix; 69 | std::copy(cont.begin(), --cont.end(), std::ostream_iterator(ss, prefix.c_str())); 70 | ss << *(--cont.end()); 71 | return ss.str(); 72 | } 73 | 74 | }; 75 | 76 | } // namespace rosbook_arm_hardware_gazebo 77 | 78 | #endif // ROSBOOK_ARM_HARDWARE_GAZEBO_H 79 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_hardware_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rosbook_arm_hardware_gazebo 4 | 0.0.0 5 | ROS book hardware for gazebo simulation 6 | 7 | Enrique Fernandez 8 | Enrique Fernandez 9 | 10 | BSD 11 | 12 | catkin 13 | 14 | roscpp 15 | angles 16 | control_toolbox 17 | gazebo_ros_control 18 | hardware_interface 19 | joint_limits_interface 20 | 21 | gazebo 22 | 23 | angles 24 | control_toolbox 25 | gazebo_ros_control 26 | hardware_interface 27 | joint_limits_interface 28 | roscpp 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_hardware_gazebo/rosbook_arm_hardware_gazebo_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 7 | 8 | ROS Book arm simulation interface. 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_moveit_config/.setup_assistant: -------------------------------------------------------------------------------- 1 | moveit_setup_assistant_config: 2 | URDF: 3 | package: rosbook_arm_description 4 | relative_path: robots/rosbook_arm_rgbd.urdf.xacro 5 | SRDF: 6 | relative_path: config/rosbook_arm.srdf 7 | CONFIG: 8 | generated_timestamp: 1433090399 9 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_moveit_config/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rosbook_arm_moveit_config) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | 8 | foreach(dir config launch) 9 | install(DIRECTORY ${dir}/ 10 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) 11 | endforeach() 12 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_moveit_config/config/controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_manager_ns: controller_manager 2 | controller_list: 3 | - name: arm_controller 4 | action_ns: follow_joint_trajectory 5 | type: FollowJointTrajectory 6 | default: true 7 | joints: 8 | - shoulder_joint 9 | - rotor_0_joint 10 | - upper_arm_joint 11 | - rotor_1_joint 12 | - forearm_joint 13 | - rotor_2_joint 14 | - tool_joint 15 | - name: gripper_controller 16 | action_ns: follow_joint_trajectory 17 | type: FollowJointTrajectory 18 | default: true 19 | joints: 20 | - finger_1_joint 21 | - finger_2_joint 22 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_moveit_config/config/fake_controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_list: 2 | - name: fake_arm_controller 3 | joints: 4 | - shoulder_joint 5 | - rotor_0_joint 6 | - upper_arm_joint 7 | - rotor_1_joint 8 | - forearm_joint 9 | - rotor_2_joint 10 | - tool_joint 11 | - name: fake_gripper_controller 12 | joints: 13 | - finger_1_joint 14 | - finger_2_joint 15 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_moveit_config/config/joint_limits.yaml: -------------------------------------------------------------------------------- 1 | joint_limits: 2 | shoulder_joint: 3 | has_velocity_limits: true 4 | max_velocity: &max_arm_vel 5 5 | has_acceleration_limits: false 6 | max_acceleration: &max_arm_acc 0 7 | rotor_0_joint: 8 | has_velocity_limits: true 9 | max_velocity: *max_arm_vel 10 | has_acceleration_limits: false 11 | max_acceleration: *max_arm_acc 12 | upper_arm_joint: 13 | has_velocity_limits: true 14 | max_velocity: *max_arm_vel 15 | has_acceleration_limits: false 16 | max_acceleration: *max_arm_acc 17 | rotor_1_joint: 18 | has_velocity_limits: true 19 | max_velocity: *max_arm_vel 20 | has_acceleration_limits: false 21 | max_acceleration: *max_arm_acc 22 | forearm_joint: 23 | has_velocity_limits: true 24 | max_velocity: *max_arm_vel 25 | has_acceleration_limits: false 26 | max_acceleration: *max_arm_acc 27 | rotor_2_joint: 28 | has_velocity_limits: true 29 | max_velocity: *max_arm_vel 30 | has_acceleration_limits: false 31 | max_acceleration: *max_arm_acc 32 | tool_joint: 33 | has_velocity_limits: true 34 | max_velocity: *max_arm_vel 35 | has_acceleration_limits: false 36 | max_acceleration: *max_arm_acc 37 | 38 | finger_1_joint: 39 | has_velocity_limits: true 40 | max_velocity: &max_finger_vel 5 41 | has_acceleration_limits: false 42 | max_acceleration: &max_finger_acc 0 43 | finger_2_joint: 44 | has_velocity_limits: true 45 | max_velocity: *max_finger_vel 46 | has_acceleration_limits: false 47 | max_acceleration: *max_finger_acc 48 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_moveit_config/config/kinematics.yaml: -------------------------------------------------------------------------------- 1 | arm: 2 | kinematics_solver : kdl_kinematics_plugin/KDLKinematicsPlugin 3 | kinematics_solver_search_resolution: 0.005 4 | kinematics_solver_timeout : 0.005 5 | kinematics_solver_attempts : 3 6 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_moveit_config/config/ompl_planning.yaml: -------------------------------------------------------------------------------- 1 | planner_configs: 2 | SBLkConfigDefault: 3 | type: geometric::SBL 4 | ESTkConfigDefault: 5 | type: geometric::EST 6 | LBKPIECEkConfigDefault: 7 | type: geometric::LBKPIECE 8 | BKPIECEkConfigDefault: 9 | type: geometric::BKPIECE 10 | KPIECEkConfigDefault: 11 | type: geometric::KPIECE 12 | RRTkConfigDefault: 13 | type: geometric::RRT 14 | RRTConnectkConfigDefault: 15 | type: geometric::RRTConnect 16 | RRTstarkConfigDefault: 17 | type: geometric::RRTstar 18 | TRRTkConfigDefault: 19 | type: geometric::TRRT 20 | PRMkConfigDefault: 21 | type: geometric::PRM 22 | PRMstarkConfigDefault: 23 | type: geometric::PRMstar 24 | arm: 25 | planner_configs: 26 | - SBLkConfigDefault 27 | - ESTkConfigDefault 28 | - LBKPIECEkConfigDefault 29 | - BKPIECEkConfigDefault 30 | - KPIECEkConfigDefault 31 | - RRTkConfigDefault 32 | - RRTConnectkConfigDefault 33 | - RRTstarkConfigDefault 34 | - TRRTkConfigDefault 35 | - PRMkConfigDefault 36 | - PRMstarkConfigDefault 37 | projection_evaluator: joints(shoulder_joint,upper_arm_joint) 38 | longest_valid_segment_fraction: 0.05 39 | gripper: 40 | planner_configs: 41 | - SBLkConfigDefault 42 | - ESTkConfigDefault 43 | - LBKPIECEkConfigDefault 44 | - BKPIECEkConfigDefault 45 | - KPIECEkConfigDefault 46 | - RRTkConfigDefault 47 | - RRTConnectkConfigDefault 48 | - RRTstarkConfigDefault 49 | - TRRTkConfigDefault 50 | - PRMkConfigDefault 51 | - PRMstarkConfigDefault 52 | projection_evaluator: joints(finger_1_joint,finger_2_joint) 53 | longest_valid_segment_fraction: 0.05 54 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_moveit_config/config/sensors_rgbd.yaml: -------------------------------------------------------------------------------- 1 | sensors: 2 | - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater 3 | point_cloud_topic: /rgbd_camera/depth/points 4 | max_range: 10 5 | padding_offset: 0.01 6 | padding_scale: 1.0 7 | point_subsample: 1 8 | filtered_cloud_topic: output_cloud 9 | # this last topic is just to debug, having it adds processing time 10 | 11 | 12 | # sensors: 13 | # - sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater 14 | # image_topic: /head_mount_xtion/depth/image_raw 15 | # queue_size: 5 16 | # near_clipping_plane_distance: 0.3 17 | # far_clipping_plane_distance: 5.0 18 | # skip_vertical_pixels: 1 19 | # skip_horizontal_pixels: 1 20 | # shadow_threshold: 0.2 21 | # padding_scale: 4.0 22 | # padding_offset: 0.03 23 | # filtered_cloud_topic: output_cloud 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_moveit_config/launch/default_warehouse_db.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_moveit_config/launch/demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | [/move_group/fake_controller_joint_states] 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_moveit_config/launch/fake_moveit_controller_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_moveit_config/launch/moveit_planning_execution.launch: -------------------------------------------------------------------------------- 1 | 2 | # The planning and execution components of MoveIt! configured to 3 | # publish the current configuration of the robot (simulated or real) 4 | # and the current state of the world as seen by the planner 5 | 6 | 7 | 8 | # The visualization component of MoveIt! 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_moveit_config/launch/moveit_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_moveit_config/launch/ompl_planning_pipeline.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_moveit_config/launch/planning_context.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 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_moveit_config/launch/planning_pipeline.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_moveit_config/launch/rosbook_arm_moveit_controller_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_moveit_config/launch/rosbook_arm_moveit_sensor_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_moveit_config/launch/run_benchmark_ompl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_moveit_config/launch/sensor_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_moveit_config/launch/setup_assistant.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_moveit_config/launch/trajectory_execution.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_moveit_config/launch/warehouse.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_moveit_config/launch/warehouse_settings.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_moveit_config/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rosbook_arm_moveit_config 4 | 0.0.0 5 | ROS book arm MoveIt! configuration 6 | 7 | Enrique Fernandez 8 | Enrique Fernandez 9 | 10 | BSD 11 | 12 | catkin 13 | 14 | joint_state_publisher 15 | moveit_fake_controller_manager 16 | moveit_planners_ompl 17 | moveit_ros_move_group 18 | moveit_ros_visualization 19 | moveit_simple_controller_manager 20 | robot_state_publisher 21 | rosbook_arm_description 22 | xacro 23 | 24 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_pick_and_place/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rosbook_arm_pick_and_place) 3 | 4 | find_package(catkin REQUIRED 5 | COMPONENTS 6 | actionlib 7 | geometry_msgs 8 | moveit_msgs 9 | moveit_simple_grasps 10 | rospy 11 | std_msgs 12 | tf 13 | trajectory_msgs 14 | ) 15 | 16 | catkin_package( 17 | CATKIN_DEPENDS 18 | geometry_msgs 19 | moveit_msgs 20 | std_msgs 21 | trajectory_msgs 22 | ) 23 | 24 | foreach(dir config launch) 25 | install(DIRECTORY ${dir}/ 26 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) 27 | endforeach() 28 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_pick_and_place/config/rosbook_arm_grasp_data.yaml: -------------------------------------------------------------------------------- 1 | base_link: base_link 2 | 3 | gripper: 4 | end_effector_name: gripper 5 | 6 | # Default grasp params 7 | joints: [finger_1_joint, finger_2_joint] 8 | 9 | pregrasp_posture: [0.0, 0.0] 10 | pregrasp_time_from_start: &time_from_start 4.0 11 | 12 | grasp_posture: [1.0, 1.0] 13 | grasp_time_from_start: *time_from_start 14 | 15 | postplace_time_from_start: *time_from_start 16 | 17 | # Desired pose from end effector to grasp [x, y, z] + [R, P, Y] 18 | grasp_pose_to_eef: [0.0, 0.0, 0.0] 19 | grasp_pose_to_eef_rotation: [0.0, 0.0, 0.0] 20 | 21 | end_effector_parent_link: tool_link 22 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_pick_and_place/launch/grasp_generator_server.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_pick_and_place/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rosbook_arm_pick_and_place 4 | 0.0.0 5 | ROS book arm Pick and Place 6 | 7 | Enrique Fernandez 8 | Sam Pfeiffer 9 | Enrique Fernandez 10 | 11 | BSD 12 | 13 | catkin 14 | 15 | actionlib 16 | geometry_msgs 17 | moveit_msgs 18 | moveit_simple_grasps 19 | rospy 20 | std_msgs 21 | tf 22 | trajectory_msgs 23 | 24 | actionlib 25 | geometry_msgs 26 | moveit_msgs 27 | moveit_simple_grasps 28 | rospy 29 | std_msgs 30 | tf 31 | trajectory_msgs 32 | 33 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_snippets/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rosbook_arm_snippets) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS moveit_core moveit_ros_planning moveit_ros_planning_interface pluginlib cmake_modules) 8 | find_package(Boost REQUIRED system filesystem date_time thread) 9 | find_package(Eigen REQUIRED) 10 | 11 | include_directories(SYSTEM ${Boost_INCLUDE_DIR} ${EIGEN_INCLUDE_DIRS}) 12 | include_directories(${catkin_INCLUDE_DIRS}) 13 | link_directories(${catkin_LIBRARY_DIRS}) 14 | 15 | catkin_package( 16 | CATKIN_DEPENDS 17 | moveit_core 18 | moveit_ros_planning_interface 19 | ) 20 | 21 | add_executable(move_group_plan_single_target src/move_group_plan_single_target.cpp) 22 | target_link_libraries(move_group_plan_single_target ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 23 | 24 | add_executable(move_group_plan_group_state src/move_group_plan_group_state.cpp) 25 | target_link_libraries(move_group_plan_group_state ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 26 | 27 | add_executable(move_group_plan_random_target src/move_group_plan_random_target.cpp) 28 | target_link_libraries(move_group_plan_random_target ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 29 | 30 | add_executable(move_group_add_object src/move_group_add_object.cpp) 31 | target_link_libraries(move_group_add_object ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 32 | 33 | add_executable(move_group_remove_object src/move_group_remove_object.cpp) 34 | target_link_libraries(move_group_remove_object ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_snippets/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rosbook_arm_snippets 4 | 0.0.0 5 | The rosbook_arm_snippets package 6 | 7 | anil 8 | 9 | 10 | TODO 11 | 12 | 13 | catkin 14 | 15 | pluginlib 16 | moveit_core 17 | moveit_ros_planning_interface 18 | moveit_ros_perception 19 | cmake_modules 20 | 21 | pluginlib 22 | moveit_core 23 | moveit_fake_controller_manager 24 | moveit_ros_planning_interface 25 | moveit_ros_perception 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_snippets/src/move_group_add_object.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | 7 | int main(int argc, char **argv) 8 | { 9 | // Initialize ROS, create the node handle and an async spinner 10 | ros::init(argc, argv, "move_group_add_object"); 11 | ros::NodeHandle nh; 12 | 13 | ros::AsyncSpinner spin(1); 14 | spin.start(); 15 | 16 | // We obtain the current planning scene and wait until everything is up 17 | // and running, otherwise the request won't succeed 18 | moveit::planning_interface::PlanningSceneInterface current_scene; 19 | 20 | sleep(5.0); 21 | 22 | // We create a box with certain dimensions and orientation, we also 23 | // give it a name which can later be used to remove it from the scene 24 | // The dimensions of the box (and also the object type which in this case 25 | // is box) is defined by a SolidPrimitive message, the pose of the box by a 26 | // pose message 27 | moveit_msgs::CollisionObject box; 28 | 29 | box.id = "rosbook_box"; 30 | 31 | shape_msgs::SolidPrimitive primitive; 32 | primitive.type = primitive.BOX; 33 | primitive.dimensions.resize(3); 34 | primitive.dimensions[0] = 0.2; 35 | primitive.dimensions[1] = 0.2; 36 | primitive.dimensions[2] = 0.2; 37 | 38 | geometry_msgs::Pose pose; 39 | pose.orientation.w = 1.0; 40 | pose.position.x = 0.7; 41 | pose.position.y = -0.5; 42 | pose.position.z = 1.0; 43 | 44 | box.primitives.push_back(primitive); 45 | box.primitive_poses.push_back(pose); 46 | box.operation = box.ADD; 47 | 48 | std::vector collision_objects; 49 | collision_objects.push_back(box); 50 | 51 | // Once all of the objects (in this case just one) have been added to the 52 | // vector, we tell the planning scene to add our new box 53 | current_scene.addCollisionObjects(collision_objects); 54 | 55 | ros::shutdown(); 56 | 57 | return 0; 58 | } 59 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_snippets/src/move_group_plan_group_state.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | int main(int argc, char **argv) 6 | { 7 | // Initialize ROS, create the node handle and an async spinner 8 | ros::init(argc, argv, "move_group_plan_group_state"); 9 | ros::NodeHandle nh; 10 | 11 | ros::AsyncSpinner spin(1); 12 | spin.start(); 13 | 14 | // Get the arm planning group 15 | moveit::planning_interface::MoveGroup plan_group("arm"); 16 | 17 | // Create a published for the arm plan visualization 18 | ros::Publisher display_pub = nh.advertise("/move_group/display_planned_path", 1, true); 19 | 20 | // Get the current RobotState, which will be used to set the arm 21 | // to one of the predefined group states, in this case home 22 | robot_state::RobotState current_state = *plan_group.getCurrentState(); 23 | 24 | // We set the state values for this robot state to the predefined 25 | // group state values 26 | current_state.setToDefaultValues(current_state.getJointModelGroup("arm"), "home"); 27 | 28 | // We set the current state values to the target values 29 | plan_group.setJointValueTarget(current_state); 30 | 31 | 32 | // Perform the planning step, and if it succeeds display the current 33 | // arm trajectory and move the arm 34 | moveit::planning_interface::MoveGroup::Plan goal_plan; 35 | if (plan_group.plan(goal_plan)) 36 | { 37 | moveit_msgs::DisplayTrajectory display_msg; 38 | display_msg.trajectory_start = goal_plan.start_state_; 39 | display_msg.trajectory.push_back(goal_plan.trajectory_); 40 | display_pub.publish(display_msg); 41 | 42 | sleep(5.0); 43 | 44 | plan_group.move(); 45 | } 46 | 47 | ros::shutdown(); 48 | 49 | return 0; 50 | } 51 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_snippets/src/move_group_plan_single_target.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | int main(int argc, char **argv) 6 | { 7 | // Initialize ROS, create the node handle and an async spinner 8 | ros::init(argc, argv, "move_group_plan_single_target"); 9 | ros::NodeHandle nh; 10 | 11 | ros::AsyncSpinner spin(1); 12 | spin.start(); 13 | 14 | // Get the arm planning group 15 | moveit::planning_interface::MoveGroup plan_group("arm"); 16 | 17 | // Create a published for the arm plan visualization 18 | ros::Publisher display_pub = nh.advertise("/move_group/display_planned_path", 1, true); 19 | 20 | // Set a goal message as a pose of the end effector 21 | geometry_msgs::Pose goal; 22 | goal.orientation.x = -0.000764819; 23 | goal.orientation.y = 0.0366097; 24 | goal.orientation.z = 0.00918912; 25 | goal.orientation.w = 0.999287; 26 | goal.position.x = 0.775884; 27 | goal.position.y = 0.43172; 28 | goal.position.z = 2.71809; 29 | 30 | // Set the tolerance to consider the goal achieved 31 | plan_group.setGoalTolerance(0.2); 32 | 33 | // Set the target pose, which is the goal we already defined 34 | plan_group.setPoseTarget(goal); 35 | 36 | // Perform the planning step, and if it succeeds display the current 37 | // arm trajectory and move the arm 38 | moveit::planning_interface::MoveGroup::Plan goal_plan; 39 | if (plan_group.plan(goal_plan)) 40 | { 41 | moveit_msgs::DisplayTrajectory display_msg; 42 | display_msg.trajectory_start = goal_plan.start_state_; 43 | display_msg.trajectory.push_back(goal_plan.trajectory_); 44 | display_pub.publish(display_msg); 45 | 46 | sleep(5.0); 47 | 48 | plan_group.move(); 49 | } 50 | 51 | ros::shutdown(); 52 | 53 | return 0; 54 | } 55 | -------------------------------------------------------------------------------- /chapter10_tutorials/rosbook_arm_snippets/src/move_group_remove_object.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | int main(int argc, char **argv) 5 | { 6 | // Initialize ROS, create the node handle and an async spinner 7 | ros::init(argc, argv, "move_group_remove_object"); 8 | ros::NodeHandle nh; 9 | 10 | ros::AsyncSpinner spin(1); 11 | spin.start(); 12 | 13 | // We obtain the current planning scene and wait until everything is up 14 | // and running, otherwise the request won't succeed 15 | moveit::planning_interface::PlanningSceneInterface current_scene; 16 | 17 | sleep(5.0); 18 | 19 | // We add the name of the objects we want to remove into the vector and 20 | // use the planning scene interface to remove those collision objects 21 | std::vector object_ids; 22 | object_ids.push_back("rosbook_box"); 23 | current_scene.removeCollisionObjects(object_ids); 24 | 25 | ros::shutdown(); 26 | 27 | return 0; 28 | } 29 | -------------------------------------------------------------------------------- /chapter2_tutorials/cfg/chapter2.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "chapter2_tutorials" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("int_param", int_t, 0, "An Integer parameter", 1, 0, 100) 9 | gen.add("double_param", double_t, 0, "A double parameter", .1, 0, 1) 10 | gen.add("str_param", str_t, 0, "A string parameter", "Chapter2_dynamic_reconfigure") 11 | gen.add("bool_param", bool_t, 0, "A Boolean parameter", True) 12 | 13 | size_enum = gen.enum([ gen.const("Low", int_t, 0, "Low is 0"), 14 | gen.const("Medium", int_t, 1, "Medium is 1"), 15 | gen.const("High", int_t, 2, "Hight is 2")], 16 | "Select from the list") 17 | 18 | gen.add("size", int_t, 0, "Select from the list", 1, 0, 3, edit_method=size_enum) 19 | 20 | exit(gen.generate(PACKAGE, "chapter2_tutorials", "chapter2_")) 21 | -------------------------------------------------------------------------------- /chapter2_tutorials/launch/chapter2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /chapter2_tutorials/msg/chapter2_msg1.msg: -------------------------------------------------------------------------------- 1 | int32 A 2 | int32 B 3 | int32 C 4 | -------------------------------------------------------------------------------- /chapter2_tutorials/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | chapter2_tutorials 4 | 0.0.0 5 | The chapter2_tutorials package 6 | 7 | 8 | 9 | 10 | aaronmr 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | message_generation 36 | 37 | 38 | 39 | message_runtime 40 | 41 | 42 | catkin 43 | roscpp 44 | std_msgs 45 | dynamic_reconfigure 46 | 47 | roscpp 48 | std_msgs 49 | dynamic_reconfigure 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /chapter2_tutorials/src/example1_a.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "std_msgs/String.h" 3 | #include 4 | 5 | int main(int argc, char **argv) 6 | { 7 | ros::init(argc, argv, "example1_a"); 8 | ros::NodeHandle n; 9 | ros::Publisher pub = n.advertise("message", 1000); 10 | ros::Rate loop_rate(10); 11 | while (ros::ok()) 12 | { 13 | std_msgs::String msg; 14 | std::stringstream ss; 15 | ss << " I am the example1_a node "; 16 | msg.data = ss.str(); 17 | //ROS_INFO("%s", msg.data.c_str()); 18 | pub.publish(msg); 19 | ros::spinOnce(); 20 | loop_rate.sleep(); 21 | } 22 | return 0; 23 | } 24 | -------------------------------------------------------------------------------- /chapter2_tutorials/src/example1_b.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "std_msgs/String.h" 3 | 4 | void messageCallback(const std_msgs::String::ConstPtr& msg) 5 | { 6 | ROS_INFO("I heard: [%s]", msg->data.c_str()); 7 | } 8 | 9 | int main(int argc, char **argv) 10 | { 11 | ros::init(argc, argv, "example1_b"); 12 | ros::NodeHandle n; 13 | ros::Subscriber sub = n.subscribe("message", 1000, messageCallback); 14 | ros::spin(); 15 | return 0; 16 | } 17 | -------------------------------------------------------------------------------- /chapter2_tutorials/src/example2_a.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "chapter2_tutorials/chapter2_srv1.h" 3 | 4 | bool add(chapter2_tutorials::chapter2_srv1::Request &req, 5 | chapter2_tutorials::chapter2_srv1::Response &res) 6 | { 7 | res.sum = req.A + req.B + req.C; 8 | ROS_INFO("request: A=%d, B=%d C=%d", (int)req.A, (int)req.B, (int)req.C); 9 | ROS_INFO("sending back response: [%d]", (int)res.sum); 10 | return true; 11 | } 12 | 13 | int main(int argc, char **argv) 14 | { 15 | ros::init(argc, argv, "add_3_ints_server"); 16 | ros::NodeHandle n; 17 | 18 | ros::ServiceServer service = n.advertiseService("add_3_ints", add); 19 | ROS_INFO("Ready to add 3 ints."); 20 | ros::spin(); 21 | 22 | return 0; 23 | } 24 | -------------------------------------------------------------------------------- /chapter2_tutorials/src/example2_b.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "chapter2_tutorials/chapter2_srv1.h" 3 | #include 4 | 5 | int main(int argc, char **argv) 6 | { 7 | ros::init(argc, argv, "add_3_ints_client"); 8 | if (argc != 4) 9 | { 10 | ROS_INFO("usage: add_3_ints_client A B C "); 11 | return 1; 12 | } 13 | 14 | ros::NodeHandle n; 15 | ros::ServiceClient client = n.serviceClient("add_3_ints"); 16 | chapter2_tutorials::chapter2_srv1 srv; 17 | srv.request.A = atoll(argv[1]); 18 | srv.request.B = atoll(argv[2]); 19 | srv.request.C = atoll(argv[3]); 20 | if (client.call(srv)) 21 | { 22 | ROS_INFO("Sum: %ld", (long int)srv.response.sum); 23 | } 24 | else 25 | { 26 | ROS_ERROR("Failed to call service add_two_ints"); 27 | return 1; 28 | } 29 | 30 | return 0; 31 | } 32 | -------------------------------------------------------------------------------- /chapter2_tutorials/src/example3_a.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "chapter2_tutorials/chapter2_msg1.h" 3 | #include 4 | 5 | int main(int argc, char **argv) 6 | { 7 | ros::init(argc, argv, "example3_a"); 8 | ros::NodeHandle n; 9 | ros::Publisher pub = n.advertise("message", 1000); 10 | ros::Rate loop_rate(10); 11 | while (ros::ok()) 12 | { 13 | chapter2_tutorials::chapter2_msg1 msg; 14 | msg.A = 1; 15 | msg.B = 2; 16 | msg.C = 3; 17 | pub.publish(msg); 18 | ros::spinOnce(); 19 | loop_rate.sleep(); 20 | } 21 | return 0; 22 | } 23 | -------------------------------------------------------------------------------- /chapter2_tutorials/src/example3_b.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "chapter2_tutorials/chapter2_msg1.h" 3 | 4 | void messageCallback(const chapter2_tutorials::chapter2_msg1::ConstPtr& msg) 5 | { 6 | ROS_INFO("I heard: [%d] [%d] [%d]", msg->A, msg->B, msg->C); 7 | } 8 | 9 | int main(int argc, char **argv) 10 | { 11 | ros::init(argc, argv, "example3_b"); 12 | ros::NodeHandle n; 13 | ros::Subscriber sub = n.subscribe("message", 1000, messageCallback); 14 | ros::spin(); 15 | return 0; 16 | } 17 | -------------------------------------------------------------------------------- /chapter2_tutorials/src/example4.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | void callback(chapter2_tutorials::chapter2_Config &config, uint32_t level) { 7 | ROS_INFO("Reconfigure Request: %d %f %s %s %d", 8 | config.int_param, config.double_param, 9 | config.str_param.c_str(), 10 | config.bool_param?"True":"False", 11 | config.size); 12 | } 13 | 14 | int main(int argc, char **argv) { 15 | ros::init(argc, argv, "example4"); 16 | 17 | dynamic_reconfigure::Server server; 18 | dynamic_reconfigure::Server::CallbackType f; 19 | 20 | f = boost::bind(&callback, _1, _2); 21 | server.setCallback(f); 22 | 23 | ROS_INFO("Spinning node"); 24 | ros::spin(); 25 | return 0; 26 | } 27 | -------------------------------------------------------------------------------- /chapter2_tutorials/srv/chapter2_srv1.srv: -------------------------------------------------------------------------------- 1 | int32 A 2 | int32 B 3 | int32 C 4 | --- 5 | int32 sum 6 | 7 | -------------------------------------------------------------------------------- /chapter3_tutorials/bag/2014-07-01-22-54-34.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AaronMR/Learning_ROS_for_Robotics_Programming_2nd_edition/afc45a0a207f36ced489876abce4414afa2f11ca/chapter3_tutorials/bag/2014-07-01-22-54-34.bag -------------------------------------------------------------------------------- /chapter3_tutorials/cfg/DynamicParam.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | PACKAGE='chapter3_tutorials' 4 | 5 | from math import pi 6 | from dynamic_reconfigure.parameter_generator_catkin import * 7 | from driver_base.msg import SensorLevels 8 | 9 | gen = ParameterGenerator() 10 | 11 | gen.add('bool_param', bool_t, SensorLevels.RECONFIGURE_RUNNING, 12 | 'Bool param', True) 13 | gen.add('int_param', int_t, SensorLevels.RECONFIGURE_STOP, 14 | 'Int param', 0, -10, 10) 15 | gen.add('double_param', double_t, SensorLevels.RECONFIGURE_CLOSE, 16 | 'Double param', 0.0, -pi, pi) 17 | 18 | foo = gen.const('foo', str_t, 'Foo', 'FOO') 19 | bar = gen.const('bar', str_t, 'Bar', 'BAR') 20 | baz = gen.const('baz', str_t, 'Baz', 'BAZ') 21 | strings = gen.enum([foo, bar, baz], 'Strings') 22 | gen.add('string_param', str_t, SensorLevels.RECONFIGURE_RUNNING, 23 | 'String param', 'Foo', edit_method = strings) 24 | 25 | exit(gen.generate(PACKAGE, PACKAGE, 'DynamicParam')) 26 | -------------------------------------------------------------------------------- /chapter3_tutorials/config/chapter3_tutorials.config: -------------------------------------------------------------------------------- 1 | # Override chapter3_tutorials to output everything 2 | log4j.logger.ros.chapter3_tutorials=DEBUG 3 | 4 | -------------------------------------------------------------------------------- /chapter3_tutorials/config/diagnostic_aggregator.yaml: -------------------------------------------------------------------------------- 1 | type: AnalyzerGroup 2 | path: Sensors 3 | analyzers: 4 | status: 5 | type: GenericAnalyzer 6 | path: Status 7 | startswith: example7 8 | num_items: 1 9 | -------------------------------------------------------------------------------- /chapter3_tutorials/launch/example1.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /chapter3_tutorials/launch/example10.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /chapter3_tutorials/launch/example1_gdb.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /chapter3_tutorials/launch/example1_valgrind.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /chapter3_tutorials/launch/example2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /chapter3_tutorials/launch/example3.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /chapter3_tutorials/launch/example4_5.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /chapter3_tutorials/launch/example4_record.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /chapter3_tutorials/launch/example6.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /chapter3_tutorials/launch/example7.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /chapter3_tutorials/launch/example8.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /chapter3_tutorials/launch/example9.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /chapter3_tutorials/output/gdb_run_node_example1.txt: -------------------------------------------------------------------------------- 1 | (gdb) r 2 | Starting program: /home/enrique/dev/rosbook/chapter3_tutorials/bin/example1 3 | [Thread debugging using libthread_db enabled] 4 | Using host libthread_db library "/lib/x86_64-linux-gnu/libthread_db.so.1". 5 | [New Thread 0x7ffff2664700 (LWP 3204)] 6 | [New Thread 0x7ffff1e63700 (LWP 3205)] 7 | [New Thread 0x7ffff1662700 (LWP 3206)] 8 | [New Thread 0x7ffff0e61700 (LWP 3211)] 9 | [DEBUG] [1356342615.325647326]: This is a simple DEBUG message! 10 | [DEBUG] [1356342615.326124607]: This is a DEBUG message with an argument: 3.140000 11 | [DEBUG] [1356342615.326254667]: This is DEBUG stream message with an argument: 3.14 12 | [Thread 0x7ffff0e61700 (LWP 3211) exited] 13 | [Thread 0x7ffff1662700 (LWP 3206) exited] 14 | [Thread 0x7ffff2664700 (LWP 3204) exited] 15 | [Thread 0x7ffff1e63700 (LWP 3205) exited] 16 | [Inferior 1 (process 3200) exited normally] 17 | 18 | -------------------------------------------------------------------------------- /chapter3_tutorials/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | chapter3_tutorials 4 | 0.1.0 5 | Chapter 3 tutorials. 6 | 7 | Enrique Fernández 8 | Enrique Fernández 9 | 10 | BSD 11 | 12 | catkin 13 | 14 | roscpp 15 | message_generation 16 | std_msgs 17 | geometry_msgs 18 | sensor_msgs 19 | visualization_msgs 20 | dynamic_reconfigure 21 | driver_base 22 | diagnostic_updater 23 | cv_bridge 24 | image_transport 25 | libpcl-all-dev 26 | pcl_conversions 27 | interactive_markers 28 | 29 | roscpp 30 | message_runtime 31 | std_msgs 32 | geometry_msgs 33 | sensor_msgs 34 | visualization_msgs 35 | dynamic_reconfigure 36 | driver_base 37 | diagnostic_updater 38 | cv_bridge 39 | image_transport 40 | libpcl-all 41 | pcl_conversions 42 | interactive_markers 43 | 44 | -------------------------------------------------------------------------------- /chapter3_tutorials/src/example1.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | 5 | #define OVERRIDE_NODE_VERBOSITY_LEVEL 1 6 | 7 | int main( int argc, char **argv ) 8 | { 9 | 10 | ros::init( argc, argv, "example1" ); 11 | 12 | #if OVERRIDE_NODE_VERBOSITY_LEVEL 13 | // Se the logging level manually to DEBUG 14 | ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug); 15 | #endif 16 | 17 | ros::NodeHandle n; 18 | 19 | const double val = 3.14; 20 | 21 | ROS_DEBUG( "This is a simple DEBUG message!" ); 22 | 23 | ROS_DEBUG( "This is a DEBUG message with an argument: %f", val ); 24 | 25 | ROS_DEBUG_STREAM( 26 | "This is DEBUG stream message with an argument: " << val 27 | ); 28 | 29 | ros::spinOnce(); 30 | 31 | return EXIT_SUCCESS; 32 | 33 | } 34 | 35 | -------------------------------------------------------------------------------- /chapter3_tutorials/src/example3.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | 5 | int main( int argc, char **argv ) 6 | { 7 | 8 | ros::init( argc, argv, "example3" ); 9 | 10 | ros::NodeHandle n; 11 | 12 | ros::Rate rate( 1 ); 13 | while( ros::ok() ) { 14 | 15 | ROS_DEBUG_STREAM( "DEBUG message." ); 16 | ROS_INFO_STREAM ( "INFO message." ); 17 | ROS_WARN_STREAM ( "WARN message." ); 18 | ROS_ERROR_STREAM( "ERROR message." ); 19 | ROS_FATAL_STREAM( "FATAL message." ); 20 | 21 | ROS_INFO_STREAM_NAMED( "named_msg", "INFO named message." ); 22 | 23 | ROS_INFO_STREAM_THROTTLE( 2, "INFO throttle message." ); 24 | 25 | ros::spinOnce(); 26 | rate.sleep(); 27 | } 28 | 29 | return EXIT_SUCCESS; 30 | 31 | } 32 | 33 | -------------------------------------------------------------------------------- /chapter3_tutorials/src/example4.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | int main( int argc, char **argv ) 11 | { 12 | 13 | ros::init( argc, argv, "example4" ); 14 | 15 | ros::NodeHandle n; 16 | 17 | ros::Publisher pub_temp = n.advertise< std_msgs::Int32 >( "temp", 1000 ); 18 | ros::Publisher pub_accel = n.advertise< geometry_msgs::Vector3 >( "accel", 1000 ); 19 | 20 | ros::ServiceClient srv_speed = n.serviceClient< chapter3_tutorials::SetSpeed>( "speed" ); 21 | 22 | std_msgs::Int32 msg_temp; 23 | geometry_msgs::Vector3 msg_accel; 24 | chapter3_tutorials::SetSpeed msg_speed; 25 | 26 | int i = 0; 27 | 28 | ros::Rate rate( 1 ); 29 | while( ros::ok() ) { 30 | 31 | msg_temp.data = i; 32 | 33 | msg_accel.x = 0.1 * i; 34 | msg_accel.y = 0.2 * i; 35 | msg_accel.z = 0.3 * i; 36 | 37 | msg_speed.request.desired_speed = 0.01 * i; 38 | 39 | pub_temp.publish( msg_temp ); 40 | pub_accel.publish( msg_accel ); 41 | 42 | if( srv_speed.call( msg_speed ) ) 43 | { 44 | ROS_INFO_STREAM( 45 | "SetSpeed response:\n" << 46 | "previous speed = " << msg_speed.response.previous_speed << "\n" << 47 | "new speed = " << msg_speed.response.new_speed << "\n" << 48 | "motor stalled = " << ( msg_speed.response.stalled ? "true" : "false" ) 49 | ); 50 | } 51 | else 52 | { 53 | // Note that this might happen at the beginning, because 54 | // the service server could have not started yet! 55 | ROS_ERROR_STREAM( "Call to speed service failed!" ); 56 | } 57 | 58 | ++i; 59 | 60 | ros::spinOnce(); 61 | rate.sleep(); 62 | } 63 | 64 | return EXIT_SUCCESS; 65 | 66 | } 67 | 68 | -------------------------------------------------------------------------------- /chapter3_tutorials/src/example5.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | float previous_speed = 0.; 11 | float new_speed = 0.; 12 | 13 | void callback_temp( const std_msgs::Int32::ConstPtr& msg ) 14 | { 15 | ROS_INFO_STREAM( "Temp = " << msg->data ); 16 | } 17 | 18 | void callback_accel( const geometry_msgs::Vector3::ConstPtr& msg ) 19 | { 20 | ROS_INFO_STREAM( 21 | "Accel = (" << msg->x << ", " << msg->y << ", " << msg->z << ")" 22 | ); 23 | } 24 | 25 | bool callback_speed( 26 | chapter3_tutorials::SetSpeed::Request &req, 27 | chapter3_tutorials::SetSpeed::Response &res 28 | ) 29 | { 30 | ROS_INFO_STREAM( 31 | "speed service request: desired speed = " << req.desired_speed 32 | ); 33 | 34 | new_speed = 0.9 * req.desired_speed; 35 | 36 | res.previous_speed = previous_speed; 37 | res.new_speed = new_speed; 38 | res.stalled = new_speed < 0.1; 39 | 40 | previous_speed = new_speed; 41 | 42 | return true; 43 | } 44 | 45 | 46 | int main( int argc, char **argv ) 47 | { 48 | 49 | ros::init( argc, argv, "example5" ); 50 | 51 | ros::NodeHandle n; 52 | 53 | ros::Subscriber sub_temp = n.subscribe( "temp", 1000, callback_temp ); 54 | ros::Subscriber sub_accel = n.subscribe( "accel", 1000, callback_accel ); 55 | 56 | ros::ServiceServer srv_speed = n.advertiseService( "speed", callback_speed ); 57 | 58 | while( ros::ok() ) { 59 | ros::spin(); 60 | } 61 | 62 | return EXIT_SUCCESS; 63 | 64 | } 65 | 66 | -------------------------------------------------------------------------------- /chapter3_tutorials/src/example6.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | 5 | #include 6 | 7 | class DynamicParamServer 8 | { 9 | public: 10 | DynamicParamServer() 11 | { 12 | _cfg_server.setCallback(boost::bind(&DynamicParamServer::callback, this, _1, _2)); 13 | } 14 | 15 | void callback(chapter3_tutorials::DynamicParamConfig& config, uint32_t level) 16 | { 17 | ROS_INFO_STREAM( 18 | "New configuration received with level = " << level << ":\n" << 19 | "bool = " << config.bool_param << "\n" << 20 | "int = " << config.int_param << "\n" << 21 | "double = " << config.double_param << "\n" << 22 | "string = " << config.string_param 23 | ); 24 | } 25 | 26 | private: 27 | dynamic_reconfigure::Server _cfg_server; 28 | }; 29 | 30 | int main(int argc, char** argv) 31 | { 32 | ros::init(argc, argv, "example6"); 33 | 34 | DynamicParamServer dps; 35 | 36 | while(ros::ok()) 37 | { 38 | ros::spin(); 39 | } 40 | 41 | return EXIT_SUCCESS; 42 | } 43 | -------------------------------------------------------------------------------- /chapter3_tutorials/src/example7.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | 4 | #include 5 | 6 | class DiagnosticNode 7 | { 8 | public: 9 | DiagnosticNode() 10 | : _iterator(0) 11 | { 12 | _diagnostic.add("status", this, &DiagnosticNode::diagnostics); 13 | _diagnostic.setHardwareID("none"); 14 | } 15 | 16 | void update() 17 | { 18 | _diagnostic.update(); 19 | } 20 | 21 | void diagnostics(diagnostic_updater::DiagnosticStatusWrapper& status) 22 | { 23 | if (_iterator % 2 == 0) 24 | status.summary(0, "ok"); 25 | else 26 | status.summary(2, "error"); 27 | 28 | status.add("iterator i", _iterator); 29 | 30 | ROS_DEBUG_THROTTLE(1, "Diagnostics published"); 31 | 32 | ++_iterator; 33 | } 34 | 35 | private: 36 | diagnostic_updater::Updater _diagnostic; 37 | 38 | int _iterator; 39 | }; 40 | 41 | int main(int argc, char** argv) 42 | { 43 | ros::init(argc, argv, "example7"); 44 | 45 | DiagnosticNode dn; 46 | 47 | ros::Rate rate(10.0); 48 | while(ros::ok()) 49 | { 50 | dn.update(); 51 | 52 | ros::spinOnce(); 53 | rate.sleep(); 54 | } 55 | 56 | return EXIT_SUCCESS; 57 | } 58 | -------------------------------------------------------------------------------- /chapter3_tutorials/src/example8.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | int main( int argc, char **argv ) 11 | { 12 | ros::init( argc, argv, "example8" ); 13 | 14 | ros::NodeHandle n; 15 | 16 | // Open camera with CAMERA_INDEX (webcam is typically #0). 17 | const int CAMERA_INDEX = 0; 18 | cv::VideoCapture capture( CAMERA_INDEX ); 19 | if( not capture.isOpened() ) 20 | { 21 | ROS_ERROR_STREAM( 22 | "Failed to open camera with index " << CAMERA_INDEX << "!" 23 | ); 24 | ros::shutdown(); 25 | } 26 | 27 | image_transport::ImageTransport it( n ); 28 | image_transport::Publisher pub_image = it.advertise( "camera", 1 ); 29 | 30 | cv_bridge::CvImagePtr frame = boost::make_shared< cv_bridge::CvImage >(); 31 | frame->encoding = sensor_msgs::image_encodings::BGR8; 32 | 33 | while( ros::ok() ) { 34 | capture >> frame->image; 35 | 36 | if( frame->image.empty() ) 37 | { 38 | ROS_ERROR_STREAM( "Failed to capture frame!" ); 39 | ros::shutdown(); 40 | } 41 | 42 | frame->header.stamp = ros::Time::now(); 43 | pub_image.publish( frame->toImageMsg() ); 44 | 45 | cv::waitKey( 3 ); 46 | 47 | ros::spinOnce(); 48 | } 49 | 50 | capture.release(); 51 | 52 | return EXIT_SUCCESS; 53 | } 54 | 55 | -------------------------------------------------------------------------------- /chapter3_tutorials/srv/SetSpeed.srv: -------------------------------------------------------------------------------- 1 | float32 desired_speed 2 | --- 3 | float32 previous_speed 4 | float32 new_speed 5 | bool stalled 6 | -------------------------------------------------------------------------------- /chapter4_tutorials/launch/example1.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /chapter4_tutorials/launch/example2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /chapter4_tutorials/launch/example6.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /chapter4_tutorials/src/c4_example1.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | using namespace std; 7 | 8 | class TeleopJoy{ 9 | public: 10 | TeleopJoy(); 11 | private: 12 | void callBack(const sensor_msgs::Joy::ConstPtr& joy); 13 | ros::NodeHandle n; 14 | ros::Publisher pub; 15 | ros::Subscriber sub; 16 | int i_velLinear, i_velAngular; 17 | }; 18 | 19 | TeleopJoy::TeleopJoy() 20 | { 21 | n.param("axis_linear",i_velLinear,i_velLinear); 22 | n.param("axis_angular",i_velAngular,i_velAngular); 23 | pub = n.advertise("/turtle1/cmd_vel",1); 24 | sub = n.subscribe("joy", 10, &TeleopJoy::callBack, this); 25 | } 26 | 27 | void TeleopJoy::callBack(const sensor_msgs::Joy::ConstPtr& joy) 28 | { 29 | geometry_msgs::Twist vel; 30 | vel.angular.z = joy->axes[i_velAngular]; 31 | vel.linear.x = joy->axes[i_velLinear]; 32 | pub.publish(vel); 33 | } 34 | 35 | int main(int argc, char** argv) 36 | { 37 | ros::init(argc, argv, "teleopJoy"); 38 | TeleopJoy teleop_turtle; 39 | ros::spin(); 40 | } 41 | -------------------------------------------------------------------------------- /chapter4_tutorials/src/c4_example2.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "std_msgs/String.h" 3 | #include 4 | #include 5 | 6 | using namespace std; 7 | class Scan2{ 8 | public: 9 | Scan2(); 10 | private: 11 | ros::NodeHandle n; 12 | ros::Publisher scan_pub; 13 | ros::Subscriber scan_sub; 14 | void scanCallBack(const sensor_msgs::LaserScan::ConstPtr& scan2); 15 | }; 16 | 17 | Scan2::Scan2() 18 | { 19 | scan_pub = n.advertise("/scan2",1); 20 | scan_sub = n.subscribe("/scan",1, &Scan2::scanCallBack, this); 21 | } 22 | 23 | void Scan2::scanCallBack(const sensor_msgs::LaserScan::ConstPtr& scan2) 24 | { 25 | int ranges = scan2->ranges.size(); 26 | //populate the LaserScan message 27 | sensor_msgs::LaserScan scan; 28 | scan.header.stamp = scan2->header.stamp; 29 | scan.header.frame_id = scan2->header.frame_id; 30 | scan.angle_min = scan2->angle_min; 31 | scan.angle_max = scan2->angle_max; 32 | scan.angle_increment = scan2->angle_increment; 33 | scan.time_increment = scan2->time_increment; 34 | scan.range_min = 0.0; 35 | scan.range_max = 100.0; 36 | scan.ranges.resize(ranges); 37 | for(int i = 0; i < ranges; ++i) 38 | { 39 | scan.ranges[i] = scan2->ranges[i] + 1; 40 | } 41 | scan_pub.publish(scan); 42 | } 43 | 44 | int main(int argc, char** argv) 45 | { 46 | ros::init(argc, argv, "example2_laser_scan_publisher"); 47 | Scan2 scan2; 48 | ros::spin(); 49 | } 50 | -------------------------------------------------------------------------------- /chapter4_tutorials/src/c4_example3.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | // PCL specific includes 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | ros::Publisher pub; 11 | void cloud_cb (const pcl::PCLPointCloud2ConstPtr& input) 12 | { 13 | pcl::PCLPointCloud2 cloud_filtered; 14 | pcl::VoxelGrid sor; 15 | sor.setInputCloud (input); 16 | sor.setLeafSize (0.01, 0.01, 0.01); 17 | sor.filter (cloud_filtered); 18 | 19 | // Publish the dataSize 20 | pub.publish (cloud_filtered); 21 | } 22 | 23 | int main (int argc, char** argv) 24 | { 25 | // Initialize ROS 26 | ros::init (argc, argv, "my_pcl_tutorial"); 27 | ros::NodeHandle nh; 28 | // Create a ROS subscriber for the input point cloud 29 | ros::Subscriber sub = nh.subscribe ("/camera/depth/points", 1, cloud_cb); 30 | // Create a ROS publisher for the output point cloud 31 | pub = nh.advertise ("output", 1); 32 | // Spin 33 | ros::spin (); 34 | } 35 | -------------------------------------------------------------------------------- /chapter4_tutorials/src/c4_example4.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | using namespace std; 6 | 7 | class Dynamixel{ 8 | private: 9 | ros::NodeHandle n; 10 | ros::Publisher pub_n; 11 | public: 12 | Dynamixel(); 13 | int moveMotor(double position); 14 | }; 15 | 16 | Dynamixel::Dynamixel(){ 17 | pub_n = n.advertise("/tilt_controller/command",1); 18 | } 19 | int Dynamixel::moveMotor(double position) 20 | { 21 | std_msgs::Float64 aux; 22 | aux.data = position; 23 | pub_n.publish(aux); 24 | return 1; 25 | } 26 | 27 | int main(int argc,char** argv) 28 | { 29 | ros::init(argc, argv, "example4_move_motor"); 30 | Dynamixel motors; 31 | 32 | float counter = -180; 33 | ros::Rate loop_rate(100); 34 | while(ros::ok()) 35 | { 36 | if(counter < 180) 37 | { 38 | motors.moveMotor(counter*3.14/180); 39 | counter++; 40 | }else{ 41 | counter = -180; 42 | } 43 | loop_rate.sleep(); 44 | } 45 | } 46 | -------------------------------------------------------------------------------- /chapter4_tutorials/src/c4_example5_1.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | ros::NodeHandle nh; 5 | 6 | std_msgs::String str_msg; 7 | ros::Publisher chatter("chatter", &str_msg); 8 | 9 | char hello[19] = "chapter4_tutorials"; 10 | 11 | void setup() 12 | { 13 | nh.initNode(); 14 | nh.advertise(chatter); 15 | } 16 | 17 | void loop() 18 | { 19 | str_msg.data = hello; 20 | chatter.publish( &str_msg ); 21 | nh.spinOnce(); 22 | delay(1000); 23 | } 24 | -------------------------------------------------------------------------------- /chapter4_tutorials/src/c4_example5_2.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | using namespace std; 7 | ros::Publisher pub; 8 | ros::Subscriber sub; 9 | 10 | void rangeCallBack(const std_msgs::Int16 &range) 11 | { 12 | geometry_msgs::Twist vel; 13 | if (range.data > 40) 14 | { 15 | vel.angular.z = 0; 16 | vel.linear.x = 1; 17 | } 18 | else if (range.data >20) 19 | { 20 | vel.angular.z = 1; 21 | vel.linear.x = 0; 22 | 23 | } 24 | else 25 | { 26 | vel.angular.z = 0; 27 | vel.linear.x = -1; 28 | 29 | } 30 | pub.publish(vel); 31 | } 32 | 33 | 34 | int main(int argc, char** argv) 35 | { 36 | ros::init(argc, argv, "turtle_ultra_sound"); 37 | ros::NodeHandle n; 38 | pub = n.advertise("/turtle1/cmd_vel",1); 39 | sub = n.subscribe("range", 10, &rangeCallBack); 40 | ros::spin(); 41 | } 42 | -------------------------------------------------------------------------------- /chapter4_tutorials/src/c4_example5_2.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | ros::NodeHandle nh; 5 | 6 | std_msgs::Int16 range; 7 | ros::Publisher range_pub("range", &range); 8 | 9 | const int trigpin = 7; 10 | const int echopin = 3; 11 | long duration = 0; 12 | 13 | void setup() 14 | { 15 | nh.initNode(); 16 | nh.advertise(range_pub); 17 | 18 | } 19 | 20 | void loop() 21 | { 22 | range.data = ping(); 23 | range_pub.publish(&range); 24 | nh.spinOnce(); 25 | delay(100); 26 | } 27 | 28 | long ping() 29 | { 30 | // Send out PING))) signal pulse 31 | pinMode(trigpin, OUTPUT); 32 | pinMode(echopin,INPUT); 33 | digitalWrite(trigpin, LOW); 34 | delayMicroseconds(2); 35 | digitalWrite(trigpin, HIGH); 36 | delayMicroseconds(10); 37 | digitalWrite(trigpin, LOW); 38 | 39 | //Get duration it takes to receive echo 40 | duration = pulseIn(echopin, HIGH); 41 | //Convert duration into distance 42 | return duration /290/2; 43 | } 44 | -------------------------------------------------------------------------------- /chapter4_tutorials/src/c4_example6.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | using namespace std; 9 | 10 | class TeleopImu{ 11 | public: 12 | TeleopImu(); 13 | private: 14 | void callBack(const sensor_msgs::Imu::ConstPtr& imu); 15 | ros::NodeHandle n; 16 | ros::Publisher pub; 17 | ros::Subscriber sub; 18 | }; 19 | 20 | TeleopImu::TeleopImu() 21 | { 22 | pub = n.advertise("/turtle1/cmd_vel",1); 23 | sub = n.subscribe("imu/data", 10, &TeleopImu::callBack, this); 24 | } 25 | 26 | void TeleopImu::callBack(const sensor_msgs::Imu::ConstPtr& imu) 27 | { 28 | geometry_msgs::Twist vel; 29 | tf::Quaternion bq(imu->orientation.x,imu->orientation.y,imu->orientation.z,imu->orientation.w); 30 | double roll,pitch,yaw; 31 | tf::Matrix3x3(bq).getRPY(roll,pitch,yaw); 32 | vel.angular.z = roll; 33 | vel.linear.x = pitch; 34 | pub.publish(vel); 35 | } 36 | 37 | int main(int argc, char** argv) 38 | { 39 | ros::init(argc, argv, "teleopImu"); 40 | TeleopImu teleop_turtle; 41 | ros::spin(); 42 | } 43 | -------------------------------------------------------------------------------- /chapter4_tutorials/src/c4_example7.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | class TeleopImu{ 6 | public: 7 | TeleopImu(); 8 | private: 9 | void velLinearCallBack(const std_msgs::Float32::ConstPtr& vx); 10 | void velAngularCallBack(const std_msgs::Float32::ConstPtr& wz); 11 | ros::NodeHandle n; 12 | ros::Publisher pub; 13 | ros::Subscriber velAngular_z_sub; 14 | ros::Subscriber velLinear_x_sub; 15 | geometry_msgs::Twist vel; 16 | }; 17 | 18 | TeleopImu::TeleopImu() 19 | { 20 | pub = n.advertise("turtle1/cmd_vel",1); 21 | 22 | velLinear_x_sub = n.subscribe("/velLinear_x", 1, &TeleopImu::velLinearCallBack, this); 23 | velAngular_z_sub = n.subscribe("/velAngular_z", 1, &TeleopImu::velAngularCallBack, this); 24 | } 25 | void TeleopImu::velAngularCallBack(const std_msgs::Float32::ConstPtr& wz){ 26 | vel.linear.x = -1 * wz->data; 27 | pub.publish(vel); 28 | } 29 | 30 | void TeleopImu::velLinearCallBack(const std_msgs::Float32::ConstPtr& vx){ 31 | vel.angular.z = vx->data; 32 | pub.publish(vel); 33 | } 34 | 35 | int main(int argc, char** argv) 36 | { 37 | ros::init(argc, argv, "example8"); 38 | TeleopImu teleop_turtle; 39 | ros::spin(); 40 | } 41 | -------------------------------------------------------------------------------- /chapter4_tutorials/src/c4_example9.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | geometry_msgs::Point global_position; 9 | 10 | ros::Publisher position_pub; 11 | void gpsCallBack(const sensor_msgs::NavSatFixConstPtr& gps) 12 | { 13 | double northing, easting; 14 | char zone; 15 | //LLtoUTM(gps->latitude, gps->longitude, northing, easting , &zone); 16 | global_position.x = easting; 17 | global_position.y = northing; 18 | global_position.z = gps->altitude; 19 | } 20 | 21 | int main(int argc, char** argv){ 22 | ros::init(argc,argv, "Geoposition"); 23 | ros::NodeHandle n; 24 | ros::Subscriber gps_sub = n.subscribe("fix",10, gpsCallBack); 25 | position_pub = n.advertise ("global_position", 1); 26 | ros::Rate loop_rate(10); 27 | while(n.ok()) 28 | { 29 | ros::spinOnce(); 30 | loop_rate.sleep(); 31 | } 32 | } 33 | -------------------------------------------------------------------------------- /chapter5_tutorials/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(chapter5_tutorials) 3 | 4 | find_package(catkin REQUIRED 5 | COMPONENTS 6 | roscpp 7 | message_generation 8 | sensor_msgs 9 | dynamic_reconfigure 10 | driver_base 11 | cv_bridge 12 | image_transport 13 | camera_info_manager 14 | tf) 15 | 16 | find_package(OpenCV REQUIRED) 17 | 18 | generate_dynamic_reconfigure_options( 19 | cfg/Camera.cfg 20 | cfg/CameraStereo.cfg 21 | cfg/Homography.cfg) 22 | 23 | catkin_package( 24 | CATKIN_DEPENDS 25 | roscpp 26 | message_runtime 27 | sensor_msgs 28 | dynamic_reconfigure 29 | driver_base 30 | cv_bridge 31 | image_transport 32 | camera_info_manager 33 | tf) 34 | 35 | include_directories(${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}) 36 | 37 | add_executable(camera src/camera.cpp) 38 | add_dependencies(camera ${PROJECT_NAME}_gencfg) 39 | target_link_libraries(camera ${catkin_LIBRARIES}) 40 | 41 | add_executable(camera_timer src/camera_timer.cpp) 42 | target_link_libraries(camera_timer ${catkin_LIBRARIES}) 43 | 44 | add_executable(camera_polling src/camera_polling.cpp) 45 | target_link_libraries(camera_polling ${catkin_LIBRARIES}) 46 | 47 | add_executable(camera_stereo src/camera_stereo.cpp) 48 | add_dependencies(camera_stereo ${PROJECT_NAME}_gencfg) 49 | target_link_libraries(camera_stereo ${catkin_LIBRARIES}) 50 | 51 | add_executable(homography src/homography.cpp) 52 | add_dependencies(homography ${PROJECT_NAME}_gencfg) 53 | target_link_libraries(homography ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) 54 | 55 | install(TARGETS camera camera_timer camera_polling camera_stereo homography 56 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 57 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 58 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 59 | ) 60 | 61 | foreach(dir bag config launch) 62 | install(DIRECTORY ${dir}/ 63 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) 64 | endforeach(dir) 65 | -------------------------------------------------------------------------------- /chapter5_tutorials/bag/viso2_demo/download_amphoras_pool_bag_files.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | wget --mirror ftp://opt.uib.es/bagfiles/viso2_ros 4 | 5 | -------------------------------------------------------------------------------- /chapter5_tutorials/calibration/camera/calibrationdata.tar.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AaronMR/Learning_ROS_for_Robotics_Programming_2nd_edition/afc45a0a207f36ced489876abce4414afa2f11ca/chapter5_tutorials/calibration/camera/calibrationdata.tar.gz -------------------------------------------------------------------------------- /chapter5_tutorials/calibration/camera_stereo/calibrationdata.tar.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AaronMR/Learning_ROS_for_Robotics_Programming_2nd_edition/afc45a0a207f36ced489876abce4414afa2f11ca/chapter5_tutorials/calibration/camera_stereo/calibrationdata.tar.gz -------------------------------------------------------------------------------- /chapter5_tutorials/calibration/firewire_camera/calibrationdata.tar.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AaronMR/Learning_ROS_for_Robotics_Programming_2nd_edition/afc45a0a207f36ced489876abce4414afa2f11ca/chapter5_tutorials/calibration/firewire_camera/calibrationdata.tar.gz -------------------------------------------------------------------------------- /chapter5_tutorials/calibration/gscam/calibrationdata.tar.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AaronMR/Learning_ROS_for_Robotics_Programming_2nd_edition/afc45a0a207f36ced489876abce4414afa2f11ca/chapter5_tutorials/calibration/gscam/calibrationdata.tar.gz -------------------------------------------------------------------------------- /chapter5_tutorials/calibration/pattern/calibration_pattern_chessboard.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AaronMR/Learning_ROS_for_Robotics_Programming_2nd_edition/afc45a0a207f36ced489876abce4414afa2f11ca/chapter5_tutorials/calibration/pattern/calibration_pattern_chessboard.pdf -------------------------------------------------------------------------------- /chapter5_tutorials/cfg/.gitignore: -------------------------------------------------------------------------------- 1 | cpp 2 | -------------------------------------------------------------------------------- /chapter5_tutorials/cfg/Camera.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | PACKAGE='chapter5_tutorials' 4 | 5 | from dynamic_reconfigure.parameter_generator_catkin import * 6 | from driver_base.msg import SensorLevels 7 | 8 | gen = ParameterGenerator() 9 | 10 | gen.add("camera_index", int_t, SensorLevels.RECONFIGURE_CLOSE, 11 | "Camera device index, e.g. 0 for /dev/video0", 0, 0, 10) 12 | 13 | gen.add("frame_width", int_t, SensorLevels.RECONFIGURE_CLOSE, 14 | "Frame width", 640, 1, 2000) 15 | 16 | gen.add("frame_height", int_t, SensorLevels.RECONFIGURE_CLOSE, 17 | "Frame height", 480, 1, 2000) 18 | 19 | gen.add("fps", double_t, SensorLevels.RECONFIGURE_RUNNING, 20 | "Frames Per Second (FPS)", 30., 0., 120.) 21 | 22 | gen.add("fourcc", str_t, SensorLevels.RECONFIGURE_CLOSE, 23 | "FourCC code", "YUYV") 24 | 25 | gen.add("brightness", double_t, SensorLevels.RECONFIGURE_RUNNING, 26 | "Brightness", 0.5, 0., 1.) 27 | 28 | gen.add("contrast", double_t, SensorLevels.RECONFIGURE_RUNNING, 29 | "Contrast", 1., 0., 1.) 30 | 31 | gen.add("saturation", double_t, SensorLevels.RECONFIGURE_RUNNING, 32 | "Saturation", 1., 0., 1.) 33 | 34 | gen.add("hue", double_t, SensorLevels.RECONFIGURE_RUNNING, 35 | "Hue", 0.5, 0., 1.) 36 | 37 | gen.add("gain", double_t, SensorLevels.RECONFIGURE_RUNNING, 38 | "Gain", 0., 0., 1.) 39 | 40 | gen.add("exposure", double_t, SensorLevels.RECONFIGURE_RUNNING, 41 | "Exposure", 0.5, 0., 1.) 42 | 43 | gen.add("frame_id", str_t, SensorLevels.RECONFIGURE_CLOSE, 44 | "ROS tf frame of reference, resolved with tf_prefix unless absolute.", 45 | "camera") 46 | 47 | gen.add("camera_info_url", str_t, SensorLevels.RECONFIGURE_RUNNING, 48 | "Camera [[camera_info_manager#URL_Names|calibration URL]] for this video_mode (uncalibrated if null).", "") 49 | 50 | exit(gen.generate(PACKAGE, "Camera", "Camera")) 51 | 52 | -------------------------------------------------------------------------------- /chapter5_tutorials/cfg/CameraStereo.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | PACKAGE='chapter5_tutorials' 4 | 5 | from dynamic_reconfigure.parameter_generator_catkin import * 6 | from driver_base.msg import SensorLevels 7 | 8 | gen = ParameterGenerator() 9 | 10 | gen.add("camera_index_left", int_t, SensorLevels.RECONFIGURE_CLOSE, 11 | "Left camera device index, e.g. 0 for /dev/video0", 1, 0, 10) 12 | 13 | gen.add("camera_index_right", int_t, SensorLevels.RECONFIGURE_CLOSE, 14 | "Right camera device index, e.g. 1 for /dev/video1", 2, 0, 10) 15 | 16 | gen.add("frame_width", int_t, SensorLevels.RECONFIGURE_CLOSE, 17 | "Frame width", 640, 1, 2000) 18 | 19 | gen.add("frame_height", int_t, SensorLevels.RECONFIGURE_CLOSE, 20 | "Frame height", 480, 1, 2000) 21 | 22 | gen.add("fps", double_t, SensorLevels.RECONFIGURE_RUNNING, 23 | "Frames Per Second (FPS)", 30., 0., 120.) 24 | 25 | gen.add("fourcc", str_t, SensorLevels.RECONFIGURE_CLOSE, 26 | "FourCC code", "YUYV") 27 | 28 | gen.add("brightness", double_t, SensorLevels.RECONFIGURE_RUNNING, 29 | "Brightness", 0.5, 0., 1.) 30 | 31 | gen.add("contrast", double_t, SensorLevels.RECONFIGURE_RUNNING, 32 | "Contrast", 1., 0., 1.) 33 | 34 | gen.add("saturation", double_t, SensorLevels.RECONFIGURE_RUNNING, 35 | "Saturation", 1., 0., 1.) 36 | 37 | gen.add("hue", double_t, SensorLevels.RECONFIGURE_RUNNING, 38 | "Hue", 0.5, 0., 1.) 39 | 40 | gen.add("gain", double_t, SensorLevels.RECONFIGURE_RUNNING, 41 | "Gain", 0., 0., 1.) 42 | 43 | gen.add("exposure", double_t, SensorLevels.RECONFIGURE_RUNNING, 44 | "Exposure", 0.5, 0., 1.) 45 | 46 | gen.add("frame_id", str_t, SensorLevels.RECONFIGURE_CLOSE, 47 | "ROS tf frame of reference, resolved with tf_prefix unless absolute.", 48 | "camera") 49 | 50 | gen.add("camera_info_url_left", str_t, SensorLevels.RECONFIGURE_RUNNING, 51 | "Left camera [[camera_info_manager#URL_Names|calibration URL]] for this video_mode (uncalibrated if null).", "") 52 | 53 | gen.add("camera_info_url_right", str_t, SensorLevels.RECONFIGURE_RUNNING, 54 | "Right camera [[camera_info_manager#URL_Names|calibration URL]] for this video_mode (uncalibrated if null).", "") 55 | 56 | exit(gen.generate(PACKAGE, "CameraStereo", "CameraStereo")) 57 | 58 | -------------------------------------------------------------------------------- /chapter5_tutorials/cfg/Homography.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | PACKAGE='chapter5_tutorials' 4 | 5 | from dynamic_reconfigure.parameter_generator_catkin import * 6 | from driver_base.msg import SensorLevels 7 | 8 | gen = ParameterGenerator() 9 | 10 | # @todo verify the detectors, descriptors, matchers, etc. supported!!! 11 | fast = gen.const("FAST", str_t, "FAST", "FAST") 12 | gftt = gen.const("GFTT", str_t, "GFTT", "Good Features To Track") 13 | mser = gen.const("MSER", str_t, "MSER", "Maximal Stable Extremal Regions") 14 | orb = gen.const("ORB", str_t, "ORB", "ORiented BRIEF") 15 | sift = gen.const("SIFT", str_t, "SIFT", "SIFT") 16 | star = gen.const("STAR", str_t, "STAR", "STAR") 17 | surf = gen.const("SURF", str_t, "SURF", "SURF") 18 | detectors = gen.enum([fast, gftt, mser, orb, sift, star, surf], 19 | "Feature detectors.") 20 | gen.add("detector", str_t, SensorLevels.RECONFIGURE_RUNNING, 21 | "Feature detector.", "ORB", edit_method = detectors) 22 | 23 | descriptors = gen.enum([sift, surf], 24 | "Descriptor extractors.") 25 | gen.add("descriptor", str_t, SensorLevels.RECONFIGURE_RUNNING, 26 | "Descriptor extractor.", "SURF", edit_method = descriptors) 27 | 28 | brute_force = gen.const("BruteForce", str_t, "BruteForce", "BruteForce") 29 | flann_based = gen.const("FlannBased", str_t, "FlannBased", "FlannBased") 30 | matchers = gen.enum([brute_force, flann_based], 31 | "Descriptor matchers.") 32 | gen.add("matcher", str_t, SensorLevels.RECONFIGURE_RUNNING, 33 | "Descriptor matcher.", "FlannBased", edit_method = matchers) 34 | 35 | none_filter = gen.const("NoneFilter", str_t, "NoneFilter", "NoneFilter") 36 | cross_check_filter = gen.const("CrossCheckFilter", str_t, "CrossCheckFilter", "CrossCheckFilter") 37 | filters = gen.enum([none_filter, cross_check_filter], 38 | "Matcher filters.") 39 | gen.add("filter", str_t, SensorLevels.RECONFIGURE_RUNNING, 40 | "Matcher filter.", "CrossCheckFilter", edit_method = filters) 41 | 42 | gen.add("threshold", double_t, SensorLevels.RECONFIGURE_RUNNING, 43 | "RANSAC reprojection threshold.", 1.0, 0.0, 10.0) 44 | 45 | exit(gen.generate(PACKAGE, "Homography", "Homography")) 46 | 47 | -------------------------------------------------------------------------------- /chapter5_tutorials/config/camera/calibration_webcam.yaml: -------------------------------------------------------------------------------- 1 | image_width: 640 2 | image_height: 480 3 | camera_name: webcam 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [628.605769204058, 0, 330.525636957696, 0, 626.873332341329, 252.463246920169, 0, 0, 1] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [-0.0709132085265088, 0.102063002021842, -0.00113116095948083, -3.15823206599607e-05, 0] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [1, 0, 0, 0, 1, 0, 0, 0, 1] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [620.937438964844, 0, 329.896849376601, 0, 0, 620.437438964844, 251.769990192599, 0, 0, 0, 1, 0] -------------------------------------------------------------------------------- /chapter5_tutorials/config/camera/webcam.yaml: -------------------------------------------------------------------------------- 1 | # Example OpenCV camera parameters 2 | 3 | # Parameters, according with OpenCV VideoCapture::set method. 4 | camera_index: 0 5 | camera_name: webcam 6 | frame_width: 640 7 | frame_height: 480 8 | fps: 7.5 9 | # Check available pixel formats with (e.g. for /dev/video0): 10 | # v4l2-ctl -d /dev/video0 --list-formats 11 | # Typical values are: YUYV, MJPG (compressed) 12 | fourcc: YUYV 13 | brightness: 0.5 14 | contrast: 0.5 15 | saturation: 1. 16 | hue: 0.5 17 | gain: 0. 18 | exposure: 0.5 19 | frame_id: camera 20 | camera_info_url: package://chapter5_tutorials/config/camera/calibration_webcam.yaml 21 | 22 | -------------------------------------------------------------------------------- /chapter5_tutorials/config/camera_stereo/disparity.yaml: -------------------------------------------------------------------------------- 1 | {correlation_window_size: 33, disparity_range: 32, min_disparity: 25, prefilter_cap: 5, 2 | prefilter_size: 15, speckle_range: 15, speckle_size: 50, texture_threshold: 1000, 3 | uniqueness_ratio: 5.0} 4 | -------------------------------------------------------------------------------- /chapter5_tutorials/config/camera_stereo/logitech_c120.yaml: -------------------------------------------------------------------------------- 1 | # Example OpenCV Logitech Webcam C120 camera parameters 2 | 3 | # Parameters, according with OpenCV VideoCapture::set method. 4 | camera_index_left: 1 5 | camera_index_right: 2 6 | camera_name: logitech_c120 7 | frame_width: 640 8 | frame_height: 480 9 | fps: 7.5 10 | # Check available pixel formats with (e.g. for /dev/video0): 11 | # v4l2-ctl -d /dev/video0 --list-formats 12 | # Typical values are: YUYV, MJPG (compressed) 13 | fourcc: YUYV 14 | brightness: 0.2 15 | contrast: 0.1 16 | saturation: 0.1 17 | hue: 0. 18 | gain: 0. 19 | exposure: 0. 20 | frame_id: stereo_optical 21 | camera_info_url_left: package://chapter5_tutorials/config/camera_stereo/${NAME}.yaml 22 | camera_info_url_right: package://chapter5_tutorials/config/camera_stereo/${NAME}.yaml 23 | 24 | -------------------------------------------------------------------------------- /chapter5_tutorials/config/camera_stereo/logitech_c120_left.yaml: -------------------------------------------------------------------------------- 1 | image_width: 640 2 | image_height: 480 3 | camera_name: logitech_c120_left 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [733.331336778539, 0, 311.622739302916, 0, 727.330202909555, 247.044686047471, 0, 0, 1] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [0.119707170953865, -0.395781484171961, -0.00587813422000847, -0.0071214094185699, 0] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [0.923889231977271, 0.0195266220698682, 0.382161481663168, -0.0234502371507184, 0.999709256785772, 0.0056114413847382, -0.381940798310449, -0.0141461276461025, 0.924078521370666] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [829.254367168777, 0, -29.7742435187101, 0, 0, 829.254367168777, 242.694038391113, 0, 0, 0, 1, 0] -------------------------------------------------------------------------------- /chapter5_tutorials/config/camera_stereo/logitech_c120_right.yaml: -------------------------------------------------------------------------------- 1 | image_width: 640 2 | image_height: 480 3 | camera_name: logitech_c120_right 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [679.286769973595, 0, 305.814258310302, 0, 676.053001508839, 237.207871324979, 0, 0, 1] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [0.0726799499851729, -0.277321135716807, -0.000299382652772575, -0.00606495985473834, 0] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [0.937533880069875, -0.00257696349242685, 0.347884439117767, 0.00613686027063614, 0.999939475491544, -0.0091315004087395, -0.347839852039905, 0.0106960092022596, 0.937492950757499] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [829.254367168777, 0, -29.7742435187101, -109.679272423374, 0, 829.254367168777, 242.694038391113, 0, 0, 0, 1, 0] -------------------------------------------------------------------------------- /chapter5_tutorials/config/chapter5_tutorials.config: -------------------------------------------------------------------------------- 1 | # Override chapter5_tutorials to output everything 2 | log4j.logger.ros.chapter5_tutorials=DEBUG 3 | 4 | -------------------------------------------------------------------------------- /chapter5_tutorials/config/firewire_camera/calibration_firewire_camera.yaml: -------------------------------------------------------------------------------- 1 | image_width: 1384 2 | image_height: 1036 3 | camera_name: 00b09d0100ab1324 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [694.903357008225, 0, 689.847393876007, 0, 694.078783236935, 457.826372638728, 0, 0, 1] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [-0.293214939314933, 0.0564190484836527, 0.00539031080079255, -0.000911555139287203, 0] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [1, 0, 0, 0, 1, 0, 0, 0, 1] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [470.999450683594, 0, 690.323303014738, 0, 0, 536.534484863281, 430.407110685832, 0, 0, 0, 1, 0] -------------------------------------------------------------------------------- /chapter5_tutorials/config/firewire_camera/format7_mode0.yaml: -------------------------------------------------------------------------------- 1 | # Example firewire camera parameters 2 | 3 | # Parameters set for Sony 3.5mm 1:1.4 1/2" (aperture, focus marked with screw) 4 | guid: 00b09d0100ab1324 # (defaults to first camera on bus) 5 | iso_speed: 800 # IEEE1394b 6 | video_mode: format7_mode0 # 1384x1036 @ 30fps bayer pattern 7 | # Note that frame_rate is overwritten by frame_rate_feature; some useful values: 8 | # 21fps (480) 9 | frame_rate: 21 # max fps (Hz) 10 | auto_frame_rate_feature: 3 # Manual (3) 11 | frame_rate_feature: 480 12 | format7_color_coding: raw8 # for bayer (we can use others, even rgb8 directly) 13 | # With raw8, we don't need to configure bayer pattern and method, actually we 14 | # cannot change them 15 | bayer_pattern: rggb 16 | bayer_method: HQ 17 | auto_brightness: 3 # Manual (3) 18 | brightness: 0 19 | auto_exposure: 3 # Manual (3) 20 | exposure: 350 21 | auto_gain: 3 # Manual (3) 22 | gain: 700 23 | # We cannot set gamma manually in ROS (diamondback), so we switch it off 24 | auto_gamma: 0 # Off (0) 25 | #gamma: 1024 # gamma 1 26 | auto_saturation: 3 # Manual (3) 27 | saturation: 1000 28 | auto_sharpness: 3 # Manual (3) 29 | sharpness: 1000 30 | auto_shutter: 3 # Manual (3) 31 | #shutter: 1000 # = 10ms 32 | shutter: 1512 # = 20ms (1/50Hz), max. in 30fps 33 | auto_white_balance: 3 # Manual (3) 34 | white_balance_BU: 820 35 | white_balance_RV: 520 36 | frame_id: firewire_camera 37 | camera_info_url: package://chapter5_tutorials/config/firewire_camera/calibration_firewire_camera.yaml 38 | 39 | -------------------------------------------------------------------------------- /chapter5_tutorials/config/fovis_demo/openni_kinect_hw_registered.yaml: -------------------------------------------------------------------------------- 1 | depth_registration: true 2 | 3 | rgb_processing : true 4 | ir_processing : false 5 | depth_processing : true 6 | depth_registered_processing : true 7 | disparity_processing : false 8 | disparity_registered_processing: false 9 | hw_registered_processing : true 10 | sw_registered_processing : false 11 | 12 | -------------------------------------------------------------------------------- /chapter5_tutorials/config/fovis_demo/openni_kinect_no_registered.yaml: -------------------------------------------------------------------------------- 1 | depth_registration: true 2 | 3 | rgb_processing : true 4 | ir_processing : false 5 | depth_processing : false 6 | depth_registered_processing : false 7 | disparity_processing : false 8 | disparity_registered_processing: false 9 | hw_registered_processing : false 10 | sw_registered_processing : false 11 | 12 | -------------------------------------------------------------------------------- /chapter5_tutorials/config/fovis_demo/openni_kinect_sw_registered.yaml: -------------------------------------------------------------------------------- 1 | depth_registration: true 2 | 3 | rgb_processing : true 4 | ir_processing : false 5 | depth_processing : true 6 | depth_registered_processing : true 7 | disparity_processing : false 8 | disparity_registered_processing: false 9 | hw_registered_processing : false 10 | sw_registered_processing : true 11 | 12 | -------------------------------------------------------------------------------- /chapter5_tutorials/config/gscam/calibration_gscam.yaml: -------------------------------------------------------------------------------- 1 | image_width: 640 2 | image_height: 480 3 | camera_name: camera 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [655.18131, 0, 333.36765, 0, 653.04438, 228.90739, 0, 0, 1] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [0.11287, -0.22707, -0.00083, 0.00298, 0] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [1, 0, 0, 0, 1, 0, 0, 0, 1] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [663.30322, 0, 334.29676, 0, 0, 662.06476, 228.15963, 0, 0, 0, 1, 0] 21 | -------------------------------------------------------------------------------- /chapter5_tutorials/config/gscam/logitech.yaml: -------------------------------------------------------------------------------- 1 | # Example USB logitech camera parameters 2 | 3 | # Parameters 4 | gscam_config: v4l2src device=/dev/video0 ! video/x-raw-rgb,framerate=30/1 ! ffmpegcolorspace 5 | frame_id: gscam 6 | camera_info_url: package://chapter5_tutorials/config/gscam/calibration_gscam.yaml 7 | 8 | -------------------------------------------------------------------------------- /chapter5_tutorials/config/usb_cam/logitech.yaml: -------------------------------------------------------------------------------- 1 | # Example USB logitech camera parameters 2 | 3 | # Parameters 4 | video_device: /dev/video0 5 | image_width: 640 6 | image_height: 480 7 | pixel_format: mjpeg 8 | io_method: mmap 9 | camera_frame_id: usb_cam 10 | frame_id: usb_cam 11 | camera_info_url: 12 | 13 | -------------------------------------------------------------------------------- /chapter5_tutorials/config/viso2_demo/disparity.yaml: -------------------------------------------------------------------------------- 1 | {correlation_window_size: 23, disparity_range: 48, min_disparity: 77, prefilter_cap: 5, 2 | prefilter_size: 15, speckle_range: 5, speckle_size: 100, texture_threshold: 500, 3 | uniqueness_ratio: 5.0} 4 | -------------------------------------------------------------------------------- /chapter5_tutorials/launch/calibration/calibration_camera_chessboard.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 17 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /chapter5_tutorials/launch/calibration/calibration_camera_stereo_chessboard.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 17 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /chapter5_tutorials/launch/calibration/calibration_firewire_camera_acircles.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 14 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /chapter5_tutorials/launch/calibration/calibration_firewire_camera_chessboard.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 14 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /chapter5_tutorials/launch/calibration/calibration_firewire_camera_circles.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 14 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /chapter5_tutorials/launch/calibration/calibration_gscam_chessboard.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 18 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /chapter5_tutorials/launch/camera_polling.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 7 | 8 | 9 | 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 | -------------------------------------------------------------------------------- /chapter5_tutorials/launch/camera_timer.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 7 | 8 | 9 | 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 | -------------------------------------------------------------------------------- /chapter5_tutorials/launch/firewire_camera.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 | -------------------------------------------------------------------------------- /chapter5_tutorials/launch/fovis_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /chapter5_tutorials/launch/fovis_hw_registered.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 12 | 13 | 14 | 15 | 16 | 17 | 19 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /chapter5_tutorials/launch/fovis_no_registered.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /chapter5_tutorials/launch/fovis_sw_registered.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 12 | 13 | 14 | 15 | 16 | 17 | 19 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /chapter5_tutorials/launch/frames/stereo_frames.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 14 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /chapter5_tutorials/launch/gscam.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 | -------------------------------------------------------------------------------- /chapter5_tutorials/launch/homography.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /chapter5_tutorials/launch/openni_kinect_hw_registered.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /chapter5_tutorials/launch/openni_kinect_no_registered.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /chapter5_tutorials/launch/openni_kinect_sw_registered.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /chapter5_tutorials/launch/usb_cam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /chapter5_tutorials/launch/visual_odometry/viso2_mono_odometer.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /chapter5_tutorials/launch/visual_odometry/viso2_stereo_odometer.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /chapter5_tutorials/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | chapter5_tutorials 4 | 0.1.0 5 | Chapter 5 tutorials. 6 | 7 | Enrique Fernández 8 | Enrique Fernández 9 | 10 | BSD 11 | 12 | catkin 13 | 14 | roscpp 15 | message_generation 16 | sensor_msgs 17 | dynamic_reconfigure 18 | driver_base 19 | cv_bridge 20 | image_transport 21 | camera_info_manager 22 | tf 23 | 24 | roscpp 25 | message_runtime 26 | sensor_msgs 27 | visualization_msgs 28 | dynamic_reconfigure 29 | driver_base 30 | cv_bridge 31 | image_transport 32 | camera_info_manager 33 | tf 34 | 35 | -------------------------------------------------------------------------------- /chapter5_tutorials/src/camera_timer.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | class CameraDriver 9 | { 10 | public: 11 | enum Default 12 | { 13 | DEFAULT_CAMERA_INDEX = 0, 14 | DEFAULT_FPS = 30 15 | }; 16 | 17 | CameraDriver( int camera_index = DEFAULT_CAMERA_INDEX ) 18 | : nh( "~" ) 19 | , it( nh ) 20 | , camera_index( camera_index ) 21 | , camera( camera_index ) 22 | { 23 | nh.param( "camera_index", camera_index, DEFAULT_CAMERA_INDEX ); 24 | if ( not camera.isOpened() ) 25 | { 26 | ROS_ERROR_STREAM( "Failed to open camera device!" ); 27 | ros::shutdown(); 28 | return; 29 | } 30 | 31 | nh.param( "fps", fps, DEFAULT_FPS ); 32 | ros::Duration period = ros::Duration( 1. / fps ); 33 | 34 | pub_image_raw = it.advertise( "image_raw", 1 ); 35 | 36 | frame = boost::make_shared< cv_bridge::CvImage >(); 37 | frame->encoding = sensor_msgs::image_encodings::BGR8; 38 | 39 | timer = nh.createTimer( period, &CameraDriver::capture, this ); 40 | } 41 | 42 | ~CameraDriver() 43 | { 44 | camera.release(); 45 | } 46 | 47 | void capture( const ros::TimerEvent& te ) 48 | { 49 | camera >> frame->image; 50 | if( not frame->image.empty() ) 51 | { 52 | frame->header.stamp = ros::Time::now(); 53 | pub_image_raw.publish( frame->toImageMsg() ); 54 | } 55 | } 56 | 57 | private: 58 | ros::NodeHandle nh; 59 | image_transport::ImageTransport it; 60 | image_transport::Publisher pub_image_raw; 61 | 62 | int camera_index; 63 | int fps; 64 | 65 | cv::VideoCapture camera; 66 | cv::Mat image; 67 | cv_bridge::CvImagePtr frame; 68 | 69 | ros::Timer timer; 70 | }; 71 | 72 | 73 | int main( int argc, char* argv[] ) 74 | { 75 | ros::init( argc, argv, "camera" ); 76 | 77 | ros::NodeHandle nh( "~" ); 78 | 79 | int camera_index = CameraDriver::DEFAULT_CAMERA_INDEX; 80 | nh.param( "camera_index", camera_index, camera_index ); 81 | 82 | CameraDriver camera_driver( camera_index ); 83 | 84 | while( ros::ok() ) 85 | { 86 | ros::spin(); 87 | } 88 | 89 | return EXIT_SUCCESS; 90 | } 91 | 92 | -------------------------------------------------------------------------------- /chapter6_tutorials/src/pcl_create.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | main (int argc, char **argv) 7 | { 8 | ros::init (argc, argv, "pcl_create"); 9 | 10 | ros::NodeHandle nh; 11 | ros::Publisher pcl_pub = nh.advertise ("pcl_output", 1); 12 | pcl::PointCloud cloud; 13 | sensor_msgs::PointCloud2 output; 14 | 15 | // Fill in the cloud data 16 | cloud.width = 100; 17 | cloud.height = 1; 18 | cloud.points.resize(cloud.width * cloud.height); 19 | 20 | for (size_t i = 0; i < cloud.points.size (); ++i) 21 | { 22 | cloud.points[i].x = 1024 * rand () / (RAND_MAX + 1.0f); 23 | cloud.points[i].y = 1024 * rand () / (RAND_MAX + 1.0f); 24 | cloud.points[i].z = 1024 * rand () / (RAND_MAX + 1.0f); 25 | } 26 | 27 | //Convert the cloud to ROS message 28 | pcl::toROSMsg(cloud, output); 29 | output.header.frame_id = "odom"; 30 | 31 | ros::Rate loop_rate(1); 32 | while (ros::ok()) 33 | { 34 | pcl_pub.publish(output); 35 | ros::spinOnce(); 36 | loop_rate.sleep(); 37 | } 38 | 39 | return 0; 40 | } 41 | 42 | -------------------------------------------------------------------------------- /chapter6_tutorials/src/pcl_downsampling.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | class cloudHandler 8 | { 9 | public: 10 | cloudHandler() 11 | { 12 | pcl_sub = nh.subscribe("pcl_filtered", 10, &cloudHandler::cloudCB, this); 13 | pcl_pub = nh.advertise("pcl_downsampled", 1); 14 | } 15 | 16 | void cloudCB(const sensor_msgs::PointCloud2 &input) 17 | { 18 | pcl::PointCloud cloud; 19 | pcl::PointCloud cloud_downsampled; 20 | sensor_msgs::PointCloud2 output; 21 | 22 | pcl::fromROSMsg(input, cloud); 23 | 24 | pcl::VoxelGrid voxelSampler; 25 | voxelSampler.setInputCloud(cloud.makeShared()); 26 | voxelSampler.setLeafSize(0.01f, 0.01f, 0.01f); 27 | voxelSampler.filter(cloud_downsampled); 28 | 29 | pcl::toROSMsg(cloud_downsampled, output); 30 | pcl_pub.publish(output); 31 | 32 | } 33 | protected: 34 | ros::NodeHandle nh; 35 | ros::Subscriber pcl_sub; 36 | ros::Publisher pcl_pub; 37 | }; 38 | 39 | main(int argc, char **argv) 40 | { 41 | ros::init(argc, argv, "pcl_downsampling"); 42 | 43 | cloudHandler handler; 44 | 45 | ros::spin(); 46 | 47 | return 0; 48 | } 49 | 50 | -------------------------------------------------------------------------------- /chapter6_tutorials/src/pcl_filter.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | class cloudHandler 8 | { 9 | public: 10 | cloudHandler() 11 | { 12 | pcl_sub = nh.subscribe("pcl_output", 10, &cloudHandler::cloudCB, this); 13 | pcl_pub = nh.advertise("pcl_filtered", 1); 14 | } 15 | 16 | void cloudCB(const sensor_msgs::PointCloud2& input) 17 | { 18 | pcl::PointCloud cloud; 19 | pcl::PointCloud cloud_filtered; 20 | sensor_msgs::PointCloud2 output; 21 | 22 | pcl::fromROSMsg(input, cloud); 23 | 24 | pcl::StatisticalOutlierRemoval statFilter; 25 | statFilter.setInputCloud(cloud.makeShared()); 26 | statFilter.setMeanK(10); 27 | statFilter.setStddevMulThresh(0.2); 28 | statFilter.filter(cloud_filtered); 29 | 30 | pcl::toROSMsg(cloud_filtered, output); 31 | pcl_pub.publish(output); 32 | } 33 | 34 | protected: 35 | ros::NodeHandle nh; 36 | ros::Subscriber pcl_sub; 37 | ros::Publisher pcl_pub; 38 | }; 39 | 40 | main(int argc, char** argv) 41 | { 42 | ros::init(argc, argv, "pcl_filter"); 43 | 44 | cloudHandler handler; 45 | 46 | ros::spin(); 47 | 48 | return 0; 49 | } 50 | 51 | -------------------------------------------------------------------------------- /chapter6_tutorials/src/pcl_matching.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | class cloudHandler 8 | { 9 | public: 10 | cloudHandler() 11 | { 12 | pcl_sub = nh.subscribe("pcl_downsampled", 10, &cloudHandler::cloudCB, this); 13 | pcl_pub = nh.advertise("pcl_matched", 1); 14 | } 15 | 16 | void cloudCB(const sensor_msgs::PointCloud2 &input) 17 | { 18 | pcl::PointCloud cloud_in; 19 | pcl::PointCloud cloud_out; 20 | pcl::PointCloud cloud_aligned; 21 | sensor_msgs::PointCloud2 output; 22 | 23 | pcl::fromROSMsg(input, cloud_in); 24 | 25 | cloud_out = cloud_in; 26 | 27 | for (size_t i = 0; i < cloud_in.points.size (); ++i) 28 | { 29 | cloud_out.points[i].x = cloud_in.points[i].x + 0.7f; 30 | } 31 | 32 | pcl::IterativeClosestPoint icp; 33 | icp.setInputSource(cloud_in.makeShared()); 34 | icp.setInputTarget(cloud_out.makeShared()); 35 | 36 | icp.setMaxCorrespondenceDistance(5); 37 | icp.setMaximumIterations(100); 38 | icp.setTransformationEpsilon (1e-12); 39 | icp.setEuclideanFitnessEpsilon(0.1); 40 | 41 | icp.align(cloud_aligned); 42 | 43 | pcl::toROSMsg(cloud_aligned, output); 44 | pcl_pub.publish(output); 45 | } 46 | 47 | protected: 48 | ros::NodeHandle nh; 49 | ros::Subscriber pcl_sub; 50 | ros::Publisher pcl_pub; 51 | }; 52 | 53 | main(int argc, char **argv) 54 | { 55 | ros::init(argc, argv, "pcl_matching"); 56 | 57 | cloudHandler handler; 58 | 59 | ros::spin(); 60 | 61 | return 0; 62 | } 63 | -------------------------------------------------------------------------------- /chapter6_tutorials/src/pcl_model_estimation.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | class cloudHandler 9 | { 10 | public: 11 | cloudHandler() 12 | { 13 | pcl_sub = nh.subscribe("pcl_downsampled", 10, &cloudHandler::cloudCB, this); 14 | pcl_pub = nh.advertise("pcl_plane", 1); 15 | } 16 | 17 | void cloudCB(const sensor_msgs::PointCloud2 &input) 18 | { 19 | // initialize PointClouds 20 | pcl::PointCloud cloud; 21 | pcl::PointCloud final; 22 | sensor_msgs::PointCloud2 output; 23 | 24 | pcl::fromROSMsg(input, cloud); 25 | 26 | // populate our PointCloud with points 27 | cloud.width = 500; 28 | cloud.height = 1; 29 | cloud.is_dense = false; 30 | cloud.points.resize(cloud.width * cloud.height); 31 | 32 | std::vector inliers; 33 | 34 | // created RandomSampleConsensus object and compute the appropriated model 35 | pcl::SampleConsensusModelPlane::Ptr 36 | model_p(new pcl::SampleConsensusModelPlane(cloud.makeShared())); 37 | 38 | pcl::RandomSampleConsensus ransac(model_p); 39 | ransac.setDistanceThreshold(.01); 40 | ransac.computeModel(); 41 | ransac.getInliers(inliers); 42 | 43 | // copies all inliers of the model computed to another PointCloud 44 | pcl::copyPointCloud(cloud, inliers, final); 45 | pcl::toROSMsg(final, output); 46 | pcl_pub.publish(output); 47 | } 48 | 49 | protected: 50 | ros::NodeHandle nh; 51 | ros::Subscriber pcl_sub; 52 | ros::Publisher pcl_pub; 53 | }; 54 | 55 | main(int argc, char **argv) 56 | { 57 | ros::init(argc, argv, "pcl_model_estimation"); 58 | 59 | cloudHandler handler; 60 | 61 | ros::spin(); 62 | 63 | return 0; 64 | } 65 | 66 | -------------------------------------------------------------------------------- /chapter6_tutorials/src/pcl_partitioning.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | class cloudHandler 8 | { 9 | public: 10 | cloudHandler() 11 | { 12 | pcl_sub = nh.subscribe("pcl_downsampled", 10, &cloudHandler::cloudCB, this); 13 | pcl_pub = nh.advertise("pcl_partitioned", 1); 14 | } 15 | 16 | void cloudCB(const sensor_msgs::PointCloud2 &input) 17 | { 18 | pcl::PointCloud cloud; 19 | pcl::PointCloud cloud_partitioned; 20 | sensor_msgs::PointCloud2 output; 21 | 22 | pcl::fromROSMsg(input, cloud); 23 | 24 | float resolution = 128.0f; 25 | pcl::octree::OctreePointCloudSearch octree (resolution); 26 | 27 | octree.setInputCloud (cloud.makeShared()); 28 | octree.addPointsFromInputCloud (); 29 | 30 | pcl::PointXYZ center_point; 31 | center_point.x = 0 ; 32 | center_point.y = 0.4; 33 | center_point.z = -1.4; 34 | 35 | float radius = 0.5; 36 | std::vector radiusIdx; 37 | std::vector radiusSQDist; 38 | if (octree.radiusSearch (center_point, radius, radiusIdx, radiusSQDist) > 0) 39 | { 40 | for (size_t i = 0; i < radiusIdx.size (); ++i) 41 | { 42 | cloud_partitioned.points.push_back(cloud.points[radiusIdx[i]]); 43 | } 44 | } 45 | 46 | pcl::toROSMsg(cloud_partitioned, output); 47 | output.header.frame_id = "odom"; 48 | pcl_pub.publish(output); 49 | } 50 | 51 | protected: 52 | ros::NodeHandle nh; 53 | ros::Subscriber pcl_sub; 54 | ros::Publisher pcl_pub; 55 | }; 56 | 57 | main(int argc, char **argv) 58 | { 59 | ros::init(argc, argv, "pcl_partitioning"); 60 | 61 | cloudHandler handler; 62 | 63 | ros::spin(); 64 | 65 | return 0; 66 | } 67 | -------------------------------------------------------------------------------- /chapter6_tutorials/src/pcl_read.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | main(int argc, char **argv) 8 | { 9 | ros::init (argc, argv, "pcl_read"); 10 | 11 | ros::NodeHandle nh; 12 | ros::Publisher pcl_pub = nh.advertise ("pcl_output", 1); 13 | 14 | sensor_msgs::PointCloud2 output; 15 | pcl::PointCloud cloud; 16 | 17 | pcl::io::loadPCDFile ("test_pcd.pcd", cloud); 18 | 19 | pcl::toROSMsg(cloud, output); 20 | output.header.frame_id = "odom"; 21 | 22 | ros::Rate loop_rate(1); 23 | while (ros::ok()) 24 | { 25 | pcl_pub.publish(output); 26 | ros::spinOnce(); 27 | loop_rate.sleep(); 28 | } 29 | 30 | return 0; 31 | } 32 | 33 | -------------------------------------------------------------------------------- /chapter6_tutorials/src/pcl_visualize.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | class cloudHandler 8 | { 9 | public: 10 | cloudHandler() 11 | : viewer("Cloud Viewer") 12 | { 13 | pcl_sub = nh.subscribe("pcl_output", 10, &cloudHandler::cloudCB, this); 14 | viewer_timer = nh.createTimer(ros::Duration(0.1), &cloudHandler::timerCB, this); 15 | } 16 | 17 | void cloudCB(const sensor_msgs::PointCloud2 &input) 18 | { 19 | pcl::PointCloud cloud; 20 | pcl::fromROSMsg(input, cloud); 21 | 22 | viewer.showCloud(cloud.makeShared()); 23 | } 24 | 25 | void timerCB(const ros::TimerEvent&) 26 | { 27 | if (viewer.wasStopped()) 28 | { 29 | ros::shutdown(); 30 | } 31 | } 32 | 33 | protected: 34 | ros::NodeHandle nh; 35 | ros::Subscriber pcl_sub; 36 | pcl::visualization::CloudViewer viewer; 37 | ros::Timer viewer_timer; 38 | }; 39 | 40 | main (int argc, char **argv) 41 | { 42 | ros::init (argc, argv, "pcl_visualize"); 43 | 44 | cloudHandler handler; 45 | 46 | ros::spin(); 47 | 48 | return 0; 49 | } 50 | -------------------------------------------------------------------------------- /chapter6_tutorials/src/pcl_visualize2.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | class cloudHandler 8 | { 9 | public: 10 | cloudHandler() 11 | : viewer("Cloud Viewer") 12 | { 13 | output_sub = nh.subscribe("pcl_output", 1, &cloudHandler::outputCB, this); 14 | downsampled_sub = nh.subscribe("pcl_partitioned", 1, &cloudHandler::downsampledCB, this); 15 | viewer_timer = nh.createTimer(ros::Duration(0.1), &cloudHandler::timerCB, this); 16 | 17 | viewer.createViewPort(0.0, 0.0, 0.5, 1.0, output_view); 18 | viewer.setBackgroundColor(0, 0, 0, output_view); 19 | 20 | viewer.createViewPort(0.5, 0.0, 1.0, 1.0, downsampled_view); 21 | viewer.setBackgroundColor(0, 0, 0, downsampled_view); 22 | 23 | viewer.addCoordinateSystem(1.0); 24 | viewer.initCameraParameters(); 25 | } 26 | 27 | void outputCB(const sensor_msgs::PointCloud2ConstPtr& input) 28 | { 29 | pcl::PointCloud cloud; 30 | pcl::fromROSMsg(*input, cloud); 31 | 32 | viewer.removeAllPointClouds(output_view); 33 | viewer.addPointCloud(cloud.makeShared(), "output", output_view); 34 | } 35 | 36 | void downsampledCB(const sensor_msgs::PointCloud2ConstPtr& input) 37 | { 38 | pcl::PointCloud cloud; 39 | pcl::fromROSMsg(*input, cloud); 40 | 41 | viewer.removeAllPointClouds(downsampled_view); 42 | viewer.addPointCloud(cloud.makeShared(), "downsampled", downsampled_view); 43 | } 44 | 45 | void timerCB(const ros::TimerEvent&) 46 | { 47 | viewer.spinOnce(); 48 | 49 | if (viewer.wasStopped()) 50 | { 51 | ros::shutdown(); 52 | } 53 | } 54 | 55 | protected: 56 | ros::NodeHandle nh; 57 | ros::Subscriber output_sub, downsampled_sub; 58 | pcl::visualization::PCLVisualizer viewer; 59 | int output_view, downsampled_view; 60 | ros::Timer viewer_timer; 61 | }; 62 | 63 | main (int argc, char **argv) 64 | { 65 | ros::init (argc, argv, "pcl_visualize2"); 66 | 67 | cloudHandler handler; 68 | 69 | ros::spin(); 70 | 71 | return 0; 72 | } 73 | -------------------------------------------------------------------------------- /chapter6_tutorials/src/pcl_write.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | void cloudCB(const sensor_msgs::PointCloud2 &input) 8 | { 9 | pcl::PointCloud cloud; 10 | pcl::fromROSMsg(input, cloud); 11 | pcl::io::savePCDFileASCII ("write_pcd_test.pcd", cloud); 12 | } 13 | 14 | main (int argc, char **argv) 15 | { 16 | ros::init (argc, argv, "pcl_write"); 17 | 18 | ros::NodeHandle nh; 19 | ros::Subscriber bat_sub = nh.subscribe("pcl_output", 10, cloudCB); 20 | 21 | ros::spin(); 22 | 23 | return 0; 24 | } 25 | 26 | -------------------------------------------------------------------------------- /chapter7_tutorials/chapter7_tutorials/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(chapter7_tutorials) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /chapter7_tutorials/chapter7_tutorials/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | chapter7_tutorials 4 | 0.0.0 5 | The chapter7_tutorials covers examples from 3D modelling and gazebo simulation 6 | 7 | 8 | 9 | 10 | 11 | anil 12 | 13 | 14 | 15 | 16 | 17 | TODO 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 | catkin 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /chapter7_tutorials/robot1_description/launch/display.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /chapter7_tutorials/robot1_description/launch/display_state_pub.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /chapter7_tutorials/robot1_description/launch/state_xacro.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /chapter7_tutorials/robot1_description/urdf/dae.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /chapter7_tutorials/robot1_description/urdf/origins.gv: -------------------------------------------------------------------------------- 1 | digraph G { 2 | node [shape=box]; 3 | "base_link" [label="base_link"]; 4 | "arm_base" [label="arm_base"]; 5 | "arm_1" [label="arm_1"]; 6 | "arm_2" [label="arm_2"]; 7 | "left_gripper" [label="left_gripper"]; 8 | "left_tip" [label="left_tip"]; 9 | "right_gripper" [label="right_gripper"]; 10 | "right_tip" [label="right_tip"]; 11 | "wheel_1" [label="wheel_1"]; 12 | "wheel_2" [label="wheel_2"]; 13 | "wheel_3" [label="wheel_3"]; 14 | "wheel_4" [label="wheel_4"]; 15 | node [shape=ellipse, color=blue, fontcolor=blue]; 16 | "base_link" -> "base_to_arm_base" [label="xyz: 0 0 0 \nrpy: 0 -0 0"] 17 | "base_to_arm_base" -> "arm_base" 18 | "arm_base" -> "arm_1_to_arm_base" [label="xyz: 0 0 0.15 \nrpy: 0 -0 0"] 19 | "arm_1_to_arm_base" -> "arm_1" 20 | "arm_1" -> "arm_2_to_arm_1" [label="xyz: 0 0 0.45 \nrpy: 0 -0 0"] 21 | "arm_2_to_arm_1" -> "arm_2" 22 | "arm_2" -> "left_gripper_joint" [label="xyz: 0.06 0 0.4 \nrpy: 0 -1.57 0"] 23 | "left_gripper_joint" -> "left_gripper" 24 | "left_gripper" -> "left_tip_joint" [label="xyz: 0 0 0 \nrpy: 0 -0 0"] 25 | "left_tip_joint" -> "left_tip" 26 | "arm_2" -> "right_gripper_joint" [label="xyz: 0.06 0 0.4 \nrpy: 0 -1.57 0"] 27 | "right_gripper_joint" -> "right_gripper" 28 | "right_gripper" -> "right_tip_joint" [label="xyz: 0 0 0 \nrpy: 0 -0 0"] 29 | "right_tip_joint" -> "right_tip" 30 | "base_link" -> "base_to_wheel1" [label="xyz: 0 0 0 \nrpy: 0 -0 0"] 31 | "base_to_wheel1" -> "wheel_1" 32 | "base_link" -> "base_to_wheel2" [label="xyz: 0 0 0 \nrpy: 0 -0 0"] 33 | "base_to_wheel2" -> "wheel_2" 34 | "base_link" -> "base_to_wheel3" [label="xyz: 0 0 0 \nrpy: 0 -0 0"] 35 | "base_to_wheel3" -> "wheel_3" 36 | "base_link" -> "base_to_wheel4" [label="xyz: 0 0 0 \nrpy: 0 -0 0"] 37 | "base_to_wheel4" -> "wheel_4" 38 | } 39 | -------------------------------------------------------------------------------- /chapter7_tutorials/robot1_description/urdf/origins.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AaronMR/Learning_ROS_for_Robotics_Programming_2nd_edition/afc45a0a207f36ced489876abce4414afa2f11ca/chapter7_tutorials/robot1_description/urdf/origins.pdf -------------------------------------------------------------------------------- /chapter7_tutorials/robot1_description/urdf/robot1_processed.urdf: -------------------------------------------------------------------------------- 1 | No input given 2 | Usage: xacro.py [-o ] 3 | xacro.py --deps Prints dependencies 4 | xacro.py --includes Only evalutes includes 5 | -------------------------------------------------------------------------------- /chapter7_tutorials/robot1_gazebo/launch/gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 25 | 26 | 27 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /chapter7_tutorials/robot1_gazebo/launch/gazebo_wg.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 11 | 12 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /chapter7_tutorials/robot1_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | robot1_gazebo 4 | 0.0.0 5 | The robot1_gazebo package 6 | 7 | 8 | 9 | 10 | luis 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | gazebo_ros 44 | gazebo_ros 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /chapter7_tutorials/robot1_gazebo/worlds/robot.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | model://ground_plane 8 | 9 | 10 | 11 | 12 | model://sun 13 | 14 | 15 | 16 | 17 | 18 | 4.927360 -4.376610 3.740080 0.000000 0.275643 2.356190 19 | orbit 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /chapter8_tutorials/.comandos.swp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AaronMR/Learning_ROS_for_Robotics_Programming_2nd_edition/afc45a0a207f36ced489876abce4414afa2f11ca/chapter8_tutorials/.comandos.swp -------------------------------------------------------------------------------- /chapter8_tutorials/launch/gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 25 | 26 | 27 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /chapter8_tutorials/launch/gazebo_map_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 | -------------------------------------------------------------------------------- /chapter8_tutorials/launch/gazebo_mapping_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 | -------------------------------------------------------------------------------- /chapter8_tutorials/launch/gazebo_xacro.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 | -------------------------------------------------------------------------------- /chapter8_tutorials/maps/map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AaronMR/Learning_ROS_for_Robotics_Programming_2nd_edition/afc45a0a207f36ced489876abce4414afa2f11ca/chapter8_tutorials/maps/map.pgm -------------------------------------------------------------------------------- /chapter8_tutorials/maps/map.yaml: -------------------------------------------------------------------------------- 1 | image: map.pgm 2 | resolution: 0.050000 3 | origin: [-50.000000, -50.000000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /chapter8_tutorials/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | chapter8_tutorials 4 | 0.0.0 5 | The chapter8_tutorials package 6 | 7 | 8 | 9 | 10 | retrorov-laptop 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | roscpp 44 | rospy 45 | roscpp 46 | rospy 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /chapter8_tutorials/src/laser.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | int main(int argc, char** argv){ 6 | 7 | ros::init(argc, argv, "laser_scan_publisher"); 8 | 9 | ros::NodeHandle n; 10 | 11 | ros::Publisher scan_pub = n.advertise("scan", 50); 12 | 13 | unsigned int num_readings = 100; 14 | 15 | double laser_frequency = 40; 16 | 17 | double ranges[num_readings]; 18 | 19 | double intensities[num_readings]; 20 | 21 | int count = 0; 22 | 23 | ros::Rate r(1.0); 24 | 25 | while(n.ok()){ 26 | 27 | //generate some fake data for our laser scan 28 | 29 | for(unsigned int i = 0; i < num_readings; ++i){ 30 | 31 | ranges[i] = count; 32 | 33 | intensities[i] = 100 + count; 34 | 35 | } 36 | 37 | ros::Time scan_time = ros::Time::now(); 38 | 39 | //populate the LaserScan message 40 | 41 | sensor_msgs::LaserScan scan; 42 | 43 | scan.header.stamp = scan_time; 44 | 45 | scan.header.frame_id = "base_link"; 46 | 47 | scan.angle_min = -1.57; 48 | 49 | scan.angle_max = 1.57; 50 | 51 | scan.angle_increment = 3.14 / num_readings; 52 | 53 | scan.time_increment = (1 / laser_frequency) / (num_readings); 54 | 55 | scan.range_min = 0.0; 56 | 57 | scan.range_max = 100.0; 58 | 59 | scan.ranges.resize(num_readings); 60 | 61 | scan.intensities.resize(num_readings); 62 | 63 | for(unsigned int i = 0; i < num_readings; ++i){ 64 | 65 | scan.ranges[i] = ranges[i]; 66 | 67 | scan.intensities[i] = intensities[i]; 68 | 69 | } 70 | 71 | scan_pub.publish(scan); 72 | 73 | ++count; 74 | 75 | r.sleep(); 76 | 77 | } 78 | 79 | } 80 | 81 | 82 | 83 | -------------------------------------------------------------------------------- /chapter8_tutorials/src/tf_broadcaster.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | int main(int argc, char** argv){ 5 | ros::init(argc, argv, "robot_tf_publisher"); 6 | ros::NodeHandle n; 7 | 8 | ros::Rate r(100); 9 | 10 | tf::TransformBroadcaster broadcaster; 11 | 12 | while(n.ok()){ 13 | broadcaster.sendTransform( 14 | tf::StampedTransform( 15 | tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.0, 0.0, 0.0)), 16 | ros::Time::now(),"base_link", "base_laser")); 17 | r.sleep(); 18 | } 19 | } 20 | -------------------------------------------------------------------------------- /chapter8_tutorials/src/tf_listener.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | void transformPoint(const tf::TransformListener& listener){ 6 | //we'll create a point in the base_laser frame that we'd like to transform to the base_link frame 7 | 8 | geometry_msgs::PointStamped laser_point; 9 | laser_point.header.frame_id = "base_laser"; 10 | 11 | //we'll just use the most recent transform available for our simple example 12 | laser_point.header.stamp = ros::Time(); 13 | 14 | //just an arbitrary point in space 15 | laser_point.point.x = 1.0; 16 | laser_point.point.y = 2.0; 17 | laser_point.point.z = 0.0; 18 | 19 | geometry_msgs::PointStamped base_point; 20 | 21 | 22 | //listener.waitForTransform( 23 | 24 | listener.transformPoint("base_link", laser_point, base_point); 25 | 26 | 27 | // listener.lookupTransform("base_link", "wheel_2", ros::Time(0), ros::Duration(10.0)); 28 | ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f", 29 | laser_point.point.x, laser_point.point.y, laser_point.point.z, 30 | base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec()); 31 | 32 | } 33 | 34 | int main(int argc, char** argv){ 35 | ros::init(argc, argv, "robot_tf_listener"); 36 | ros::NodeHandle n; 37 | 38 | tf::TransformListener listener(ros::Duration(10)); 39 | 40 | //we'll transform a point once every second 41 | ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPoint, boost::ref(listener))); 42 | 43 | ros::spin(); 44 | 45 | } 46 | -------------------------------------------------------------------------------- /chapter8_tutorials/urdf/frames.gv: -------------------------------------------------------------------------------- 1 | digraph G { 2 | "base_footprint" -> "base_link"[label="Broadcaster: /robot_state_publisher\nAverage rate: 50.205 Hz\nMost recent transform: 1418486564.748 \nBuffer length: 4.940 sec\n"]; 3 | "base_link" -> "base_laser"[label="Broadcaster: /robot_tf_publisher\nAverage rate: 100.211 Hz\nMost recent transform: 247.407 \nBuffer length: 4.750 sec\n"]; 4 | "base_link" -> "wheel_1"[label="Broadcaster: /robot_state_publisher\nAverage rate: 10.204 Hz\nMost recent transform: 1418486564.233 \nBuffer length: 4.900 sec\n"]; 5 | "base_link" -> "wheel_2"[label="Broadcaster: /robot_state_publisher\nAverage rate: 10.204 Hz\nMost recent transform: 1418486564.233 \nBuffer length: 4.900 sec\n"]; 6 | "base_link" -> "wheel_3"[label="Broadcaster: /robot_state_publisher\nAverage rate: 10.204 Hz\nMost recent transform: 1418486564.233 \nBuffer length: 4.900 sec\n"]; 7 | "base_link" -> "wheel_4"[label="Broadcaster: /robot_state_publisher\nAverage rate: 10.204 Hz\nMost recent transform: 1418486564.233 \nBuffer length: 4.900 sec\n"]; 8 | } -------------------------------------------------------------------------------- /chapter9_tutorials/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(chapter9_tutorials) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | move_base_msgs 7 | actionlib 8 | tf 9 | ) 10 | 11 | catkin_package() 12 | 13 | include_directories( 14 | ${catkin_INCLUDE_DIRS} 15 | ) 16 | 17 | add_executable(sendGoals src/sendGoals.cpp) 18 | target_link_libraries(sendGoals ${catkin_LIBRARIES}) 19 | 20 | install(TARGETS sendGoals 21 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 22 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 23 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 24 | ) 25 | 26 | foreach(dir launch maps) 27 | install(DIRECTORY ${dir}/ 28 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) 29 | endforeach(dir) 30 | -------------------------------------------------------------------------------- /chapter9_tutorials/launch/base_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | TrajectoryPlannerROS: 2 | max_vel_x: 0.2 3 | min_vel_x: 0.05 4 | max_rotational_vel: 0.15 5 | min_in_place_rotational_vel: 0.01 6 | min_in_place_vel_theta: 0.01 7 | max_vel_theta: 0.15 8 | min_vel_theta: -0.15 9 | acc_lim_th: 3.2 10 | acc_lim_x: 2.5 11 | acc_lim_y: 2.5 12 | 13 | holonomic_robot: false 14 | -------------------------------------------------------------------------------- /chapter9_tutorials/launch/chapter9_configuration_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /chapter9_tutorials/launch/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | obstacle_range: 2.5 2 | raytrace_range: 3.0 3 | footprint: [[-0.2,-0.2],[-0.2,0.2], [0.2, 0.2], [0.2,-0.2]] 4 | #robot_radius: ir_of_robot 5 | inflation_radius: 0.5 6 | cost_scaling_factor: 10.0 7 | observation_sources: scan 8 | scan: {sensor_frame: base_link, observation_persistence: 0.0, max_obstacle_height: 0.4, min_obstacle_height: 0.0, data_type: LaserScan, topic: /scan, marking: true, clearing: true} 9 | 10 | -------------------------------------------------------------------------------- /chapter9_tutorials/launch/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: /map 3 | robot_base_frame: /base_footprint 4 | update_frequency: 1.0 5 | static_map: true 6 | -------------------------------------------------------------------------------- /chapter9_tutorials/launch/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: /map 3 | robot_base_frame: /base_footprint 4 | update_frequency: 5.0 5 | publish_frequency: 2.0 6 | static_map: false 7 | rolling_window: true 8 | width: 5.0 9 | height: 5.0 10 | resolution: 0.02 11 | tranform_tolerance: 0.5 12 | planner_frequency: 1.0 13 | planner_patiente: 5.0 14 | #plugins: 15 | # {name: obstacles, type: "costmal_2d::ObstacleLayer", 16 | # name: inflation, type: "costmal_2d::InflationLayer"} 17 | -------------------------------------------------------------------------------- /chapter9_tutorials/launch/move_base.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /chapter9_tutorials/maps/map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AaronMR/Learning_ROS_for_Robotics_Programming_2nd_edition/afc45a0a207f36ced489876abce4414afa2f11ca/chapter9_tutorials/maps/map.pgm -------------------------------------------------------------------------------- /chapter9_tutorials/maps/map.yaml: -------------------------------------------------------------------------------- 1 | image: map.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 | -------------------------------------------------------------------------------- /chapter9_tutorials/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | chapter9_tutorials 4 | 0.0.0 5 | The chapter9_tutorials package 6 | 7 | Enrique Fernandez 8 | Enrique Fernandez 9 | 10 | BSD 11 | 12 | catkin 13 | 14 | roscpp 15 | move_base_msgs 16 | actionlib 17 | tf 18 | 19 | roscpp 20 | move_base_msgs 21 | actionlib 22 | tf 23 | amcl 24 | gazebo_ros 25 | xacro 26 | move_base 27 | map_server 28 | joint_state_publisher 29 | robot_state_publisher 30 | rviz 31 | robot1_description 32 | 33 | -------------------------------------------------------------------------------- /chapter9_tutorials/src/sendGoals.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | typedef actionlib::SimpleActionClient MoveBaseClient; 8 | 9 | int main(int argc, char** argv){ 10 | ros::init(argc, argv, "navigation_goals"); 11 | 12 | MoveBaseClient ac("move_base", true); 13 | 14 | while(!ac.waitForServer(ros::Duration(5.0))){ 15 | ROS_INFO("Waiting for the move_base action server"); 16 | } 17 | 18 | move_base_msgs::MoveBaseGoal goal; 19 | 20 | goal.target_pose.header.frame_id = "map"; 21 | goal.target_pose.header.stamp = ros::Time::now(); 22 | 23 | goal.target_pose.pose.position.x = 1.0; 24 | goal.target_pose.pose.position.y = 1.0; 25 | goal.target_pose.pose.orientation.w = 1.0; 26 | 27 | ROS_INFO("Sending goal"); 28 | ac.sendGoal(goal); 29 | 30 | ac.waitForResult(); 31 | 32 | if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) 33 | ROS_INFO("You have arrived to the goal position"); 34 | else{ 35 | ROS_INFO("The base failed for some reason"); 36 | } 37 | return 0; 38 | } 39 | -------------------------------------------------------------------------------- /contributors.txt: -------------------------------------------------------------------------------- 1 | Aaron Martínez 2 | Anil Mahtani 3 | Enrique Fernández 4 | Luis Sánchez 5 | --------------------------------------------------------------------------------