├── gazebo_screenshot.png ├── pepper_gazebo_plugin ├── images │ ├── TopicVisu.png │ ├── GazeboCamSonar.png │ └── MoveitCamSonar.png ├── models │ ├── bench │ │ ├── meshes │ │ │ └── bench.stl │ │ ├── model.config │ │ └── bench.sdf │ ├── sofa │ │ ├── meshes │ │ │ └── sofa.stl │ │ ├── model.config │ │ └── sofa.sdf │ ├── closet │ │ ├── meshes │ │ │ └── closet.stl │ │ ├── model.config │ │ └── closet.sdf │ ├── wardrobe │ │ ├── meshes │ │ │ └── wardrobe.stl │ │ ├── model.config │ │ └── wardrobe.sdf │ ├── floor_lamp │ │ ├── meshes │ │ │ └── floor_lamp.stl │ │ ├── model.config │ │ └── floor_lamp.sdf │ ├── dining_chair │ │ ├── meshes │ │ │ └── dining_chair.stl │ │ ├── model.config │ │ └── dining_chair.sdf │ ├── kitchen_table │ │ ├── meshes │ │ │ └── kitchen_table.stl │ │ ├── model.config │ │ └── kitchen_table.sdf │ ├── citizen_extras_male_03 │ │ ├── images │ │ │ ├── hair1noCulling.JPG │ │ │ ├── hrad1noCulling.JPG │ │ │ └── Material__1noCulling.JPG │ │ ├── model.config │ │ └── model.sdf │ ├── citizen_extras_female_03 │ │ ├── images │ │ │ ├── Material__1noCulling.JPG │ │ │ ├── Material__2noCulling.JPG │ │ │ └── _03_-_Default1noCulling.JPG │ │ ├── model.config │ │ └── model.sdf │ └── citizen_extras_female_02 │ │ ├── images │ │ ├── _01_-_Default1noCulling.JPG │ │ ├── _02_-_Default1noCulling.JPG │ │ └── _03_-_Default1noCulling.JPG │ │ ├── model.config │ │ └── model.sdf ├── CMakeLists.txt ├── worlds │ ├── nao_test2.world │ ├── nao_test.world │ └── simple_office_with_people.world ├── package.xml ├── launch │ ├── pc_to_laserscan.launch │ ├── pepper_gazebo_plugin_Y20_CPU_no_arms.launch │ ├── pepper_gazebo_plugin_in_office_CPU_no_arms.launch │ ├── pepper_gazebo_plugin_Y20.launch │ ├── pepper_gazebo_plugin_Y20_CPU.launch │ ├── pepper_gazebo_plugin_in_tiago_world.launch │ ├── pepper_gazebo_plugin_in_office.launch │ └── pepper_gazebo_plugin_in_office_CPU.launch ├── scripts │ ├── arms_down.sh │ ├── convertTrajMsg.py │ ├── pcl_checker.py │ ├── pub_lasers.py │ └── laser_publisher.py ├── CHANGELOG.rst ├── README.rst └── testfiles │ ├── trajBack_modified.txt │ ├── traj_modified.txt │ ├── coucou │ ├── coucou2_modified.txt │ ├── coucou3_modified.txt │ ├── coucou2.txt │ ├── coucou3.txt │ ├── coucou1_modified.txt │ └── coucou1.txt │ ├── trajBack.txt │ └── traj.txt ├── pepper_control ├── CMakeLists.txt ├── launch │ ├── pepper_control_no_wheels.launch │ ├── pepper_control_trajectory_no_arms.launch │ ├── pepper_control_trajectory_all.launch │ ├── pepper_control_trajectory.launch │ └── pepper_control_position.launch ├── CHANGELOG.rst ├── package.xml └── config │ ├── pepper_position_control.yaml │ └── pepper_trajectory_control.yaml ├── Dockerfile ├── start_docker.sh └── README.md /gazebo_screenshot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awesomebytes/pepper_virtual/HEAD/gazebo_screenshot.png -------------------------------------------------------------------------------- /pepper_gazebo_plugin/images/TopicVisu.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awesomebytes/pepper_virtual/HEAD/pepper_gazebo_plugin/images/TopicVisu.png -------------------------------------------------------------------------------- /pepper_gazebo_plugin/images/GazeboCamSonar.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awesomebytes/pepper_virtual/HEAD/pepper_gazebo_plugin/images/GazeboCamSonar.png -------------------------------------------------------------------------------- /pepper_gazebo_plugin/images/MoveitCamSonar.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awesomebytes/pepper_virtual/HEAD/pepper_gazebo_plugin/images/MoveitCamSonar.png -------------------------------------------------------------------------------- /pepper_gazebo_plugin/models/bench/meshes/bench.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awesomebytes/pepper_virtual/HEAD/pepper_gazebo_plugin/models/bench/meshes/bench.stl -------------------------------------------------------------------------------- /pepper_gazebo_plugin/models/sofa/meshes/sofa.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awesomebytes/pepper_virtual/HEAD/pepper_gazebo_plugin/models/sofa/meshes/sofa.stl -------------------------------------------------------------------------------- /pepper_gazebo_plugin/models/closet/meshes/closet.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awesomebytes/pepper_virtual/HEAD/pepper_gazebo_plugin/models/closet/meshes/closet.stl -------------------------------------------------------------------------------- /pepper_gazebo_plugin/models/wardrobe/meshes/wardrobe.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awesomebytes/pepper_virtual/HEAD/pepper_gazebo_plugin/models/wardrobe/meshes/wardrobe.stl -------------------------------------------------------------------------------- /pepper_gazebo_plugin/models/floor_lamp/meshes/floor_lamp.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awesomebytes/pepper_virtual/HEAD/pepper_gazebo_plugin/models/floor_lamp/meshes/floor_lamp.stl -------------------------------------------------------------------------------- /pepper_gazebo_plugin/models/dining_chair/meshes/dining_chair.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awesomebytes/pepper_virtual/HEAD/pepper_gazebo_plugin/models/dining_chair/meshes/dining_chair.stl -------------------------------------------------------------------------------- /pepper_gazebo_plugin/models/kitchen_table/meshes/kitchen_table.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awesomebytes/pepper_virtual/HEAD/pepper_gazebo_plugin/models/kitchen_table/meshes/kitchen_table.stl -------------------------------------------------------------------------------- /pepper_gazebo_plugin/models/citizen_extras_male_03/images/hair1noCulling.JPG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awesomebytes/pepper_virtual/HEAD/pepper_gazebo_plugin/models/citizen_extras_male_03/images/hair1noCulling.JPG -------------------------------------------------------------------------------- /pepper_gazebo_plugin/models/citizen_extras_male_03/images/hrad1noCulling.JPG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awesomebytes/pepper_virtual/HEAD/pepper_gazebo_plugin/models/citizen_extras_male_03/images/hrad1noCulling.JPG -------------------------------------------------------------------------------- /pepper_gazebo_plugin/models/citizen_extras_male_03/images/Material__1noCulling.JPG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awesomebytes/pepper_virtual/HEAD/pepper_gazebo_plugin/models/citizen_extras_male_03/images/Material__1noCulling.JPG -------------------------------------------------------------------------------- /pepper_gazebo_plugin/models/citizen_extras_female_03/images/Material__1noCulling.JPG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awesomebytes/pepper_virtual/HEAD/pepper_gazebo_plugin/models/citizen_extras_female_03/images/Material__1noCulling.JPG -------------------------------------------------------------------------------- /pepper_gazebo_plugin/models/citizen_extras_female_03/images/Material__2noCulling.JPG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awesomebytes/pepper_virtual/HEAD/pepper_gazebo_plugin/models/citizen_extras_female_03/images/Material__2noCulling.JPG -------------------------------------------------------------------------------- /pepper_gazebo_plugin/models/citizen_extras_female_02/images/_01_-_Default1noCulling.JPG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awesomebytes/pepper_virtual/HEAD/pepper_gazebo_plugin/models/citizen_extras_female_02/images/_01_-_Default1noCulling.JPG -------------------------------------------------------------------------------- /pepper_gazebo_plugin/models/citizen_extras_female_02/images/_02_-_Default1noCulling.JPG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awesomebytes/pepper_virtual/HEAD/pepper_gazebo_plugin/models/citizen_extras_female_02/images/_02_-_Default1noCulling.JPG -------------------------------------------------------------------------------- /pepper_gazebo_plugin/models/citizen_extras_female_02/images/_03_-_Default1noCulling.JPG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awesomebytes/pepper_virtual/HEAD/pepper_gazebo_plugin/models/citizen_extras_female_02/images/_03_-_Default1noCulling.JPG -------------------------------------------------------------------------------- /pepper_gazebo_plugin/models/citizen_extras_female_03/images/_03_-_Default1noCulling.JPG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awesomebytes/pepper_virtual/HEAD/pepper_gazebo_plugin/models/citizen_extras_female_03/images/_03_-_Default1noCulling.JPG -------------------------------------------------------------------------------- /pepper_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(pepper_control) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | 8 | install(DIRECTORY config launch 9 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 10 | ) 11 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(pepper_gazebo_plugin) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | 8 | install(DIRECTORY launch worlds 9 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 10 | ) 11 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/models/sofa/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | sofa 4 | 0.1.0 5 | 6 | Joan Guasch 7 | guasch5000@pal-robotics.com 8 | 9 | sofa.sdf 10 | 11 | Sofa 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/models/bench/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | bench 4 | 0.1.0 5 | 6 | Joan Guasch 7 | guasch5000@pal-robotics.com 8 | 9 | bench.sdf 10 | 11 | bench 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/models/closet/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | closet 4 | 0.1.0 5 | 6 | Joan Guasch 7 | guasch5000@pal-robotics.com 8 | 9 | closet.sdf 10 | 11 | closet 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/models/wardrobe/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | wardrobe 4 | 0.1.0 5 | 6 | Joan Guasch 7 | guasch5000@pal-robotics.com 8 | 9 | wardrobe.sdf 10 | 11 | Wardrobe 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/models/floor_lamp/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | floor_lamp 4 | 0.1.0 5 | 6 | Joan Guasch 7 | guasch5000@pal-robotics.com 8 | 9 | floor_lamp.sdf 10 | 11 | floor_lamp 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/models/dining_chair/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | dining_chair 4 | 0.1.0 5 | 6 | Joan Guasch 7 | guasch5000@pal-robotics.com 8 | 9 | dining_chair.sdf 10 | 11 | dining_chair 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/models/kitchen_table/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | kitchen_table 4 | 0.1.0 5 | 6 | Joan Guasch 7 | guasch5000@pal-robotics.com 8 | 9 | kitchen_table.sdf 10 | 11 | kitchen_table 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/worlds/nao_test2.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://ground_plane 6 | 7 | 8 | model://sun 9 | 10 | 11 | model://gas_station 12 | gas_station 13 | 8.0 7.0 0 0 0 0 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/models/citizen_extras_male_03/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | citizen_extras_male_03 5 | 0.1.0 6 | 7 | Jordi Pages 8 | jordi.page@pal-robotics.com 9 | 10 | model.sdf 11 | 12 | 3D City People - Male 03 - iClone Citizen Extras downloaded from sketchup.google.com 13 | 14 | 15 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/models/citizen_extras_female_02/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | citizen_extras_female_02 5 | 0.1.0 6 | 7 | Jordi Pages 8 | jordi.page@pal-robotics.com 9 | 10 | model.sdf 11 | 12 | 3D City People - Female 02 - iClone Citizen Extras downloaded from sketchup.google.com 13 | 14 | 15 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/models/citizen_extras_female_03/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | citizen_extras_female_03 5 | 0.1.0 6 | 7 | Jordi Pages 8 | jordi.page@pal-robotics.com 9 | 10 | model.sdf 11 | 12 | 3D City People - Female 03 - iClone Citizen Extras downloaded from sketchup.google.com 13 | 14 | 15 | -------------------------------------------------------------------------------- /pepper_control/launch/pepper_control_no_wheels.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 13 | 14 | -------------------------------------------------------------------------------- /pepper_control/launch/pepper_control_trajectory_no_arms.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 13 | 14 | -------------------------------------------------------------------------------- /pepper_control/launch/pepper_control_trajectory_all.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 15 | 16 | -------------------------------------------------------------------------------- /pepper_control/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package pepper_control 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.0.4 (2017-11-16) 6 | ------------------ 7 | * removing wheels controllers 8 | * adding Pelvis controller 9 | * adding head controller 10 | * fixed wheels controllers names 11 | * fixing wheels controllers in position_control 12 | * ading a launch file for all controllers 13 | * fixing wheels controllers 14 | * Contributors: Natalia Lyubova 15 | 16 | 0.0.3 (2016-09-20) 17 | ------------------ 18 | * changes in the launch file 19 | * Contributors: nlyubova 20 | 21 | 0.0.2 (2016-07-06) 22 | ------------------ 23 | 24 | 0.0.1 (2016-07-06) 25 | ------------------ 26 | * adding pepper_control package 27 | * Contributors: nlyubova 28 | -------------------------------------------------------------------------------- /pepper_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | pepper_control 4 | 0.0.4 5 | Control for Pepper robot 6 | 7 | Mikael Arguedas 8 | Natalia Lyubova 9 | Natalia Lyubova 10 | 11 | Apache 2.0 12 | 13 | https://github.com/ros-naoqi/pepper_virtual/issues 14 | https://github.com/ros-naoqi/pepper_virtual.git 15 | 16 | controller_manager 17 | robot_state_publisher 18 | 19 | catkin 20 | 21 | -------------------------------------------------------------------------------- /pepper_control/launch/pepper_control_trajectory.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 17 | 18 | 23 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | pepper_gazebo_plugin 4 | 0.0.4 5 | Gazebo plugin for Pepper robot 6 | 7 | Natalia Lyubova 8 | Natalia Lyubova 9 | Mikael Arguedas 10 | 11 | Apache 2.0 12 | 13 | https://github.com/ros-naoqi/pepper_virtual/issues 14 | https://github.com/ros-naoqi/pepper_virtual.git 15 | 16 | gazebo_ros 17 | pepper_description 18 | 19 | gazebo_plugins 20 | gazebo_ros 21 | gazebo_ros_control 22 | pepper_control 23 | pepper_description 24 | ros_control 25 | ros_controllers 26 | 27 | catkin 28 | 29 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/models/citizen_extras_male_03/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 20 12 | 13 | 14 | 15 | 0.0 16 | 17 | 0.0 18 | 19 | 0.0 20 | 21 | 0.0 22 | 23 | 0.0 24 | 25 | 0.0 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 0 0 0 0 0 0 34 | 35 | 36 | 37 | 38 | 39 | model://citizen_extras_male_03/meshes/mesh.dae 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 0 0 0 0 0 0 50 | 51 | 52 | 53 | 54 | 55 | model://citizen_extras_male_03/meshes/mesh.dae 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/models/citizen_extras_female_02/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 20 12 | 13 | 14 | 15 | 0.0 16 | 17 | 0.0 18 | 19 | 0.0 20 | 21 | 0.0 22 | 23 | 0.0 24 | 25 | 0.0 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 0 0 0 0 0 0 34 | 35 | 36 | 37 | 38 | 39 | model://citizen_extras_female_02/meshes/mesh.dae 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 0 0 0 0 0 0 50 | 51 | 52 | 53 | 54 | 55 | model://citizen_extras_female_02/meshes/mesh.dae 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/models/citizen_extras_female_03/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 20 12 | 13 | 14 | 15 | 0.0 16 | 17 | 0.0 18 | 19 | 0.0 20 | 21 | 0.0 22 | 23 | 0.0 24 | 25 | 0.0 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 0 0 0 0 0 0 34 | 35 | 36 | 37 | 38 | 39 | model://citizen_extras_female_03/meshes/mesh.dae 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 0 0 0 0 0 0 50 | 51 | 52 | 53 | 54 | 55 | model://citizen_extras_female_03/meshes/mesh.dae 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/launch/pc_to_laserscan.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | target_frame: base_footprint # Leave disabled to output scan in pointcloud frame 10 | transform_tolerance: 0.01 11 | min_height: 0.0 12 | max_height: 1.0 13 | 14 | angle_min: -2.0943951023931953 # -M_PI/2 15 | angle_max: 2.0943951023931953 # M_PI/2 16 | angle_increment: 0.0001 # M_PI/360.0 17 | scan_time: 0.0 18 | range_min: 0.1 19 | range_max: 7.0 20 | use_inf: true 21 | inf_epsilon: 1.0 22 | 23 | # Concurrency level, affects number of pointclouds queued for processing and number of threads used 24 | # 0 : Detect number of cores 25 | # 1 : Single threaded 26 | # 2->inf : Parallelism level 27 | concurrency_level: 1 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/launch/pepper_gazebo_plugin_Y20_CPU_no_arms.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/scripts/arms_down.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Wait for the command topic to be there 4 | until rostopic info /pepper/LeftArm_controller/command > /dev/null 2>&1 && \ 5 | rostopic info /pepper/RightArm_controller/command > /dev/null 2>&1 6 | do 7 | echo "Waiting for controllers to be ready..." 8 | sleep 1.0 9 | done 10 | 11 | echo "Lowering arms." 12 | sleep 5.0 13 | 14 | rostopic pub --once /pepper/LeftArm_controller/command trajectory_msgs/JointTrajectory "header: 15 | seq: 0 16 | stamp: 17 | secs: 0 18 | nsecs: 0 19 | frame_id: '' 20 | joint_names: [LElbowRoll, LElbowYaw, LShoulderPitch, LShoulderRoll, LWristYaw] 21 | points: 22 | - positions: [-0.10, 0.0, 1.45, 0.10, -1.0] 23 | velocities: [] 24 | accelerations: [] 25 | effort: [] 26 | time_from_start: {secs: 1, nsecs: 0}" & 27 | 28 | 29 | 30 | rostopic pub --once /pepper/RightArm_controller/command trajectory_msgs/JointTrajectory "header: 31 | seq: 0 32 | stamp: 33 | secs: 0 34 | nsecs: 0 35 | frame_id: '' 36 | joint_names: [RElbowRoll, RElbowYaw, RShoulderPitch, RShoulderRoll, RWristYaw] 37 | points: 38 | - positions: [0.10, 0.0, 1.45, -0.10, 1.0] 39 | velocities: [] 40 | accelerations: [] 41 | effort: [] 42 | time_from_start: {secs: 1, nsecs: 0}" & 43 | 44 | wait -------------------------------------------------------------------------------- /pepper_gazebo_plugin/worlds/nao_test.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://ground_plane 6 | 7 | 8 | model://sun 9 | 10 | 11 | model://gas_station 12 | gas_station 13 | -2.0 7.0 0 0 0 0 14 | 15 | 23 | 31 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /pepper_control/launch/pepper_control_position.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/models/bench/bench.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 1 4 | 5 | 6 | 7 | 0 0 0 0 0 0 8 | 9 | 10 | model://bench/meshes/bench.stl 11 | 1 1 1 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 0 0 0 0 0 0 27 | 28 | 29 | model://bench/meshes/bench.stl 30 | 1 1 1 31 | 32 | 33 | 34 | 38 | 39 | 40 | 41 | 42 | 0.000000 43 | 0.000000 44 | 45 | 0 46 | 0 47 | 1 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/models/sofa/sofa.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 1 4 | 5 | 6 | 7 | 0 0 0 0 0 0 8 | 9 | 10 | model://sofa/meshes/sofa.stl 11 | 1 1 1 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 0 0 0 0 0 0 27 | 28 | 29 | model://sofa/meshes/sofa.stl 30 | 1 1 1 31 | 32 | 33 | 34 | 38 | 39 | 40 | 41 | 42 | 0.000000 43 | 0.000000 44 | 45 | 0 46 | 0 47 | 1 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/models/closet/closet.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 1 4 | 5 | 6 | 7 | 0 0 0 0 0 0 8 | 9 | 10 | model://closet/meshes/closet.stl 11 | 1 1 1 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 0 0 0 0 0 0 27 | 28 | 29 | model://closet/meshes/closet.stl 30 | 1 1 1 31 | 32 | 33 | 34 | 38 | 39 | 40 | 41 | 42 | 0.000000 43 | 0.000000 44 | 45 | 0 46 | 0 47 | 1 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/models/wardrobe/wardrobe.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 1 4 | 5 | 6 | 7 | 0 0 0 0 0 0 8 | 9 | 10 | model://wardrobe/meshes/wardrobe.stl 11 | 1 1 1 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 0 0 0 0 0 0 27 | 28 | 29 | model://wardrobe/meshes/wardrobe.stl 30 | 1 1 1 31 | 32 | 33 | 34 | 38 | 39 | 40 | 41 | 42 | 0.000000 43 | 0.000000 44 | 45 | 0 46 | 0 47 | 1 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/models/floor_lamp/floor_lamp.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 1 4 | 5 | 6 | 7 | 0 0 0 0 0 0 8 | 9 | 10 | model://floor_lamp/meshes/floor_lamp.stl 11 | 1 1 1 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 0 0 0 0 0 0 27 | 28 | 29 | model://floor_lamp/meshes/floor_lamp.stl 30 | 1 1 1 31 | 32 | 33 | 34 | 38 | 39 | 40 | 41 | 42 | 0.000000 43 | 0.000000 44 | 45 | 0 46 | 0 47 | 1 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/launch/pepper_gazebo_plugin_in_office_CPU_no_arms.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 19 | 20 | 21 | 22 | 24 | 25 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/models/dining_chair/dining_chair.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 1 4 | 5 | 6 | 7 | 0 0 0 0 0 0 8 | 9 | 10 | model://dining_chair/meshes/dining_chair.stl 11 | 1 1 1 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 0 0 0 0 0 0 27 | 28 | 29 | model://dining_chair/meshes/dining_chair.stl 30 | 1 1 1 31 | 32 | 33 | 34 | 38 | 39 | 40 | 41 | 42 | 0.000000 43 | 0.000000 44 | 45 | 0 46 | 0 47 | 1 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/models/kitchen_table/kitchen_table.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 1 4 | 5 | 6 | 7 | 0 0 0 0 0 0 8 | 9 | 10 | model://kitchen_table/meshes/kitchen_table.stl 11 | 1 1 1 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 0 0 0 0 0 0 27 | 28 | 29 | model://kitchen_table/meshes/kitchen_table.stl 30 | 1 1 1 31 | 32 | 33 | 34 | 38 | 39 | 40 | 41 | 42 | 0.000000 43 | 0.000000 44 | 45 | 0 46 | 0 47 | 1 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/launch/pepper_gazebo_plugin_Y20.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 20 | 21 | 22 | 23 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/launch/pepper_gazebo_plugin_Y20_CPU.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 20 | 21 | 22 | 23 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/scripts/convertTrajMsg.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (C) 2014 Aldebaran Robotics 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | # 17 | 18 | # authors: Mikael Arguedas [mikael DOT arguedas AT gmail DOT com] 19 | #FIXME Add : Fingers for Humanoids and Sensors 20 | 21 | import sys 22 | import argparse 23 | import subprocess 24 | import os 25 | import math 26 | from os import path 27 | 28 | parser = argparse.ArgumentParser(usage='Load an URDF file') 29 | parser.add_argument('-i','--input', default=None, help='file containing the trajectory (rostopic echo controller/follow_joint_trajectory/goal)') 30 | 31 | 32 | args = parser.parse_args() 33 | if os.path.isfile(args.input): 34 | output = args.input[0:args.input.rfind('.')] + '_modified' + args.input[args.input.rfind('.'):] 35 | print output 36 | file = open(args.input,'r') 37 | lines = file.readlines() 38 | outfile = open(output,'w+') 39 | for i in range(12,len(lines)-6): 40 | outfile.write(lines[i][2:]) 41 | file.close() 42 | outfile.close() 43 | else: 44 | print "input file doesn't exist" 45 | 46 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/launch/pepper_gazebo_plugin_in_tiago_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 22 | 23 | 24 | 25 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/launch/pepper_gazebo_plugin_in_office.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 22 | 23 | 24 | 25 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/launch/pepper_gazebo_plugin_in_office_CPU.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 22 | 23 | 24 | 25 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package pepper_gazebo_plugin 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.0.4 (2017-11-16) 6 | ------------------ 7 | * Merge pull request `#13 `_ from nlyubova/master 8 | updating README 9 | * removing ros-indigo-humanoid-nav-msgs from README 10 | * updating README 11 | * Update README.rst 12 | * Merge pull request `#9 `_ from ros-naoqi/mikaelarguedas-clone-using-https-in-readme 13 | [pepper_gazebo_plugin/README.rst] clone using https 14 | * [pepper_gazebo_plugin/README.rst] clone using https 15 | * Merge pull request `#5 `_ from kochigami/add-trajectory-all 16 | [pepper_gazebo_plugin] enable to launch pepper_control_position_all.launch 17 | * [pepper_gazebo_plugin] enable to launch pepper_control_position_all.launch 18 | * Contributors: Kanae Kochigami, Mikael Arguedas, Natalia Lyubova 19 | 20 | 0.0.3 (2016-09-20) 21 | ------------------ 22 | * Update README.rst 23 | * Update package.xml 24 | * [doc] Add note for the dependency that needs manually added. 25 | * Contributors: Isaac I.Y. Saito, Natalia Lyubova 26 | 27 | 0.0.2 (2016-07-06) 28 | ------------------ 29 | * fixing install in CMakeLists 30 | * updating README 31 | * Contributors: nlyubova 32 | 33 | 0.0.1 (2016-07-06) 34 | ------------------ 35 | * moving pepper_gazebo_plugin to pepper_virtual 36 | * updating the dependency on pepper_dcm_control 37 | * updating README 38 | * updating the dependency on pepper_dcm_control package 39 | * removing unused launch file 40 | * initial commit 41 | * Contributors: nlyubova 42 | -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | # Base image 2 | FROM osrf/ros:kinetic-desktop-full 3 | 4 | MAINTAINER Finn Rietz <5rietz@informatik.uni-hamburg.de> 5 | 6 | # (RUN is executed when image is build) 7 | RUN apt-get update 8 | RUN apt-get install -y vim wget libprotobuf-dev protobuf-compiler 9 | 10 | # packages required for Pepper Gazebo simulation 11 | RUN apt-get install -y ros-kinetic-tf2-sensor-msgs ros-kinetic-ros-control ros-kinetic-ros-controllers ros-kinetic-gazebo-ros ros-kinetic-gazebo-ros-control ros-kinetic-gazebo-plugins ros-kinetic-controller-manager ros-kinetic-ddynamic-reconfigure-python 12 | 13 | # install pepper meshes. We have to pipe this into 'yes' to agree to the license. otherwise docker build get's stuck on this step... 14 | # we also have these debian environment params, otherwise the yes still gets stuck on the prompt in the mesh installation 15 | ENV DEBIAN_FRONTEND noninteractive 16 | ENV DEBIAN_FRONTEND teletype 17 | RUN yes | apt-get install ros-kinetic-pepper-meshes 18 | 19 | # Clone the required repos into a catkin workspace, which we than compile 20 | RUN git clone -b correct_chain_model_and_gazebo_enabled https://github.com/awesomebytes/pepper_robot /catkin_ws/src/pepper_robot 21 | RUN git clone -b simulation_that_works https://github.com/awesomebytes/pepper_virtual /catkin_ws/src/pepper_virtual 22 | RUN git clone https://github.com/awesomebytes/gazebo_model_velocity_plugin /catkin_ws/src/gazebo_model_velocity_plugin 23 | 24 | # build the catkin_ws inside the container 25 | RUN /bin/bash -c '. /opt/ros/kinetic/setup.bash && cd /catkin_ws && catkin_make' 26 | 27 | # we add these two commands to the bashrc in the container, so that the entrypoint and workspacea will be sourced, 28 | # whenever a new bash session is instantiated in the container 29 | RUN echo 'source /ros_entrypoint.sh' >> /root/.bashrc 30 | RUN echo 'source /catkin_ws/devel/setup.bash' >> /root/.bashrc 31 | -------------------------------------------------------------------------------- /pepper_control/config/pepper_position_control.yaml: -------------------------------------------------------------------------------- 1 | pepper_dcm: 2 | # Publish all joint states ----------------------------------- 3 | joint_state_controller: 4 | type: joint_state_controller/JointStateController 5 | publish_rate: 50 6 | 7 | # Controllers ------------------------------------------------ 8 | HeadYaw_position_controller: 9 | type: position_controllers/JointPositionController 10 | joint: HeadYaw 11 | 12 | HeadPitch_position_controller: 13 | type: position_controllers/JointPositionController 14 | joint: HeadPitch 15 | 16 | LShoulderPitch_position_controller: 17 | type: position_controllers/JointPositionController 18 | joint: LShoulderPitch 19 | 20 | LShoulderRoll_position_controller: 21 | type: position_controllers/JointPositionController 22 | joint: LShoulderRoll 23 | 24 | LElbowYaw_position_controller: 25 | type: position_controllers/JointPositionController 26 | joint: LElbowYaw 27 | 28 | LElbowRoll_position_controller: 29 | type: position_controllers/JointPositionController 30 | joint: LElbowRoll 31 | 32 | LWristYaw_position_controller: 33 | type: position_controllers/JointPositionController 34 | joint: LWristYaw 35 | 36 | LHand_position_controller: 37 | type: position_controllers/JointPositionController 38 | joint: LHand 39 | 40 | RShoulderPitch_position_controller: 41 | type: position_controllers/JointPositionController 42 | joint: RShoulderPitch 43 | 44 | RShoulderRoll_position_controller: 45 | type: position_controllers/JointPositionController 46 | joint: RShoulderRoll 47 | 48 | RElbowYaw_position_controller: 49 | type: position_controllers/JointPositionController 50 | joint: RElbowYaw 51 | 52 | RWristYaw_position_controller: 53 | type: position_controllers/JointPositionController 54 | joint: RWristYaw 55 | 56 | RElbowRoll_position_controller: 57 | type: position_controllers/JointPositionController 58 | joint: RElbowRoll 59 | 60 | RHand_position_controller: 61 | type: position_controllers/JointPositionController 62 | joint: RHand 63 | 64 | HipRoll_position_controller: 65 | type: position_controllers/JointPositionController 66 | joint: HipRoll 67 | 68 | HipPitch_position_controller: 69 | type: position_controllers/JointPositionController 70 | joint: HipPitch 71 | 72 | KneePitch_position_controller: 73 | type: position_controllers/JointPositionController 74 | joint: KneePitch 75 | 76 | # Effort Controllers --------------------------- 77 | WheelFL_controller: 78 | type: effort_controllers/JointEffortController 79 | joint: WheelFL 80 | WheelFR_controller: 81 | type: effort_controllers/JointEffortController 82 | joint: WheelFR 83 | WheelB_controller: 84 | type: effort_controllers/JointEffortController 85 | joint: WheelB 86 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/README.rst: -------------------------------------------------------------------------------- 1 | pepper_gazebo_plugin 2 | ================= 3 | 4 | Package developed against ROS Indigo and default gazebo (gazebo 2.2.3) 5 | 6 | Dependencies 7 | ------------ 8 | 9 | The package requires Gazebo and robot-specific packages, such as pepper_description, pepper_meshes, and pepper_control packages 10 | 11 | NOTE: `pepper_meshes` needs to be **manually** installed; `apt`, `rosdep` or any package management tools does not handle the dependency to it. See `a relevant discussion `_. 12 | 13 | .. code-block:: bash 14 | 15 | sudo apt-get install ros-indigo-pepper-description ros-indigo-pepper-meshes ros-indigo-pepper-control 16 | 17 | Other plugins to fetch and compile: 18 | 19 | .. code-block:: bash 20 | 21 | git clone https://github.com/roboticsgroup/roboticsgroup_gazebo_plugins.git 22 | git clone https://github.com/pal-robotics/pal_msgs.git 23 | git clone https://github.com/pal-robotics/pal_gazebo_plugins.git 24 | catkin_make 25 | 26 | Please also make sure that the package and all the dependencies are up to date 27 | 28 | .. code-block:: bash 29 | 30 | sudo apt-get update 31 | sudo apt-get install ros-indigo-pepper-gazebo-plugin 32 | 33 | If you prefer to install from source, then 34 | 35 | * install additional dependencies: 36 | 37 | .. code-block:: bash 38 | 39 | sudo apt-get install ros-indigo-gazebo-ros ros-indigo-gazebo-ros-pkgs ros-indigo-gazebo-ros-control ros-indigo-effort-controllers ros-indigo-joint-trajectory-controller 40 | 41 | * clone the code and compile 42 | 43 | .. code-block:: bash 44 | 45 | cd src 46 | git clone https://github.com/ros-naoqi/pepper_virtual.git 47 | cd .. 48 | catkin_make 49 | 50 | 51 | How to run it 52 | ------------- 53 | 54 | .. code-block:: bash 55 | 56 | roslaunch pepper_gazebo_plugin pepper_gazebo_plugin_Y20.launch 57 | 58 | 59 | This will spawn gazebo with Pepper on a robocup field. 60 | The ball has the same specs as the official RoboCup ball (size and mass). 61 | 62 | The simulation will be in pause mode to allow initialization of all the controllers. 63 | Wait until eveything is successfully loaded: 64 | 65 | .. code-block:: bash 66 | 67 | [INFO] [WallTime: 1413899465.061789] [0.000000] Controller Spawner: Loaded controllers: /pepper_dcm/Head_controller, /pepper_dcm/RightArm_controller, /pepper_dcm/LeftArm_controller, /pepper_dcm/LeftLeg_controller, /pepper_dcm/RightLeg_controller, /pepper_dcm/RightHand_controller, /pepper_dcm/LeftHand_controller, /pepper_dcm/joint_state_controller 68 | 69 | 70 | Click the Play button. 71 | 72 | Your Pepper should be standing in front of the ball at the center of the field. 73 | 74 | 75 | Get sensor data from gazebo 76 | --------------------------- 77 | 78 | All the sensors are simulated using plugins. These plugins are included in the robot description via pepperGazebo.xacro file. 79 | Each sensor publish data on rostopics. 80 | 81 | We can visualize topics using Ctrl+T or Window/Topic Visualization 82 | 83 | .. image:: images/TopicVisu.png 84 | :width: 100% 85 | 86 | For example, visualizing Cameras and sonar 87 | 88 | .. image:: images/GazeboCamSonar.png 89 | :width: 100% 90 | 91 | 92 | We can also visualize these messages using Rviz plugins 93 | 94 | .. image:: images/MoveitCamSonar.png 95 | :width: 100% 96 | 97 | 98 | How to interact with simulated robot 99 | ------------------------------------ 100 | 101 | Using MoveIt!: 102 | 103 | To control your simulated robot using MoveIt, run: 104 | 105 | .. code-block:: bash 106 | 107 | roslaunch pepper_moveit_config moveit_planner.launch 108 | 109 | 110 | Then you can control the robot with MoveIt!, check the tutorial https://github.com/ros-naoqi/pepper_moveit_config/blob/master/README.rst 111 | -------------------------------------------------------------------------------- /pepper_control/config/pepper_trajectory_control.yaml: -------------------------------------------------------------------------------- 1 | pepper: 2 | # Publish all joint states ----------------------------------- 3 | joint_state_controller: 4 | type: joint_state_controller/JointStateController 5 | publish_rate: 50 6 | 7 | # Controllers ------------------------------------------------ 8 | # LeftArm_controller: 9 | # type: position_controllers/JointTrajectoryController 10 | # joints: 11 | # - LShoulderPitch 12 | # - LShoulderRoll 13 | # - LElbowYaw 14 | # - LElbowRoll 15 | # - LWristYaw 16 | # RightArm_controller: 17 | # type: position_controllers/JointTrajectoryController 18 | # joints: 19 | # - RShoulderPitch 20 | # - RShoulderRoll 21 | # - RElbowYaw 22 | # - RElbowRoll 23 | # - RWristYaw 24 | # Head_controller: 25 | # type: position_controllers/JointTrajectoryController 26 | # joints: 27 | # - HeadYaw 28 | # - HeadPitch 29 | # Pelvis_controller: 30 | # type: position_controllers/JointTrajectoryController 31 | # joints: 32 | # - HipRoll 33 | # - HipPitch 34 | # - KneePitch 35 | # LeftHand_controller: 36 | # type: position_controllers/JointTrajectoryController 37 | # joints: 38 | # - LHand 39 | # RightHand_controller: 40 | # type: position_controllers/JointTrajectoryController 41 | # joints: 42 | # - RHand 43 | 44 | # Effort Controllers --------------------------------------- 45 | # WheelFL_controller: 46 | # type: effort_controllers/JointEffortController 47 | # joint: WheelFL 48 | # WheelFR_controller: 49 | # type: effort_controllers/JointEffortController 50 | # joint: WheelFR 51 | # WheelB_controller: 52 | # type: effort_controllers/JointEffortController 53 | # joint: WheelB 54 | 55 | 56 | # gazebo_ros_control: 57 | # pid_gains: 58 | # HeadYaw: {p: 1000.0, i: 0.01, d: 10.0} 59 | # HeadPitch: {p: 1000.0, i: 0.01, d: 10.0} 60 | # HipRoll: {p: 1000.0, i: 0.01, d: 10.0} 61 | # HipPitch: {p: 1000.0, i: 0.01, d: 10.0} 62 | # KneePitch: {p: 1000.0, i: 0.01, d: 10.0} 63 | 64 | # LShoulderPitch: {p: 1000.0, i: 0.01, d: 10.0} 65 | # LShoulderRoll: {p: 1000.0, i: 0.01, d: 10.0} 66 | # LElbowYaw: {p: 1000.0, i: 0.01, d: 10.0} 67 | # LElbowRoll: {p: 1000.0, i: 0.01, d: 10.0} 68 | # LWristYaw: {p: 1000.0, i: 0.01, d: 10.0} 69 | 70 | # RShoulderPitch: {p: 1000.0, i: 0.01, d: 10.0} 71 | # RShoulderRoll: {p: 1000.0, i: 0.01, d: 10.0} 72 | # RElbowYaw: {p: 1000.0, i: 0.01, d: 10.0} 73 | # RElbowRoll: {p: 1000.0, i: 0.01, d: 10.0} 74 | # RWristYaw: {p: 1000.0, i: 0.01, d: 10.0} 75 | 76 | 77 | 78 | LeftArm_controller: 79 | type: velocity_controllers/JointTrajectoryController 80 | joints: 81 | - LShoulderPitch 82 | - LShoulderRoll 83 | - LElbowYaw 84 | - LElbowRoll 85 | - LWristYaw 86 | gains: 87 | LShoulderPitch: {p: 1000.0, i: 0.01, d: 10.0} 88 | LShoulderRoll: {p: 1000.0, i: 0.01, d: 10.0} 89 | LElbowYaw: {p: 1000.0, i: 0.01, d: 10.0} 90 | LElbowRoll: {p: 1000.0, i: 0.01, d: 10.0} 91 | LWristYaw: {p: 1000.0, i: 0.01, d: 10.0} 92 | RightArm_controller: 93 | type: velocity_controllers/JointTrajectoryController 94 | joints: 95 | - RShoulderPitch 96 | - RShoulderRoll 97 | - RElbowYaw 98 | - RElbowRoll 99 | - RWristYaw 100 | gains: 101 | RShoulderPitch: {p: 1000.0, i: 0.01, d: 10.0} 102 | RShoulderRoll: {p: 1000.0, i: 0.01, d: 10.0} 103 | RElbowYaw: {p: 1000.0, i: 0.01, d: 10.0} 104 | RElbowRoll: {p: 1000.0, i: 0.01, d: 10.0} 105 | RWristYaw: {p: 1000.0, i: 0.01, d: 10.0} 106 | Head_controller: 107 | type: velocity_controllers/JointTrajectoryController 108 | joints: 109 | - HeadYaw 110 | - HeadPitch 111 | gains: 112 | HeadYaw: {p: 100000.0, i: 0.01, d: 10.0} 113 | HeadPitch: {p: 100000.0, i: 0.01, d: 10.0} 114 | Pelvis_controller: 115 | type: velocity_controllers/JointTrajectoryController 116 | joints: 117 | - HipRoll 118 | - HipPitch 119 | - KneePitch 120 | gains: 121 | HipRoll: {p: 100000.0, i: 0.01, d: 10.0} 122 | HipPitch: {p: 100000.0, i: 0.01, d: 10.0} 123 | KneePitch: {p: 100000.0, i: 0.01, d: 10.0} 124 | 125 | 126 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/testfiles/trajBack_modified.txt: -------------------------------------------------------------------------------- 1 | trajectory: 2 | header: 3 | seq: 0 4 | stamp: 5 | secs: 0 6 | nsecs: 0 7 | frame_id: /base_link 8 | joint_names: ['LElbowRoll', 'LElbowYaw', 'LShoulderPitch', 'LShoulderRoll'] 9 | points: 10 | - 11 | positions: [-0.07724205088025382, 0.010846075758542462, -0.6355002718368405, 0.053354730753871316] 12 | velocities: [0.0, 0.0, 0.0, 0.0] 13 | accelerations: [0.0, 0.0, 2.9041990290887116, 0.0] 14 | effort: [] 15 | time_from_start: 16 | secs: 0 17 | nsecs: 0 18 | - 19 | positions: [-0.07253811189355895, 0.010139104226287644, -0.5641118176967026, 0.04769509947817775] 20 | velocities: [0.03634740024319942, -0.005462778602379165, 0.5516195517659253, -0.04373204750070621] 21 | accelerations: [0.19332158938765642, -0.029054981506756162, 2.93390910412218, -0.2325984492273241] 22 | effort: [] 23 | time_from_start: 24 | secs: 0 25 | nsecs: 221725579 26 | - 27 | positions: [-0.0678341729068641, 0.009432132694032827, -0.4927233635565647, 0.04203546820248418] 28 | velocities: [0.05893176273812148, -0.008857061862258738, 0.8943669238754044, -0.07090484133995442] 29 | accelerations: [0.18373666964274812, -0.027614430211816764, 2.788445354362267, -0.221066175590963] 30 | effort: [] 31 | time_from_start: 32 | secs: 0 33 | nsecs: 313100286 34 | - 35 | positions: [-0.06313023392016923, 0.00872516116177801, -0.4213349094164268, 0.03637583692679061] 36 | velocities: [0.0721120826182598, -0.01083797848716106, 1.0943955943103347, -0.08676298721052614] 37 | accelerations: [0.17452068123372091, -0.02622932690473776, 2.6485806223250634, -0.20997778852138763] 38 | effort: [] 39 | time_from_start: 40 | secs: 0 41 | nsecs: 383959960 42 | - 43 | positions: [-0.05842629493347437, 0.008018189629523192, -0.34994645527628887, 0.030716205651097048] 44 | velocities: [0.08277635053951654, -0.012440748815175926, 1.2562398706897993, -0.09959389858719095] 45 | accelerations: [0.17310397017896006, -0.026016404418289264, 2.6270801707993794, -0.2082732463883267] 46 | effort: [] 47 | time_from_start: 48 | secs: 0 49 | nsecs: 444390597 50 | - 51 | positions: [-0.0537223559467795, 0.0073112180972683734, -0.2785580011361509, 0.025056574375403477] 52 | velocities: [0.08316555204583133, -0.01249924327823199, 1.2621465148795745, -0.10006217358478686] 53 | accelerations: [-0.1602957952726974, 0.02409141877020179, -2.4326992892657064, 0.19286285363258227] 54 | effort: [] 55 | time_from_start: 56 | secs: 0 57 | nsecs: 498019719 58 | - 59 | positions: [-0.04901841696008464, 0.006604246565013556, -0.20716954699601303, 0.019396943099709914] 60 | velocities: [0.07283320344444247, -0.01094635827203267, 1.1053395502534378, -0.08763061708263141] 61 | accelerations: [-0.17802854692652184, 0.02675653637552318, -2.7018170928337883, 0.21419834207066918] 62 | effort: [] 63 | time_from_start: 64 | secs: 0 65 | nsecs: 557852033 66 | - 67 | positions: [-0.04431447797338977, 0.005897275032758739, -0.13578109285587509, 0.013737311824016343] 68 | velocities: [0.059185895838940586, -0.008895256419669227, 0.8982237275594305, -0.07121060628496177] 69 | accelerations: [-0.1943468952817073, 0.029209078335179186, -2.949469468107331, 0.23383206499519957] 70 | effort: [] 71 | time_from_start: 72 | secs: 0 73 | nsecs: 628010127 74 | - 75 | positions: [-0.039610538986694914, 0.005190303500503922, -0.0643926387157372, 0.00807768054832278] 76 | velocities: [0.036240211139808826, -0.00544666877508774, 0.5499928162973391, -0.043603080946573815] 77 | accelerations: [-0.19215524968788006, 0.02887968821170856, -2.9162083668454377, 0.23119514602513302] 78 | effort: [] 79 | time_from_start: 80 | secs: 0 81 | nsecs: 719661807 82 | - 83 | positions: [-0.034906600000000045, 0.004483331968249103, 0.006995815424400778, 0.0024180492726292102] 84 | velocities: [0.0, 0.0, 0.0, 0.0] 85 | accelerations: [-0.19030452409027476, 0.02860153615335417, -2.8881211744255593, 0.22896841125995765] 86 | effort: [] 87 | time_from_start: 88 | secs: 0 89 | nsecs: 942003691 90 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/testfiles/traj_modified.txt: -------------------------------------------------------------------------------- 1 | trajectory: 2 | header: 3 | seq: 0 4 | stamp: 5 | secs: 0 6 | nsecs: 0 7 | frame_id: /base_link 8 | joint_names: ['LElbowRoll', 'LElbowYaw', 'LShoulderPitch', 'LShoulderRoll'] 9 | points: 10 | - 11 | positions: [-0.037175756763097034, 0.0012583609645888671, 0.00703808603645939, -0.0004088266224000492] 12 | velocities: [0.0, 0.0, 0.0, 0.0] 13 | accelerations: [0.0, 0.0, -2.834163700478893, 0.0] 14 | effort: [] 15 | time_from_start: 16 | secs: 0 17 | nsecs: 0 18 | - 19 | positions: [-0.041682635405575055, -0.0006184850363736194, -0.06565363413334838, 0.005273382575327419] 20 | velocities: [-0.03454733408061231, -0.014386902989129802, -0.5572160559042572, 0.04355679285872327] 21 | accelerations: [-0.18419856553504027, -0.07670771026516579, -2.9709498843286144, 0.23223496045057274] 22 | effort: [] 23 | time_from_start: 24 | secs: 0 25 | nsecs: 226487900 26 | - 27 | positions: [-0.04618951404805307, -0.002495331037336106, -0.13834535430315614, 0.010955591773054886] 28 | velocities: [-0.05614143324219118, -0.02337955663500494, -0.9055085967279214, 0.070782329334473] 29 | accelerations: [-0.17039535016882706, -0.07095949478930876, -2.7483169828372476, 0.21483206067579966] 30 | effort: [] 31 | time_from_start: 32 | secs: 0 33 | nsecs: 318099156 34 | - 35 | positions: [-0.050696392690531084, -0.004372177038298592, -0.21103707447296394, 0.016637800970782355] 36 | velocities: [-0.06853093034648777, -0.02853904282018209, -1.1053395502534378, 0.08640283301028169] 37 | accelerations: [-0.16450908786517326, -0.06850821780991417, -2.653377099568452, 0.2074107439607617] 38 | effort: [] 39 | time_from_start: 40 | secs: 0 41 | nsecs: 389538055 42 | - 43 | positions: [-0.055203271333009105, -0.006249023039261079, -0.2837287946427717, 0.022320010168509822] 44 | velocities: [-0.07866560092557427, -0.03275952831720719, -1.268802269396697, 0.09918048311997268] 45 | accelerations: [-0.1631736481812247, -0.06795208687558661, -2.6318377115552076, 0.20572704039182077] 46 | effort: [] 47 | time_from_start: 48 | secs: 0 49 | nsecs: 450462666 50 | - 51 | positions: [-0.059710149975487126, -0.008125869040223567, -0.3564205148125795, 0.028002219366237296] 52 | velocities: [-0.07903547432752811, -0.03291355851646233, -1.2747679800283702, 0.09964681430243866] 53 | accelerations: [0.15110022996998235, 0.06292422868696142, 2.43710481375026, -0.19050504453833808] 54 | effort: [] 55 | time_from_start: 56 | secs: 0 57 | nsecs: 504530165 58 | - 59 | positions: [-0.06421702861796513, -0.010002715041186052, -0.42911223498238726, 0.03368442856396476] 60 | velocities: [-0.06921623964995272, -0.028824433248383914, -1.116392945755972, 0.08726686134038456] 61 | accelerations: [0.16781572053125443, 0.06988523298789252, 2.706709979269775, -0.2115796999143699] 62 | effort: [] 63 | time_from_start: 64 | secs: 0 65 | nsecs: 564851562 66 | - 67 | positions: [-0.06872390726044317, -0.011879561042148538, -0.501803955152195, 0.03936663776169223] 68 | velocities: [-0.05627429692833494, -0.023434886431476842, -0.9076515632153817, 0.07094984200107418] 69 | accelerations: [0.18263759016302034, 0.07605765717587865, 2.9457728174633764, -0.23026690465853614] 70 | effort: [] 71 | time_from_start: 72 | secs: 0 73 | nsecs: 635583144 74 | - 75 | positions: [-0.07323078590292117, -0.013756407043111025, -0.5744956753220027, 0.04504884695941969] 76 | velocities: [-0.034531732288200466, -0.014380405773646797, -0.5569644136440337, 0.04353712233830855] 77 | accelerations: [0.18154563570786722, 0.07560292330905967, 2.9281606175377624, -0.2288901838411453] 78 | effort: [] 79 | time_from_start: 80 | secs: 0 81 | nsecs: 727879451 82 | - 83 | positions: [-0.0777376645453992, -0.015633253044073512, -0.6471873954918106, 0.05073105615714717] 84 | velocities: [0.0, 0.0, 0.0, 0.0] 85 | accelerations: [0.18166504958951835, 0.07565265206459398, 2.9300866513092347, -0.22904073918342982] 86 | effort: [] 87 | time_from_start: 88 | secs: 0 89 | nsecs: 950629208 90 | -------------------------------------------------------------------------------- /start_docker.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | # This scripts accepts a special first parameter --devel which mounts the current folder into the container so changes 4 | # from inside or outside the container affect the repository 5 | 6 | # This needs 7 | # sudo apt-get install x11-xserver-utils 8 | # and it's needed for the docker container to be able to use the host X 9 | xhost +local:root 10 | 11 | USE_NVIDIA=false 12 | 13 | 14 | # Is there an nvidia card in the system? 15 | if [[ $(lshw -C display 2> /dev/null | grep -i NVIDIA) ]]; then 16 | echo "Nvidia GPU in the system, trying to use it." 17 | # If we have nvidia-container-runtime... 18 | if nvidia-container-runtime -v > /dev/null; then 19 | # Check if we have an nvidia driver 20 | NVIDIA_DRIVER_VERSION=`nvidia-smi --query-gpu=driver_version --format=csv,noheader | awk '{ print substr($0, 0, 4) }'` 21 | if [ -z "$NVIDIA_DRIVER_VERSION" ]; then 22 | echo "No Nvidia driver version found, but nvidia-container-runtime is installed... can you run 'nvidia-smi?" 23 | echo "You may want to install a driver to take advantage of it." 24 | else 25 | USE_NVIDIA=true 26 | fi 27 | else 28 | echo "Warning:" 29 | echo "The system has a Nvidia GPU but doesn't have nvidia-container-runtime installed" 30 | echo "You may want to install it to take advantage of it, e.g.:" 31 | echo "sudo apt-get install nvidia-container-runtime" 32 | echo "You may need to add the nvidia repository, instructions here: https://nvidia.github.io/nvidia-container-runtime/" 33 | fi 34 | if ! "$USE_NVIDIA"; then 35 | echo "Using non-nvidia alternative." 36 | fi 37 | fi 38 | 39 | 40 | # If the user wants to develop on pepper_virtual (flag --devel) mount the folder of this script as it 41 | if [ "$#" -ge "1" ]; then 42 | if [ "$1" == "--devel" ]; then 43 | # Remove this argument from "$@" so we can forward other arguments 44 | shift 45 | 46 | # Get script path 47 | # Absolute path to this script 48 | SCRIPT=$(readlink -f $0) 49 | # Absolute path this script is in 50 | SCRIPTPATH=`dirname $SCRIPT` 51 | REPOSITORY_PATH=`realpath $SCRIPTPATH` 52 | # Create the --volume command 53 | 54 | MOUNT_LOCAL_REPOSITORY_VOLUME="--volume $REPOSITORY_PATH:/catkin_ws/src/pepper_virtual:rw" 55 | 56 | echo 57 | echo " *** DEVELOPER MODE ***" 58 | echo " Your current repository folder ($REPOSITORY_PATH) is mounted in the docker image" 59 | echo " This means you can develop editing the files directly on that path." 60 | echo " It also means that the repository folder from inside of the docker image is overwritten by yours." 61 | echo 62 | fi 63 | fi 64 | 65 | 66 | XSOCK=/tmp/.X11-unix 67 | XAUTH=/tmp/.docker.xauth 68 | 69 | 70 | # Compose the long list of .so files from Nvidia to make X work (yeah, this is absurd) 71 | # This provides a list of all .so files installed from packages from 'nvidia-'' 72 | NVIDIA_SOS=$(dpkg -l | grep nvidia- | awk '{print $2}' | xargs dpkg-query -L | grep lib | grep .so) 73 | # This generates --volume /path/to/.so:/path/to/.so lines for every .so 74 | for so in $NVIDIA_SOS; do DOCKER_NVIDIA_SO_VOLUMES+="--volume $so:$so "; done 75 | 76 | # ram, also QT_X11_NO_XRENDER may help with some glitches, so leaving it here in case 77 | # we want to use it (but it may consume more cpu/bring other glitches) 78 | 79 | if "$USE_NVIDIA"; then 80 | # If executing this script from an unmanned shell (like from a piece of code that doesn't have a terminal starting it) 81 | # We can't use "-i" 82 | if [ -t 1 ]; then 83 | TERMINAL_FLAGS='-it' 84 | else 85 | TERMINAL_FLAGS='-t' 86 | fi 87 | docker run $TERMINAL_FLAGS \ 88 | $DOCKER_NVIDIA_SO_VOLUMES \ 89 | --volume $XSOCK:$XSOCK:rw \ 90 | $MOUNT_LOCAL_REPOSITORY_VOLUME \ 91 | --env DISPLAY=$DISPLAY \ 92 | --env QT_X11_NO_XRENDER=0 \ 93 | --env ROS_MASTER_URI=$ROS_MASTER_URI \ 94 | --env ROS_IP \ 95 | --net host \ 96 | --pid host \ 97 | --privileged \ 98 | --name=pepper-virtual-container \ 99 | frietz58/pepper-virtual:with-gazebo-files "$@" 100 | # Note: no --rm provided so the container won't disappear 101 | fi 102 | # --env XAUTHORITY=$XAUTH \ 103 | # --volume $XAUTH:$XAUTH:rw \ 104 | 105 | xhost -local:root 106 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/testfiles/coucou/coucou2_modified.txt: -------------------------------------------------------------------------------- 1 | trajectory: 2 | header: 3 | seq: 0 4 | stamp: 5 | secs: 0 6 | nsecs: 0 7 | frame_id: /base_link 8 | joint_names: ['LElbowRoll', 'LElbowYaw', 'LShoulderPitch', 'LShoulderRoll'] 9 | points: 10 | - 11 | positions: [-1.3601664483828344, -0.26150438081626426, -1.1091288657547782, 1.326529204847227] 12 | velocities: [0.0, 0.0, 0.0, 0.0] 13 | accelerations: [0.0, 0.0, 0.0, 0.0] 14 | effort: [] 15 | time_from_start: 16 | secs: 0 17 | nsecs: 0 18 | - 19 | positions: [-1.3601664483828344, -0.26150438081626426, -1.1091288657547782, 1.32645] 20 | velocities: [0.25786068244393545, 0.0012437661204819789, -0.024734568643556592, -0.0003515264009807792] 21 | accelerations: [2.9206051748428474, 0.014087257248160257, -0.28015092682298914, 0.003493332577957609] 22 | effort: [] 23 | time_from_start: 24 | secs: 0 25 | nsecs: 120015840 26 | - 27 | positions: [-1.2399287055276205, -0.26092442566548607, -1.120662336759398, 1.3264399515702363] 28 | velocities: [0.7762558971816609, 0.003744195418193728, -0.07446018753937098, -6.487274857543432e-05] 29 | accelerations: [2.9850778768507524, 0.014398235105247032, -0.28633529141218683, -0.0002494669699666938] 30 | effort: [] 31 | time_from_start: 32 | secs: 0 33 | nsecs: 353160618 34 | - 35 | positions: [-1.1196909626724063, -0.2603444705147079, -1.132195807764018, 1.3264299031404725] 36 | velocities: [1.1777345207941157, 0.005680688820034763, -0.1129709076714406, -9.842485671683344e-05] 37 | accelerations: [2.7215642518773535, 0.013127202561933527, -0.2610584799818869, -0.0002274447821497941] 38 | effort: [] 39 | time_from_start: 40 | secs: 0 41 | nsecs: 469131734 42 | - 43 | positions: [-0.9994532198171924, -0.2597645153639297, -1.1437292787686377, 1.326419854710709] 44 | velocities: [1.4320517429506805, 0.006907363401733337, -0.13736557974417107, -0.00011967848876044047] 45 | accelerations: [2.683659862782023, 0.012944374398571892, -0.2574226069008398, -0.00022427706143007792] 46 | effort: [] 47 | time_from_start: 48 | secs: 0 49 | nsecs: 560312209 50 | - 51 | positions: [-0.8792154769619782, -0.2591845602131515, -1.1552627497732577, 1.3264098062809453] 52 | velocities: [1.6521312690309147, 0.00796889576004447, -0.15847609606360447, -0.00013807083053187123] 53 | accelerations: [2.920174026038989, 0.014085177643518104, -0.2801095700733164, -0.00024404286796047875] 54 | effort: [] 55 | time_from_start: 56 | secs: 0 57 | nsecs: 638114595 58 | - 59 | positions: [-0.7589777341067642, -0.25860460506237326, -1.1667962207778775, 1.3263997578511815] 60 | velocities: [1.6598583933998579, 0.008006166798838582, -0.15921729897329012, -0.00013871659670023175] 61 | accelerations: [-2.723061318851354, -0.013134423520039809, 0.26120208196678424, 0.00022756989411195256] 62 | effort: [] 63 | time_from_start: 64 | secs: 0 65 | nsecs: 706476642 66 | - 67 | positions: [-0.6387399912515502, -0.25802464991159507, -1.1783296917824975, 1.3263897094214179] 68 | velocities: [1.43942515701794, 0.006942928353017207, -0.1380728539771231, -0.00012029469488352126] 69 | accelerations: [-2.8872672222290685, -0.013926454850546784, 0.27695307645845674, 0.00024129280209662948] 70 | effort: [] 71 | time_from_start: 72 | secs: 0 73 | nsecs: 783508708 74 | - 75 | positions: [-0.5185022483963362, -0.2574446947608169, -1.1898631627871172, 1.3263796609916543] 76 | velocities: [1.162797141553844, 0.00560863981259494, -0.11153808112079469, -9.71765198586467e-05] 77 | accelerations: [-2.947870149792967, -0.01421876722400203, 0.2827662436990335, 0.0002463574701826082] 78 | effort: [] 79 | time_from_start: 80 | secs: 0 81 | nsecs: 874738124 82 | - 83 | positions: [-0.39826450554112214, -0.2568647396100387, -1.2013966337917372, 1.3263696125618905] 84 | velocities: [0.7163653050489787, 0.003455318925956762, -0.06871534909339673, -5.986761130108045e-05] 85 | accelerations: [-2.89687048618245, -0.013972775267595364, 0.2778742428386853, 0.00024209535978074632] 86 | effort: [] 87 | time_from_start: 88 | secs: 0 89 | nsecs: 994066218 90 | - 91 | positions: [-0.27802676268590815, -0.2562847844592605, -1.212930104796357, 1.3263595641321269] 92 | velocities: [0.0, 0.0, 0.0, 0.0] 93 | accelerations: [-3.0059846100499463, -0.014499076715517646, 0.2883407116357134, 0.00025121417374673004] 94 | effort: [] 95 | time_from_start: 96 | secs: 1 97 | nsecs: 276907000 98 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/testfiles/coucou/coucou3_modified.txt: -------------------------------------------------------------------------------- 1 | trajectory: 2 | header: 3 | seq: 0 4 | stamp: 5 | secs: 0 6 | nsecs: 0 7 | frame_id: /base_link 8 | joint_names: ['LElbowRoll', 'LElbowYaw', 'LShoulderPitch', 'LShoulderRoll'] 9 | points: 10 | - 11 | positions: [-0.2722547729088527, -0.23359506064526592, -1.2310094257184687, 1.3265571413079886] 12 | velocities: [0.0, 0.0, 0.0, 0.0] 13 | accelerations: [0.0, 0.0, 0.0, 0.0] 14 | effort: [] 15 | time_from_start: 16 | secs: 0 17 | nsecs: 0 18 | - 19 | positions: [-0.2722547729088527, -0.23359506064526592, -1.2310094257184687, 1.32645] 20 | velocities: [-0.2626430335715894, -0.005399422156538498, 0.0043103477059237985, -0.00043548943081728474] 21 | accelerations: [-2.934063448300421, -0.06031855090923199, 0.04815217628823725, 0.004756875980007335] 22 | effort: [] 23 | time_from_start: 24 | secs: 0 25 | nsecs: 124394625 26 | - 27 | positions: [-0.39499618462796837, -0.23611838170559918, -1.22899506365742, 1.3264477387876532] 28 | velocities: [-0.7816814650766328, -0.016069827417447245, 0.012828510484127621, -1.4400582128402473e-05] 29 | accelerations: [-2.914369989385129, -0.05991369228054452, 0.04782897846987937, -5.369018744812833e-05] 30 | effort: [] 31 | time_from_start: 32 | secs: 0 33 | nsecs: 358060468 34 | - 35 | positions: [-0.517737596347084, -0.23864170276593244, -1.2269807015963714, 1.3264454775753065] 36 | velocities: [-1.191269637734419, -0.024490151476420485, 0.019550438023499393, -2.194624923540303e-05] 37 | accelerations: [-2.924457185661667, -0.060121065117860285, 0.047994523783374154, -5.387601953564182e-05] 38 | effort: [] 39 | time_from_start: 40 | secs: 0 41 | nsecs: 476299701 42 | - 43 | positions: [-0.6404790080661996, -0.2411650238262657, -1.2249663395353227, 1.3264432163629598] 44 | velocities: [-1.4604751631052406, -0.030024485506085892, 0.02396848560284951, -2.69057070845695e-05] 45 | accelerations: [-2.7434038288577423, -0.0563989656090741, 0.04502317932950731, -5.054055125250597e-05] 46 | effort: [] 47 | time_from_start: 48 | secs: 0 49 | nsecs: 567593746 50 | - 51 | positions: [-0.7632204197853153, -0.24368834488659896, -1.222951977474274, 1.326440955150613] 52 | velocities: [-1.6764569773338547, -0.034464645129956104, 0.027513055983498163, -3.088464734571485e-05] 53 | accelerations: [-2.7211336285321273, -0.055941134265012196, 0.04465769350040096, -5.013027686668325e-05] 54 | effort: [] 55 | time_from_start: 56 | secs: 0 57 | nsecs: 645451248 58 | - 59 | positions: [-0.885961831504431, -0.24621166594693222, -1.2209376154132254, 1.3264386939382666] 60 | velocities: [-1.6686525817212225, -0.034304202166683104, 0.02738497469282932, -3.0740870315040054e-05] 61 | accelerations: [2.918106797085496, 0.0599905136681809, -0.04789030482710136, 5.375902900902333e-05] 62 | effort: [] 63 | time_from_start: 64 | secs: 0 65 | nsecs: 714545829 66 | - 67 | positions: [-1.0087032432235465, -0.24873498700726548, -1.2189232533521768, 1.32643643272592] 68 | velocities: [-1.446015012975486, -0.029727213372362258, 0.02373117386420743, -2.663931394511831e-05] 69 | accelerations: [2.68934793535705, 0.05528768317721304, -0.04413604482845606, 4.954470272768039e-05] 70 | effort: [] 71 | time_from_start: 72 | secs: 0 73 | nsecs: 793181906 74 | - 75 | positions: [-1.1314446549426624, -0.25125830806759875, -1.2169088912911281, 1.3264341715135732] 76 | velocities: [-1.1746815989977957, -0.02414913415465842, 0.01927820459037963, -2.164065491745095e-05] 77 | accelerations: [2.9418023853294737, 0.060477648173280024, -0.04827918330995675, 5.41955627044498e-05] 78 | effort: [] 79 | time_from_start: 80 | secs: 0 81 | nsecs: 885388892 82 | - 83 | positions: [-1.254186066661778, -0.253781629127932, -1.2148945292300795, 1.3264319103012265] 84 | velocities: [-0.7184407529560064, -0.014769723251058596, 0.011790639977138428, -1.323552563232393e-05] 85 | accelerations: [2.898329015574202, 0.059583921533423144, -0.04756572315437971, 5.339467147249875e-05] 86 | effort: [] 87 | time_from_start: 88 | secs: 1 89 | nsecs: 5934884 90 | - 91 | positions: [-1.3769274783808936, -0.25630495018826527, -1.2128801671690308, 1.3264296490888798] 92 | velocities: [0.0, 0.0, 0.0, 0.0] 93 | accelerations: [2.856149462024424, 0.058716793199986725, -0.0468734963726225, 5.261761566118627e-05] 94 | effort: [] 95 | time_from_start: 96 | secs: 1 97 | nsecs: 299105264 98 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/testfiles/trajBack.txt: -------------------------------------------------------------------------------- 1 | header: 2 | seq: 1 3 | stamp: 4 | secs: 33 5 | nsecs: 894000000 6 | frame_id: '' 7 | goal_id: 8 | stamp: 9 | secs: 33 10 | nsecs: 894000000 11 | id: /move_group-2-33.894000000 12 | goal: 13 | trajectory: 14 | header: 15 | seq: 0 16 | stamp: 17 | secs: 0 18 | nsecs: 0 19 | frame_id: /base_link 20 | joint_names: ['LElbowRoll', 'LElbowYaw', 'LShoulderPitch', 'LShoulderRoll'] 21 | points: 22 | - 23 | positions: [-0.07724205088025382, 0.010846075758542462, -0.6355002718368405, 0.053354730753871316] 24 | velocities: [0.0, 0.0, 0.0, 0.0] 25 | accelerations: [0.0, 0.0, 2.9041990290887116, 0.0] 26 | effort: [] 27 | time_from_start: 28 | secs: 0 29 | nsecs: 0 30 | - 31 | positions: [-0.07253811189355895, 0.010139104226287644, -0.5641118176967026, 0.04769509947817775] 32 | velocities: [0.03634740024319942, -0.005462778602379165, 0.5516195517659253, -0.04373204750070621] 33 | accelerations: [0.19332158938765642, -0.029054981506756162, 2.93390910412218, -0.2325984492273241] 34 | effort: [] 35 | time_from_start: 36 | secs: 0 37 | nsecs: 221725579 38 | - 39 | positions: [-0.0678341729068641, 0.009432132694032827, -0.4927233635565647, 0.04203546820248418] 40 | velocities: [0.05893176273812148, -0.008857061862258738, 0.8943669238754044, -0.07090484133995442] 41 | accelerations: [0.18373666964274812, -0.027614430211816764, 2.788445354362267, -0.221066175590963] 42 | effort: [] 43 | time_from_start: 44 | secs: 0 45 | nsecs: 313100286 46 | - 47 | positions: [-0.06313023392016923, 0.00872516116177801, -0.4213349094164268, 0.03637583692679061] 48 | velocities: [0.0721120826182598, -0.01083797848716106, 1.0943955943103347, -0.08676298721052614] 49 | accelerations: [0.17452068123372091, -0.02622932690473776, 2.6485806223250634, -0.20997778852138763] 50 | effort: [] 51 | time_from_start: 52 | secs: 0 53 | nsecs: 383959960 54 | - 55 | positions: [-0.05842629493347437, 0.008018189629523192, -0.34994645527628887, 0.030716205651097048] 56 | velocities: [0.08277635053951654, -0.012440748815175926, 1.2562398706897993, -0.09959389858719095] 57 | accelerations: [0.17310397017896006, -0.026016404418289264, 2.6270801707993794, -0.2082732463883267] 58 | effort: [] 59 | time_from_start: 60 | secs: 0 61 | nsecs: 444390597 62 | - 63 | positions: [-0.0537223559467795, 0.0073112180972683734, -0.2785580011361509, 0.025056574375403477] 64 | velocities: [0.08316555204583133, -0.01249924327823199, 1.2621465148795745, -0.10006217358478686] 65 | accelerations: [-0.1602957952726974, 0.02409141877020179, -2.4326992892657064, 0.19286285363258227] 66 | effort: [] 67 | time_from_start: 68 | secs: 0 69 | nsecs: 498019719 70 | - 71 | positions: [-0.04901841696008464, 0.006604246565013556, -0.20716954699601303, 0.019396943099709914] 72 | velocities: [0.07283320344444247, -0.01094635827203267, 1.1053395502534378, -0.08763061708263141] 73 | accelerations: [-0.17802854692652184, 0.02675653637552318, -2.7018170928337883, 0.21419834207066918] 74 | effort: [] 75 | time_from_start: 76 | secs: 0 77 | nsecs: 557852033 78 | - 79 | positions: [-0.04431447797338977, 0.005897275032758739, -0.13578109285587509, 0.013737311824016343] 80 | velocities: [0.059185895838940586, -0.008895256419669227, 0.8982237275594305, -0.07121060628496177] 81 | accelerations: [-0.1943468952817073, 0.029209078335179186, -2.949469468107331, 0.23383206499519957] 82 | effort: [] 83 | time_from_start: 84 | secs: 0 85 | nsecs: 628010127 86 | - 87 | positions: [-0.039610538986694914, 0.005190303500503922, -0.0643926387157372, 0.00807768054832278] 88 | velocities: [0.036240211139808826, -0.00544666877508774, 0.5499928162973391, -0.043603080946573815] 89 | accelerations: [-0.19215524968788006, 0.02887968821170856, -2.9162083668454377, 0.23119514602513302] 90 | effort: [] 91 | time_from_start: 92 | secs: 0 93 | nsecs: 719661807 94 | - 95 | positions: [-0.034906600000000045, 0.004483331968249103, 0.006995815424400778, 0.0024180492726292102] 96 | velocities: [0.0, 0.0, 0.0, 0.0] 97 | accelerations: [-0.19030452409027476, 0.02860153615335417, -2.8881211744255593, 0.22896841125995765] 98 | effort: [] 99 | time_from_start: 100 | secs: 0 101 | nsecs: 942003691 102 | path_tolerance: [] 103 | goal_tolerance: [] 104 | goal_time_tolerance: 105 | secs: 0 106 | nsecs: 0 107 | --- 108 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/testfiles/traj.txt: -------------------------------------------------------------------------------- 1 | header: 2 | seq: 0 3 | stamp: 4 | secs: 22 5 | nsecs: 147000000 6 | frame_id: '' 7 | goal_id: 8 | stamp: 9 | secs: 22 10 | nsecs: 147000000 11 | id: /move_group-1-22.147000000 12 | goal: 13 | trajectory: 14 | header: 15 | seq: 0 16 | stamp: 17 | secs: 0 18 | nsecs: 0 19 | frame_id: /base_link 20 | joint_names: ['LElbowRoll', 'LElbowYaw', 'LShoulderPitch', 'LShoulderRoll'] 21 | points: 22 | - 23 | positions: [-0.037175756763097034, 0.0012583609645888671, 0.00703808603645939, -0.0004088266224000492] 24 | velocities: [0.0, 0.0, 0.0, 0.0] 25 | accelerations: [0.0, 0.0, -2.834163700478893, 0.0] 26 | effort: [] 27 | time_from_start: 28 | secs: 0 29 | nsecs: 0 30 | - 31 | positions: [-0.041682635405575055, -0.0006184850363736194, -0.06565363413334838, 0.005273382575327419] 32 | velocities: [-0.03454733408061231, -0.014386902989129802, -0.5572160559042572, 0.04355679285872327] 33 | accelerations: [-0.18419856553504027, -0.07670771026516579, -2.9709498843286144, 0.23223496045057274] 34 | effort: [] 35 | time_from_start: 36 | secs: 0 37 | nsecs: 226487900 38 | - 39 | positions: [-0.04618951404805307, -0.002495331037336106, -0.13834535430315614, 0.010955591773054886] 40 | velocities: [-0.05614143324219118, -0.02337955663500494, -0.9055085967279214, 0.070782329334473] 41 | accelerations: [-0.17039535016882706, -0.07095949478930876, -2.7483169828372476, 0.21483206067579966] 42 | effort: [] 43 | time_from_start: 44 | secs: 0 45 | nsecs: 318099156 46 | - 47 | positions: [-0.050696392690531084, -0.004372177038298592, -0.21103707447296394, 0.016637800970782355] 48 | velocities: [-0.06853093034648777, -0.02853904282018209, -1.1053395502534378, 0.08640283301028169] 49 | accelerations: [-0.16450908786517326, -0.06850821780991417, -2.653377099568452, 0.2074107439607617] 50 | effort: [] 51 | time_from_start: 52 | secs: 0 53 | nsecs: 389538055 54 | - 55 | positions: [-0.055203271333009105, -0.006249023039261079, -0.2837287946427717, 0.022320010168509822] 56 | velocities: [-0.07866560092557427, -0.03275952831720719, -1.268802269396697, 0.09918048311997268] 57 | accelerations: [-0.1631736481812247, -0.06795208687558661, -2.6318377115552076, 0.20572704039182077] 58 | effort: [] 59 | time_from_start: 60 | secs: 0 61 | nsecs: 450462666 62 | - 63 | positions: [-0.059710149975487126, -0.008125869040223567, -0.3564205148125795, 0.028002219366237296] 64 | velocities: [-0.07903547432752811, -0.03291355851646233, -1.2747679800283702, 0.09964681430243866] 65 | accelerations: [0.15110022996998235, 0.06292422868696142, 2.43710481375026, -0.19050504453833808] 66 | effort: [] 67 | time_from_start: 68 | secs: 0 69 | nsecs: 504530165 70 | - 71 | positions: [-0.06421702861796513, -0.010002715041186052, -0.42911223498238726, 0.03368442856396476] 72 | velocities: [-0.06921623964995272, -0.028824433248383914, -1.116392945755972, 0.08726686134038456] 73 | accelerations: [0.16781572053125443, 0.06988523298789252, 2.706709979269775, -0.2115796999143699] 74 | effort: [] 75 | time_from_start: 76 | secs: 0 77 | nsecs: 564851562 78 | - 79 | positions: [-0.06872390726044317, -0.011879561042148538, -0.501803955152195, 0.03936663776169223] 80 | velocities: [-0.05627429692833494, -0.023434886431476842, -0.9076515632153817, 0.07094984200107418] 81 | accelerations: [0.18263759016302034, 0.07605765717587865, 2.9457728174633764, -0.23026690465853614] 82 | effort: [] 83 | time_from_start: 84 | secs: 0 85 | nsecs: 635583144 86 | - 87 | positions: [-0.07323078590292117, -0.013756407043111025, -0.5744956753220027, 0.04504884695941969] 88 | velocities: [-0.034531732288200466, -0.014380405773646797, -0.5569644136440337, 0.04353712233830855] 89 | accelerations: [0.18154563570786722, 0.07560292330905967, 2.9281606175377624, -0.2288901838411453] 90 | effort: [] 91 | time_from_start: 92 | secs: 0 93 | nsecs: 727879451 94 | - 95 | positions: [-0.0777376645453992, -0.015633253044073512, -0.6471873954918106, 0.05073105615714717] 96 | velocities: [0.0, 0.0, 0.0, 0.0] 97 | accelerations: [0.18166504958951835, 0.07565265206459398, 2.9300866513092347, -0.22904073918342982] 98 | effort: [] 99 | time_from_start: 100 | secs: 0 101 | nsecs: 950629208 102 | path_tolerance: [] 103 | goal_tolerance: [] 104 | goal_time_tolerance: 105 | secs: 0 106 | nsecs: 0 107 | --- 108 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/testfiles/coucou/coucou2.txt: -------------------------------------------------------------------------------- 1 | header: 2 | seq: 1 3 | stamp: 4 | secs: 67 5 | nsecs: 457000000 6 | frame_id: '' 7 | goal_id: 8 | stamp: 9 | secs: 67 10 | nsecs: 457000000 11 | id: /move_group-2-67.457000000 12 | goal: 13 | trajectory: 14 | header: 15 | seq: 0 16 | stamp: 17 | secs: 0 18 | nsecs: 0 19 | frame_id: /base_link 20 | joint_names: ['LElbowRoll', 'LElbowYaw', 'LShoulderPitch', 'LShoulderRoll'] 21 | points: 22 | - 23 | positions: [-1.3601664483828344, -0.26150438081626426, -1.1091288657547782, 1.326529204847227] 24 | velocities: [0.0, 0.0, 0.0, 0.0] 25 | accelerations: [0.0, 0.0, 0.0, 0.0] 26 | effort: [] 27 | time_from_start: 28 | secs: 0 29 | nsecs: 0 30 | - 31 | positions: [-1.3601664483828344, -0.26150438081626426, -1.1091288657547782, 1.32645] 32 | velocities: [0.25786068244393545, 0.0012437661204819789, -0.024734568643556592, -0.0003515264009807792] 33 | accelerations: [2.9206051748428474, 0.014087257248160257, -0.28015092682298914, 0.003493332577957609] 34 | effort: [] 35 | time_from_start: 36 | secs: 0 37 | nsecs: 120015840 38 | - 39 | positions: [-1.2399287055276205, -0.26092442566548607, -1.120662336759398, 1.3264399515702363] 40 | velocities: [0.7762558971816609, 0.003744195418193728, -0.07446018753937098, -6.487274857543432e-05] 41 | accelerations: [2.9850778768507524, 0.014398235105247032, -0.28633529141218683, -0.0002494669699666938] 42 | effort: [] 43 | time_from_start: 44 | secs: 0 45 | nsecs: 353160618 46 | - 47 | positions: [-1.1196909626724063, -0.2603444705147079, -1.132195807764018, 1.3264299031404725] 48 | velocities: [1.1777345207941157, 0.005680688820034763, -0.1129709076714406, -9.842485671683344e-05] 49 | accelerations: [2.7215642518773535, 0.013127202561933527, -0.2610584799818869, -0.0002274447821497941] 50 | effort: [] 51 | time_from_start: 52 | secs: 0 53 | nsecs: 469131734 54 | - 55 | positions: [-0.9994532198171924, -0.2597645153639297, -1.1437292787686377, 1.326419854710709] 56 | velocities: [1.4320517429506805, 0.006907363401733337, -0.13736557974417107, -0.00011967848876044047] 57 | accelerations: [2.683659862782023, 0.012944374398571892, -0.2574226069008398, -0.00022427706143007792] 58 | effort: [] 59 | time_from_start: 60 | secs: 0 61 | nsecs: 560312209 62 | - 63 | positions: [-0.8792154769619782, -0.2591845602131515, -1.1552627497732577, 1.3264098062809453] 64 | velocities: [1.6521312690309147, 0.00796889576004447, -0.15847609606360447, -0.00013807083053187123] 65 | accelerations: [2.920174026038989, 0.014085177643518104, -0.2801095700733164, -0.00024404286796047875] 66 | effort: [] 67 | time_from_start: 68 | secs: 0 69 | nsecs: 638114595 70 | - 71 | positions: [-0.7589777341067642, -0.25860460506237326, -1.1667962207778775, 1.3263997578511815] 72 | velocities: [1.6598583933998579, 0.008006166798838582, -0.15921729897329012, -0.00013871659670023175] 73 | accelerations: [-2.723061318851354, -0.013134423520039809, 0.26120208196678424, 0.00022756989411195256] 74 | effort: [] 75 | time_from_start: 76 | secs: 0 77 | nsecs: 706476642 78 | - 79 | positions: [-0.6387399912515502, -0.25802464991159507, -1.1783296917824975, 1.3263897094214179] 80 | velocities: [1.43942515701794, 0.006942928353017207, -0.1380728539771231, -0.00012029469488352126] 81 | accelerations: [-2.8872672222290685, -0.013926454850546784, 0.27695307645845674, 0.00024129280209662948] 82 | effort: [] 83 | time_from_start: 84 | secs: 0 85 | nsecs: 783508708 86 | - 87 | positions: [-0.5185022483963362, -0.2574446947608169, -1.1898631627871172, 1.3263796609916543] 88 | velocities: [1.162797141553844, 0.00560863981259494, -0.11153808112079469, -9.71765198586467e-05] 89 | accelerations: [-2.947870149792967, -0.01421876722400203, 0.2827662436990335, 0.0002463574701826082] 90 | effort: [] 91 | time_from_start: 92 | secs: 0 93 | nsecs: 874738124 94 | - 95 | positions: [-0.39826450554112214, -0.2568647396100387, -1.2013966337917372, 1.3263696125618905] 96 | velocities: [0.7163653050489787, 0.003455318925956762, -0.06871534909339673, -5.986761130108045e-05] 97 | accelerations: [-2.89687048618245, -0.013972775267595364, 0.2778742428386853, 0.00024209535978074632] 98 | effort: [] 99 | time_from_start: 100 | secs: 0 101 | nsecs: 994066218 102 | - 103 | positions: [-0.27802676268590815, -0.2562847844592605, -1.212930104796357, 1.3263595641321269] 104 | velocities: [0.0, 0.0, 0.0, 0.0] 105 | accelerations: [-3.0059846100499463, -0.014499076715517646, 0.2883407116357134, 0.00025121417374673004] 106 | effort: [] 107 | time_from_start: 108 | secs: 1 109 | nsecs: 276907000 110 | path_tolerance: [] 111 | goal_tolerance: [] 112 | goal_time_tolerance: 113 | secs: 0 114 | nsecs: 0 115 | --- 116 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/testfiles/coucou/coucou3.txt: -------------------------------------------------------------------------------- 1 | header: 2 | seq: 2 3 | stamp: 4 | secs: 74 5 | nsecs: 720000000 6 | frame_id: '' 7 | goal_id: 8 | stamp: 9 | secs: 74 10 | nsecs: 720000000 11 | id: /move_group-3-74.720000000 12 | goal: 13 | trajectory: 14 | header: 15 | seq: 0 16 | stamp: 17 | secs: 0 18 | nsecs: 0 19 | frame_id: /base_link 20 | joint_names: ['LElbowRoll', 'LElbowYaw', 'LShoulderPitch', 'LShoulderRoll'] 21 | points: 22 | - 23 | positions: [-0.2722547729088527, -0.23359506064526592, -1.2310094257184687, 1.3265571413079886] 24 | velocities: [0.0, 0.0, 0.0, 0.0] 25 | accelerations: [0.0, 0.0, 0.0, 0.0] 26 | effort: [] 27 | time_from_start: 28 | secs: 0 29 | nsecs: 0 30 | - 31 | positions: [-0.2722547729088527, -0.23359506064526592, -1.2310094257184687, 1.32645] 32 | velocities: [-0.2626430335715894, -0.005399422156538498, 0.0043103477059237985, -0.00043548943081728474] 33 | accelerations: [-2.934063448300421, -0.06031855090923199, 0.04815217628823725, 0.004756875980007335] 34 | effort: [] 35 | time_from_start: 36 | secs: 0 37 | nsecs: 124394625 38 | - 39 | positions: [-0.39499618462796837, -0.23611838170559918, -1.22899506365742, 1.3264477387876532] 40 | velocities: [-0.7816814650766328, -0.016069827417447245, 0.012828510484127621, -1.4400582128402473e-05] 41 | accelerations: [-2.914369989385129, -0.05991369228054452, 0.04782897846987937, -5.369018744812833e-05] 42 | effort: [] 43 | time_from_start: 44 | secs: 0 45 | nsecs: 358060468 46 | - 47 | positions: [-0.517737596347084, -0.23864170276593244, -1.2269807015963714, 1.3264454775753065] 48 | velocities: [-1.191269637734419, -0.024490151476420485, 0.019550438023499393, -2.194624923540303e-05] 49 | accelerations: [-2.924457185661667, -0.060121065117860285, 0.047994523783374154, -5.387601953564182e-05] 50 | effort: [] 51 | time_from_start: 52 | secs: 0 53 | nsecs: 476299701 54 | - 55 | positions: [-0.6404790080661996, -0.2411650238262657, -1.2249663395353227, 1.3264432163629598] 56 | velocities: [-1.4604751631052406, -0.030024485506085892, 0.02396848560284951, -2.69057070845695e-05] 57 | accelerations: [-2.7434038288577423, -0.0563989656090741, 0.04502317932950731, -5.054055125250597e-05] 58 | effort: [] 59 | time_from_start: 60 | secs: 0 61 | nsecs: 567593746 62 | - 63 | positions: [-0.7632204197853153, -0.24368834488659896, -1.222951977474274, 1.326440955150613] 64 | velocities: [-1.6764569773338547, -0.034464645129956104, 0.027513055983498163, -3.088464734571485e-05] 65 | accelerations: [-2.7211336285321273, -0.055941134265012196, 0.04465769350040096, -5.013027686668325e-05] 66 | effort: [] 67 | time_from_start: 68 | secs: 0 69 | nsecs: 645451248 70 | - 71 | positions: [-0.885961831504431, -0.24621166594693222, -1.2209376154132254, 1.3264386939382666] 72 | velocities: [-1.6686525817212225, -0.034304202166683104, 0.02738497469282932, -3.0740870315040054e-05] 73 | accelerations: [2.918106797085496, 0.0599905136681809, -0.04789030482710136, 5.375902900902333e-05] 74 | effort: [] 75 | time_from_start: 76 | secs: 0 77 | nsecs: 714545829 78 | - 79 | positions: [-1.0087032432235465, -0.24873498700726548, -1.2189232533521768, 1.32643643272592] 80 | velocities: [-1.446015012975486, -0.029727213372362258, 0.02373117386420743, -2.663931394511831e-05] 81 | accelerations: [2.68934793535705, 0.05528768317721304, -0.04413604482845606, 4.954470272768039e-05] 82 | effort: [] 83 | time_from_start: 84 | secs: 0 85 | nsecs: 793181906 86 | - 87 | positions: [-1.1314446549426624, -0.25125830806759875, -1.2169088912911281, 1.3264341715135732] 88 | velocities: [-1.1746815989977957, -0.02414913415465842, 0.01927820459037963, -2.164065491745095e-05] 89 | accelerations: [2.9418023853294737, 0.060477648173280024, -0.04827918330995675, 5.41955627044498e-05] 90 | effort: [] 91 | time_from_start: 92 | secs: 0 93 | nsecs: 885388892 94 | - 95 | positions: [-1.254186066661778, -0.253781629127932, -1.2148945292300795, 1.3264319103012265] 96 | velocities: [-0.7184407529560064, -0.014769723251058596, 0.011790639977138428, -1.323552563232393e-05] 97 | accelerations: [2.898329015574202, 0.059583921533423144, -0.04756572315437971, 5.339467147249875e-05] 98 | effort: [] 99 | time_from_start: 100 | secs: 1 101 | nsecs: 5934884 102 | - 103 | positions: [-1.3769274783808936, -0.25630495018826527, -1.2128801671690308, 1.3264296490888798] 104 | velocities: [0.0, 0.0, 0.0, 0.0] 105 | accelerations: [2.856149462024424, 0.058716793199986725, -0.0468734963726225, 5.261761566118627e-05] 106 | effort: [] 107 | time_from_start: 108 | secs: 1 109 | nsecs: 299105264 110 | path_tolerance: [] 111 | goal_tolerance: [] 112 | goal_time_tolerance: 113 | secs: 0 114 | nsecs: 0 115 | --- 116 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/scripts/pcl_checker.py: -------------------------------------------------------------------------------- 1 | from math import atan2, sqrt 2 | import rospy 3 | from sensor_msgs.point_cloud2 import read_points 4 | from sensor_msgs.msg import LaserScan, PointCloud2 5 | from message_filters import TimeSynchronizer, Subscriber 6 | 7 | 8 | class PCLChecker(object): 9 | def __init__(self): 10 | rospy.init_node('pcl_checker', anonymous=True, disable_signals=True) 11 | # Original laser scan 12 | lleft_sub = Subscriber('/pepper/scan_left', LaserScan) 13 | lfront_sub = Subscriber('/pepper/scan_front', LaserScan) 14 | lright_sub = Subscriber('/pepper/scan_right', LaserScan) 15 | 16 | # Original point cloud 17 | pcleft_sub = Subscriber('/pepper/pc_left', PointCloud2) 18 | pcfront_sub = Subscriber('/pepper/pc_front', PointCloud2) 19 | pcright_sub = Subscriber('/pepper/pc_right', PointCloud2) 20 | 21 | # Translated point cloud 22 | pcall_sub = Subscriber('/cloud_in', PointCloud2) 23 | 24 | self.ts = TimeSynchronizer( 25 | [lleft_sub, lfront_sub, lright_sub, 26 | pcleft_sub, pcfront_sub, pcright_sub, pcall_sub], 10) 27 | 28 | self.ts.registerCallback(self.check_values) 29 | 30 | def get_dist(self, x0, y0, x1=0, y1=0): 31 | return sqrt((x1 - x0)**2 + (y1 - y0)**2) 32 | 33 | def check_values(self, olleft, olfront, olright, 34 | opc_right, opc_left, opc_front, allpc): 35 | # Front 36 | olfront_angles = [] 37 | opc_dist = [] 38 | opc_angles = [] 39 | 40 | # Left 41 | olleft_angles = [] 42 | opcleft_dist = [] 43 | opcleft_angles = [] 44 | 45 | # Right 46 | olright_angles = [] 47 | opcright_dist = [] 48 | opcright_angles = [] 49 | 50 | # Combined 51 | alllaser_dist = [] 52 | alllaser_angles = [] 53 | allpc_dist = [] 54 | allpc_angles = [] 55 | 56 | # Right 57 | for i in range(len(olright.ranges)): 58 | alllaser_dist.append(olright.ranges[i]) 59 | alllaser_angles.append(olright.angle_min + i * 60 | olright.angle_increment - 1.757) 61 | olright_angles.append(olright.angle_min + i * 62 | olright.angle_increment - 1.757) 63 | 64 | for p in read_points(opc_right, skip_nans=True): 65 | distance = self.get_dist(p[0], p[1]) 66 | opcright_dist.append(distance) 67 | opcright_angles.append(atan2(p[1], p[0]) - 1.757) 68 | 69 | # Front 70 | for i in range(len(olfront.ranges)): 71 | alllaser_dist.append(olfront.ranges[i]) 72 | alllaser_angles.append(olfront.angle_min + 73 | i * olfront.angle_increment) 74 | olfront_angles.append(olfront.angle_min + 75 | i * olfront.angle_increment) 76 | 77 | for p in read_points(opc_front, skip_nans=True): 78 | opc_dist.append(self.get_dist(p[0], p[1])) 79 | opc_angles.append(atan2(p[1], p[0])) 80 | 81 | # Left 82 | for i in range(len(olleft.ranges)): 83 | alllaser_dist.append(olleft.ranges[i]) 84 | alllaser_angles.append(olleft.angle_min + i * 85 | olleft.angle_increment + 1.757) 86 | olleft_angles.append(olleft.angle_min + i * 87 | olleft.angle_increment + 1.757) 88 | 89 | for p in read_points(opc_left, skip_nans=True): 90 | distance = self.get_dist(p[0], p[1]) 91 | opcleft_dist.append(distance) 92 | opcleft_angles.append(atan2(p[1], p[0]) + 1.757) 93 | 94 | # All combined 95 | for p in read_points(allpc, skip_nans=True): 96 | allpc_dist.append(self.get_dist(p[0], p[1])) 97 | allpc_angles.append(atan2(p[1], p[0])) 98 | 99 | rospy.loginfo("===============================================================") 100 | rospy.loginfo("original right laser") 101 | rospy.loginfo(olright.ranges) 102 | rospy.loginfo(olright_angles) 103 | rospy.loginfo("===============================================================") 104 | rospy.loginfo("original front laser") 105 | rospy.loginfo(olfront.ranges) 106 | rospy.loginfo(olfront_angles) 107 | rospy.loginfo("===============================================================") 108 | rospy.loginfo("original left laser") 109 | rospy.loginfo(olleft.ranges) 110 | rospy.loginfo(olleft_angles) 111 | rospy.loginfo("===============================================================") 112 | rospy.loginfo("all combined laser") 113 | rospy.loginfo(alllaser_dist) 114 | rospy.loginfo(alllaser_angles) 115 | rospy.loginfo("==============================================================") 116 | rospy.loginfo("original right point cloud") 117 | rospy.loginfo(opcright_dist) 118 | rospy.loginfo(opcright_angles) 119 | rospy.loginfo("===============================================================") 120 | rospy.loginfo("original front point cloud") 121 | rospy.loginfo(opc_dist) 122 | rospy.loginfo(opc_angles) 123 | rospy.loginfo("===============================================================") 124 | rospy.loginfo("original left point cloud") 125 | rospy.loginfo(opcleft_dist) 126 | rospy.loginfo(opcleft_angles) 127 | rospy.loginfo("===============================================================") 128 | rospy.loginfo("all combined point cloud") 129 | rospy.loginfo(allpc_dist) 130 | rospy.loginfo(allpc_angles) 131 | rospy.loginfo("===============================================================") 132 | 133 | 134 | if __name__ == "__main__": 135 | PCLC = PCLChecker() 136 | rospy.spin() 137 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | pepper_virtual 2 | ============== 3 | 4 | Tools to simulate a Pepper robot: 5 | - pepper_control: provides ROS controllers for a Pepper robot, these allow to control a simulated robot as well as a real one using [pepper_gazebo_plugin](https://github.com/ros-naoqi/pepper_virtual/tree/master/pepper_gazebo_plugin) or [pepper_moveit_config](https://github.com/ros-naoqi/pepper_moveit_config). 6 | - pepper_gazebo_plugin: Gazebo plugin for a Pepper robot 7 | 8 | # MODIFIED VERSION that works (at least on Kinetic in 21/04/2020) 9 | 10 | For this modified version (differs from the origin upstream) you'll also need: 11 | - [https://github.com/awesomebytes/gazebo_model_velocity_plugin](https://github.com/awesomebytes/gazebo_model_velocity_plugin) 12 | - `sudo apt-get install ros-kinetic-tf2-sensor-msgs` 13 | - [https://github.com/awesomebytes/pepper_robot](https://github.com/awesomebytes/pepper_robot) IMPORTANT: Use the branch correct_chain_model_and_gazebo_enabled 14 | 15 | Note that the provided launchfiles that have been tested are: 16 | - `roslaunch pepper_gazebo_plugin pepper_gazebo_plugin_Y20.launch` 17 | - `roslaunch pepper_gazebo_plugin pepper_gazebo_plugin_in_office.launch` 18 | 19 | Or their variants that don't need a GPU (for the laser plugin): 20 | - `roslaunch pepper_gazebo_plugin pepper_gazebo_plugin_Y20_CPU.launch` 21 | - `roslaunch pepper_gazebo_plugin pepper_gazebo_plugin_in_office_CPU.launch` 22 | 23 | And further variants that don't have the arms of the robot which reduce CPU load (useful for navigation tests, specially on underpowered computers). In my machine this used 30% less CPU on `gzserver`. Note that if you close/don't launch `gzclient` (the graphical interface of Gazebo) you'll save a lot of CPU too (more than 1 core or 110% CPU in `top -c` for me): 24 | - `roslaunch pepper_gazebo_plugin pepper_gazebo_plugin_Y20_CPU_no_arms.launch` 25 | - `roslaunch pepper_gazebo_plugin pepper_gazebo_plugin_in_office_CPU_no_arms.launch` 26 | 27 | 28 | # Installation 29 | This should get your workspace up and running: 30 | ```bash 31 | mkdir -p pepper_sim_ws/src 32 | cd pepper_sim_ws/src 33 | git clone -b correct_chain_model_and_gazebo_enabled https://github.com/awesomebytes/pepper_robot 34 | git clone -b simulation_that_works https://github.com/awesomebytes/pepper_virtual 35 | git clone https://github.com/awesomebytes/gazebo_model_velocity_plugin 36 | # In case you are missing any of these 37 | sudo apt-get install ros-kinetic-tf2-sensor-msgs ros-kinetic-ros-control ros-kinetic-ros-controllers ros-kinetic-gazebo-ros ros-kinetic-gazebo-ros-control ros-kinetic-gazebo-plugins ros-kinetic-controller-manager ros-kinetic-ddynamic-reconfigure-python 38 | cd .. 39 | catkin_make 40 | source devel/setup.bash 41 | # Launch your preferred simulation here 42 | roslaunch pepper_gazebo_plugin pepper_gazebo_plugin_in_office_CPU.launch 43 | 44 | # (On another shell that you sourced the workspace) Check stuff on Rviz 45 | rosrun rviz rviz -d `rospack find pepper_gazebo_plugin`/config/pepper_sensors.rviz 46 | ``` 47 | 48 | 49 | ## Docker 50 | We provide a docker image which is plattform independent and containerized, meaning it can't interfere with any local ROS installations or other incompatible packages you might have. You have two options to obtain the image: 51 | + Build it locally, from the provided dockerfile: `docker build -t awesome-pepper-sim .` 52 | + Pull it from dockerhub: `docker pull frietz58/pepper-virtual:with-gazebo-files` 53 | 54 | Pulling it from dockerhub comes with the advantage of having the commited gazebo files, which otherwise have to be loaded when gazebo is started in the container. This happens automatically but takes about 30 seconds. Note that docker, per default, does not have permissions to spawn GUIs on the host system from within the container. There are a few options to address this (more about this described in [this fork](https://github.com/frietz58/pepper_virtual)), but we recommend installing the docker extension "rocker" to take care of this. Then, launch the container, depending on which option you chose earlier: 55 | + If build locally: `rocker --nvidia --x11 awesome-pepper-sim` 56 | + If pulled from dockerhub: `rocker --nvidia --x11 frietz58/pepper-virtual:with-gazebo-files` 57 | 58 | Alternatively, you can use the `start_docker.sh` script which will setup a lot of docker flags to enable the utilization of your Nvidia GPU and your X graphical system. 59 | 60 | Then, just as above but in the container, do: 61 | ``` 62 | # Launch your preferred simulation here 63 | roslaunch pepper_gazebo_plugin pepper_gazebo_plugin_in_office_CPU.launch 64 | ``` 65 | In order to start a new shell in the container and, for example open RVIZ, run: 66 | ``` 67 | # First, find container ID with: sudo docker ps, then 68 | docker exec -it bash 69 | # Launch RVIZ in the second shell in the container: 70 | rosrun rviz rviz -d `rospack find pepper_gazebo_plugin`/config/pepper_sensors.rviz 71 | ``` 72 | ![screenshot of Pepper in Gazebo](gazebo_screenshot.png) 73 | 74 | 75 | You'll find a topic `/pepper/laser_2` that unifies the 3 lasers of the robot in 1 topic (by reprojecting the points in a pointcloud and transforming them into `base_footprint`). 76 | 77 | You'll find `/pepper/cmd_vel` to command the base, for example, using `rosrun rqt_robot_steering rqt_robot_steering`. 78 | 79 | You can also move the joints via the *follow_joint_trajectory* controllers, for example, using `rosrun 80 | rqt_joint_trajectory_controller rqt_joint_trajectory_controller`. 81 | 82 | # Summary of changes in this simulation 83 | * The robot model has been modified to invert its TF tree to start in `base_footprint` and from there branch up. The original model starts on `base_link` in the torso and branches differently. This setup is different to all mobile based robots I used before and became problematic when using some whole body control frameworks that also aim to use the base. The details of this process are here: [pepper_description modified](https://github.com/awesomebytes/pepper_robot/tree/correct_chain_model_and_gazebo_enabled/pepper_description) 84 | 85 | * The base collision meshes has been changed to a box to simplify collision physics for the [gazebo_model_velocity_plugin](https://github.com/awesomebytes/gazebo_model_velocity_plugin). This complicated non-spherical collision meshes for the wheels made the base not move properly in my experience. 86 | 87 | * The gazebo_model_velocity_plugin provides realistic odometry drift (on `/pepper/odom`, based on real-life measured drift in our robots at the Magic Lab in the **UTS Unleashed!** RoboCup@Home SSPL team) with a odometry groundtruth topic (on `/pepper/odom_groundtruth` in a imaginary Gazebo `/world` frame). It also implements velocity-related limits that make the motion of the robot behave similar to a real one. Taken from the gazebo_model_velocity_plugin README: 88 | 89 | * gazebo_ros_p3d: Publishes odometry, but it supports gaussian noise for XY & Yaw separately (and it doesn't add the noise when the robot is stopped). It allows the odom frame to be odom (which gazebo_ros_p3d doesn't for some reason). 90 | * ros_planar_move: It moves the robot by sending velocities in Gazebo in a Twist topic, but it also implements velocity, acceleration and jerk limits to better behave like a real robot. It also stops the robot if no commands are sent for a while. Kind of a diff_drive_controller but hacky. 91 | 92 | * The fingers have been removed from the simulation as they were too CPU expensive to simulate and also made the simulation more unstable. 93 | 94 | * The laser sensor is simulated by having 3 lasers simulated in gazebo (with a GPU and a CPU version so this simulation can be run on underpowered computers) and then the script [laser_publisher.py](https://github.com/awesomebytes/pepper_virtual/blob/simulation_that_works/pepper_gazebo_plugin/scripts/laser_publisher.py) transforms these 3 into a pointcloud and transforms it to the `base_footprint` frame in one single LaserScan message. This LaserScan contains many many `NaN` values because to fit the LaserScan beams from 3 different poses (each laser) in the robot into one LaserScan with a different origin a trade-off must be made. This behaves differently than the [naoqi_driver laser implementation](https://github.com/ros-naoqi/naoqi_driver/blob/master/src/converters/laser.cpp#L142-L211) to provide more precise laser scans. If you want to use a driver for your robot with this same kind of implementation, you can base it of: [UTS Unleashed! to-be-published laser driver](https://gist.github.com/awesomebytes/17f14345e39970932984837434c36c24). 95 | 96 | * The simulated cameras have been updated with similar to real life parameters. 97 | 98 | If you want to simulate the RGB cameras with different resolutions you must go to `pepper_robot/pepper_description/urdf/pepper1.0_generated_urdf/pepperGazebo.xacro` (and/or `pepperGazeboCPU.xacro`) and change the `CameraTop_frame` `640` `480` tags. 99 | 100 | # Video showing the simulation working 101 | [![Video of the simulation working](http://img.youtube.com/vi/W_Ag_C8Dvm0/0.jpg)](http://youtu.be/W_Ag_C8Dvm0) 102 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/worlds/simple_office_with_people.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 0 0 -9.81 8 | 9 | 10 | quick 11 | 20 12 | 1.0 13 | 14 | 15 | 0.0 16 | 0.2 17 | 100.0 18 | 0.0 19 | 20 | 21 | 1000 22 | 0.001 23 | 24 | 25 | 26 | model://sun 27 | 28 | 29 | 30 | model://ground_plane 31 | 32 | 33 | 0 -5 1 0 0 0 34 | true 35 | 36 | 37 | 1.0 38 | 39 | 1.0 40 | 0.0 41 | 0.0 42 | 1.0 43 | 0.0 44 | 1.0 45 | 46 | 47 | 48 | 49 | 50 | 10 0.2 2 51 | 52 | 53 | 54 | 55 | 56 | 57 | 10 0.2 2 58 | 59 | 60 | 61 | 62 | 63 | 64 | 0 10 1 0 0 0 65 | true 66 | 67 | 68 | 1.0 69 | 70 | 1.0 71 | 0.0 72 | 0.0 73 | 1.0 74 | 0.0 75 | 1.0 76 | 77 | 78 | 79 | 80 | 81 | 10 0.2 2 82 | 83 | 84 | 85 | 86 | 87 | 88 | 10 0.2 2 89 | 90 | 91 | 92 | 93 | 94 | 95 | -5 2.5 1 0 0 0 96 | true 97 | 98 | 99 | 1.0 100 | 101 | 1.0 102 | 0.0 103 | 0.0 104 | 1.0 105 | 0.0 106 | 1.0 107 | 108 | 109 | 110 | 111 | 112 | 0.2 15 2 113 | 114 | 115 | 116 | 117 | 118 | 119 | 0.2 15 2 120 | 121 | 122 | 123 | 124 | 125 | 126 | 5 2.5 1 0 0 0 127 | true 128 | 129 | 130 | 1.0 131 | 132 | 1.0 133 | 0.0 134 | 0.0 135 | 1.0 136 | 0.0 137 | 1.0 138 | 139 | 140 | 141 | 142 | 143 | 0.2 15 2 144 | 145 | 146 | 147 | 148 | 149 | 150 | 0.2 15 2 151 | 152 | 153 | 154 | 155 | 156 | 157 | -2 5 1 0 0 0 158 | true 159 | 160 | 161 | 1.0 162 | 163 | 1.0 164 | 0.0 165 | 0.0 166 | 1.0 167 | 0.0 168 | 1.0 169 | 170 | 171 | 172 | 173 | 174 | 6 0.2 2 175 | 176 | 177 | 178 | 179 | 180 | 181 | 6 0.2 2 182 | 183 | 184 | 185 | 186 | 187 | 188 | 3.5 5 1 0 0 0 189 | true 190 | 191 | 192 | 1.0 193 | 194 | 1.0 195 | 0.0 196 | 0.0 197 | 1.0 198 | 0.0 199 | 1.0 200 | 201 | 202 | 203 | 204 | 205 | 3 0.2 2 206 | 207 | 208 | 209 | 210 | 211 | 212 | 3 0.2 2 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | model://dining_chair 221 | 2.5 -4.5 0.01 0 0 0 222 | 223 | 224 | 225 | model://kitchen_table 226 | 2.000000 -3.551060 0.000000 0.000000 0.000000 0.000000 227 | 228 | 229 | 230 | model://wardrobe 231 | 2.000000 9.000000 0.000000 0.000000 0.000000 0.000000 232 | 233 | 234 | 235 | model://closet 236 | -4.000000 -5.000000 0.000000 0.000000 0.000000 0.000000 237 | 238 | 239 | 240 | model://bench 241 | -4.465430 1.502710 0.000000 0.000000 0.000000 0.000000 242 | 243 | 244 | 245 | model://sofa 246 | -0.518195 9.000000 0.000000 0.000000 0.000000 0.000000 247 | 248 | 249 | 250 | floor_lamp_0 251 | model://floor_lamp 252 | -1.000000 9.417350 0.000000 0.000000 0.000000 0.000000 253 | 254 | 255 | 256 | floor_lamp_1 257 | model://floor_lamp 258 | 4.000000 4.00000 0.000000 0.000000 0.000000 0.000000 259 | 260 | 261 | 262 | male03 263 | model://citizen_extras_male_03 264 | 4.0 0.53 0.01 0.0125 0.0023 -1.718 265 | 266 | 267 | 268 | 269 | 3.6 0.53 1.6 0 0 0 270 | 0.5 0.5 0.5 1 271 | 0.1 0.1 0.1 1 272 | 273 | 274 | 1.0 275 | 0.01 276 | 0.3 277 | 0.0 278 | 279 | 0 0 -1 280 | 281 | 0.6 282 | 0.8 283 | 1.2 284 | 285 | false 286 | 287 | 288 | 289 | 290 | female02 291 | model://citizen_extras_female_02 292 | -2.84 -2.63 0.01 0 -0.025 1.683 293 | 294 | 295 | 296 | 297 | -2.5 -2.63 1.5 0 0 0 298 | 0.5 0.5 0.5 1 299 | 0.1 0.1 0.1 1 300 | 301 | 302 | 1.0 303 | 0.01 304 | 0.3 305 | 0.0 306 | 307 | 0 0 -1 308 | 309 | 0.6 310 | 0.8 311 | 1.2 312 | 313 | false 314 | 315 | 316 | 317 | 318 | 319 | female03 320 | model://citizen_extras_female_03 321 | -3.3 5.8 0.01 0.011 0.0126 1.6451 322 | 323 | 324 | 325 | 326 | -3.0 6.2 1.5 0 0 0 327 | 0.5 0.5 0.5 1 328 | 0.1 0.1 0.1 1 329 | 330 | 331 | 1.0 332 | 0.01 333 | 0.3 334 | 0.0 335 | 336 | 0 0 -1 337 | 338 | 0.6 339 | 0.8 340 | 1.2 341 | 342 | false 343 | 344 | 345 | 346 | 347 | female02bis 348 | model://citizen_extras_female_02 349 | 4.000000 7.000000 0.000000 0.000000 0.000000 0.000000 350 | 351 | 352 | 353 | 354 | 3.8 6.7 1.5 0 0 0 355 | 0.5 0.5 0.5 1 356 | 0.1 0.1 0.1 1 357 | 358 | 359 | 1.0 360 | 0.01 361 | 0.3 362 | 0.0 363 | 364 | 0 0 -1 365 | 366 | 0.6 367 | 0.8 368 | 1.2 369 | 370 | false 371 | 372 | 373 | 374 | 375 | male03bis 376 | model://citizen_extras_male_03 377 | -3.000000 4.000000 0.000000 0.000000 0.000000 0.000000 378 | 379 | 380 | 381 | 382 | -2.9 3.7 1.5 0 0 0 383 | 0.5 0.5 0.5 1 384 | 0.1 0.1 0.1 1 385 | 386 | 387 | 1.0 388 | 0.01 389 | 0.3 390 | 0.0 391 | 392 | 0 0 -1 393 | 394 | 0.6 395 | 0.8 396 | 1.2 397 | 398 | false 399 | 400 | 401 | 402 | 403 | 404 | 0.685291 2.730300 18.477800 0.000000 1.529800 -3.138990 405 | orbit 406 | 407 | 408 | 409 | 410 | 411 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/testfiles/coucou/coucou1_modified.txt: -------------------------------------------------------------------------------- 1 | trajectory: 2 | header: 3 | seq: 0 4 | stamp: 5 | secs: 0 6 | nsecs: 0 7 | frame_id: /base_link 8 | joint_names: ['LElbowRoll', 'LElbowYaw', 'LShoulderPitch', 'LShoulderRoll'] 9 | points: 10 | - 11 | positions: [-0.03485785332450053, -0.012152274976565458, 0.006718742717878534, 3.182719041650017e-05] 12 | velocities: [0.0, 0.0, 0.0, 0.0] 13 | accelerations: [0.0, 0.0, 0.0, 0.0] 14 | effort: [] 15 | time_from_start: 16 | secs: 0 17 | nsecs: 0 18 | - 19 | positions: [-0.0349066, -0.012152274976565458, 0.006718742717878534, 3.182719041650017e-05] 20 | velocities: [-0.15050693408126192, -0.02766641168371117, -0.13821249261125107, 0.15030493525257793] 21 | accelerations: [-2.909640581762744, -0.5370284378856813, -2.6828213160366747, 2.9175458497468907] 22 | effort: [] 23 | time_from_start: 24 | secs: 0 25 | nsecs: 80009749 26 | - 27 | positions: [-0.07277579024829593, -0.019127564810767675, -0.02812756137433644, 0.03792689283476226] 28 | velocities: [-0.4385944639481072, -0.08078661005571064, -0.4035839151481592, 0.4388941483455161] 29 | accelerations: [-2.8832149992360074, -0.5310718328574576, -2.6530640335287954, 2.8851850527151797] 30 | effort: [] 31 | time_from_start: 32 | secs: 0 33 | nsecs: 206070365 34 | - 35 | positions: [-0.11064498049659185, -0.02610285464496989, -0.06297386546655141, 0.07582195847910803] 36 | velocities: [-0.6581689063738112, -0.12123097567484487, -0.6056309550103051, 0.6586186223832755] 37 | accelerations: [-2.785683807568797, -0.5131071411042571, -2.563318212690581, 2.787587219585736] 38 | effort: [] 39 | time_from_start: 40 | secs: 0 41 | nsecs: 271726086 42 | - 43 | positions: [-0.1485141707448878, -0.03307814447917211, -0.09782016955876639, 0.11371702412345379] 44 | velocities: [-0.8077049868926409, -0.1487746726868604, -0.7432304045985242, 0.8082568784511939] 45 | accelerations: [-2.8864838238744808, -0.5316739317964901, -2.656071926135668, 2.8884561108878652] 46 | effort: [] 47 | time_from_start: 48 | secs: 0 49 | nsecs: 322931564 50 | - 51 | positions: [-0.18638336099318373, -0.040053434313374325, -0.13266647365098136, 0.15161208976779955] 52 | velocities: [-0.9313967421982206, -0.17155799173066974, -0.8570485372498298, 0.9320331503026951] 53 | accelerations: [-2.7223224742649323, -0.5014363432557155, -2.505014661082589, 2.724182592522952] 54 | effort: [] 55 | time_from_start: 56 | secs: 0 57 | nsecs: 366168317 58 | - 59 | positions: [-0.22425255124147964, -0.04702872414757654, -0.1675127777431963, 0.1895071554121453] 60 | velocities: [-1.0331677356208924, -0.1903036308949444, -0.9506957201264913, 1.0338736821745373] 61 | accelerations: [-2.51753277731309, -0.4637152438096549, -2.3165721829586525, 2.519252966132928] 62 | effort: [] 63 | time_from_start: 64 | secs: 0 65 | nsecs: 404538740 66 | - 67 | positions: [-0.2621217414897756, -0.05400401398177876, -0.20235908183541132, 0.22740222105649108] 68 | velocities: [-1.118329861045663, -0.20599001087401425, -1.0290598282638297, 1.1190939974816203] 69 | accelerations: [-2.296609848873957, -0.4230224947231424, -2.113284299197238, 2.298179084683387] 70 | effort: [] 71 | time_from_start: 72 | secs: 0 73 | nsecs: 439622346 74 | - 75 | positions: [-0.2999909317380715, -0.06097930381598098, -0.2372053859276263, 0.26529728670083685] 76 | velocities: [-1.1928586986340317, -0.2197177996061339, -1.097639444601426, 1.1936737594012745] 77 | accelerations: [-2.240602684552759, -0.41270629304648926, -2.0617478742965907, 2.2421336515861596] 78 | effort: [] 79 | time_from_start: 80 | secs: 0 81 | nsecs: 472345459 82 | - 83 | positions: [-0.33786012198636745, -0.06795459365018319, -0.27205169001984125, 0.3031923523451826] 84 | velocities: [-1.2662435454197105, -0.23323487172762372, -1.165166388538904, 1.2671087488481025] 85 | accelerations: [-2.5247671875349, -0.46504778109779504, -2.3232291105788496, 2.526492319508488] 86 | effort: [] 87 | time_from_start: 88 | secs: 0 89 | nsecs: 503172111 90 | - 91 | positions: [-0.3757293122346633, -0.07492988348438541, -0.3068979941120562, 0.3410874179895283] 92 | velocities: [-1.3372902908670574, -0.2463212788576081, -1.2305418687218173, 1.338204039370337] 93 | accelerations: [-2.3475486697502372, -0.43240513631368405, -2.1601569582299165, 2.3491527112198427] 94 | effort: [] 95 | time_from_start: 96 | secs: 0 97 | nsecs: 532212212 98 | - 99 | positions: [-0.41359850248295926, -0.08190517331858763, -0.34174429820427116, 0.3789824836338741] 100 | velocities: [-1.398374538383018, -0.25757265043260846, -1.2867501015947407, 1.3993300247498874] 101 | accelerations: [-2.0541360174347782, -0.3783601916633449, -1.8901658007732254, 2.055539574855393] 102 | effort: [] 103 | time_from_start: 104 | secs: 0 105 | nsecs: 559842871 106 | - 107 | positions: [-0.45146769273125525, -0.08888046315278984, -0.37659060229648617, 0.41687754927821985] 108 | velocities: [-1.4551541521232674, -0.2680311329065006, -1.3389973155873953, 1.4561484350681322] 109 | accelerations: [-2.2243349607511385, -0.4097098706853574, -2.0467787121159717, 2.225854812316108] 110 | effort: [] 111 | time_from_start: 112 | secs: 0 113 | nsecs: 586395392 114 | - 115 | positions: [-0.4893368829795512, -0.09585575298699206, -0.4114369063887012, 0.45477261492256565] 116 | velocities: [-1.5065938516529105, -0.27750603350117886, -1.3863308709253739, 1.5076232825000506] 117 | accelerations: [-1.7886943984450154, -0.32946735254078885, -1.645912905123744, 1.7899165839649165] 118 | effort: [] 119 | time_from_start: 120 | secs: 0 121 | nsecs: 611911842 122 | - 123 | positions: [-0.5272060732278471, -0.10283104282119429, -0.4462832104809161, 0.4926676805669114] 124 | velocities: [-1.5522451519518454, -0.2859147438222981, -1.4283380826452836, 1.5533057755830848] 125 | accelerations: [-1.898735147216573, -0.34973623368713663, -1.7471697149231487, 1.9000325217737304] 126 | effort: [] 127 | time_from_start: 128 | secs: 0 129 | nsecs: 636677858 130 | - 131 | positions: [-0.565075263476143, -0.10980633265539651, -0.48112951457313113, 0.5305627462112572] 132 | velocities: [-1.5992797323011416, -0.29457824647485736, -1.4716181548875193, 1.6003724938890276] 133 | accelerations: [-2.0155456194250054, -0.37125205945426176, -1.8546558589107511, 2.0169228086599866] 134 | effort: [] 135 | time_from_start: 136 | secs: 0 137 | nsecs: 660715508 138 | - 139 | positions: [-0.602944453724439, -0.11678162248959871, -0.515975818665346, 0.5684578118556028] 140 | velocities: [-1.656101162526091, -0.3050444313078102, -1.523903847388226, 1.6572327492644912] 141 | accelerations: [-2.8810839045154677, -0.5306792973096568, -2.6511030522089842, 2.8830525018530344] 142 | effort: [] 143 | time_from_start: 144 | secs: 0 145 | nsecs: 684046215 146 | - 147 | positions: [-0.6408136439727349, -0.12375691232380093, -0.550822122757561, 0.6063528774999487] 148 | velocities: [-1.6724137999626751, -0.30804912650552574, -1.5389143379990446, 1.6735565328584558] 149 | accelerations: [1.4696423294319516, 0.2706997729074049, 1.352328982542252, -1.4706465119107557] 150 | effort: [] 151 | time_from_start: 152 | secs: 0 153 | nsecs: 706466566 154 | - 155 | positions: [-0.6786828342210308, -0.13073220215800313, -0.585668426849776, 0.6442479431442945] 156 | velocities: [-1.6314252549203931, -0.30049926922900244, -1.5011976798007594, 1.6325399810161971] 157 | accelerations: [2.09738485391131, 0.3863263817887474, 1.9299623239524837, -2.098817962552083] 158 | effort: [] 159 | time_from_start: 160 | secs: 0 161 | nsecs: 729337566 162 | - 163 | positions: [-0.7165520244693266, -0.13770749199220536, -0.6205147309419909, 0.6821430087886401] 164 | velocities: [-1.5834452795060774, -0.2916616301731262, -1.4570476781064539, 1.5845272216723048] 165 | accelerations: [1.9758314081216581, 0.36393692721728726, 1.818111811499703, -1.977181461288106] 166 | effort: [] 167 | time_from_start: 168 | secs: 0 169 | nsecs: 752901580 170 | - 171 | positions: [-0.7544212147176226, -0.14468278182640756, -0.655361035034206, 0.720038074432986] 172 | velocities: [-1.5368763880711338, -0.28308390477455203, -1.4141961214309717, 1.5379265104783015] 173 | accelerations: [1.861322563686287, 0.3428450482179145, 1.7127435691825736, -1.8625943748394558] 174 | effort: [] 175 | time_from_start: 176 | secs: 0 177 | nsecs: 777179607 178 | - 179 | positions: [-0.7922904049659185, -0.1516580716606098, -0.6902073391264208, 0.7579331400773317] 180 | velocities: [-1.484402750580947, -0.273418558677922, -1.3659111616307007, 1.4854170186130013] 181 | accelerations: [2.3146518797408016, 0.42634573437177764, 2.1298861354104, -2.316233443373903] 182 | effort: [] 183 | time_from_start: 184 | secs: 0 185 | nsecs: 802193282 186 | - 187 | positions: [-0.8301595952142146, -0.15863336149481203, -0.7250536432186359, 0.7958282057216775] 188 | velocities: [-1.4264818666045183, -0.262749860706304, -1.3126137786367966, 1.42745655824736] 189 | accelerations: [2.13754217682809, 0.393723132669275, 1.9669141118493554, -2.1390027243082415] 190 | effort: [] 191 | time_from_start: 192 | secs: 0 193 | nsecs: 828222613 194 | - 195 | positions: [-0.8680287854625105, -0.16560865132901423, -0.7598999473108509, 0.8337232713660232] 196 | velocities: [-1.3641698257134842, -0.2512723365626457, -1.255275760283126, 1.3651019405616809] 197 | accelerations: [2.4428685594124175, 0.449962518792622, 2.2478679929636707, -2.444537731397687] 198 | effort: [] 199 | time_from_start: 200 | secs: 0 201 | nsecs: 855308840 202 | - 203 | positions: [-0.9058979757108064, -0.17258394116321646, -0.7947462514030659, 0.8716183370103691] 204 | velocities: [-1.2979607812348588, -0.2390770064841321, -1.1943518143938687, 1.2988476565298268] 205 | accelerations: [2.211497038865472, 0.40734519836378236, 2.0349655699016975, -2.2130081184891353] 206 | effort: [] 207 | time_from_start: 208 | secs: 0 209 | nsecs: 883776736 210 | - 211 | positions: [-0.9437671659591024, -0.17955923099741866, -0.8295925554952809, 0.9095134026547148] 212 | velocities: [-1.2290035100613417, -0.22637546865199942, -1.1308990174122933, 1.2298432679848923] 213 | accelerations: [2.378444899143439, 0.4380960463486739, 2.1885869140247483, -2.38007005149933] 214 | effort: [] 215 | time_from_start: 216 | secs: 0 217 | nsecs: 913696781 218 | - 219 | positions: [-0.9816363562073983, -0.1865345208316209, -0.8644388595874958, 0.9474084682990606] 220 | velocities: [-1.152216374165206, -0.21223171419350811, -1.060241370120051, 1.1530036646993103] 221 | accelerations: [2.4378976326485264, 0.4490469023061153, 2.2432938675465035, -2.4395634080811766] 222 | effort: [] 223 | time_from_start: 224 | secs: 0 225 | nsecs: 945457511 226 | - 227 | positions: [-1.0195055464556941, -0.19350981066582312, -0.8992851636797108, 0.9853035339434063] 228 | velocities: [-1.0666086528239842, -0.19646325797658315, -0.9814672355019494, 1.0673374490074026] 229 | accelerations: [2.558219164840072, 0.4712094453874602, 2.354010803190179, -2.5599671540006232] 230 | effort: [] 231 | time_from_start: 232 | secs: 0 233 | nsecs: 979509314 234 | - 235 | positions: [-1.05737473670399, -0.20048510050002533, -0.9341314677719258, 1.0231985995877522] 236 | velocities: [-1.066608652823986, -0.19646325797658315, -0.9814672355019494, 1.0673374490074026] 237 | accelerations: [-2.5582191648401658, -0.4712094453874602, -2.354010803190179, 2.5599671540006232] 238 | effort: [] 239 | time_from_start: 240 | secs: 1 241 | nsecs: 16595540 242 | - 243 | positions: [-1.095243926952286, -0.20746039033422756, -0.9689777718641408, 1.061093665232098] 244 | velocities: [-1.134683287517099, -0.2090022191803619, -1.044107851948438, 1.1354585979809113] 245 | accelerations: [-1.3524820152497394, -0.2491195082349669, -1.2445209225132854, 1.3534061440089087] 246 | effort: [] 247 | time_from_start: 248 | secs: 1 249 | nsecs: 50647342 250 | - 251 | positions: [-1.133113117200582, -0.21443568016842976, -1.0038240759563557, 1.0989887308764437] 252 | velocities: [-1.1183298610456665, -0.2059900108740144, -1.0290598282638301, 1.1190939974816199] 253 | accelerations: [2.2966098488737687, 0.4230224947231005, 2.113284299197081, -2.2981790846833823] 254 | effort: [] 255 | time_from_start: 256 | secs: 1 257 | nsecs: 83370455 258 | - 259 | positions: [-1.1709823074488779, -0.22141097000263196, -1.0386703800485706, 1.1368837965207892] 260 | velocities: [-1.0331677356208937, -0.19030363089494454, -0.9506957201264897, 1.0338736821745376] 261 | accelerations: [2.5175327773133986, 0.46371524380969426, 2.3165721829589065, -2.5192529661329046] 262 | effort: [] 263 | time_from_start: 264 | secs: 1 265 | nsecs: 118454060 266 | - 267 | positions: [-1.2088514976971738, -0.2283862598368342, -1.0735166841407855, 1.1747788621651352] 268 | velocities: [-0.9357760245980518, -0.1723646306845094, -0.8610782459710796, 0.9364154249943037] 269 | accelerations: [2.52089449797068, 0.4643344536679946, 2.319665556213624, -2.522616983799201] 270 | effort: [] 271 | time_from_start: 272 | secs: 1 273 | nsecs: 156824483 274 | - 275 | positions: [-1.2467206879454698, -0.2353615496710364, -1.1083629882330006, 1.212673927809481] 276 | velocities: [-0.8157820367615674, -0.15026241941372898, -0.7506627086445103, 0.8163394472357055] 277 | accelerations: [2.9445021487343563, 0.5423605778255867, 2.709458971851103, -2.94651407871671] 278 | effort: [] 279 | time_from_start: 280 | secs: 1 281 | nsecs: 199633149 282 | - 283 | positions: [-1.2845898781937657, -0.24233683950523863, -1.1432092923252155, 1.2505689934538267] 284 | velocities: [-0.6606990613008243, -0.12169701584696463, -0.6079591417871821, 0.6611505061236158] 285 | accelerations: [2.9582967922255317, 0.5449014728349686, 2.722152465923173, -2.9603181478613343] 286 | effort: [] 287 | time_from_start: 288 | secs: 1 289 | nsecs: 250331643 290 | - 291 | positions: [-1.3224590684420616, -0.24931212933944083, -1.1780555964174306, 1.2884640590981724] 292 | velocities: [-0.4021483738890168, -0.07407344719643787, -0.37004711309762056, 0.40242315527137384] 293 | accelerations: [2.9876957963791204, 0.5503166024816084, 2.7492047116150027, -2.9897372398718596] 294 | effort: [] 295 | time_from_start: 296 | secs: 1 297 | nsecs: 316254264 298 | - 299 | positions: [-1.3603282586903576, -0.25628741917364306, -1.2129019005096455, 1.3263591247425182] 300 | velocities: [0.0, 0.0, 0.0, 0.0] 301 | accelerations: [2.790127999684651, 0.5139257360592524, 2.5674076497473997, -2.7920344483451096] 302 | effort: [] 303 | time_from_start: 304 | secs: 1 305 | nsecs: 481012049 306 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/testfiles/coucou/coucou1.txt: -------------------------------------------------------------------------------- 1 | header: 2 | seq: 0 3 | stamp: 4 | secs: 55 5 | nsecs: 959000000 6 | frame_id: '' 7 | goal_id: 8 | stamp: 9 | secs: 55 10 | nsecs: 959000000 11 | id: /move_group-1-55.959000000 12 | goal: 13 | trajectory: 14 | header: 15 | seq: 0 16 | stamp: 17 | secs: 0 18 | nsecs: 0 19 | frame_id: /base_link 20 | joint_names: ['LElbowRoll', 'LElbowYaw', 'LShoulderPitch', 'LShoulderRoll'] 21 | points: 22 | - 23 | positions: [-0.03485785332450053, -0.012152274976565458, 0.006718742717878534, 3.182719041650017e-05] 24 | velocities: [0.0, 0.0, 0.0, 0.0] 25 | accelerations: [0.0, 0.0, 0.0, 0.0] 26 | effort: [] 27 | time_from_start: 28 | secs: 0 29 | nsecs: 0 30 | - 31 | positions: [-0.0349066, -0.012152274976565458, 0.006718742717878534, 3.182719041650017e-05] 32 | velocities: [-0.15050693408126192, -0.02766641168371117, -0.13821249261125107, 0.15030493525257793] 33 | accelerations: [-2.909640581762744, -0.5370284378856813, -2.6828213160366747, 2.9175458497468907] 34 | effort: [] 35 | time_from_start: 36 | secs: 0 37 | nsecs: 80009749 38 | - 39 | positions: [-0.07277579024829593, -0.019127564810767675, -0.02812756137433644, 0.03792689283476226] 40 | velocities: [-0.4385944639481072, -0.08078661005571064, -0.4035839151481592, 0.4388941483455161] 41 | accelerations: [-2.8832149992360074, -0.5310718328574576, -2.6530640335287954, 2.8851850527151797] 42 | effort: [] 43 | time_from_start: 44 | secs: 0 45 | nsecs: 206070365 46 | - 47 | positions: [-0.11064498049659185, -0.02610285464496989, -0.06297386546655141, 0.07582195847910803] 48 | velocities: [-0.6581689063738112, -0.12123097567484487, -0.6056309550103051, 0.6586186223832755] 49 | accelerations: [-2.785683807568797, -0.5131071411042571, -2.563318212690581, 2.787587219585736] 50 | effort: [] 51 | time_from_start: 52 | secs: 0 53 | nsecs: 271726086 54 | - 55 | positions: [-0.1485141707448878, -0.03307814447917211, -0.09782016955876639, 0.11371702412345379] 56 | velocities: [-0.8077049868926409, -0.1487746726868604, -0.7432304045985242, 0.8082568784511939] 57 | accelerations: [-2.8864838238744808, -0.5316739317964901, -2.656071926135668, 2.8884561108878652] 58 | effort: [] 59 | time_from_start: 60 | secs: 0 61 | nsecs: 322931564 62 | - 63 | positions: [-0.18638336099318373, -0.040053434313374325, -0.13266647365098136, 0.15161208976779955] 64 | velocities: [-0.9313967421982206, -0.17155799173066974, -0.8570485372498298, 0.9320331503026951] 65 | accelerations: [-2.7223224742649323, -0.5014363432557155, -2.505014661082589, 2.724182592522952] 66 | effort: [] 67 | time_from_start: 68 | secs: 0 69 | nsecs: 366168317 70 | - 71 | positions: [-0.22425255124147964, -0.04702872414757654, -0.1675127777431963, 0.1895071554121453] 72 | velocities: [-1.0331677356208924, -0.1903036308949444, -0.9506957201264913, 1.0338736821745373] 73 | accelerations: [-2.51753277731309, -0.4637152438096549, -2.3165721829586525, 2.519252966132928] 74 | effort: [] 75 | time_from_start: 76 | secs: 0 77 | nsecs: 404538740 78 | - 79 | positions: [-0.2621217414897756, -0.05400401398177876, -0.20235908183541132, 0.22740222105649108] 80 | velocities: [-1.118329861045663, -0.20599001087401425, -1.0290598282638297, 1.1190939974816203] 81 | accelerations: [-2.296609848873957, -0.4230224947231424, -2.113284299197238, 2.298179084683387] 82 | effort: [] 83 | time_from_start: 84 | secs: 0 85 | nsecs: 439622346 86 | - 87 | positions: [-0.2999909317380715, -0.06097930381598098, -0.2372053859276263, 0.26529728670083685] 88 | velocities: [-1.1928586986340317, -0.2197177996061339, -1.097639444601426, 1.1936737594012745] 89 | accelerations: [-2.240602684552759, -0.41270629304648926, -2.0617478742965907, 2.2421336515861596] 90 | effort: [] 91 | time_from_start: 92 | secs: 0 93 | nsecs: 472345459 94 | - 95 | positions: [-0.33786012198636745, -0.06795459365018319, -0.27205169001984125, 0.3031923523451826] 96 | velocities: [-1.2662435454197105, -0.23323487172762372, -1.165166388538904, 1.2671087488481025] 97 | accelerations: [-2.5247671875349, -0.46504778109779504, -2.3232291105788496, 2.526492319508488] 98 | effort: [] 99 | time_from_start: 100 | secs: 0 101 | nsecs: 503172111 102 | - 103 | positions: [-0.3757293122346633, -0.07492988348438541, -0.3068979941120562, 0.3410874179895283] 104 | velocities: [-1.3372902908670574, -0.2463212788576081, -1.2305418687218173, 1.338204039370337] 105 | accelerations: [-2.3475486697502372, -0.43240513631368405, -2.1601569582299165, 2.3491527112198427] 106 | effort: [] 107 | time_from_start: 108 | secs: 0 109 | nsecs: 532212212 110 | - 111 | positions: [-0.41359850248295926, -0.08190517331858763, -0.34174429820427116, 0.3789824836338741] 112 | velocities: [-1.398374538383018, -0.25757265043260846, -1.2867501015947407, 1.3993300247498874] 113 | accelerations: [-2.0541360174347782, -0.3783601916633449, -1.8901658007732254, 2.055539574855393] 114 | effort: [] 115 | time_from_start: 116 | secs: 0 117 | nsecs: 559842871 118 | - 119 | positions: [-0.45146769273125525, -0.08888046315278984, -0.37659060229648617, 0.41687754927821985] 120 | velocities: [-1.4551541521232674, -0.2680311329065006, -1.3389973155873953, 1.4561484350681322] 121 | accelerations: [-2.2243349607511385, -0.4097098706853574, -2.0467787121159717, 2.225854812316108] 122 | effort: [] 123 | time_from_start: 124 | secs: 0 125 | nsecs: 586395392 126 | - 127 | positions: [-0.4893368829795512, -0.09585575298699206, -0.4114369063887012, 0.45477261492256565] 128 | velocities: [-1.5065938516529105, -0.27750603350117886, -1.3863308709253739, 1.5076232825000506] 129 | accelerations: [-1.7886943984450154, -0.32946735254078885, -1.645912905123744, 1.7899165839649165] 130 | effort: [] 131 | time_from_start: 132 | secs: 0 133 | nsecs: 611911842 134 | - 135 | positions: [-0.5272060732278471, -0.10283104282119429, -0.4462832104809161, 0.4926676805669114] 136 | velocities: [-1.5522451519518454, -0.2859147438222981, -1.4283380826452836, 1.5533057755830848] 137 | accelerations: [-1.898735147216573, -0.34973623368713663, -1.7471697149231487, 1.9000325217737304] 138 | effort: [] 139 | time_from_start: 140 | secs: 0 141 | nsecs: 636677858 142 | - 143 | positions: [-0.565075263476143, -0.10980633265539651, -0.48112951457313113, 0.5305627462112572] 144 | velocities: [-1.5992797323011416, -0.29457824647485736, -1.4716181548875193, 1.6003724938890276] 145 | accelerations: [-2.0155456194250054, -0.37125205945426176, -1.8546558589107511, 2.0169228086599866] 146 | effort: [] 147 | time_from_start: 148 | secs: 0 149 | nsecs: 660715508 150 | - 151 | positions: [-0.602944453724439, -0.11678162248959871, -0.515975818665346, 0.5684578118556028] 152 | velocities: [-1.656101162526091, -0.3050444313078102, -1.523903847388226, 1.6572327492644912] 153 | accelerations: [-2.8810839045154677, -0.5306792973096568, -2.6511030522089842, 2.8830525018530344] 154 | effort: [] 155 | time_from_start: 156 | secs: 0 157 | nsecs: 684046215 158 | - 159 | positions: [-0.6408136439727349, -0.12375691232380093, -0.550822122757561, 0.6063528774999487] 160 | velocities: [-1.6724137999626751, -0.30804912650552574, -1.5389143379990446, 1.6735565328584558] 161 | accelerations: [1.4696423294319516, 0.2706997729074049, 1.352328982542252, -1.4706465119107557] 162 | effort: [] 163 | time_from_start: 164 | secs: 0 165 | nsecs: 706466566 166 | - 167 | positions: [-0.6786828342210308, -0.13073220215800313, -0.585668426849776, 0.6442479431442945] 168 | velocities: [-1.6314252549203931, -0.30049926922900244, -1.5011976798007594, 1.6325399810161971] 169 | accelerations: [2.09738485391131, 0.3863263817887474, 1.9299623239524837, -2.098817962552083] 170 | effort: [] 171 | time_from_start: 172 | secs: 0 173 | nsecs: 729337566 174 | - 175 | positions: [-0.7165520244693266, -0.13770749199220536, -0.6205147309419909, 0.6821430087886401] 176 | velocities: [-1.5834452795060774, -0.2916616301731262, -1.4570476781064539, 1.5845272216723048] 177 | accelerations: [1.9758314081216581, 0.36393692721728726, 1.818111811499703, -1.977181461288106] 178 | effort: [] 179 | time_from_start: 180 | secs: 0 181 | nsecs: 752901580 182 | - 183 | positions: [-0.7544212147176226, -0.14468278182640756, -0.655361035034206, 0.720038074432986] 184 | velocities: [-1.5368763880711338, -0.28308390477455203, -1.4141961214309717, 1.5379265104783015] 185 | accelerations: [1.861322563686287, 0.3428450482179145, 1.7127435691825736, -1.8625943748394558] 186 | effort: [] 187 | time_from_start: 188 | secs: 0 189 | nsecs: 777179607 190 | - 191 | positions: [-0.7922904049659185, -0.1516580716606098, -0.6902073391264208, 0.7579331400773317] 192 | velocities: [-1.484402750580947, -0.273418558677922, -1.3659111616307007, 1.4854170186130013] 193 | accelerations: [2.3146518797408016, 0.42634573437177764, 2.1298861354104, -2.316233443373903] 194 | effort: [] 195 | time_from_start: 196 | secs: 0 197 | nsecs: 802193282 198 | - 199 | positions: [-0.8301595952142146, -0.15863336149481203, -0.7250536432186359, 0.7958282057216775] 200 | velocities: [-1.4264818666045183, -0.262749860706304, -1.3126137786367966, 1.42745655824736] 201 | accelerations: [2.13754217682809, 0.393723132669275, 1.9669141118493554, -2.1390027243082415] 202 | effort: [] 203 | time_from_start: 204 | secs: 0 205 | nsecs: 828222613 206 | - 207 | positions: [-0.8680287854625105, -0.16560865132901423, -0.7598999473108509, 0.8337232713660232] 208 | velocities: [-1.3641698257134842, -0.2512723365626457, -1.255275760283126, 1.3651019405616809] 209 | accelerations: [2.4428685594124175, 0.449962518792622, 2.2478679929636707, -2.444537731397687] 210 | effort: [] 211 | time_from_start: 212 | secs: 0 213 | nsecs: 855308840 214 | - 215 | positions: [-0.9058979757108064, -0.17258394116321646, -0.7947462514030659, 0.8716183370103691] 216 | velocities: [-1.2979607812348588, -0.2390770064841321, -1.1943518143938687, 1.2988476565298268] 217 | accelerations: [2.211497038865472, 0.40734519836378236, 2.0349655699016975, -2.2130081184891353] 218 | effort: [] 219 | time_from_start: 220 | secs: 0 221 | nsecs: 883776736 222 | - 223 | positions: [-0.9437671659591024, -0.17955923099741866, -0.8295925554952809, 0.9095134026547148] 224 | velocities: [-1.2290035100613417, -0.22637546865199942, -1.1308990174122933, 1.2298432679848923] 225 | accelerations: [2.378444899143439, 0.4380960463486739, 2.1885869140247483, -2.38007005149933] 226 | effort: [] 227 | time_from_start: 228 | secs: 0 229 | nsecs: 913696781 230 | - 231 | positions: [-0.9816363562073983, -0.1865345208316209, -0.8644388595874958, 0.9474084682990606] 232 | velocities: [-1.152216374165206, -0.21223171419350811, -1.060241370120051, 1.1530036646993103] 233 | accelerations: [2.4378976326485264, 0.4490469023061153, 2.2432938675465035, -2.4395634080811766] 234 | effort: [] 235 | time_from_start: 236 | secs: 0 237 | nsecs: 945457511 238 | - 239 | positions: [-1.0195055464556941, -0.19350981066582312, -0.8992851636797108, 0.9853035339434063] 240 | velocities: [-1.0666086528239842, -0.19646325797658315, -0.9814672355019494, 1.0673374490074026] 241 | accelerations: [2.558219164840072, 0.4712094453874602, 2.354010803190179, -2.5599671540006232] 242 | effort: [] 243 | time_from_start: 244 | secs: 0 245 | nsecs: 979509314 246 | - 247 | positions: [-1.05737473670399, -0.20048510050002533, -0.9341314677719258, 1.0231985995877522] 248 | velocities: [-1.066608652823986, -0.19646325797658315, -0.9814672355019494, 1.0673374490074026] 249 | accelerations: [-2.5582191648401658, -0.4712094453874602, -2.354010803190179, 2.5599671540006232] 250 | effort: [] 251 | time_from_start: 252 | secs: 1 253 | nsecs: 16595540 254 | - 255 | positions: [-1.095243926952286, -0.20746039033422756, -0.9689777718641408, 1.061093665232098] 256 | velocities: [-1.134683287517099, -0.2090022191803619, -1.044107851948438, 1.1354585979809113] 257 | accelerations: [-1.3524820152497394, -0.2491195082349669, -1.2445209225132854, 1.3534061440089087] 258 | effort: [] 259 | time_from_start: 260 | secs: 1 261 | nsecs: 50647342 262 | - 263 | positions: [-1.133113117200582, -0.21443568016842976, -1.0038240759563557, 1.0989887308764437] 264 | velocities: [-1.1183298610456665, -0.2059900108740144, -1.0290598282638301, 1.1190939974816199] 265 | accelerations: [2.2966098488737687, 0.4230224947231005, 2.113284299197081, -2.2981790846833823] 266 | effort: [] 267 | time_from_start: 268 | secs: 1 269 | nsecs: 83370455 270 | - 271 | positions: [-1.1709823074488779, -0.22141097000263196, -1.0386703800485706, 1.1368837965207892] 272 | velocities: [-1.0331677356208937, -0.19030363089494454, -0.9506957201264897, 1.0338736821745376] 273 | accelerations: [2.5175327773133986, 0.46371524380969426, 2.3165721829589065, -2.5192529661329046] 274 | effort: [] 275 | time_from_start: 276 | secs: 1 277 | nsecs: 118454060 278 | - 279 | positions: [-1.2088514976971738, -0.2283862598368342, -1.0735166841407855, 1.1747788621651352] 280 | velocities: [-0.9357760245980518, -0.1723646306845094, -0.8610782459710796, 0.9364154249943037] 281 | accelerations: [2.52089449797068, 0.4643344536679946, 2.319665556213624, -2.522616983799201] 282 | effort: [] 283 | time_from_start: 284 | secs: 1 285 | nsecs: 156824483 286 | - 287 | positions: [-1.2467206879454698, -0.2353615496710364, -1.1083629882330006, 1.212673927809481] 288 | velocities: [-0.8157820367615674, -0.15026241941372898, -0.7506627086445103, 0.8163394472357055] 289 | accelerations: [2.9445021487343563, 0.5423605778255867, 2.709458971851103, -2.94651407871671] 290 | effort: [] 291 | time_from_start: 292 | secs: 1 293 | nsecs: 199633149 294 | - 295 | positions: [-1.2845898781937657, -0.24233683950523863, -1.1432092923252155, 1.2505689934538267] 296 | velocities: [-0.6606990613008243, -0.12169701584696463, -0.6079591417871821, 0.6611505061236158] 297 | accelerations: [2.9582967922255317, 0.5449014728349686, 2.722152465923173, -2.9603181478613343] 298 | effort: [] 299 | time_from_start: 300 | secs: 1 301 | nsecs: 250331643 302 | - 303 | positions: [-1.3224590684420616, -0.24931212933944083, -1.1780555964174306, 1.2884640590981724] 304 | velocities: [-0.4021483738890168, -0.07407344719643787, -0.37004711309762056, 0.40242315527137384] 305 | accelerations: [2.9876957963791204, 0.5503166024816084, 2.7492047116150027, -2.9897372398718596] 306 | effort: [] 307 | time_from_start: 308 | secs: 1 309 | nsecs: 316254264 310 | - 311 | positions: [-1.3603282586903576, -0.25628741917364306, -1.2129019005096455, 1.3263591247425182] 312 | velocities: [0.0, 0.0, 0.0, 0.0] 313 | accelerations: [2.790127999684651, 0.5139257360592524, 2.5674076497473997, -2.7920344483451096] 314 | effort: [] 315 | time_from_start: 316 | secs: 1 317 | nsecs: 481012049 318 | path_tolerance: [] 319 | goal_tolerance: [] 320 | goal_time_tolerance: 321 | secs: 0 322 | nsecs: 0 323 | --- 324 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/scripts/pub_lasers.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (C) 2014 Aldebaran Robotics 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | # 17 | 18 | 19 | # import ROS dependencies 20 | import rospy 21 | from sensor_msgs.msg import PointCloud2, PointField 22 | 23 | # navigation test 24 | from sensor_msgs.msg import LaserScan 25 | 26 | # import PEPPER dependencies 27 | #from naoqi_driver.naoqi_node import NaoqiNode 28 | from qi import Session 29 | 30 | # general 31 | import copy 32 | import math 33 | import struct 34 | 35 | # PEPPER specifications: 36 | # https://community.aldebaran.com/doc/1-14/family/robots/laser_robot.html 37 | 38 | 39 | class NaoqiLaser(object): 40 | 41 | # PEPPER laser specs 42 | # see https://community.aldebaran.com/doc/2-1/family/juliette_technical/laser_juliette.html#juliette-laser 43 | PEPPER_LASER_FREQ = 6 # [hZ] --> check on that 44 | PEPPER_LASER_MIN_ANGLE = -0.523598776 # [rad] 45 | PEPPER_LASER_MAX_ANGLE = 0.523598776 # [radi] 46 | PEPPER_LASER_FOV = math.fabs( 47 | PEPPER_LASER_MIN_ANGLE) + math.fabs(PEPPER_LASER_MAX_ANGLE) 48 | 49 | PEPPER_LASER_MIN_RANGE = 0.1 # [m] --> no spec given here 50 | # [m] --> same here, 5m as quality guess 51 | PEPPER_LASER_MAX_RANGE = 5.0 52 | 53 | # FRONT GROUND LASERS 54 | PEPPER_LASER_GROUND_SHOVEL_POINTS = 3 55 | PEPPER_LASER_GROUND_LEFT_POINTS = 1 56 | PEPPER_LASER_GROUND_RIGHT_POINTS = 1 57 | # SURROUNDING LASER 58 | PEPPER_LASER_SRD_POINTS = 15 59 | 60 | # memory key to fetch laser readings from 61 | # see memory key listing https://community.aldebaran.com/doc/2-1/family/juliette_technical/juliette_dcm/actuator_sensor_names.html#lasers 62 | # iterate over all segments e.g./SubDeviceList/Platform/LaserSensor/Front/Shovel/Seg01/X/Value 63 | PEPPER_MEM_KEY_GROUND_SHOVEL = 'Device/SubDeviceList/Platform/LaserSensor/Front/Shovel/' 64 | PEPPER_MEM_KEY_GROUND_LEFT = 'Device/SubDeviceList/Platform/LaserSensor/Front/Vertical/Left/' 65 | PEPPER_MEM_KEY_GROUND_RIGHT = 'Device/SubDeviceList/Platform/LaserSensor/Front/Vertical/Right/' 66 | PEPPER_MEM_KEY_SRD_FRONT = 'Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/' 67 | PEPPER_MEM_KEY_SRD_LEFT = 'Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/' 68 | PEPPER_MEM_KEY_SRD_RIGHT = 'Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/' 69 | 70 | # ROS params to check 71 | # acc. to spec: 40 kHz 72 | PARAM_LASER_RATE = '~laser_rate' 73 | PARAM_LASER_RATE_DEFAULT = PEPPER_LASER_FREQ 74 | 75 | # frame id to publish 76 | PARAM_LASER_SHOVEL_FRAME = '~laser_shovel_frame_id' 77 | PARAM_LASER_SHOVEL_FRAME_DEFAULT = 'ShovelLaser_frame' 78 | 79 | PARAM_LASER_GROUND_LEFT_FRAME = '~laser_ground_left_frame_id' 80 | PARAM_LASER_GROUND_LEFT_FRAME_DEFAULT = 'VerticalLeftLaser_frame' 81 | 82 | PARAM_LASER_GROUND_RIGHT_FRAME = '~laser_ground_right_frame_id' 83 | PARAM_LASER_GROUND_RIGHT_FRAME_DEFAULT = 'VerticalRightLaser_frame' 84 | 85 | PARAM_LASER_SRD_FRONT_FRAME = '~laser_srd_front_frame_id' 86 | PARAM_LASER_SRD_FRONT_FRAME_DEFAULT = 'SurroundingFrontLaser_frame' 87 | 88 | PARAM_LASER_SRD_LEFT_FRAME = '~laser_srd_left_frame_id' 89 | PARAM_LASER_SRD_LEFT_FRAME_DEFAULT = 'SurroundingLeftLaser_frame' 90 | 91 | PARAM_LASER_SRD_RIGHT_FRAME = '~laser_srd_right_frame_id' 92 | PARAM_LASER_SRD_RIGHT_FRAME_DEFAULT = 'SurroundingRightLaser_frame' 93 | 94 | TOPIC_LASER_SHOVEL = '~/laser/shovel/' 95 | TOPIC_LASER_GROUND_LEFT = '~/laser/ground_left/' 96 | TOPIC_LASER_GROUND_RIGHT = '~/laser/ground_right/' 97 | TOPIC_LASER_SRD_FRONT = '/pepper/scan_front' 98 | TOPIC_LASER_SRD_LEFT = '/pepper/scan_left' 99 | TOPIC_LASER_SRD_RIGHT = '/pepper/scan_right' 100 | 101 | PEPPER_LASER_SUB_NAME = 'pepper_ros_laser' 102 | 103 | def __init__(self, pointcloud=True, laserscan=False): 104 | self.pointcloud = pointcloud 105 | self.laserscan = laserscan 106 | 107 | # NaoqiNode.__init__(self, 'pepper_node') 108 | self.session = Session() 109 | self.session.connect('tcp://localhost:9559') 110 | 111 | # ROS initialization; 112 | self.connectNaoQi() 113 | 114 | # default sensor rate: 25 Hz (50 is max, stresses Nao's CPU) 115 | self.laserRate = rospy.Rate(rospy.get_param( 116 | self.PARAM_LASER_RATE, 117 | self.PARAM_LASER_RATE_DEFAULT)) 118 | 119 | self.laserShovelFrame = rospy.get_param( 120 | self.PARAM_LASER_SHOVEL_FRAME, 121 | self.PARAM_LASER_SHOVEL_FRAME_DEFAULT) 122 | self.laserGroundLeftFrame = rospy.get_param( 123 | self.PARAM_LASER_GROUND_LEFT_FRAME, 124 | self.PARAM_LASER_GROUND_LEFT_FRAME_DEFAULT) 125 | self.laserGroundRightFrame = rospy.get_param( 126 | self.PARAM_LASER_GROUND_RIGHT_FRAME, 127 | self.PARAM_LASER_GROUND_RIGHT_FRAME_DEFAULT) 128 | self.laserSRDFrontFrame = rospy.get_param( 129 | self.PARAM_LASER_SRD_FRONT_FRAME, 130 | self.PARAM_LASER_SRD_FRONT_FRAME_DEFAULT) 131 | self.laserSRDLeftFrame = rospy.get_param( 132 | self.PARAM_LASER_SRD_LEFT_FRAME, 133 | self.PARAM_LASER_SRD_LEFT_FRAME_DEFAULT) 134 | self.laserSRDRightFrame = rospy.get_param( 135 | self.PARAM_LASER_SRD_RIGHT_FRAME, 136 | self.PARAM_LASER_SRD_RIGHT_FRAME_DEFAULT) 137 | 138 | if self.pointcloud: 139 | self.pcShovelPublisher = rospy.Publisher( 140 | self.TOPIC_LASER_SHOVEL + 'pointcloud', PointCloud2, queue_size=1) 141 | self.pcGroundLeftPublisher = rospy.Publisher( 142 | self.TOPIC_LASER_GROUND_LEFT + 'pointcloud', PointCloud2, queue_size=1) 143 | self.pcGroundRightPublisher = rospy.Publisher( 144 | self.TOPIC_LASER_GROUND_RIGHT + 'pointcloud', PointCloud2, queue_size=1) 145 | self.pcSRDFrontPublisher = rospy.Publisher( 146 | self.TOPIC_LASER_SRD_FRONT + 'pointcloud', PointCloud2, queue_size=1) 147 | self.pcSRDLeftPublisher = rospy.Publisher( 148 | self.TOPIC_LASER_SRD_LEFT + 'pointcloud', PointCloud2, queue_size=1) 149 | self.pcSRDRightPublisher = rospy.Publisher( 150 | self.TOPIC_LASER_SRD_RIGHT + 'pointcloud', PointCloud2, queue_size=1) 151 | 152 | if self.laserscan: 153 | self.laserShovelPublisher = rospy.Publisher( 154 | self.TOPIC_LASER_SHOVEL + 'scan', LaserScan, queue_size=1) 155 | self.laserGroundLeftPublisher = rospy.Publisher( 156 | self.TOPIC_LASER_GROUND_LEFT + 'scan', LaserScan, queue_size=1) 157 | self.laserGroundRightPublisher = rospy.Publisher( 158 | self.TOPIC_LASER_GROUND_RIGHT + 'scan', LaserScan, queue_size=1) 159 | self.laserSRDFrontPublisher = rospy.Publisher( 160 | self.TOPIC_LASER_SRD_FRONT, LaserScan, queue_size=1) 161 | self.laserSRDLeftPublisher = rospy.Publisher( 162 | self.TOPIC_LASER_SRD_LEFT, LaserScan, queue_size=1) 163 | self.laserSRDRightPublisher = rospy.Publisher( 164 | self.TOPIC_LASER_SRD_RIGHT, LaserScan, queue_size=1) 165 | 166 | self.laserSRDFrontPublisher_test = rospy.Publisher( 167 | "~/pepper_navigation/front", LaserScan, queue_size=1) 168 | 169 | # (re-) connect to NaoQI: 170 | def connectNaoQi(self): 171 | # rospy.loginfo("Connecting to NaoQi at %s:%d", self.pip, self.pport) 172 | # self.laserProxy = self.session.service("ALLaser") 173 | self.memProxy = self.session.service("ALMemory") 174 | if self.memProxy is None: 175 | print "could not start either laser or memory proxy" 176 | exit(1) 177 | 178 | # fetch laser values 179 | ''' the idea here is to get the point xy 180 | calculate the length from the origin (range) 181 | assumption: field of view spanning from very min to very max 182 | ''' 183 | 184 | def fetchLaserValues(self, keyPrefix, scanNum): 185 | ranges = [] 186 | # traverse backwards 187 | tmp_array = [] 188 | for i in xrange(scanNum, 0, -1): 189 | keyX = keyPrefix + 'Seg' + '%02d' % (i,) + '/X/Sensor/Value' 190 | keyY = keyPrefix + 'Seg' + '%02d' % (i,) + '/Y/Sensor/Value' 191 | tmp_array.append(keyX) 192 | tmp_array.append(keyY) 193 | memData = self.memProxy.getListData(tmp_array) 194 | for i in xrange(0, len(tmp_array), 2): 195 | x = memData[i] 196 | y = memData[i + 1] 197 | ranges.append(math.sqrt(math.pow(x, 2.0) + math.pow(y, 2.0))) 198 | return ranges 199 | 200 | def fetchPCValues(self, keyPrefix, scanNum): 201 | scans = [] 202 | for i in xrange(scanNum, 0, -1): 203 | keyX = keyPrefix + 'Seg' + '%02d' % (i,) + '/X/Sensor/Value' 204 | keyY = keyPrefix + 'Seg' + '%02d' % (i,) + '/Y/Sensor/Value' 205 | x = self.memProxy.getData(keyX) 206 | y = self.memProxy.getData(keyY) 207 | scans.append(x) 208 | scans.append(y) 209 | scans.append(0.0) 210 | ba = struct.pack('%sf' % len(scans), *scans) 211 | return ba 212 | 213 | def createPointCloudMessage(self, frameID, keyPrefix, scanNum): 214 | pointCloudMsg = PointCloud2() 215 | pointCloudMsg.header.frame_id = frameID 216 | pointCloudMsg.header.stamp = rospy.Time.now() 217 | pointCloudMsg.height = 1 218 | pointCloudMsg.width = scanNum 219 | pointCloudMsg.is_dense = False 220 | pointCloudMsg.is_bigendian = False 221 | pointCloudMsg.fields = [PointField('x', 0, PointField.FLOAT32, 1), 222 | PointField('y', 4, PointField.FLOAT32, 1), 223 | PointField('z', 8, PointField.FLOAT32, 1)] 224 | pointCloudMsg.point_step = 4 * 3 225 | pointCloudMsg.row_step = pointCloudMsg.point_step * pointCloudMsg.width 226 | pointCloudMsg.data = self.fetchLaserValues(keyPrefix, scanNum) 227 | return pointCloudMsg 228 | 229 | def createLaserMessage(self, frameID, keyPrefix, scanNum): 230 | laserScanMsg = LaserScan() 231 | laserScanMsg.header.frame_id = frameID 232 | laserScanMsg.angle_min = self.PEPPER_LASER_MIN_ANGLE 233 | laserScanMsg.angle_max = self.PEPPER_LASER_MAX_ANGLE 234 | laserScanMsg.angle_increment = self.PEPPER_LASER_FOV / scanNum 235 | laserScanMsg.range_min = self.PEPPER_LASER_MIN_RANGE 236 | laserScanMsg.range_max = self.PEPPER_LASER_MAX_RANGE 237 | return laserScanMsg 238 | 239 | # do it! 240 | def run(self): 241 | # start subscriber to laser sensor 242 | # self.laserProxy.subscribe(self.PEPPER_LASER_SUB_NAME) 243 | 244 | # we publish 6 laser messages in total 245 | # 1. shovel, 2. ground_left, 3. ground_right 246 | # 4. srd_front 5. srd_left 6. srd_right 247 | if self.pointcloud: 248 | shovelPC = self.createPointCloudMessage( 249 | self.laserShovelFrame, 250 | self.PEPPER_MEM_KEY_GROUND_SHOVEL, 251 | self.PEPPER_LASER_GROUND_SHOVEL_POINTS) 252 | groundLeftPC = self.createPointCloudMessage( 253 | self.laserGroundLeftFrame, 254 | self.PEPPER_MEM_KEY_GROUND_LEFT, 255 | self.PEPPER_LASER_GROUND_LEFT_POINTS) 256 | groundRightPC = self.createPointCloudMessage( 257 | self.laserGroundRightFrame, 258 | self.PEPPER_MEM_KEY_GROUND_RIGHT, 259 | self.PEPPER_LASER_GROUND_RIGHT_POINTS) 260 | srdFrontPC = self.createPointCloudMessage( 261 | self.laserSRDFrontFrame, 262 | self.PEPPER_MEM_KEY_SRD_FRONT, 263 | self.PEPPER_LASER_SRD_POINTS) 264 | srdLeftPC = self.createPointCloudMessage( 265 | self.laserSRDLeftFrame, 266 | self.PEPPER_MEM_KEY_SRD_LEFT, 267 | self.PEPPER_LASER_SRD_POINTS) 268 | srdRightPC = self.createPointCloudMessage( 269 | self.laserSRDRightFrame, 270 | self.PEPPER_MEM_KEY_SRD_RIGHT, 271 | self.PEPPER_LASER_SRD_POINTS) 272 | 273 | if self.laserscan: 274 | shovelScan = self.createLaserMessage( 275 | self.laserShovelFrame, 276 | self.PEPPER_MEM_KEY_GROUND_SHOVEL, 277 | self.PEPPER_LASER_GROUND_SHOVEL_POINTS) 278 | groundLeftScan = self.createLaserMessage( 279 | self.laserGroundLeftFrame, 280 | self.PEPPER_MEM_KEY_GROUND_LEFT, 281 | self.PEPPER_LASER_GROUND_LEFT_POINTS) 282 | groundRightScan = self.createLaserMessage( 283 | self.laserGroundRightFrame, 284 | self.PEPPER_MEM_KEY_GROUND_RIGHT, 285 | self.PEPPER_LASER_GROUND_RIGHT_POINTS) 286 | srdFrontScan = self.createLaserMessage( 287 | self.laserSRDFrontFrame, 288 | self.PEPPER_MEM_KEY_SRD_FRONT, 289 | self.PEPPER_LASER_SRD_POINTS) 290 | srdLeftScan = self.createLaserMessage( 291 | self.laserSRDLeftFrame, 292 | self.PEPPER_MEM_KEY_SRD_LEFT, 293 | self.PEPPER_LASER_SRD_POINTS) 294 | srdRightScan = self.createLaserMessage( 295 | self.laserSRDRightFrame, 296 | self.PEPPER_MEM_KEY_SRD_RIGHT, 297 | self.PEPPER_LASER_SRD_POINTS) 298 | 299 | while(not rospy.is_shutdown()): 300 | 301 | if self.laserscan: 302 | # fetch values 303 | now = rospy.Time.now() 304 | shovelScan.header.stamp = now 305 | shovelScan.ranges = self.fetchLaserValues( 306 | self.PEPPER_MEM_KEY_GROUND_SHOVEL, 307 | self.PEPPER_LASER_GROUND_SHOVEL_POINTS 308 | ) 309 | 310 | groundLeftScan.header.stamp = now 311 | groundLeftScan.ranges = self.fetchLaserValues( 312 | self.PEPPER_MEM_KEY_GROUND_LEFT, 313 | self.PEPPER_LASER_GROUND_LEFT_POINTS 314 | ) 315 | 316 | groundRightScan.header.stamp = now 317 | groundRightScan.ranges = self.fetchLaserValues( 318 | self.PEPPER_MEM_KEY_GROUND_RIGHT, 319 | self.PEPPER_LASER_GROUND_RIGHT_POINTS 320 | ) 321 | 322 | srdFrontScan.header.stamp = now 323 | srdFrontScan.ranges = self.fetchLaserValues( 324 | self.PEPPER_MEM_KEY_SRD_FRONT, 325 | self.PEPPER_LASER_SRD_POINTS 326 | ) 327 | 328 | srdLeftScan.header.stamp = now 329 | srdLeftScan.ranges = self.fetchLaserValues( 330 | self.PEPPER_MEM_KEY_SRD_LEFT, 331 | self.PEPPER_LASER_SRD_POINTS 332 | ) 333 | 334 | srdRightScan.header.stamp = now 335 | srdRightScan.ranges = self.fetchLaserValues( 336 | self.PEPPER_MEM_KEY_SRD_RIGHT, 337 | self.PEPPER_LASER_SRD_POINTS 338 | ) 339 | 340 | # publish messages 341 | self.laserShovelPublisher.publish(shovelScan) 342 | self.laserGroundLeftPublisher.publish(groundLeftScan) 343 | self.laserGroundRightPublisher.publish(groundRightScan) 344 | self.laserSRDFrontPublisher.publish(srdFrontScan) 345 | self.laserSRDLeftPublisher.publish(srdLeftScan) 346 | self.laserSRDRightPublisher.publish(srdRightScan) 347 | 348 | if self.pointcloud: 349 | # fetch values 350 | shovelPC.header.stamp = rospy.Time.now() 351 | shovelPC.data = self.fetchPCValues( 352 | self.PEPPER_MEM_KEY_GROUND_SHOVEL, 353 | self.PEPPER_LASER_GROUND_SHOVEL_POINTS 354 | ) 355 | 356 | groundLeftPC.header.stamp = rospy.Time.now() 357 | groundLeftPC.data = self.fetchPCValues( 358 | self.PEPPER_MEM_KEY_GROUND_LEFT, 359 | self.PEPPER_LASER_GROUND_LEFT_POINTS 360 | ) 361 | 362 | groundRightPC.header.stamp = rospy.Time.now() 363 | groundRightPC.data = self.fetchPCValues( 364 | self.PEPPER_MEM_KEY_GROUND_RIGHT, 365 | self.PEPPER_LASER_GROUND_RIGHT_POINTS 366 | ) 367 | 368 | srdFrontPC.header.stamp = rospy.Time.now() 369 | srdFrontPC.data = self.fetchPCValues( 370 | self.PEPPER_MEM_KEY_SRD_FRONT, 371 | self.PEPPER_LASER_SRD_POINTS 372 | ) 373 | 374 | srdLeftPC.header.stamp = rospy.Time.now() 375 | srdLeftPC.data = self.fetchPCValues( 376 | self.PEPPER_MEM_KEY_SRD_LEFT, 377 | self.PEPPER_LASER_SRD_POINTS 378 | ) 379 | 380 | srdRightPC.header.stamp = rospy.Time.now() 381 | srdRightPC.data = self.fetchPCValues( 382 | self.PEPPER_MEM_KEY_SRD_RIGHT, 383 | self.PEPPER_LASER_SRD_POINTS 384 | ) 385 | 386 | # publish messages 387 | self.pcShovelPublisher.publish(shovelPC) 388 | self.pcGroundLeftPublisher.publish(groundLeftPC) 389 | self.pcGroundRightPublisher.publish(groundRightPC) 390 | self.pcSRDFrontPublisher.publish(srdFrontPC) 391 | self.pcSRDLeftPublisher.publish(srdLeftPC) 392 | self.pcSRDRightPublisher.publish(srdRightPC) 393 | 394 | # sleep 395 | self.laserRate.sleep() 396 | 397 | 398 | if __name__ == '__main__': 399 | # from optparse import OptionParser 400 | # parser = OptionParser() 401 | # parser.add_option("--ppointcloud", dest="ppointcloud", default=True) 402 | # parser.add_option("--plaser", dest="plaser", default=False) 403 | # 404 | # (options, args) = parser.parse_args() 405 | # with_pc = options.ppointcloud 406 | # with_laser = options.plaser 407 | rospy.init_node('laserspub') 408 | 409 | with_pc = False 410 | with_laser = True 411 | laser = NaoqiLaser(with_pc, with_laser) 412 | laser.run() 413 | # laser.start() 414 | 415 | rospy.spin() 416 | exit(0) 417 | -------------------------------------------------------------------------------- /pepper_gazebo_plugin/scripts/laser_publisher.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from math import atan2, cos, radians, sin, sqrt, degrees, isnan 3 | 4 | import rospy 5 | 6 | from tf import TransformListener 7 | # from laser_geometry import LaserProjection 8 | from sensor_msgs.msg import LaserScan, PointCloud2 9 | from message_filters import TimeSynchronizer, Subscriber 10 | from sensor_msgs.point_cloud2 import read_points, create_cloud_xyz32 11 | from tf2_sensor_msgs.tf2_sensor_msgs import do_transform_cloud 12 | from geometry_msgs.msg import TransformStamped 13 | from geometry_msgs.msg import Vector3, Quaternion 14 | from ddynamic_reconfigure_python.ddynamic_reconfigure import DDynamicReconfigure as DDR 15 | 16 | import sensor_msgs.point_cloud2 as pc2 17 | 18 | import numpy as np 19 | """ 20 | Author: Sammy Pfeiffer 21 | With LaserProject implementation borrowed from the laser_geometry package. 22 | """ 23 | 24 | 25 | class LaserProjection: 26 | """ 27 | A class to Project Laser Scan 28 | This calls will project laser scans into point clouds. It caches 29 | unit vectors between runs (provided the angular resolution of 30 | your scanner is not changing) to avoid excess computation. 31 | By default all range values less thatn the scanner min_range, 32 | greater than the scanner max_range are removed from the generated 33 | point cloud, as these are assumed to be invalid. 34 | If it is important to preserve a mapping between the index of 35 | range values and points in the cloud, the recommended approach is to 36 | pre-filter your laser scan message to meet the requirement that all 37 | ranges are between min and max_range. 38 | The generate PointClouds have a number of channels which can be enabled 39 | through the use of ChannelOption. 40 | - ChannelOption.INTENSITY - Create a channel named "intensities" with the 41 | intensity of the return for each point. 42 | - ChannelOption.INDEX - Create a channel named "index" containing the 43 | index from the original array for each point. 44 | - ChannelOption.DISTANCE - Create a channel named "distance" containing 45 | the distance from the laser to each point. 46 | - ChannelOption.TIMESTAMP - Create a channel named "stamps" containing the 47 | specific timestamp at which each point was measured. 48 | """ 49 | 50 | LASER_SCAN_INVALID = -1.0 51 | LASER_SCAN_MIN_RANGE = -2.0 52 | LASER_SCAN_MAX_RANGE = -3.0 53 | 54 | class ChannelOption: 55 | NONE = 0x00 # Enable no channels 56 | INTENSITY = 0x01 # Enable "intensities" channel 57 | INDEX = 0x02 # Enable "index" channel 58 | DISTANCE = 0x04 # Enable "distances" channel 59 | TIMESTAMP = 0x08 # Enable "stamps" channel 60 | VIEWPOINT = 0x10 # Enable "viewpoint" channel 61 | DEFAULT = (INTENSITY | INDEX) 62 | 63 | def __init__(self): 64 | self.__angle_min = 0.0 65 | self.__angle_max = 0.0 66 | 67 | self.__cos_sin_map = np.array([[]]) 68 | 69 | def projectLaser(self, scan_in, 70 | range_cutoff=-1.0, channel_options=ChannelOption.DEFAULT): 71 | """ 72 | Project a sensor_msgs::LaserScan into a sensor_msgs::PointCloud2. 73 | Project a single laser scan from a linear array into a 3D 74 | point cloud. The generated cloud will be in the same frame 75 | as the original laser scan. 76 | Keyword arguments: 77 | scan_in -- The input laser scan. 78 | range_cutoff -- An additional range cutoff which can be 79 | applied which is more limiting than max_range in the scan 80 | (default -1.0). 81 | channel_options -- An OR'd set of channels to include. 82 | """ 83 | return self.__projectLaser(scan_in, range_cutoff, channel_options) 84 | 85 | def __projectLaser(self, scan_in, range_cutoff, channel_options): 86 | N = len(scan_in.ranges) 87 | 88 | ranges = np.array(scan_in.ranges) 89 | 90 | if (self.__cos_sin_map.shape[1] != N or 91 | self.__angle_min != scan_in.angle_min or 92 | self.__angle_max != scan_in.angle_max): 93 | rospy.logdebug("No precomputed map given. Computing one.") 94 | 95 | self.__angle_min = scan_in.angle_min 96 | self.__angle_max = scan_in.angle_max 97 | 98 | angles = scan_in.angle_min + np.arange(N) * scan_in.angle_increment 99 | self.__cos_sin_map = np.array([np.cos(angles), np.sin(angles)]) 100 | 101 | output = ranges * self.__cos_sin_map 102 | 103 | # Set the output cloud accordingly 104 | cloud_out = PointCloud2() 105 | 106 | fields = [pc2.PointField() for _ in range(3)] 107 | 108 | fields[0].name = "x" 109 | fields[0].offset = 0 110 | fields[0].datatype = pc2.PointField.FLOAT32 111 | fields[0].count = 1 112 | 113 | fields[1].name = "y" 114 | fields[1].offset = 4 115 | fields[1].datatype = pc2.PointField.FLOAT32 116 | fields[1].count = 1 117 | 118 | fields[2].name = "z" 119 | fields[2].offset = 8 120 | fields[2].datatype = pc2.PointField.FLOAT32 121 | fields[2].count = 1 122 | 123 | idx_intensity = idx_index = idx_distance = idx_timestamp = -1 124 | idx_vpx = idx_vpy = idx_vpz = -1 125 | 126 | offset = 12 127 | 128 | if (channel_options & self.ChannelOption.INTENSITY and 129 | len(scan_in.intensities) > 0): 130 | field_size = len(fields) 131 | fields.append(pc2.PointField()) 132 | fields[field_size].name = "intensity" 133 | fields[field_size].datatype = pc2.PointField.FLOAT32 134 | fields[field_size].offset = offset 135 | fields[field_size].count = 1 136 | offset += 4 137 | idx_intensity = field_size 138 | 139 | if channel_options & self.ChannelOption.INDEX: 140 | field_size = len(fields) 141 | fields.append(pc2.PointField()) 142 | fields[field_size].name = "index" 143 | fields[field_size].datatype = pc2.PointField.INT32 144 | fields[field_size].offset = offset 145 | fields[field_size].count = 1 146 | offset += 4 147 | idx_index = field_size 148 | 149 | if channel_options & self.ChannelOption.DISTANCE: 150 | field_size = len(fields) 151 | fields.append(pc2.PointField()) 152 | fields[field_size].name = "distances" 153 | fields[field_size].datatype = pc2.PointField.FLOAT32 154 | fields[field_size].offset = offset 155 | fields[field_size].count = 1 156 | offset += 4 157 | idx_distance = field_size 158 | 159 | if channel_options & self.ChannelOption.TIMESTAMP: 160 | field_size = len(fields) 161 | fields.append(pc2.PointField()) 162 | fields[field_size].name = "stamps" 163 | fields[field_size].datatype = pc2.PointField.FLOAT32 164 | fields[field_size].offset = offset 165 | fields[field_size].count = 1 166 | offset += 4 167 | idx_timestamp = field_size 168 | 169 | if channel_options & self.ChannelOption.VIEWPOINT: 170 | field_size = len(fields) 171 | fields.extend([pc2.PointField() for _ in range(3)]) 172 | fields[field_size].name = "vp_x" 173 | fields[field_size].datatype = pc2.PointField.FLOAT32 174 | fields[field_size].offset = offset 175 | fields[field_size].count = 1 176 | offset += 4 177 | idx_vpx = field_size 178 | field_size += 1 179 | 180 | fields[field_size].name = "vp_y" 181 | fields[field_size].datatype = pc2.PointField.FLOAT32 182 | fields[field_size].offset = offset 183 | fields[field_size].count = 1 184 | offset += 4 185 | idx_vpy = field_size 186 | field_size += 1 187 | 188 | fields[field_size].name = "vp_z" 189 | fields[field_size].datatype = pc2.PointField.FLOAT32 190 | fields[field_size].offset = offset 191 | fields[field_size].count = 1 192 | offset += 4 193 | idx_vpz = field_size 194 | 195 | if range_cutoff < 0: 196 | range_cutoff = scan_in.range_max 197 | else: 198 | range_cutoff = min(range_cutoff, scan_in.range_max) 199 | 200 | points = [] 201 | big_str = "\n" 202 | for i in range(N): 203 | ri = scan_in.ranges[i] 204 | # if ri < range_cutoff and ri >= scan_in.range_min: 205 | if True: 206 | point = output[:, i].tolist() 207 | point.append(0) 208 | p = point 209 | angle_increment = scan_in.angle_increment 210 | min_angle = scan_in.angle_min 211 | dist = ri 212 | idx = i 213 | 214 | if idx_intensity != -1: 215 | point.append(scan_in.intensities[i]) 216 | 217 | if idx_index != -1: 218 | point.append(i) 219 | 220 | if idx_distance != -1: 221 | point.append(scan_in.ranges[i]) 222 | 223 | if idx_timestamp != -1: 224 | point.append(i * scan_in.time_increment) 225 | 226 | if idx_vpx != -1 and idx_vpy != -1 and idx_vpz != -1: 227 | point.extend([0 for _ in range(3)]) 228 | 229 | points.append(point) 230 | 231 | big_str += " " + str(idx).zfill(2) + ": x: " + str(round(p[0], 2)) + ", y: " + str(round( 232 | p[1], 2)) + ", z: " + str(round(p[2], 2)) + " = " + str(round(dist, 2)) + "m (at " + str(round(degrees(idx * angle_increment + min_angle), 2)) + "deg)\n" 233 | 234 | rospy.loginfo("Projected cloud:") 235 | rospy.loginfo(big_str) 236 | cloud_out = pc2.create_cloud(scan_in.header, fields, points) 237 | 238 | return cloud_out 239 | 240 | 241 | class LaserPublisher(object): 242 | def __init__(self): 243 | if not rospy.core.is_initialized(): 244 | rospy.init_node('laser_test') 245 | rospy.loginfo("Initialised rospy node: laser_test") 246 | 247 | self.tl = TransformListener() 248 | self.lp = LaserProjection() 249 | 250 | # Publishers 251 | self.all_laser_pub = rospy.Publisher( 252 | '/pepper/laser_2', LaserScan, queue_size=1) 253 | self.pc_pub = rospy.Publisher('/cloud', PointCloud2, queue_size=1) 254 | self.pcl_pub = rospy.Publisher('/cloudl', PointCloud2, queue_size=1) 255 | self.pcr_pub = rospy.Publisher('/cloudr', PointCloud2, queue_size=1) 256 | self.pc_redone_pub = rospy.Publisher('/cloud_redone', 257 | PointCloud2, queue_size=1) 258 | self.pc_rereprojected_pub = rospy.Publisher('/cloud_rereprojected', 259 | PointCloud2, 260 | queue_size=1) 261 | 262 | # Subscribers 263 | left_sub = Subscriber('/pepper/scan_left', LaserScan) 264 | front_sub = Subscriber('/pepper/scan_front', LaserScan) 265 | right_sub = Subscriber('/pepper/scan_right', LaserScan) 266 | 267 | self.ts = TimeSynchronizer([left_sub, front_sub, right_sub], 268 | 10) 269 | rospy.loginfo("Finished intialising") 270 | self.ddr = DDR('increment') 271 | default_increment = radians(120.0 * 2.0) / 61.0 272 | self.ddr.add_variable('angle_increment', '', default_increment, 273 | 0.05, 0.08) 274 | # 130.665 275 | self.ddr.add_variable('half_max_angle', '', 120., 115., 145.0) 276 | self.ddr.start(self.dyn_rec_callback) 277 | self.ts.registerCallback(self.scan_cb) 278 | rospy.loginfo("Ready to go.") 279 | 280 | def add_variables_to_self(self): 281 | var_names = self.ddr.get_variable_names() 282 | for var_name in var_names: 283 | self.__setattr__(var_name, None) 284 | 285 | def dyn_rec_callback(self, config, level): 286 | rospy.loginfo("Received reconf call: " + str(config)) 287 | # Update all variables 288 | var_names = self.ddr.get_variable_names() 289 | for var_name in var_names: 290 | self.__dict__[var_name] = config[var_name] 291 | return config 292 | 293 | def scan_cb(self, left, front, right): 294 | rospy.loginfo("We got scan_cb") 295 | translated_points = [] 296 | try: 297 | pc_left = self.lp.projectLaser(left, channel_options=0x00) 298 | pc_front = self.lp.projectLaser(front, channel_options=0x00) 299 | pc_right = self.lp.projectLaser(right, channel_options=0x00) 300 | except Exception as e: 301 | rospy.logerr("Failed to transform laser scan because: " + str(e)) 302 | 303 | pc_left.header.stamp = rospy.Time.now() 304 | pc_left.header.frame_id = 'SurroundingLeftLaser_frame' 305 | self.pcl_pub.publish(pc_left) 306 | self.pcr_pub.publish(pc_right) 307 | 308 | transform_right_to_front = self.tl.lookupTransform( 309 | 'base_footprint', 'SurroundingRightLaser_frame', rospy.Time.now()) 310 | rospy.logdebug("Transform Right to Front:") 311 | rospy.logdebug(transform_right_to_front) 312 | ts = TransformStamped() 313 | ts.transform.translation = Vector3(*transform_right_to_front[0]) 314 | ts.transform.rotation = Quaternion(*transform_right_to_front[1]) 315 | ts.header.stamp = rospy.Time.now() 316 | transformed_cloud = do_transform_cloud(pc_right, ts) 317 | # right point cloud translation 318 | for p in read_points(transformed_cloud, 319 | field_names=('x', 'y', 'z'), 320 | skip_nans=False): 321 | translated_points.append(p) 322 | 323 | for i in range(8): 324 | translated_points.append( 325 | (float('nan'), float('nan'), float('nan'))) 326 | 327 | transform_front_to_front = self.tl.lookupTransform( 328 | 'base_footprint', 'SurroundingFrontLaser_frame', rospy.Time.now()) 329 | rospy.logdebug("Transform Front to Front:") 330 | rospy.logdebug(transform_front_to_front) 331 | ts = TransformStamped() 332 | ts.transform.translation = Vector3(*transform_front_to_front[0]) 333 | ts.transform.rotation = Quaternion(*transform_front_to_front[1]) 334 | ts.header.stamp = rospy.Time.now() 335 | transformed_cloud_f = do_transform_cloud(pc_front, ts) 336 | 337 | # front point cloud 338 | for p in read_points(transformed_cloud_f, 339 | field_names=('x', 'y', 'z'), 340 | skip_nans=False): 341 | translated_points.append(p) 342 | 343 | transform_left_to_front = self.tl.lookupTransform( 344 | 'base_footprint', 'SurroundingLeftLaser_frame', rospy.Time.now()) 345 | rospy.logdebug("Transform Left to Front:") 346 | rospy.logdebug(transform_left_to_front) 347 | ts = TransformStamped() 348 | ts.transform.translation = Vector3(*transform_left_to_front[0]) 349 | ts.transform.rotation = Quaternion(*transform_left_to_front[1]) 350 | ts.header.stamp = rospy.Time.now() 351 | from copy import deepcopy 352 | transformed_cloud_l = do_transform_cloud(deepcopy(pc_left), ts) 353 | 354 | for i in range(8): 355 | translated_points.append( 356 | (float('nan'), float('nan'), float('nan'))) 357 | 358 | # left pc translation 359 | for p in read_points(transformed_cloud_l, 360 | field_names=('x', 'y', 'z'), 361 | skip_nans=False): 362 | translated_points.append(p) 363 | 364 | # Create a point cloud from the combined points wrt the front 365 | # laser frame 366 | pc_front.header.frame_id = 'base_footprint' 367 | point_cloud = create_cloud_xyz32(pc_front.header, translated_points) 368 | self.pc_pub.publish(point_cloud) 369 | rospy.logdebug("pointcloud all together len: " + 370 | str(point_cloud.width)) 371 | 372 | # # double check we have the same thing 373 | # compare_str = "\n" 374 | # for idx, (tp, pcp) in enumerate(zip(translated_points, read_points(point_cloud))): 375 | # compare_str += str(idx).zfill(2) + ":\n" 376 | # compare_str += " tp : " + str(tp) 377 | # compare_str += "\n pcp: " + str(pcp) + "\n" 378 | # rospy.loginfo(compare_str) 379 | # # OK we know they are the same 380 | # # translated_points and point_cloud contain virtually the same data 381 | 382 | # Convert combined point cloud into LaserScan 383 | all_laser_msg = LaserScan() 384 | laser_ranges, angle_min, angle_max, angle_increment = self.pc_to_laser( 385 | point_cloud) 386 | all_laser_msg.header.frame_id = 'base_footprint' 387 | all_laser_msg.header.stamp = rospy.Time.now() 388 | all_laser_msg.ranges = laser_ranges 389 | all_laser_msg.angle_min = angle_min 390 | all_laser_msg.angle_max = angle_max 391 | all_laser_msg.angle_increment = angle_increment 392 | all_laser_msg.range_min = 0.1 393 | all_laser_msg.range_max = 7.0 394 | all_laser_msg.intensities = [] 395 | self.all_laser_pub.publish(all_laser_msg) 396 | 397 | rospy.logdebug("all_laser_msg len: " + str(len(all_laser_msg.ranges))) 398 | pc_redone = self.lp.projectLaser(all_laser_msg, channel_options=0x00) 399 | rospy.logdebug("all_laser pc_redone len: " + str(pc_redone.width)) 400 | self.pc_redone_pub.publish(pc_redone) 401 | 402 | # compare what came in and what came out 403 | rospy.logdebug("point_cloud frame_id, pc_redone frame_id:") 404 | rospy.logdebug((point_cloud.header.frame_id, 405 | pc_redone.header.frame_id)) 406 | rospy.logdebug("point_cloud is correct, pc_redone is incorrect") 407 | compare_str = "\n" 408 | for idx, (point_in, point_out) in enumerate(zip(read_points(point_cloud), read_points(pc_redone))): 409 | point_out = [point_out[0], point_out[1], 0.0] 410 | point_in = [point_in[0], point_in[1], 0.0] 411 | compare_str += str(idx).zfill(2) + ":\n" 412 | compare_str += " in : " + str(point_in) 413 | compare_str += "\n out: " + str(point_out) + "\n" 414 | dist = np.linalg.norm(np.array(point_out) - np.array(point_in)) 415 | compare_str += " dist: " + str(dist) + "\n" 416 | # angle 417 | angle1 = atan2(point_in[1], point_in[0]) 418 | angle2 = atan2(point_out[1], point_out[0]) 419 | angle_dif = angle2 - angle1 420 | compare_str += " angle dif: " + str(angle_dif) + "\n" 421 | 422 | rospy.logdebug(compare_str) 423 | 424 | def pc_to_laser(self, cloud): 425 | laser_points = [] 426 | points_rereprojected = [] 427 | multiply_num_rays = 8 428 | num_rays = 61 * multiply_num_rays 429 | laser_points2 = [float('nan')] * num_rays 430 | min_angle = -radians(self.half_max_angle) 431 | max_angle = radians(self.half_max_angle) 432 | # angle_increment = self.angle_increment 433 | angle_increment = (radians(self.half_max_angle) 434 | * 2.0) / float(num_rays) 435 | big_str = "\n" 436 | for idx, p in enumerate(read_points(cloud, skip_nans=False)): 437 | #dist = self.get_dist(p[0], p[1]) 438 | p = [p[0], p[1], 0.0] 439 | dist = np.linalg.norm(np.array((0., 0., 0.)) - np.array(p)) 440 | # dist1 = self.get_dist(p[0], p[1]) 441 | big_str += " " + str(idx).zfill(2) + ": x: " + str(round(p[0], 2)) + ", y: " + str(round( 442 | p[1], 2)) + ", z: " + str(round(p[2], 2)) + " = " + str(round(dist, 2)) + "m (at " + str(round(degrees(idx * angle_increment + min_angle), 2)) + "deg)\n" 443 | 444 | laser_points.append(dist) 445 | # coords from dist 446 | x = dist * cos(idx * angle_increment + min_angle) 447 | y = dist * sin(idx * angle_increment + min_angle) 448 | print(" [ px, py, are the correct points ] ") 449 | print("dist, px, py: " + str(dist) + 450 | " " + str(p[0])) + " " + str(p[1]) 451 | print("dist, x, y: " + str(dist) + " " + str(x) + " " + str(y)) 452 | 453 | dist_from_rereproj = self.get_dist(x, y) 454 | print("dist rereproj: " + str(dist_from_rereproj)) 455 | # print("dist1 : " + str(dist1)) 456 | 457 | # what if a make a pointcloud based in the cos sin version 458 | points_rereprojected.append((x, y, 0.0)) 459 | 460 | # angle from point 461 | angle = atan2(p[1], p[0]) 462 | # angle2 = atan2(y, x) 463 | expected_angle = idx * self.angle_increment + min_angle 464 | if not isnan(angle): 465 | tmp_angle = angle - min_angle 466 | print("tmp_angle: " + str(degrees(tmp_angle))) + " deg" 467 | print("angle_increment: " + str(degrees(angle_increment))) 468 | closest_index = int(tmp_angle / angle_increment) 469 | print("closest index: " + str(closest_index)) 470 | if closest_index >= len(laser_points2): 471 | laser_points2[-1] = dist 472 | elif closest_index < 0: 473 | laser_points2[0] = dist 474 | else: 475 | laser_points2[closest_index] = dist 476 | else: 477 | print("nan, not adding anything to scan") 478 | 479 | # laser_points[] 480 | print("Angle from p : " + str(round(degrees(angle), 2))) 481 | # print("Angle from xy: " + str(round(degrees(angle2), 2))) 482 | print("Expected angle: " + str(round(degrees(expected_angle), 2))) 483 | 484 | rospy.logdebug("Lasered cloud") 485 | rospy.logdebug(big_str) 486 | 487 | laser_points = laser_points2 488 | print("Len of laser points after new technique: " + str(len(laser_points))) 489 | 490 | rereprojected_pc = PointCloud2() 491 | rereprojected_pc.header.frame_id = 'base_footprint' 492 | rereprojected_pc.header.stamp = rospy.Time.now() 493 | point_cloud_rere = create_cloud_xyz32( 494 | rereprojected_pc.header, points_rereprojected) 495 | self.pc_rereprojected_pub.publish(point_cloud_rere) 496 | 497 | return laser_points, min_angle, max_angle, angle_increment 498 | 499 | def get_dist(self, x0, y0, x1=0.0, y1=0.0): 500 | return sqrt((x1 - x0)**2 + (y1 - y0)**2) 501 | 502 | 503 | if __name__ == "__main__": 504 | lp = LaserPublisher() 505 | rospy.spin() 506 | --------------------------------------------------------------------------------