From e02bb434ac1c43a2c9c963061a8f81b853607733 Mon Sep 17 00:00:00 2001 From: Steven Peters Date: Thu, 10 Oct 2013 11:38:32 -0700 Subject: [PATCH 001/153] Add dependencies on dynamic reconfigure files Occasionally the build can fail due to some targets having an undeclared dependency on automatically generated dynamic reconfigure files (GazeboRosCameraConfig.h for example). This commit declares several of those dependencies. --- gazebo_plugins/CMakeLists.txt | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index 43f68b5c5..b1f90624e 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -126,10 +126,11 @@ target_link_libraries(hokuyo_node ) add_library(vision_reconfigure src/vision_reconfigure.cpp) -add_dependencies(vision_reconfigure gazebo_plugins_gencfg) +add_dependencies(vision_reconfigure ${PROJECT_NAME}_gencfg) target_link_libraries(vision_reconfigure ${catkin_LIBRARIES}) add_executable(camera_synchronizer src/camera_synchronizer.cpp) +add_dependencies(camera_synchronizer ${PROJECT_NAME}_gencfg) target_link_libraries(camera_synchronizer vision_reconfigure ${catkin_LIBRARIES}) add_executable(pub_joint_trajectory_test test/pub_joint_trajectory_test.cpp) @@ -142,6 +143,7 @@ add_definitions(-fPIC) # what is this for? ## Plugins add_library(gazebo_ros_camera_utils src/gazebo_ros_camera_utils.cpp) +add_dependencies(gazebo_ros_camera_utils ${PROJECT_NAME}_gencfg) set_target_properties(gazebo_ros_camera_utils PROPERTIES LINK_FLAGS "${ld_flags}") set_target_properties(gazebo_ros_camera_utils PROPERTIES COMPILE_FLAGS "${cxx_flags}") target_link_libraries(gazebo_ros_camera_utils ${GAZEBO_LIBRARIES} ${SDFormat_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) @@ -152,21 +154,25 @@ set_target_properties(MultiCameraPlugin PROPERTIES COMPILE_FLAGS "${cxx_flags}") target_link_libraries(MultiCameraPlugin ${GAZEBO_LIBRARIES} ${SDFormat_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_library(gazebo_ros_camera src/gazebo_ros_camera.cpp) +add_dependencies(gazebo_ros_camera ${PROJECT_NAME}_gencfg) set_target_properties(gazebo_ros_camera PROPERTIES LINK_FLAGS "${ld_flags}") set_target_properties(gazebo_ros_camera PROPERTIES COMPILE_FLAGS "${cxx_flags}") target_link_libraries(gazebo_ros_camera gazebo_ros_camera_utils ${GAZEBO_LIBRARIES} ${SDFormat_LIBRARIES} CameraPlugin ${catkin_LIBRARIES}) add_library(gazebo_ros_multicamera src/gazebo_ros_multicamera.cpp) +add_dependencies(gazebo_ros_multicamera ${PROJECT_NAME}_gencfg) set_target_properties(gazebo_ros_multicamera PROPERTIES LINK_FLAGS "${ld_flags}") set_target_properties(gazebo_ros_multicamera PROPERTIES COMPILE_FLAGS "${cxx_flags}") target_link_libraries(gazebo_ros_multicamera gazebo_ros_camera_utils ${GAZEBO_LIBRARIES} ${SDFormat_LIBRARIES} MultiCameraPlugin ${catkin_LIBRARIES}) add_library(gazebo_ros_depth_camera src/gazebo_ros_depth_camera.cpp) +add_dependencies(gazebo_ros_depth_camera ${PROJECT_NAME}_gencfg) set_target_properties(gazebo_ros_depth_camera PROPERTIES LINK_FLAGS "${ld_flags}") set_target_properties(gazebo_ros_depth_camera PROPERTIES COMPILE_FLAGS "${cxx_flags}") target_link_libraries(gazebo_ros_depth_camera gazebo_ros_camera_utils ${GAZEBO_LIBRARIES} ${SDFormat_LIBRARIES} DepthCameraPlugin ${catkin_LIBRARIES}) add_library(gazebo_ros_openni_kinect src/gazebo_ros_openni_kinect.cpp) +add_dependencies(gazebo_ros_openni_kinect ${PROJECT_NAME}_gencfg) set_target_properties(gazebo_ros_openni_kinect PROPERTIES LINK_FLAGS "${ld_flags}") set_target_properties(gazebo_ros_openni_kinect PROPERTIES COMPILE_FLAGS "${cxx_flags}") target_link_libraries(gazebo_ros_openni_kinect gazebo_ros_camera_utils ${GAZEBO_LIBRARIES} ${SDFormat_LIBRARIES} DepthCameraPlugin ${catkin_LIBRARIES}) @@ -213,6 +219,7 @@ set_target_properties(gazebo_ros_projector PROPERTIES COMPILE_FLAGS "${cxx_flags target_link_libraries(gazebo_ros_projector ${GAZEBO_LIBRARIES} ${SDFormat_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES}) add_library(gazebo_ros_prosilica src/gazebo_ros_prosilica.cpp) +add_dependencies(gazebo_ros_prosilica ${PROJECT_NAME}_gencfg) set_target_properties(gazebo_ros_prosilica PROPERTIES LINK_FLAGS "${ld_flags}") set_target_properties(gazebo_ros_prosilica PROPERTIES COMPILE_FLAGS "${cxx_flags}") target_link_libraries(gazebo_ros_prosilica gazebo_ros_camera_utils ${GAZEBO_LIBRARIES} ${SDFormat_LIBRARIES} CameraPlugin ${catkin_LIBRARIES}) From af973a994dbe1809e8d947adc6665c6bed83c6de Mon Sep 17 00:00:00 2001 From: Steven Peters Date: Thu, 10 Oct 2013 11:39:29 -0700 Subject: [PATCH 002/153] Put some cmake lists on multiple lines to improve readability. --- gazebo_plugins/CMakeLists.txt | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index b1f90624e..cbf3cfc96 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -49,9 +49,21 @@ execute_process(COMMAND catkin_python_setup() -generate_dynamic_reconfigure_options(cfg/CameraSynchronizer.cfg cfg/GazeboRosCamera.cfg cfg/GazeboRosOpenniKinect.cfg cfg/Hokuyo.cfg) +generate_dynamic_reconfigure_options( + cfg/CameraSynchronizer.cfg + cfg/GazeboRosCamera.cfg + cfg/GazeboRosOpenniKinect.cfg + cfg/Hokuyo.cfg +) -include_directories(include ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} ${OGRE_INCLUDE_DIRS} ${OGRE-Terrain_INCLUDE_DIRS} ${SDFormat_INCLUDE_DIRS}) +include_directories(include + ${Boost_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} + ${GAZEBO_INCLUDE_DIRS} + ${OGRE_INCLUDE_DIRS} + ${OGRE-Terrain_INCLUDE_DIRS} + ${SDFormat_INCLUDE_DIRS} +) # \todo: remove these? set(cxx_flags) From f7effe9bcdac8ba7e63399c0b07733375534ad66 Mon Sep 17 00:00:00 2001 From: Steven Peters Date: Thu, 10 Oct 2013 12:56:35 -0700 Subject: [PATCH 003/153] Simplify gazebo_plugins/CMakeLists.txt Replace cxx_flags and ld_flags variables with simpler cmake macros and eliminate unnecessary references to SDFormat_LIBRARIES, since they are already part of GAZEBO_LIBRARIES. --- gazebo_plugins/CMakeLists.txt | 111 +++++++++------------------------- 1 file changed, 27 insertions(+), 84 deletions(-) diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index cbf3cfc96..7729ba51e 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -62,20 +62,11 @@ include_directories(include ${GAZEBO_INCLUDE_DIRS} ${OGRE_INCLUDE_DIRS} ${OGRE-Terrain_INCLUDE_DIRS} - ${SDFormat_INCLUDE_DIRS} ) -# \todo: remove these? -set(cxx_flags) -foreach (item ${GAZEBO_CFLAGS}) - set(cxx_flags "${cxx_flags} ${item}") -endforeach () - -set(ld_flags) -foreach (item ${GAZEBO_LDFLAGS}) - set(ld_flags "${ld_flags} ${item}") -endforeach () - +link_directories( + ${GAZEBO_LIBRARY_DIRS} +) catkin_package( INCLUDE_DIRS include @@ -146,9 +137,7 @@ add_dependencies(camera_synchronizer ${PROJECT_NAME}_gencfg) target_link_libraries(camera_synchronizer vision_reconfigure ${catkin_LIBRARIES}) add_executable(pub_joint_trajectory_test test/pub_joint_trajectory_test.cpp) -set_target_properties(pub_joint_trajectory_test PROPERTIES LINK_FLAGS "${ld_flags}") -set_target_properties(pub_joint_trajectory_test PROPERTIES COMPILE_FLAGS "${cxx_flags}") -target_link_libraries(pub_joint_trajectory_test ${GAZEBO_LIBRARIES} ${SDFormat_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +target_link_libraries(pub_joint_trajectory_test ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_dependencies(pub_joint_trajectory_test ${catkin_EXPORTED_TARGETS}) # don't build until gazebo_msgs is done add_definitions(-fPIC) # what is this for? @@ -156,122 +145,78 @@ add_definitions(-fPIC) # what is this for? ## Plugins add_library(gazebo_ros_camera_utils src/gazebo_ros_camera_utils.cpp) add_dependencies(gazebo_ros_camera_utils ${PROJECT_NAME}_gencfg) -set_target_properties(gazebo_ros_camera_utils PROPERTIES LINK_FLAGS "${ld_flags}") -set_target_properties(gazebo_ros_camera_utils PROPERTIES COMPILE_FLAGS "${cxx_flags}") -target_link_libraries(gazebo_ros_camera_utils ${GAZEBO_LIBRARIES} ${SDFormat_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +target_link_libraries(gazebo_ros_camera_utils ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_library(MultiCameraPlugin src/MultiCameraPlugin.cpp) -set_target_properties(MultiCameraPlugin PROPERTIES LINK_FLAGS "${ld_flags}") -set_target_properties(MultiCameraPlugin PROPERTIES COMPILE_FLAGS "${cxx_flags}") -target_link_libraries(MultiCameraPlugin ${GAZEBO_LIBRARIES} ${SDFormat_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +target_link_libraries(MultiCameraPlugin ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_library(gazebo_ros_camera src/gazebo_ros_camera.cpp) add_dependencies(gazebo_ros_camera ${PROJECT_NAME}_gencfg) -set_target_properties(gazebo_ros_camera PROPERTIES LINK_FLAGS "${ld_flags}") -set_target_properties(gazebo_ros_camera PROPERTIES COMPILE_FLAGS "${cxx_flags}") -target_link_libraries(gazebo_ros_camera gazebo_ros_camera_utils ${GAZEBO_LIBRARIES} ${SDFormat_LIBRARIES} CameraPlugin ${catkin_LIBRARIES}) +target_link_libraries(gazebo_ros_camera gazebo_ros_camera_utils ${GAZEBO_LIBRARIES} CameraPlugin ${catkin_LIBRARIES}) add_library(gazebo_ros_multicamera src/gazebo_ros_multicamera.cpp) add_dependencies(gazebo_ros_multicamera ${PROJECT_NAME}_gencfg) -set_target_properties(gazebo_ros_multicamera PROPERTIES LINK_FLAGS "${ld_flags}") -set_target_properties(gazebo_ros_multicamera PROPERTIES COMPILE_FLAGS "${cxx_flags}") -target_link_libraries(gazebo_ros_multicamera gazebo_ros_camera_utils ${GAZEBO_LIBRARIES} ${SDFormat_LIBRARIES} MultiCameraPlugin ${catkin_LIBRARIES}) +target_link_libraries(gazebo_ros_multicamera gazebo_ros_camera_utils ${GAZEBO_LIBRARIES} MultiCameraPlugin ${catkin_LIBRARIES}) add_library(gazebo_ros_depth_camera src/gazebo_ros_depth_camera.cpp) add_dependencies(gazebo_ros_depth_camera ${PROJECT_NAME}_gencfg) -set_target_properties(gazebo_ros_depth_camera PROPERTIES LINK_FLAGS "${ld_flags}") -set_target_properties(gazebo_ros_depth_camera PROPERTIES COMPILE_FLAGS "${cxx_flags}") -target_link_libraries(gazebo_ros_depth_camera gazebo_ros_camera_utils ${GAZEBO_LIBRARIES} ${SDFormat_LIBRARIES} DepthCameraPlugin ${catkin_LIBRARIES}) +target_link_libraries(gazebo_ros_depth_camera gazebo_ros_camera_utils ${GAZEBO_LIBRARIES} DepthCameraPlugin ${catkin_LIBRARIES}) add_library(gazebo_ros_openni_kinect src/gazebo_ros_openni_kinect.cpp) add_dependencies(gazebo_ros_openni_kinect ${PROJECT_NAME}_gencfg) -set_target_properties(gazebo_ros_openni_kinect PROPERTIES LINK_FLAGS "${ld_flags}") -set_target_properties(gazebo_ros_openni_kinect PROPERTIES COMPILE_FLAGS "${cxx_flags}") -target_link_libraries(gazebo_ros_openni_kinect gazebo_ros_camera_utils ${GAZEBO_LIBRARIES} ${SDFormat_LIBRARIES} DepthCameraPlugin ${catkin_LIBRARIES}) +target_link_libraries(gazebo_ros_openni_kinect gazebo_ros_camera_utils ${GAZEBO_LIBRARIES} DepthCameraPlugin ${catkin_LIBRARIES}) add_library(gazebo_ros_gpu_laser src/gazebo_ros_gpu_laser.cpp) -set_target_properties(gazebo_ros_gpu_laser PROPERTIES LINK_FLAGS "${ld_flags}") -set_target_properties(gazebo_ros_gpu_laser PROPERTIES COMPILE_FLAGS "${cxx_flags}") -target_link_libraries(gazebo_ros_gpu_laser ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES} ${SDFormat_LIBRARIES} GpuRayPlugin) +target_link_libraries(gazebo_ros_gpu_laser ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES} GpuRayPlugin) add_library(gazebo_ros_laser src/gazebo_ros_laser.cpp) -set_target_properties(gazebo_ros_laser PROPERTIES LINK_FLAGS "${ld_flags}") -set_target_properties(gazebo_ros_laser PROPERTIES COMPILE_FLAGS "${cxx_flags}") -target_link_libraries(gazebo_ros_laser ${GAZEBO_LIBRARIES} ${SDFormat_LIBRARIES} RayPlugin ${catkin_LIBRARIES}) +target_link_libraries(gazebo_ros_laser ${GAZEBO_LIBRARIES} RayPlugin ${catkin_LIBRARIES}) add_library(gazebo_ros_block_laser src/gazebo_ros_block_laser.cpp) -set_target_properties(gazebo_ros_block_laser PROPERTIES LINK_FLAGS "${ld_flags}") -set_target_properties(gazebo_ros_block_laser PROPERTIES COMPILE_FLAGS "${cxx_flags}") -target_link_libraries(gazebo_ros_block_laser ${GAZEBO_LIBRARIES} ${SDFormat_LIBRARIES} RayPlugin ${catkin_LIBRARIES}) +target_link_libraries(gazebo_ros_block_laser ${GAZEBO_LIBRARIES} RayPlugin ${catkin_LIBRARIES}) add_library(gazebo_ros_p3d src/gazebo_ros_p3d.cpp) -set_target_properties(gazebo_ros_p3d PROPERTIES LINK_FLAGS "${ld_flags}") -set_target_properties(gazebo_ros_p3d PROPERTIES COMPILE_FLAGS "${cxx_flags}") -target_link_libraries(gazebo_ros_p3d ${GAZEBO_LIBRARIES} ${SDFormat_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +target_link_libraries(gazebo_ros_p3d ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_library(gazebo_ros_imu src/gazebo_ros_imu.cpp) -set_target_properties(gazebo_ros_imu PROPERTIES LINK_FLAGS "${ld_flags}") -set_target_properties(gazebo_ros_imu PROPERTIES COMPILE_FLAGS "${cxx_flags}") -target_link_libraries(gazebo_ros_imu ${GAZEBO_LIBRARIES} ${SDFormat_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +target_link_libraries(gazebo_ros_imu ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_library(gazebo_ros_f3d src/gazebo_ros_f3d.cpp) -set_target_properties(gazebo_ros_f3d PROPERTIES LINK_FLAGS "${ld_flags}") -set_target_properties(gazebo_ros_f3d PROPERTIES COMPILE_FLAGS "${cxx_flags}") -target_link_libraries(gazebo_ros_f3d ${GAZEBO_LIBRARIES} ${SDFormat_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +target_link_libraries(gazebo_ros_f3d ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_library(gazebo_ros_bumper src/gazebo_ros_bumper.cpp) add_dependencies(gazebo_ros_bumper gazebo_msgs_gencpp) -set_target_properties(gazebo_ros_bumper PROPERTIES LINK_FLAGS "${ld_flags}") -set_target_properties(gazebo_ros_bumper PROPERTIES COMPILE_FLAGS "${cxx_flags}") -target_link_libraries(gazebo_ros_bumper ${GAZEBO_LIBRARIES} ${SDFormat_LIBRARIES} ${Boost_LIBRARIES} ContactPlugin ${catkin_LIBRARIES}) +target_link_libraries(gazebo_ros_bumper ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} ContactPlugin ${catkin_LIBRARIES}) add_library(gazebo_ros_projector src/gazebo_ros_projector.cpp) -set_target_properties(gazebo_ros_projector PROPERTIES LINK_FLAGS "${ld_flags}") -set_target_properties(gazebo_ros_projector PROPERTIES COMPILE_FLAGS "${cxx_flags}") -target_link_libraries(gazebo_ros_projector ${GAZEBO_LIBRARIES} ${SDFormat_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES}) +target_link_libraries(gazebo_ros_projector ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES}) add_library(gazebo_ros_prosilica src/gazebo_ros_prosilica.cpp) add_dependencies(gazebo_ros_prosilica ${PROJECT_NAME}_gencfg) -set_target_properties(gazebo_ros_prosilica PROPERTIES LINK_FLAGS "${ld_flags}") -set_target_properties(gazebo_ros_prosilica PROPERTIES COMPILE_FLAGS "${cxx_flags}") -target_link_libraries(gazebo_ros_prosilica gazebo_ros_camera_utils ${GAZEBO_LIBRARIES} ${SDFormat_LIBRARIES} CameraPlugin ${catkin_LIBRARIES}) +target_link_libraries(gazebo_ros_prosilica gazebo_ros_camera_utils ${GAZEBO_LIBRARIES} CameraPlugin ${catkin_LIBRARIES}) add_library(gazebo_ros_force src/gazebo_ros_force.cpp) -set_target_properties(gazebo_ros_force PROPERTIES LINK_FLAGS "${ld_flags}") -set_target_properties(gazebo_ros_force PROPERTIES COMPILE_FLAGS "${cxx_flags}") -target_link_libraries(gazebo_ros_force ${GAZEBO_LIBRARIES} ${SDFormat_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +target_link_libraries(gazebo_ros_force ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_library(gazebo_ros_joint_trajectory src/gazebo_ros_joint_trajectory.cpp) -set_target_properties(gazebo_ros_joint_trajectory PROPERTIES LINK_FLAGS "${ld_flags}") -set_target_properties(gazebo_ros_joint_trajectory PROPERTIES COMPILE_FLAGS "${cxx_flags}") add_dependencies(gazebo_ros_joint_trajectory gazebo_msgs_gencpp) -target_link_libraries(gazebo_ros_joint_trajectory ${GAZEBO_LIBRARIES} ${SDFormat_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +target_link_libraries(gazebo_ros_joint_trajectory ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_library(gazebo_ros_joint_pose_trajectory src/gazebo_ros_joint_pose_trajectory.cpp) -set_target_properties(gazebo_ros_joint_pose_trajectory PROPERTIES LINK_FLAGS "${ld_flags}") -set_target_properties(gazebo_ros_joint_pose_trajectory PROPERTIES COMPILE_FLAGS "${cxx_flags}") add_dependencies(gazebo_ros_joint_pose_trajectory gazebo_msgs_gencpp) -target_link_libraries(gazebo_ros_joint_pose_trajectory ${GAZEBO_LIBRARIES} ${SDFormat_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +target_link_libraries(gazebo_ros_joint_pose_trajectory ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_library(gazebo_ros_diff_drive src/gazebo_ros_diff_drive.cpp) -set_target_properties(gazebo_ros_diff_drive PROPERTIES LINK_FLAGS "${ld_flags}") -set_target_properties(gazebo_ros_diff_drive PROPERTIES COMPILE_FLAGS "${cxx_flags}") -target_link_libraries(gazebo_ros_diff_drive ${GAZEBO_LIBRARIES} ${SDFormat_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +target_link_libraries(gazebo_ros_diff_drive ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_library(gazebo_ros_skid_steer_drive src/gazebo_ros_skid_steer_drive.cpp) -set_target_properties(gazebo_ros_skid_steer_drive PROPERTIES LINK_FLAGS "${ld_flags}") -set_target_properties(gazebo_ros_skid_steer_drive PROPERTIES COMPILE_FLAGS "${cxx_flags}") -target_link_libraries(gazebo_ros_skid_steer_drive ${GAZEBO_LIBRARIES} ${SDFormat_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +target_link_libraries(gazebo_ros_skid_steer_drive ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_library(gazebo_ros_video src/gazebo_ros_video.cpp) -set_target_properties(gazebo_ros_video PROPERTIES LINK_FLAGS "${ld_flags}") -set_target_properties(gazebo_ros_video PROPERTIES COMPILE_FLAGS "${cxx_flags}") -target_link_libraries(gazebo_ros_video ${GAZEBO_LIBRARIES} ${SDFormat_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${OGRE_LIBRARIES}) +target_link_libraries(gazebo_ros_video ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${OGRE_LIBRARIES}) add_library(gazebo_ros_planar_move src/gazebo_ros_planar_move.cpp) -set_target_properties(gazebo_ros_planar_move PROPERTIES LINK_FLAGS "${ld_flags}") -set_target_properties(gazebo_ros_planar_move PROPERTIES COMPILE_FLAGS "${cxx_flags}") -target_link_libraries(gazebo_ros_planar_move ${GAZEBO_LIBRARIES} ${SDFormat_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +target_link_libraries(gazebo_ros_planar_move ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) ## ## Add your new plugin here @@ -279,9 +224,7 @@ target_link_libraries(gazebo_ros_planar_move ${GAZEBO_LIBRARIES} ${SDFormat_LIBR ## Template add_library(gazebo_ros_template src/gazebo_ros_template.cpp) -set_target_properties(gazebo_ros_template PROPERTIES LINK_FLAGS "${ld_flags}") -set_target_properties(gazebo_ros_template PROPERTIES COMPILE_FLAGS "${cxx_flags}") -target_link_libraries(gazebo_ros_template ${GAZEBO_LIBRARIES} ${SDFormat_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +target_link_libraries(gazebo_ros_template ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) install(TARGETS hokuyo_node From 2993a49632038cf10711245007cc4e13dd7bb1ca Mon Sep 17 00:00:00 2001 From: Paul Mathieu Date: Tue, 15 Oct 2013 09:57:05 +0200 Subject: [PATCH 004/153] Remove dependency to meta-package ros_controllers --- gazebo_ros_control/package.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/gazebo_ros_control/package.xml b/gazebo_ros_control/package.xml index 03d74ea18..af38e39f5 100644 --- a/gazebo_ros_control/package.xml +++ b/gazebo_ros_control/package.xml @@ -29,7 +29,6 @@ gazebo_ros controller_manager pluginlib - ros_controllers transmission_interface From ac432d8161b20af19b508e806207e1c3d6d505c6 Mon Sep 17 00:00:00 2001 From: Paul Mathieu Date: Tue, 15 Oct 2013 13:35:46 +0200 Subject: [PATCH 005/153] Fix iterator erase() problems --- gazebo_ros/src/gazebo_ros_api_plugin.cpp | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/gazebo_ros/src/gazebo_ros_api_plugin.cpp b/gazebo_ros/src/gazebo_ros_api_plugin.cpp index 81b5dd259..c2431db76 100644 --- a/gazebo_ros/src/gazebo_ros_api_plugin.cpp +++ b/gazebo_ros/src/gazebo_ros_api_plugin.cpp @@ -81,16 +81,12 @@ GazeboRosApiPlugin::~GazeboRosApiPlugin() // Delete Force and Wrench Jobs lock_.lock(); for (std::vector::iterator iter=force_joint_jobs_.begin();iter!=force_joint_jobs_.end();) - { delete (*iter); - force_joint_jobs_.erase(iter); - } + force_joint_jobs_.clear(); ROS_DEBUG_STREAM_NAMED("api_plugin","ForceJointJobs deleted"); for (std::vector::iterator iter=wrench_body_jobs_.begin();iter!=wrench_body_jobs_.end();) - { delete (*iter); - wrench_body_jobs_.erase(iter); - } + wrench_body_jobs_.clear(); lock_.unlock(); ROS_DEBUG_STREAM_NAMED("api_plugin","WrenchBodyJobs deleted"); @@ -1661,7 +1657,7 @@ void GazeboRosApiPlugin::wrenchBodySchedulerSlot() { // remove from queue once expires delete (*iter); - wrench_body_jobs_.erase(iter); + iter = wrench_body_jobs_.erase(iter); } else iter++; @@ -1691,7 +1687,7 @@ void GazeboRosApiPlugin::forceJointSchedulerSlot() (*iter)->duration.toSec() >= 0.0) { // remove from queue once expires - force_joint_jobs_.erase(iter); + iter = force_joint_jobs_.erase(iter); } else iter++; From aa5af1fca0edc6f8cbec83e936ce72a7436afa69 Mon Sep 17 00:00:00 2001 From: Paul Mathieu Date: Tue, 15 Oct 2013 13:42:32 +0200 Subject: [PATCH 006/153] Use pre-increment for iterators --- gazebo_ros/src/gazebo_ros_api_plugin.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gazebo_ros/src/gazebo_ros_api_plugin.cpp b/gazebo_ros/src/gazebo_ros_api_plugin.cpp index c2431db76..c800778f3 100644 --- a/gazebo_ros/src/gazebo_ros_api_plugin.cpp +++ b/gazebo_ros/src/gazebo_ros_api_plugin.cpp @@ -1319,7 +1319,7 @@ bool GazeboRosApiPlugin::clearJointForces(std::string joint_name) while(search) { search = false; - for (std::vector::iterator iter=force_joint_jobs_.begin();iter!=force_joint_jobs_.end();iter++) + for (std::vector::iterator iter=force_joint_jobs_.begin();iter!=force_joint_jobs_.end();++iter) { if ((*iter)->joint->GetName() == joint_name) { @@ -1347,7 +1347,7 @@ bool GazeboRosApiPlugin::clearBodyWrenches(std::string body_name) while(search) { search = false; - for (std::vector::iterator iter=wrench_body_jobs_.begin();iter!=wrench_body_jobs_.end();iter++) + for (std::vector::iterator iter=wrench_body_jobs_.begin();iter!=wrench_body_jobs_.end();++iter) { //ROS_ERROR("search %s %s",(*iter)->body->GetScopedName().c_str(), body_name.c_str()); if ((*iter)->body->GetScopedName() == body_name) From f8e8b59e54f768dd360221a8f057a66fb5553948 Mon Sep 17 00:00:00 2001 From: Paul Mathieu Date: Tue, 15 Oct 2013 14:33:56 +0200 Subject: [PATCH 007/153] Add missing run_depend to urdf in gazebo_ros_control --- gazebo_ros_control/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/gazebo_ros_control/package.xml b/gazebo_ros_control/package.xml index af38e39f5..07632a743 100644 --- a/gazebo_ros_control/package.xml +++ b/gazebo_ros_control/package.xml @@ -30,6 +30,7 @@ controller_manager pluginlib transmission_interface + urdf From 4dc7ba07fb01d23b727b9aa2b9527bbb87c2ac84 Mon Sep 17 00:00:00 2001 From: Johannes Meyer Date: Thu, 24 Oct 2013 14:00:50 +0200 Subject: [PATCH 008/153] gazebo_ros_control: use the model NodeHandle to get the robot_description parameter --- .../include/gazebo_ros_control/gazebo_ros_control_plugin.h | 1 - gazebo_ros_control/src/gazebo_ros_control_plugin.cpp | 7 +++---- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/gazebo_ros_control/include/gazebo_ros_control/gazebo_ros_control_plugin.h b/gazebo_ros_control/include/gazebo_ros_control/gazebo_ros_control_plugin.h index e9a9fdfa3..3062ebc7d 100644 --- a/gazebo_ros_control/include/gazebo_ros_control/gazebo_ros_control_plugin.h +++ b/gazebo_ros_control/include/gazebo_ros_control/gazebo_ros_control_plugin.h @@ -81,7 +81,6 @@ class GazeboRosControlPlugin : public gazebo::ModelPlugin protected: // Node Handles - ros::NodeHandle nh_; // no namespace ros::NodeHandle model_nh_; // namespaces to robot name // Pointer to the model diff --git a/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp b/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp index 093334b97..c54d840ce 100644 --- a/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp +++ b/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp @@ -140,7 +140,6 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element // Get parameters/settings for controllers from ROS param server model_nh_ = ros::NodeHandle(robot_namespace_); - nh_ = ros::NodeHandle(); ROS_INFO_NAMED("gazebo_ros_control", "Starting gazebo_ros_control plugin in namespace: %s", robot_namespace_.c_str()); // Read urdf from ros parameter server then @@ -224,19 +223,19 @@ std::string GazeboRosControlPlugin::getURDF(std::string param_name) const while (urdf_string.empty()) { std::string search_param_name; - if (nh_.searchParam(param_name, search_param_name)) + if (model_nh_.searchParam(param_name, search_param_name)) { ROS_INFO_ONCE_NAMED("gazebo_ros_control", "gazebo_ros_control plugin is waiting for model" " URDF in parameter [%s] on the ROS param server.", search_param_name.c_str()); - nh_.getParam(search_param_name, urdf_string); + model_nh_.getParam(search_param_name, urdf_string); } else { ROS_INFO_ONCE_NAMED("gazebo_ros_control", "gazebo_ros_control plugin is waiting for model" " URDF in parameter [%s] on the ROS param server.", robot_description_.c_str()); - nh_.getParam(param_name, urdf_string); + model_nh_.getParam(param_name, urdf_string); } usleep(100000); From b8213a2c3453b1f9bfa09cfa56b9e845a544da9a Mon Sep 17 00:00:00 2001 From: Jim Rothrock Date: Thu, 24 Oct 2013 10:15:03 -0700 Subject: [PATCH 009/153] Removed the "support more hardware interfaces" line. --- gazebo_ros_control/README.md | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/gazebo_ros_control/README.md b/gazebo_ros_control/README.md index 036198b33..01ccb5935 100644 --- a/gazebo_ros_control/README.md +++ b/gazebo_ros_control/README.md @@ -10,5 +10,4 @@ controller manager and connects it to a Gazebo model. ## Future Direction - - Implement tranmissions - - In the default plugin support more hardware interfaces + - Implement transmissions From 273db1034dca73faddcc77d43d6596d2b8f632d9 Mon Sep 17 00:00:00 2001 From: Jim Rothrock Date: Thu, 24 Oct 2013 10:17:19 -0700 Subject: [PATCH 010/153] Added support for the position hardware interface. Completed support for the velocity hardware interface. --- .../src/default_robot_hw_sim.cpp | 171 +++++++++++++----- 1 file changed, 130 insertions(+), 41 deletions(-) diff --git a/gazebo_ros_control/src/default_robot_hw_sim.cpp b/gazebo_ros_control/src/default_robot_hw_sim.cpp index c4b4332b8..df850ff4c 100644 --- a/gazebo_ros_control/src/default_robot_hw_sim.cpp +++ b/gazebo_ros_control/src/default_robot_hw_sim.cpp @@ -86,10 +86,12 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim // Resize vectors to our DOF n_dof_ = transmissions.size(); joint_names_.resize(n_dof_); + joint_hw_interface_types_.resize(n_dof_); joint_position_.resize(n_dof_); joint_velocity_.resize(n_dof_); joint_effort_.resize(n_dof_); joint_effort_command_.resize(n_dof_); + joint_position_command_.resize(n_dof_); joint_velocity_command_.resize(n_dof_); // Initialize values @@ -131,6 +133,7 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim joint_velocity_[j] = 0.0; joint_effort_[j] = 1.0; // N/m for continuous joints joint_effort_command_[j] = 0.0; + joint_position_command_[j] = 0.0; joint_velocity_command_[j] = 0.0; const std::string& hardware_interface = transmissions[j].actuators_[0].hardware_interface_; @@ -144,19 +147,30 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim joint_names_[j], &joint_position_[j], &joint_velocity_[j], &joint_effort_[j])); // Decide what kind of command interface this actuator/joint has + hardware_interface::JointHandle joint_handle; if(hardware_interface == "EffortJointInterface") { // Create effort joint interface - hardware_interface::JointHandle joint_handle(js_interface_.getHandle(joint_names_[j]), - &joint_effort_command_[j]); + joint_hw_interface_types_[j] = EFFORT; + joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]), + &joint_effort_command_[j]); ej_interface_.registerHandle(joint_handle); - registerEffortJointLimits(joint_names_[j], joint_handle, joint_limit_nh, urdf_model); + } + else if(hardware_interface == "PositionJointInterface") + { + // Create position joint interface + joint_hw_interface_types_[j] = POSITION; + joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]), + &joint_position_command_[j]); + pj_interface_.registerHandle(joint_handle); } else if(hardware_interface == "VelocityJointInterface") { // Create velocity joint interface - vj_interface_.registerHandle(hardware_interface::JointHandle( - js_interface_.getHandle(joint_names_[j]),&joint_velocity_command_[j])); + joint_hw_interface_types_[j] = VELOCITY; + joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]), + &joint_velocity_command_[j]); + vj_interface_.registerHandle(joint_handle); } else { @@ -164,31 +178,35 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim << hardware_interface ); return false; } - } - - // Register interfaces - registerInterface(&js_interface_); - registerInterface(&ej_interface_); - registerInterface(&vj_interface_); - // Get the gazebo joints that correspond to the robot joints - for(unsigned int j=0; j < n_dof_; j++) - { + // Get the gazebo joint that corresponds to the robot joint. //ROS_DEBUG_STREAM_NAMED("default_robot_hw_sim", "Getting pointer to gazebo joint: " // << joint_names_[j]); gazebo::physics::JointPtr joint = parent_model->GetJoint(joint_names_[j]); - if (joint) - { - sim_joints_.push_back(joint); - } - else + if (!joint) { ROS_ERROR_STREAM("This robot has a joint named \"" << joint_names_[j] << "\" which is not in the gazebo model."); return false; } + sim_joints_.push_back(joint); + + const double max_effort = registerJointLimits(joint_names_[j], joint_handle, + joint_hw_interface_types_[j], joint_limit_nh, + urdf_model); + // joint->SetMaxForce() must be called if joint->SetAngle() or joint->SetVelocity() are + // going to be called. joint->SetMaxForce() must *not* be called if joint->SetForce() is + // going to be called. + if (joint_hw_interface_types_[j] != EFFORT) + joint->SetMaxForce(0, max_effort); } + // Register interfaces + registerInterface(&js_interface_); + registerInterface(&ej_interface_); + registerInterface(&pj_interface_); + registerInterface(&vj_interface_); + return true; } @@ -209,29 +227,46 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim { ej_sat_interface_.enforceLimits(period); ej_limits_interface_.enforceLimits(period); + pj_sat_interface_.enforceLimits(period); + pj_limits_interface_.enforceLimits(period); + vj_sat_interface_.enforceLimits(period); + vj_limits_interface_.enforceLimits(period); - // \todo check if this joint is using a position, velocity, or effort hardware interface - // and set gazebo accordingly for(unsigned int j=0; j < n_dof_; j++) { - // Gazebo has an interesting API... - sim_joints_[j]->SetForce(0,joint_effort_command_[j]); - - // Output debug every nth msg - if( !(j % 10) && false) + switch (joint_hw_interface_types_[j]) { - ROS_DEBUG_STREAM_NAMED("robot_hw_sim","SetForce " << joint_effort_command_[j]); + case EFFORT: + // Gazebo has an interesting API... + sim_joints_[j]->SetForce(0,joint_effort_command_[j]); + + // Output debug every nth msg + if( !(j % 10) && false) + { + ROS_DEBUG_STREAM_NAMED("robot_hw_sim","SetForce " << joint_effort_command_[j]); + } + break; + case POSITION: + sim_joints_[j]->SetAngle(0,joint_position_command_[j]); + break; + case VELOCITY: + sim_joints_[j]->SetVelocity(0,joint_velocity_command_[j]); + break; } } } private: + enum HWInterfaceType {EFFORT, POSITION, VELOCITY}; // Hardware interface types + // Register the limits of the joint specified by joint_name and joint_handle. The limits are // retrieved from joint_limit_nh. If urdf_model is not NULL, limits are retrieved from it also. - void registerEffortJointLimits(const std::string& joint_name, - const hardware_interface::JointHandle& joint_handle, - const ros::NodeHandle& joint_limit_nh, - const urdf::Model *const urdf_model) + // Return the maximum effort that can be applied to the joint. + double registerJointLimits(const std::string& joint_name, + const hardware_interface::JointHandle& joint_handle, + const HWInterfaceType hw_iface_type, + const ros::NodeHandle& joint_limit_nh, + const urdf::Model *const urdf_model) { joint_limits_interface::JointLimits limits; bool has_limits = false; @@ -254,36 +289,90 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim if (joint_limits_interface::getJointLimits(joint_name, joint_limit_nh, limits)) has_limits = true; - if (has_limits) + if (!has_limits) + return 0; + + if (has_soft_limits) { - if (has_soft_limits) + switch (hw_iface_type) { - const joint_limits_interface::EffortJointSoftLimitsHandle - limits_handle(joint_handle, limits, soft_limits); - ej_limits_interface_.registerHandle(limits_handle); + case EFFORT: + { + const joint_limits_interface::EffortJointSoftLimitsHandle + limits_handle(joint_handle, limits, soft_limits); + ej_limits_interface_.registerHandle(limits_handle); + } + break; + case POSITION: + { + const joint_limits_interface::PositionJointSoftLimitsHandle + limits_handle(joint_handle, limits, soft_limits); + pj_limits_interface_.registerHandle(limits_handle); + } + break; + case VELOCITY: + { + const joint_limits_interface::VelocityJointSoftLimitsHandle + limits_handle(joint_handle, limits, soft_limits); + vj_limits_interface_.registerHandle(limits_handle); + } + break; } - else + } + else + { + switch (hw_iface_type) { - const joint_limits_interface::EffortJointSaturationHandle sat_handle(joint_handle, limits); - ej_sat_interface_.registerHandle(sat_handle); + case EFFORT: + { + const joint_limits_interface::EffortJointSaturationHandle + sat_handle(joint_handle, limits); + ej_sat_interface_.registerHandle(sat_handle); + } + break; + case POSITION: + { + const joint_limits_interface::PositionJointSaturationHandle + sat_handle(joint_handle, limits); + pj_sat_interface_.registerHandle(sat_handle); + } + break; + case VELOCITY: + { + const joint_limits_interface::VelocityJointSaturationHandle + sat_handle(joint_handle, limits); + vj_sat_interface_.registerHandle(sat_handle); + } + break; } } + + if (limits.has_effort_limits) + return limits.max_effort; + return 0; } unsigned int n_dof_; hardware_interface::JointStateInterface js_interface_; hardware_interface::EffortJointInterface ej_interface_; + hardware_interface::PositionJointInterface pj_interface_; hardware_interface::VelocityJointInterface vj_interface_; - joint_limits_interface::EffortJointSaturationInterface ej_sat_interface_; - joint_limits_interface::EffortJointSoftLimitsInterface ej_limits_interface_; + joint_limits_interface::EffortJointSaturationInterface ej_sat_interface_; + joint_limits_interface::EffortJointSoftLimitsInterface ej_limits_interface_; + joint_limits_interface::PositionJointSaturationInterface pj_sat_interface_; + joint_limits_interface::PositionJointSoftLimitsInterface pj_limits_interface_; + joint_limits_interface::VelocityJointSaturationInterface vj_sat_interface_; + joint_limits_interface::VelocityJointSoftLimitsInterface vj_limits_interface_; std::vector joint_names_; + std::vector joint_hw_interface_types_; std::vector joint_position_; std::vector joint_velocity_; std::vector joint_effort_; std::vector joint_effort_command_; + std::vector joint_position_command_; std::vector joint_velocity_command_; std::vector sim_joints_; From d7b2c00ede5839777180fc6206719e97d3c4b775 Mon Sep 17 00:00:00 2001 From: Francisco Date: Mon, 28 Oct 2013 17:01:07 +0100 Subject: [PATCH 011/153] Add time import When using the -wait option the script fails because is missing the time import --- gazebo_ros/scripts/spawn_model | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gazebo_ros/scripts/spawn_model b/gazebo_ros/scripts/spawn_model index 1b12418c3..3b3399942 100755 --- a/gazebo_ros/scripts/spawn_model +++ b/gazebo_ros/scripts/spawn_model @@ -18,7 +18,7 @@ # Author: John Hsu, Dave Coleman # -import rospy, sys, os +import rospy, sys, os, time import string import warnings From ed2782cb7df21dcbe333e101d03762eda6e0deff Mon Sep 17 00:00:00 2001 From: v4hn Date: Fri, 1 Nov 2013 14:45:22 +0100 Subject: [PATCH 012/153] replace time.sleep by rospy.Rate.sleep time was not even imported, so I don't know why this could ever have worked... --- gazebo_ros/scripts/spawn_model | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gazebo_ros/scripts/spawn_model b/gazebo_ros/scripts/spawn_model index 1b12418c3..699d01003 100755 --- a/gazebo_ros/scripts/spawn_model +++ b/gazebo_ros/scripts/spawn_model @@ -198,8 +198,9 @@ class SpawnModel(): if not self.wait_for_model == "": rospy.Subscriber("%s/model_states"%(self.gazebo_namespace), ModelStates, self.checkForModel) + r= rospy.Rate(10) while not rospy.is_shutdown() and not self.wait_for_model_exists: - time.sleep(0.1) + r.sleep() if rospy.is_shutdown(): sys.exit(0) From e582c4ad94225eceaee1bd64ac3ca837eae0619d Mon Sep 17 00:00:00 2001 From: Markus Bader Date: Mon, 4 Nov 2013 12:12:41 +0100 Subject: [PATCH 013/153] Gazebo ROS joint state publisher added --- gazebo_plugins/CMakeLists.txt | 9 ++ .../gazebo_ros_joint_state_publisher.h | 91 +++++++++++++ .../src/gazebo_ros_joint_state_publisher.cpp | 128 ++++++++++++++++++ 3 files changed, 228 insertions(+) create mode 100644 gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_state_publisher.h create mode 100644 gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index 43f68b5c5..a8adabed1 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -86,6 +86,7 @@ catkin_package( gazebo_ros_prosilica gazebo_ros_force gazebo_ros_joint_trajectory + gazebo_ros_joint_state_publisher gazebo_ros_joint_pose_trajectory gazebo_ros_diff_drive gazebo_ros_skid_steer_drive @@ -228,6 +229,13 @@ set_target_properties(gazebo_ros_joint_trajectory PROPERTIES COMPILE_FLAGS "${cx add_dependencies(gazebo_ros_joint_trajectory gazebo_msgs_gencpp) target_link_libraries(gazebo_ros_joint_trajectory ${GAZEBO_LIBRARIES} ${SDFormat_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +add_library(gazebo_ros_joint_state_publisher src/gazebo_ros_joint_state_publisher.cpp) +set_target_properties(gazebo_ros_joint_state_publisher PROPERTIES LINK_FLAGS "${ld_flags}") +set_target_properties(gazebo_ros_joint_state_publisher PROPERTIES COMPILE_FLAGS "${cxx_flags}") +add_dependencies(gazebo_ros_joint_state_publisher gazebo_msgs_gencpp) +target_link_libraries(gazebo_ros_joint_state_publisher ${GAZEBO_LIBRARIES} ${SDFormat_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + add_library(gazebo_ros_joint_pose_trajectory src/gazebo_ros_joint_pose_trajectory.cpp) set_target_properties(gazebo_ros_joint_pose_trajectory PROPERTIES LINK_FLAGS "${ld_flags}") set_target_properties(gazebo_ros_joint_pose_trajectory PROPERTIES COMPILE_FLAGS "${cxx_flags}") @@ -286,6 +294,7 @@ install(TARGETS gazebo_ros_prosilica gazebo_ros_force gazebo_ros_joint_trajectory + gazebo_ros_joint_state_publisher gazebo_ros_joint_pose_trajectory gazebo_ros_diff_drive gazebo_ros_skid_steer_drive diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_state_publisher.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_state_publisher.h new file mode 100644 index 000000000..ec705652c --- /dev/null +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_state_publisher.h @@ -0,0 +1,91 @@ +/* + * Copyright (c) 2013, Markus Bader + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY Antons Rebguns ''AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL Antons Rebguns BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + **/ + + +#ifndef JOINT_STATE_PUBLISHER_PLUGIN_HH +#define JOINT_STATE_PUBLISHER_PLUGIN_HH + +#include +#include +#include +#include +#include + +// ROS +#include +#include +#include + +// Usage in URDF: +// +// +// /pioneer2dx +// chassis_swivel_joint, swivel_wheel_joint, left_hub_joint, right_hub_joint +// 100.0 +// true +// +// + + + +namespace gazebo { +class GazeboRosJointStatePublisher : public ModelPlugin { +public: + GazeboRosJointStatePublisher(); + ~GazeboRosJointStatePublisher(); + void Load ( physics::ModelPtr _parent, sdf::ElementPtr _sdf ); + void OnUpdate ( const common::UpdateInfo & _info ); + void publishTF(); + + // Pointer to the model +private: + event::ConnectionPtr updateConnection; + physics::WorldPtr world_; + physics::ModelPtr parent_; + std::vector joints_; + + // ROS STUFF + boost::shared_ptr rosnode_; + sensor_msgs::JointState joint_state; + ros::Publisher joint_state_publisher_; + std::string tf_prefix_; + std::string robot_namespace_; + std::vector joint_names_; + + // Update Rate + double update_rate_; + double update_period_; + common::Time last_update_time_; +}; + +// Register this plugin with the simulator +GZ_REGISTER_MODEL_PLUGIN ( GazeboRosJointStatePublisher ) +} + +#endif //JOINT_STATE_PUBLISHER_PLUGIN_HH + diff --git a/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp b/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp new file mode 100644 index 000000000..0b4a9de9c --- /dev/null +++ b/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp @@ -0,0 +1,128 @@ +/* + * Copyright (c) 2013, Markus Bader + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY Antons Rebguns ''AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL Antons Rebguns BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + **/ +#include +#include +#include + +using namespace gazebo; + +GazeboRosJointStatePublisher::GazeboRosJointStatePublisher() {} + +// Destructor +GazeboRosJointStatePublisher::~GazeboRosJointStatePublisher() { + rosnode_->shutdown(); +} + +void GazeboRosJointStatePublisher::Load ( physics::ModelPtr _parent, sdf::ElementPtr _sdf ) { + // Store the pointer to the model + this->parent_ = _parent; + this->world_ = _parent->GetWorld(); + + this->robot_namespace_ = parent_->GetName (); + if ( !_sdf->HasElement ( "robotNamespace" ) ) { + ROS_INFO ( "GazeboRosJointStatePublisher Plugin missing , defaults to \"%s\"", + this->robot_namespace_.c_str() ); + } else { + this->robot_namespace_ = _sdf->GetElement ( "robotNamespace" )->Get(); + if ( this->robot_namespace_.empty() ) this->robot_namespace_ = parent_->GetName (); + } + if ( !robot_namespace_.empty() ) this->robot_namespace_ += "/"; + rosnode_ = boost::shared_ptr ( new ros::NodeHandle ( this->robot_namespace_ ) ); + + if ( !_sdf->HasElement ( "jointName" ) ) { + ROS_ASSERT ( "GazeboRosJointStatePublisher Plugin missing jointNames" ); + } else { + sdf::ElementPtr element = _sdf->GetElement ( "jointName" ) ; + std::string joint_names = element->Get(); + boost::erase_all ( joint_names, " " ); + boost::split ( joint_names_, joint_names, boost::is_any_of ( "," ) ); + } + + this->update_rate_ = 100.0; + if ( !_sdf->HasElement ( "updateRate" ) ) { + ROS_WARN ( "GazeboRosJointStatePublisher Plugin (ns = %s) missing , defaults to %f", + this->robot_namespace_.c_str(), this->update_rate_ ); + } else { + this->update_rate_ = _sdf->GetElement ( "updateRate" )->Get(); + } + + // Initialize update rate stuff + if ( this->update_rate_ > 0.0 ) { + this->update_period_ = 1.0 / this->update_rate_; + } else { + this->update_period_ = 0.0; + } + last_update_time_ = this->world_->GetSimTime(); + + for ( unsigned int i = 0; i< joint_names_.size(); i++ ) { + joints_.push_back ( this->parent_->GetJoint ( joint_names_[i] ) ); + ROS_INFO ( "joint_name: %s", joint_names_[i].c_str() ); + } + + ROS_INFO ( "Starting GazeboRosJointStatePublisher Plugin (ns = %s)!, parent name: %s", this->robot_namespace_.c_str(), parent_->GetName ().c_str() ); + + tf_prefix_ = tf::getPrefixParam ( *rosnode_ ); + joint_state_publisher_ = rosnode_->advertise ( "joint_states",1000 ); + + last_update_time_ = this->world_->GetSimTime(); + // Listen to the update event. This event is broadcast every + // simulation iteration. + this->updateConnection = event::Events::ConnectWorldUpdateBegin ( + boost::bind ( &GazeboRosJointStatePublisher::OnUpdate, this, _1 ) ); +} + +void GazeboRosJointStatePublisher::OnUpdate ( const common::UpdateInfo & _info ) { + // Apply a small linear velocity to the model. + common::Time current_time = this->world_->GetSimTime(); + double seconds_since_last_update = ( current_time - last_update_time_ ).Double(); + if ( seconds_since_last_update > update_period_ ) { + + publishTF(); + + last_update_time_+= common::Time ( update_period_ ); + + } + +} + +void GazeboRosJointStatePublisher::publishTF() { + ros::Time current_time = ros::Time::now(); + + joint_state.header.stamp = current_time; + joint_state.name.resize ( joints_.size() ); + joint_state.position.resize ( joints_.size() ); + + for ( int i = 0; i < joints_.size(); i++ ) { + physics::JointPtr joint = joints_[i]; + math::Angle angle = joint->GetAngle ( 0 ); + joint_state.name[i] = joint->GetName(); + joint_state.position[i] = angle.Radian () ; + } + joint_state_publisher_.publish ( joint_state ); +} + From 7f95b70c91d90e80c141a5f71201dc49ee9beff8 Mon Sep 17 00:00:00 2001 From: John Hsu Date: Mon, 4 Nov 2013 09:38:19 -0800 Subject: [PATCH 014/153] fix issue #38, gui segfault on model deletion by removing an obsolete call to set selected object state to "normal". --- gazebo_ros/src/gazebo_ros_api_plugin.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/gazebo_ros/src/gazebo_ros_api_plugin.cpp b/gazebo_ros/src/gazebo_ros_api_plugin.cpp index 81b5dd259..e479972a5 100644 --- a/gazebo_ros/src/gazebo_ros_api_plugin.cpp +++ b/gazebo_ros/src/gazebo_ros_api_plugin.cpp @@ -731,8 +731,6 @@ bool GazeboRosApiPlugin::deleteModel(gazebo_msgs::DeleteModel::Request &req, clearJointForces(joints[i]->GetName()); } - // clear entity from selection @todo: need to clear links if selected individually - gazebo::event::Events::setSelectedEntity(req.model_name, "normal"); // send delete model request gazebo::msgs::Request *msg = gazebo::msgs::CreateRequest("entity_delete",req.model_name); request_pub_->Publish(*msg,true); From 4b84f4d8c02599a3114031aeb5c7c94e65b98538 Mon Sep 17 00:00:00 2001 From: Johannes Meyer Date: Mon, 4 Nov 2013 21:00:58 +0100 Subject: [PATCH 015/153] gazebo_ros_pkgs: use GetMaxStepSize() for the Gazebo simulation period --- gazebo_ros_control/src/gazebo_ros_control_plugin.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp b/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp index 093334b97..ba5aff309 100644 --- a/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp +++ b/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp @@ -111,7 +111,7 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element } // Get the Gazebo simulation period - ros::Duration gazebo_period(parent_model_->GetWorld()->GetPhysicsEngine()->GetUpdatePeriod()); + ros::Duration gazebo_period(parent_model_->GetWorld()->GetPhysicsEngine()->GetMaxStepSize()); // Decide the plugin control period if(sdf_->HasElement("controlPeriod")) From fc3c93dc89134323c0d407da8b7a05acca15dca1 Mon Sep 17 00:00:00 2001 From: Markus Bader Date: Tue, 5 Nov 2013 10:29:58 +0100 Subject: [PATCH 016/153] gazebo_ros_diff_drive was enhanced to publish the wheels tf or the wheels joint state depending on two additinal xml options --- .../gazebo_plugins/gazebo_ros_diff_drive.h | 16 +++- .../gazebo_ros_joint_state_publisher.h | 6 +- gazebo_plugins/src/gazebo_ros_diff_drive.cpp | 83 +++++++++++++++---- .../src/gazebo_ros_joint_state_publisher.cpp | 16 ++-- 4 files changed, 93 insertions(+), 28 deletions(-) diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_diff_drive.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_diff_drive.h index 339c73691..3b1102a3b 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_diff_drive.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_diff_drive.h @@ -54,6 +54,7 @@ #include #include #include +#include // Custom Callback Queue #include @@ -82,6 +83,9 @@ namespace gazebo { private: void publishOdometry(double step_time); void getWheelVelocities(); + void publishWheelTF(); /// publishes the wheel tf's + void publishWheelJointState(); + physics::WorldPtr world; physics::ModelPtr parent; @@ -95,13 +99,15 @@ namespace gazebo { double torque; double wheel_speed_[2]; - physics::JointPtr joints[2]; + std::vector joints_; // ROS STUFF - ros::NodeHandle* rosnode_; + boost::shared_ptr rosnode_; ros::Publisher odometry_publisher_; ros::Subscriber cmd_vel_subscriber_; - tf::TransformBroadcaster *transform_broadcaster_; + boost::shared_ptr transform_broadcaster_; + sensor_msgs::JointState joint_state_; + ros::Publisher joint_state_publisher_; nav_msgs::Odometry odom_; std::string tf_prefix_; @@ -129,6 +135,10 @@ namespace gazebo { double update_rate_; double update_period_; common::Time last_update_time_; + + // Flags + bool publishWheelTF_; + bool publishWheelJointState_; }; diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_state_publisher.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_state_publisher.h index ec705652c..550a65b8f 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_state_publisher.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_state_publisher.h @@ -60,8 +60,7 @@ class GazeboRosJointStatePublisher : public ModelPlugin { ~GazeboRosJointStatePublisher(); void Load ( physics::ModelPtr _parent, sdf::ElementPtr _sdf ); void OnUpdate ( const common::UpdateInfo & _info ); - void publishTF(); - + void publishJointStates(); // Pointer to the model private: event::ConnectionPtr updateConnection; @@ -71,7 +70,7 @@ class GazeboRosJointStatePublisher : public ModelPlugin { // ROS STUFF boost::shared_ptr rosnode_; - sensor_msgs::JointState joint_state; + sensor_msgs::JointState joint_state_; ros::Publisher joint_state_publisher_; std::string tf_prefix_; std::string robot_namespace_; @@ -81,6 +80,7 @@ class GazeboRosJointStatePublisher : public ModelPlugin { double update_rate_; double update_period_; common::Time last_update_time_; + }; // Register this plugin with the simulator diff --git a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp index a702016a3..b23d657c1 100644 --- a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp +++ b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp @@ -66,8 +66,6 @@ namespace gazebo { // Destructor GazeboRosDiffDrive::~GazeboRosDiffDrive() { - delete rosnode_; - delete transform_broadcaster_; } // Load the controller @@ -75,6 +73,7 @@ namespace gazebo { this->parent = _parent; this->world = _parent->GetWorld(); + this->robot_namespace_ = ""; if (!_sdf->HasElement("robotNamespace")) { @@ -166,6 +165,21 @@ namespace gazebo { this->update_rate_ = _sdf->GetElement("updateRate")->Get(); } + + this->publishWheelTF_ = false; + if (_sdf->HasElement("publishWheelTF")) { + this->publishWheelTF_ = _sdf->GetElement("publishWheelTF")->Get(); + } + ROS_INFO("GazeboRosDiffDrive Plugin (ns = %s) , set to %i=%s", + this->robot_namespace_.c_str(), this->publishWheelTF_, (this->publishWheelTF_?"ture":"false")); + + this->publishWheelJointState_ = false; + if (_sdf->HasElement("publishWheelJointState")) { + this->publishWheelJointState_ = _sdf->GetElement("publishWheelJointState")->Get(); + } + ROS_INFO("GazeboRosDiffDrive Plugin (ns = %s) , set to %i=%s", + this->robot_namespace_.c_str(), this->publishWheelJointState_, (this->publishWheelJointState_?"ture":"false")); + // Initialize update rate stuff if (this->update_rate_ > 0.0) { this->update_period_ = 1.0 / this->update_rate_; @@ -182,17 +196,18 @@ namespace gazebo { rot_ = 0; alive_ = true; - joints[LEFT] = this->parent->GetJoint(left_joint_name_); - joints[RIGHT] = this->parent->GetJoint(right_joint_name_); + joints_.resize(2); + joints_[LEFT] = this->parent->GetJoint(left_joint_name_); + joints_[RIGHT] = this->parent->GetJoint(right_joint_name_); - if (!joints[LEFT]) { + if (!joints_[LEFT]) { char error[200]; snprintf(error, 200, "GazeboRosDiffDrive Plugin (ns = %s) couldn't get left hinge joint named \"%s\"", this->robot_namespace_.c_str(), this->left_joint_name_.c_str()); gzthrow(error); } - if (!joints[RIGHT]) { + if (!joints_[RIGHT]) { char error[200]; snprintf(error, 200, "GazeboRosDiffDrive Plugin (ns = %s) couldn't get right hinge joint named \"%s\"", @@ -200,8 +215,8 @@ namespace gazebo { gzthrow(error); } - joints[LEFT]->SetMaxForce(0, torque); - joints[RIGHT]->SetMaxForce(0, torque); + joints_[LEFT]->SetMaxForce(0, torque); + joints_[RIGHT]->SetMaxForce(0, torque); // Make sure the ROS node for Gazebo has already been initialized if (!ros::isInitialized()) @@ -211,12 +226,14 @@ namespace gazebo { return; } - rosnode_ = new ros::NodeHandle(this->robot_namespace_); - + rosnode_ = boost::shared_ptr ( new ros::NodeHandle ( this->robot_namespace_ ) ); ROS_INFO("Starting GazeboRosDiffDrive Plugin (ns = %s)!", this->robot_namespace_.c_str()); - + + if(this->publishWheelJointState_){ + joint_state_publisher_ = rosnode_->advertise ( "joint_states",1000 ); + } tf_prefix_ = tf::getPrefixParam(*rosnode_); - transform_broadcaster_ = new tf::TransformBroadcaster(); + transform_broadcaster_ = boost::shared_ptr ( new tf::TransformBroadcaster() ); // ROS: Subscribe to the velocity command topic (usually "cmd_vel") ros::SubscribeOptions so = @@ -238,6 +255,42 @@ namespace gazebo { boost::bind(&GazeboRosDiffDrive::UpdateChild, this)); } + +void GazeboRosDiffDrive::publishWheelJointState() { + ros::Time current_time = ros::Time::now(); + + joint_state_.header.stamp = current_time; + joint_state_.name.resize ( joints_.size() ); + joint_state_.position.resize ( joints_.size() ); + + for ( int i = 0; i < 2; i++ ) { + physics::JointPtr joint = joints_[i]; + math::Angle angle = joint->GetAngle ( 0 ); + joint_state_.name[i] = joint->GetName(); + joint_state_.position[i] = angle.Radian () ; + } + joint_state_publisher_.publish ( joint_state_ ); +} + +void GazeboRosDiffDrive::publishWheelTF() { + ros::Time current_time = ros::Time::now(); + for(int i = 0; i < 2; i++){ + std::string wheel_frame = + tf::resolve(tf_prefix_, joints_[i]->GetChild()->GetName ()); + std::string wheel_parent_frame = + tf::resolve(tf_prefix_, joints_[i]->GetParent()->GetName ()); + + math::Pose poseWheel = joints_[i]->GetChild()->GetRelativePose(); + + tf::Quaternion qt(poseWheel.rot.x, poseWheel.rot.y, poseWheel.rot.z, poseWheel.rot.w); + tf::Vector3 vt(poseWheel.pos.x, poseWheel.pos.y, poseWheel.pos.z); + + tf::Transform tfWheel(qt, vt); + transform_broadcaster_->sendTransform( + tf::StampedTransform(tfWheel, current_time, + wheel_parent_frame, wheel_frame)); + } + } // Update the controller void GazeboRosDiffDrive::UpdateChild() { @@ -247,11 +300,13 @@ namespace gazebo { if (seconds_since_last_update > update_period_) { publishOdometry(seconds_since_last_update); + if(publishWheelTF_) publishWheelTF(); + if(publishWheelJointState_) publishWheelJointState(); // Update robot in case new velocities have been requested getWheelVelocities(); - joints[LEFT]->SetVelocity(0, wheel_speed_[LEFT] / wheel_diameter_); - joints[RIGHT]->SetVelocity(0, wheel_speed_[RIGHT] / wheel_diameter_); + joints_[LEFT]->SetVelocity(0, wheel_speed_[LEFT] / wheel_diameter_); + joints_[RIGHT]->SetVelocity(0, wheel_speed_[RIGHT] / wheel_diameter_); last_update_time_+= common::Time(update_period_); diff --git a/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp b/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp index 0b4a9de9c..141555f0e 100644 --- a/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp +++ b/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp @@ -102,7 +102,7 @@ void GazeboRosJointStatePublisher::OnUpdate ( const common::UpdateInfo & _info ) double seconds_since_last_update = ( current_time - last_update_time_ ).Double(); if ( seconds_since_last_update > update_period_ ) { - publishTF(); + publishJointStates(); last_update_time_+= common::Time ( update_period_ ); @@ -110,19 +110,19 @@ void GazeboRosJointStatePublisher::OnUpdate ( const common::UpdateInfo & _info ) } -void GazeboRosJointStatePublisher::publishTF() { +void GazeboRosJointStatePublisher::publishJointStates() { ros::Time current_time = ros::Time::now(); - joint_state.header.stamp = current_time; - joint_state.name.resize ( joints_.size() ); - joint_state.position.resize ( joints_.size() ); + joint_state_.header.stamp = current_time; + joint_state_.name.resize ( joints_.size() ); + joint_state_.position.resize ( joints_.size() ); for ( int i = 0; i < joints_.size(); i++ ) { physics::JointPtr joint = joints_[i]; math::Angle angle = joint->GetAngle ( 0 ); - joint_state.name[i] = joint->GetName(); - joint_state.position[i] = angle.Radian () ; + joint_state_.name[i] = joint->GetName(); + joint_state_.position[i] = angle.Radian () ; } - joint_state_publisher_.publish ( joint_state ); + joint_state_publisher_.publish ( joint_state_ ); } From e0ada805c7f874837e43d66e6152614f01cac837 Mon Sep 17 00:00:00 2001 From: Markus Bader Date: Tue, 5 Nov 2013 10:32:42 +0100 Subject: [PATCH 017/153] fetched with upstream --- gazebo_plugins/src/gazebo_ros_diff_drive.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp index b23d657c1..798f44425 100644 --- a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp +++ b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp @@ -232,6 +232,7 @@ namespace gazebo { if(this->publishWheelJointState_){ joint_state_publisher_ = rosnode_->advertise ( "joint_states",1000 ); } + tf_prefix_ = tf::getPrefixParam(*rosnode_); transform_broadcaster_ = boost::shared_ptr ( new tf::TransformBroadcaster() ); From 12bcd77fe2ea081f5ccab5dfeeeb55e773a30611 Mon Sep 17 00:00:00 2001 From: Johannes Meyer Date: Tue, 5 Nov 2013 11:30:47 +0100 Subject: [PATCH 018/153] gazebo_ros_control: call writeSim() for each Gazebo world update independent of the control period --- .../gazebo_ros_control/gazebo_ros_control_plugin.h | 3 ++- .../src/gazebo_ros_control_plugin.cpp | 13 +++++++------ 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/gazebo_ros_control/include/gazebo_ros_control/gazebo_ros_control_plugin.h b/gazebo_ros_control/include/gazebo_ros_control/gazebo_ros_control_plugin.h index e9a9fdfa3..bd859af34 100644 --- a/gazebo_ros_control/include/gazebo_ros_control/gazebo_ros_control_plugin.h +++ b/gazebo_ros_control/include/gazebo_ros_control/gazebo_ros_control_plugin.h @@ -114,7 +114,8 @@ class GazeboRosControlPlugin : public gazebo::ModelPlugin // Timing ros::Duration control_period_; - ros::Time last_sim_time_ros_; + ros::Time last_update_sim_time_ros_; + ros::Time last_write_sim_time_ros_; }; diff --git a/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp b/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp index ba5aff309..828b786b5 100644 --- a/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp +++ b/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp @@ -196,23 +196,24 @@ void GazeboRosControlPlugin::Update() // Get the simulation time and period gazebo::common::Time gz_time_now = parent_model_->GetWorld()->GetSimTime(); ros::Time sim_time_ros(gz_time_now.sec, gz_time_now.nsec); - ros::Duration sim_period = sim_time_ros - last_sim_time_ros_; + ros::Duration sim_period = sim_time_ros - last_update_sim_time_ros_; // Check if we should update the controllers if(sim_period >= control_period_) { // Store this simulation time - last_sim_time_ros_ = sim_time_ros; + last_update_sim_time_ros_ = sim_time_ros; // Update the robot simulation with the state of the gazebo model robot_hw_sim_->readSim(sim_time_ros, sim_period); // Compute the controller commands controller_manager_->update(sim_time_ros, sim_period); - - // Update the gazebo model with the result of the controller - // computation - robot_hw_sim_->writeSim(sim_time_ros, sim_period); } + + // Update the gazebo model with the result of the controller + // computation + robot_hw_sim_->writeSim(sim_time_ros, sim_time_ros - last_write_sim_time_ros_); + last_write_sim_time_ros_ = sim_time_ros; } // Get the URDF XML from the parameter server From 7ccf94424a2f8ed6516d394c99e42ead816cb992 Mon Sep 17 00:00:00 2001 From: Johannes Meyer Date: Tue, 5 Nov 2013 11:40:27 +0100 Subject: [PATCH 019/153] gazebo_ros_control: added GazeboRosControlPlugin::Reset() method that resets the timestamps on world reset --- .../gazebo_ros_control/gazebo_ros_control_plugin.h | 3 +++ gazebo_ros_control/src/gazebo_ros_control_plugin.cpp | 8 ++++++++ 2 files changed, 11 insertions(+) diff --git a/gazebo_ros_control/include/gazebo_ros_control/gazebo_ros_control_plugin.h b/gazebo_ros_control/include/gazebo_ros_control/gazebo_ros_control_plugin.h index bd859af34..938e2dbaa 100644 --- a/gazebo_ros_control/include/gazebo_ros_control/gazebo_ros_control_plugin.h +++ b/gazebo_ros_control/include/gazebo_ros_control/gazebo_ros_control_plugin.h @@ -72,6 +72,9 @@ class GazeboRosControlPlugin : public gazebo::ModelPlugin // Called by the world update start event void Update(); + // Called on world reset + virtual void Reset(); + // Get the URDF XML from the parameter server std::string getURDF(std::string param_name) const; diff --git a/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp b/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp index 828b786b5..d46dc4f0d 100644 --- a/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp +++ b/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp @@ -216,6 +216,14 @@ void GazeboRosControlPlugin::Update() last_write_sim_time_ros_ = sim_time_ros; } +// Called on world reset +void GazeboRosControlPlugin::Reset() +{ + // Reset timing variables to not pass negative update periods to controllers on world reset + last_update_sim_time_ros_ = ros::Time(); + last_write_sim_time_ros_ = ros::Time(); +} + // Get the URDF XML from the parameter server std::string GazeboRosControlPlugin::getURDF(std::string param_name) const { From a4d94605c2969b71d1ca0e544e51c252313d2ac7 Mon Sep 17 00:00:00 2001 From: Jonathan Bohren Date: Thu, 31 Oct 2013 00:04:09 -0400 Subject: [PATCH 020/153] ros_camera_utils: Adding CameraInfoManager to satisfy full ROS camera API (relies on https://github.com/ros-perception/image_common/pull/20 ) ros_camera_utils: Adding CameraInfoManager to satisfy full ROS camera API (relies on https://github.com/ros-perception/image_common/pull/20 ) --- gazebo_plugins/CMakeLists.txt | 2 + .../gazebo_plugins/gazebo_ros_camera_utils.h | 4 + gazebo_plugins/package.xml | 2 + .../src/gazebo_ros_camera_utils.cpp | 108 ++++++++++-------- 4 files changed, 67 insertions(+), 49 deletions(-) diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index 43f68b5c5..aa6fe4d62 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -25,6 +25,7 @@ find_package(catkin REQUIRED COMPONENTS cv_bridge polled_camera diagnostic_updater + camera_info_manager ) include (FindPkgConfig) @@ -113,6 +114,7 @@ catkin_package( pcl_conversions image_transport rosconsole + camera_info_manager DEPENDS gazebo SDF diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera_utils.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera_utils.h index d74cb992e..54581a87c 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera_utils.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera_utils.h @@ -34,6 +34,7 @@ #include #include #include +#include // dynamic reconfigure stuff #include @@ -147,6 +148,9 @@ namespace gazebo protected: double distortion_t1_; protected: double distortion_t2_; + protected: boost::shared_ptr camera_info_manager_; + + /// \brief A mutex to lock access to fields /// that are used in ROS message callbacks protected: boost::mutex lock_; diff --git a/gazebo_plugins/package.xml b/gazebo_plugins/package.xml index 8397e1cf6..b1396fd9d 100644 --- a/gazebo_plugins/package.xml +++ b/gazebo_plugins/package.xml @@ -42,6 +42,7 @@ cv_bridge polled_camera diagnostic_updater + camera_info_manager gazebo gazebo_msgs @@ -66,5 +67,6 @@ message_generation cv_bridge polled_camera + camera_info_manager diff --git a/gazebo_plugins/src/gazebo_ros_camera_utils.cpp b/gazebo_plugins/src/gazebo_ros_camera_utils.cpp index 70edb4362..78ff18229 100644 --- a/gazebo_plugins/src/gazebo_ros_camera_utils.cpp +++ b/gazebo_plugins/src/gazebo_ros_camera_utils.cpp @@ -27,6 +27,7 @@ #include #include #include +#include #include @@ -271,6 +272,10 @@ void GazeboRosCameraUtils::LoadThread() this->itnode_ = new image_transport::ImageTransport(*this->rosnode_); + // initialize camera_info_manager + this->camera_info_manager_.reset(new camera_info_manager::CameraInfoManager( + *this->rosnode_, this->camera_name_)); + // resolve tf prefix std::string key; if(this->rosnode_->searchParam("tf_prefix", key)){ @@ -473,6 +478,59 @@ void GazeboRosCameraUtils::Init() } } + // fill CameraInfo + sensor_msgs::CameraInfo camera_info_msg; + + camera_info_msg.header.frame_id = this->frame_name_; + + camera_info_msg.height = this->height_; + camera_info_msg.width = this->width_; + // distortion +#if ROS_VERSION_MINIMUM(1, 3, 0) + camera_info_msg.distortion_model = "plumb_bob"; + camera_info_msg.D.resize(5); +#endif + camera_info_msg.D[0] = this->distortion_k1_; + camera_info_msg.D[1] = this->distortion_k2_; + camera_info_msg.D[2] = this->distortion_k3_; + camera_info_msg.D[3] = this->distortion_t1_; + camera_info_msg.D[4] = this->distortion_t2_; + // original camera_ matrix + camera_info_msg.K[0] = this->focal_length_; + camera_info_msg.K[1] = 0.0; + camera_info_msg.K[2] = this->cx_; + camera_info_msg.K[3] = 0.0; + camera_info_msg.K[4] = this->focal_length_; + camera_info_msg.K[5] = this->cy_; + camera_info_msg.K[6] = 0.0; + camera_info_msg.K[7] = 0.0; + camera_info_msg.K[8] = 1.0; + // rectification + camera_info_msg.R[0] = 1.0; + camera_info_msg.R[1] = 0.0; + camera_info_msg.R[2] = 0.0; + camera_info_msg.R[3] = 0.0; + camera_info_msg.R[4] = 1.0; + camera_info_msg.R[5] = 0.0; + camera_info_msg.R[6] = 0.0; + camera_info_msg.R[7] = 0.0; + camera_info_msg.R[8] = 1.0; + // camera_ projection matrix (same as camera_ matrix due + // to lack of distortion/rectification) (is this generated?) + camera_info_msg.P[0] = this->focal_length_; + camera_info_msg.P[1] = 0.0; + camera_info_msg.P[2] = this->cx_; + camera_info_msg.P[3] = -this->focal_length_ * this->hack_baseline_; + camera_info_msg.P[4] = 0.0; + camera_info_msg.P[5] = this->focal_length_; + camera_info_msg.P[6] = this->cy_; + camera_info_msg.P[7] = 0.0; + camera_info_msg.P[8] = 0.0; + camera_info_msg.P[9] = 0.0; + camera_info_msg.P[10] = 1.0; + camera_info_msg.P[11] = 0.0; + + this->camera_info_manager_->setCameraInfo(camera_info_msg); // start custom queue for camera_ this->callback_queue_thread_ = boost::thread( @@ -546,58 +604,10 @@ void GazeboRosCameraUtils::PublishCameraInfo() void GazeboRosCameraUtils::PublishCameraInfo( ros::Publisher camera_info_publisher) { - sensor_msgs::CameraInfo camera_info_msg; - // fill CameraInfo - camera_info_msg.header.frame_id = this->frame_name_; + sensor_msgs::CameraInfo camera_info_msg = camera_info_manager_->getCameraInfo(); camera_info_msg.header.stamp.sec = this->sensor_update_time_.sec; camera_info_msg.header.stamp.nsec = this->sensor_update_time_.nsec; - camera_info_msg.height = this->height_; - camera_info_msg.width = this->width_; - // distortion -#if ROS_VERSION_MINIMUM(1, 3, 0) - camera_info_msg.distortion_model = "plumb_bob"; - camera_info_msg.D.resize(5); -#endif - camera_info_msg.D[0] = this->distortion_k1_; - camera_info_msg.D[1] = this->distortion_k2_; - camera_info_msg.D[2] = this->distortion_k3_; - camera_info_msg.D[3] = this->distortion_t1_; - camera_info_msg.D[4] = this->distortion_t2_; - // original camera_ matrix - camera_info_msg.K[0] = this->focal_length_; - camera_info_msg.K[1] = 0.0; - camera_info_msg.K[2] = this->cx_; - camera_info_msg.K[3] = 0.0; - camera_info_msg.K[4] = this->focal_length_; - camera_info_msg.K[5] = this->cy_; - camera_info_msg.K[6] = 0.0; - camera_info_msg.K[7] = 0.0; - camera_info_msg.K[8] = 1.0; - // rectification - camera_info_msg.R[0] = 1.0; - camera_info_msg.R[1] = 0.0; - camera_info_msg.R[2] = 0.0; - camera_info_msg.R[3] = 0.0; - camera_info_msg.R[4] = 1.0; - camera_info_msg.R[5] = 0.0; - camera_info_msg.R[6] = 0.0; - camera_info_msg.R[7] = 0.0; - camera_info_msg.R[8] = 1.0; - // camera_ projection matrix (same as camera_ matrix due - // to lack of distortion/rectification) (is this generated?) - camera_info_msg.P[0] = this->focal_length_; - camera_info_msg.P[1] = 0.0; - camera_info_msg.P[2] = this->cx_; - camera_info_msg.P[3] = -this->focal_length_ * this->hack_baseline_; - camera_info_msg.P[4] = 0.0; - camera_info_msg.P[5] = this->focal_length_; - camera_info_msg.P[6] = this->cy_; - camera_info_msg.P[7] = 0.0; - camera_info_msg.P[8] = 0.0; - camera_info_msg.P[9] = 0.0; - camera_info_msg.P[10] = 1.0; - camera_info_msg.P[11] = 0.0; camera_info_publisher.publish(camera_info_msg); } From c1463cf5c696e0c4bf03d33d111bb1ccf1ddd593 Mon Sep 17 00:00:00 2001 From: Markus Bader Date: Thu, 7 Nov 2013 08:41:32 +0100 Subject: [PATCH 021/153] ROS_INFO on laser plugin added to see if it starts --- gazebo_plugins/src/gazebo_ros_gpu_laser.cpp | 5 +++-- gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp | 2 +- gazebo_plugins/src/gazebo_ros_laser.cpp | 5 +++-- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/gazebo_plugins/src/gazebo_ros_gpu_laser.cpp b/gazebo_plugins/src/gazebo_ros_gpu_laser.cpp index e780fa240..1302814f5 100644 --- a/gazebo_plugins/src/gazebo_ros_gpu_laser.cpp +++ b/gazebo_plugins/src/gazebo_ros_gpu_laser.cpp @@ -84,7 +84,7 @@ void GazeboRosLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) if (!this->sdf->HasElement("frameName")) { - ROS_INFO("Laser plugin missing , defaults to /world"); + ROS_INFO("GazeboRosLaser plugin missing , defaults to /world"); this->frame_name_ = "/world"; } else @@ -92,7 +92,7 @@ void GazeboRosLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) if (!this->sdf->HasElement("topicName")) { - ROS_INFO("Laser plugin missing , defaults to /world"); + ROS_INFO("GazeboRosLaser plugin missing , defaults to /world"); this->topic_name_ = "/world"; } else @@ -109,6 +109,7 @@ void GazeboRosLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) return; } + ROS_INFO ( "Starting GazeboRosLaser Plugin (ns = %s)!", this->robot_namespace_.c_str() ); // ros callback queue for processing subscription this->deferred_load_thread_ = boost::thread( boost::bind(&GazeboRosLaser::LoadThread, this)); diff --git a/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp b/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp index 141555f0e..6c1ede190 100644 --- a/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp +++ b/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp @@ -81,7 +81,7 @@ void GazeboRosJointStatePublisher::Load ( physics::ModelPtr _parent, sdf::Elemen for ( unsigned int i = 0; i< joint_names_.size(); i++ ) { joints_.push_back ( this->parent_->GetJoint ( joint_names_[i] ) ); - ROS_INFO ( "joint_name: %s", joint_names_[i].c_str() ); + ROS_INFO ( "GazeboRosJointStatePublisher is going to publish joint: %s", joint_names_[i].c_str() ); } ROS_INFO ( "Starting GazeboRosJointStatePublisher Plugin (ns = %s)!, parent name: %s", this->robot_namespace_.c_str(), parent_->GetName ().c_str() ); diff --git a/gazebo_plugins/src/gazebo_ros_laser.cpp b/gazebo_plugins/src/gazebo_ros_laser.cpp index a6ee8cfa2..1ccbc2c47 100644 --- a/gazebo_plugins/src/gazebo_ros_laser.cpp +++ b/gazebo_plugins/src/gazebo_ros_laser.cpp @@ -104,8 +104,9 @@ void GazeboRosLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. " << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); return; - } - + } + + ROS_INFO ( "Starting GazeboRosLaser Plugin (ns = %s)!", this->robot_namespace_.c_str() ); // ros callback queue for processing subscription this->deferred_load_thread_ = boost::thread( boost::bind(&GazeboRosLaser::LoadThread, this)); From 61fa83c0e11ee178479f8a5d8cf259869cafaeb5 Mon Sep 17 00:00:00 2001 From: John Hsu Date: Fri, 8 Nov 2013 18:54:43 -0800 Subject: [PATCH 022/153] fix spawn initial pose. When model has a non-zero initial pose and user specified initial model spawn pose, add the two. --- gazebo_ros/src/gazebo_ros_api_plugin.cpp | 95 ++++++++++++++++++++++-- gazebo_ros/src/gazebo_ros_api_plugin.h | 10 ++- 2 files changed, 97 insertions(+), 8 deletions(-) diff --git a/gazebo_ros/src/gazebo_ros_api_plugin.cpp b/gazebo_ros/src/gazebo_ros_api_plugin.cpp index 81b5dd259..9a1797a75 100644 --- a/gazebo_ros/src/gazebo_ros_api_plugin.cpp +++ b/gazebo_ros/src/gazebo_ros_api_plugin.cpp @@ -1987,11 +1987,17 @@ void GazeboRosApiPlugin::updateSDFAttributes(TiXmlDocument &gazebo_model_xml, st // Set the pose value for both SDFs types here if (pose_element) { + // convert pose_element to math::Pose + gazebo::math::Pose pose = this->parsePose(pose_element->GetText()); + + // add pose_element Pose to initial pose + gazebo::math::Pose model_pose = pose + gazebo::math::Pose(initial_xyz, initial_q); + // Create the string of 6 numbers std::ostringstream pose_stream; - gazebo::math::Vector3 initial_rpy = initial_q.GetAsEuler(); // convert to Euler angles for Gazebo XML - pose_stream << initial_xyz.x << " " << initial_xyz.y << " " << initial_xyz.z << " " - << initial_rpy.x << " " << initial_rpy.y << " " << initial_rpy.z; + gazebo::math::Vector3 model_rpy = model_pose.rot.GetAsEuler(); // convert to Euler angles for Gazebo XML + pose_stream << model_pose.pos.x << " " << model_pose.pos.y << " " << model_pose.pos.z << " " + << model_rpy.x << " " << model_rpy.y << " " << model_rpy.z; // Add value to pose element TiXmlText* text = new TiXmlText(pose_stream.str()); @@ -2000,6 +2006,72 @@ void GazeboRosApiPlugin::updateSDFAttributes(TiXmlDocument &gazebo_model_xml, st } +gazebo::math::Pose GazeboRosApiPlugin::parsePose(const std::string &str) +{ + std::vector pieces; + std::vector vals; + + boost::split(pieces, str, boost::is_any_of(" ")); + for (unsigned int i = 0; i < pieces.size(); ++i) + { + if (pieces[i] != "") + { + try + { + vals.push_back(boost::lexical_cast(pieces[i].c_str())); + } + catch(boost::bad_lexical_cast &e) + { + sdferr << "xml key [" << str + << "][" << i << "] value [" << pieces[i] + << "] is not a valid double from a 3-tuple\n"; + return gazebo::math::Pose(); + } + } + } + + if (vals.size() == 6) + return gazebo::math::Pose(vals[0], vals[1], vals[2], vals[3], vals[4], vals[5]); + else + { + ROS_ERROR("Beware: failed to parse string [%s] as gazebo::math::Pose, returning zeros.", str.c_str()); + return gazebo::math::Pose(); + } +} + +gazebo::math::Vector3 GazeboRosApiPlugin::parseVector3(const std::string &str) +{ + std::vector pieces; + std::vector vals; + + boost::split(pieces, str, boost::is_any_of(" ")); + for (unsigned int i = 0; i < pieces.size(); ++i) + { + if (pieces[i] != "") + { + try + { + vals.push_back(boost::lexical_cast(pieces[i].c_str())); + } + catch(boost::bad_lexical_cast &e) + { + sdferr << "xml key [" << str + << "][" << i << "] value [" << pieces[i] + << "] is not a valid double from a 3-tuple\n"; + return gazebo::math::Vector3(); + } + } + } + + if (vals.size() == 3) + return gazebo::math::Vector3(vals[0], vals[1], vals[2]); + else + { + ROS_ERROR("Beware: failed to parse string [%s] as gazebo::math::Vector3, returning zeros.", str.c_str()); + return gazebo::math::Vector3(); + } +} + void GazeboRosApiPlugin::updateURDFModelPose(TiXmlDocument &gazebo_model_xml, gazebo::math::Vector3 initial_xyz, gazebo::math::Quaternion initial_q) { TiXmlElement* model_tixml = (gazebo_model_xml.FirstChildElement("robot")); @@ -2015,17 +2087,28 @@ void GazeboRosApiPlugin::updateURDFModelPose(TiXmlDocument &gazebo_model_xml, ga model_tixml->LinkEndChild(origin_key); } + gazebo::math::Vector3 xyz; + gazebo::math::Vector3 rpy; if (origin_key->Attribute("xyz")) + { + xyz = this->parseVector3(origin_key->Attribute("xyz")); origin_key->RemoveAttribute("xyz"); + } if (origin_key->Attribute("rpy")) + { + rpy = this->parseVector3(origin_key->Attribute("rpy")); origin_key->RemoveAttribute("rpy"); + } + + // add xyz, rpy to initial pose + gazebo::math::Pose model_pose = gazebo::math::Pose(xyz, rpy) + gazebo::math::Pose(initial_xyz, initial_q); std::ostringstream xyz_stream; - xyz_stream << initial_xyz.x << " " << initial_xyz.y << " " << initial_xyz.z; + xyz_stream << model_pose.pos.x << " " << model_pose.pos.y << " " << model_pose.pos.z; std::ostringstream rpy_stream; - gazebo::math::Vector3 initial_rpy = initial_q.GetAsEuler(); // convert to Euler angles for Gazebo XML - rpy_stream << initial_rpy.x << " " << initial_rpy.y << " " << initial_rpy.z; + gazebo::math::Vector3 model_rpy = model_pose.rot.GetAsEuler(); // convert to Euler angles for Gazebo XML + rpy_stream << model_rpy.x << " " << model_rpy.y << " " << model_rpy.z; origin_key->SetAttribute("xyz",xyz_stream.str()); origin_key->SetAttribute("rpy",rpy_stream.str()); diff --git a/gazebo_ros/src/gazebo_ros_api_plugin.h b/gazebo_ros/src/gazebo_ros_api_plugin.h index 7305bbf5a..ed8b75c1b 100644 --- a/gazebo_ros/src/gazebo_ros_api_plugin.h +++ b/gazebo_ros/src/gazebo_ros_api_plugin.h @@ -251,11 +251,11 @@ class GazeboRosApiPlugin : public SystemPlugin void updateSDFAttributes(TiXmlDocument &gazebo_model_xml, std::string model_name, gazebo::math::Vector3 initial_xyz, gazebo::math::Quaternion initial_q); - /// \brief Update the model name and pose of the URDF file before sending to Gazebo + /// \brief Update the model pose of the URDF file before sending to Gazebo void updateURDFModelPose(TiXmlDocument &gazebo_model_xml, gazebo::math::Vector3 initial_xyz, gazebo::math::Quaternion initial_q); - /// \brief + /// \brief Update the model name of the URDF file before sending to Gazebo void updateURDFName(TiXmlDocument &gazebo_model_xml, std::string model_name); /// \brief @@ -289,6 +289,12 @@ class GazeboRosApiPlugin : public SystemPlugin /// \brief Connect to Gazebo via its plugin interface, get a pointer to the world, start events void loadGazeboRosApiPlugin(std::string world_name); + /// \brief convert xml to Pose + gazebo::math::Pose parsePose(const std::string &str); + + /// \brief convert xml to Pose + gazebo::math::Vector3 parseVector3(const std::string &str); + // track if the desconstructor event needs to occur bool plugin_loaded_; From dee2aaabeaec19b56c42b1d59cac896b711ee581 Mon Sep 17 00:00:00 2001 From: John Hsu Date: Sat, 9 Nov 2013 11:46:55 -0800 Subject: [PATCH 023/153] fix indentation --- gazebo_ros/scripts/spawn_model | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gazebo_ros/scripts/spawn_model b/gazebo_ros/scripts/spawn_model index de6d13f8f..3bf688aeb 100755 --- a/gazebo_ros/scripts/spawn_model +++ b/gazebo_ros/scripts/spawn_model @@ -198,7 +198,7 @@ class SpawnModel(): if not self.wait_for_model == "": rospy.Subscriber("%s/model_states"%(self.gazebo_namespace), ModelStates, self.checkForModel) - r= rospy.Rate(10) + r= rospy.Rate(10) while not rospy.is_shutdown() and not self.wait_for_model_exists: r.sleep() From cafce361678b82ece059785ba91f1b8fbc1ef48c Mon Sep 17 00:00:00 2001 From: John Hsu Date: Sat, 9 Nov 2013 12:36:37 -0800 Subject: [PATCH 024/153] fix sdf spawn with initial pose --- gazebo_ros/scripts/spawn_model | 2 +- gazebo_ros/src/gazebo_ros_api_plugin.cpp | 30 ++++++++++-------------- 2 files changed, 14 insertions(+), 18 deletions(-) diff --git a/gazebo_ros/scripts/spawn_model b/gazebo_ros/scripts/spawn_model index 3bf688aeb..d1e94fca9 100755 --- a/gazebo_ros/scripts/spawn_model +++ b/gazebo_ros/scripts/spawn_model @@ -198,7 +198,7 @@ class SpawnModel(): if not self.wait_for_model == "": rospy.Subscriber("%s/model_states"%(self.gazebo_namespace), ModelStates, self.checkForModel) - r= rospy.Rate(10) + r= rospy.Rate(10) while not rospy.is_shutdown() and not self.wait_for_model_exists: r.sleep() diff --git a/gazebo_ros/src/gazebo_ros_api_plugin.cpp b/gazebo_ros/src/gazebo_ros_api_plugin.cpp index c8b1e95d2..88dd730de 100644 --- a/gazebo_ros/src/gazebo_ros_api_plugin.cpp +++ b/gazebo_ros/src/gazebo_ros_api_plugin.cpp @@ -1929,12 +1929,7 @@ void GazeboRosApiPlugin::updateSDFAttributes(TiXmlDocument &gazebo_model_xml, st // Create the pose element if it doesn't exist if (!pose_element) - { pose_element = new TiXmlElement("pose"); - model_tixml->LinkEndChild(pose_element); - } - - // Pose is set at bottom of function } else { @@ -1945,21 +1940,21 @@ void GazeboRosApiPlugin::updateSDFAttributes(TiXmlDocument &gazebo_model_xml, st ROS_WARN("Could not find or element in sdf, so name and initial position cannot be applied"); return; } - // Check SDF for required include element - TiXmlElement* include_tixml = world_tixml->FirstChildElement("include"); - if (!include_tixml) + // If not element, check SDF for required include element + model_tixml = world_tixml->FirstChildElement("include"); + if (!model_tixml) { ROS_WARN("Could not find element in sdf, so name and initial position cannot be applied"); return; } // Check for name element - TiXmlElement* name_tixml = include_tixml->FirstChildElement("name"); + TiXmlElement* name_tixml = model_tixml->FirstChildElement("name"); if (!name_tixml) { // Create the name element name_tixml = new TiXmlElement("name"); - include_tixml->LinkEndChild(name_tixml); + model_tixml->LinkEndChild(name_tixml); } // Set the text within the name element @@ -1967,18 +1962,14 @@ void GazeboRosApiPlugin::updateSDFAttributes(TiXmlDocument &gazebo_model_xml, st name_tixml->LinkEndChild( text ); // Check for the pose element - pose_element = include_tixml->FirstChildElement("pose"); + pose_element = model_tixml->FirstChildElement("pose"); // Create the pose element if it doesn't exist if (!pose_element) - { pose_element = new TiXmlElement("pose"); - include_tixml->LinkEndChild(pose_element); - } - // Pose is set at bottom of function } - // Set the pose value for both SDFs types here + // Set and link the pose element after adding initial pose if (pose_element) { // convert pose_element to math::Pose @@ -1993,11 +1984,16 @@ void GazeboRosApiPlugin::updateSDFAttributes(TiXmlDocument &gazebo_model_xml, st pose_stream << model_pose.pos.x << " " << model_pose.pos.y << " " << model_pose.pos.z << " " << model_rpy.x << " " << model_rpy.y << " " << model_rpy.z; + ROS_ERROR("debug: %s", pose_stream.str().c_str()); + // Add value to pose element TiXmlText* text = new TiXmlText(pose_stream.str()); - pose_element->LinkEndChild( text ); + TiXmlElement* new_pose_element = new TiXmlElement("pose"); + new_pose_element->LinkEndChild(text); + model_tixml->LinkEndChild(new_pose_element); } + } gazebo::math::Pose GazeboRosApiPlugin::parsePose(const std::string &str) From a7a91a9dd8a2820705a32e2775325b5f258f6aff Mon Sep 17 00:00:00 2001 From: John Hsu Date: Sat, 9 Nov 2013 12:36:53 -0800 Subject: [PATCH 025/153] fix sdf spawn with initial pose --- gazebo_ros/src/gazebo_ros_api_plugin.cpp | 38 ++++++++++-------------- 1 file changed, 16 insertions(+), 22 deletions(-) diff --git a/gazebo_ros/src/gazebo_ros_api_plugin.cpp b/gazebo_ros/src/gazebo_ros_api_plugin.cpp index 88dd730de..68c51f9d9 100644 --- a/gazebo_ros/src/gazebo_ros_api_plugin.cpp +++ b/gazebo_ros/src/gazebo_ros_api_plugin.cpp @@ -1923,13 +1923,6 @@ void GazeboRosApiPlugin::updateSDFAttributes(TiXmlDocument &gazebo_model_xml, st } // replace with user specified name model_tixml->SetAttribute("name",model_name); - - // Check for the pose element - pose_element = model_tixml->FirstChildElement("pose"); - - // Create the pose element if it doesn't exist - if (!pose_element) - pose_element = new TiXmlElement("pose"); } else { @@ -1960,28 +1953,31 @@ void GazeboRosApiPlugin::updateSDFAttributes(TiXmlDocument &gazebo_model_xml, st // Set the text within the name element TiXmlText* text = new TiXmlText(model_name); name_tixml->LinkEndChild( text ); + } - // Check for the pose element - pose_element = model_tixml->FirstChildElement("pose"); - // Create the pose element if it doesn't exist - if (!pose_element) - pose_element = new TiXmlElement("pose"); - } + // Check for the pose element + pose_element = model_tixml->FirstChildElement("pose"); + gazebo::math::Pose model_pose; - // Set and link the pose element after adding initial pose + // Create the pose element if it doesn't exist + // Remove it if it exists, since we are inserting a new one if (pose_element) { - // convert pose_element to math::Pose - gazebo::math::Pose pose = this->parsePose(pose_element->GetText()); + // save pose_element in math::Pose and remove child + model_pose = this->parsePose(pose_element->GetText()); + model_tixml->RemoveChild(pose_element); + } + // Set and link the pose element after adding initial pose + { // add pose_element Pose to initial pose - gazebo::math::Pose model_pose = pose + gazebo::math::Pose(initial_xyz, initial_q); + gazebo::math::Pose new_model_pose = model_pose + gazebo::math::Pose(initial_xyz, initial_q); // Create the string of 6 numbers std::ostringstream pose_stream; - gazebo::math::Vector3 model_rpy = model_pose.rot.GetAsEuler(); // convert to Euler angles for Gazebo XML - pose_stream << model_pose.pos.x << " " << model_pose.pos.y << " " << model_pose.pos.z << " " + gazebo::math::Vector3 model_rpy = new_model_pose.rot.GetAsEuler(); // convert to Euler angles for Gazebo XML + pose_stream << new_model_pose.pos.x << " " << new_model_pose.pos.y << " " << new_model_pose.pos.z << " " << model_rpy.x << " " << model_rpy.y << " " << model_rpy.z; ROS_ERROR("debug: %s", pose_stream.str().c_str()); @@ -1992,8 +1988,6 @@ void GazeboRosApiPlugin::updateSDFAttributes(TiXmlDocument &gazebo_model_xml, st new_pose_element->LinkEndChild(text); model_tixml->LinkEndChild(new_pose_element); } - - } gazebo::math::Pose GazeboRosApiPlugin::parsePose(const std::string &str) @@ -2159,7 +2153,7 @@ bool GazeboRosApiPlugin::spawnAndConform(TiXmlDocument &gazebo_model_xml, std::s std::ostringstream stream; stream << gazebo_model_xml; std::string gazebo_model_xml_string = stream.str(); - //ROS_DEBUG("Gazebo Model XML\n\n%s\n\n ",gazebo_model_xml_string.c_str()); + ROS_ERROR("Gazebo Model XML\n\n%s\n\n ",gazebo_model_xml_string.c_str()); // publish to factory topic gazebo::msgs::Factory msg; From 262c01e129c6b1b028300d55820f06ea0e09488f Mon Sep 17 00:00:00 2001 From: John Hsu Date: Sat, 9 Nov 2013 12:37:50 -0800 Subject: [PATCH 026/153] remove debug statement --- gazebo_ros/src/gazebo_ros_api_plugin.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/gazebo_ros/src/gazebo_ros_api_plugin.cpp b/gazebo_ros/src/gazebo_ros_api_plugin.cpp index 68c51f9d9..5ff3c3e48 100644 --- a/gazebo_ros/src/gazebo_ros_api_plugin.cpp +++ b/gazebo_ros/src/gazebo_ros_api_plugin.cpp @@ -1980,8 +1980,6 @@ void GazeboRosApiPlugin::updateSDFAttributes(TiXmlDocument &gazebo_model_xml, st pose_stream << new_model_pose.pos.x << " " << new_model_pose.pos.y << " " << new_model_pose.pos.z << " " << model_rpy.x << " " << model_rpy.y << " " << model_rpy.z; - ROS_ERROR("debug: %s", pose_stream.str().c_str()); - // Add value to pose element TiXmlText* text = new TiXmlText(pose_stream.str()); TiXmlElement* new_pose_element = new TiXmlElement("pose"); @@ -2153,7 +2151,7 @@ bool GazeboRosApiPlugin::spawnAndConform(TiXmlDocument &gazebo_model_xml, std::s std::ostringstream stream; stream << gazebo_model_xml; std::string gazebo_model_xml_string = stream.str(); - ROS_ERROR("Gazebo Model XML\n\n%s\n\n ",gazebo_model_xml_string.c_str()); + ROS_DEBUG("Gazebo Model XML\n\n%s\n\n ",gazebo_model_xml_string.c_str()); // publish to factory topic gazebo::msgs::Factory msg; From e610345d08fd62174f3aaaeb1d743a49fbdf7590 Mon Sep 17 00:00:00 2001 From: John Hsu Date: Wed, 13 Nov 2013 18:02:51 -0800 Subject: [PATCH 027/153] preparing for 2.3.4 release (catkin_generate_changelog, catkin_tag_changelog) --- gazebo_msgs/CHANGELOG.rst | 3 +++ gazebo_plugins/CHANGELOG.rst | 13 +++++++++++++ gazebo_ros/CHANGELOG.rst | 23 ++++++++++++++++++++++- gazebo_ros_control/CHANGELOG.rst | 13 +++++++++++++ gazebo_ros_pkgs/CHANGELOG.rst | 3 +++ 5 files changed, 54 insertions(+), 1 deletion(-) diff --git a/gazebo_msgs/CHANGELOG.rst b/gazebo_msgs/CHANGELOG.rst index 04b5c14e8..611bfbe89 100644 --- a/gazebo_msgs/CHANGELOG.rst +++ b/gazebo_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gazebo_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.3.4 (2013-11-13) +------------------ + 2.3.3 (2013-10-10) ------------------ diff --git a/gazebo_plugins/CHANGELOG.rst b/gazebo_plugins/CHANGELOG.rst index d363bd470..caa2cc3bd 100644 --- a/gazebo_plugins/CHANGELOG.rst +++ b/gazebo_plugins/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package gazebo_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.3.4 (2013-11-13) +------------------ +* Simplify ``gazebo_plugins/CMakeLists.txt`` + Replace ``cxx_flags`` and ``ld_flags`` variables with simpler cmake macros + and eliminate unnecessary references to ``SDFormat_LIBRARIES``, since + they are already part of ``GAZEBO_LIBRARIES``. +* Put some cmake lists on multiple lines to improve readability. +* Add dependencies on dynamic reconfigure files + Occasionally the build can fail due to some targets having an + undeclared dependency on automatically generated dynamic + reconfigure files (GazeboRosCameraConfig.h for example). This + commit declares several of those dependencies. + 2.3.3 (2013-10-10) ------------------ * gazebo_plugins: use shared pointers for variables shared among cameras diff --git a/gazebo_ros/CHANGELOG.rst b/gazebo_ros/CHANGELOG.rst index f4ece2d1b..5e02f1399 100644 --- a/gazebo_ros/CHANGELOG.rst +++ b/gazebo_ros/CHANGELOG.rst @@ -2,10 +2,31 @@ Changelog for package gazebo_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.3.4 (2013-11-13) +------------------ +* remove debug statement +* fix sdf spawn with initial pose +* fix sdf spawn with initial pose +* Merge branch 'hydro-devel' into ``spawn_model_pose_fix`` +* fix indentation +* Merge pull request `#142 `_ from hsu/hydro-devel + fix issue `#38 `_, gui segfault on model deletion +* Merge pull request `#140 `_ from ``v4hn/spawn_model_sleep`` + replace time.sleep by rospy.Rate.sleep +* fix spawn initial pose. When model has a non-zero initial pose and user specified initial model spawn pose, add the two. +* fix issue `#38 `_, gui segfault on model deletion by removing an obsolete call to set selected object state to "normal". +* replace time.sleep by rospy.Rate.sleep + time was not even imported, so I don't know + why this could ever have worked... +* Add time import + When using the -wait option the script fails because is missing the time import +* Use pre-increment for iterators +* Fix iterator erase() problems + 2.3.3 (2013-10-10) ------------------ * Cleaned up unnecessary debug output that was recently added -* Fixed issue where catkin_find returns more than one library if it is installed from both source and debian +* Fixed issue where ``catkin_find`` returns more than one library if it is installed from both source and debian 2.3.2 (2013-09-19) ------------------ diff --git a/gazebo_ros_control/CHANGELOG.rst b/gazebo_ros_control/CHANGELOG.rst index d3f29f860..95cd2d361 100644 --- a/gazebo_ros_control/CHANGELOG.rst +++ b/gazebo_ros_control/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package gazebo_ros_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.3.4 (2013-11-13) +------------------ +* Merge pull request `#144 `_ from meyerj/fix-125 + Fixed `#125 `_: ``gazebo_ros_control``: controlPeriod greater than the simulation period causes unexpected results +* Merge pull request `#134 `_ from meyerj/gazebo-ros-control-use-model-nh + ``gazebo_ros_control``: Use the model NodeHandle to get the ``robot_description`` parameter +* ``gazebo_ros_control``: added GazeboRosControlPlugin::Reset() method that resets the timestamps on world reset +* ``gazebo_ros_control``: call writeSim() for each Gazebo world update independent of the control period +* ``gazebo_ros_pkgs``: use GetMaxStepSize() for the Gazebo simulation period +* ``gazebo_ros_control``: use the model NodeHandle to get the ``robot_description`` parameter +* Add missing ``run_depend`` to urdf in ``gazebo_ros_control`` +* Remove dependency to meta-package ``ros_controllers`` + 2.3.3 (2013-10-10) ------------------ * Eliminated a joint_name variable and replaced it with `joint_names_[j]`. diff --git a/gazebo_ros_pkgs/CHANGELOG.rst b/gazebo_ros_pkgs/CHANGELOG.rst index 093f59b8d..8acd1d72d 100644 --- a/gazebo_ros_pkgs/CHANGELOG.rst +++ b/gazebo_ros_pkgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gazebo_ros_pkgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.3.4 (2013-11-13) +------------------ + 2.3.3 (2013-10-10) ------------------ From df5dd3715cb5379f557b46bf8d369a53e4a850c4 Mon Sep 17 00:00:00 2001 From: John Hsu Date: Wed, 13 Nov 2013 18:05:45 -0800 Subject: [PATCH 028/153] "2.3.4" --- gazebo_msgs/package.xml | 2 +- gazebo_plugins/package.xml | 2 +- gazebo_ros/package.xml | 2 +- gazebo_ros_control/package.xml | 2 +- gazebo_ros_pkgs/package.xml | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/gazebo_msgs/package.xml b/gazebo_msgs/package.xml index 98846003c..9bd9e8554 100644 --- a/gazebo_msgs/package.xml +++ b/gazebo_msgs/package.xml @@ -1,7 +1,7 @@ gazebo_msgs - 2.3.3 + 2.3.4 Message and service data structures for interacting with Gazebo from ROS. diff --git a/gazebo_plugins/package.xml b/gazebo_plugins/package.xml index 8397e1cf6..069db344a 100644 --- a/gazebo_plugins/package.xml +++ b/gazebo_plugins/package.xml @@ -1,6 +1,6 @@ gazebo_plugins - 2.3.3 + 2.3.4 Robot-independent Gazebo plugins for sensors, motors and dynamic reconfigurable components. diff --git a/gazebo_ros/package.xml b/gazebo_ros/package.xml index b9c2738fc..f30c93af1 100644 --- a/gazebo_ros/package.xml +++ b/gazebo_ros/package.xml @@ -1,7 +1,7 @@ gazebo_ros - 2.3.3 + 2.3.4 Provides ROS plugins that offer message and service publishers for interfacing with Gazebo through ROS. Formally simulator_gazebo/gazebo diff --git a/gazebo_ros_control/package.xml b/gazebo_ros_control/package.xml index 07632a743..e67842143 100644 --- a/gazebo_ros_control/package.xml +++ b/gazebo_ros_control/package.xml @@ -1,6 +1,6 @@ gazebo_ros_control - 2.3.3 + 2.3.4 gazebo_ros_control Jonathan Bohren diff --git a/gazebo_ros_pkgs/package.xml b/gazebo_ros_pkgs/package.xml index 9299fcde4..c25a70d60 100644 --- a/gazebo_ros_pkgs/package.xml +++ b/gazebo_ros_pkgs/package.xml @@ -1,7 +1,7 @@ gazebo_ros_pkgs - 2.3.3 + 2.3.4 Interface for using ROS with the Gazebo simulator. John Hsu From 8cabe91d44e22e92f7d0597032fb3eee026a9282 Mon Sep 17 00:00:00 2001 From: John Hsu Date: Wed, 13 Nov 2013 18:52:38 -0800 Subject: [PATCH 029/153] bump patch version for indigo-devel to 2.4.1 --- gazebo_msgs/CHANGELOG.rst | 2 +- gazebo_msgs/package.xml | 2 +- gazebo_plugins/CHANGELOG.rst | 2 +- gazebo_plugins/package.xml | 2 +- gazebo_ros/CHANGELOG.rst | 2 +- gazebo_ros/package.xml | 2 +- gazebo_ros_control/CHANGELOG.rst | 2 +- gazebo_ros_control/package.xml | 2 +- gazebo_ros_pkgs/CHANGELOG.rst | 2 +- gazebo_ros_pkgs/package.xml | 2 +- 10 files changed, 10 insertions(+), 10 deletions(-) diff --git a/gazebo_msgs/CHANGELOG.rst b/gazebo_msgs/CHANGELOG.rst index eebbcdf0c..85219e2b4 100644 --- a/gazebo_msgs/CHANGELOG.rst +++ b/gazebo_msgs/CHANGELOG.rst @@ -2,7 +2,7 @@ Changelog for package gazebo_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.4.0-2 (2013-11-13) +2.4.1 (2013-11-13) ------------------ * rerelease because sdformat became libsdformat, but we also based change on 2.3.4 in hydro-devel. diff --git a/gazebo_msgs/package.xml b/gazebo_msgs/package.xml index ae772e5d7..c8b42c940 100644 --- a/gazebo_msgs/package.xml +++ b/gazebo_msgs/package.xml @@ -1,7 +1,7 @@ gazebo_msgs - 2.4.0 + 2.4.1 Message and service data structures for interacting with Gazebo from ROS. diff --git a/gazebo_plugins/CHANGELOG.rst b/gazebo_plugins/CHANGELOG.rst index d2e4ed5d3..ec6b5bf84 100644 --- a/gazebo_plugins/CHANGELOG.rst +++ b/gazebo_plugins/CHANGELOG.rst @@ -2,7 +2,7 @@ Changelog for package gazebo_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.4.0-2 (2013-11-13) +2.4.1 (2013-11-13) ------------------ * rerelease because sdformat became libsdformat, but we also based change on 2.3.4 in hydro-devel. * Simplify ``gazebo_plugins/CMakeLists.txt`` diff --git a/gazebo_plugins/package.xml b/gazebo_plugins/package.xml index e3b03dbdd..cdc8010df 100644 --- a/gazebo_plugins/package.xml +++ b/gazebo_plugins/package.xml @@ -1,6 +1,6 @@ gazebo_plugins - 2.4.0 + 2.4.1 Robot-independent Gazebo plugins for sensors, motors and dynamic reconfigurable components. diff --git a/gazebo_ros/CHANGELOG.rst b/gazebo_ros/CHANGELOG.rst index 31f0b4955..bbf54c029 100644 --- a/gazebo_ros/CHANGELOG.rst +++ b/gazebo_ros/CHANGELOG.rst @@ -2,7 +2,7 @@ Changelog for package gazebo_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.4.0-2 (2013-11-13) +2.4.1 (2013-11-13) ------------------ * rerelease because sdformat became libsdformat, but we also based change on 2.3.4 in hydro-devel. * remove debug statement diff --git a/gazebo_ros/package.xml b/gazebo_ros/package.xml index abc42de64..6215f9cf2 100644 --- a/gazebo_ros/package.xml +++ b/gazebo_ros/package.xml @@ -1,7 +1,7 @@ gazebo_ros - 2.4.0 + 2.4.1 Provides ROS plugins that offer message and service publishers for interfacing with Gazebo through ROS. Formally simulator_gazebo/gazebo diff --git a/gazebo_ros_control/CHANGELOG.rst b/gazebo_ros_control/CHANGELOG.rst index e86055b77..0658b31ed 100644 --- a/gazebo_ros_control/CHANGELOG.rst +++ b/gazebo_ros_control/CHANGELOG.rst @@ -2,7 +2,7 @@ Changelog for package gazebo_ros_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.4.0-2 (2013-11-13) +2.4.1 (2013-11-13) ------------------ * rerelease because sdformat became libsdformat, but we also based change on 2.3.4 in hydro-devel. * Merge pull request `#144 `_ from meyerj/fix-125 diff --git a/gazebo_ros_control/package.xml b/gazebo_ros_control/package.xml index 9870ebdd3..1a42cd114 100644 --- a/gazebo_ros_control/package.xml +++ b/gazebo_ros_control/package.xml @@ -1,6 +1,6 @@ gazebo_ros_control - 2.4.0 + 2.4.1 gazebo_ros_control Jonathan Bohren diff --git a/gazebo_ros_pkgs/CHANGELOG.rst b/gazebo_ros_pkgs/CHANGELOG.rst index 77a3cc0e7..c2af8fcf0 100644 --- a/gazebo_ros_pkgs/CHANGELOG.rst +++ b/gazebo_ros_pkgs/CHANGELOG.rst @@ -2,7 +2,7 @@ Changelog for package gazebo_ros_pkgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.4.0-2 (2013-11-13) +2.4.1 (2013-11-13) ------------------ * rerelease because sdformat became libsdformat, but we also based change on 2.3.4 in hydro-devel. diff --git a/gazebo_ros_pkgs/package.xml b/gazebo_ros_pkgs/package.xml index 75ee87432..33aac4248 100644 --- a/gazebo_ros_pkgs/package.xml +++ b/gazebo_ros_pkgs/package.xml @@ -1,7 +1,7 @@ gazebo_ros_pkgs - 2.4.0 + 2.4.1 Interface for using ROS with the Gazebo simulator. John Hsu From d5e1beed9ad8a6ee7015e42eb74a4b2696729c28 Mon Sep 17 00:00:00 2001 From: Jim Rothrock Date: Tue, 3 Dec 2013 12:28:46 -0800 Subject: [PATCH 030/153] gazebo_ros_control now depends on control_toolbox. --- gazebo_ros_control/CMakeLists.txt | 1 + gazebo_ros_control/package.xml | 2 ++ 2 files changed, 3 insertions(+) diff --git a/gazebo_ros_control/CMakeLists.txt b/gazebo_ros_control/CMakeLists.txt index 3969d1e8d..049f927b1 100644 --- a/gazebo_ros_control/CMakeLists.txt +++ b/gazebo_ros_control/CMakeLists.txt @@ -5,6 +5,7 @@ project(gazebo_ros_control) find_package(catkin REQUIRED COMPONENTS roscpp gazebo_ros + control_toolbox controller_manager hardware_interface transmission_interface diff --git a/gazebo_ros_control/package.xml b/gazebo_ros_control/package.xml index e67842143..a6676bdaf 100644 --- a/gazebo_ros_control/package.xml +++ b/gazebo_ros_control/package.xml @@ -19,6 +19,7 @@ roscpp gazebo_ros + control_toolbox controller_manager pluginlib transmission_interface @@ -27,6 +28,7 @@ roscpp gazebo_ros + control_toolbox controller_manager pluginlib transmission_interface From cd2bb58691b9e1332a2750d442bf398a105415b4 Mon Sep 17 00:00:00 2001 From: Jim Rothrock Date: Tue, 3 Dec 2013 12:39:45 -0800 Subject: [PATCH 031/153] Position-controlled and velocity-controlled joints now use PID controllers instead of calling SetAngle() or SetVelocity(). readSim() now longer calls angles::shortest_angular_distance() when a joint is prismatic. PLUGINLIB_EXPORT_CLASS is now used to register the plugin. --- .../src/default_robot_hw_sim.cpp | 156 ++++++++++++++---- 1 file changed, 123 insertions(+), 33 deletions(-) diff --git a/gazebo_ros_control/src/default_robot_hw_sim.cpp b/gazebo_ros_control/src/default_robot_hw_sim.cpp index df850ff4c..67e62b139 100644 --- a/gazebo_ros_control/src/default_robot_hw_sim.cpp +++ b/gazebo_ros_control/src/default_robot_hw_sim.cpp @@ -42,6 +42,7 @@ #define _GAZEBO_ROS_CONTROL___DEFAULT_ROBOT_HW_SIM_H_ // ros_control +#include #include #include #include @@ -65,6 +66,16 @@ // URDF #include +namespace +{ + +double clamp(const double val, const double min_val, const double max_val) +{ + return std::min(std::max(val, min_val), max_val); +} + +} + namespace gazebo_ros_control { @@ -86,7 +97,12 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim // Resize vectors to our DOF n_dof_ = transmissions.size(); joint_names_.resize(n_dof_); + joint_types_.resize(n_dof_); + joint_lower_limits_.resize(n_dof_); + joint_upper_limits_.resize(n_dof_); + joint_effort_limits_.resize(n_dof_); joint_hw_interface_types_.resize(n_dof_); + pid_controllers_.resize(n_dof_); joint_position_.resize(n_dof_); joint_velocity_.resize(n_dof_); joint_effort_.resize(n_dof_); @@ -191,14 +207,23 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim } sim_joints_.push_back(joint); - const double max_effort = registerJointLimits(joint_names_[j], joint_handle, - joint_hw_interface_types_[j], joint_limit_nh, - urdf_model); - // joint->SetMaxForce() must be called if joint->SetAngle() or joint->SetVelocity() are - // going to be called. joint->SetMaxForce() must *not* be called if joint->SetForce() is - // going to be called. + registerJointLimits(joint_names_[j], joint_handle, joint_hw_interface_types_[j], + joint_limit_nh, urdf_model, + &joint_types_[j], &joint_lower_limits_[j], &joint_upper_limits_[j], + &joint_effort_limits_[j]); if (joint_hw_interface_types_[j] != EFFORT) - joint->SetMaxForce(0, max_effort); + { + // Initialize the PID controller. + + const ros::NodeHandle nh(model_nh, robot_namespace + "/gazebo_ros_control/pid_gains/" + + joint_names_[j]); + if (!pid_controllers_[j].init(nh)) + { + ROS_ERROR_STREAM("No PID controller gain values were found for joint \"" << + joint_names_[j] << "\"."); + return false; + } + } } // Register interfaces @@ -215,9 +240,15 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim for(unsigned int j=0; j < n_dof_; j++) { // Gazebo has an interesting API... - // \todo If the joint is a slider joint, do not call shortest_angular_distance(). - joint_position_[j] += angles::shortest_angular_distance(joint_position_[j], - sim_joints_[j]->GetAngle(0).Radian()); + if (joint_types_[j] == urdf::Joint::PRISMATIC) + { + joint_position_[j] = sim_joints_[j]->GetAngle(0).Radian(); + } + else + { + joint_position_[j] += angles::shortest_angular_distance(joint_position_[j], + sim_joints_[j]->GetAngle(0).Radian()); + } joint_velocity_[j] = sim_joints_[j]->GetVelocity(0); joint_effort_[j] = sim_joints_[j]->GetForce((unsigned int)(0)); } @@ -234,25 +265,54 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim for(unsigned int j=0; j < n_dof_; j++) { + double effort; + switch (joint_hw_interface_types_[j]) { case EFFORT: - // Gazebo has an interesting API... - sim_joints_[j]->SetForce(0,joint_effort_command_[j]); - - // Output debug every nth msg - if( !(j % 10) && false) - { - ROS_DEBUG_STREAM_NAMED("robot_hw_sim","SetForce " << joint_effort_command_[j]); - } + effort = joint_effort_command_[j]; break; + case POSITION: - sim_joints_[j]->SetAngle(0,joint_position_command_[j]); + { + double error; + switch (joint_types_[j]) + { + case urdf::Joint::REVOLUTE: + angles::shortest_angular_distance_with_limits(joint_position_[j], + joint_position_command_[j], + joint_lower_limits_[j], + joint_upper_limits_[j], + error); + break; + case urdf::Joint::CONTINUOUS: + error = angles::shortest_angular_distance(joint_position_[j], + joint_position_command_[j]); + break; + default: + error = joint_position_command_[j] - joint_position_[j]; + } + + const double effort_limit = joint_effort_limits_[j]; + effort = clamp(pid_controllers_[j].computeCommand(error, period), + -effort_limit, effort_limit); + } break; + case VELOCITY: - sim_joints_[j]->SetVelocity(0,joint_velocity_command_[j]); + const double error = joint_velocity_command_[j] - joint_velocity_[j]; + const double effort_limit = joint_effort_limits_[j]; + effort = clamp(pid_controllers_[j].computeCommand(error, period), + -effort_limit, effort_limit); break; } + + sim_joints_[j]->SetForce(0, effort); + // Output debug every nth msg + if( !(j % 10) && false) + { + ROS_DEBUG_STREAM_NAMED("robot_hw_sim","SetForce " << effort); + } } } @@ -261,13 +321,20 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim // Register the limits of the joint specified by joint_name and joint_handle. The limits are // retrieved from joint_limit_nh. If urdf_model is not NULL, limits are retrieved from it also. - // Return the maximum effort that can be applied to the joint. - double registerJointLimits(const std::string& joint_name, + // Return the joint's type, lower position limit, upper position limit, and effort limit. + void registerJointLimits(const std::string& joint_name, const hardware_interface::JointHandle& joint_handle, const HWInterfaceType hw_iface_type, const ros::NodeHandle& joint_limit_nh, - const urdf::Model *const urdf_model) + const urdf::Model *const urdf_model, + int *const joint_type, double *const lower_limit, + double *const upper_limit, double *const effort_limit) { + *joint_type = urdf::Joint::UNKNOWN; + *lower_limit = -std::numeric_limits::max(); + *upper_limit = std::numeric_limits::max(); + *effort_limit = std::numeric_limits::max(); + joint_limits_interface::JointLimits limits; bool has_limits = false; joint_limits_interface::SoftJointLimits soft_limits; @@ -278,6 +345,7 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim const boost::shared_ptr urdf_joint = urdf_model->getJoint(joint_name); if (urdf_joint != NULL) { + *joint_type = urdf_joint->type; // Get limits from the URDF file. if (joint_limits_interface::getJointLimits(urdf_joint, limits)) has_limits = true; @@ -290,7 +358,32 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim has_limits = true; if (!has_limits) - return 0; + return; + + if (*joint_type == urdf::Joint::UNKNOWN) + { + // Infer the joint type. + + if (limits.has_position_limits) + { + *joint_type = urdf::Joint::REVOLUTE; + } + else + { + if (limits.angle_wraparound) + *joint_type = urdf::Joint::CONTINUOUS; + else + *joint_type = urdf::Joint::PRISMATIC; + } + } + + if (limits.has_position_limits) + { + *lower_limit = limits.min_position; + *upper_limit = limits.max_position; + } + if (limits.has_effort_limits) + *effort_limit = limits.max_effort; if (has_soft_limits) { @@ -346,10 +439,6 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim break; } } - - if (limits.has_effort_limits) - return limits.max_effort; - return 0; } unsigned int n_dof_; @@ -367,7 +456,12 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim joint_limits_interface::VelocityJointSoftLimitsInterface vj_limits_interface_; std::vector joint_names_; + std::vector joint_types_; + std::vector joint_lower_limits_; + std::vector joint_upper_limits_; + std::vector joint_effort_limits_; std::vector joint_hw_interface_types_; + std::vector pid_controllers_; std::vector joint_position_; std::vector joint_velocity_; std::vector joint_effort_; @@ -382,10 +476,6 @@ typedef boost::shared_ptr DefaultRobotHWSimPtr; } -// \todo PLUGINLIB_DECLARE_CLASS has been deprecated. Replace it with PLUGINLIB_EXPORT_CLASS. -PLUGINLIB_DECLARE_CLASS(gazebo_ros_control, - DefaultRobotHWSim, - gazebo_ros_control::DefaultRobotHWSim, - gazebo_ros_control::RobotHWSim) +PLUGINLIB_EXPORT_CLASS(gazebo_ros_control::DefaultRobotHWSim, gazebo_ros_control::RobotHWSim) #endif // #ifndef __GAZEBO_ROS_CONTROL_PLUGIN_DEFAULT_ROBOT_HW_SIM_H_ From ffa698008ce48c1ccdc0901917fb2641dbf065b5 Mon Sep 17 00:00:00 2001 From: Jim Rothrock Date: Wed, 4 Dec 2013 11:11:42 -0800 Subject: [PATCH 032/153] joint->SetAngle() and joint->SetVelocity() are now used to control position-controlled joints and velocity-controlled joints that do not have PID gain values stored on the Parameter Server. --- .../src/default_robot_hw_sim.cpp | 87 ++++++++++++------- 1 file changed, 55 insertions(+), 32 deletions(-) diff --git a/gazebo_ros_control/src/default_robot_hw_sim.cpp b/gazebo_ros_control/src/default_robot_hw_sim.cpp index 67e62b139..7faa275b7 100644 --- a/gazebo_ros_control/src/default_robot_hw_sim.cpp +++ b/gazebo_ros_control/src/default_robot_hw_sim.cpp @@ -101,7 +101,7 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim joint_lower_limits_.resize(n_dof_); joint_upper_limits_.resize(n_dof_); joint_effort_limits_.resize(n_dof_); - joint_hw_interface_types_.resize(n_dof_); + joint_control_methods_.resize(n_dof_); pid_controllers_.resize(n_dof_); joint_position_.resize(n_dof_); joint_velocity_.resize(n_dof_); @@ -167,7 +167,7 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim if(hardware_interface == "EffortJointInterface") { // Create effort joint interface - joint_hw_interface_types_[j] = EFFORT; + joint_control_methods_[j] = EFFORT; joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]), &joint_effort_command_[j]); ej_interface_.registerHandle(joint_handle); @@ -175,7 +175,7 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim else if(hardware_interface == "PositionJointInterface") { // Create position joint interface - joint_hw_interface_types_[j] = POSITION; + joint_control_methods_[j] = POSITION; joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]), &joint_position_command_[j]); pj_interface_.registerHandle(joint_handle); @@ -183,7 +183,7 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim else if(hardware_interface == "VelocityJointInterface") { // Create velocity joint interface - joint_hw_interface_types_[j] = VELOCITY; + joint_control_methods_[j] = VELOCITY; joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]), &joint_velocity_command_[j]); vj_interface_.registerHandle(joint_handle); @@ -207,21 +207,34 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim } sim_joints_.push_back(joint); - registerJointLimits(joint_names_[j], joint_handle, joint_hw_interface_types_[j], + registerJointLimits(joint_names_[j], joint_handle, joint_control_methods_[j], joint_limit_nh, urdf_model, &joint_types_[j], &joint_lower_limits_[j], &joint_upper_limits_[j], &joint_effort_limits_[j]); - if (joint_hw_interface_types_[j] != EFFORT) + if (joint_control_methods_[j] != EFFORT) { - // Initialize the PID controller. - + // Initialize the PID controller. If no PID gain values are found, use joint->SetAngle() or + // joint->SetVelocity() to control the joint. const ros::NodeHandle nh(model_nh, robot_namespace + "/gazebo_ros_control/pid_gains/" + joint_names_[j]); - if (!pid_controllers_[j].init(nh)) + if (pid_controllers_[j].init(nh)) { - ROS_ERROR_STREAM("No PID controller gain values were found for joint \"" << - joint_names_[j] << "\"."); - return false; + switch (joint_control_methods_[j]) + { + case POSITION: + joint_control_methods_[j] = POSITION_PID; + break; + case VELOCITY: + joint_control_methods_[j] = VELOCITY_PID; + break; + } + } + else + { + // joint->SetMaxForce() must be called if joint->SetAngle() or joint->SetVelocity() are + // going to be called. joint->SetMaxForce() must *not* be called if joint->SetForce() is + // going to be called. + joint->SetMaxForce(0, joint_effort_limits_[j]); } } } @@ -265,15 +278,25 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim for(unsigned int j=0; j < n_dof_; j++) { - double effort; - - switch (joint_hw_interface_types_[j]) + switch (joint_control_methods_[j]) { case EFFORT: - effort = joint_effort_command_[j]; + { + const double effort = joint_effort_command_[j]; + sim_joints_[j]->SetForce(0, effort); + // Output debug every nth msg + if( !(j % 10) && false) + { + ROS_DEBUG_STREAM_NAMED("robot_hw_sim","SetForce " << effort); + } + } break; case POSITION: + sim_joints_[j]->SetAngle(0, joint_position_command_[j]); + break; + + case POSITION_PID: { double error; switch (joint_types_[j]) @@ -294,37 +317,37 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim } const double effort_limit = joint_effort_limits_[j]; - effort = clamp(pid_controllers_[j].computeCommand(error, period), - -effort_limit, effort_limit); + const double effort = clamp(pid_controllers_[j].computeCommand(error, period), + -effort_limit, effort_limit); + sim_joints_[j]->SetForce(0, effort); } break; case VELOCITY: + sim_joints_[j]->SetVelocity(0, joint_velocity_command_[j]); + break; + + case VELOCITY_PID: const double error = joint_velocity_command_[j] - joint_velocity_[j]; const double effort_limit = joint_effort_limits_[j]; - effort = clamp(pid_controllers_[j].computeCommand(error, period), - -effort_limit, effort_limit); + const double effort = clamp(pid_controllers_[j].computeCommand(error, period), + -effort_limit, effort_limit); + sim_joints_[j]->SetForce(0, effort); break; } - - sim_joints_[j]->SetForce(0, effort); - // Output debug every nth msg - if( !(j % 10) && false) - { - ROS_DEBUG_STREAM_NAMED("robot_hw_sim","SetForce " << effort); - } } } private: - enum HWInterfaceType {EFFORT, POSITION, VELOCITY}; // Hardware interface types + // Methods used to control a joint. + enum ControlMethod {EFFORT, POSITION, POSITION_PID, VELOCITY, VELOCITY_PID}; // Register the limits of the joint specified by joint_name and joint_handle. The limits are // retrieved from joint_limit_nh. If urdf_model is not NULL, limits are retrieved from it also. // Return the joint's type, lower position limit, upper position limit, and effort limit. void registerJointLimits(const std::string& joint_name, const hardware_interface::JointHandle& joint_handle, - const HWInterfaceType hw_iface_type, + const ControlMethod ctrl_method, const ros::NodeHandle& joint_limit_nh, const urdf::Model *const urdf_model, int *const joint_type, double *const lower_limit, @@ -387,7 +410,7 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim if (has_soft_limits) { - switch (hw_iface_type) + switch (ctrl_method) { case EFFORT: { @@ -414,7 +437,7 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim } else { - switch (hw_iface_type) + switch (ctrl_method) { case EFFORT: { @@ -460,7 +483,7 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim std::vector joint_lower_limits_; std::vector joint_upper_limits_; std::vector joint_effort_limits_; - std::vector joint_hw_interface_types_; + std::vector joint_control_methods_; std::vector pid_controllers_; std::vector joint_position_; std::vector joint_velocity_; From 0a2180e08cc0321436863dc4fd3f4fc7cba5108d Mon Sep 17 00:00:00 2001 From: Ugo Cupcic Date: Wed, 18 Dec 2013 11:39:23 +0100 Subject: [PATCH 033/153] the parent sensor in gazebo seems not to be active --- gazebo_plugins/src/gazebo_ros_bumper.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/gazebo_plugins/src/gazebo_ros_bumper.cpp b/gazebo_plugins/src/gazebo_ros_bumper.cpp index 4d8ad1bdc..1e81974a9 100644 --- a/gazebo_plugins/src/gazebo_ros_bumper.cpp +++ b/gazebo_plugins/src/gazebo_ros_bumper.cpp @@ -121,6 +121,9 @@ void GazeboRosBumper::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) // simulation iteration. this->update_connection_ = this->parentSensor->ConnectUpdated( boost::bind(&GazeboRosBumper::OnContact, this)); + + // Make sure the parent sensor is active. + this->parentSensor->SetActive(true); } //////////////////////////////////////////////////////////////////////////////// @@ -205,7 +208,7 @@ void GazeboRosBumper::OnContact() // For each collision contact // Create a ContactState gazebo_msgs::ContactState state; - /// \TODO: + /// \TODO: gazebo::msgs::Contact contact = contacts.contact(i); state.collision1_name = contact.collision1(); From 746b78d8090afb7fe6e8071bf28668c7970fe71d Mon Sep 17 00:00:00 2001 From: Ugo Cupcic Date: Wed, 18 Dec 2013 11:48:56 +0100 Subject: [PATCH 034/153] readded the trailing whitespace for cleaner diff --- gazebo_plugins/src/gazebo_ros_bumper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gazebo_plugins/src/gazebo_ros_bumper.cpp b/gazebo_plugins/src/gazebo_ros_bumper.cpp index 1e81974a9..059f1d935 100644 --- a/gazebo_plugins/src/gazebo_ros_bumper.cpp +++ b/gazebo_plugins/src/gazebo_ros_bumper.cpp @@ -208,7 +208,7 @@ void GazeboRosBumper::OnContact() // For each collision contact // Create a ContactState gazebo_msgs::ContactState state; - /// \TODO: + /// \TODO: gazebo::msgs::Contact contact = contacts.contact(i); state.collision1_name = contact.collision1(); From c76085e61b0c1f9dfcb6d5a22eb86592c33d1701 Mon Sep 17 00:00:00 2001 From: Paul Mathieu Date: Tue, 24 Dec 2013 15:04:19 +0100 Subject: [PATCH 035/153] gazebo_ros: remove assignment to self If this is needed for any twisted reason, it should be made clear anyway. Assuming this line is harmless and removing it because it generates cppcheck warnings. --- gazebo_ros/src/gazebo_ros_api_plugin.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/gazebo_ros/src/gazebo_ros_api_plugin.cpp b/gazebo_ros/src/gazebo_ros_api_plugin.cpp index 5ff3c3e48..56828b084 100644 --- a/gazebo_ros/src/gazebo_ros_api_plugin.cpp +++ b/gazebo_ros/src/gazebo_ros_api_plugin.cpp @@ -1491,7 +1491,6 @@ void GazeboRosApiPlugin::transformWrench( gazebo::math::Vector3 &target_force, g target_torque = target_to_reference.rot.RotateVector(reference_torque); // target force is the refence force rotated by the target->reference transform - target_force = target_force; target_torque = target_torque + target_to_reference.pos.Cross(target_force); } From a54481d8df2a01270d65ae74ad47cf88d76f5f48 Mon Sep 17 00:00:00 2001 From: Paul Mathieu Date: Tue, 24 Dec 2013 15:07:29 +0100 Subject: [PATCH 036/153] gazebo_ros: [less-than-minor] fix newlines --- gazebo_ros/src/gazebo_ros_api_plugin.cpp | 54 ++++++++++++------------ 1 file changed, 27 insertions(+), 27 deletions(-) diff --git a/gazebo_ros/src/gazebo_ros_api_plugin.cpp b/gazebo_ros/src/gazebo_ros_api_plugin.cpp index 56828b084..4fc33226d 100644 --- a/gazebo_ros/src/gazebo_ros_api_plugin.cpp +++ b/gazebo_ros/src/gazebo_ros_api_plugin.cpp @@ -113,9 +113,9 @@ void GazeboRosApiPlugin::Load(int argc, char** argv) ROS_ERROR("Something other than this gazebo_ros_api plugin started ros::init(...), command line arguments may not be parsed properly."); // check if the ros master is available - required - while(!ros::master::check()) + while(!ros::master::check()) { - ROS_WARN_STREAM_NAMED("api_plugin","No ROS master - start roscore to continue..."); + ROS_WARN_STREAM_NAMED("api_plugin","No ROS master - start roscore to continue..."); // wait 0.5 second usleep(500*1000); // can't use ROS Time here b/c node handle is not yet initialized @@ -643,20 +643,20 @@ bool GazeboRosApiPlugin::spawnSDFModel(gazebo_msgs::SpawnModel::Request &req, if (isSDF(model_xml)) { updateSDFAttributes(gazebo_model_xml, model_name, initial_xyz, initial_q); - + // Walk recursively through the entire SDF, locate plugin tags and // add robotNamespace as a child with the correct namespace - if (!this->robot_namespace_.empty()) + if (!this->robot_namespace_.empty()) { // Get root element for SDF TiXmlNode* model_tixml = gazebo_model_xml.FirstChild("sdf"); - model_tixml = (!model_tixml) ? + model_tixml = (!model_tixml) ? gazebo_model_xml.FirstChild("gazebo") : model_tixml; - if (model_tixml) + if (model_tixml) { walkChildAddRobotNamespace(model_tixml); - } - else + } + else { ROS_WARN("Unable to add robot namespace to xml"); } @@ -666,18 +666,18 @@ bool GazeboRosApiPlugin::spawnSDFModel(gazebo_msgs::SpawnModel::Request &req, { updateURDFModelPose(gazebo_model_xml, initial_xyz, initial_q); updateURDFName(gazebo_model_xml, model_name); - + // Walk recursively through the entire URDF, locate plugin tags and // add robotNamespace as a child with the correct namespace - if (!this->robot_namespace_.empty()) + if (!this->robot_namespace_.empty()) { // Get root element for URDF TiXmlNode* model_tixml = gazebo_model_xml.FirstChild("robot"); - if (model_tixml) + if (model_tixml) { walkChildAddRobotNamespace(model_tixml); - } - else + } + else { ROS_WARN("Unable to add robot namespace to xml"); } @@ -1868,7 +1868,7 @@ void GazeboRosApiPlugin::physicsReconfigureThread() physics_reconfigure_set_client_ = nh_->serviceClient("/gazebo/set_physics_properties"); physics_reconfigure_get_client_ = nh_->serviceClient("/gazebo/get_physics_properties"); - // Wait until the rest of this plugin is loaded and the services are being offered + // Wait until the rest of this plugin is loaded and the services are being offered physics_reconfigure_set_client_.waitForExistence(); physics_reconfigure_get_client_.waitForExistence(); @@ -1897,7 +1897,7 @@ void GazeboRosApiPlugin::stripXmlDeclaration(std::string &model_xml) void GazeboRosApiPlugin::updateSDFAttributes(TiXmlDocument &gazebo_model_xml, std::string model_name, gazebo::math::Vector3 initial_xyz, gazebo::math::Quaternion initial_q) { - // This function can handle both regular SDF files and SDFs that are used with the + // This function can handle both regular SDF files and SDFs that are used with the // Gazebo Model Database TiXmlElement* pose_element; // This is used by both reguar and database SDFs @@ -1928,30 +1928,30 @@ void GazeboRosApiPlugin::updateSDFAttributes(TiXmlDocument &gazebo_model_xml, st // Check SDF for world element TiXmlElement* world_tixml = gazebo_tixml->FirstChildElement("world"); if (!world_tixml) - { + { ROS_WARN("Could not find or element in sdf, so name and initial position cannot be applied"); return; } // If not element, check SDF for required include element - model_tixml = world_tixml->FirstChildElement("include"); + model_tixml = world_tixml->FirstChildElement("include"); if (!model_tixml) - { + { ROS_WARN("Could not find element in sdf, so name and initial position cannot be applied"); return; } // Check for name element - TiXmlElement* name_tixml = model_tixml->FirstChildElement("name"); + TiXmlElement* name_tixml = model_tixml->FirstChildElement("name"); if (!name_tixml) - { + { // Create the name element name_tixml = new TiXmlElement("name"); model_tixml->LinkEndChild(name_tixml); - } + } // Set the text within the name element TiXmlText* text = new TiXmlText(model_name); - name_tixml->LinkEndChild( text ); + name_tixml->LinkEndChild( text ); } @@ -1980,7 +1980,7 @@ void GazeboRosApiPlugin::updateSDFAttributes(TiXmlDocument &gazebo_model_xml, st << model_rpy.x << " " << model_rpy.y << " " << model_rpy.z; // Add value to pose element - TiXmlText* text = new TiXmlText(pose_stream.str()); + TiXmlText* text = new TiXmlText(pose_stream.str()); TiXmlElement* new_pose_element = new TiXmlElement("pose"); new_pose_element->LinkEndChild(text); model_tixml->LinkEndChild(new_pose_element); @@ -2143,7 +2143,7 @@ void GazeboRosApiPlugin::walkChildAddRobotNamespace(TiXmlNode* robot_xml) } } -bool GazeboRosApiPlugin::spawnAndConform(TiXmlDocument &gazebo_model_xml, std::string model_name, +bool GazeboRosApiPlugin::spawnAndConform(TiXmlDocument &gazebo_model_xml, std::string model_name, gazebo_msgs::SpawnModel::Response &res) { // push to factory iface @@ -2156,7 +2156,7 @@ bool GazeboRosApiPlugin::spawnAndConform(TiXmlDocument &gazebo_model_xml, std::s gazebo::msgs::Factory msg; gazebo::msgs::Init(msg, "spawn_model"); msg.set_sdf( gazebo_model_xml_string ); - + //ROS_ERROR("attempting to spawn model name [%s] [%s]", model_name.c_str(),gazebo_model_xml_string.c_str()); // FIXME: should use entity_info or add lock to World::receiveMutex @@ -2196,11 +2196,11 @@ bool GazeboRosApiPlugin::spawnAndConform(TiXmlDocument &gazebo_model_xml, std::s { //boost::recursive_mutex::scoped_lock lock(*world->GetMRMutex()); - if (world_->GetModel(model_name)) + if (world_->GetModel(model_name)) break; } - ROS_DEBUG_STREAM_ONCE_NAMED("api_plugin","Waiting for " << timeout - ros::Time::now() + ROS_DEBUG_STREAM_ONCE_NAMED("api_plugin","Waiting for " << timeout - ros::Time::now() << " for model " << model_name << " to spawn"); usleep(2000); From 6d07aec4f89d8d9a758e17c5275cc5538c18e223 Mon Sep 17 00:00:00 2001 From: Markus Bader Date: Tue, 14 Jan 2014 14:47:51 +0100 Subject: [PATCH 037/153] joint velocity fixed using radius instead of diameter --- gazebo_plugins/src/gazebo_ros_diff_drive.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp index 798f44425..2ab56dbb8 100644 --- a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp +++ b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp @@ -306,8 +306,8 @@ void GazeboRosDiffDrive::publishWheelTF() { // Update robot in case new velocities have been requested getWheelVelocities(); - joints_[LEFT]->SetVelocity(0, wheel_speed_[LEFT] / wheel_diameter_); - joints_[RIGHT]->SetVelocity(0, wheel_speed_[RIGHT] / wheel_diameter_); + joints_[LEFT]->SetVelocity(0, wheel_speed_[LEFT] / (wheel_diameter_ / 2.0)); + joints_[RIGHT]->SetVelocity(0, wheel_speed_[RIGHT] / (wheel_diameter_ / 2.0)); last_update_time_+= common::Time(update_period_); From 847d7f564cad6646b6350fe622d6f0184f226889 Mon Sep 17 00:00:00 2001 From: Jim Rothrock Date: Thu, 16 Jan 2014 11:10:08 -0800 Subject: [PATCH 038/153] Removed some debugging code. --- gazebo_ros_control/src/default_robot_hw_sim.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/gazebo_ros_control/src/default_robot_hw_sim.cpp b/gazebo_ros_control/src/default_robot_hw_sim.cpp index 7faa275b7..11882922a 100644 --- a/gazebo_ros_control/src/default_robot_hw_sim.cpp +++ b/gazebo_ros_control/src/default_robot_hw_sim.cpp @@ -284,11 +284,6 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim { const double effort = joint_effort_command_[j]; sim_joints_[j]->SetForce(0, effort); - // Output debug every nth msg - if( !(j % 10) && false) - { - ROS_DEBUG_STREAM_NAMED("robot_hw_sim","SetForce " << effort); - } } break; From af286014dbf45bb899b32fce3cf4dc5e4284b443 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 12 Feb 2014 13:37:11 -0800 Subject: [PATCH 039/153] Use function for accessing scene node in gazebo_ros_video --- gazebo_plugins/src/gazebo_ros_video.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gazebo_plugins/src/gazebo_ros_video.cpp b/gazebo_plugins/src/gazebo_ros_video.cpp index 43de193a6..75d534a8f 100644 --- a/gazebo_plugins/src/gazebo_ros_video.cpp +++ b/gazebo_plugins/src/gazebo_ros_video.cpp @@ -74,7 +74,7 @@ namespace gazebo mo.convertToMesh(name + "__VideoMesh__"); Ogre::MovableObject *obj = (Ogre::MovableObject*) - this->sceneNode->getCreator()->createEntity( + this->GetSceneNode()->getCreator()->createEntity( name + "__VideoEntity__", name + "__VideoMesh__"); obj->setCastShadows(false); From 9e7ba6906a5863c8240d1048918ddb8bf1d9c547 Mon Sep 17 00:00:00 2001 From: Jordi Pages Date: Mon, 3 Mar 2014 12:36:08 +0100 Subject: [PATCH 040/153] multicamera bad namespace. Fixes #161 There was a race condition between GazeboRosCameraUtils::LoadThread creating the ros::NodeHandle and GazeboRosCameraUtils::Load suffixing the camera name in the namespace --- .../gazebo_plugins/gazebo_ros_camera_utils.h | 5 ++++- gazebo_plugins/src/gazebo_ros_camera_utils.cpp | 18 +++++++++++------- 2 files changed, 15 insertions(+), 8 deletions(-) diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera_utils.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera_utils.h index d74cb992e..f2413f886 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera_utils.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera_utils.h @@ -62,7 +62,10 @@ namespace gazebo /// \brief Load the plugin. /// \param[in] _parent Take in SDF root element. /// \param[in] _sdf SDF values. - public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf); + /// \param[in] _camera_name_suffix required before calling LoadThread + public: void Load(sensors::SensorPtr _parent, + sdf::ElementPtr _sdf, + const std::string &_camera_name_suffix = ""); /// \brief Load the plugin. /// \param[in] _parent Take in SDF root element. diff --git a/gazebo_plugins/src/gazebo_ros_camera_utils.cpp b/gazebo_plugins/src/gazebo_ros_camera_utils.cpp index 70edb4362..fe6db7e38 100644 --- a/gazebo_plugins/src/gazebo_ros_camera_utils.cpp +++ b/gazebo_plugins/src/gazebo_ros_camera_utils.cpp @@ -85,22 +85,22 @@ void GazeboRosCameraUtils::Load(sensors::SensorPtr _parent, const std::string &_camera_name_suffix, double _hack_baseline) { - // default Load - this->Load(_parent, _sdf); + // default Load: + // provide _camera_name_suffix to prevent LoadThread() creating the ros::NodeHandle with + //an incomplete this->camera_name_ namespace. There was a race condition when the _camera_name_suffix + //was appended in this function. + this->Load(_parent, _sdf, _camera_name_suffix); // overwrite hack baseline if specified at load // example usage in gazebo_ros_multicamera this->hack_baseline_ = _hack_baseline; - - // overwrite camera suffix - // example usage in gazebo_ros_multicamera - this->camera_name_ += _camera_name_suffix; } //////////////////////////////////////////////////////////////////////////////// // Load the controller void GazeboRosCameraUtils::Load(sensors::SensorPtr _parent, - sdf::ElementPtr _sdf) + sdf::ElementPtr _sdf, + const std::string &_camera_name_suffix) { // Get the world name. std::string world_name = _parent->GetWorldName(); @@ -133,6 +133,10 @@ void GazeboRosCameraUtils::Load(sensors::SensorPtr _parent, else this->camera_name_ = this->sdf->Get("cameraName"); + // overwrite camera suffix + // example usage in gazebo_ros_multicamera + this->camera_name_ += _camera_name_suffix; + if (!this->sdf->HasElement("frameName")) ROS_DEBUG("Camera plugin missing , defaults to /world"); else From bcd65be653be12b16fab46e14e36ad9ac8a5e5e1 Mon Sep 17 00:00:00 2001 From: Toni Oliver Date: Thu, 13 Mar 2014 18:53:02 +0000 Subject: [PATCH 041/153] Initialize depth_image_connect_count_ in openni_kinect plugin --- gazebo_plugins/src/gazebo_ros_openni_kinect.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp b/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp index fbe3b2513..42faf3c31 100644 --- a/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp +++ b/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp @@ -48,6 +48,7 @@ GazeboRosOpenniKinect::GazeboRosOpenniKinect() { this->point_cloud_connect_count_ = 0; this->depth_info_connect_count_ = 0; + this->depth_image_connect_count_ = 0; this->last_depth_image_camera_info_update_time_ = common::Time(0); } From 3d6e57f9117e7238b835ecbf8a8f86cbebf0a4d9 Mon Sep 17 00:00:00 2001 From: Markus Bader Date: Thu, 20 Mar 2014 15:57:11 +0100 Subject: [PATCH 042/153] the ros_laser checkes now for the model name and adds it als prefix --- .../include/gazebo_plugins/gazebo_ros_laser.h | 3 + gazebo_plugins/src/gazebo_ros_diff_drive.cpp | 295 ++++++++++-------- gazebo_plugins/src/gazebo_ros_laser.cpp | 47 ++- 3 files changed, 202 insertions(+), 143 deletions(-) diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_laser.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_laser.h index d89cf93e4..bbec66a3c 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_laser.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_laser.h @@ -74,6 +74,9 @@ namespace gazebo /// \brief frame transform name, should match link name private: std::string frame_name_; + + /// \brief tf prefix + private: std::string tf_prefix_; /// \brief for setting ROS name space private: std::string robot_namespace_; diff --git a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp index 2ab56dbb8..25bd2bde2 100644 --- a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp +++ b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp @@ -29,7 +29,7 @@ /* * \file gazebo_ros_diff_drive.cpp * - * \brief A differential drive plugin for gazebo. Based on the diffdrive plugin + * \brief A differential drive plugin for gazebo. Based on the diffdrive plugin * developed for the erratic robot (see copyright notice above). The original * plugin can be found in the ROS package gazebo_erratic_plugins. * @@ -57,134 +57,162 @@ namespace gazebo { - enum { +enum { RIGHT, LEFT, - }; +}; - GazeboRosDiffDrive::GazeboRosDiffDrive() {} +GazeboRosDiffDrive::GazeboRosDiffDrive() {} - // Destructor - GazeboRosDiffDrive::~GazeboRosDiffDrive() { - } +// Destructor +GazeboRosDiffDrive::~GazeboRosDiffDrive() { +} - // Load the controller - void GazeboRosDiffDrive::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) { +// Load the controller +void GazeboRosDiffDrive::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) { this->parent = _parent; this->world = _parent->GetWorld(); - - this->robot_namespace_ = ""; - if (!_sdf->HasElement("robotNamespace")) { - ROS_INFO("GazeboRosDiffDrive Plugin missing , defaults to \"%s\"", - this->robot_namespace_.c_str()); + + this->robot_namespace_ = this->parent->GetName (); + if ( !_sdf->HasElement ( "robotNamespace" ) ) { + ROS_INFO ( "GazeboRosDiffDrive Plugin missing , defaults to \"%s\"", + this->robot_namespace_.c_str() ); } else { - this->robot_namespace_ = - _sdf->GetElement("robotNamespace")->Get() + "/"; + this->robot_namespace_ = _sdf->GetElement ( "robotNamespace" )->Get(); + if ( this->robot_namespace_.empty() ) this->robot_namespace_ = this->parent->GetName (); + } + if ( !robot_namespace_.empty() ) this->robot_namespace_ += "/"; + rosnode_ = boost::shared_ptr ( new ros::NodeHandle ( this->robot_namespace_ ) ); + + tf_prefix_ = tf::getPrefixParam(*rosnode_); + if(tf_prefix_.empty()) { + tf_prefix_ = robot_namespace_; + boost::trim_right_if(tf_prefix_,boost::is_any_of("/")); } + ROS_INFO("GazeboRosDiffDrive Plugin (ns = %s) , set to \"%s\"", + this->robot_namespace_.c_str(), this->tf_prefix_.c_str()); + this->left_joint_name_ = "left_joint"; if (!_sdf->HasElement("leftJoint")) { - ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to \"%s\"", - this->robot_namespace_.c_str(), this->left_joint_name_.c_str()); + ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to \"%s\"", + this->robot_namespace_.c_str(), this->left_joint_name_.c_str()); } else { - this->left_joint_name_ = _sdf->GetElement("leftJoint")->Get(); + this->left_joint_name_ = _sdf->GetElement("leftJoint")->Get(); } this->right_joint_name_ = "right_joint"; if (!_sdf->HasElement("rightJoint")) { - ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to \"%s\"", - this->robot_namespace_.c_str(), this->right_joint_name_.c_str()); + ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to \"%s\"", + this->robot_namespace_.c_str(), this->right_joint_name_.c_str()); } else { - this->right_joint_name_ = _sdf->GetElement("rightJoint")->Get(); + this->right_joint_name_ = _sdf->GetElement("rightJoint")->Get(); } this->wheel_separation_ = 0.34; if (!_sdf->HasElement("wheelSeparation")) { - ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to %f", - this->robot_namespace_.c_str(), this->wheel_separation_); + ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to %f", + this->robot_namespace_.c_str(), this->wheel_separation_); } else { - this->wheel_separation_ = - _sdf->GetElement("wheelSeparation")->Get(); + this->wheel_separation_ = + _sdf->GetElement("wheelSeparation")->Get(); } this->wheel_diameter_ = 0.15; if (!_sdf->HasElement("wheelDiameter")) { - ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to %f", - this->robot_namespace_.c_str(), this->wheel_diameter_); + ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to %f", + this->robot_namespace_.c_str(), this->wheel_diameter_); } else { - this->wheel_diameter_ = _sdf->GetElement("wheelDiameter")->Get(); + this->wheel_diameter_ = _sdf->GetElement("wheelDiameter")->Get(); } this->torque = 5.0; if (!_sdf->HasElement("torque")) { - ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to %f", - this->robot_namespace_.c_str(), this->torque); + ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to %f", + this->robot_namespace_.c_str(), this->torque); } else { - this->torque = _sdf->GetElement("torque")->Get(); + this->torque = _sdf->GetElement("torque")->Get(); } this->command_topic_ = "cmd_vel"; if (!_sdf->HasElement("commandTopic")) { - ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to \"%s\"", - this->robot_namespace_.c_str(), this->command_topic_.c_str()); + ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to \"%s\"", + this->robot_namespace_.c_str(), this->command_topic_.c_str()); } else { - this->command_topic_ = _sdf->GetElement("commandTopic")->Get(); + this->command_topic_ = _sdf->GetElement("commandTopic")->Get(); } this->odometry_topic_ = "odom"; if (!_sdf->HasElement("odometryTopic")) { - ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to \"%s\"", - this->robot_namespace_.c_str(), this->odometry_topic_.c_str()); + ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to \"%s\"", + this->robot_namespace_.c_str(), this->odometry_topic_.c_str()); } else { - this->odometry_topic_ = _sdf->GetElement("odometryTopic")->Get(); + this->odometry_topic_ = _sdf->GetElement("odometryTopic")->Get(); } this->odometry_frame_ = "odom"; if (!_sdf->HasElement("odometryFrame")) { - ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to \"%s\"", - this->robot_namespace_.c_str(), this->odometry_frame_.c_str()); + ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to \"%s\"", + this->robot_namespace_.c_str(), this->odometry_frame_.c_str()); } else { - this->odometry_frame_ = _sdf->GetElement("odometryFrame")->Get(); + this->odometry_frame_ = _sdf->GetElement("odometryFrame")->Get(); } this->robot_base_frame_ = "base_footprint"; if (!_sdf->HasElement("robotBaseFrame")) { - ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to \"%s\"", - this->robot_namespace_.c_str(), this->robot_base_frame_.c_str()); + ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to \"%s\"", + this->robot_namespace_.c_str(), this->robot_base_frame_.c_str()); } else { - this->robot_base_frame_ = _sdf->GetElement("robotBaseFrame")->Get(); + this->robot_base_frame_ = _sdf->GetElement("robotBaseFrame")->Get(); } this->update_rate_ = 100.0; if (!_sdf->HasElement("updateRate")) { - ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to %f", - this->robot_namespace_.c_str(), this->update_rate_); + ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to %f", + this->robot_namespace_.c_str(), this->update_rate_); } else { - this->update_rate_ = _sdf->GetElement("updateRate")->Get(); + this->update_rate_ = _sdf->GetElement("updateRate")->Get(); } - + this->publishWheelTF_ = false; if (_sdf->HasElement("publishWheelTF")) { - this->publishWheelTF_ = _sdf->GetElement("publishWheelTF")->Get(); + + std::string value = _sdf->GetElement("publishWheelTF")->Get(); + if(boost::iequals(value, std::string("true"))) { + this->publishWheelTF_ = true; + } else if(boost::iequals(value, std::string("false"))) { + this->publishWheelTF_ = false; + } else { + ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) was set to '%s', it must be set to 'true' or 'false'!", + this->robot_namespace_.c_str(), value.c_str()); + } } - ROS_INFO("GazeboRosDiffDrive Plugin (ns = %s) , set to %i=%s", - this->robot_namespace_.c_str(), this->publishWheelTF_, (this->publishWheelTF_?"ture":"false")); - + ROS_INFO("GazeboRosDiffDrive Plugin (ns = %s) , set to %s", + this->robot_namespace_.c_str(), (this->publishWheelTF_?"ture":"false")); + this->publishWheelJointState_ = false; if (_sdf->HasElement("publishWheelJointState")) { - this->publishWheelJointState_ = _sdf->GetElement("publishWheelJointState")->Get(); + std::string value = _sdf->GetElement("publishWheelJointState")->Get(); + if(boost::iequals(value, std::string("true"))) { + this->publishWheelJointState_ = true; + } else if(boost::iequals(value, std::string("false"))) { + this->publishWheelJointState_ = false; + } else { + ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) was set to '%s', it must be set to 'true' or 'false'!", + this->robot_namespace_.c_str(), value.c_str()); + } } - ROS_INFO("GazeboRosDiffDrive Plugin (ns = %s) , set to %i=%s", - this->robot_namespace_.c_str(), this->publishWheelJointState_, (this->publishWheelJointState_?"ture":"false")); - + ROS_INFO("GazeboRosDiffDrive Plugin (ns = %s) , set to %s", + this->robot_namespace_.c_str(), (this->publishWheelJointState_?"ture":"false")); + // Initialize update rate stuff if (this->update_rate_ > 0.0) { - this->update_period_ = 1.0 / this->update_rate_; + this->update_period_ = 1.0 / this->update_rate_; } else { - this->update_period_ = 0.0; + this->update_period_ = 0.0; } last_update_time_ = this->world->GetSimTime(); @@ -200,19 +228,19 @@ namespace gazebo { joints_[LEFT] = this->parent->GetJoint(left_joint_name_); joints_[RIGHT] = this->parent->GetJoint(right_joint_name_); - if (!joints_[LEFT]) { - char error[200]; - snprintf(error, 200, - "GazeboRosDiffDrive Plugin (ns = %s) couldn't get left hinge joint named \"%s\"", - this->robot_namespace_.c_str(), this->left_joint_name_.c_str()); - gzthrow(error); + if (!joints_[LEFT]) { + char error[200]; + snprintf(error, 200, + "GazeboRosDiffDrive Plugin (ns = %s) couldn't get left hinge joint named \"%s\"", + this->robot_namespace_.c_str(), this->left_joint_name_.c_str()); + gzthrow(error); } - if (!joints_[RIGHT]) { - char error[200]; - snprintf(error, 200, - "GazeboRosDiffDrive Plugin (ns = %s) couldn't get right hinge joint named \"%s\"", - this->robot_namespace_.c_str(), this->right_joint_name_.c_str()); - gzthrow(error); + if (!joints_[RIGHT]) { + char error[200]; + snprintf(error, 200, + "GazeboRosDiffDrive Plugin (ns = %s) couldn't get right hinge joint named \"%s\"", + this->robot_namespace_.c_str(), this->right_joint_name_.c_str()); + gzthrow(error); } joints_[LEFT]->SetMaxForce(0, torque); @@ -221,42 +249,41 @@ namespace gazebo { // Make sure the ROS node for Gazebo has already been initialized if (!ros::isInitialized()) { - ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. " - << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); - return; + ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; } rosnode_ = boost::shared_ptr ( new ros::NodeHandle ( this->robot_namespace_ ) ); ROS_INFO("Starting GazeboRosDiffDrive Plugin (ns = %s)!", this->robot_namespace_.c_str()); - - if(this->publishWheelJointState_){ - joint_state_publisher_ = rosnode_->advertise ( "joint_states",1000 ); + + if(this->publishWheelJointState_) { + joint_state_publisher_ = rosnode_->advertise ( "joint_states",1000 ); } - - tf_prefix_ = tf::getPrefixParam(*rosnode_); + transform_broadcaster_ = boost::shared_ptr ( new tf::TransformBroadcaster() ); // ROS: Subscribe to the velocity command topic (usually "cmd_vel") ros::SubscribeOptions so = - ros::SubscribeOptions::create(command_topic_, 1, - boost::bind(&GazeboRosDiffDrive::cmdVelCallback, this, _1), - ros::VoidPtr(), &queue_); + ros::SubscribeOptions::create(command_topic_, 1, + boost::bind(&GazeboRosDiffDrive::cmdVelCallback, this, _1), + ros::VoidPtr(), &queue_); cmd_vel_subscriber_ = rosnode_->subscribe(so); odometry_publisher_ = rosnode_->advertise(odometry_topic_, 1); // start custom queue for diff drive - this->callback_queue_thread_ = - boost::thread(boost::bind(&GazeboRosDiffDrive::QueueThread, this)); + this->callback_queue_thread_ = + boost::thread(boost::bind(&GazeboRosDiffDrive::QueueThread, this)); // listen to the update event (broadcast every simulation iteration) - this->update_connection_ = - event::Events::ConnectWorldUpdateBegin( - boost::bind(&GazeboRosDiffDrive::UpdateChild, this)); + this->update_connection_ = + event::Events::ConnectWorldUpdateBegin( + boost::bind(&GazeboRosDiffDrive::UpdateChild, this)); + +} - } - void GazeboRosDiffDrive::publishWheelJointState() { ros::Time current_time = ros::Time::now(); @@ -275,55 +302,55 @@ void GazeboRosDiffDrive::publishWheelJointState() { void GazeboRosDiffDrive::publishWheelTF() { ros::Time current_time = ros::Time::now(); - for(int i = 0; i < 2; i++){ - std::string wheel_frame = - tf::resolve(tf_prefix_, joints_[i]->GetChild()->GetName ()); - std::string wheel_parent_frame = - tf::resolve(tf_prefix_, joints_[i]->GetParent()->GetName ()); + for(int i = 0; i < 2; i++) { + std::string wheel_frame = + tf::resolve(tf_prefix_, joints_[i]->GetChild()->GetName ()); + std::string wheel_parent_frame = + tf::resolve(tf_prefix_, joints_[i]->GetParent()->GetName ()); - math::Pose poseWheel = joints_[i]->GetChild()->GetRelativePose(); + math::Pose poseWheel = joints_[i]->GetChild()->GetRelativePose(); - tf::Quaternion qt(poseWheel.rot.x, poseWheel.rot.y, poseWheel.rot.z, poseWheel.rot.w); - tf::Vector3 vt(poseWheel.pos.x, poseWheel.pos.y, poseWheel.pos.z); + tf::Quaternion qt(poseWheel.rot.x, poseWheel.rot.y, poseWheel.rot.z, poseWheel.rot.w); + tf::Vector3 vt(poseWheel.pos.x, poseWheel.pos.y, poseWheel.pos.z); - tf::Transform tfWheel(qt, vt); - transform_broadcaster_->sendTransform( - tf::StampedTransform(tfWheel, current_time, - wheel_parent_frame, wheel_frame)); + tf::Transform tfWheel(qt, vt); + transform_broadcaster_->sendTransform( + tf::StampedTransform(tfWheel, current_time, + wheel_parent_frame, wheel_frame)); } - } +} - // Update the controller - void GazeboRosDiffDrive::UpdateChild() { +// Update the controller +void GazeboRosDiffDrive::UpdateChild() { common::Time current_time = this->world->GetSimTime(); - double seconds_since_last_update = - (current_time - last_update_time_).Double(); + double seconds_since_last_update = + (current_time - last_update_time_).Double(); if (seconds_since_last_update > update_period_) { - publishOdometry(seconds_since_last_update); - if(publishWheelTF_) publishWheelTF(); - if(publishWheelJointState_) publishWheelJointState(); + publishOdometry(seconds_since_last_update); + if(publishWheelTF_) publishWheelTF(); + if(publishWheelJointState_) publishWheelJointState(); - // Update robot in case new velocities have been requested - getWheelVelocities(); - joints_[LEFT]->SetVelocity(0, wheel_speed_[LEFT] / (wheel_diameter_ / 2.0)); - joints_[RIGHT]->SetVelocity(0, wheel_speed_[RIGHT] / (wheel_diameter_ / 2.0)); + // Update robot in case new velocities have been requested + getWheelVelocities(); + joints_[LEFT]->SetVelocity(0, wheel_speed_[LEFT] / (wheel_diameter_ / 2.0)); + joints_[RIGHT]->SetVelocity(0, wheel_speed_[RIGHT] / (wheel_diameter_ / 2.0)); - last_update_time_+= common::Time(update_period_); + last_update_time_+= common::Time(update_period_); } - } +} - // Finalize the controller - void GazeboRosDiffDrive::FiniChild() { +// Finalize the controller +void GazeboRosDiffDrive::FiniChild() { alive_ = false; queue_.clear(); queue_.disable(); rosnode_->shutdown(); callback_queue_thread_.join(); - } +} - void GazeboRosDiffDrive::getWheelVelocities() { +void GazeboRosDiffDrive::getWheelVelocities() { boost::mutex::scoped_lock scoped_lock(lock); double vr = x_; @@ -331,30 +358,30 @@ void GazeboRosDiffDrive::publishWheelTF() { wheel_speed_[LEFT] = vr + va * wheel_separation_ / 2.0; wheel_speed_[RIGHT] = vr - va * wheel_separation_ / 2.0; - } +} - void GazeboRosDiffDrive::cmdVelCallback( - const geometry_msgs::Twist::ConstPtr& cmd_msg) { +void GazeboRosDiffDrive::cmdVelCallback( + const geometry_msgs::Twist::ConstPtr& cmd_msg) { boost::mutex::scoped_lock scoped_lock(lock); x_ = cmd_msg->linear.x; rot_ = cmd_msg->angular.z; - } +} - void GazeboRosDiffDrive::QueueThread() { +void GazeboRosDiffDrive::QueueThread() { static const double timeout = 0.01; while (alive_ && rosnode_->ok()) { - queue_.callAvailable(ros::WallDuration(timeout)); + queue_.callAvailable(ros::WallDuration(timeout)); } - } +} - void GazeboRosDiffDrive::publishOdometry(double step_time) { +void GazeboRosDiffDrive::publishOdometry(double step_time) { ros::Time current_time = ros::Time::now(); - std::string odom_frame = - tf::resolve(tf_prefix_, odometry_frame_); - std::string base_footprint_frame = - tf::resolve(tf_prefix_, robot_base_frame_); + std::string odom_frame = + tf::resolve(tf_prefix_, odometry_frame_); + std::string base_footprint_frame = + tf::resolve(tf_prefix_, robot_base_frame_); // getting data for base_footprint to odom transform math::Pose pose = this->parent->GetWorldPose(); @@ -364,8 +391,8 @@ void GazeboRosDiffDrive::publishWheelTF() { tf::Transform base_footprint_to_odom(qt, vt); transform_broadcaster_->sendTransform( - tf::StampedTransform(base_footprint_to_odom, current_time, - odom_frame, base_footprint_frame)); + tf::StampedTransform(base_footprint_to_odom, current_time, + odom_frame, base_footprint_frame)); // publish odom topic odom_.pose.pose.position.x = pose.pos.x; @@ -397,7 +424,7 @@ void GazeboRosDiffDrive::publishWheelTF() { odom_.child_frame_id = base_footprint_frame; odometry_publisher_.publish(odom_); - } +} - GZ_REGISTER_MODEL_PLUGIN(GazeboRosDiffDrive) +GZ_REGISTER_MODEL_PLUGIN(GazeboRosDiffDrive) } diff --git a/gazebo_plugins/src/gazebo_ros_laser.cpp b/gazebo_plugins/src/gazebo_ros_laser.cpp index 1ccbc2c47..bd0888e95 100644 --- a/gazebo_plugins/src/gazebo_ros_laser.cpp +++ b/gazebo_plugins/src/gazebo_ros_laser.cpp @@ -35,6 +35,7 @@ #include #include +#include #include @@ -70,16 +71,37 @@ void GazeboRosLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) // save pointers this->sdf = _sdf; + // accessing model name like suggested by nkoenig at http://answers.gazebosim.org/question/4878/multiple-robots-with-ros-plugins-sensor-plugin-vs/ + std::vector values; + std::string scopedName = _parent->GetScopedName(); + boost::replace_all(scopedName, "::", ","); + boost::split(values, scopedName, boost::is_any_of(",")); + std::string modelName; + if(values.size() < 2){ + modelName = ""; + } else { + modelName = values[1]; + } + + this->parent_ray_sensor_ = boost::dynamic_pointer_cast(_parent); if (!this->parent_ray_sensor_) gzthrow("GazeboRosLaser controller requires a Ray Sensor as its parent"); - this->robot_namespace_ = ""; - if (this->sdf->HasElement("robotNamespace")) - this->robot_namespace_ = this->sdf->Get("robotNamespace") + "/"; + this->robot_namespace_ = modelName; + if (this->sdf->HasElement("robotNamespace")){ + std::string tmp = this->sdf->Get("robotNamespace"); + if(!tmp.empty()){ + this->robot_namespace_ = tmp; + } + } + this->rosnode_ = new ros::NodeHandle(this->robot_namespace_); + + ROS_INFO ( "Laser Plugin (robotNamespace = %s)" , this->robot_namespace_.c_str() ); + if (!this->sdf->HasElement("frameName")) { ROS_INFO("Laser plugin missing , defaults to /world"); @@ -87,7 +109,17 @@ void GazeboRosLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) } else this->frame_name_ = this->sdf->Get("frameName"); - + + + this->tf_prefix_ = tf::getPrefixParam(*this->rosnode_); + if(this->tf_prefix_.empty()) { + this->tf_prefix_ = this->robot_namespace_; + boost::trim_right_if(this->tf_prefix_,boost::is_any_of("/")); + } + ROS_INFO("Laser Plugin (ns = %s) , set to \"%s\"", + this->robot_namespace_.c_str(), this->tf_prefix_.c_str()); + + if (!this->sdf->HasElement("topicName")) { ROS_INFO("Laser plugin missing , defaults to /world"); @@ -106,7 +138,7 @@ void GazeboRosLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) return; } - ROS_INFO ( "Starting GazeboRosLaser Plugin (ns = %s)!", this->robot_namespace_.c_str() ); + ROS_INFO ( "Starting Laser Plugin (ns = %s)!", this->robot_namespace_.c_str() ); // ros callback queue for processing subscription this->deferred_load_thread_ = boost::thread( boost::bind(&GazeboRosLaser::LoadThread, this)); @@ -117,16 +149,13 @@ void GazeboRosLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) // Load the controller void GazeboRosLaser::LoadThread() { - this->rosnode_ = new ros::NodeHandle(this->robot_namespace_); this->gazebo_node_ = gazebo::transport::NodePtr(new gazebo::transport::Node()); this->gazebo_node_->Init(this->world_name_); this->pmq.startServiceThread(); // resolve tf prefix - std::string prefix; - this->rosnode_->getParam(std::string("tf_prefix"), prefix); - this->frame_name_ = tf::resolve(prefix, this->frame_name_); + this->frame_name_ = tf::resolve(this->tf_prefix_, this->frame_name_); if (this->topic_name_ != "") { From 611e7405b2381b7271f1f83c4c1f68504a0f223c Mon Sep 17 00:00:00 2001 From: Markus Bader Date: Thu, 20 Mar 2014 15:58:24 +0100 Subject: [PATCH 043/153] A test model for the ros_diff_drive ros_laser and joint_state_publisher added --- .../test/diff_drive/launch/diff_drive.launch | 28 ++ .../launch/pioneer3dx.gazebo.launch | 21 + .../test/diff_drive/launch/pioneer3dx.rviz | 427 ++++++++++++++++++ .../diff_drive/launch/pioneer3dx.urdf.launch | 12 + .../test/diff_drive/meshes/laser/hokuyo.dae | 253 +++++++++++ .../test/diff_drive/meshes/p3dx/Coordinates | 20 + .../test/diff_drive/meshes/p3dx/back_rim.stl | Bin 0 -> 1094884 bytes .../diff_drive/meshes/p3dx/back_sonar.stl | Bin 0 -> 12084 bytes .../diff_drive/meshes/p3dx/center_hubcap.stl | Bin 0 -> 1684 bytes .../diff_drive/meshes/p3dx/center_wheel.stl | Bin 0 -> 11284 bytes .../test/diff_drive/meshes/p3dx/chassis.stl | Bin 0 -> 11084 bytes .../test/diff_drive/meshes/p3dx/front_rim.stl | Bin 0 -> 1094884 bytes .../diff_drive/meshes/p3dx/front_sonar.stl | Bin 0 -> 12084 bytes .../diff_drive/meshes/p3dx/left_hubcap.stl | Bin 0 -> 7284 bytes .../diff_drive/meshes/p3dx/left_wheel.stl | Bin 0 -> 28884 bytes .../diff_drive/meshes/p3dx/right_hubcap.stl | Bin 0 -> 7284 bytes .../diff_drive/meshes/p3dx/right_wheel.stl | Bin 0 -> 28884 bytes .../test/diff_drive/meshes/p3dx/swivel.stl | Bin 0 -> 10284 bytes .../test/diff_drive/meshes/p3dx/top.stl | Bin 0 -> 30684 bytes .../test/diff_drive/xacro/laser/hokuyo.xacro | 68 +++ .../diff_drive/xacro/p3dx/pioneer3dx.xacro | 13 + .../xacro/p3dx/pioneer3dx_body.xacro | 373 +++++++++++++++ .../xacro/p3dx/pioneer3dx_drive.xacro | 35 ++ 23 files changed, 1250 insertions(+) create mode 100644 gazebo_plugins/test/diff_drive/launch/diff_drive.launch create mode 100644 gazebo_plugins/test/diff_drive/launch/pioneer3dx.gazebo.launch create mode 100644 gazebo_plugins/test/diff_drive/launch/pioneer3dx.rviz create mode 100644 gazebo_plugins/test/diff_drive/launch/pioneer3dx.urdf.launch create mode 100644 gazebo_plugins/test/diff_drive/meshes/laser/hokuyo.dae create mode 100644 gazebo_plugins/test/diff_drive/meshes/p3dx/Coordinates create mode 100644 gazebo_plugins/test/diff_drive/meshes/p3dx/back_rim.stl create mode 100644 gazebo_plugins/test/diff_drive/meshes/p3dx/back_sonar.stl create mode 100644 gazebo_plugins/test/diff_drive/meshes/p3dx/center_hubcap.stl create mode 100644 gazebo_plugins/test/diff_drive/meshes/p3dx/center_wheel.stl create mode 100644 gazebo_plugins/test/diff_drive/meshes/p3dx/chassis.stl create mode 100644 gazebo_plugins/test/diff_drive/meshes/p3dx/front_rim.stl create mode 100644 gazebo_plugins/test/diff_drive/meshes/p3dx/front_sonar.stl create mode 100644 gazebo_plugins/test/diff_drive/meshes/p3dx/left_hubcap.stl create mode 100644 gazebo_plugins/test/diff_drive/meshes/p3dx/left_wheel.stl create mode 100644 gazebo_plugins/test/diff_drive/meshes/p3dx/right_hubcap.stl create mode 100644 gazebo_plugins/test/diff_drive/meshes/p3dx/right_wheel.stl create mode 100644 gazebo_plugins/test/diff_drive/meshes/p3dx/swivel.stl create mode 100644 gazebo_plugins/test/diff_drive/meshes/p3dx/top.stl create mode 100644 gazebo_plugins/test/diff_drive/xacro/laser/hokuyo.xacro create mode 100644 gazebo_plugins/test/diff_drive/xacro/p3dx/pioneer3dx.xacro create mode 100644 gazebo_plugins/test/diff_drive/xacro/p3dx/pioneer3dx_body.xacro create mode 100644 gazebo_plugins/test/diff_drive/xacro/p3dx/pioneer3dx_drive.xacro diff --git a/gazebo_plugins/test/diff_drive/launch/diff_drive.launch b/gazebo_plugins/test/diff_drive/launch/diff_drive.launch new file mode 100644 index 000000000..abf99065d --- /dev/null +++ b/gazebo_plugins/test/diff_drive/launch/diff_drive.launch @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gazebo_plugins/test/diff_drive/launch/pioneer3dx.gazebo.launch b/gazebo_plugins/test/diff_drive/launch/pioneer3dx.gazebo.launch new file mode 100644 index 000000000..9424fe5d4 --- /dev/null +++ b/gazebo_plugins/test/diff_drive/launch/pioneer3dx.gazebo.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/gazebo_plugins/test/diff_drive/launch/pioneer3dx.rviz b/gazebo_plugins/test/diff_drive/launch/pioneer3dx.rviz new file mode 100644 index 000000000..0d72dde2a --- /dev/null +++ b/gazebo_plugins/test/diff_drive/launch/pioneer3dx.rviz @@ -0,0 +1,427 @@ +Panels: + - Class: rviz/Displays + Help Height: 84 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + - /TF1/Tree1 + - /LaserScan1 + - /LaserScan2 + Splitter Ratio: 0.528662 + Tree Height: 538 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: LaserScan +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + center_hubcap: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + center_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + chassis: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_laser: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_sonar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + swivel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top_plate: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: r1/robot_description + TF Prefix: r1 + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + base_link: + Value: true + r1/base_link: + Value: true + r1/center_hubcap: + Value: true + r1/center_wheel: + Value: true + r1/chassis: + Value: true + r1/front_laser: + Value: true + r1/front_sonar: + Value: true + r1/left_hub: + Value: true + r1/left_wheel: + Value: true + r1/odom: + Value: true + r1/right_hub: + Value: true + r1/right_wheel: + Value: true + r1/swivel: + Value: true + r1/top_plate: + Value: true + r2/base_link: + Value: true + r2/center_hubcap: + Value: true + r2/center_wheel: + Value: true + r2/chassis: + Value: true + r2/front_laser: + Value: true + r2/front_sonar: + Value: true + r2/left_hub: + Value: true + r2/left_wheel: + Value: true + r2/odom: + Value: true + r2/right_hub: + Value: true + r2/right_wheel: + Value: true + r2/swivel: + Value: true + r2/top_plate: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + base_link: + r1/odom: + r1/base_link: + r1/chassis: + r1/swivel: + r1/center_wheel: + r1/center_hubcap: + {} + r1/top_plate: + {} + r1/front_laser: + {} + r1/front_sonar: + {} + r1/left_hub: + r1/left_wheel: + {} + r1/right_hub: + r1/right_wheel: + {} + r2/odom: + r2/base_link: + r2/chassis: + r2/swivel: + r2/center_wheel: + r2/center_hubcap: + {} + r2/top_plate: + {} + r2/front_laser: + {} + r2/front_sonar: + {} + r2/left_hub: + r2/left_wheel: + {} + r2/right_hub: + r2/right_wheel: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + center_hubcap: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + center_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + chassis: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_laser: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_sonar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + swivel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top_plate: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: r2/robot_description + TF Prefix: r2 + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 85; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 999999 + Min Color: 0; 0; 0 + Min Intensity: -999999 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.03 + Style: Flat Squares + Topic: /r1/front_laser/scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 170; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 3.49902e-38 + Min Color: 0; 0; 0 + Min Intensity: -999999 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.03 + Style: Flat Squares + Topic: /r2/front_laser/scan + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 16.9585 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.0773502 + Y: -0.369527 + Z: -0.109698 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.845398 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.555372 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000013c000002affc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006a00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f000002af000000e900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002affc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f000002af000000b600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000029a00fffffffb0000000800540069006d0065010000000000000450000000000000000000000259000002af00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 2265 + Y: 97 diff --git a/gazebo_plugins/test/diff_drive/launch/pioneer3dx.urdf.launch b/gazebo_plugins/test/diff_drive/launch/pioneer3dx.urdf.launch new file mode 100644 index 000000000..1a2120564 --- /dev/null +++ b/gazebo_plugins/test/diff_drive/launch/pioneer3dx.urdf.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/gazebo_plugins/test/diff_drive/meshes/laser/hokuyo.dae b/gazebo_plugins/test/diff_drive/meshes/laser/hokuyo.dae new file mode 100644 index 000000000..bc65151bb --- /dev/null +++ b/gazebo_plugins/test/diff_drive/meshes/laser/hokuyo.dae @@ -0,0 +1,253 @@ + + + + + Blender User + Blender 2.64.0 r51232 + + 2013-03-22T08:19:53 + 2013-03-22T08:19:53 + + Z_UP + + + + + + + 49.13434 + 1.777778 + 0.1 + 100 + + + + + + + + + + 1 1 1 + 1 + 0 + 0.00111109 + + + + + 0.000999987 + 0 + 1 + 1 + 1 + 1 + 1 + 2 + 0 + 1 + 1 + 1 + 1 + 1 + 0 + 2880 + 2 + 30.002 + 1.000799 + 0.04999995 + 29.99998 + 1 + 2 + 0 + 0 + 1 + 1 + 1 + 1 + 8192 + 1 + 1 + 0 + 1 + 1 + 1 + 3 + 0 + 0 + 0 + 0 + 45 + 0 + 1 + 1 + 1 + 3 + 0.15 + 75 + 1 + 1 + 0 + 1 + 1 + 0 + + + + + + + + + + + + 0 0 0 1 + + + 0 0 0 1 + + + 0.020312 0.020312 0.020312 1 + + + 0.25 0.25 0.25 1 + + + 12 + + + 1 + + + + + + 1 + + + + 1 + + + + + + + 0 0 0 1 + + + 0 0 0 1 + + + 0.695112 0.695112 0.695112 1 + + + 0.0625 0.0625 0.0625 1 + + + 0 + + + 1 + + + + + + 1 + + + + 1 + + + + + + + + + + + + + + + -0.02213996 0.005949974 -0.004869997 -0.02213996 0.005949974 0.004869997 -0.02164 0.009949982 -0.004999995 -0.02164 0.009949982 -0.005999982 -0.02164 0.009949982 0.004999995 -0.02164 0.009949982 0.005999982 -0.02149999 0.009949982 -0.006499946 -0.02149999 0.009949982 0.006499946 -0.02113997 0.009949982 -0.006869971 -0.02113997 0.009949982 0.006869971 -0.02063995 0.005949974 0.006999969 -0.02063995 0.009949982 -0.006999969 -0.02063995 0.009949982 0.006999969 -0.01979994 0.009949982 -0.006999969 -0.01979994 0.005949974 -0.006999969 -0.01979994 0.005949974 0.006999969 -0.01979994 0.009949982 0.006999969 -0.01838999 0.005949974 0.01014 -0.02063995 0.005949974 -0.006999969 -0.02113997 0.005949974 0.006869971 -0.02113997 0.005949974 -0.006869971 -0.02149999 0.005949974 0.006499946 -0.02149999 0.005949974 -0.006499946 -0.02164 0.005949974 0.005999982 -0.02164 0.005949974 0.004999995 -0.02164 0.005949974 -0.004999995 -0.02164 0.005949974 -0.005999982 -0.01920998 0.01990997 0.004999995 -0.01920998 0.01990997 -0.004999995 -0.005279958 0.02695 -0.01798999 -0.009719967 0.02695 -0.01595997 -0.01014 0.02695 -0.01576995 -0.01365 0.02695 -0.01273 -0.01416999 0.02695 -0.01227998 -0.01655995 0.02695 -0.008569955 -0.01705998 0.02695 -0.007789969 -0.01822996 0.02695 -0.003779947 -0.01855999 0.02695 -0.00266999 -0.01855999 0.02695 0.00127995 -0.01855999 0.02695 0.00266999 -0.01750999 0.02695 0.006229996 -0.01705998 0.02695 0.007789969 -0.01516997 0.02695 0.01071995 -0.01416999 0.02695 0.01227998 -0.01170998 0.02695 0.01440995 -0.01014 0.02695 0.01576995 -0.007389962 0.02695 0.01703 -0.005279958 0.02695 0.01798999 -0.002529978 0.02695 0.01838999 0 0.02695 0.01874995 0.002529978 0.02695 0.01838999 0.005279958 0.02695 0.01798999 0.007389962 0.02695 0.01703 0.01014 0.02695 0.01576995 0.01170998 0.02695 0.01440995 0.01416999 0.02695 0.01227998 0.01516997 0.02695 0.01071995 0.01705998 0.02695 0.007789969 0.01750999 0.02695 0.006229996 0.01855999 0.02695 0.00266999 0.01855999 0.02695 0.00127995 0.01855999 0.02695 -0.00266999 0.01822996 0.02695 -0.003779947 0.01705998 0.02695 -0.007789969 0.01655995 0.02695 -0.008569955 0.01416999 0.02695 -0.01227998 0.01365 0.02695 -0.01273 0.01014 0.02695 -0.01576995 0.009719967 0.02695 -0.01595997 0.005279958 0.02695 -0.01798999 0.005049943 0.02695 -0.01801997 -0.005049943 0.02695 -0.01801997 0 0.02695 -0.01874995 -0.01885998 0.02594995 0.001289963 -0.01850998 0.02594995 -0.003849983 -0.02039998 0.009949982 -0.004999995 -0.02039998 0.009949982 0.004999995 -0.01848 0.02594995 0.003999948 -0.01836997 0.02572995 0.004629969 -0.01840996 0.02588999 0.004329979 -0.01838999 0.02579998 0.004489958 -0.01835 0.02544999 0.004859983 -0.01838999 0.02494996 0.004999995 -0.01835995 0.02517998 0.004969954 -0.02164 0.02494996 0.004999995 -0.02164 0.02544999 0.004869997 -0.02188998 0.02537995 0.004869997 -0.02206999 0.02519994 0.004869997 -0.02213996 0.02494996 0.004869997 -0.02164 0.02581995 0.004499971 -0.02206999 0.02569997 0.004499971 -0.02238994 0.02537995 0.004499971 -0.02249997 0.02494996 0.004499971 -0.02164 0.02594995 0.003999948 -0.02213996 0.02581995 0.003999948 -0.02263998 0.02494996 0.003999948 -0.02249997 0.02544999 0.003999948 -0.02213996 0.02581995 -0.003999948 -0.02249997 0.02544999 -0.003999948 -0.02263998 0.02494996 -0.003999948 -0.02206999 0.02569997 -0.004499971 -0.02188998 0.02537995 -0.004869997 -0.02238994 0.02537995 -0.004499971 -0.02249997 0.02494996 -0.004499971 -0.02206999 0.02519994 -0.004869997 -0.02213996 0.02494996 -0.004869997 -0.01838999 0.02494996 -0.004999995 -0.02164 0.02494996 -0.004999995 -0.01835 0.02555996 -0.004789948 -0.01835995 0.02526998 -0.004949986 -0.01835 0.02543997 -0.00484997 -0.01836997 0.02577996 -0.004559993 -0.02164 0.02544999 -0.004869997 -0.02164 0.02581995 -0.004499971 -0.01848 0.02594995 -0.003999948 -0.02164 0.02594995 -0.003999948 -0.01841998 0.02590996 -0.004269957 -0.02001994 0.01871997 -0.004999995 -0.01986998 0.01879996 -0.004999995 -0.02025997 0.01844996 -0.004999995 -0.02039998 0.01794999 -0.004999995 -0.02037996 0.01813995 -0.004999995 -0.02039998 0.01794999 0.004999995 -0.02033996 0.01826995 0.004999995 -0.01993995 0.01877999 0.004999995 -0.01936 0.01894998 0.004999995 -0.01962 0.01892 0.004999995 -0.01879996 0.01894998 0.006679952 -0.01845997 0.01894998 0.007699966 -0.01936 0.01894998 -0.004999995 -0.01971 0.01888996 -0.004999995 -0.01845997 0.01894998 -0.007699966 -0.01668 0.01894998 -0.01102995 -0.01141995 0.01894998 -0.01641994 -7.8e-4 0.01894998 -0.01997995 0 0.01894998 -0.01993995 0.002989947 0.01894998 -0.01976996 0.01315999 0.01894998 -0.01505994 0.01454997 0.01894998 -0.01358997 0.01576 0.01894998 -0.01230996 0.01990997 0.01894998 0.001369953 0.01990997 0.01894998 -0.001889944 0.01919996 0.01894998 0.005609989 0.01779997 0.01894998 0.009119987 0.00792998 0.01894998 0.01826995 0.01008999 0.01894998 0.01726996 -0.007959961 0.01894998 0.01832997 -0.004529953 0.01894998 0.01947999 -0.01668 0.01894998 0.01102995 -0.01892 0.01881998 0.007889986 -0.02024996 0.01843994 0.004999995 -0.02017998 0.01855999 0.004999995 -0.00812 0.01894998 -0.01827996 0.006659984 0.01894998 -0.01885998 0.01576 0.01894998 0.01230996 0.002989947 0.01894998 0.01976996 0.002719998 0.01894998 0.01978999 -0.01141995 0.01894998 0.01641994 -0.01256996 0.01894998 0.01545 -0.01767998 0.01894998 -0.009159982 -0.01892 0.01881998 -0.007889986 -0.01937997 0.01794999 -0.008079946 -0.01830995 0.01794999 -0.01009994 -0.01925998 0.01844996 -0.008029997 -0.01458996 0.01894998 -0.01362997 -0.01709997 0.01881998 -0.01130998 -0.01750999 0.01794999 -0.01159 -0.0151 0.01794999 -0.01457995 -0.01739996 0.01844996 -0.01150995 -0.01431 0.01894998 -0.01397997 -0.01466 0.01881998 -0.01432996 -0.01501995 0.01794999 -0.01467996 -0.01491999 0.01844996 -0.01457995 -0.01034998 0.01894998 -0.01701998 -0.008319973 0.01881998 -0.01872998 -0.01169997 0.01881998 -0.01682996 -0.01198995 0.01794999 -0.01723998 -0.01190996 0.01844996 -0.01712995 -8.2e-4 0.01794999 -0.02098 -0.004759967 0.01794999 -0.02044999 -0.004729986 0.01844996 -0.02031999 -0.005689978 0.01794999 -0.02013999 -0.01073998 0.01794999 -0.01794999 -0.008529961 0.01794999 -0.01918995 -0.008469998 0.01844996 -0.01906996 -2.5e-4 0.01794999 -0.02094995 -8.2e-4 0.01844996 -0.02085 -8.1e-4 0.01881998 -0.02047997 -0.005379974 0.01894998 -0.01919996 -0.004649996 0.01881998 -0.01996999 -0.004529953 0.01894998 -0.01947999 0.005379974 0.01894998 -0.01917999 0.006829977 0.01881998 -0.01932996 0.003069996 0.01881998 -0.02026998 0.003139972 0.01794999 -0.02075999 0.003119945 0.01844996 -0.02063 0.01381999 0.01794999 -0.01580995 0.01059997 0.01794999 -0.01813 0.01052999 0.01844996 -0.01800996 0.01034998 0.01794999 -0.01824998 0.005209982 0.01794999 -0.02024996 0.006989955 0.01794999 -0.01979994 0.006949961 0.01844996 -0.01966995 0.01469999 0.01794999 -0.01488 0.01372998 0.01844996 -0.01570999 0.01348996 0.01881998 -0.01542997 0.01008999 0.01894998 -0.01726996 0.01034998 0.01881998 -0.01769995 0.01036995 0.01894998 -0.01705998 0.01774996 0.01894998 -0.009199976 0.01823997 0.01881998 -0.009349942 0.01615995 0.01881998 -0.01261997 0.01655 0.01794999 -0.01291996 0.01644998 0.01844996 -0.01283997 0.02090996 0.01794999 -0.00198996 0.02023994 0.01794999 -0.005469977 0.02002996 0.01844996 -0.005849957 0.02015995 0.01794999 -0.005889952 0.01807999 0.01794999 -0.01052999 0.01868999 0.01794999 -0.009579956 0.01857 0.01844996 -0.009519994 0.02090996 0.01794999 0 0.02076995 0.01844996 -0.001969993 0.02041 0.01881998 -0.001939952 0.01919996 0.01894998 -0.005609989 0.01967996 0.01881998 -0.00575 0.01949995 0.01894998 -0.004049956 0.01990997 0.01894998 0.001889944 0.02041 0.01881998 0.001939952 0.02090996 0.01794999 0.00198996 0.02023994 0.01794999 0.005469977 0.02076995 0.01844996 0.001969993 0.01876997 0.01894998 0.006669998 0.01967996 0.01881998 0.00575 0.02015995 0.01794999 0.005889952 0.02002996 0.01844996 0.005849957 0.01627999 0.01894998 0.0115 0.01615995 0.01881998 0.01261997 0.01823997 0.01881998 0.009349942 0.01868999 0.01794999 0.009579956 0.01857 0.01844996 0.009519994 0.01059997 0.01794999 0.01813 0.01381999 0.01794999 0.01580995 0.01372998 0.01844996 0.01570999 0.01469999 0.01794999 0.01488 0.01807999 0.01794999 0.01052999 0.01655 0.01794999 0.01291996 0.01644998 0.01844996 0.01283997 0.01034998 0.01794999 0.01824998 0.01052999 0.01844996 0.01800996 0.01034998 0.01881998 0.01769995 0.01315999 0.01894998 0.01505994 0.01348996 0.01881998 0.01542997 0.01258999 0.01894998 0.01546996 0.006659984 0.01894998 0.01885998 0.003069996 0.01881998 0.02026998 0.006829977 0.01881998 0.01932996 0.006989955 0.01794999 0.01979994 0.006949961 0.01844996 0.01966995 -0.004759967 0.01794999 0.02044999 -8.2e-4 0.01794999 0.02098 -8.2e-4 0.01844996 0.02085 -2.5e-4 0.01794999 0.02094995 0.005209982 0.01794999 0.02024996 0.003139972 0.01794999 0.02075999 0.003119945 0.01844996 0.02063 -0.005689978 0.01794999 0.02013999 -0.004729986 0.01844996 0.02031999 -0.004649996 0.01881998 0.01996999 -7.8e-4 0.01894998 0.01997995 -8.1e-4 0.01881998 0.02047997 -0.002709984 0.01894998 0.01972997 -0.00812 0.01894998 0.01827996 -0.01169997 0.01881998 0.01682996 -0.008319973 0.01881998 0.01872998 -0.008529961 0.01794999 0.01918995 -0.008469998 0.01844996 0.01906996 -0.01750999 0.01794999 0.01159 -0.0151 0.01794999 0.01457995 -0.01491999 0.01844996 0.01457995 -0.01501995 0.01794999 0.01467996 -0.01073998 0.01794999 0.01794999 -0.01198995 0.01794999 0.01723998 -0.01190996 0.01844996 0.01712995 -0.01937997 0.01794999 0.008079946 -0.01830995 0.01794999 0.01009994 -0.01925998 0.01844996 0.008029997 -0.01739996 0.01844996 0.01150995 -0.01709997 0.01881998 0.01130998 -0.01431 0.01894998 0.01397997 -0.01466 0.01881998 0.01432996 -0.01629996 0.01894998 0.0115 -0.01014 0.03504997 -0.01576995 -0.005279958 0.03504997 0.01798999 0 0.03504997 0.01874995 0.005279958 0.03504997 0.01798999 0.01014 0.03504997 0.01576995 0.01416999 0.03504997 0.01227998 0.01705998 0.03504997 0.007789969 0.01855999 0.03504997 0.00266999 0.01855999 0.03504997 -0.00266999 -0.01014 0.03504997 0.01576995 -0.01416999 0.03504997 0.01227998 -0.01705998 0.03504997 0.007789969 -0.01855999 0.03504997 0.00266999 -0.01855999 0.03504997 -0.00266999 -0.01705998 0.03504997 -0.007789969 -0.01416999 0.03504997 -0.01227998 -0.005279958 0.03504997 -0.01798999 0 0.03504997 -0.01874995 0.005279958 0.03504997 -0.01798999 0.01014 0.03504997 -0.01576995 0.01416999 0.03504997 -0.01227998 0.01705998 0.03504997 -0.007789969 0.001609981 0.03494995 -0.01717996 -0.003309965 0.03504997 -0.01692998 -0.003309965 0.03494995 -0.01692998 0.001759946 0.03494995 -0.01713997 0.001969993 0.03494995 -0.01708996 0.001609981 0.03504997 -0.01717996 0.004629969 0.03494995 -0.01644998 0.006399989 0.03494995 -0.01601999 0.007199943 0.03494995 -0.01555997 0.007989943 0.03494995 -0.0151 0.006399989 0.03504997 -0.01601999 0.009659945 0.03494995 -0.01413995 0.01066994 0.03494995 -0.01354998 0.01066994 0.03504997 -0.01354998 0.01512998 0.03494995 -0.007909953 0.01104998 0.03494995 -0.01314997 0.01406997 0.03504997 -0.009979963 0.01406997 0.03494995 -0.009979963 0.01695996 0.03494995 -0.002209961 0.01631999 0.03494995 -0.005589962 0.01631999 0.03504997 -0.005589962 0.01652997 0.03494995 -0.004479944 -0.007969975 0.03494995 -0.01529997 -0.007969975 0.03504997 -0.01529997 -0.009269952 0.03494995 -0.01454997 -0.01002997 0.03494995 0.01388996 -0.01088994 0.03494995 0.01314997 -0.01299995 0.03494995 0.01133996 -0.01299995 0.03504997 0.01133996 -0.01378995 0.03494995 0.01010996 -0.01567995 0.03494995 0.007189989 -0.01567995 0.03504997 0.007189989 -0.01626998 0.03494995 0.005199968 -0.01706999 0.03494995 0.002459943 -0.01706999 0.03494995 0.002019941 -0.01706999 0.03494995 9.7e-4 -0.01706999 0.03494995 6.9e-4 -0.01706999 0.03504997 0.002459943 -0.01706999 0.03494995 -0.002209961 -0.01706999 0.03494995 -0.002459943 -0.01706999 0.03504997 -0.002459943 -0.01647996 0.03494995 -0.004479944 -0.01567995 0.03494995 -0.007189989 -0.01567995 0.03504997 -0.007189989 -0.01521998 0.03494995 -0.007909953 -0.01299995 0.03504997 -0.01133996 -0.01299995 0.03494995 -0.01133996 -0.01002997 0.03494995 -0.01388996 -0.01088994 0.03494995 -0.01314997 -0.009269952 0.03504997 -0.01454997 -0.009269952 0.03494995 0.01454997 -0.009269952 0.03504997 0.01454997 0.01718997 0.03494995 9.7e-4 0.01699 0.03494995 0.002019941 0.01638996 0.03494995 0.005199968 0.01631999 0.03494995 0.005589962 0.01631999 0.03504997 0.005589962 0.01104998 0.03494995 0.01314997 0.01406997 0.03494995 0.009979963 0.01406997 0.03504997 0.009979963 0.01394999 0.03494995 0.01010996 0.01066994 0.03494995 0.01354998 0.009659945 0.03494995 0.01413995 0.01066994 0.03504997 0.01354998 0.007199943 0.03494995 0.01555997 0.006399989 0.03494995 0.01601999 0.006399989 0.03504997 0.01601999 0.004629969 0.03494995 0.01644998 0.001969993 0.03494995 0.01708996 0.001759946 0.03494995 0.01713997 0.001609981 0.03494995 0.01717996 0.001609981 0.03504997 0.01717996 -0.003309965 0.03494995 0.01692998 -0.007969975 0.03504997 0.01529997 -0.007969975 0.03494995 0.01529997 -0.003309965 0.03504997 0.01692998 0.01722997 0.03504997 7.5e-4 0.01722997 0.03494995 7.5e-4 0.01722997 0.03494995 -7.5e-4 0.01722997 0.03504997 -7.5e-4 -0.02063995 0.005949974 0.006999969 -0.02213996 0.005949974 0.004869997 -0.02213996 0.005949974 -0.004869997 -0.02263998 0.005949974 -0.003999948 -0.02263998 0.005949974 0 -0.02263998 0.005949974 0.003999948 -0.02249997 0.005949974 -0.004499971 -0.02249997 0.005949974 0.004499971 -0.01838999 0.005949974 -0.01014 -0.01510995 0.005949974 -0.01458996 -0.01510995 0.005949974 0.01458996 -0.01077997 0.005949974 -0.01801997 -0.01077997 0.005949974 0.01801997 -0.005709946 0.005949974 -0.02020996 -0.005709946 0.005949974 0.02020996 -2.5e-4 0.005949974 -0.02099996 -2.5e-4 0.005949974 0.02099996 0 0.005949974 -0.02096998 0 0.005949974 0.02096998 0.005239963 0.005949974 -0.02033996 0.005239963 0.005949974 0.02033996 0.01036 0.005949974 -0.01826995 0.01036 0.005949974 0.01826995 0.01475995 0.005949974 -0.01493996 0.01475995 0.005949974 0.01493996 0.01814997 0.005949974 -0.01056998 0.01814997 0.005949974 0.01056998 0.02026998 0.005949974 -0.005479991 0.02099996 0.005949974 0 0.02026998 0.005949974 0.005479991 -0.02499997 -0.03504997 -0.023 -0.02499997 -0.03504997 0.023 -0.02472996 -0.03504997 -0.02399998 -0.02472996 -0.03504997 0.02399998 -0.02399998 -0.03504997 -0.02472996 -0.02399998 -0.03504997 0.02472996 -0.02363997 0.003949999 -0.02164 -0.02363997 0.003949999 0.02164 -0.02363997 0.00544995 -0.02164 -0.02363997 0.00544995 0.02164 -0.02357 0.005699992 -0.02164 -0.02357 0.005699992 0.02164 -0.02338999 0.005879998 -0.02164 -0.02338999 0.005879998 0.02164 -0.02336996 0.003959953 0.02263998 -0.02336996 0.00544995 -0.02263998 -0.02336996 0.00544995 0.02263998 -0.02336996 0.003959953 -0.02263998 -0.02331 0.005699992 -0.02260994 -0.02331 0.005699992 0.02260994 -0.02314996 0.005879998 -0.02250999 -0.02314996 0.005879998 0.02250999 -0.02313995 0.005949974 -0.02164 -0.02313995 0.005949974 0 -0.02313995 0.005949974 0.02164 -0.023 -0.03504997 -0.02499997 -0.023 -0.03504997 0.02499997 -0.02293998 0.005949974 -0.02238994 -0.02293998 0.005949974 0.02238994 0 -0.03504997 0 -0.02263998 0.003959953 0.02336996 -0.02263998 0.003959953 -0.02336996 -0.02263998 0.00544995 -0.02336996 -0.02263998 0.00544995 0.02336996 -0.02260994 0.005699992 -0.02331 -0.02260994 0.005699992 0.02331 -0.02250999 0.005879998 -0.02314996 -0.02250999 0.005879998 0.02314996 0 0.005949974 -0.02313995 -0.02164 0.005949974 -0.02313995 -0.02238994 0.005949974 -0.02293998 -0.02238994 0.005949974 0.02293998 -0.02164 0.003949999 -0.02363997 -0.02164 0.003949999 0.02363997 -0.02164 0.00544995 -0.02363997 -0.02164 0.00544995 0.02363997 -0.02164 0.005699992 -0.02357 -0.02164 0.005699992 0.02357 -0.02164 0.005879998 -0.02338999 -0.02164 0.005879998 0.02338999 -0.02164 0.005949974 0.02313995 -0.005709946 0.005949974 -0.02020996 -0.01077997 0.005949974 -0.01801997 -0.01510995 0.005949974 -0.01458996 -0.01838999 0.005949974 -0.01014 -0.02213996 0.005949974 -0.004869997 0 0.005949974 0.02313995 0.02164 0.003949999 -0.02363997 0.02164 0.003949999 0.02363997 0.02164 0.00544995 -0.02363997 0.02164 0.00544995 0.02363997 0.02164 0.005699992 -0.02357 0.02164 0.005699992 0.02357 0.02164 0.005879998 -0.02338999 0.02164 0.005879998 0.02338999 0.02164 0.005949974 -0.02313995 0.02164 0.005949974 0.02313995 0.02238994 0.005949974 -0.02293998 0.02238994 0.005949974 0.02293998 0.02250999 0.005879998 -0.02314996 0.02250999 0.005879998 0.02314996 0.02260994 0.005699992 -0.02331 0.02260994 0.005699992 0.02331 0.02263998 0.003959953 -0.02336996 0.02263998 0.003959953 0.02336996 0.02263998 0.00544995 -0.02336996 0.02263998 0.00544995 0.02336996 0.02293998 0.005949974 -0.02238994 0.02293998 0.005949974 0.02238994 0.023 -0.03504997 -0.02499997 0.023 -0.03504997 0.02499997 0.02313995 0.005949974 -0.02164 0.02313995 0.005949974 0 0.02313995 0.005949974 0.02164 0.02314996 0.005879998 -0.02250999 0.02314996 0.005879998 0.02250999 0.02331 0.005699992 -0.02260994 0.02331 0.005699992 0.02260994 0.02336996 0.003959953 -0.02263998 0.02336996 0.003959953 0.02263998 0.02336996 0.00544995 -0.02263998 0.02336996 0.00544995 0.02263998 0.02338999 0.005879998 -0.02164 0.02338999 0.005879998 0.02164 0.02357 0.005699992 -0.02164 0.02357 0.005699992 0.02164 0.02363997 0.003949999 -0.02164 0.02363997 0.003949999 0.02164 0.02363997 0.00544995 -0.02164 0.02363997 0.00544995 0.02164 0.02399998 -0.03504997 -0.02472996 0.02399998 -0.03504997 0.02472996 0.02472996 -0.03504997 -0.02399998 0.02472996 -0.03504997 0.02399998 0.02499997 -0.03504997 -0.023 0.02499997 -0.03504997 0.023 0 0.03504997 0 -0.02213996 0.005949974 0.004869997 -0.02063995 0.005949974 0.006999969 -0.01979994 0.005949974 0.006999969 -0.01838999 0.005949974 0.01014 -0.02113997 0.005949974 0.006869971 -0.02149999 0.005949974 0.006499946 -0.02164 0.005949974 0.005999982 -0.02164 0.005949974 0.004999995 -0.02263998 0.005949974 -0.003999948 -0.02263998 0.005949974 0.003999948 -0.02249997 0.005949974 -0.004499971 -0.02249997 0.005949974 0.004499971 -0.01838999 0.005949974 -0.01014 -0.01510995 0.005949974 -0.01458996 -0.01510995 0.005949974 0.01458996 -0.01077997 0.005949974 -0.01801997 -0.01077997 0.005949974 0.01801997 -0.005709946 0.005949974 -0.02020996 -0.005709946 0.005949974 0.02020996 -2.5e-4 0.005949974 -0.02099996 -2.5e-4 0.005949974 0.02099996 0 0.005949974 -0.02096998 0 0.005949974 0.02096998 0.005239963 0.005949974 -0.02033996 0.005239963 0.005949974 0.02033996 0.01036 0.005949974 -0.01826995 0.01036 0.005949974 0.01826995 0.01475995 0.005949974 -0.01493996 0.01475995 0.005949974 0.01493996 0.01814997 0.005949974 -0.01056998 0.01814997 0.005949974 0.01056998 0.02026998 0.005949974 -0.005479991 0.02099996 0.005949974 0 0.02026998 0.005949974 0.005479991 -0.005709946 0.005949974 -0.02020996 -0.01077997 0.005949974 -0.01801997 -0.01510995 0.005949974 -0.01458996 -0.01838999 0.005949974 -0.01014 -0.01979994 0.005949974 0.006999969 -0.01838999 0.005949974 0.01014 -0.02113997 0.005949974 0.006869971 -0.02149999 0.005949974 0.006499946 -0.02164 0.005949974 0.005999982 -0.02164 0.005949974 0.004999995 -0.02263998 0.005949974 -0.003999948 -0.02263998 0.005949974 0.003999948 -0.02249997 0.005949974 -0.004499971 -0.02249997 0.005949974 0.004499971 -0.01838999 0.005949974 -0.01014 -0.01510995 0.005949974 -0.01458996 -0.01510995 0.005949974 0.01458996 -0.01077997 0.005949974 -0.01801997 -0.01077997 0.005949974 0.01801997 -0.005709946 0.005949974 -0.02020996 -0.005709946 0.005949974 0.02020996 -2.5e-4 0.005949974 -0.02099996 -2.5e-4 0.005949974 0.02099996 0 0.005949974 -0.02096998 0 0.005949974 0.02096998 0.005239963 0.005949974 -0.02033996 0.005239963 0.005949974 0.02033996 0.01036 0.005949974 -0.01826995 0.01036 0.005949974 0.01826995 0.01475995 0.005949974 -0.01493996 0.01475995 0.005949974 0.01493996 0.01814997 0.005949974 -0.01056998 0.01814997 0.005949974 0.01056998 0.02026998 0.005949974 -0.005479991 0.02099996 0.005949974 0 0.02026998 0.005949974 0.005479991 -0.005709946 0.005949974 -0.02020996 -0.01077997 0.005949974 -0.01801997 -0.01510995 0.005949974 -0.01458996 -0.01838999 0.005949974 -0.01014 -0.02063995 0.005949974 0.006999969 -0.02213996 0.005949974 0.004869997 -0.02213996 0.005949974 -0.004869997 -0.01979994 0.005949974 0.006999969 -0.01838999 0.005949974 0.01014 -0.02113997 0.005949974 0.006869971 -0.02149999 0.005949974 0.006499946 -0.02164 0.005949974 0.005999982 -0.02164 0.005949974 0.004999995 -0.02263998 0.005949974 -0.003999948 -0.02263998 0.005949974 0.003999948 -0.02249997 0.005949974 -0.004499971 -0.02249997 0.005949974 0.004499971 -0.01838999 0.005949974 -0.01014 -0.01510995 0.005949974 -0.01458996 -0.01510995 0.005949974 0.01458996 -0.01077997 0.005949974 -0.01801997 -0.01077997 0.005949974 0.01801997 -0.005709946 0.005949974 -0.02020996 -0.005709946 0.005949974 0.02020996 -2.5e-4 0.005949974 -0.02099996 -2.5e-4 0.005949974 0.02099996 0 0.005949974 -0.02096998 0 0.005949974 0.02096998 0.005239963 0.005949974 -0.02033996 0.005239963 0.005949974 0.02033996 0.01036 0.005949974 -0.01826995 0.01036 0.005949974 0.01826995 0.01475995 0.005949974 -0.01493996 0.01475995 0.005949974 0.01493996 0.01814997 0.005949974 -0.01056998 0.01814997 0.005949974 0.01056998 0.02026998 0.005949974 -0.005479991 0.02099996 0.005949974 0 0.02026998 0.005949974 0.005479991 -0.005709946 0.005949974 -0.02020996 -0.01077997 0.005949974 -0.01801997 -0.01510995 0.005949974 -0.01458996 -0.01838999 0.005949974 -0.01014 -0.00945729 0.003627836 -0.001710772 0 -0.01554995 0.02431994 0 0.005949974 0 + + + + + + + + + + 0 -1 0 0 -1 0 -0.9492978 0 -0.3143782 -0.9492978 0 0.3143782 -0.7785766 0.001171708 0.6275485 -0.7808654 0.001171231 0.6246982 -0.6209343 -0.001221954 0.7838616 -0.6453004 -0.01542252 0.7637733 -0.3964945 -0.0151537 0.9179121 -0.4893132 0.006718158 0.8720822 -0.4938785 0.006718337 0.8695049 -0.1431924 -0.008457064 0.9896588 -0.316222 0.006060898 0.9486659 -0.3172233 0.0060606 0.9483315 0.2392156 0.007879972 0.9709345 0.119358 -0.01385879 0.9927546 0.0559594 0.00282979 0.9984291 0.1191433 0.00413686 0.9928685 0.05255794 0.004160761 0.9986092 -0.1333157 -0.004680752 0.9910625 0.2450903 0.007883667 0.9694681 0.3747907 -0.01294112 0.9270192 0.4188861 0.001862525 0.9080369 0.4327281 0.001863181 0.9015226 0.6034716 -0.002766609 0.7973797 0.5845207 -0.01302987 0.8112742 0.7900453 -0.01468151 0.6128727 0.7272006 0.007067918 0.6863886 0.7263449 0.007068157 0.6872941 0.9823358 0.002611696 0.1871086 0.9289886 -0.004128932 0.3700852 0.9230861 -0.009820222 0.3844679 0.8414472 0.006709396 0.5402976 0.8421885 0.006709873 0.5391413 0.9819628 0.002612411 0.1890563 0.9911401 -0.01446169 0.1320315 0.9999719 0.007499694 0 0.9999719 0.007499694 0 0.9911401 -0.01446169 -0.1320315 0.9819628 0.002612411 -0.1890563 0.9823358 0.002611696 -0.1871086 0.9231205 -0.004674613 -0.3844823 0.9289375 -0.01127177 -0.3700649 0.7900453 -0.01468151 -0.6128727 0.8421885 0.006709873 -0.5391413 0.8414472 0.006709396 -0.5402976 0.4327281 0.001863181 -0.9015226 0.5845689 -0.002225697 -0.8113411 0.6034404 -0.0105375 -0.7973385 0.7263449 0.007068157 -0.6872941 0.7272006 0.007067918 -0.6863886 0.4188861 0.001862525 -0.9080369 0.3747907 -0.01294112 -0.9270192 0.2450903 0.007883667 -0.9694681 0.2392156 0.007879972 -0.9709345 0.119358 -0.01385879 -0.9927546 0.0559594 0.00282979 -0.9984291 0.1191433 0.00413686 -0.9928685 0.05255794 0.004160761 -0.9986092 -0.1431956 -0.005152344 -0.989681 -0.1333114 -0.009266734 -0.9910309 -0.3964945 -0.0151537 -0.9179121 -0.3172233 0.0060606 -0.9483315 -0.316222 0.006060898 -0.9486659 -0.7808654 0.001171231 -0.6246982 -0.6453768 -8.88654e-4 -0.7638639 -0.6208926 -0.01165908 -0.783809 -0.4938785 0.006718337 -0.8695049 -0.4893132 0.006718158 -0.8720822 -0.7785766 0.001171708 -0.6275485 -0.8049004 -0.01266139 -0.5932749 -0.881016 0.007450222 -0.4730277 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 -0.9471516 0.1680965 0.2732169 -0.407418 0.1514139 -0.9006023 -0.9422059 0.2770094 -0.1884511 -0.9113164 0.1482697 -0.3840813 -0.9315509 0.1626105 -0.3252239 -0.7991091 -0.3146805 -0.5122509 -0.9081746 0.2506373 -0.3352608 -0.9111543 0.1456485 -0.3854666 -0.928664 0.2243673 -0.2953684 -0.925579 0.2859641 -0.2480482 -0.9177774 0.2868903 0.2745518 -0.9178981 0.2856691 0.2754201 -0.9143468 0.1813663 0.3620446 -0.9384292 0.1466302 0.31281 -0.9311535 0.1683419 0.3234411 -0.901751 0.1467134 0.4065961 -0.9103941 0.1680585 0.3780726 -0.912325 0.3225979 0.2521777 -0.9188216 0.2869244 0.2710005 -0.9848498 0.1048805 0.138097 -0.9578264 0.2873474 0 -0.9559739 0.2861407 -0.06509518 -0.9987523 -0.04993659 0 -0.9210147 0.2770513 -0.2738146 -0.4861897 0.1523008 -0.8604789 -0.4091901 0.1777053 -0.8949773 -0.3149025 0.1457877 -0.9378605 -0.1278769 0.1498839 -0.9803991 -0.309273 0.1512408 -0.9388697 -0.141013 0.1688628 -0.9755002 -0.1303948 0.1630746 -0.977959 -0.4837509 0.1474936 -0.8626881 -0.636475 0.163087 -0.7538581 -0.645197 0.1696269 -0.744948 -0.7708169 0.1599442 -0.6166515 -0.6459589 0.159875 -0.746443 -0.7711567 0.1450923 -0.6198915 -0.8274903 0.1763324 -0.533073 -0.8709936 0.1562899 -0.4657721 -0.9149418 0.1555567 -0.3724025 -0.7441748 0.5367867 -0.3975726 -0.9384292 0.1466302 -0.3128098 -0.9364062 0.1603595 0.3121352 -0.8722791 0.1474033 0.4662632 -0.8271023 0.1767897 0.5335236 -0.7664864 0.1686882 0.6197121 -0.8297607 0.1690638 0.5318973 -0.7716946 0.1450895 0.6192225 -0.6450954 0.1696101 0.7450398 -0.6363618 0.1663306 0.7532449 -0.64563 0.1662972 0.7453235 -0.6361207 0.1630551 0.754164 -0.4856274 0.1477051 0.8615968 -0.4099487 0.177209 0.8947284 -0.2937462 0.1736751 0.9399735 -0.4077602 0.1746891 0.8962228 -0.3145152 0.1452308 0.9380769 -0.1418845 0.1683772 0.9754577 -0.1341822 0.1666406 0.976845 -0.1389042 0.1666364 0.9761855 -0.1267469 0.1627997 0.9784843 0.1395421 0.1371724 0.980669 0.05338537 0.1733645 0.9834097 0.07274723 0.1735997 0.9821257 0.1416881 0.1761806 0.9741073 0.2381073 0.1455035 0.9602779 0.4082419 0.1680009 0.8972816 0.4154286 0.1666471 0.8942303 0.4107152 0.1666655 0.8964015 0.4143497 0.165222 0.894995 0.6486326 0.1363442 0.7487897 0.575868 0.1693211 0.7998164 0.575499 0.1693159 0.8000832 0.6442187 0.1772168 0.7440273 0.7189208 0.1454344 0.6797072 0.8300612 0.16697 0.5320899 0.8296917 0.1670521 0.5326398 0.8285282 0.1670665 0.5344434 0.8312138 0.1651425 0.5308593 0.9521918 0.1337416 0.2746708 0.9171259 0.1644181 0.3631072 0.914055 0.1643574 0.3707966 0.9438654 0.1778166 0.2783871 0.9717582 0.1459009 0.1854698 0.9860588 0.1663975 0 0.9860587 0.1663975 0 0.9860587 0.1663975 0 0.9860587 0.1663975 0 0.9500489 0.1327823 -0.2824464 0.9697747 0.1601643 -0.1840775 0.9693279 0.1601721 -0.1864089 0.9447124 0.1775989 -0.2756393 0.830715 0.1654286 -0.5315507 0.8288937 0.166745 -0.533977 0.7189707 0.1454441 -0.6796524 0.6466634 0.1530798 -0.747257 0.717962 0.1537514 -0.6788895 0.6443232 0.1772074 -0.743939 0.5763537 0.1462174 -0.8040131 0.4070715 0.1568021 -0.8998366 0.5925795 0.1567866 -0.7901059 0.4147536 0.1656929 -0.8947209 0.4098841 0.1681987 -0.8964955 0.2399455 0.1457665 -0.9597803 0.1279237 0.1474865 -0.9807565 0.2369926 0.1489789 -0.9600207 0.1408129 0.1768286 -0.9741167 0.05614864 0.1468989 -0.9875566 0.05065917 0.1469421 -0.987847 -0.1424711 -3.79589e-4 -0.9897989 -0.129338 0 -0.9916005 -0.4158082 0 -0.9094523 -0.4154945 -2.07829e-4 -0.9095956 -0.4121701 0 -0.911107 -0.6546844 0 -0.7559022 -0.6546447 -3.01789e-5 -0.7559368 -0.6543759 0 -0.7561693 -0.840663 0 -0.5415588 -0.8408741 2.12565e-4 -0.5412307 -0.8418785 0 -0.5396671 -0.9599732 0 -0.280092 -0.9596633 -5.69179e-4 -0.2811512 -0.9585365 0 -0.2849697 -1 0 0 -1 0 0 -1 0 0 -0.9591508 0 0.2828954 -0.9596631 -8.33003e-4 0.2811512 -0.9608235 0 0.2771607 -0.8403387 0 0.5420618 -0.840874 -4.25545e-4 0.5412307 -0.8418794 0 0.5396656 -0.6545795 0 0.7559933 -0.6546447 -3.46783e-5 0.7559368 -0.6547469 0 0.7558482 -0.4165412 0 0.9091168 -0.4154945 4.29833e-4 0.9095956 -0.4141279 0 0.9102187 -0.1439396 0 0.9895865 -0.142471 5.09058e-4 0.9897989 -0.1408738 0 0.9900277 0.1408738 0 0.9900277 0.142471 5.09058e-4 0.9897989 0.1439396 0 0.9895865 0.4141279 0 0.9102187 0.4154945 4.29833e-4 0.9095956 0.4165412 0 0.9091168 0.6547469 0 0.7558482 0.6546447 -3.46783e-5 0.7559368 0.6545795 0 0.7559933 0.8418794 0 0.5396656 0.840874 -4.25545e-4 0.5412307 0.8403387 0 0.5420618 0.9608235 0 0.2771607 0.9596631 -8.33003e-4 0.2811512 0.9591508 0 0.2828954 1 0 0 1 0 0 1 0 0 0.9585365 0 -0.2849697 0.9596633 -5.69179e-4 -0.2811512 0.9599732 0 -0.280092 0.8418785 0 -0.5396671 0.8408741 2.12565e-4 -0.5412307 0.840663 0 -0.5415588 0.6543759 0 -0.7561693 0.6546447 -3.01789e-5 -0.7559368 0.6546844 0 -0.7559022 0.4121701 0 -0.911107 0.4154945 -2.07829e-4 -0.9095956 0.4158082 0 -0.9094523 0.129338 0 -0.9916005 0.1424711 -3.79589e-4 -0.9897989 0.1430675 0 -0.9897129 -0.1430675 0 -0.9897129 -1 0 0 -1 0 0 -1 0 0 0 1 0 0 1 0 0 1 0 0 1 0 0 0.9838701 0.1788842 -0.007730364 0.9677938 0.2516258 -0.008649408 0.7070808 0.7070799 0.006879746 0.8947982 0.4464178 0.006871819 0.8719238 0.4895934 0.002814829 0.377206 0.9261251 -0.008792817 0.2516237 0.9677851 0 0.1293374 0.9916006 -0.9629649 0 0.269627 -0.9629649 0 0.269627 -0.251012 0.07028329 0.965429 -0.1845505 0.1845505 0.9653404 -0.07028359 0.2510112 0.9654293 -0.1942211 0.6936423 0.6936414 -0.1935422 0.6935194 0.6939541 -0.5112335 0.5112336 0.690855 -0.5112314 0.5112344 0.690856 -0.6932809 0.1941184 0.6940315 -0.7049736 0.1803423 0.6859219 -0.2607492 0.9343423 0.2429284 -0.243086 0.9349478 0.2584215 -0.682882 0.6828859 0.2594976 -0.6944488 0.6756823 0.2473751 -0.9403539 0.2405559 0.2405565 -0.9297617 0.2603295 0.2603303 -0.2516335 0.9678226 0 -0.2516335 0.9678226 0 -0.7167247 0.6973562 0 -0.7167247 0.6973562 0 -0.962965 0.2696263 0 -0.962965 0.2696263 0 -0.7167251 0 -0.6973558 -0.9629649 0 -0.269627 -0.9629649 0 -0.269627 -0.07028359 0.2510112 -0.9654293 -0.1845506 0.1845506 -0.9653404 -0.2440263 0.938564 -0.2440261 -0.2595868 0.9301772 -0.2595866 -0.6923445 0.673635 -0.2586021 -0.6852509 0.685255 -0.2466917 -0.9343009 0.2616005 -0.2421712 -0.9350127 0.2391895 -0.2618005 -0.1936007 0.6937291 -0.6937282 -0.1941202 0.693282 -0.6940299 -0.5112318 0.5112348 -0.6908552 -0.5112326 0.5112326 -0.6908563 -0.6994224 0.1789222 -0.6919503 -0.7027144 0.1967598 -0.6837237 -0.251012 0.07028329 -0.965429 0 0.1543763 -0.9880121 -0.009795784 0.2516214 -0.9677761 -0.007334709 0.722306 -0.6915347 0.00679636 0.4472005 -0.8944079 0.006780803 0.5067117 -0.8620889 -0.009076535 0.9677828 -0.2516229 0 0.9892032 -0.1465499 -0.2263051 0.7072249 0.6697902 0.3015977 0.696307 0.6513029 0.7167264 0.6973544 0 0.5183387 0.69911 -0.4925142 0.03862148 0.6995103 -0.7135781 -0.1644762 0.9669772 0.1946862 -0.1248043 0.9671578 0.2214269 0.01370066 0.9675313 0.2523798 0.06004744 0.9683741 0.2421693 0.1849305 0.9670732 0.1748434 0.1059339 0.9677575 -0.2285243 -0.1261059 0.966584 -0.2231875 -0.199501 0.9668129 -0.1596003 -0.6712515 0.704275 -0.2311238 -0.4654791 0.8727707 -0.1469718 -0.4844728 0.8612704 -0.1532949 -0.8913083 0.3450198 -0.2941612 -0.915989 0.251125 -0.3128905 -0.944593 0.09943693 -0.3128201 -0.9346077 0.1752427 0.3095133 -0.9163941 0.251105 0.3117182 -0.8494461 0.4497035 0.2760577 -0.6587777 0.7186641 0.2225626 -0.11454 0.9926847 0.03817999 -0.2079164 0.9745261 0.08408081 -0.2403051 0.9673867 0.08010166 -0.2239108 0.9672326 0.119688 -0.2239329 0.9672339 -0.1196353 -0.2369856 0.9674997 -0.08821576 -0.168695 0.984063 -0.05623167 -0.220901 0.9681158 -0.1181289 -0.195109 0.9681603 -0.1568376 -0.1642051 0.9670629 -0.194489 -0.1243403 0.9671458 -0.2217399 -0.03328311 0.9677709 -0.249623 0.01294058 0.9675522 -0.2523397 0.01371669 0.9672559 -0.2534325 0.01418453 0.9682756 -0.2494817 0.05979275 0.9683793 -0.2422114 0.1485582 0.966945 -0.2072386 0.1849181 0.9670735 -0.1748546 0.1827625 0.9676989 -0.173657 0.181624 0.9682638 -0.1716915 0.2103747 0.9683087 -0.1346127 0.2516335 0.9678226 0 0.2516335 0.9678226 0 0.2516335 0.9678227 0 0.2469634 0.9678903 -0.04687726 0.2482241 0.9675434 0.04737603 0.2343087 0.9675045 0.09504997 0.2367252 0.9670455 0.09372389 0.21423 0.9671535 0.1368191 0.1064419 0.9675605 0.2291214 0.1058055 0.9677801 0.2284879 0.1058276 0.9677543 0.2285876 0.1467888 0.9679301 0.2038734 -0.0766099 0.966454 0.245148 -0.08143335 0.9670985 0.2410164 -0.07958346 0.9681548 0.2373665 -0.03413015 0.9680389 0.2484672 -0.1947117 0.9681448 0.1574264 -0.2211804 0.9681038 0.1177042 -0.6276769 0.7031698 0.3340269 -0.6718416 0.7042246 0.2295574 -0.8316648 0.4851421 0.270131 -0.224141 0.7071188 -0.6706294 -0.08158397 0.9665949 -0.242978 -0.08118349 0.9673035 -0.240277 -0.230294 0.6945442 -0.6815959 -0.09658795 0.6949279 -0.7125629 -0.1300739 0.2494957 -0.9596003 0.05087167 0.2513077 -0.9665695 0.3019366 0.6962896 -0.6511644 0.06135189 0.9674767 -0.2454073 0.1061488 0.9675673 -0.2292293 0.3024179 0.6942894 -0.653074 0.4201316 0.6941274 -0.584531 0.5638182 0.2583763 -0.7844429 0.7015674 0.2590524 -0.6638488 0.657499 0.7065963 -0.2615662 0.6635753 0.6994388 -0.2654304 0.7016292 0.6998641 -0.133816 0.5189352 0.6991608 0.4918134 0.2101477 0.9683169 0.1349092 0.1812267 0.9682484 0.1721977 0.5147917 0.7040795 0.4891439 0.4141255 0.7046437 0.5761747 0.562515 0.2665852 0.7826297 0.4170309 0.2669032 0.8688198 0.03983807 0.6995611 0.7134614 0.01843094 0.9683725 0.2488275 0.01364505 0.9676027 0.2521088 0.03827118 0.7060714 0.7071057 -0.09523808 0.7051784 0.7026044 -0.1298174 0.2567928 0.9577082 -0.305629 0.2567279 0.916887 -0.4603919 0.7019324 0.5434429 -0.1641054 0.967066 0.194558 -0.164548 0.966937 0.1948249 -0.463243 0.6961148 0.5484799 -0.5580259 0.6967077 0.4507831 -0.8506854 0.2602313 -0.4567428 -0.2229626 0.9675791 -0.1186526 -0.6337479 0.6961472 -0.3372576 -0.6271286 0.7031058 -0.3351895 -0.8501825 0.2659004 -0.454408 -0.8554773 0.2506282 -0.4531491 -0.7517282 0.2706233 -0.6013882 -0.1978173 0.9671215 -0.159826 -0.5579887 0.6967101 -0.4508255 -0.5582559 0.6964031 -0.4509689 -0.7490752 0.2696682 -0.6051161 -0.7513393 0.2621917 -0.6055946 -0.1644515 0.9669765 -0.1947106 -0.4595423 0.7019816 -0.5440982 -0.464033 0.6961706 -0.5477408 -0.6238481 0.261821 -0.7363854 -0.621176 0.2712791 -0.7352197 -0.1242595 0.9673138 -0.221051 -0.3465486 0.7069951 -0.6164916 -0.3499536 0.7017597 -0.6205367 -0.4731478 0.2687683 -0.8389843 -0.4767771 0.2609524 -0.839397 -0.1288474 0.2567702 -0.9578452 -0.305629 0.2567279 -0.9168869 -0.3062461 0.2581284 -0.9162877 -0.3065935 0.2567667 -0.916554 -0.4726419 0.2588881 -0.8423696 0.05413478 0.2532923 -0.965874 0.05391412 0.2545722 -0.9655498 0.03948223 0.7060217 -0.7070888 -0.09333491 0.7052802 -0.7027577 -0.03303414 0.9680098 -0.2487279 -0.07829689 0.9681807 -0.2376884 0.06135183 0.9674766 -0.2454078 0.1740778 0.696309 -0.6963121 0.1739403 0.6986982 -0.6939493 0.2341818 0.2688215 -0.9342879 0.2314112 0.2534631 -0.9392579 0.5634199 0.2665578 -0.7819878 0.4170309 0.2669032 -0.8688198 0.4046592 0.2732187 -0.8726983 0.4044461 0.2603067 -0.8767347 0.2365246 0.2621741 -0.9355859 0.7046238 0.2473371 -0.6650787 0.7013934 0.2572429 -0.6647355 0.5154538 0.7040323 -0.488514 0.415661 0.7047259 -0.5749672 0.1455301 0.9686573 -0.2013059 0.15098 0.9678227 -0.2013067 0.2147524 0.9670686 -0.1366009 0.5968894 0.7068039 -0.3796725 0.6017773 0.7001448 -0.3842673 0.8154708 0.2526968 -0.5207222 0.8161497 0.2468252 -0.5224721 0.9629638 0.2696306 0 0.9449788 0.2718722 -0.1819358 0.9482918 0.2608299 -0.18086 0.9482879 0.261008 -0.1806234 0.8967889 0.2617092 -0.3567605 0.8974719 0.258296 -0.3575298 0.8130294 0.2577725 -0.5220504 0.9629638 0.2696306 0 0.9629638 0.2696306 0 0.7167264 0.6973544 0 0.70467 0.6965711 -0.1350152 0.2486808 0.9674128 -0.04764741 0.2481271 0.9675516 -0.04771661 0.9482879 0.261008 0.1806234 0.2469177 0.9678809 0.04730957 0.7014893 0.6998912 0.1344058 0.7047534 0.6966033 0.1344119 0.9482918 0.2608299 0.18086 0.9449788 0.2718721 0.1819358 0.2322279 0.9682156 0.09289127 0.6569463 0.7066604 0.2627788 0.6640112 0.699507 0.2641569 0.8976452 0.2582861 0.3571012 0.8966205 0.2616998 0.3571906 0.2123748 0.9678058 0.1350885 0.6024934 0.7000937 0.3832371 0.59627 0.7067469 0.3807505 0.8154708 0.2526969 0.5207222 0.8130294 0.2577725 0.5220504 0.5647161 0.2584066 0.7837867 0.7015674 0.2590524 0.6638488 0.7013934 0.2572429 0.6647355 0.7046238 0.2473371 0.6650787 0.8161497 0.2468252 0.5224721 0.4044461 0.2603067 0.8767347 0.4046592 0.2732187 0.8726983 0.3027438 0.6943063 0.6529051 0.4217932 0.6940296 0.5834496 0.1488554 0.9671839 0.2059057 0.1488695 0.9669557 0.2069647 0.06135171 0.9674767 0.2454074 0.1735082 0.6987219 0.6940336 0.1745008 0.6963297 0.6961856 0.2341818 0.2688215 0.9342879 0.2365246 0.2621741 0.9355859 -0.1291 0.2495277 0.9597235 0.05087167 0.2513077 0.9665695 0.05391412 0.2545722 0.9655498 0.05413478 0.2532923 0.965874 0.2314112 0.2534631 0.9392579 -0.3065935 0.2567667 0.916554 -0.3062461 0.2581284 0.9162877 -0.2280214 0.6946619 0.6822397 -0.09468263 0.6948394 0.7129049 -0.03354847 0.9669889 0.2526004 -0.03234249 0.9677872 0.2496837 -0.1255912 0.9665973 0.22342 -0.3491178 0.7017115 0.6210619 -0.3474251 0.7069473 0.616053 -0.4731478 0.2687683 0.8389843 -0.4726419 0.2588881 0.8423696 -0.8506854 0.2602313 0.4567428 -0.7513393 0.2621915 0.6055946 -0.7490752 0.2696682 0.6051161 -0.7517282 0.2706233 0.6013882 -0.6221522 0.2713071 0.7343835 -0.6228598 0.2618451 0.7372128 -0.4767771 0.2609524 0.839397 -0.8554773 0.2506282 0.4531491 -0.8501825 0.2659004 0.454408 -0.6330834 0.6962109 0.3383722 -0.5582225 0.6964006 0.4510145 -0.1946848 0.9681715 0.1572951 -0.1992351 0.9668231 0.15987 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0.2576645 0 -0.9662344 -0.05074721 0 -0.9987115 -0.05074721 0 -0.9987115 -0.3301701 0 -0.9439214 -0.3301701 0 -0.9439214 0.2314149 -0.04166501 -0.9719626 0.2337992 -0.0328713 -0.9717291 -0.2353472 -0.01319801 0.9718218 0.2360719 0 -0.9717356 0.498471 0 -0.8669065 0.5025548 0.05025458 -0.8630836 0.4977039 -0.0518344 -0.8657968 0.5000953 0.04980254 -0.8645372 0.5044022 0 -0.8634689 0.7249996 0 -0.6887493 0.7241208 -0.006897449 -0.6896386 0.8897777 0 -0.4563943 0.8898923 -0.008304476 -0.4560952 0.8900858 0 -0.4557931 0.7240293 0 -0.6897691 -0.9833266 0 0.1818479 -0.9818185 -0.044227 0.1845983 0.9825241 -0.002599418 -0.1861173 0.9825702 0 -0.1858918 0.6556872 0 0.7550326 -0.6556872 0 0.7550326 -0.6515577 -0.04579263 0.7572157 -0.6516571 -0.04426085 0.7572213 -0.6510875 0 0.7590027 -0.8414006 0 0.540412 -0.8395078 -0.03620713 0.54214 -0.8394931 0 0.5433704 -0.9587494 0 0.2842525 -0.9582376 0.04984301 0.2815961 -0.9599216 0 0.2802686 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -0.9598937 0 -0.2803642 -0.9588532 0.03467482 -0.281777 -0.9590831 0 -0.2831244 -0.8426965 0 -0.5383889 0.8393291 -0.04166615 0.5420246 0.8395044 0 0.5433529 0.6522963 0 0.757964 0.6490944 0.0781871 0.7566791 0.6515577 0.04579263 0.7572157 0.8899229 0 0.4561109 0.9838697 0 0.1788867 0.9821668 0.01870387 0.187079 0.9826549 -0.003704786 0.1854068 0.9822553 -0.03267395 0.1846804 0.9842713 0 0.1766641 0.8899229 0 0.4561109 0.7249996 0 0.6887493 0.7241208 -0.006897449 0.6896386 0.7232667 0.02902907 0.6899583 0.7348049 0 0.6782786 0.5044022 0 0.8634689 0.4990119 0.0604375 0.864485 0.5005733 0.02391499 0.8653637 0.498471 0 0.8669065 0.2360719 0 0.9717356 0.2353472 0.01319825 0.9718218 0.2337992 -0.03287124 0.9717291 0.2314148 -0.04166501 0.9719625 0.2576645 0 0.9662344 -0.05074721 0 0.9987115 -0.05074721 0 0.9987115 -0.3301701 0 0.9439214 0.3301701 0 -0.9439214 0 -1 0 0 -1 0 0 -1 0 0 0.03485059 -0.9993925 0.9993925 0.03485059 0 1 0 0 0 0 -1 -0.260666 0 0.9654291 -0.9654291 0 -0.260666 -0.9654291 0 -0.260666 -0.260666 0 -0.9654291 -0.9645484 0.04270619 -0.2604277 -0.9646556 0.04270696 -0.2600302 0.9654291 0 -0.260666 0.2604277 0.04270619 -0.9645484 0.7071068 0 0.7071068 0.2689354 0.9604846 -0.07171601 0.2758985 0.9581687 -0.07611 0.2031767 0.95783 -0.2031767 0.2031767 0.95783 -0.2031767 0.07389479 0.9579936 -0.2771059 0.07417577 0.9603113 -0.2688869 0.6940287 0.6940234 -0.1914564 0.6821739 0.707958 -0.1828504 0.4946855 0.7145437 -0.4946855 0.4946855 0.7145437 -0.4946855 0.1876068 0.7087336 -0.6800739 0.1862189 0.6947355 -0.694741 0.9324051 0.2610736 -0.2499225 0.9336825 0.2543331 -0.2520944 0.6852533 0.2466892 -0.6852533 0.6852534 0.2466892 -0.6852534 0.2503941 0.2542446 -0.9341641 0.2516342 0.2609541 -0.9319781 0 0 0 0 0.7071041 -0.7071095 0 0.7071041 -0.7071095 0 0.2696301 -0.9629639 0 0.2696301 -0.9629639 0.2696297 0.9629641 0 0.2696297 0.9629641 0 0.2696297 0.9629641 0 0.7071095 0.7071041 0 0.7071095 0.7071041 0 0.9629639 0.2696301 0 0.9629639 0.2696301 0 -0.07611 0.9581687 -0.2758985 -0.2031767 0.95783 -0.2031767 -0.2031767 0.95783 -0.2031767 -0.2771059 0.9579936 -0.07389479 -0.2688869 0.9603113 -0.07417577 -0.1914564 0.6940234 -0.6940287 -0.1828504 0.7079579 -0.6821739 -0.4946855 0.7145437 -0.4946855 -0.4946855 0.7145437 -0.4946855 -0.6800737 0.7087335 -0.1876068 -0.694741 0.6947355 -0.1862189 -0.2499225 0.2610736 -0.9324051 -0.2520944 0.2543331 -0.9336825 -0.6852533 0.2466892 -0.6852533 -0.6852534 0.2466892 -0.6852534 -0.9341641 0.2542446 -0.2503941 -0.9319781 0.2609541 -0.2516342 0.07171601 0.9604846 0.2689354 0.07611 0.9581687 0.2758985 0.2031767 0.95783 0.2031767 0.2031767 0.95783 0.2031767 0.2771059 0.9579936 0.07389479 0.2688869 0.9603113 0.07417577 0.1914564 0.6940234 0.6940287 0.1828504 0.7079579 0.6821739 0.4946855 0.7145437 0.4946855 0.4946855 0.7145437 0.4946855 0.6800737 0.7087335 0.1876068 0.694741 0.6947355 0.1862189 0.2499225 0.2610736 0.9324051 0.2520944 0.2543331 0.9336825 0.6852533 0.2466892 0.6852533 0.6852534 0.2466892 0.6852534 0.9341641 0.2542446 0.2503941 0.9319781 0.2609541 0.2516342 -0.2696297 0.9629641 0 -0.2696297 0.9629641 0 -0.2696297 0.9629641 0 -0.7071095 0.7071041 0 -0.7071095 0.7071041 0 -0.9629639 0.2696301 0 -0.9629639 0.2696301 0 0 0.9629641 0.2696297 0 0.9629641 0.2696297 0 0.9629641 0.2696297 0 0.7071041 0.7071095 0 0.7071041 0.7071095 0 0.2696301 0.9629639 0 0.2696301 0.9629639 -0.2689354 0.9604846 0.07171601 -0.2758985 0.9581687 0.07611 -0.2031767 0.95783 0.2031767 -0.2031767 0.95783 0.2031767 -0.07389479 0.9579936 0.2771059 -0.07417577 0.9603113 0.2688869 -0.6940287 0.6940234 0.1914564 -0.6821739 0.707958 0.1828504 -0.4946855 0.7145437 0.4946855 -0.4946855 0.7145437 0.4946855 -0.1876068 0.7087336 0.6800739 -0.1862189 0.6947355 0.694741 -0.9324051 0.2610736 0.2499225 -0.9336825 0.2543331 0.2520944 -0.6852533 0.2466892 0.6852533 -0.6852534 0.2466892 0.6852534 -0.2503941 0.2542446 0.9341641 -0.2516342 0.2609541 0.9319781 -0.9629639 0 -0.2696301 -0.9629639 0 -0.2696301 -0.7167251 0 -0.6973558 -0.7167237 -2.52567e-7 -0.6973572 -0.2516344 0 -0.9678224 -0.2516335 0 -0.9678226 -0.2516344 0 -0.9678224 -0.2516344 0 0.9678224 -0.2516344 0 0.9678224 -0.7167251 0 0.6973558 -0.7167251 0 0.6973558 -0.9629639 0 0.2696301 -0.9629639 0 0.2696301 -1 0 0 -1 0 0 0 0 1 0 0 -1 0 0 -1 -1 0 0 -1 0 0 -0.9122474 0 0.4096398 -0.8835567 -0.01679629 -0.4680228 -0.957763 0.01149314 -0.2873293 -0.957763 0.01149314 0.2873293 -0.8835567 -0.01679629 0.4680228 -0.9086417 0.007449328 0.4175103 -0.881016 0.007450222 0.4730277 -0.8049004 -0.01266139 0.5932749 -0.9122221 0.007446944 -0.4096284 -0.901296 0 -0.4332039 0 0 1 0 0 1 0 0 -1 0 0 -1 0 0 -1 -0.9190827 0.28875 -0.2681613 -0.9068856 0.3034356 -0.292379 -0.9189138 0.2091057 0.3344731 0.9175001 0.1444488 -0.3705781 0.8302804 0.1654177 -0.5322325 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 0 1 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 -0.2516344 0 0.9678224 0.002348303 0.6348381 0.7726417 -0.7167251 0 0.6973558 -0.7167251 0 0.6973558 -0.2516344 0 0.9678224 -0.2516344 0 0.9678224 -0.2516344 0 -0.9678224 -0.2516344 0 -0.9678224 -0.7167251 0 -0.6973558 -0.004324674 0.7071006 -0.7070997 0.003669202 0.9127396 -0.4085254 0.2344187 0.967514 -0.09468168 -0.7256583 0.6450302 -0.2394912 -0.3975354 0.9086532 0.1277299 0.2290577 0.9690912 -0.09162318 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 0 0 0 0 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 0 0 0 0 0 0 0.03485059 -0.9993925 0.9993925 0.03485059 0 -0.9993925 0.03485059 0 -0.9993925 0.03485059 0 0 0 1 0 0 1 1 0 0 0 0 -1 -1 0 0 -1 0 0 -0.7071068 0 0.7071068 -0.7071068 0 0.7071068 -0.260666 0 0.9654291 -0.9654291 0 0.260666 -0.9654291 0 0.260666 -0.9646556 0.04270696 0.2600302 -0.9645484 0.04270619 0.2604277 -0.706249 0.04924374 0.7062489 -0.7062489 0.04924374 0.7062489 -0.2604277 0.04270619 0.9645484 -0.2600302 0.04270696 0.9646556 -0.7071068 0 -0.7071068 -0.7071068 0 -0.7071068 -0.260666 0 -0.9654291 -0.2600301 0.04270696 -0.9646555 -0.2604277 0.04270619 -0.9645484 -0.7062489 0.04924374 -0.706249 -0.7062489 0.04924374 -0.7062489 0.260666 0 -0.9654291 0.7071068 0 -0.7071068 0.7071068 0 -0.7071068 0.260666 0 -0.9654291 0.9654291 0 -0.260666 0.9646556 0.04270696 -0.2600302 0.9645484 0.04270619 -0.2604277 0.706249 0.04924374 -0.7062489 0.7062489 0.04924374 -0.7062489 0.2600302 0.04270696 -0.9646556 0.2604278 0.04270619 0.9645484 0.2600301 0.04270696 0.9646556 0.9645479 0.04271709 0.2604277 0.964656 0.04269605 0.2600303 0.7062489 0.04924374 0.7062489 0.706249 0.04924374 0.7062489 0.7071068 0 0.7071068 0.9654291 0 0.260666 0.9654291 0 0.260666 0.260666 0 0.9654291 0.260666 0 0.9654291 0 0 0 0 0 0 -1.5404e-7 1 -6.36081e-7 3.24032e-7 1 5.61656e-7 6.54708e-7 1 0 -6.48629e-7 1 0 -5.82531e-7 1 2.98564e-7 4.26935e-7 1 4.96095e-7 -3.27791e-7 1 5.66667e-7 6.28108e-7 1 1.84581e-7 -1.5404e-7 1 6.36081e-7 5.49908e-7 1 -3.55121e-7 5.49908e-7 1 3.55121e-7 -4.74061e-7 1 -4.51486e-7 0 1 6.53629e-7 2.16121e-7 1 -6.17868e-7 3.24032e-7 1 -5.61656e-7 -5.82531e-7 1 -2.98564e-7 -4.74061e-7 1 4.51486e-7 4.26935e-7 1 -4.96095e-7 -3.27791e-7 1 -5.66667e-7 -6.43363e-7 1 1.20963e-7 2.16121e-7 1 6.17868e-7 6.28108e-7 1 -1.84581e-7 -6.43363e-7 1 -1.20963e-7 0 1 -6.53629e-7 0 0.9629641 -0.2696297 0 0.9629641 -0.2696297 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.1872387 -0.9823144 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.1364855 -0.9817008 -0.1327975 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.08765232 -0.9373711 -0.3371237 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.1734797 -0.9848374 0 0 0 0 0 0 0 0 0 0 -0.1734797 -0.9848374 0 0 0 0 0 0 0 0 0.9629641 -0.2696297 0 0 0 -0.07171601 0.9604846 -0.2689354 -0.08764123 -0.9899891 0.1106374 0.07545632 -0.9970984 0.01005166 -0.165484 -0.9851234 -0.04633498 0.06710433 -0.9963871 -0.05205571 -0.01855289 -0.9915714 -0.1282264 -0.1596735 -0.9861569 0.0447086 0.01166564 -0.9952132 0.09702873 0.1506127 0.98234 0.1110134 0.01357448 -0.9934884 -0.1131218 0 0 0 -0.1199531 -0.9858954 0.1167114 0.0334047 -0.9960209 0.08262425 0.07395762 -0.9967855 -0.03080356 -0.1550459 -0.986953 0.04341232 -0.04902708 -0.9923275 0.1135011 -0.05939245 -0.9717465 0.2284322 0.01359909 -0.9934894 -0.1131098 0 0 0 -0.1297165 -0.9869306 0.09561127 0.05006933 -0.9965522 0.06615769 0.07658731 -0.9970107 -0.01020228 -0.1044685 -0.9857454 -0.1318801 0.1044685 0.9857454 0.1318801 0 -0.9662557 0.2575849 -0.01562213 -0.9940313 0.1079711 -0.1443563 -0.9795069 -0.1404551 0.03801149 -0.9948446 -0.09401881 0.06233876 -0.9968829 0.04835891 -0.05876457 -0.9889584 -0.136044 -0.05089199 -0.9793348 0.195738 0.01164436 -0.9952126 0.09703755 -0.1872387 -0.9823144 0 0.05545455 -0.995769 -0.0732733 -0.1144365 -0.9871711 0.1113438 0.07075601 -0.9970582 0.02947002 -0.1506127 -0.98234 -0.1110134 0.05876457 0.9889584 0.136044 -0.1603716 -0.9844262 0.07201397 -0.01855289 -0.9915714 -0.1282265 0 0.03485065 0.9993925 0 0.03485059 0.9993926 0 0.03485053 0.9993925 0 0.03485059 0.9993926 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 -0.1707559 -0.9841526 -0.04781168 0.070692 0.9597275 0.2718927 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.1790391 0.9805517 0.08039653 -0.1790391 -0.9805517 -0.08039653 0 -0.9156407 -0.4019977 + + + + + + + + + + + + + + + 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 4 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 4 4 4 4 4 4 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 4 4 4 4 4 4 3 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 3 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 3 4 4 4 4 4 4 4 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 +

376 0 371 0 375 0 325 1 326 1 329 1 161 2 75 2 120 2 122 3 76 3 284 3 277 4 404 4 278 4 278 5 404 5 280 5 280 6 404 6 406 6 280 7 406 7 282 7 408 8 275 8 406 8 406 9 275 9 281 9 406 10 281 10 282 10 410 11 259 11 408 11 408 12 259 12 266 12 408 13 266 13 275 13 414 14 263 14 264 14 414 15 264 15 412 15 412 16 264 16 262 16 412 17 262 17 410 17 410 18 262 18 260 18 410 19 260 19 259 19 263 20 414 20 257 20 257 21 414 21 416 21 257 22 416 22 248 22 248 23 416 23 241 23 241 24 416 24 418 24 241 25 418 25 242 25 420 26 246 26 418 26 418 27 246 27 244 27 418 28 244 28 242 28 230 29 234 29 423 29 423 30 234 30 239 30 423 31 239 31 420 31 420 32 239 32 245 32 420 33 245 33 246 33 230 34 423 34 229 34 229 35 423 35 422 35 229 36 422 36 221 36 221 37 422 37 214 37 214 38 422 38 421 38 214 39 421 39 215 39 215 40 421 40 217 40 217 41 421 41 419 41 217 42 419 42 219 42 417 43 212 43 419 43 419 44 212 44 218 44 419 45 218 45 219 45 199 46 197 46 415 46 415 47 197 47 196 47 415 48 196 48 417 48 417 49 196 49 203 49 417 50 203 50 212 50 199 51 415 51 201 51 201 52 415 52 413 52 201 53 413 53 200 53 200 54 413 54 194 54 194 55 413 55 411 55 194 56 411 56 185 56 185 57 411 57 409 57 185 58 409 58 178 58 178 59 409 59 407 59 178 60 407 60 179 60 405 61 183 61 407 61 407 62 183 62 181 62 407 63 181 63 179 63 167 64 171 64 403 64 403 65 171 65 176 65 403 66 176 66 405 66 405 67 176 67 182 67 405 68 182 68 183 68 167 69 403 69 166 69 166 70 403 70 402 70 166 71 402 71 162 71 84 72 76 72 122 72 122 73 123 73 84 73 84 74 123 74 150 74 84 75 150 75 151 75 151 76 124 76 84 76 126 77 125 77 84 77 84 78 125 78 27 78 84 79 27 79 82 79 118 80 117 80 107 80 107 81 106 81 28 81 120 82 107 82 121 82 121 83 107 83 119 83 118 84 107 84 130 84 130 85 107 85 28 85 130 86 28 86 129 86 41 87 40 87 127 87 31 88 30 88 173 88 114 89 74 89 36 89 28 90 106 90 34 90 108 91 35 91 110 91 110 92 35 92 34 92 110 93 34 93 109 93 109 94 34 94 106 94 108 95 111 95 35 95 36 96 116 96 114 96 78 97 40 97 80 97 80 98 40 98 79 98 40 99 81 99 83 99 125 100 127 100 27 100 27 101 127 101 40 101 27 102 40 102 82 102 82 103 40 103 83 103 79 104 40 104 77 104 77 105 40 105 39 105 77 106 39 106 73 106 73 107 39 107 38 107 73 108 38 108 74 108 74 109 38 109 37 109 74 110 37 110 36 110 173 111 30 111 152 111 152 112 30 112 29 112 152 113 29 113 188 113 188 114 29 114 71 114 188 115 71 115 190 115 190 116 71 116 72 116 190 117 72 117 134 117 173 118 133 118 31 118 31 119 133 119 169 119 31 120 169 120 32 120 32 121 169 121 164 121 32 122 164 122 33 122 33 123 164 123 132 123 33 124 132 124 34 124 34 125 132 125 159 125 34 126 159 126 28 126 28 127 159 127 131 127 28 128 131 128 129 128 127 129 128 129 41 129 41 130 128 130 148 130 41 131 148 131 42 131 42 132 148 132 291 132 42 133 291 133 43 133 43 134 291 134 289 134 43 135 289 135 44 135 44 136 289 136 158 136 44 137 158 137 45 137 158 138 157 138 45 138 45 139 157 139 272 139 45 140 272 140 46 140 46 141 272 141 146 141 46 142 146 142 47 142 47 143 146 143 147 143 47 144 147 144 48 144 48 145 147 145 271 145 48 146 271 146 49 146 49 147 271 147 269 147 49 148 269 148 50 148 269 149 156 149 50 149 50 150 156 150 155 150 50 151 155 151 51 151 51 152 155 152 254 152 51 153 254 153 52 153 52 154 254 154 144 154 52 155 144 155 53 155 53 156 144 156 145 156 53 157 145 157 54 157 145 158 253 158 54 158 54 159 253 159 251 159 54 160 251 160 55 160 55 161 251 161 154 161 55 162 154 162 56 162 56 163 154 163 236 163 56 164 236 164 57 164 57 165 236 165 143 165 57 166 143 166 58 166 143 167 232 167 58 167 58 168 232 168 142 168 58 169 142 169 59 169 59 170 142 170 227 170 59 171 227 171 60 171 60 172 227 172 140 172 60 173 140 173 61 173 61 174 140 174 141 174 61 175 141 175 62 175 141 176 226 176 62 176 62 177 226 177 224 177 62 178 224 178 63 178 64 179 209 179 139 179 64 180 139 180 65 180 65 181 139 181 138 181 65 182 138 182 66 182 66 183 138 183 137 183 66 184 137 184 67 184 67 185 137 185 208 185 67 186 208 186 68 186 208 187 206 187 68 187 68 188 206 188 153 188 68 189 153 189 69 189 69 190 153 190 191 190 69 191 191 191 70 191 70 192 191 192 136 192 70 193 136 193 72 193 72 194 136 194 135 194 72 195 135 195 134 195 71 196 308 196 309 196 71 197 29 197 308 197 308 198 29 198 30 198 308 199 30 199 292 199 30 200 31 200 292 200 292 201 31 201 32 201 292 202 32 202 307 202 32 203 33 203 307 203 307 204 33 204 34 204 307 205 34 205 306 205 34 206 35 206 306 206 306 207 35 207 36 207 306 208 36 208 305 208 36 209 37 209 305 209 305 210 37 210 38 210 305 211 38 211 304 211 38 212 39 212 304 212 304 213 39 213 40 213 304 214 40 214 303 214 40 215 41 215 303 215 303 216 41 216 42 216 303 217 42 217 302 217 42 218 43 218 302 218 302 219 43 219 44 219 302 220 44 220 301 220 44 221 45 221 301 221 301 222 45 222 46 222 301 223 46 223 293 223 46 224 47 224 293 224 293 225 47 225 48 225 293 226 48 226 294 226 48 227 49 227 294 227 294 228 49 228 50 228 294 229 50 229 295 229 50 230 51 230 295 230 295 231 51 231 52 231 295 232 52 232 296 232 52 233 53 233 296 233 296 234 53 234 54 234 296 235 54 235 297 235 54 236 55 236 297 236 297 237 55 237 56 237 297 238 56 238 298 238 56 239 57 239 298 239 298 240 57 240 58 240 298 241 58 241 299 241 58 242 59 242 299 242 299 243 59 243 60 243 299 244 60 244 300 244 60 245 61 245 300 245 300 246 61 246 62 246 300 247 62 247 313 247 62 248 63 248 313 248 313 249 63 249 64 249 313 250 64 250 312 250 64 251 65 251 312 251 312 252 65 252 66 252 312 253 66 253 311 253 66 254 67 254 311 254 311 255 67 255 68 255 311 256 68 256 310 256 68 257 69 257 310 257 310 258 69 258 70 258 310 259 70 259 309 259 309 260 70 260 72 260 309 261 72 261 71 261 399 262 95 262 398 262 398 263 95 263 99 263 398 264 99 264 397 264 93 265 77 265 73 265 93 266 73 266 115 266 115 267 73 267 74 267 115 268 74 268 114 268 77 269 93 269 79 269 79 270 93 270 89 270 85 271 78 271 89 271 89 272 78 272 80 272 89 273 80 273 79 273 81 274 85 274 83 274 83 275 85 275 84 275 83 276 84 276 82 276 95 277 399 277 401 277 95 278 401 278 92 278 84 279 87 279 88 279 86 280 87 280 84 280 85 281 86 281 84 281 85 282 89 282 86 282 86 283 89 283 90 283 86 284 90 284 87 284 87 285 90 285 91 285 87 286 91 286 88 286 88 287 91 287 92 287 89 288 93 288 90 288 90 289 93 289 94 289 90 290 94 290 91 290 91 291 94 291 96 291 91 292 96 292 92 292 92 293 96 293 95 293 93 294 115 294 97 294 93 295 97 295 94 295 94 296 97 296 98 296 94 297 98 297 96 297 96 298 98 298 99 298 96 299 99 299 95 299 105 300 400 300 103 300 103 301 400 301 397 301 103 302 397 302 99 302 101 303 112 303 107 303 101 304 107 304 104 304 115 305 113 305 97 305 97 306 113 306 100 306 97 307 100 307 98 307 98 308 100 308 102 308 98 309 102 309 99 309 99 310 102 310 103 310 113 311 112 311 100 311 100 312 112 312 101 312 100 313 101 313 102 313 102 314 101 314 104 314 102 315 104 315 103 315 103 316 104 316 105 316 105 317 104 317 107 317 106 318 107 318 109 318 109 319 107 319 112 319 111 320 108 320 112 320 112 321 108 321 110 321 112 322 110 322 109 322 116 323 113 323 115 323 116 324 115 324 114 324 268 325 274 325 276 325 250 326 256 326 258 326 223 327 228 327 231 327 205 328 211 328 213 328 187 329 193 329 195 329 158 330 289 330 290 330 272 331 157 331 273 331 156 332 269 332 270 332 254 333 155 333 255 333 154 334 251 334 252 334 153 335 206 335 207 335 173 336 152 336 174 336 164 337 169 337 170 337 163 338 117 338 160 338 160 339 117 339 118 339 160 340 118 340 130 340 119 341 163 341 121 341 121 342 163 342 161 342 121 343 161 343 120 343 122 344 284 344 123 344 123 345 284 345 286 345 123 346 286 346 150 346 149 347 124 347 151 347 125 348 126 348 127 348 127 349 126 349 149 349 127 350 149 350 128 350 128 351 149 351 148 351 159 352 160 352 131 352 131 353 160 353 130 353 131 354 130 354 129 354 159 355 132 355 165 355 165 356 132 356 164 356 169 357 133 357 175 357 175 358 133 358 173 358 190 359 134 359 187 359 187 360 134 360 135 360 187 361 135 361 193 361 193 362 135 362 136 362 193 363 136 363 191 363 208 364 137 364 205 364 205 365 137 365 138 365 205 366 138 366 211 366 211 367 138 367 139 367 211 368 139 368 209 368 227 369 228 369 140 369 140 370 228 370 223 370 140 371 223 371 141 371 141 372 223 372 226 372 227 373 142 373 233 373 233 374 142 374 232 374 232 375 143 375 238 375 238 376 143 376 236 376 254 377 256 377 144 377 144 378 256 378 250 378 144 379 250 379 145 379 145 380 250 380 253 380 272 381 274 381 146 381 146 382 274 382 268 382 146 383 268 383 147 383 147 384 268 384 271 384 291 385 148 385 288 385 288 386 148 386 149 386 288 387 149 387 286 387 286 388 149 388 151 388 286 389 151 389 150 389 180 390 184 390 174 390 152 391 188 391 174 391 174 392 188 392 189 392 174 393 189 393 180 393 180 394 189 394 186 394 180 395 186 395 178 395 178 396 186 396 185 396 198 397 202 397 192 397 191 398 153 398 192 398 192 399 153 399 207 399 192 400 207 400 198 400 198 401 207 401 204 401 198 402 204 402 196 402 196 403 204 403 203 403 216 404 220 404 210 404 210 405 225 405 216 405 216 406 225 406 222 406 243 407 247 407 237 407 236 408 154 408 237 408 237 409 154 409 252 409 237 410 252 410 243 410 243 411 252 411 249 411 243 412 249 412 241 412 241 413 249 413 248 413 261 414 265 414 255 414 155 415 156 415 255 415 255 416 156 416 270 416 255 417 270 417 261 417 261 418 270 418 267 418 261 419 267 419 259 419 259 420 267 420 266 420 279 421 283 421 273 421 157 422 158 422 273 422 273 423 158 423 290 423 273 424 290 424 279 424 279 425 290 425 287 425 168 426 166 426 162 426 159 427 165 427 160 427 160 428 165 428 168 428 160 429 168 429 163 429 163 430 168 430 162 430 163 431 162 431 161 431 172 432 171 432 167 432 164 433 170 433 165 433 165 434 170 434 172 434 165 435 172 435 168 435 168 436 172 436 167 436 168 437 167 437 166 437 169 438 175 438 170 438 170 439 175 439 177 439 170 440 177 440 172 440 172 441 177 441 176 441 172 442 176 442 171 442 173 443 174 443 175 443 175 444 174 444 184 444 175 445 184 445 177 445 177 446 184 446 182 446 177 447 182 447 176 447 178 448 179 448 180 448 180 449 179 449 181 449 180 450 181 450 184 450 184 451 181 451 183 451 184 452 183 452 182 452 194 453 185 453 195 453 195 454 185 454 186 454 195 455 186 455 187 455 187 456 186 456 189 456 187 457 189 457 190 457 190 458 189 458 188 458 191 459 192 459 193 459 193 460 192 460 202 460 193 461 202 461 195 461 195 462 202 462 200 462 195 463 200 463 194 463 196 464 197 464 198 464 198 465 197 465 199 465 198 466 199 466 202 466 202 467 199 467 201 467 202 468 201 468 200 468 212 469 203 469 213 469 213 470 203 470 204 470 213 471 204 471 205 471 205 472 204 472 207 472 205 473 207 473 208 473 208 474 207 474 206 474 209 475 210 475 211 475 211 476 210 476 220 476 211 477 220 477 213 477 213 478 220 478 218 478 213 479 218 479 212 479 221 480 214 480 222 480 222 481 214 481 215 481 222 482 215 482 216 482 216 483 215 483 217 483 216 484 217 484 220 484 220 485 217 485 219 485 220 486 219 486 218 486 229 487 221 487 231 487 231 488 221 488 222 488 231 489 222 489 223 489 223 490 222 490 225 490 223 491 225 491 226 491 226 492 225 492 224 492 235 493 234 493 230 493 227 494 233 494 228 494 228 495 233 495 235 495 228 496 235 496 231 496 231 497 235 497 230 497 231 498 230 498 229 498 232 499 238 499 233 499 233 500 238 500 240 500 233 501 240 501 235 501 235 502 240 502 239 502 235 503 239 503 234 503 236 504 237 504 238 504 238 505 237 505 247 505 238 506 247 506 240 506 240 507 247 507 245 507 240 508 245 508 239 508 241 509 242 509 243 509 243 510 242 510 244 510 243 511 244 511 247 511 247 512 244 512 246 512 247 513 246 513 245 513 257 514 248 514 258 514 258 515 248 515 249 515 258 516 249 516 250 516 250 517 249 517 252 517 250 518 252 518 253 518 253 519 252 519 251 519 254 520 255 520 256 520 256 521 255 521 265 521 256 522 265 522 258 522 258 523 265 523 263 523 258 524 263 524 257 524 259 525 260 525 261 525 261 526 260 526 262 526 261 527 262 527 265 527 265 528 262 528 264 528 265 529 264 529 263 529 275 530 266 530 276 530 276 531 266 531 267 531 276 532 267 532 268 532 268 533 267 533 270 533 268 534 270 534 271 534 271 535 270 535 269 535 272 536 273 536 274 536 274 537 273 537 283 537 274 538 283 538 276 538 276 539 283 539 281 539 276 540 281 540 275 540 285 541 277 541 287 541 287 542 277 542 278 542 287 543 278 543 279 543 279 544 278 544 280 544 279 545 280 545 283 545 283 546 280 546 282 546 283 547 282 547 281 547 284 548 285 548 286 548 286 549 285 549 287 549 286 550 287 550 288 550 288 551 287 551 290 551 288 552 290 552 291 552 291 553 290 553 289 553 301 554 293 554 389 554 308 555 292 555 363 555 363 556 292 556 307 556 389 557 293 557 385 557 385 558 293 558 294 558 385 559 294 559 380 559 380 560 294 560 295 560 380 561 295 561 377 561 377 562 295 562 296 562 377 563 296 563 373 563 373 564 296 564 297 564 373 565 297 565 370 565 370 566 297 566 298 566 370 567 298 567 390 567 390 568 298 568 299 568 390 569 299 569 393 569 393 570 299 570 300 570 393 571 300 571 313 571 389 572 387 572 301 572 301 573 387 573 365 573 301 574 365 574 302 574 302 575 365 575 342 575 302 576 342 576 303 576 303 577 342 577 345 577 303 578 345 578 304 578 304 579 345 579 351 579 304 580 351 580 305 580 305 581 351 581 354 581 305 582 354 582 306 582 306 583 354 583 357 583 306 584 357 584 307 584 307 585 357 585 359 585 307 586 359 586 363 586 363 587 337 587 308 587 308 588 337 588 315 588 308 589 315 589 309 589 309 590 315 590 319 590 309 591 319 591 310 591 310 592 319 592 324 592 310 593 324 593 311 593 311 594 324 594 327 594 311 595 327 595 312 595 312 596 327 596 330 596 312 597 330 597 313 597 313 598 330 598 334 598 313 599 334 599 393 599 317 600 314 600 319 600 314 601 315 601 319 601 314 602 316 602 315 602 316 603 337 603 315 603 316 604 336 604 337 604 317 605 319 605 318 605 319 606 320 606 318 606 319 607 320 607 324 607 324 608 321 608 320 608 324 609 322 609 321 609 322 610 324 610 323 610 324 611 325 611 323 611 324 612 327 612 325 612 325 613 327 613 326 613 327 614 329 614 326 614 327 615 330 615 329 615 333 616 328 616 334 616 328 617 330 617 334 617 328 618 331 618 330 618 331 619 329 619 330 619 392 620 393 620 332 620 332 621 393 621 334 621 332 622 335 622 334 622 335 623 333 623 334 623 361 624 338 624 363 624 364 625 365 625 339 625 365 626 340 626 339 626 365 627 342 627 340 627 340 628 342 628 341 628 342 629 343 629 341 629 342 630 345 630 343 630 343 631 345 631 344 631 345 632 346 632 344 632 345 633 351 633 346 633 346 634 351 634 347 634 351 635 348 635 347 635 351 636 349 636 348 636 349 637 351 637 350 637 351 638 352 638 350 638 351 639 354 639 352 639 352 640 354 640 353 640 354 641 355 641 353 641 354 642 357 642 355 642 355 643 357 643 356 643 357 644 358 644 356 644 357 645 358 645 359 645 359 646 358 646 360 646 359 647 360 647 363 647 363 648 360 648 362 648 363 649 362 649 361 649 372 650 370 650 373 650 390 651 366 651 391 651 366 652 390 652 367 652 390 653 368 653 367 653 390 654 370 654 368 654 370 655 369 655 368 655 370 656 372 656 369 656 375 657 371 657 377 657 371 658 373 658 377 658 371 659 374 659 373 659 374 660 372 660 373 660 375 661 377 661 376 661 377 662 378 662 376 662 377 663 380 663 378 663 378 664 380 664 379 664 380 665 381 665 379 665 380 666 385 666 381 666 385 667 382 667 381 667 385 668 383 668 382 668 383 669 385 669 384 669 385 670 386 670 384 670 385 671 389 671 386 671 389 672 388 672 386 672 389 673 388 673 387 673 559 708 557 708 593 708 595 708 6 787 26 787 3 787 6 788 22 788 26 788 8 789 22 789 6 789 8 790 20 790 22 790 11 791 20 791 8 791 11 792 18 792 20 792 25 793 0 793 2 793 1 794 24 794 4 794 9 795 19 795 10 795 7 796 19 796 9 796 7 797 21 797 19 797 5 798 21 798 7 798 5 799 23 799 21 799 23 800 4 800 24 800 5 801 4 801 23 801 10 802 15 802 12 802 18 803 13 803 14 803 11 804 13 804 18 804 3 805 25 805 2 805 26 806 25 806 3 806 16 807 15 807 17 807 162 808 13 808 161 808 161 809 13 809 75 809 284 810 76 810 16 810 284 811 16 811 285 811 285 812 16 812 17 812 285 813 17 813 277 813 277 814 17 814 404 814 162 815 402 815 14 815 162 816 14 816 13 816 4 817 76 817 84 817 84 818 124 818 126 818 107 819 117 819 119 819 75 820 2 820 120 820 120 821 2 821 107 821 35 822 111 822 36 822 36 823 111 823 116 823 78 824 81 824 40 824 63 825 224 825 209 825 63 826 209 826 64 826 75 827 13 827 11 827 2 828 75 828 3 828 3 829 75 829 11 829 3 830 11 830 6 830 6 831 11 831 8 831 12 832 15 832 16 832 16 833 76 833 12 833 12 834 76 834 4 834 12 835 4 835 9 835 9 836 4 836 5 836 9 837 5 837 7 837 9 838 10 838 12 838 85 839 81 839 78 839 92 840 401 840 1 840 92 841 1 841 88 841 88 842 1 842 4 842 88 843 4 843 84 843 105 844 107 844 2 844 2 845 0 845 105 845 105 846 0 846 400 846 111 847 112 847 113 847 111 848 113 848 116 848 209 849 224 849 225 849 163 850 119 850 117 850 126 851 124 851 149 851 210 852 209 852 225 852 550 859 548 859 407 859 409 859 544 860 543 860 402 860 403 860 545 868 534 868 570 868 581 868 423 869 420 869 561 869 564 869 590 919 588 919 552 919 554 919 558 920 556 920 415 920 417 920 530 921 385 921 380 921 530 922 337 922 363 922 530 923 354 923 351 923 530 924 390 924 393 924 530 925 334 925 330 925 530 926 363 926 359 926 530 927 327 927 324 927 530 928 357 928 354 928 530 929 324 929 319 929 530 930 345 930 342 930 530 931 359 931 357 931 530 932 377 932 373 932 530 933 319 933 315 933 389 934 530 934 387 934 530 935 365 935 387 935 530 936 373 936 370 936 530 937 330 937 327 937 365 938 530 938 342 938 530 939 380 939 377 939 530 940 393 940 334 940 337 941 530 941 315 941 530 942 351 942 345 942 390 943 530 943 370 943 530 944 389 944 385 944 395 947 574 947 538 947 531 947 414 948 412 948 553 948 555 948 1 949 401 949 542 949 531 949 536 950 535 950 19 950 21 950 560 951 558 951 417 951 419 951 541 952 539 952 397 952 400 952 25 953 26 953 644 953 535 954 532 954 10 954 19 954 476 955 477 955 567 955 566 955 422 956 423 956 564 956 563 956 406 957 404 957 545 957 547 957 552 958 550 958 409 958 411 958 416 959 414 959 555 959 557 959 531 960 538 960 24 960 1 960 537 961 536 961 21 961 23 961 562 962 560 962 419 962 421 962 401 963 399 963 540 963 542 963 408 964 406 964 547 964 549 964 554 965 552 965 411 965 413 965 400 966 0 966 479 966 541 966 533 967 534 967 17 967 15 967 418 968 416 968 557 968 559 968 23 969 24 969 538 969 537 969 475 970 476 970 566 970 565 970 421 971 422 971 563 971 562 971 546 972 544 972 403 972 405 972 404 973 17 973 534 973 545 973 410 974 408 974 549 974 551 974 556 975 554 975 413 975 415 975 532 976 533 976 15 976 10 976 420 977 418 977 559 977 561 977 409 978 475 978 565 978 550 978 477 979 478 979 568 979 567 979 548 980 546 980 405 980 407 980 412 981 410 981 551 981 553 981 587 982 585 982 624 982 626 982 605 983 608 983 569 983 394 983 602 984 603 984 642 984 641 984 621 985 619 985 580 985 582 985 598 986 599 986 638 986 637 986 595 987 593 987 632 987 634 987 581 988 570 988 609 988 620 988 573 989 572 989 536 989 537 989 562 990 563 990 599 990 598 990 582 991 580 991 544 991 546 991 566 992 567 992 603 992 602 992 394 993 569 993 533 993 532 993 551 994 549 994 585 994 587 994 396 995 577 995 541 995 479 995 592 996 590 996 554 996 556 996 561 997 559 997 595 997 597 997 537 998 538 998 574 998 573 998 584 999 582 999 546 999 548 999 571 1000 394 1000 532 1000 535 1000 553 1001 551 1001 587 1001 589 1001 594 1002 592 1002 556 1002 558 1002 564 1003 561 1003 597 1003 600 1003 580 1004 579 1004 543 1004 544 1004 565 1005 566 1005 602 1005 601 1005 569 1006 570 1006 534 1006 533 1006 586 1007 584 1007 548 1007 550 1007 555 1008 553 1008 589 1008 591 1008 550 1009 565 1009 601 1009 586 1009 596 1010 594 1010 558 1010 560 1010 577 1011 575 1011 539 1011 541 1011 644 1012 22 1012 20 1012 563 1013 564 1013 600 1013 599 1013 547 1014 545 1014 581 1014 583 1014 567 1015 568 1015 604 1015 603 1015 572 1016 571 1016 535 1016 536 1016 588 1017 586 1017 550 1017 552 1017 531 1018 542 1018 578 1018 395 1018 557 1019 555 1019 591 1019 593 1019 598 1020 596 1020 560 1020 562 1020 542 1021 540 1021 576 1021 578 1021 549 1022 547 1022 583 1022 585 1022 607 1023 616 1023 577 1023 396 1023 631 1024 629 1024 590 1024 592 1024 597 1025 595 1025 634 1025 636 1025 623 1026 621 1026 582 1026 584 1026 610 1027 605 1027 394 1027 571 1027 589 1028 587 1028 626 1028 628 1028 573 1029 574 1029 613 1029 612 1029 633 1030 631 1030 592 1030 594 1030 611 1031 610 1031 571 1031 572 1031 600 1032 597 1032 636 1032 639 1032 619 1033 618 1033 579 1033 580 1033 601 1034 602 1034 641 1034 640 1034 608 1035 609 1035 570 1035 569 1035 625 1036 623 1036 584 1036 586 1036 591 1037 589 1037 628 1037 630 1037 612 1038 611 1038 572 1038 573 1038 586 1039 601 1039 640 1039 625 1039 635 1040 633 1040 594 1040 596 1040 616 1041 614 1041 575 1041 577 1041 599 1042 600 1042 639 1042 638 1042 583 1043 581 1043 620 1043 622 1043 603 1044 604 1044 643 1044 642 1044 627 1045 625 1045 586 1045 588 1045 607 1046 25 1046 644 1046 395 1047 578 1047 617 1047 606 1047 593 1048 591 1048 630 1048 632 1048 637 1049 635 1049 596 1049 598 1049 578 1050 576 1050 615 1050 617 1050 585 1051 583 1051 622 1051 624 1051 606 1052 613 1052 574 1052 395 1052 629 1053 627 1053 588 1053 590 1053 396 1054 25 1054 607 1054 644 1055 615 1055 398 1055 398 1056 615 1056 576 1056 614 1057 398 1057 575 1057 398 1058 540 1058 399 1058 644 1059 398 1059 614 1059 398 1060 576 1060 540 1060 539 1061 398 1061 397 1061 575 1063 398 1063 539 1063 622 1065 620 1065 644 1065 638 1066 639 1066 644 1066 644 1067 614 1067 616 1067 644 1068 633 1068 635 1068 625 1069 644 1069 640 1069 644 1070 611 1070 612 1070 630 1071 628 1071 644 1071 642 1072 643 1072 644 1072 644 1073 625 1073 627 1073 479 1074 25 1074 396 1074 606 1075 617 1075 644 1075 632 1076 630 1076 644 1076 644 1077 635 1077 637 1077 617 1078 615 1078 644 1078 624 1079 622 1079 644 1079 644 1080 613 1080 606 1080 644 1081 627 1081 629 1081 0 1082 25 1082 479 1082 620 1083 609 1083 644 1083 634 1084 632 1084 644 1084 637 1085 638 1085 644 1085 644 1086 619 1086 621 1086 641 1087 642 1087 644 1087 644 1088 608 1088 605 1088 626 1089 624 1089 644 1089 644 1090 616 1090 607 1090 644 1091 629 1091 631 1091 636 1092 634 1092 644 1092 644 1093 621 1093 623 1093 644 1094 605 1094 610 1094 628 1095 626 1095 644 1095 612 1096 613 1096 644 1096 644 1097 631 1097 633 1097 644 1098 610 1098 611 1098 639 1099 636 1099 644 1099 644 1100 618 1100 619 1100 640 1101 641 1101 644 1101 644 1102 609 1102 608 1102 644 1103 623 1103 625 1103 26 1128 22 1128 644 1128 644 1129 18 1129 20 1129 543 1130 14 1130 402 1130 478 1131 14 1131 568 1131 568 1132 14 1132 604 1132 579 1133 14 1133 543 1133 604 1134 14 1134 643 1134 618 1135 14 1135 579 1135 643 1136 14 1136 644 1136 644 1137 14 1137 618 1137 18 1138 14 1138 644 1138

+ + + + + 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 +

453 674 526 674 528 674 453 675 503 675 524 675 453 676 449 676 503 676 503 677 449 677 466 677 529 678 528 678 520 678 523 679 521 679 522 679 483 680 481 680 468 680 467 681 469 681 457 681 430 682 432 682 439 682 439 683 441 683 430 683 466 684 456 684 468 684 426 685 424 685 441 685 441 686 424 686 430 686 520 687 514 687 522 687 497 688 524 688 503 688 498 689 515 689 500 689 505 690 516 690 501 690 501 691 516 691 508 691 501 692 508 692 491 692 491 693 508 693 493 693 491 694 493 694 489 694 489 695 493 695 487 695 516 696 518 696 508 696 508 697 518 697 510 697 508 698 510 698 493 698 493 699 510 699 495 699 493 700 495 700 487 700 487 701 495 701 485 701 518 702 522 702 510 702 510 703 522 703 514 703 510 704 514 704 495 704 495 705 514 705 499 705 495 706 499 706 485 706 485 707 499 707 483 707 472 709 487 709 485 709 472 710 485 710 470 710 470 711 485 711 483 711 470 712 483 712 468 712 506 713 507 713 517 713 516 714 505 714 506 714 506 715 517 715 516 715 516 716 517 716 519 716 516 717 519 717 518 717 518 718 519 718 523 718 518 719 523 719 522 719 464 720 472 720 460 720 464 721 460 721 451 721 451 722 460 722 444 722 451 723 444 723 446 723 446 724 444 724 436 724 472 725 470 725 460 725 460 726 470 726 458 726 460 727 458 727 444 727 444 728 458 728 442 728 444 729 442 729 436 729 436 730 442 730 434 730 470 731 468 731 458 731 458 732 468 732 456 732 458 733 456 733 442 733 442 734 456 734 439 734 442 735 439 735 434 735 434 736 439 736 432 736 490 737 488 737 492 737 492 738 488 738 494 738 492 739 494 739 502 739 502 740 494 740 509 740 502 741 509 741 507 741 507 742 509 742 517 742 488 743 486 743 494 743 494 744 486 744 496 744 494 745 496 745 509 745 509 746 496 746 511 746 509 747 511 747 517 747 517 748 511 748 519 748 486 749 484 749 496 749 496 750 484 750 500 750 496 751 500 751 511 751 511 752 500 752 515 752 511 753 515 753 519 753 519 754 515 754 523 754 447 755 446 755 436 755 437 756 448 756 447 756 447 757 436 757 437 757 437 758 436 758 434 758 437 759 434 759 435 759 435 760 434 760 432 760 435 761 432 761 433 761 480 762 474 762 473 762 488 763 490 763 480 763 480 764 473 764 488 764 488 765 473 765 471 765 488 766 471 766 486 766 486 767 471 767 469 767 486 768 469 768 484 768 448 769 437 769 452 769 452 770 437 770 445 770 452 771 445 771 465 771 465 772 445 772 461 772 465 773 461 773 474 773 474 774 461 774 473 774 437 775 435 775 445 775 445 776 435 776 443 776 445 777 443 777 461 777 461 778 443 778 459 778 461 779 459 779 473 779 473 780 459 780 471 780 435 781 433 781 443 781 443 782 433 782 440 782 443 783 440 783 459 783 459 784 440 784 457 784 459 785 457 785 471 785 471 786 457 786 469 786 529 853 527 853 453 853 450 854 429 854 453 854 528 855 529 855 453 855 429 856 427 856 453 856 525 857 504 857 453 857 504 858 450 858 453 858 453 861 425 861 424 861 453 862 428 862 449 862 453 863 426 863 428 863 453 864 424 864 426 864 527 865 525 865 453 865 453 866 524 866 526 866 427 867 425 867 453 867 503 870 466 870 481 870 529 871 520 871 521 871 424 872 425 872 431 872 424 873 431 873 430 873 469 874 467 874 484 874 484 875 467 875 482 875 522 876 521 876 520 876 468 877 481 877 466 877 432 878 430 878 433 878 433 879 430 879 431 879 440 880 438 880 457 880 457 881 438 881 454 881 457 882 454 882 467 882 438 883 440 883 431 883 431 884 440 884 433 884 438 885 431 885 425 885 438 886 425 886 427 886 438 887 427 887 454 887 454 888 427 888 429 888 454 889 429 889 450 889 454 890 450 890 467 890 456 891 455 891 439 891 439 892 455 892 441 892 455 893 456 893 466 893 455 894 466 894 449 894 455 895 449 895 428 895 455 896 428 896 441 896 441 897 428 897 426 897 481 898 483 898 499 898 514 899 512 899 499 899 499 900 512 900 497 900 499 901 497 901 481 901 512 902 514 902 520 902 512 903 520 903 528 903 512 904 528 904 526 904 512 905 526 905 497 905 497 906 526 906 524 906 497 907 503 907 481 907 525 908 498 908 504 908 504 909 498 909 482 909 529 910 521 910 527 910 527 911 521 911 513 911 498 912 525 912 527 912 498 913 527 913 513 913 498 914 513 914 515 914 515 915 513 915 523 915 523 916 513 916 521 916 498 917 500 917 482 917 482 918 500 918 484 918 462 945 489 945 487 945 462 946 487 946 472 946 472 1062 463 1062 462 1062 463 1064 472 1064 464 1064 645 1104 450 1104 504 1104 504 1105 482 1105 645 1105 482 1106 467 1106 645 1106 467 1107 450 1107 645 1107 646 1108 452 1108 465 1108 489 1109 462 1109 646 1109 646 1110 506 1110 505 1110 646 1111 465 1111 474 1111 463 1112 464 1112 646 1112 447 1113 448 1113 646 1113 646 1114 492 1114 502 1114 646 1115 490 1115 492 1115 646 1116 502 1116 507 1116 646 1117 448 1117 452 1117 646 1118 507 1118 506 1118 446 1119 447 1119 646 1119 462 1120 463 1120 646 1120 501 1121 491 1121 646 1121 505 1122 501 1122 646 1122 451 1123 446 1123 646 1123 646 1124 480 1124 490 1124 646 1125 474 1125 480 1125 491 1126 489 1126 646 1126 464 1127 451 1127 646 1127

+ + + 1 + + + + + + 7.481132 -6.50764 5.343665 + 0 0 1 46.69194 + 0 1 0 0.619768 + 1 0 0 63.5593 + 1 1 1 + + + + 4.076245 1.005454 5.903862 + 0 0 1 106.9363 + 0 1 0 3.163707 + 1 0 0 37.26105 + 1 1 1 + + + + 0 0 0 + 0 0 1 0 + 0 1 0 0 + 1 0 0 90.00001 + 1 1 1 + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/gazebo_plugins/test/diff_drive/meshes/p3dx/Coordinates b/gazebo_plugins/test/diff_drive/meshes/p3dx/Coordinates new file mode 100644 index 000000000..6fba9d215 --- /dev/null +++ b/gazebo_plugins/test/diff_drive/meshes/p3dx/Coordinates @@ -0,0 +1,20 @@ +========================================= +Location Coordinates (x,y,z) +========================================= + +Chassis : -0.045 0 0.148 +Top : -0.045 0 0.234 + +Swivel : -0.185 0 0.055 +Center Hubcap : -0.211 0 0.037 +Center Wheel : -0.211 0 0.038 + +Right Hubcap : 0 0.158 0.091 +Right Wheel : 0 0.158 0.091 + +Left Hubcap : 0 -0.158 0.091 +Left Wheel : 0 -0.158 0.091 + +Front Sonar : -0.2 0 0.209 +Back Sonar : 0.109 0 0.209 + diff --git a/gazebo_plugins/test/diff_drive/meshes/p3dx/back_rim.stl b/gazebo_plugins/test/diff_drive/meshes/p3dx/back_rim.stl new file mode 100644 index 0000000000000000000000000000000000000000..09587a53524cc452d6be26ba9db18babc677dd54 GIT binary patch literal 1094884 zcmeF)d(>T3eeQ8;5sC!@D5WYY3MhJxmm-h^_5^FVOF(Wfgp!0H!En(eTqH=nRG~#_ zv0e~RffNf>)Ji>qMfQt=2NVxpTB=lgtRi~ADlH;bs(>fMzdpa^crxZ5F|T7dnm-tm zy`JxU=3HxDesiw5_xsY5UcKbBv$s3^h=aCUe&!h~&OBqg6Hie5%Au=KRu zw|n8T<*!=$!c$LPw)};sEkFH*r=PLp6{np3!ZS~Q#q-zv@AFrj^2!&UaoU-$KIMfg zc3yLJ&4126a)a{k_5U)zt-NltcES5skLJE<-}b@dXSd7F-+$!i+ifS}6$#UskT$)p zgzhO_J(9}LD`9ze{q|HlYm@6nhu?Tb=e?3JT|K;V+xCmv-DmAHdd=tNjr_cikgkMR zuKoIP?N#5|c(ng1&%D9U`v~buc;%Ew7q$)lb^p=jAK!26=Y529CA{*Szn*Hx-TsNu z<#Sh${k)Hmu7p=sTsFU*y!EVh#GzM={k)Hmu7p><^+)@*oknxo`47Ex?B{)ibS1p9 z@m2HMBdhmo^NydI>*sxhbS1oEFY@y~Lb?)Ou|N8GA0b@{ub8i&_Yu;S@QV5Rc^@HN z39pzwVAF%!Evq+eQy;l>R0-WppZxg!+UnQt(w5z^*MP7;@A=)u?fk!f+UPH~+WLk& z?qA$q_w8***YEYc>#B#N%$rVH*k1KtM~=R8=j_qfj$PO;{QaXwC$HFJ;NjT!jlY^| z$K3OY(c&jvJo>^Lr`nQZZW!(Of=dR(IlsMMyYjqU+9!9OGq#0);O>3f%6)cib06Gw zJSf#I2ev=h=NWB-tyUGnoGNk8(R;W5{Oft`vq#OJYYYF}%h!DVkMr7De|PQNO5FMX z=C|2rZq}ZC`bDL0UpGC!EnT#E+i>|sgHk>9?T59!H~4>}-M(K5bE+P*j+tt=Zh7PA zXIEcT#;9lSJJqiG(T$_`9r*5nha;KvLoYt2z2p^d8{M5xX?@=~YnS%e{d85SxjW1mbVO|o)K0LpDa?8!yhc{VUueF5cRO{j0Rh2z#;nMke zNT|yF!}-o$dmDYT)+fCSxrKfy{me7uk5uyKH-qI;LW?WWw#6?00~u}x%=R@=#;tbve)lFxVt_z zTgI~ATHOBkLpP2V&$@UmgsP*ywWxjmly^OUJ}&IdPdQy&poFU1 z_g>ac{p^mTKYhnGH{^C@t9;jn2es=yctAVo+lLRLCEJy%+^&p@e46t?lQuNppc?~lFq4ewdn7L1M`-F(;FLA#Pr<$bI7$~ejy;GTCcZSyWXe)O^nhw+t! zsvRy_+HPHW{OFpqhlC@UEhe`T2~{hX9@5_Z#@*Uu_Y6k{qZdc#2R1#XowUhb+xc8`ICOOR zP5TY*l`9A%q`$jrQQPM6pB}yMkm346Le-@=ENZj2|J3Mb9~$;S%kG`dcUMA{b*g>P zD79}MlyDwA{Mv)sNB?D?_PUiH9gJ}1YwyZENwH}TPedCpc<5eC@9M6p`PPy^m z_J8-@y`BHqO9!n)LY3out%o&tX2`RFgsOb6jMZ)VxwT8E`q*zTZnuB^#?fVat{&9f zvYWPqwdP5c38RceR}TsE&2fqIk}H-syz}_>u`}K|x@U{wu7QLq=c`&O+mdUwpFVPA z+y3cWwj-vl7~EY6RkpF};fl^^AV&Zas;+wAnD&t^|82C}1Hv3Uj6rL z#*&Zh)4p`qfrGKc7}ptYEwdV{YE{O*^v3Y-82K5x|kbh&&Uj^s9t3fIT(eT?tkC zRSzQ;d#W=-CRF+SD`C&EN97quLY02i!yON2efv!&ROwdDx-T_xc#vb_9zK+s)ViG7Vhj*31fBppgp(}&V3T*R0&6CpIROh zea^N%N7U+JyRt2ru7o|xK4H2NUNN0LywXRgGHp(^R9=~!khaBC53fv4NZVqnhgT*i zr0px!!z+^$()Ri4;g!h=={#O}WpY9~_b9JSPDtl|?v=?2=^Vp(WpYB=-c_HuSNaHh zQA$^jr1JAhSgMp>S3-Bw`elzkBCSlbMfx2id#!}&>fx2iM;{5()x#^3kD?N$tA|%6pA95TR}ZgDKC4NXt{z^Qe0G*F zT|K-qd6Xexx_WrUUgYO}gmfjmV(;?vK0>;W_*Hu7ZrbQPm&%g30;q(2(A9*ij7m5% zxMFc7Qwhf_cc$F^s)Xw$Tczu(O1LJIFsDkm?z8o|POOA$OZ%g1(Mq^}l`yADxJGtd zaZOzbRno58E8&>v9)LR*mGC*+GTeWtgssoLjNB3}mHQ>RRJK>^;a*KGmA%M)pWLHV zxwn)FuSmGBRZHc)vV`W8302v{7Or1@9ulf@|FDJI2ko1+9ulf@kFvE(+taFtgsR-5 zY>CpHJ&O18w&PkVM^VRMdsik@**7cUxT{~^NUh3IyL#AO^~+&8 zfG2q}p~`1d38OpPac+qcsyxqBJ)HH8)je62300oUs)T*Q=*1IanNVe4sf6R8(Y+_# zGNH=xu@a8@wz1qwBvg5NuX-4AI|UC1?;Q0023df1j+GZF%J-q0046^{|!n&4UuogSlN9SvbPwC|p97?YP#%7H6-`GmeBR=d0@BNM`G^ zRc1m}zE_UUJ~d~b>LH;jw_RI?tHwO5NvP7VmdaV!6=c4<5~}p89?k%+SaK9Dp~~l9 zJ&cfC(YXrCgev_i;RxW!U@ypoD*Y;9l-k#8C2X&`58A@}Mo|gdtA4c}5=JljRl+M0 z_M1$ovV@gz?sN2UjH!eXi>)%ZM4y@?QNCA3#r8paS}v9K$fdI1xq`^8-5#Y%x|YhQ z$QEvYtb{#E!kj8$Yqy0v`&7dC+&*Xzu7q=+ggI5h8Ne|(kGqcNKIc4&+A?gfj={B5 zwnST>=}OoK?M0?5;T6-_!z+D+D%0jvOXZcx32EC{_3+B%gtTp}dU$1WLfYO{J-jkG zA?+wpJ-jkGA)QA!uS`xz=N{#i$qDJ)&%H7^A)QAbuS`xz+q>#B_evjOFG}g^kyL(O z2}_mI>q_WuTEFbkN4U;(kIZ!SNQ9qP!n|Cun(iaasS@U8Nlf<<=F~^zQu(A(x_Ts) zpI5@YjFetiLifIQrOLE9)lzvSrPr0PBq?1zlFH93VcGk}D^;e=sd}jD8+TQiHmB;L zs&D>KW!jvohpN7LP?c$OsvfHP=5tl1&8d2*>WfQMnKq~Dp{g&wQf1nls)wq+I8l{p zbE+Py>_vWF300>12z!xq^-yIx6JF^fRGBuXTB^y{QIoILCSMOuzUG{KT|4>O*;Tcz zFR#xh-vgL@pTQjn`(574n0&uv@;w=Mqx$xJCf{4KCHC!KS*pH0vdQ=1>`{GteG+~9 ziR-p!Ir+%o_@Q?lOMD&@c@&jUWxuH&_BH#UEk6^g>}i#-=Sbw4Q$m$Jt$Ns_Byt2G zp~`k#Jsfw9UUIY~p~`)~>fuN&k)u8dRqjSs565;1+i@mT*&i#B-oDw6Goi|ns1j)$ z^x0%WmAk5ya8`4t+1`~2Rran*q_?`>g-odO&2J@KRogOb3z<-rSKH|=dmb4iRQV>p zmMWbVuvKP4l`HQ`q*D*J=}f5dWJD#>>5beHB~*FBqk5!MD7KhPsLJiiC*>0~imDzG zsyw4piF7K+SRxauyoZ%Yr<;s$GNH;7P?fMX*h=yUC!xx-U)3X>HZu;&gep&@RU)05 zGoH_cDo@r`!W}y2DCfjXsPcqhCDJLzJUdIM@}y(+NT)H4I5MHi6PuMtr%LntS_xI2 zJgpwyMenY&XeLy7M=Fu#!5qCxsPgP^^|0sIql}z0p-S&cr18p8A``0eSdvbU8*x+* z302M#m2lK|Ht;TFLRG$3wrX3uF?{uqP~~?7Dv|18o%#rK>LZLeGNCHRCFxrtdBq~3 z%Gs`#Dt+4}N3Rm9Y;V=WncfJ%RbM7l`CXt&I085_7{g~mmEUQqgtM2knrrGzs4@<& zMEW+EyE~as<=j;XXD??pSC*Mj<#**OVehopx*w1URkpWExH56Yl2@q`s*IwlhrQEY zYmAZ!RYo|K@Gg3HUAJXIm8-r=*s5*qu6Hw`%4o0>u1s99xGRzgRrZ2P7_k`1xVw`H zRgRUFNYC7T#!RSk^;!u>GDo<)<0YZW*sgjQ(HrUK9Y_gPu8^yT?aG#sX9EdUIW92* zFxIlY)lx~Q%CV?5bQNa&SUn_E<(R<{z>z4AJ`$>Ip|w=T?T#yXWROs$clGct+EblZ zGNDTEN;rSmQ*$IEp~|*UJ)BwWUAad|sM5Q7*utIl-4)4%D!nUVi!<^zCd!1W+y`xO zj%xOT>LH=Z(pJKJ9f_nXVV{sNr%G5V=}b6&IQrzhC0m>=F|Y2tCypgP%~}t8k$o^n zeU{340ByR=F~?>_Yv8{mZ8dY^+<%DSHhN| z%5)!LPL;4vs50G0m{TR}+p0|W5$040$17E)`v`NYggr`?={~}oDq%lYWx9_rr%D*B zsWRP1m{TR}YpP855$040`&vq`E1`QzSC6Ff^Gx*B!@MN?JbS2emm(9YOlP96RH{r{ z!t9~S^tuw3B&DlIQu%o$EU$gS&nuzIbRS{gmaZPEO!pCvQPR~zmFYghu~xczs50G0 zIA=^^i72Y>VXAK@G>T|HEp?jwvvq^pN2(|v@!NV-C-*1`_m-rqhx=3#=2QupM zC!xwwyVk?8UBY&p301jA`6k8|Zac0X5~>`DD&hFy6ZF|+LRB7poYkCV>|NDELY2L% z629Sc^zpuBLX~g+D&d<-dspsJ5~_TISv_2h+DaVxGNH;hv6b-6u`{Q=ITNaUqg)B! zfZH&b*F-|$z$6A6wId1R1Ko7 zsPbf0B|Hgc^kQGhgerSlB|On))bIG13001jm9RC~O7d(Vp~@3`)x(j@5iUnE5~@6T zSUo&}>Fn${p9xit7@5$+bRS`9E8)piV+Q+tCR90MR6=*lZakj}Ri4bPgss6=V#JXN zRh}@egx}gS)^gRC300nyu7oGMjr6<=nNXGQl~*JjSE`4EDo3SCc+%du-6%5?syu;T z2_r8fGUK33s4|MGgzlEgzLE)5_OwbkvpC~8CuTyGGioJ_CX6zCHknZ6ldXi2ml2tB zVkT5MqgKL*#Yo0=LMBwX%BY0jaB>x9f6Rm`dvhh6S)6ekB{HEZk0oima?h!HNT~9g zXqE7rbMAN;KV(9c(MKg5ksYa>OERI#nWz%h(3-p6$%HCbOOMGSXmI+n1&`P*c zbOmX|kqK2fE-^|qdiD8N4+&Mao=UjFa^>ZECljh%`&7bN&3VuklL=M1T^VgVJ{qr7 z4+&NFf=W2E*n@LRlu+etS3PXIw!S>VNvO*A$~nrX=9phgC8269ZFl=c?e4So8NKFn z^G2>3ZEw}X`A(JImGCaQ{>-zpgetwOhtamH7w45ssIo0o!j+dRI^(WPsIs(`u!TEH zP(z_DoYxE*r3HyYEIaR_^NoT_G!_mhX zwR+g%Y>7FN@t!!A_%y4By~sXjtX&C9Wj%7KY}KwH?2pw$m9%}O681Y=c#f95B4J;t z9?m$na9e&Q?5Xz09JhN#!a1>eIKDdWIx|$l(bDJdIA00JUE8rOrV_S9)43(uqfF-> zWxf)obE#CBQy(GSM`RD%k}A{HBN2XH3EPq?(|v?FRl;7R%5)!LPL*&pP-VK0FsDj5 z!l^RdN0?J3>`|&r_Yvk)3H!M!(|v?FRl?CnmFYghoGM{oQ)Rl3FsDk`*HU_23Efk= zdL)&fXQHnj<|X0h)x&);)2=@=p~`e6T$!5IFB7UvS3-Bw`ej0u=}PF{=iz-fef3L@ zYq$Ssi*~{LSC6VkQf0#Z3)3%Jy{x_Ik*(SVmk&LXDifBp% zJyhj-n6G|)gev{=-R<*8D*dX5^)Q`Fl~jF1Dq${_WzQbwYg)Q`Bvl`g=>PWE;1~aF zC8qNeOsYO2(e-&G)o;Fs?bYaI;eIEzKlsxdM~B~d#ahNhRk?qduYP@mD*fsc)WdWi zp~`gjn2CBAvHtt{E80FQ5BtULXD*ehTo3c@yH^tCt6wdZgy}v)mFes;6ZLSF^3Qvn z-tPX!lSccW^2{4@sZ`~9m~Y>`k}zNWYN;el_Yta0XOEevhbz-}9>1c!;;th{mw$Y} zaW0jrTo3c@yH^tCt6wdZgy}v)mFes;6ZLR6;*d>FYFqAm)9CWKs|U}++N#R+Fkk6D zLY02?Ug=@Fk5FYgd(1>V+=05}amTgK9==sO;?OGw&%@fP%Jnc`={`c0e)V4IVY-h{ zWjcGzL_OSf{GFSRZf|_;9_{>xUOIRl)>c)nhxtnP5vug7_eu}beS|90*<&W^;ZE}l zpD@*q{N{me-tkj&bE#D2dYEtDy^=6r{c5QsO!pD0OlOaos7G3bLv zbRVHgzv|%|(7tCb;WK~7$qU+}e?Pk|yJN3GJeS z!mYNxArq<|JnXdg;J-X~bn9N7;5~}p89-dq9 zJj3HRKB;Yb+)bm!Pr7(ea|u-seRo;g>aI_Xc6`Al1Hyd!_A(^glhLo%!+b4aE|r8T z{i=tjLi%C`31gox@4KLV{>^jR+y{3ZR}b^`bjtgeEoeXe%FkefU zOC_O7zv^Kz%%zf0rC+Uwr;Ykz9|_~D2dg>#3*Ty=kf)e$T$`wxi~+g;1qmt%vzq!dxl|Rr*yA&vNyxaU?Eb z75|NA9oKGp-R~-R zP4|cYc|!Z~AOB#q+xOR24^_Dy=4%OasU%eCSL@*!!M=S43HM75{?M{Ewc?i1`wo2f z;F+7RXBBt*)Ux)53vV9%?CRmYl2D~zt%vzq!dxl|Rr*yAPhd*-5vsPm^wjplZ(cOI zXQNZsam44O3d@W%vm4qt& zs)udY)9P=0-|}|n39lTz?A&3Gl2Db~uKD)$4+;B+ezhK+$M5Us687`kc3jz}F4%kY z<&Cbqv3i)VE7rV9l~AQ$^)O#cm`f$0O26vi9*N(A_=~MhXn*(4{~R5B&2Z%S4Zp`#uux!{}E%%-0g;Qc0-NuX>oT-!XaQ%;VbpTW7VMHy-XrNvP7VdYG?wA=g|& zm44O3Z_!Bi5vtBzcwBqp%eQDB-ehex*RPLImG6}!T%U)89(hc3UDG%2N;vMi-l@;S z^?<9%yn>WaC;)GmAd@T{|hsyvpMuO-Z-l2D~z zy;q)4x8>*7E}`l#Ph8nvzw(yR;#q6kt@Yge8}?b*&b{w5qaB|+j2Y}js_ZMZ9_DKa zbEza$=~q4cMt|S9E8)1i>d%(8z5jfNw!yZ;9Y{xNe-B{Uo0he+u6knI@QmR;gM_L) zwwtdd%%zf0rC+^Q{=$M$O`e@4RQ>GB%i5jyZrP?jFpL?D$kMkJYN;gr9z%}d%-0g; zQc0-Nua?SRyyzR-B^=u~pI+APIC`u0;SGoP%JJOa%lN}DozQ;1dZ)I-Im6jNLY3ou zt%vzq!dxl|Rr*yAe}SZLjU(Y2=g{-cZoB{OL8C7}Ww=&zRpjrNJn+HSwl95p=h1oZ z*ye^>DhXA29c8|jFqcX~m43BU{vwR~`+0|6Le==BMQ!u@4r=qiefZq!;jHHDlV=A<6-`+WI&5rN^+;dfcMbPNB~;~o2HR_&hlC!s<5~}Y`N^n1NBR<~e)o~H+TZ-y^3lsK z9PUP?--^n+Q4*>Sdi1Qe(<#eG*POjJ4@=lrDplFTxZU5_x^zwS`X4vVZI9hEj9w*F z<+$B^9kugFEul)k`po^+GFM1>RV1Nmo6}BhPrd4=ql2#+Mwaeh_?u^c_LXJrabMiB z?fkgm%qgMDeT`ZV^RzL7NZko3iLRCH|W4@L!mr6pFe)V4YD~o;aSR{PM^5C9l zxAXq(u+cm38@?;@t&G3XxXTMxwd=M%W%Qlfhi9WCROR7; z5~}p89{vicdy9E@P(sx|KVfCN{ZDQiz3-6WJ9^(|`x~Y=KYeBU-W9iwe)geZ3@4$= z_uRD}=4%OasU%eCS3P_N_Tb!WB~mt&*69CBvj?|CFbjB zmq$1WRr=L?<^3`3UC4wg)0J??%blsb8zrHtZ;!069+t$i>sRaHuhsXxE0XYC(ce6L zMtj8%FCBe*i{ZH?*Jsjwgeuo3nx3RQay3dbodK56-<-LY4a()x$pF8-e^5Kth#$rFuB`xq8Vf7710(iPgh*X?@RJ z!e{O~w(8;f%$Xt21`?`Vw^a|{bGeI`ckU!q`TnbV*h-w+^Xx34Dz_{1b)AqSeF;_i z)q0q3U;mJ>f9O{|T%YyD?GncAuG_LlUmw)NKA1hs*VTPq(Mzb(uhzqS`|g#5_e#I& z;hMZ}Z6M*=z_ohyFkgFD?okq|^s64uIDPY=g!5pYqntVW#zYCn#5~uUuO-Z-l2D~z zy;ts8_06>s&b97kRS#$9zIsSlk3657uO-Z-l2D~zt%p6znL5wT5~^|^G~d2hRKi$P zzgiFT?YmbJ-Yfm8hxvLJa?K@F=~q3Bqm62EBqO29__}(y&(JsSN;vN3eFpQjgt=4_ zs`RV%a5d2vUr88W<@JXVSzkRQtVfRF%-0g;Qc0-Nuih)8C0E{gwJo74$5);y>8po? z_3*4ot%vzq!dxl|Rr*yAXJ>bZ^Nz5Dsyv^&4`Y;>BTETY?$6YEn6D+wrIJvkU-fWy zc7>EzMG~s=d~Uvd@s)(}m43Az-XBYtOC_Po`&KsfbWe-Pc z-!$d7OA@N`*zO3|w+A5M9zY%w9U1!ecO~54&0~pURNua+g!`h7UA0}=5>4lpm{ggt zPnfpl*HR@_CM;E=tA~X7{(9?S*)3sTsZ?2~TJxFkuf)3sFQ zn@X4o%bq>V*I6{roD!<^tEDnu)6$txWjYg%M0xbFB&xE9`I?rlrAn$k!cn_#q*j$Z z%-6JZtw&Pz5zcUZvzn^xVZNrNYdw;xk1!hOivUz*5A!uGUF(rleMH|LfFAAvWDoN- zEnVx8RDFcI6nSSt4^`R2d`(N&dL&gJ;qF)7@zO(8_Ap=5(zPB*)kipAd77%vLqd;V@A)fh-d7I^ z>yhu3`I?sgRX%e|m`jyZeMDb9^su(s!+cFkuj{=sU(^5X@ymZL%_nF&Kf$ExBNAPo zc~br6d)Qu`sq^eCp(?j_^Of!+ROwfrpdP0C2vw%D$4u122(T~eQ`k}zNWYN;el_Yta0XOEevN7^0ErE*U<*Ta1K?v;f3>Q_r8VY-h{WjcGz zL_K^X(DxQVRj!Bm_T4K9^VP53D+$wmgeue7V5~}hU7W0+vBUI^EpND71`kuLj&)hR* z)x&(H`v_I~RS!?&ddfea?3YlL&+nRVUra6G+CaZr5A(Hzxl|IW^s648H}`~bK2qgnKgjRS)yE zgt=4_s`RTKexs@{W{@!U>H7}W`Z}v-zLqeTNpOeCzRru9uO-Z-l2GOO z)OxSHKkMsz6Xt6PbEza$`8|zV55H;Ax6dHqeo5bVGuGERWb?Izxl|IWJojAhmER?j z?juzB9i!^uGtjS(Q023!9-cS%TdnzS3v*`v_IJ9y9S< z7RJ#=nK`nQP-T2w@0Iylr~C^W5~}p893HyhBwI2Q|a9=-{u%GAO5jI~}ta+6xp-R76 z5A(Hzxl|IW^s653k*u%p6`8O5GkJ$aLY3cRs`W5m>*S73CRFKH3G-cF-}5qG??SG* zget#RR_oz+?WFq%RelGrdU&t&>myY8Y^sMNT%U)89(hc3UDG%2N;vMi-l_F)J>Y6G zuOKBO2Y9f|F*i%V150q z6!W!&xl|IW{M#(`UioW?ef?a*ex83n(XnKG{pPayTEbi^303~abG=udh&O)75rBlM ze1_h6aDDx~BJ;I`xl|IW{QF4tUipjGefta&?la`yz;@19U%$m_zLqeTN+A31n6D+wrIJwP-{YzG%3ou5&pPiqOQ_1f-|jn> z_4Pa3=4%OasU%eSyWaI)`Ik@JRn0r85~}jwO0jQC_Ytb>^R*uC8?UdwD`URSaCugf zQ03pTsrB$L9QD0-mhioE{+medEv&D;d zjapxS8_;}RS>)A(gew0QVXcRM3DX_Vyvr$}D*x?F;~nWfLX~mQ%-6%Ro3@0xRH{r@ z!oQr=_s&bgcV78#Z5by>_Yta$A8I|^|6X5z|J;20o`;0bz`wU%>*1NJ_4W5_&DW7A zk3JHr{Cl~z9==iZudU|4uqvS{zk__woQ3V~gO3?qvwZilZ+r6_UI|tHUDH|*iN1PB zSdZ-CUnX^5Jnw8vsLFrK)RrjSN2tne*H}cqK0=i-N_`$P@i!+ufxW)|#-aIIr~H>N zB~jFRpnR5^B44@YVv*&Mw}sLErzXJan^^rH5ox6dAZX`?G|yluyo zZR&!(M_*o(uBDPt<(ZjEm~Y=4CE*;UU-h_ddVX8FX!Ew=@{7j4;m>dNB~*RmS;w`T z-nZp1a;hG#0j2v0Rjv)IhjE)xO^#$FR2k1z58p?4r}N#FQ004y>S3SoO>BPqDxu20 zQaxNp`BpB!iIY&3*HJ!acU$rfi-anlf31gmy6)EJ9eN2>?)6p=TS;H+BVp{5+m&-h zU)(NX-0oab>tViq{X@e3pjE>tViq_e#QhrC;@MKDXuP)-Itc&*$dbHzrCrChAw~VQlCqkw*pzRmP0f z!??{6HIJeas*LBVhxuBsHzpFQa!l=b-d7I^>)|+G>tVi@FqcX~ zm44O39@Y0AN5c0wxeuDJE2O+Cl2D~zt%v#c-75+2m44O3e7y^~<`Sy(s~+xI_U(mB zxEG%HEM0T-jk^+#yRJoQJ&ck;)`K^qED&K`v5BD$n>LFo0+}Efc z=4%OasU%eCS3QiD`r=&)SFR0fJ-k2GDMuL+s=RO2!&15Xm3O=( zRONa|7@g&aOhQ%maHMu;D(^-~sLErzBb>X#c}G}6RUQ)^8QcTPyEqc6@>t>+<*sVp zNtICL*j3w=EzxvtiAj|S`-Ev*el1l}Wx`S=x_U^M@2|HWmfaHel}eR$sx_Yp56f=a z64p{BRVJ*5>Asq)GF?k$zNv(nu;`29_DLWy4EA9`Usoqv5VsWRdBqfF;-lP6UlkxE#fhaQ$ad-z=<(|%*9dL&gQEPJA>hlKfBr%ITw zccHK5s`RTKwm#E-12_|^Os_A(683rIU)V4&Re8Te!rJCvLXc49{!Ffiqk$*aGoi{7 zX2RN9+Dxdjgqd(Alg@-H)0wayeRo%)&qI~zTq;ZAyW32t^1fxlXJ9%LmVI)fuU)A! zolB+4cAN=Sef7}8KA$~Q*C`KI=|;s zWefN7>|ws9GoeR56{fqYJ`Y=oe|005N>!hS^>CEP9;z&1C2T*w?eX(I!ZYHTP?b-4 z>+b5VkFXx5bE#D2zgJ|bRM{V^hxz(W(9ipbe9GIrROK_<5#PvB=lRX#(n zyQe07Hrc~`r88m8P4^N0R$L}j`D`koyXRK?ypQmdezVS##5U zgn#EL6RLbRmC)VuP=4M=_)UULsLJ03(B0FxeT2_kI+x0to9-k0yL6dQ<+G`T?#5Jp z-beVY)=a3%-{IBWlgoXC&s;i}%9@+*BaBNkp~`1d3EjO{#@Cf_mEy_%yhAUcD(}Nd zsB*oNOXWJ#5@te`zc8N(RehyW<*GP)sIpF#aLwUU^Oq4bp~|&LCRDkDkO@`pgj6C$ zVg7=4CRF9WQKg4*w7+z2~|GLOsMkNWI~nCzY?kJo_flJs=PjT74JJ1>FOb& zYH|<9`Rt*}xug=#+s?@TvTi0+<-cR1hcl}4l;K|qsvZ)mtWzb-*Z9+4 zAj*U){VL(S)OW8WyjRXw)x)wI75mG&nNVe&Dq+4x;r^wSOsLYY5-H#OmzpG0<<*^6 zT+#WLJ!+{WROwd<^K}L3UxvzrD*Y;9zU~_2zc?kKO26vi4n*HPDB(Ql?`LKYRhBRl zs=N!8u{VHL; z*4Dd_303-4!dTt63#K#S6YL{Yna&=nOlQIwz%_0rRAmpZSaa`m^^j1NpNE7h*M`|c zResy2M@nZ8^EKT^=+{TAFAqy%I+sdSE|oE^Z@QUgDJd!FCwm4h6tCw02 z@4L^#NVXD2tm{VPH$Qr)%8|b7aGzlAwGysUeVVoA5~}p8gkzs0wdqXwoXsf{sZ67Gw+tCoAMD)Y^yl2A3dhx2Z&hxyvtP4^MzR0;F4wVUoE%&8Kt_02c0 z&m|l`99Oc3D*Y;^4lBhCr&i6`{ zIaR`vq;wzQ?oK9D>6Zyrp1IA0D$|*;RPMI)5x(=zgsQ$$SrT`2vWF^LOeQRe`=yys zWeGE3N!(}5gepr|30rkvE0Hi?Pvd6~V^Pz6geQSBp{lP`sywNZJybbLRKh!II>!vE z>`{K6J1 z*EOJ@_Yv*~WkQuZdYSNf^byv>boNl?DfUdL@=jO6d|j*gc^}~#HxsJd30n*C^RwFY zuFFR^-!*rfckV2&sy+{EZaSArm8aM*sxh`x=>0<*rC3+}G$MeCE>G z!ZXAf1@sS>U^d}^L_%!De}BAHO-4nihW<$VStFC#L~ z)>jV+Ri5R}gm09M88e~EQ*W72)mJK2o~Fwls;pBbyrZrld>57p@2=y!yqmM{~lybG1E?5?o$iFgTB)~R}!uPb^_<7YyZew9f1 zI?iW8m2*iYoVPts?6p9xjgsS@UE6z)m+OsLYY66Wjr%#-q&P^Dib%-0p2XX`Ve zO20~&uPaDT%4b5Aew8p^cM9@pbqQ7a4LsPta6b1mekN48uaODoaOd_+sIr8W(A`pb z7c!yBJ6#E*JEK0&p=Uys@4qVHsBh$LM4ky%&Lx#FUn6fL@=U1GuM*~KZM_SbP^Dib zbXVniCljjjy)s|lVE8txdPu0!FB8_>HkJuhmar0zmX2z9Z%IN`-u;kJm1l!gk~}7Q zM@{FutID?|rt7_u@Q#}9Bg`oiswVg7BlIv`OXYp{ndkY#D~_Uh4_QK$ezjDt8huxg zS49%e8O|kr9+of@s`RUbL$&tS6 zKG$ctpG&CnzSVk2_yjYNO5$p=dZdzM4++zmu&+5%o35pju(kVnCRAk)uh_0kSC2&a zc_nN==Id&*5?*oblszO==~q2cpU7jogevzhtB132Up*|ZJ=JveNQ9qPBF!1b@RhK( zrgP0zrF8Z%U+GMEfBFbLjOVh4`AYW@IRY?WRi^tq%qbJ*n+Zo8XEHyp9$qo+NK^@* zn(16BXL>zy+jT^?9;R!l65;2SFfU8x=asOAea}OcX>+O`p1(@_u$Ay{pPBYAr)5Hw z=}h>S-}TFc`I_z{CimE6kE!;&n`X5OE?+&$9;!Zb_rYz^DRbLpuit+yge9509(}~* zclX}M@7KO@)-LU_`}Z2HTzW`*_ZxR>kKMD^TI!*yujZ=mJ8G&ef6A=(;SDcci-#)z zHhp~_-jkH>Bm8Ua>x;0(WDixQGvVK(e_+#N+DSV-G&=aIO9pq>`=hGQL%NSpWxCeG z`+nysQ|+7&Zr-LoaOubrWhb+xcBLRI$I^Sg`N`G5Vi(O+z}^$oZ0y{w)3*&RoJ`i^aGc*A>^wgsc(e{nXh zdPu1H@$L)TnNL1ubn_SHjw^A;{fpb{zP;_}`n|q)-J4EY*k1KtM~=R8=j_pDw=Zpn zZo6poo!e&*+LeT=srk#=lE?2jy6fP#USB;N4SsyWA#K5%cWcXT*=ywInNTI&N92}h zzN$=T4^^i72)!%e-$FkkInQ)9U-AAZ0UF(tZ zbtKA!sytpL5AR6!P-VoC32SS*kFd0va767RRGH2ms`RUbHB9L~BDZ$)RpsZ|!zXCE zkLdGIHMz%35SFmkTzAubvw|wlXw44c5xew~D%J?CBs4{-2gssm$kw+g1RgPWR z!)VD^D-)_LVJ57tcOet1vWF3ik&JOr^^j0yOjHTuHlwo~kx8gBo~s^?gSIQveS|q> zLe=CRnefWw>)Oc){j5{1c~WJvQ(D+*XvJnTv7@9wj+c6KNG6*$dGF59jP8(aTIk7&V(xOLMCh(IRda$sT$Dr3}27(p6Y8kc55l~HXaQrW!= znNa1O&O~3YO>?-B=f-@Z8>A9~{zo%c$@boKDc4>mZv-L}J#oAFtvKJx}Y?<1ru;g!Gq`l@#KTaO-J`SJb6e%?n&SHdd?|H;bsf3En{ zc=g=XV?XaBq$}Z-cfEF5d;bPcoSr)Lim{*f5z>|L%Eoh-wBw#Ln!fO%myZ3skC3i} zSDyB!MQ!Ilf7$f!Et#6@=Y529CA?xU^7B4Ix)NToKl*teAzcZtn6IDr5z>|Liuw9^ zA0b@{ub5u`y=CoZ7i=+o#7BmwUUWD8_HQp~Ut2n7`uIDBr^4*dhu!kpcKo5wAK&=I zt#A0|=U>|vzwLjHZ~N-^uB)YTl(~A}RqcSQmyW-+=6uPcb62&KZ#{8*;)*>6=Sv*> zHhja%cKZ#V8ZUg(#iJiBT-m<#qMOItzhHRM>56YHXD2tWw(x&_{+iEk+Hd+> z*Iqk!KzwFdd-ZW!PVaR3MWt_l{XNUtWe03Ez0vZE2Bq5T)vs#Te)azGobOlSzsRY2 z>^gg8n|t3a<43Q)sEknuZojf^{rS&~FF)|z0}n?s=_hY_X1jLo#p8Q7I(0xezCQW) zSG7gAoix66%{ii*wqDh4{@F?6zg@ojfbconGEO+^?Djv-nLmEZBX{mm>tXBL?4Bj< zNw1hQ{qUOeiMb_Os@FYfReQl>+6IcirA+pB(gY?n}FPpclbS6ldX_ny#p*ylOZ+nu*^lnGUi zM3wLv`1~DLGNH;*sS@@mXP-Q)NvN`SRS)l#z4pb2Eod8^HhcQ`HK#N)q3Us8I=am| zX7}lff9XWLPtBHb^}jD`&-%yd@xoab4?HAPUG#-z?YRehYP|h(hY^4yfFr}(W-V;* z9zAP%!);d$<|qkOfBb~w+8sx3Ila*t!`a!E=qS47g5%mwm&}?zV$CV$daoo@?YGl$ z?XC}RG5wZb`o4j!&$j!VeU59}ZL(%I_@y)S5~_|m>ezPZq0gS)?wnz3x5YX4E!}Ko zJAe7S@z-mMTi5(LX|1`0D(_n*jH8?Z zo^kH-_UQg6jt{wT7+*=KT5{g<_L~2B;`ndR9ukgZwwT;XBvf6yW@MPReD?Iizx0g} zqZdc#7yR@U?Q@U+&+$RO^bG|GRaY!Lt}S@|tmzm1(m8$Ob4R$9FIdp-8qJyB^O0Q# z&qG4hOJBaA{m$!VPk(weXFxdjIY&MAf|J_3hd((!^!h6X{aixT+g7Y-XCHa!__Ukm z4G3ppqvDM}yQ2N;hYlFO<-TErETQUv>zB93-?e1?tzSB)@4V!Eb>eeQZ@+!Z6UVRq z!S;h*E1_!V16Q<*ZaiXqOS*O|ujZ*vOK?&!w?4^TNBB9E0zShHI+mff@^tTj)nOc-S(x_U^MZ;nfxmt5D}@WxZy6Ars*eD4;+ zT>}YK&R4Zmwk6kUv;S>z`|_q+O;1f-F}S-Ds%&G`!xf#;K#l+;RNeiJ6WSC04;+jo#<$d7)+-A$qtzANu@m%$=y}EkI zD;5b=w&UvIIA~OpBN+)*j*r#Db!Oi)m++ap{;VFxZH{(%gp*KZJXbwj%Nq6PNMAyg zYuxJLsBg=##biQNj!SGAMoUHm)k8v6jzx|1TzTc1OQ>>gucdO7arDU}gM=#ESoLrO zaAYtd&x9)dD&Y)ZPtA8%LY02i!-&P6>dcS{RX+bp*mLYrdB%}YrC;@M$HQ6Qev=7R z`c=Z{#mGCiL758A>V)pAReP^Dk3hlHb#ewFZwg#9KHsw`n8+(UBoag3>it2oyb-kzjNlyt=;~iO1hTH=-w7?f2@Q(O2V8fVQaUAJNs0^ zSlvEo53YoBpM*J8!qM5Mmd8Y&v#rk&wR+gDY)hsqVUMy;n689ZOlJ?T^bx8|n^P^7 zS0*Qi)9Xs;p3>DLsrH4Y?uE`|KsS>XHY<;d1 z2LxwT?2oQRE8+T8BCk?iBRj6Rrmh~Uq+PdH!ZFc30Cy}Z;d8cSxc^WITc3Lwxg}aE z_e*lAY_HbCy_#Aody)G-xkssTZz&UAk#JwDmdbl&3C$@Js&R!OVmz-)2_AQOy>USE&-Jd@EZ$?5WP2zERGED&HftEk=;O(eOsMh{ zNhO?@oIgAPlL=LxhN*tjdHc&t+A@ zK4J9Y39(G5vaeLaanR`AlWv(%<@i_$M}6B^ZY2_`JiS*vj945W^9U!Q%2SBd!@lj@ z?g`3FsIt#j!hB6jXF`?fO4zrJ89dRN303y_N*HtTzt*X9{VLY4DX^>8G!_1P*jp(@`iM`xd!vrqMqP?g)REyGn~p4B8&=~qkTEbIz0 z-(3k+`c)5S09PzI3YSpj^RFI8NUrEyg=IpOewA$MWL*W3qf z;eDg1gzZ(oS`P`M7yT;X6$$%ICRAC%N;vm9`Z&f^!idFInOmYy&5@ z%6jBd+3#FIFnW^K0=jgbE>8C%H)K!ZLE5D zWpYB=HdZ~nGC3h_@2VbNnVgVzl&BtFnVgW$Bb-+zC!}+a^2+3dbnfR~nVgW$qmNf6 zC#3CN^_hF6kFXb|boEFoKd*$PO6heabT_SE_UI#AXSzpbx_TtS&nsbGu2@a?5$040 z^RgtS`v`ODBXX&HQYl?MlFH93;a)~cuPdQ@U%OIe+MH^sypq!EN?4MVt{zF{=asPR zedCoX)81PQI?4eC_P2+SZrX=acUNOuo`@Xq0+3K;JFXs%yGAcLT9Q!ZzF+llq?X80pM)xRBddpFyM*mH z6RPZwl}K;jY{!{UMOeR$2cIA`u2^vLJ4+&MC(Wyi_6=W=t302<1N~F_GMmU*Jnh<6opY3PVkT61La-9)6l0#9B~*FRv3jJ_ zm_{6#Q00luN~BYz`F*W~Do>tP5AULP*I6_Zs=OnWNb_KhUL{m{cDQ=jbL>$@&Y4i9 zcO}wzN^{F7c!wL-z!_St=$;DdPu19I|7wR^{`HT zggNyQMjV+?mE)51Es?xpkx=DqS4)+??UJKc301bY>fuap1mLPK6RP|!P$e7z92tz^ zGoi}wG*!ac%UR7ebtY692Uj9}8_eCEOsI11s)VzbvzjZ*OsMj^a+R=m+H2hp$b>4} zTP0kXxMInxR0&l^QPsoVX|FX#$%HB+oJx2Xy}Pd4GNH;jAY#1$%HD$%1WeX?mlBCRJnSsgd>?FT;B1LP-SdaJ&fp$^z#m+ zgeq6a)x&mW%UH{7!0-R7%5jMifU%bCt(HncRgOiip{p?C$Lb-WD#r|t0FFd?^pQ|y z3$3LxZg*VCBZGu0y{m_J(VptOk_lCMSHk(jo|+>W301a*>fy{{@5((&LY3at!xrwW z@2*HDROwv_Tbz-%F;ON|qsPB3HyYEIaR_^NoT_G!_g=2 zE!pC1iFtMBJ#j4YY1Vq!i|m6r>a$eVBbUln?YwJ$tff*VZC|N`{mvHdnxPV2k+81} z2xc5xxGldD_EdYYD}YLPMZ!6;5{`X7O-JoYIPTgieg2iOU7619%Kl+G_Yd=xFr7=K z%AEQL={_QR*fLa^t{#c-^Get12y?20eOs03KEj+T;drIWbRS_( zm9R&tGTld*Qzh)@s!aD0=2Qt|HC3kj2y?20eNC0=KEj+TVP8w>btQC9>FSYGex8ZG zdYG4lpJxwM?ownzmFZ0Ml}eRqOPD=WnO;}IlB9I?NGdY>VXAK~05T|HEp?jxMTrK^W3(|v@oh;;Q( zWx9{B7fDwSRi^t0dy#bYP-Qw3Ug;xLnKq|dDx+-))0HryH(%40aMj>yBKIg)Cazd= zKX;d_Z*R%C#1qk(JcO1KVJ zC0z;E`tAYb5zhStTa3@Yde{=(%g8O!Qn_D}OXc2+^{^e+QrV;2_sKmft_> zggI5hE0!eRD+yJ4R}Y_uge^Z4s_ZwFu&>z%ZTXo{WlyVwJx3z9b_rGXwCZ7xlE^Jl zLY3{fdf1B`Yx5{7p(^)5M{0>2^+~95)UNe#Y?rVdXF^r(QND?>h1-s+hlDCeqDnY^ z_ym16nNXESA7?dZ8GBdtkWgjss)TR&9DTfRnNa1Mze@O~(%zMOl!PkZU{()TqqY)9 zzD%g{O>8B6bL`A%Z_b1&-zZnYH{iAmdvhjK`DVNlzR9;;*?Kae$~XL#@I->6L>?I= zRC!XNdZcIWxRMD~p17!lCqx{(^5`R>%9A72!;>z~AI?{qP~{1jN_b+&8Ng?g2~|F) zO4#CTiAIB&Q00lFN_c|G7~WVS6RJE}RS8dm8NJw7GNHNT~9}UiEM!bA-#0jD#vr9##)eU^+WH&SyfEBSt3lFx^L3+DdqG)tJFPp9xit z7?sf7vK!B5LX{_TD`9J}l^AhkLX{_sE8(}cjI~_#WkQuFr7PjdZX-SKLMBw@d*u}g z$Cc_Kp~_LI5}vd-Za2!zgep(qSHj54h|D-B6RM1&Dxtfjvae)9l|8Kz&MeM2&WV{& z<&0VhqY0x7pG_uI`D80$M)lwx~jk-#;jb%cWEwmD@6kS0Yab!YOj!TSEjb45J)k8v+t)~*Muv~e$-pPb2 z*FKeSR&yS-#biQNZdXRzj*rGG)k8v+y`U1#EcW2s5+zhQ+f@(SuB|VRa1yHWy>gE7 zsX6A?Qc0*~`A@M~+YX+`N&iM%!EUaK2NecO|@wu0QkaETKy8>S46)>cx2_ z6RKRMMGn{BZPfMy(#UI9pWC8P$g|&sf7K`7M`OeuSnQes)sXu0345yjF~{v*k#J6|9*(b$yUq-iaJ2OK zJI+_aao2Wii>ZVy(R6N!_9)Z2N13mL>0ByR=F~?>_Yv8{wxr5*^+<%DSHiZW%5)!L zPL;41sWRP1m{TPj4OE%#Bh0B1j&Q0>_Yvk)344?((|v?FRluct{zF{=b7lMhj~f(dG&B#%(UyzOsFzl30J13^~;1R z)0NQOw0@aTWx5i&_j!2VO~2vrsdmWmn@_*x{i{dSBdIds{)OpherA6A<5zDs{g%sz z9!Zr6OJ%#7`_qHlpUs*xeewDGucaQUay`sfzdk~he);b9c_fv7)x&z2&ZSDKJ|dMc zm&&qd5A!uGT|JVjk4W@?du;HFf3_0S`3WXfACc(#Jd)}+-^2E5^s>t_i`y=*xPE-- zjaRH?OjMQohxzK)N2t=TK0!T9_Yta0XOEevhY{(^{`#<*o#qMV=m8x71^X@gGdaFufTVJEe3T>OObS*JephFmIDxgO@*cdsPOSHD`SLVV(! zw%^;g`9<0L2vw%D$4u12mFcoGPi$ws{bl1TKfd2Mmr7NxhxvNn`Uq9})l%tUx{pw0 zI(y7SJ=~3W_O^@LQ@6iiyn62H!Sk@Ts&YNdSGtc-rC+^QdYJAbRGH2mGf@wBpibR- zs_lQvtm&ykuNXWJYpW{P!+fRt2vz#kd!>izK0=l0>@gGdaM$shcOKj}`re%B3mpF zlRFlSPh7FbfH2>_^`M07TK#H0%-0g;Qc0-NuX=cH!Sf9N_sNUf6Wa~rg-^P8P;&`Y z|L5UF?I|Dsd zI=$zEyN;`e`Fc9#taH|kK0EI^{pp?Otc6geU-d9wOPEU~p-R8%VLd#T^S^gLylwvA zGp0A(YSmimp(@wId@W%vm4qt&YCSw{)EE0m7++ogxr5q2zG3g_yRW@=5QUqsr=I@d z%LlbP58ZqElBxM?AynyC>tVi@FqcX~m44O3vs`^^90}KIAKP`R?ftv6rZ-xC(V&%> zuV>2sVbN53(z7<7-s$wU5vug7^)O#cm`f$0O26u1Jv>df%_e`;ezxG=@tp6ktsbg! zJ@C2rGAED}pA3DDMeEgT=dpA0DE%!>lK0;N#SDrofOzICm zwzNHT$FbvEzc6=DDhXBZzIJIl>TwIlf4hA50pT8EpNE7V?vd8#VX3712vxZrp6u<5 zcO{H>M-QIZKJ_Or8K3{ioqN=Jn6FW0jw~fq=~q3>*AnJZNvP7VdboD-JoR_ith~lM z>^lAM{lk@)geuom)x&&Ux8;?Wgev{2hxuB7;5~}p89`2F&Er=6$Jf_{S;)mmdt{JYpBviR~Qa#MKZ{J74eHi_!hxuB zea$9otGRxCgsOb69O3#rB=pE*qU)NzaaY1|*Y!?)93ES>pyy>uZ z`$juVZ+G6xQS~s&knST?<+#MX9#{8yMK7VsJ)l|-zp>oc2PN!-zkC0_?ISPVW%~GA zhEbm*gWq@l`bqn?k3V(x^u@0qo^_T`mB$kEwS>7;5~}p8_sSFMw*1`MB~-osEmLjf zk)IeZoVB*yTF=eDYS~mfe4ezkO-Jrqf4!U>Gx$h-|0h+Fc*s;uqgz==+9(svN`lEeK1POC_O7zk09y z#f!eNUBa>b=ii*)Zu|VE)7NY`yjPCr{$9olA3UHvyw+nqC<4J1@K&ewXFuO-Z- zl2D~z_3#%+`qnrSu5sE27PTE;v)lOVPZ_S&Tow8IC3oDhxP9pSZO8xXo!boFb4sYn z>nQWJgt=4_s`RV(%3p+We?RZgOQ^c^&*!y`j@fVe8FwE(cnjdH=IoPa90^r@^PnZn zrLrWd^sD#Ey=i}Y=kzbmZ}VQT+4N4UhU2b;D)+3bhxt09=228am44O3T?2oK?Oo4W z*sgu_=<%(04fjPQRONjJ+iRbPgdVozS`UBu$*4a^`Vy*s^7Mu69fvF!A9CSvH!A&B zRNjq}P&IeEh3$ziUNHXKv)ATf3HwT=Dtj2W`x{&5uZdpod(ke_58pG4UL{oJxZQjm zwev_Vp-R8{%>C6eS4eqPB%x}fiw7 zDWS@Jjam=$wS>7;5~}p89{!4)E2O+Cl2CQP?fbSjJhsd9o{#K0c>m%1Y=499Dc|3> zeRR)VrawIzo&%6j<$La05A(Hzxl|IW^s63zBi9ob`ILo(s;}NY)!uQ=$H#|Wf5l+! z?5P)j6Y!P4yQs}NcJJ|NH_clMp(>w~F<(oVOC_O7zk09ymBqeyEE2wB`R0;E?eve0 z#^>KRd{^XK8GobknTIZH@BPLR<8R$QJR2pUD!<1uUrU%vC80{cdawM|OHYX9Q(+RS zUUJKE?UK)JG=B9DwjV^6zVq-mGoSF?h3$E_>^Hvhrs4N-Bvkn>q}IcHEnzN|gev{2 zhrdGV-eTSzlu&i+^QYQPOK%)se#r10z3;RA4bzYQ;Z*zfn?E^z^h3iKPC}LMxobVl z*AnJZNvP7VdiV_N!MWE;sPfrV4|kpY-P)t)?A?BT!My43uK4I6(w9(`_o>adZ~scd z{VV-yJP0!*9O0 zc9QNRRJo?A_sYBLDlD(OBvj?+;mI0*XZv#(Otmxj_{8{%J%`_klTek94T<`*}go|+nd-AAah&)0hRPSC$^@X1w& zwFRercRc5T;an@B%6Em;!+hNd$-5sCs`RTKzJu~_L7e`QHS^%YdDHLz>VboKP(qdO zrmBbeI>Y5zO+uA^)x&*!S4eqPB%#Xv{p#Ug3Gwt+KCvaC>X@%AZXdkuspCt}c$dFC*9Y~m4`vVZb#)x+7j zuO1TCBhTmNYYB6yBvk2F>tT;_rp~jogsR*J&9^TWl`t07uhzqS`|g#5_e#I&VZPpl zTyqIk`c)6(Xrr1O$w;U&zOEkbGxUwS5{|ohpTT@BVJ?+~D*b9bTut=FR}#iodHrET z)>jV+>ycwP^RtVi@FqcX~m44O3 z+1cIUydx~3D$nQc!x&}e$WlU;`!ls3=4%OasU%eCS3R7aT_NRFk%X!|pPO%Ad?jIg zrC+Uw_s0_EQc0-tzEuxP)wfoYaIKc>Az^fuBQgn9*~5|AH%LFpi zzutOSc1ze-Dpl60)_f*BEW2q-SWA^unXn$F`)aPrbS;(nrV?htvS$zTbr#Jtr-Ul~ zYN^cEv~(s^na+eGQ67CPiK^^jzNV#XsgkOXaMbP_sa0hU^EEA9>ycD_gfm>JC=GG}c!KCUV`s$&Fwap&p zYg&3;@0IzQ{%?<8{%dJILDTsOCRHDi==#i)>NnrR_UcTXXJ-jjxwV_GbRVHgzxo9A zFx^L}GMznUq8>(oeNmsPTo3c@yH^tCt6#lW5~lkIRi?AYOw_}bMc-;dRj!Bm_T4K9 z^VP53D+$wmgeue7V@gGdNV_e$RPK@F zdYEtDy^=6r{c5QsO!pD0OlOaos7Knx%cXMfE!V?*`|g#5`RZ3oC1JXcP-QxM%tSrX z?r<)Zd%C$E=G%9#B+OU8S}FG~ zna&b>$LdEXjG!nIo8+4J>vUetUoVJ?+~D$l3Zd*%IEU*DTBUrU%vC85giY1Df7O^d#L z1_}2|`o5d7zRn?=uO-Z-l2GNj=X$UFE|GK}p~~+VRS%zmetm>0pH21fyt&_M&EI5| zP~|!F>fs(@pNE7V?vYjxOC{Y$sLJ)2iQlp?jyB57k)?zxK|w%F>>`pa?^I)qiAZ9U zh)RU$&6Nc81of&IF!3@x|9qb1efFp|w(oX~jI;mPqsCg#x1OpwtNB&Ude^k=`j>(I zuK^`g^|ouiWBo(I{-Iyi!(Rm+>*o^o^Zq-+=Ie^JuTmvc>6i5|UrX3aC80{c^l*=4 zz5cGqeBGbvJ1i2a{5z(shxuA3cXT?TO1~t`cfJ13%Y3~Gz2*|C{JXNOhkt7)Jw~YV zZ}8H?Yo*^9p~`!c9*%Hh9uj)=G0}C+*tjd*0F9)ns2mN~m(3kRHaLM&x5r zxGLk)^l+peixVY`6Z_ciF3#Bgm4y3Oec#7^H@5F1;l7XkF|U=sbw1X1C2YI?26}oJ zWk`<^s(M`FUXQE$zM_{<#=wLv5$nYPyc;UqowtF zZ=d;E!d@x~RlX-Mua!^Y*6ZCI<~ug-N;vNNE|9E;Z!8>(MJ0?y`#TPeUf1jWIOb~! zd#NN;`97Y!R{k2hd)9r|SwdC+{dS+Rtk>^so3AD8rIJwP?|SF8^35mis`i~!303`F zDfVsYF+!DnKI`GW@p`>i#(bUO`m83Q%J2;j`5KZa~*g(qn`w*Hl># z&t*1R)-SOe6MNNLt?BR64s-8_-0b~#rw{-gsT27Q(L0+7@?}SU1Jgb#t2o$D0x4& z#5*TFfxTYuI5c1D)Zc_Dp~`nE=CyJU(H-c%>nx$ly~Xr!Jhz0sR1&Hj=hMUWnX8w+ zVv$hgx-C7t2kwOQ-46*>-kbDr&EXDP-&K=PerLMp zhWpIBy!wAE#-80t`0VbD$3J=c^sVmslbq7SF-m%jQ03T_9*)#TvORj0P}RqF&&K@a znZG}M;h~RQ`|_>cdCC8^&kLp(ZFun7l{clcR1&H@Gn0h*j?GaL&QbcM$CYn=;dJJs z?>OJ;&{NlZ!r!0jOQ`zb1ONN<@Y~-0Cpo2uYe4BSLX~U7^e}ESs_Bu8gev2?^zeCv zSGr$a2~|F)NDupjPh$JiR|!@2mGp2OJ? z5~}n|51)DXOs7AUkx=Ecko0i>Vyqq#*28^`^e|sb*h?j$O270lS{jRYC5(4_d}V|@ zRu2j5(PL`!wS>J?5~}peYvt^`UNNGUwy|*LRGJagwa`#$Rt#C4@YWuruuG_gsMKa zJHoj;+;@Z}RP`~@k-B3OOKhr6*e6We^0QP; z)d@@0==6{<-=A+iEW0HfE0rqilr`TH9+us-CCpMaRVS>6>9LxtGM%L|-&Vp-SoZE= zzRse3=9EySUzW;zO-pw|mFZ4667|u?lBnt)=4)CyOVw0kgroM@NUf@Sn6GK+tVdIg z5zcU9vzn^zVZNrNvmQ+~Mi>o@MF6V0hxwY8&U!S}7~xKctNZSus(YBPY3cN6sxiV{ zioP?UhpO&jzNV$K9!)hyxck+2y!24jJ%9Q;iXg z&TG|F-NWl++N+QrO*KX|dUHLj`Iv`xB2fbSEtP>cm*PQf0cAN|o)n6RO7Qp@)6Gd#JL-bV3jNd?!@d zVmjd+G(AR)T`Oz5y2lt{NyZ5K#2B%9JuH>!eswLCJ-8F9#_qZG=!C7$wrd;9QZ>TQ zJE6+-7-0#=2>W~|RGIFCQCN?btf8v0tE-1O^-`%aJw}Yx+t9y(Q)^?28 zy!&CD`n9rDraNJ&Y-62JHP#X>dnX)U9S8k9OXW8braK{Rx)bJWdW;zJP_?>8C;Y~= zepwGyEuDmUnU?N^D$`?x&+9v(${DT`s;pBImbaxlF?OE1E~XX zhrjUH>!C`&PFRxu*8`SHl`Uq>!@qFsgsQQttIA(3>>jFmJ#5up1^v=PLRFs!C5&E8 zcMtP*zUqXkF%SE>^HujyWeJn;ikj}vIaS%h{k(gaujx+c(NBfxu4>G~R^nSXdZ|>6 zc~}oeiSD7w5+-5$@oA5rj}e{`?}Vy;%3F6=cVmS0Fx^X~s=u$uQmL{(ric0ZOwiBA zh+Y#Z?@jkGU+GR*bJJslzZKUBRoqI-HoaIe2nm~T05bt{|&G1o?IRyyyw!rRMy<|7-3x6 z302;kBy{&$8DA&iD#erieTQB`Ro{n`Q0023m&$dfCG3PMe__58s>VvC%2jdqP-UHx zaLwUe^Oq4jp~|&LCsetE&Y26RP?Q02Yp zgevcU60Pi>dg_F#zCL#q?=u$Z^pH@sx`*R@_fX|rl7#cNGqS&|+X+?uJtlfMbNVax z-9uIPu_)}@vTi3-S*Ik- z*C^aKt#m?_eo3@^`)eKS-iROy$5`MPV+-#8_q zO271Q2V!gK=Y$&Arm;A)%_@4+&MS4ZDY`{)~S0)j}fX&r-#=>mFY3UoRTm~HPY*QmJ+J^u9}3Z9)-7( z^f=Mcz*)v|C2KC>+$Y@$`-JHvj1yeZ_n1MI&$;?D7710Wd-VO1HfK0WWX&bK=YHM^ zRo%mHO!xcIRGqNJ+1g#bWIep@-VY<$B#c-$kLXW+^ib6!eb?dM!QN{nT%~$9v*r@2 z^h?6A&ym`6C%n(*)CpDH!*A@hrqiPlex8K;V(zN-UaQJ{d#NN;t?uExoAoeXTf6Bo z!km&YFI&6mF~XdZaIJ5?eSI$B_~E$HJyhwJgzon3PP7$-Jvcq=otDbaJE6)xpM>Sr z!*nMaorI%7ODCbb_3-l~+Kl7pNwku5!fzU#9ulUd*Nf0EJ#=sBPMELhF+#sis9N1) zjMx$$mZV3>mPD12bH7%q%qa;=($ZsuyE~mwrC%phdFHkgs!VsnQn}kQM)=IT6RO5a zWl7x8=^m61M8GRw7}(p2qJU#-gUj2u}icLe*HQ zRC!XRd#G}hNWv>>y2lKv>`{K+JESmLeKjGW%00R6;l9`yVW~`a4^^HQ?}RG*V-n`;O32U0h`zcrFIBD+yNCB< zjIbW2yN4=Iv3Ek1S2_vvbq(m}V}$!bolxbDUMIXCV}$iE-91!!ioFx6ywXXSuWL0w zA0u4jc0!dqVV&@P^qo7)t7^={nw##WQspW3PN?$UBw@a;W&M1Na9^Vns@xUng!>v} zg!f#!dsuVRV}z&JJE6*ZlZ5VL`wWfn*-Q6O<+GPgxGz3Nc+aJ~hc!1nMtF+76RNy7 zN$74w=I3L?*shu?_sF}4yYgd%_guPrSaZ{3gmGynRC#Zb(A{fge4T{r44?4%4nikX^?e2- zFC#L~)~APrD$jCv!Y9hcD4kH{skct38Y`75Pt$b|Rn{p9uc#{spM`b8tLwSqa z6PBc(R<|CiEMa=+?mEhI@10O(ojRe)5_UqBS0M?@?h3n~h?h`hozla6UD10QzZ0tT zOQPlLINu3X&Lv4WZ+o8JQ|z5k)#r0PoH;#*-aS-V!X$Jz%J6J`CsbLdBwD_nd+&s* z9=$p*ja@4Vua)yvmP&V{V$arhLX~w&!hDUwJt^M_Rr)1izOK(aDc=cI`Xyn$uIM~l z-w9RvC1Jj zDs)1XS2_u!JEK0&p?5-+&%csz)Hm`rBJYGM=aMAM*T~z5yc4SQOTv7utyiHFs`N`j zcU7);I-#mxEA#aUhEKE7Lqe5)ov`M%u}-M6gh@DBI;!=(B?(o1_d`NepAA|``k3ey zHQleSDxZ>=&TA#%6*WCZm{TWIt?n^K=wUib<#qR-_xZzb97X#cvV#&Lv|Wmar45^h?6>j;*5_(Vs_2sPejHsf<63)%&iRgev#sx`#1tCt68*r0=@V z^;z%d5~{pzSq};CU?*BhTur7&D@pf|Fx?6Jnj^L8ER}?<-OoFrs(bj2?aFj|G{Vo5 zu>F{?tH~t%##$94%-?q8;dv+!6wEU!J)bb2(x&y#3#hB15+*4A{d zxvG}#9_B0E39ru>p@;EY_b^}SF``ER=Bvu|n1?xa!hAd7h~rG==jq`$rX7ir@UEHe zrE;d%qqkj0Wb0u%OVtQJPr|$`m7gbJ4ae?>D%0ka9-hBy`>;v)?laTAIjs|_On1UJ zzw6ft^EEw2tnTs1`#xv-;-B1Me*N3eUUUytH~;L@r`tdC!Sm^_dekNeOR{=B#)#Fg z?l1oHbEi8Uw9ov*|GD4d{ZD?z^sqzso`3ky`)#5gs>W)r>OLEuGwpKgJIoi};w_u- zQ02Sn^L}_uT6&D|ZSCtt*kZbeD$||tee}O_!$H&E-0}x&kA2q}OIO$HqiW1UdW=wI zI_u$e-}V{LneKbW?dPXn@Ro%o?1U<>LMQ&?f4*?K-(h!{_t^R^i?g2foaq^_y8XPz z&6iuErBY=HyNB)7cH9Y7-Q&rhIBI&{lXhFXWV<_W{NA32O?N*2m)Ab{=DTiOd+njq zcGn-g_E%rrcP%|6RPC_+k<+t3^`f=E`rBpVpDsOW`t6e*u=c4duD$5IM;tjl`n-eJ zzIyrIi${O>(CL<6eD2y;KfU)-yOL0K{DTjhw)^;f*G~DpH+(otx58p{kEp&BH6wJyaQSbi&%29wRJmCmd172vw%LhbsM&u!b!? zM)cNhzN-AZdw2&;j}c=Ys#f>d5`-npn(J%s;1f!gx-0r{Kk@)9wW>t z3G*^NMyN79=3!gtgepte2}@#njQII_Sl-qjvmWlywOza<+AQNt)CpC6{oz$y{SHby z&S$BbsuOK=c63b-302l931>@H(w$J{9NdX^6*}Sd=^jRqwn}57^pH?xjGBZIq>-g@ zX(v<})h5x(?p5f7Dz9`W#(Hg=!@d9MA)(6np%d*YbizB>JywtOS0}8k_rrA7V@nWT zA4_67OVtQJ?}Vz=Jv!kxruEBGHB~46_|LYV{*NzTI6vm_vlbV=;TNU@A2825J>U@w zKTi+8kuco}Y15la=-$%l(Nun(gyp^22Yz+BE`<# zKY#zl`!D=_jF3*kZw`I_6Q(1-_lo&58{W3j&&LSqB>d*r?(@j$g0~+(@4W4SYkodP zNGIVpPyEk4r_X)ol=C*e2KFa5&w zt^aeLZ~LSTYkodPNGIVpm!9^6#f4A&H~D&&LSqB>cwy=;vdE zbP|4JzJ5MNNGIVp=IiHUgme;qWBRtgzQgp2-@b6Z%dyKRgmgFk;#=Huy4Tk)nXfr? zS~~k-e}4ZtkDbm~J9<83htoDbaqY3wbtfM^|J`@D-pwniiTJ{Q-eUUE zwU^8fIqAV`w(!Hwyv6jChpx>ZdHg9e+C)K zT91c5}2tF`e<`cg(jt=JZAHQL66!xE-dQo_gW@oiA;Q zFfWM@+-iqu*OM=tU-FNqFS1l#D@$ljolw<1Y~lL#`yrvK_Ydbgd#!yl>mi}4_b6Mt zv^_07BvkbtWlNME>rtv~$5|>zQO96=S0_~2HG8hi;-rsd6Mr5AT8Z-*KfAsvMP)utz!j^jS?pmAxxHyjJ$w zr=EVp;ypKRoUb`}IRcPS_4#wIU;O<6m&`le{Six7*SluRxc;H{n{IczH_tnN?bJ;W zs{ZnyetA0XId7i7e#3GE;0WNzu>T#lnVxsS`{(11JZdd#E}`mH@4CbEt$p7;Z+PeO z>}*SP6g}{@+fF|^{DS!|M=i%!5~@CT!yTq`uDf7<%*)PNs=2MtwtL;p+RlQvq6?r%N9h6Y@;+?jg4!-Wolm}Dr)ifLoi*R?h~>5`q3YmM zww>PjyBEygxqO*0K6iwB_NV`S@#Uv}Wd6w0mm_@%RrmO#8y3gk>?89--tyq3k-@po zIcoah{iZWs`p5GfU-$N^J?&MaRuM z?{whOwQ>bvgmm?TetG(f@4k7y>Xpmu4+&L!z3i8#o&Ms@^Ww_oK4{s!(*5d6sIpGk z2aQt4=0OSP!QZ&~Hy7Xh$j9e*f8vo#>p}Cicl916p-R83haGiL!S*KRP}3RtZvKitzAOZ8#mla7}YxbHhy!RV!8hw^`4J1@KUuCImORm*+ zeAFGLgWvG(`IHmRUb?yxs%&HF;fl^^pho}_s`h%r-KHn)dFK3zyDg7!#_Em|ePobO zb=y6^x%j|&AD_3r`teKAk}<9`T%Xk>R2lo`wQ^o^1$ojhY?yBR=n?ZL58G*J{*X}R ze3c%q8lB<#tR|uAu4hlvsT+=;uf6s1exhy39^8Aageu!u*28$m^-f>0NT@OnN)KZZ zdvNcy5~_?*(!=$cSGr$a301D!(!;pTmfu^ugev2?^sv3Udg&_`301b^^l%(Bs_Bu8 zgeu3!^l+UycF!fe=dM4~!??}Su8(jMs*LB-!?mnYe~kB#hPVgZAJgockoqDG5hs z?^+)dz0bBjN7VGNUD=jQCt;7WPnb@^Z%lU&zZoM`nKq{^mEWvRNZVr4!*5n6q-`(rU_O86= zelteci&{E8n##|Uuv9I*xrFYf_3IvEL|d75k4E_U=9Wr#>tVWkNSoeVLid(VkEZhT zBrNY(Yj1>fdic%iy;j0>dic%iqmP8?^zfV2M^OpW>ESo4&ju2v)5C98pVcHxr-$FH zK08a8P7l9XJ<5JqgD|_W<0nNW%MU%W(f830t3g8NDT1D)&ozscf&-!@Zg;mA%M) zpWdTXxwq5_zmafXD@*0IvV`W;302+07Or2v9}=p1|FDJI2ko0#4+&MhN7>q??P=*D zp{n;NTcY$>k5Xkj&QdvwItJUjI-$zGnS|r6eq$rGDo5?~u)XTn$94%-_O$e{z1qTk zLemLVjzmd#54``5E1gi~sFZ{~%GsySY7(mKUFqTVarE)YUnf-g6fg;Ujy=jJn4M7N z)666s)od|+l`5gir?Tl`Pj%+>iE<}Y`E)r6d#WwNC*z$^#q?Gpp~}<7>ESBQXvve(olxbe=_IVVk)9{M zJE5vyE5{H0#t2o8D_JUIQR8+`;CDimr}2|8-Z7r*5t)Q4jirZiiL1%Jf|O9z;}Y+)v2TxFB~*F;vmUN% zT(|X=mxLruFbD%)|^!xm?+?K6&qD(9>8a3r(!*(y7s zs$VNdXYZP`PkKnG>TTDS;i|FEY7(mS%ThTDyMpXjS3;G3>ER6Eils;45~{ra>0yNA ziq2J7CsgT|gd>0>gT0^=s`N|3D0QsYO4wd|AGC#!jiM5^SN*ac5=JljCE+&`_M1+q zvV=)E_c{7F#w20HVyo;e(YxkI)UTCMv3<~<)=OnQda3Mpt{{4Aw@0ax&Qci_*~0CQ zN!X(#%qa<5yDi+=Ckf+o`=C8I3Fkfub4tP)z%jUwyN>7H=RS(sGHkDo!C5L>qOH$# z681rRk?AD-#&q}an=wL_X>-a_`OWHtv~4Us{AP7R+BTLRezQ6uZSP7CzgeA-c9cjD zzgeA-?jxMvtWHSx9_2Ty6Vkn(`_1ZvbRT{EW_3c^-j(;*OkEZhTB;3nr z>CGi{A8S{tOq)}d%5PeFa|uh*(&^Duex8J79~-Y!nKq~NP&GF0sxoa(>7iGV)#x)XjgMyN7vPFbqe*HNpl)mC2*uD<46eOV^%%UFHCWc586ccaGkeOBLFvL%k~Usw z;P|0;jwRj?i9U)-sIuRrhkeaHXv^<}DtlTI_8f^mb4sYPr=^EIN}@*q5~^&+>EXC* z^wOgx303adT%g~!qpeCsyzFZ9__T5aZo2zc_J-|c52Rez7wiES(k)6 zbk0%EiJeg83Be@VDaJlKOQ`asV|ujHm_{6(Q00luB-*Lc{=8N~l_yWr!>j1kbr$V} zDz8WqZ655=tAr}g4yT7b#~x+m+zC~BC(*_$M~P0T>SIYeJ#NI29ulgYC6aK|cQ)`U zbV60XR<>$eyD@xvNT~8}1d?d=uufxyIgJrU9Gy_rweAOWLY3_;30EerSo$heLX}Zede}SdwZ$@smu51~7Hjq%&;}RnPV=dcTmP$fZk43Ga zt1#oo^pH^1V+Kb6N1{IZNT{-fW~q$Z9as9uAfZa{^zbU$Q=L~jp-S&0oImWTJ(7`7 zWm`xOXBK-`?@YCgIWY;xKJTWZb`p-ewo31R z61FSTyt z3H!Dx(_@4=CE<9b%JdjvPD$9KRGA(l%qa=`xhm6RggGT)tftEJ7-3FH*w<8<9wW>t z3Hw@0Z!V#GOQ%Ou`FSVC>S10Ie%?J)xl7RrRi-;JRw`AdEn)XiWqNZ7OVZNm(Nun( zgypqQ_<0hlOpg)vZRzw-WqOQojFL_dRi?)X$6D$1P-S|IaL$lU4^^he2GV)#dW^6aNvDS@(_@6aNIE@KneK$&j1j6#n^Ts`Xj{T`5=Qjq zYdQ&64X!47k8)+=ilz5+ce%#)mRv8{mRw(DJ^V((HCz&|`)o_D6O(XlX)kgunuOm- zxX+Y?Yh*_Q*VIY44p$|eglm2G0Qv~$eu6E=`=1`RME5d!OSDw(m-JG(_hLP4$5|?S zl>0utM@hK1B%L1aQ%RUp5`JSz`n8f!rFVLGKO}7Vols@JNy5HnAGGCnLX|x&344x2 zZ|xGQ>}ly?kCNyuQ9_mNI6dq|j(+O35^l?^mma%uGhlDD7R}wzqbM*1LbwZU-{*v%XrM+tt zJ?emaPoLTGtoao?oW7>YCz$EsYSdQZ$kz!~K8a1jC&$j5_U2Bg@`-X1J^{C7*qb|{ z$|vJV_$1$UW$WpLDxdHt;fVxCi9Rw&sPd#jdbE4)xY7w#p14TD6C#dXee{t~<;jus z@T80Lhx1h@RCxj>2~X@e19)#bq00M|ge}gNXf)UfRh~#n!V^@+@Wv9IQ02+0Bs>Xb z^kQG>gerSl5}s%?>UVtXgephZBy0_~l0F+qsPe>KdN`6f!u3c-LX{^E)58;(&d!eW zolxb7(Fr|Fj}ewO2~VyXGuY=lp~?{>3EeHb@q8y#c``Q%TZ65{h@%s#JYk%Kf3;<- z<*Kg}syr#3geSX=^t=k4P}Q%M-$*#Fq=$qmN2Me@X>Z(al-UVYp1@DS$jgY#IH(h< zjG~gzJxX=Re$%g9blkl2P6w`a!X9N$OTwAO8OJ%X6RMn1lQ5bv%JANFLX~$m2_r8f zGUvojsB%V4!idF4#&tp`RJqDX!oP5G6=r|zgerS;63#5nIF1sXP}RqhHeR{slpYeQ z{7bYX{L49aJd7VYp~~oEi8$>CiwmFl$@vRgzj7Z(YUh$psB$Js!Wvq0*E^k1T!uts?n?WKRqN=*?N+2h2_f2^-d>Lx%NrISoGw$kyDodM$E!v3ENyz&;_UN1dX-Ss`=Bk(G1y*^rIJvkcM|4n^dg;veL}*VlCV_LopAhc^l?T_ z4_llqu}3mq6UP$oW_s9*?1RSINmwfD(Mx5kb_HR7Ob=Dk_LU^;ced~zE%}XveI-4d zactqX{3PtD_QxK#`;CNiVtP2fI_^3%B;jc3{db&C!g1GjY>P?4mT0=SM0=F!-lNP{ z!gMc{Dsvhmq{oQvVOvsVIz1ZU=SkR>RGA(l%qaCsev z-ifh#n3sf~r-%Durd@w_LY3(xT$!5IuM?_FC!xD({W_t_bP~Fcd3fDTzwkBNOz+<9 zy!kPQpS4Jjrs{>;&FdI@AQC2Y@!~j zdOgfnzcE6Ue*Nl>c{G)N>0v!g_fj?07|}}DOJ&)+hxwY8PLHM z-~FbO&wS~8*@m}m?4?rG>tViQ*Gj^C^~+L8m>wfkneHB2q8_eHZ+*jqrWajv^t|)7 zn|i9Rs@KDO$F7xx`RbR~O2YIQp~`gk*b?<{H{w2@+jV-@Bi=M${epL_^-`(o^)TPD zYb9a6`emskOpg(&Om~khQ4e>ZK6Lrk(~JM+y!n)4&tAG8)>c)ohxtm65vugdYo&+j zF+!E;?y)86;jZI-um1Yt$glqSe48h2Sh^q9R#mTu`AUxws`SfirHAP;LY3+6u_fx^ zPV+H~D;9gd>tjDTsnScOs@KDO$F7xx`RbRYk}y3+s50F>wnRPJDy)~v^;xfn`Ho#H z3G>x2OC_--JjT`^mdbThuZQ_cj}fZ$%WL&t#lz=-)~VNALe;_t1;yhn)1_ zO%ST|OAqt4guPS}s`N__>*2Ya5AXfs#S5?b)AUzLv0;NSr1Pe zjm16^##gW3;mXB#emKwXIN-p2)5Cl{_4Lq3UAg%4|7V`Bx$5crbV8MWNtmxC?4^=W zrC)k@mTPQ{BjH-@>^-)bzI()Z^M*4|TWTfd>zT4!oWITV;%(2HpZ|_c5vugddYG>z z?4^=WrC)kj4^Pwm-JS0;J?+r9{^akuJE5xA!+b4aFO`HU{jyY^5gglRkZ`}`$WQDt zeR_|R=c|r9ed(T?uV)qi?#5lFU4Q(hdGUqidoH0$zpRJ(TEbo`303-~hbJ(l#|Tv? z-|+#{`+w)P^Faq4vWaV@-x#5)Un|d^dM5R*m+mz^?ES~i2i$M5R4NHoPx{NfrkkxD zJKz7ZWx_qgF%Jnn+#}8VVX36Y2vxlvp6nfqcO{H>4}b1H)73}({`{tgefA+)5A!w3 z?2)B}D*e*Kd@W%wm4qt&(!;fr=c%9kfom5>?{o2d%*o3uF9}tysnWxIUAOg>mxL<) z(!+c$VK0@0D*e*Kw(Dv24}R>XKKO}a=gYpj+@mB^^|ouiWBo(I{-Iyi!}IuK{anI+ zey0cQJAG^WBj?Y5?7U0T!+c$__EoBcD*e*Kd@W%wm4qt&(!)Ix{|e%lU$fnG@AKa_ z-|m>x7oAY$-boVX>;6pNVUbX!UwW9Yb?T*(P^Djbn6H0h@*g|jdb;3(cg^4V(x$GJ zD*e*Ke7y?2<`Sy(OAr5wMtY1;_1Z6QGrjlg@0wrokDIEweq)5Heytqg#ylkS=wqVm znz3xA?${xl*Vi^5eIm!^j!^;n!JVVu~B_~vPrPWp=HTV1&ym5ucRu#fUtB!<^bgNF-F^A2 zvxKTXmYA<4?4^=WrC(kvPpI4Sdux|a^|#yZIKB0GC(b*6ZBx6oo|}K`2X~lmchQOS z*Kb(ft+f}avae)4%-0h3Qc0-NFFp3Z!#30NE_nZZ+>uAEjg7k!j=R71sp}Tk{pdYE z$tgV?sr@~GtB$zYbkvFG&Kusjyw4z^s*mmFYYBU)Bvk2__3#%KjDz~@ED@?_ZZ*B` z(sSm!9JQ%uckQngvQ)-!{vAV);mp?(?ovx7p-R6jmA`l~HnvMRwts4ut){(xbk6*e zE0*Ic$8&!#m+H8zrIYjknot`ft}BJ>UO| zO?g(qd#@65OcjaQMCtNfi^V;R;RYFyd+s)TeyN}cos`Sfy?yr`)Lh7p` z2~{ur+uKfmGQV}c-4V<4p!*m8=Go!}TTQ!v?40>Kmv4$t<-SJN!+b4aFO`HU{nEo< zk#mLAS49%4?r`Xr7YD!lqWL3FUp@!m^K5^E?jQENa&i0LxM+UJTb9oONT~8Tch@4A))FnpV@J`*C*aM-|=?*G&y=I=ju z`R|G(RP}Q*=4%OisU%eCm)FW)SsZ)DBH=TZ4_&aw^o?^5pWn3m^0OkJ%J>_N&pUm$ z>7L&|YCh6i5| zUn7nlWk{&fFFlO<#^PNGoe&wLY3>btcSns@ANDn7+--SwmQdxh!t^j-cS8E^hlDEq(!*y^z6;{wD=u9;^u|l(t*?Ik z(vFveDxXcIhxt0g^;u0qm44~rKE5lYzABPX<^Fzp_?8e)Z}k&f5~`km#a`2$UU|VOOFw%jN!8$J_q()Dc?S9x9MSf9X(%r>*c?+kWl6G;q)+HR~CIWA)!jY z^ssL``}7${LX~|!J>0+WiCKS2CZWoGjr4FW>&(z+0|`~Ganr+Rf?nxeU0?6PxwTjKLwCbWnW1T=RQ|2eZ?Z7$~iGTe3mwL&n3L)K4VJ{*JsWQ zeKwF#<+?3Be9q-AUf;QsQ04Ql^stpUxA)muLRD{9=Ic74NBR<~^vimf?^yqkuz%>6 z96i5|-?3{Y;kD8)JzSHItqmkx8@N_a z5A(Hm^&TamO271Q#u=LjC7cKQ9OcY8HYQ3qCic13d@W%wm4qt&@>;oPH8$5uIM=$D zl^)K{WA%`*9(_JHUrX3aC80{ctcN|ynYz!;5~_M1G~cmURKi$PzpRJ(j$JDWua$o3 zVZL64UULam`lW|)v{6luWF%A>U#Exr3}fT2gyU}CXE0w&*h?j$O24d!tBJArO2YW6 zuRn~)#_AzqJ$ejhzLv0;Nd993=TzU7^wuGu4UwNivtR529!?Pw?5A(Hzy;Ksa z^h*zCXLpDDj&$k|y-4c$KN|klWnr{gY%Wm2dW~rL06V}7@Sj|^sm2I*zxs}s9;&*B`I?r_dNkD-;e6F+96eNZ4`&?Hea303F{06Vt(vNP zczsNJ71E=r#)w95u7@=r^N_IY{aSgNYRp4IkDu@PD{DSh4+-nhua)_lmi}4Zb4%Du z)l_4|SUvQxw%x;gO-pa?wK8AR|J&oI-F%*5>R|*p7WJv>^)TPDYb9a6`sKBfFg-@7GTlA4L_J(t zjIAbA^?I1^*tL={U;Xl0NthlZRGID`TcRGWSjSeWs(L-lckEh8n6G|$tt3p35vojg zk1bJ;w%gK6{>~fuYOr7 z3DaYQD%0I#OVp$74);>Ir`zjczGK%)!hH41Qc0K|BUG909$TUwJ`osu3ZSaj!+giC zm4x}~m)A?GUH;cwu>aTpo*f&z=Mvs?&y?joH(%*7 zLY02$;b~k?`S+9k5~}+7UGp7_sU=(+=$G{{UrX3aC80{c^zgj7Cye{4VhL5ALr)L$ z9a|4dxUSVNJ#Vc+TEbo`300o0&U$#_cP#di zFuoc)BfMUJ6KB4bu$M|gm4Abm*UFRRV{04<*J@*D&)4g`sQFsLUMdMyo=?qd<@H&w zznd^$OV~>#p~}D0$a?sf7GwJi67H9b{cXm2okKQXOV~>#p~`d5d9D0gBIz+gm49QD z9^M1}#t2p3oAmI!xqsE#|B_WgmFLjY!#%_?4+%ZoBTWxWB|S!{>h;(Xzhz+@ZIsy~ zO9@rR*LkhX*E;oI*pN`AUwXK9@-KkBw>6aenYYBU)Bvk2_ z9=2WoGO+(OpoFU4cFlLJe@NIr^vinqtH5LZT*7|de@EDSU9t96s)Q>2vL5DZ345s| zROy!ns`wkc@bFscH%5f2?)5psjd@7u(Z@vBHDlwhgyXL3oorXG2V70|6{LhJ z*9qxi{Aol!7KN)aE=>ajRc!Z@*y?e5}??O#c_f7SPW>~~}PJ`(Qx*dOy+`CI2> zZCAp!>u;c^hf#*~7@?}iCGPdOy6-D`303X^Wj(gUZ>xI`*6Up<=4+k$FTP8t^4%;& zl~C2+m15tP9wSuQ=d&K}8?V=UWz5$ZuFq-`s(g=4*26a(jXigk@VRq;Cz5*$>-C;d z^R@La}vy<^0D9qsxEC!xxBnq)oPjasjF1DdZZi@utWQ02P_vmU+)(;d&g%PFC% zzdO@-M|zAtWeVTf$x{Ri=~h%~@m5yd-?))!(&coFF|$s4{-Ydbt0+Uhh9Q z-?954;XUwu>sb%aT&>spYt7e@sEJQ7@^7- zCGUrCko5F#Kk+M}s=ss66WE8Y(>o5$*An(pNvQIjig~TvLv#na?>bAUa&Iv`9M3Ib zFO`HU$NBVdedg+=uUI5hxo%4j?}0lZefL8`mG>q+Tywa?)_2t;RJj&O51-w*OVM{G zBvkoKCq2&i@GnmLKI=pCx%WMEG4||E!e@71x#NwCPkrEhKglUQ9HXSi2vv?<>ETFi zB-^7`2~~Y;_iW5VpYiDF+KZ2yKlicoF1gbK_MN`9{gLzMZ%SvWBvg53CJFN$o1-M0 zqx4IUOP{s<^!$6gXWnq;X=^^=?@#q5RGq!YHq&>HIPWJprH5-k=`li;Ys2&~ZZoRs zk&J{Ya2zz^=uw7*D#yp}VGr)TRu5I(!+gi$D+%K({jwhBJ9e!kyjJ?9hx55D zzqfV?Ree4;-?1@K!ZA_5tcS6oqeLGWBvctQriXExBWfQ-B~%&DrHA=i!d@x~Rr;le z5wg!)`V$igRXwJ5JRhrvg!OQo&w7}zCG4e=P^Djb*rUdt<4E`%r}shgb%oSdMG~s? z%X*ma*tL@ITIrV_=Id4HHJ4DOUwXJ_Ikp!r;a+&(vvkcdHttF|?z$GqdYG>z?4^=W zrC)mZ%)@6o{i%$EDxZa}^^mY0?rWrn`C7tWDhXBkrH9edSiCD?yt|3`inZEU zJ&dWXM~|t^*An(pNvP5b$WlU8-?KDd>(rwR303;#wQ_b|ueF-_ zj>T6J##gQlvmRa_>(rwR2~}RV^srR!e)SzM301uw5=Lh|B9l!DM!BoncTy!(Id)~cvL%}CEwQOOVV^K<%g<6Z zRVOS}qtipee1E?6uJ3Q`US-cvyDRmM}}zRGqLMrpIco%5;{>d|L@SVcENf z`8tdCnNvcQepxE>H7(r%9Q;iXg z&TG|F-NWl++N+QrO*KX|dUHLj`Iv`OuNPqn$2|P? zlTN7W`z5+t+x|-k5~|#v>7{Zs@Z@?YR9V7KSX)cm300P`6V7DPols@E6V_ww>Pn1x zs50G4Wl4N?+X+=(w@!EuOn1VvuTG4$D^;d@sZ`mHJE3Z<9(vg4yN4=UOegfP&v!zV zEoKwMYqpu*z1w;7V-7!S;oTe~#;%pMUEO1huq0!IePWDQy&jgzbicZm${ySaRb%(u zdUV3pXWO-nWvLnwpU3ZI_)SZ94+%>+M%d>&p~`e8jKX@fWZ6}XU0prQsh3KX=`mug z=H_eqXX)X6UfpAiu(o5w=G_nL)UTDLGTjMFWgF{+sNx1*I^j2_^~-vwYUw1*%d~VSRGA(ld|uxPRnBmoP-UHxu)HnZiLvw4 zEnn&GA>BuYrWzwGNk8}6Jp6^nP1Iw$a59_h|SYipYRJI?V_W1c2;TiExsOqP@ zb$4|)MpzHiy;Q3FyW~!&vOgwazCIK5^D&~I@-{D3{miz6_hXE(9;V0Y(che9sZ@ET z(?fTk9Q*kg(f{Vhyj1nS;F0itj1ktu^jJOo4UkT#@=7P6yXOo1e2nk}ekWA*GxWN9 zYSMetJHK1TSL1f5XT{}w=ZPvede-gD_*Dr;_fjPSj5olxbyNkVsHDnB11 z{HxYZsOo>itGg$c#|ZDabT5@PH$6remv%yx_a+J5y;jE8Nw`Y!WPjhGmr&LB;Uq#; z-yR|1I@1!4)x$SpbRtwkrE;z2sc1`~p~`#H zOQov+a*H0StW$b;-Cald3)-Df<=yOrD(_7vRC)iCXl3`*QzumQ^|`BfpRq`%hlHxt zJz|`Xr>}abaxTeIId3~7`^&nWP}SdKqK7l5zhd7#RCN!_Zj|9$g3?1mm32zOe2qW- z1)@%<(k}_;rLk)z;k9zUN)O9!RO~P7c0!ePO2T}N!hO?9CsgT|M9a6osYyarU)}kQ zD>~ook)@JQrC$=}8!O0X+;iId=vU7Nz4VZ^PWb*P{gNueJ3mbV8MWNf@j9bis5dyn|zeD%0IVmFZ461GvWRgsSf0H`d%M zogNaZ`u&gyRb4?!xPt6Y`y^Vr*TZ~Gj}iKf5$ok)Nlf=rsp_RN#`TGk(Q$d)8jb$u?;qUF50mZYV}2zPfnp-R6_sPfEhCsdi<1i=}4cU#5?pGS8>)mW)4 zi90&oLshhxSb4Q8>V9eWP-O`_VM*L)?1ZX*t!(YBPU#_GzMjVKgt4gUF~al7olrGa zDpj64=^mk~V~JPPbdMQSt=`Yg*L2oH4^>7{ols>7JE4cClRBZwHr5GC;waGxRo%lj z>iWaa)5C8h`f5T#m3wmC!+o(a!cv*;9;!Sq-U(Iq$0W?xm5`s05q))MUaDLtb`S5z z7-2n3cMnybV()}1uXGZT@BX__hi`Y}yz@>6uKD>G;eJpjRJo(q3Gc@k;ab*o_fX|2 z_D-nsN+)5yuGRc}jBt(H303Zdb;A46ckV2&sxc31Zn~FBm8aM{p~`!cg!#Ib_46^p zeT`12a#y4i?rV$@-gD{hVa-jC5uRf2gevb%61tD=Gc>|yFWp0x&t5v=zW5m7J(unt z*4*?M;VJe`sPf(x8PYQmOJZUH4FBos#g1 zx`OaoSSP%?o_p_vD*ZZPN&0DZ>!HdLribpXqdfQC302mq6RIp>CscVAlCbQqu=|O4 z302l9JwUY2!IbUU|bT=yYY<(wGS*Ik-*C^bR@|{qnUlQi) z`plE^olvD;66WiQ&a?HMP^DiIkuPWSJt^M_Rr)32%G8~Lep+2Zm44~bW_?fNcS4o> z8l7+scW&>5DkF|0bhlJqg-)pQN+)4-XVm98^iHVq`BxH-`bOSH*Vd95V8qNc|PbLxbu)jh@tJxph*yzbufK7aU) zqiEklmQbZ%mdaJ5&kFjgNWwY8xn#`45_UqBeo0u~v2|1<`tv9WRbIC&mGP&sdf!!( zQ01On_b|roL@P;;^j-J4KI{ElLY3Do>mlJC>_jVxtI70eCFvd#raNI@bEGz%rIN6< z`*|l+bq~L>U71dgM)-LWwjc9#HJOCpxOVCu5~}n|kJcyp*e;>U{mb-l79OjI<+Z1p zPLD?Tc@k~TFosXU+M4b)SJl$p!+fPX;q@6K^e~?59_A}OM)U~4d{vnq^Dw7Qm~ST> zah%EgJU#rzv?Eax-Zj&`RL=Bz^tS7WY&}e8sT$$uNtlr*%S==}!3Ocl|nHzNW{B)jd9U!yTq`uDf7<%*)PNbPrXZKj-?z-yd+v zywlwuu?fPGtX_{XV)d(g`7Lib{o6lZG9PpDw7ADxZ&>`^4IAfUUOR1~9;(J_uIjqS zZ9AQR{sr?(t~h-Y9;$pdeclhRNlT9rzO8+|2wP0|P-VIkzK{Oo8+Mv@dC^((?T%Q! zx?UeuV;<6Dgeucn53l=yuibX~(cu@&cRA{eg(d8SDz8E(?)|tOrk$R8;ryL1oxV8u zlx?SX{_X|ycP?MPR+dVYCF~xySKDzXRCSN{pYz!1jJ2cZLv}cA<8|M7_;l{IN6asJ z#_1bh_0VbB@vX6AY_VLsq~%fu7c9y?uk^3n6(eRunfk9*~Y z>Db$!FduZp0~fFVa`N8PWpun(~TcJV*cb|J8jHTIT}3m^cxoM zxpCu9-d5n}olqq`M)a0wzN$=j4^^he2)&c=J(IqF(zjP8p}TF#&pV;Y^cbOE64uak z_h_n4I7XQsBUG8rdbE5Ui8`UGk5|pZE7Cnw8F6&N+L|6CENv$oQO5{Xrn`qK{gSYT zEj>o`)^5J4{JeX32ThL=V;-tj_t+AICCr-ZZhCAsP$k_xjCYNeJE5xgLETjuKXeaO z#t%u@`s@>Z^pQ~I*wsCZmW;JJp~@0=!rFQjI-#n27_k`17zd??geqgAB#hgP&U!>9 zp~`qJJsbyZSEk1ZbLxbu)jc}lH>V)5zHmCGxsxd;kxAvy;^Pgp@ zEc?&b!;*}(k~W?@g6Fl8P~}{bgniqQ!T#R~RefY=we^amhu=7gItF(_l~|2B>cvD_<1K(>DLKWrjxKF zUMnMxPN*{C=)_oSw^X)vOPC%l{_JgcbwYQqsOc=#mLRO5>0YX)+7g7`d9D1$l9(PN z%qa=;GCfA9GCk&DTj+!;OV|lZVtS1D`FdF1)*rJT?$EVeyd>Hz<4n{ERek;8Rb2fJ zN;}SHshX-2ZFF{YO%DlG)+q^ROI6aHP~{xliFOq_;q~brMv%5jW1{qsP-Tpogb}2X zrEzH|R2kJK(aP>s=!7b-bSK7oZJWcr|LGy2%J`uZ?J9J_JJ>x|kMvh3tgZLMbk<`_ z5MCckVmeFJ2tV(Hs?|L@;WwuB%ThH}C$8CUtLaToICpx|;b$%W;|pJ0eC(Ycp6>I2 zM=bn2J^V((bSI=uZ!V#GOQ%Ou`FRqS_sQGtG`;q3-#Bgm#) z=VOF)5`OcpXYV*Y@`Wc(UwXki*8F^okWRvHe)YLGn||XP=T0Xcd-j^2j}g*I_{}~3 z{ffnNZ+`LgpHJMd=I3LCbP|4Z^3(o$areKTr^TIKxsRWZ5zC*e2d>*r&HbP|4J`nS*CYWn6E&Y6Dc*fW++a_DaQ*iT=%cobe1c3hj*HXOKb636eg)%4U~J9m1{J5GzfeYcO^Y`W?# z=T7^bdD>E`9&^s^ru|PoV|v*Rrw3t9>G9at?=anZ*OR6Pec`kiqkiy&9i|UH`K0NK zN1ndq;YcQZ`?K#kz2!Bpo}PBl@_VoxUr&G9ZquE ze5T0zY|HrmzwI%-_mab>cR%d256ODi`W}DTm5b*-^P=f>H=R%HEzwf_>f>%I)vb=1 zKK|8BmCE*NJxhdej{Nhr0t8&y%58JDL zeQcLdWlu{F+p8`7OS^q*@!re-bb8-?4_%r+Bvd&PrHA*x`|r5Y3000tN!X*Defq2> zp~~Ks9@gAm`^8L1%yc_{YsQT}}a?Rq0x7;}0=ci7@d)I6k`<}4t^uW)( zX}Z_fPF=bm5~^-_uU)4L?s4+;%^Q{@07n2vhTFX7KNoL!@q4Gkk34E=9+XgZ;5W9O z?(!e!P5Zrbd3LrXI*R_$W!p^m-1ofcmu@=6oL5&u)g6C*o9WUIo;N+|r~cl+)@R$@ z^Ud2#pZ(qQro(>f484S^JzsOf;$I&0{^|X9TW;;PIOo2Pe|?{6_j~;QwDI!gc~C;t z{(CH@A07Jg>8hulz7+M@N^F(?_P#GHe(Qc8pPqfEm+zC;N+_du*%Y-9Yw3ymTBy3CP-}BnV+vba>*ZtIAj2OK*I$yrucGDvs zaOQNUpZW_0303ERd7J6eyPY>(|5NAmjn5t7u6fG8E`EI1KcC+F^yNrjLe+cja_!>X ze|hn=$6Fq}G%`5%IY<4$boc4wd%S7d{&jC(>gN)w?tS;Yr|0c?%(T_>_Fp2Lg^h~u zzSBO_F2^4=y?giN2w6haXK($$>6xEDb~^N@&gnZZIbUt{|2}m3r}Gb)9(=-?OTAV? z)kS~ypy|SIA2V%t)9){5+jRwDgmm{~cA36;(aF;nU%9;gkWh8xMY~J~oN)5=pevXA zpk?<;_p2+R$~t8|j8ezuK?&!<8~^Cbi@Ut)qtjph)gzbIgXU}R>OD$Am3~FbL#4&FHZ=QB4fwGs(cj`LX$Ywpa@X9EdU{aP8T+wyyBmr(V& zH|#nc_n9|MUwX+qmTGR-4g_ zEst=<>W&h9WROtx{!6~Rxavh8o&Lu+9={YV8RI&`^;u0qm9cMLE9WIwkpK6i7SmHU zynK4kq04{gC85gsDm`2^I>Yr@O+wYPzP{JAw#{+V4{p7@pJ-dM2lrkpp~^Ov^)TLX z_0m@?5~_@Y(!*H99^8Aageqf{^l*LVmF`zpLY3>b^e}F-<@eSup~`qJJ#4S8Uiykf zLY3_}Jsby(YI-Cip~~?wJzQsw-E#@=x$DpLFm7|S>m!_mD&x8Ia4l=p-y?kqRjzT< z!%^RsVT~S64!ne(7PvVo!Bu=!7coe-ic_dsLrsBvk2_9`1NJ>)UTSp-R6b zj9!eqdrOp1)%&3Rz)`KYL;Wj!Puee_GhZzSwDols>7lW-5o(Z?|+30HBp2aGe)Q~ZBAJ#zgeA- zw#B4}->gnZ+hWqgZ&oLy?JMcwH>(rU_WAVio7D;FK3@6F>V$OfQGT;JA>I4A->gnZ z_ZZG^RwtzGU3t&_W{j{GwRCzkm7gbJsakq-3EfTW*FDCFwleJ=jqvl$EtT%p!*utM zHoduo?k$}jP37lFSl+SL-U#XR@SD|pt%T|H@SD{~9|_ax;Ww*~q7tUl!*5of4J1sb zhu^F|t4Wwn55HM`c9t-m9)7cWlp$d{J^aRAcy*5^7g3D=hP zN7tfBxPFx|rzL`C@s2C5sgqFU3fpyi5{`-P0k~t4g!kE&;r>Gswm$bVdP}ra?w9mZ z*bZ0BjLVQmda~o3C*b!s=9|QT)%!lBvkeOVGFkp+BdTv z5~_NSvb9Uw)6zpiRqs)@MCq{}rOI}krE(N?47PW5LX~|p3CCUi#ztyYj@s#Ad)2Rx z?Gmc&Y3X5mwT1hHW)lSG>>P=b@E&;o9alP`%26o^dz7*MI-lfO== z@+n{v_8fbZPcS>7%BPu0II7uV`YKgIl}}~U!=CEQ=@aEnsPgG@682PEhEK*jp~|P^ zN!T)MS3cqIgesr*C*cU-$lytZPN?$KLK2Q2WA|Lbd+zCr^l+4M^zr0KCscWgBnjsw z=MPW7bV8M?`Tv+~@4mXB-Jt&WY*aJuu?vQHF#n?@f9*_ZiRih)hD2b7Fe99xx8- zk&J{Y*9l7={Ed?9Gyn3b6RPZwNjR1``}7${LRBA2+(Q~0uOu9={HwIAhjD^C9{xpL zCsY|fB;lOlsNF|u302M|>EXJ@tI%sMq0046df1j+4Yjvn1pkmqmN@u5=Jbx%H9&aYmP+yS{W7F2kmLSRMw-H%6{hx zqPKQ?lq%^gl~Iu`-2Rw^JxaoylCZVg!kvARFg~{r+JlpD?vpU5B%A>pgZsGacYU%W7DnC!cy^NOLTtfG;cBRU+Ic2H*rlmKRup}*=9!=%vNm%x=@k*6xb4m|Y zW8>Os>bFIRi@1;Jyea&gQ`rMQ+lWxo6l95HmCGZH5QksGHp)jp=vC?Qf1nl z(nHl)oT$pQIi-gxdy$_fq000aVK0(S4^^f+;WuN1D%0karCNO*wfb6Z_4VNDYtGfz zwX3h4T~*uq`ucqJJ%H8s8Qg)e-}SwW)%QzQ-;;4SYHZ(U^}Quq;@JL`r5f8KTYVqS z9yPYtCo#64xOsb)tB(whAA09l;{A~5qo{-``%QY-*X)C~{7$H{rzK&}k?1q0gerSl zdf1~RdITV$%66O{j=M%LJzA1b<-T8fI8saWs82$byOHVP*e+o^?u07)V-oG@o9(z0 zsvL=uXyc&wrW2~%RZYTK&7Ed@S0_~2yOL;6b-fCmQ00@~BwSV7GHeT-P}Nu4?J0X7 z86;HsBtA>kP7By7JE6*zcM|Q?gKfGKsyrEyL_59FTcU(2Pk5w9JB4D4>4d7@uDny; zL8GYjkWl3rog~_+AY+M6sPY;n(M~rR;dDZkC!mtBHP}k}2q&S+GneVnPMaAAbwZUV z(voPW=8We%p~{nWNw`Dj9OazY300mDOro7)?6b3kDo;A5M>~ya#L)>=p4d#Hoht3m zYb8{9@-#iXie6o3(N3uHiX_qI!5+OzsPgP^df0R9QAW<4P^EVgZM<@n=!B|1mbBC3 zMjYuOp~_hz2}gZr1Fu3SRP}3RtG2Zp!>5OYD*r|xiB=EmG)9=y7-7WG2~|BVX@4cs zS1b~$ob9qy?XO*W^eUms_Ld&b^hN-#`Z}S?zXeLd5x|kb7`_v#{F|mEoV}dYTvK;K zm2q$q?XSVy-RXoX=dL82y`0rtS$0B|e=C=Sz0+Rnen2Nw+1`?HW#WpZuTmvc8AYXs zz0+Q6jM52JMmR}$6}`Hy+d84jRbLXeYFoSO-A<@78cf2Ki7OU&MLMC%UXX+li;;}G zJDpJFSeZn-=k7ChLY1r6Bpk^c;rfo3geqgZ^f00~((gNv5~^Gwr-$vzmeFSe2~|BV zF#<5wvb|-gBvkcS)Ec@9Gk#1D2~|C2a0GB9>Z6Z@DqCol%DCNerH>2}s`O3|ucAHG zd8HGo^iIP0!=BnB83|Rkh4gS{v3K$@w`2~~P0VT&{JHYVzXs@?}} zagJ*Cg7lD3WoeTzUq>S8Ba$eVqnFB7?YwJ$%u=b6wy!J^ym7`B?wTP9zmc%7B;kx>3%BJbVNbOOy8=kUZzP-( zlW^?wZaQiw;kaw7^!_JdyE5I|mHoqX?;qwXVY-(}l{t+O(qlyTuw|$+ogR(w^CWB; zs!Wd&=9GkeLY3(;!km(@Z>usrMwn9)j#sKoj}hjSggr`?=`q5blCYnvGCf9^Qxe8% zs!Wd&=9GkeO_k{}!km(@ueJ2%61ul^dNh@vcVes_<|X0h-9weT6rE6Ix)WoiQf1l_ zb`MpiHyIQUi*ZfC!xyp7-8R*P7hV4#|XzL>GV)#dW>+al}-;;rpE~9 z4C(YxWqOQo?vqXrRi?)X=WyxtP-S|IFcy(c4^^he2z!xqdZ;ozM%as_(?gZ%PWa6j zp~|#5WvPs|B}^w_L~p*PlW^7GYNGcjS0=7ldOvrUYiw`H^^$GL^;OoxZzNp9CE>cy zw&Xf73D=hPBG;lx_>F}7Oi8##b~JEJorLRfRnkeg)^`t}k8tiM*kZi@>0wKBFQd0a zOXYq^FO_>Q*28w3rLsr4@6&sfgnLWU>ES+=ggGVQH}&QxTYe{0+0&A+=ScL{E}_bvmLB#fiQW<=RN0Qx!(QZA+ec9eRlN^7QcLuxPePTW zcGknOUBY(U300fuQT)QRE!=jT9zMHKZ6Z@ zs{fC@dk@>aob&%*+7L1tl?FqZVP={_hE(hN$TB5kPa}S|;b$`qiin78iZHgNwT|;Xa^HT3W7c2ivF_LNe7(=} zbM8OqbzR;ZNe^$jIDa@_bwZUlV3P30jx&JIrW2}sPD$9}Y>BQ0JE6)ONlAEv$~C-e ziB72UW>pg21atLbU+IJ@ds-6SXmi!?_}B?mj+IH+8f+zfHjq%|jlJ}6By)u8D;Wt@ z-aJeXZ(uq*JI;4Pl_N$c^e{a}7;O^XTy@Q0pYMb!M~ozNH+I+aolxb?+$3xbwh~ty zolxZs<0SmlmTN6{eVtI{P3a`O+3iZtTIhtTURVA_!f_=%Bvd&nCE-nb*X^z{JE6)O z_({0(az*Aks1vGOMJ1uTQQ22Ip~{|?gfojXj&ousR5_z2;cCKFhR>!Gs(i9ZxbkvE z=A76GRnDkMxMGQw%-60r{o9^rO_!g0%B&OaCfsEt;U}Ejh1nlFp~~KzgfojXj-x~; zRQ0i>jaQyIrH6znKZ%xvpPci=!}UWaRJr;{!V%e#+PS0?s+@_Eu!NS}{Z1!Txm!xY z-Ke`%+t_9ZP7rLNNw`yV2kDBV6RP^U#8s-RSD$}+NT{;)B;gLrotOKaPN;J4lZ3OH z^PnxJ6RLW?CW;_BH^5v9*(b$yUq+rI9mGr9p{s9+_fFsVv?{Wn(i&p9%Z`s zDD#yt-J?=vPGf}h7|}g!OR7w#M4DFD%fiWBQ+2}g3)8cw>@wZ;QSY1{c*LSdQ+2|qY*!Ds!xgi=?zeIJlRfUS znR2M=}GM z^*^7W>HY+pYK&-fK98pQ-S@D)x_UY0u75uL;@Pj8c6sHSH!~)x>ixrf^&2Bp>6cGX z57T3WD%0I#Ez04Fb+)AxRG|LL_K zf5CK<9qzT%qf*t&VZLK^C1Jk$WmFQT#|TxXyT@9T!_$Z_ykht1qc=Hmy5NatEIbcO ztE!j7e5J<-Rr+OJ>0x?|P-VJ%tVKCIfqM9>Zan?V>(8EEb?oU2&%@HH>g6zB=`li; zepy#~m>wfkneHBIQ4UWXk2?7B*}4C?ak}~e2QEAhORK7v!+fR32vz!JUFl(Zj8J8| zd#pt{JZb*+`@TCn<>~)CO*=kspB|N}UJmmet1Aif)i0xxFg-@7GTlAaq8x1()}wNN z*2`hOV|68AzWQZU5^Lcxw*N3H_ffqZ<|{o$sM0U%>UYJ%_kfnEms~>COFzHE^vD;! zeY(Z5Z6ae90Udkh5s|*=hi~!h>038Fd10?6q3WA|b>Q@w^PV+*@W;C>5av6!rj~GTpkJ25e2uV2C80{c z^zhBPZ`?m~{ku+2*y}~p;m^AB!V{EG_0aE3)6XC9qUo}m?7Kji@7R7&!hNlNSq}3x z!XA}`D*e*KdkfxYxYO~wPe=U0iPLSq^ZJF7OQ_o7w!2Rk|Luv>rPrLiK$!2?S%!pX zGWumX%-0BeR1&K6OAl{_jI9|YT>D)7;QyZOcj$)c-S^vPDLu^B+bL)5a^>uj8*G^F z@ap|GL#Wa(JMS3h~xrLzZq`s365z#PlukdIYO0w zSq}3x!XA}`D*e*Ka(J8WnP0l)^nuxFzZ!c_*-Xh*^>Uc65%#DgROy%H@Q&cvIfI1f zCAWIspG~*9`o!t0&t5FK`FdCJs1NZEy3rk9K0V}dPhDswmR!FvLRGIT@1A-m^`DNLrstjcqG|8lXA4nDsQT~c zP1D2Q`J(AopIan6Lmcyv(8Dv*d>%$6Jw~YN0!P`*rSqArC)luck({<8MnQ1cK$XSrW0Pfxbu=w<(?`%%-4Ne z-+4)>(l0&C*9d!55~}n|58JM{)gS(xX*%}ZFPc97t;HTCp{loC^BwCS67~=MvK-#W zAM585_VbP3SwEfef#*#dK6ci|^e|s{tbLa%p-R8>Fkd6=QAw!MFFiaX@lz0AxZ5qJ z7d+!F(~ie1e#%rrm1igEVZLMMJ`&q=YfiuPFkd6=QAw!MFFnlH&zQXR!X2jT9QF3; z@(VUsSE}?&5A(GaddVeJ>6adUibi^jP_@TNJ51O6z}u&%e|>W$*Kdqa)$7U;Zp=eM zk3J^4uNfP6B^-C%@8t7vKj3b%?;s^qxlc$B*PpJ)$5!F0T$iSYBlXxiQNnd%AKN{} z89Tp{@cgQu``GWs&V3|2_pv`_UHQqE=+uiAaZKvIiegE{HKRtYw9EjnyeU1!%-ubT&*lPOQ_syq2 zz4hW`_Um(l6`E8|t?F-r6NpJ@$QjO=lnT`sp?oZ|=0#d-Jz{&|cHc zzxev;(*IsOt+f}avae)0%-0BeR1&K6OAkNMKQ``4IPSjZMZY&)z4HgAXFhxJ1k#b( z?*n+oO?R1I_V#y9`@ePZoIyfWAKT5>2zyi#s`Sgc@*5Uh2ld%mLe)9H*kwBT+wYie z@tn=QyKA3T$f#Vy`8kHZhBIFy>`_Um(l4X(8!yJjb_vJ!hktyRY5mUcoSuIE;`++* z+;7#m`w`nu54+@p(|d2ZI2%Z)a-3f%2j9bCggq)FQKesc_zfgudmIV(I6wIPznZQ; zy>Qxa?&4m}U6J2k^0$w?&vfypkD0#lkW)7E)>2j9N13k?_NXLO>6dloH^O*Y-%scz zRIR)I&u3pa?~~J^J07|47Qk7}*{9Dq5~{}LK_l!@8Hpv8yC@P^!zx43bz;9yv;d>95K6Jg~r@imAcqSvEs-H91UdKEn z^spUgIsE1)SN(mZFQMwCXB{woWQXIYkNse=QzEjU$>jDqjn#uB~?Svyw*R*` z{`x$r?}{W;{qo6wG;RFq8Pkr>TAT+xzwkTHzO?NhOzXb(j{lEuNt00Jxki@5e2uV2 zC80{c^zd8c+#&T{k%X#0e8|?*k)K$a-u=+UdjP)A_B-f~{m9nSEe~Is?r`ejJpc(+ zzUR(zn6DA`s3cVBmmYp1*Bcl8mW70>cOG`f>5ktxY1-w4H!tj+z4hXE0zTmW2TgzU z$rnvqK6deQMG~s|JsI;g!XA}`D*dvq{FcSBcPtXVW4ZS|?>Qa+!xv6(zy0F7BHzmR z9gUaoec<$#la8AXf6C(BC<#^lJ&ySrVUJ2em3~=Qe(R+-#QLo;301rAdjIK@S3hgo z@3_+zR+hf=@H;av`qn+B&-~Ggr<>e-@$)zms(crc`_Um(l0&y7E;d^`{|&B zs{5RMhv`v|IeGf(^A_LH`##(6Fn#MKcbGnW`;(`=FI-&1NvQHYcb3C^jj%^0p-R8> z@EO>Hd#{yH<+Di-Po4eV+Q0hlFJ`a!vrkN)_|!oQH-scq^>b?T9Xr30@cc@@EQk5J z;^?ak303-~hpWD^^{#~L-M)tNo0+}0)^DCksQTV{*O_j(!}9dYhb?~c#l4gC7@^8N zRo0cY>n^PCyd+fh=i&Vlzi0c(5AQYo^r+WQ7d(0KGjS5C`u!5~b+qdvoP;X6ae9gYvf^uG#6oXAjuv6Vs7DxZlEw zmxLYr@O+uA^>ESuPJEXoVl2G-Y`fkEs3Gwz;zp*8u>IO&MXPVx0%=FH~ zZ@#eJb*(NvMyPTPpLOMXV1Fy+CBJ{*wATlZn|{9C;wLR6RQY~5J1JWGxXU&LX~^m^zfaaHQj4hLY40d)5G%% zdvNcy5~@7cNDupjZv^^V00~w0mGp4#bNAABEE1}m6Vt$^x?i2b-UqY3BSq}3Z>mL&K5B<`^ z{n^;MUBY#{`?l^e)(7>l4|Wgpb$8!)^b)G{%W|0SSY1h2SNf%gd-AcpfrNVl_v-0k zzV@!(qa;-6mmbbIWAmVd^I)H&oH@tFL<(bvkTr1&R>seNM zI6IG(L&9?O`P_Vsutz1KO1~_JJ<6H7&(0F6dLK03v9+j#Yf=5O9OgS#R}$8he(7Pp z)3*~s_b*ER|_ievE zff!p~x&Lr|)%PE+$i~VcVLAF5&U}rqMb zyCMlyeLgqevGtXN>nr`T9M+E!_NXLOS#RlKRAYNJ3HNHf91^b1`ie|KRrheD_DxfN zyCk8ikL`|dV`l&oo&oeR(UD>7d{@Hr-9DB$Mva||N_Z~n*p=b}YK(By9vi7ubr173EuH0P zsxiVDZfsUl)jiDDv~-rEsm2Ib17j-yRo%mUO-pAvnre&~I|I zQ}@|fLRD|=<|{o$sM0T=pdO~j2vw%L$6A!b72w#aPgO66`Ht0%@8hxv}xm4x}~mvtp! zdW=wIx_hieIofGUkIFNWUJmmet1Aif)i0xxFg-@7GTlAaq8#lMuSezCTQ7(Cj@6Zf z`RbQZNthlZRGID`Yf+ANI^3i3Ot+WAe8=ia!hH41s3c5}5vojgkF_X=Zv@8P0;uZc zFyFDdk}zNWvaTddj}fX&caOCwN85$60mr&L3u$Zs(7@-B%8++ywK6CGsrHA=Sj}fZ$OAl}3 zddt7x?3Ym0@9&!L*qU0xy@7sN4)Zm_9+iYD{nEqx=H4*ww~8fHc@I53%y(=*DB-?V zzw|I)BkWO0sM0Sz{2YU~?}jVGa3ET!+edfM#no;8exx0Le+cf4Qy}xj;(zpTwjgd5nip& z#F?)V_NXLO`5C-?=H4V9+v7;MR~x%~zFPN1&DRKfR1&JZpPF@L{jApKCd}6edsGst z{G3LX!%td_oij*yUNZLCjMch_Y`#X=qmoeNz2~eeKT9M%MyT>LM(N=*&~J=T<+Di- z@0@Qh@&K38PE zp3n3X770~;jw#DwzLv=ooldCIFA4KqtKa-WbMu0LIokFCO0xh_o) zN9wV4qJ-Hn_R}zj_{kzqD2CMa3Ddua0Jt_%R{x(b2mEVRq*3TvE z=l%N=9ZOd0JD1JZ2zyi#s{D@UtSfKCyME{^00~w74!!f>YW-f3`5Ix5N}3h`Ej?sTHl#zzLu%~`jCVwzoRqj z%5Oa#n?EF+Kl=BiI^(R?Z(5kI5%#DgRQVekSyz5r=-Apv!nIHTeoivrY}}P_-1WCWvK;MWtEe}B90SXS$Mw$0ZFdsGst{9f;@D}VXKQ`LTwDxs?X zR*HRFdW=wIpU-l5ZoFE*D`URSaD7&jQ04E~WI6nWqp|nS625otzlr48!fO4_sQDUU zk4i$7ziXOx_Reb-}lnEvIWw@-(j zvG_|g?wzE^2vzQ>vK-#aSgqd}F<(c!KEg?;@;6Pg9G*t4)^7uvuRDvrn~+fDZxLoW z{3T3JJo_o9gsT4AnXY%F#|TxfgVw$r#%|gOdsM1SC*dz=jlJ`d@SRuxtu5CH(qn`w z*AH0^&%ame_s`9D?0HD|4E(+IEQfclR_pg`&DW8rk3JHr{Jq>Phi?@9wblL$s}idE zJIF6RV9)7G*F1N+)#qkQzU}RAcqLT%yQWzViLr7>SdQ-DFOzyM-cPnARQ2C7wIxcA z5vqFIbuFUb7@^8FN3^s{Wgk-oRe1-#9d1BkWO0sPZ=zv#vZt^aQ$} zI!mbXY%x6?&yBE0C85f3K0Vx@xqIn57711E+tR~l;7Le7{g6=Qvq=y49G9=^pgn*Rld_nj|=wv;q2Y#{^RtXKRtXl_U=x?cXvA-uDFQMw7OK&{g|G=|LY3>e^zeOzHQj4hLY40+(!)OCo7n#LRYH}0B|Y3n`BtvKiIY&( z_fbA)Ph0v4i-anlf0n~DT~F)#3B80W&wA6tRx-Bsk#OzP+m&<1*t%W9b-Qy(mcx9< z`iF%5L%;MeU(d4oX_SO2{nEp6&=p5tWk{%UeC!_f;NENXP}M!mcWiwn;rdFyEQk4y z)s=*GrC)kDpWE_#YnM>f=X3KN8xtiQ6ZOk-xHfc@=p%!KD%XtZ;kwNcwU43_s$9>d zhxr;|k4i$7e(B)~+4n8|jfsS+zNU6OA1jB1<#3$Oa+t3X_NXLO>6ae%sIm7r628aj zeb9W}A@yC6gev{A9OgS#R}$8he(7Pp)pFUHCtVL3e4NDuQh!XA}`D*e*K)za8{ zSHksfUthUG9xI20<>+f_^EJXAm4qt&vaXz+S8Gk}nWd}DzOs~1)z2)=*E020hJ-5p zvaXz+S8K0kzGLev3D;Nd4YM59k7eqs3<*`%TY4Ckr(gZVOF~sIhlH!Mz9N%Q)jb@k zJ(=pKQ4*^9*zO4D>2N<0mQdBlL`MeCfZkPa4oIl#V~Jywr>gxVRYH|xSGFr#qUqie zo2nD`3DdUxjH;K^86S~|Vw>MrU0$Rrj!dOj`@-(NtqZqpz)pB_H#UF!o+oe&)w?|MtD6>V%((51{49}aKQWXZP1OlwZ*+P{n6G6@!hEfTv68FOFFkC1ru_tPCsdhU zEy4)LJo-0mn3t-4ULs*>`zOOBRCzwr%i(C?&Gk;GGQv(+TBGfRDkJQKGnsTJRGIFC z`rONaeF;;T(HT|3P@HwyC zV~nu0W5l(49+s)sl~I}Qgi+bXI-zQ;B^rAt9A6y={r`;0zet$wgtX~Sn6K$EV$4I; z$~`*aUrg(lFy!jM~0>v zBaEcqdu<+m!(%UpD*ZZPB>mF^My1LYGv?tZ96OoPs^9X~-QC?7VL43qs8sp6m>w%f|K%*BQe{o2hwi>P_W#F-{+S>1Qq@1fBjNKH zBP@sMv2yqwAe~TUO(&td_Y3_0F~S@8olw>9(ChB4NuN#kFkk6TSaQ>2gx?j{2~|Fu zBy{)QivK@Gc+0;Ns`@>B-MwKtM)=I7dsLR(^cdmqJas~q&n5}oy$|L8j}d;7pcAV4 zX90A77k75Y2%ouhkIIsp9wYo+x=yI_*(9O6YbyVLjPO&folw<3!>hYDm&XX7xpa@p zlA9hQT$gr2mCq&#-K{Iv*Gafb@n(NNp_fqA&*3Cgx!>tgxz9AhPN?!5<~yNkEGkv* zio1s@%anwB4xgIejMxcP?nOGG$`gc6sPZHviM9&szjh;`s{clngzIR(@uo**zJ6zF zCsgUz2_x}0u{xp32$OJK>-S~!6`6#p{_SECs(dy*DpmcPTl7$6nbO00cOT_9Xm>)D zPqP!Md^Vj>!baNc%C_M3G(p{oCm zNqEdJo$dVok5Bjg!9(})TkN}ss_tRzt}^_Up!ASXWtoyNU)P_015qbb>6e7_lB*1B zp%bc{uaYo!SH*s_ZYNY(rX<*7$_P85%34Um*qtq%MLVI& zG9}^a&Q+h^VAly%zW++XQGaaRF5$Y}xg{cWEfE!{oL*Yp^n z-x#r49!6riN2RJq#FO0JgELqe75F~XcW;f|%hl`$_>mZ^J4j}fX&r-yZ-%Jdjv zPD!{*b*0zOEG1O+Q#A=yeHGp!>FY#C17{h>l`Of0bDwl4>=UMwaGl_ezONZn?Y`^% zr%zt}tZBdFPWxzo$0DI>-M#vPrmNy>>)@^P`8VzS4Ie?i1|2R>EDXPcus{p-R6b9Qz!pO?SfQY)+j})jj-+ zz1DPkG{XN+!gDcC)q1a0WxhQs2~{iiaNf;wn6Itf^cZ1INtlKzCtzD?&bt$;#-cJ3PjtG6DqBn^jKuTOPN*`% zP8f;jjGa(rgh|+{$6ASm`EI{{hiRKD-#$I?nQxhO57(lm#|ZBycS6-zRI0qG(mhl; zN+e;8n(k``RrV%Fx@>= zd5gUhs;ucG%-21j|35}}9@Gg{p6GSL=P^cD4%6L3mABYCp~{+0!hGGU`Tt{td)!W_ z@+7PiK97EKXS}M$JS@5C9+fI@v3Ek1&n5};bua7xj}e}0bV8MXK>}^ip;z9=^>%YyWE}djk0T$PN?$MTPIYFMWxEyblpRhWlF*tbqC?QuufRJ z-h1zaD*ZZPB>lF!7l#(DDS;@LX~CegeoKKgeq$x31fGM-EYK8sIpAyVZQF@ zy^Y@qRr)2-@^zf=gevEfB%HV7KK(9N&MvvZhUpHk-fv+IC*fW9KA%fCb9xWGN2SUL zlhEB&hIi{bp~^BP(em})dnZ)&)vNQ;SY1h2SI$=%mF}*Jy<6W2RhB6U^K}*OP5Dl! z(k}_~b${kf`A(?PFA4K?N9W!8PN>o^3G;Oa=}q}gsM0S9^Yx^l-&U7UrC)lqS>N0E zolxbuMkk!Zo!dL1$`wZvx*L_X&`O(l2Fx8KO|K3*`P(z$3$z?bgx}iz9li8btPeqnjRy}sS~PJ?lDH_VLGF--hJkM z{_ro3qWuh6LY00QmAgjY74%(^gmZ>-$(V-`c0!eYNf__gKB^J@eUyYM>n)>l{pni0 zpQ=fy@=UIKxW?^7i=?mg-S@da>-}6pmGze8knjn1qDA6vGCf)(-9y53C+usE)TT2k z30u4W-w9RS!@t!qS@VC0Etb-NSsPJ7N8d5qh|u>mKGSJx25ufcdI2J?3Ff zoiN`{IN~^y`TyzRUraj^CE-&u-J^1**Q2*xM`X)kI-_cY|DS|;8I}K^ge4q%9;!^6 zQ+jy+s-43o;cuUr_LtK-p~`e8{N;E3I$^%1$B31C{QljpH+|;)XH5@0V)6E;d8s<& zcHfy@vCT)PKe_ci7Ve8mm|l4~#)y^I?)^S@;p{OFJ7+rKwbSfvfBgN~o8}*$PI&3G znR2KaE4iw@PQ2dq#ly~;o__u*oAFTPZ`0>9w@zAmjPTdmSBtR4bPrXgJK^uqpS%A} zrhDvh+O*@d7Hik~Q8nfvJw~W9o#n9J|L$|woBnv;v!+`-=M6I>?1U<7p%c&j$1SJV zeCF?`%P%-(_O`EGZ~C`A&zdejcd@RFN|h0I58JElxD%?n$4*<{d3yQTPn|Bl>B%2m ze}~z0)!|1@n;vq?M?d-EJ52{X@&(i0cbYA{kCISz%0u>?cKq3Mr@ePyB+j|Tou`YR z|J3P&Ki=h|Z{4)#^t~HAcRKu8cbNGIlc4n zn}0M*?r8AU-M&9NclpE9Pai+c{Qpjp6RP@n)jX_`?xD&RM<*<;=`q4+ zJK=~rMyN8~JyhwJge7e0F`~D2^Ht^lcMqSS=`mu=L)FSX)`Bp?EV=Hc$7Tan(%r-L zuB+uvsOo)CcU7(*x`!&)4@ubi>=S+Tkx=E>)jeD-xz_50DkJQKrL`71p{jehVsRzo zIw(CPRJkTf!gZUgv%VseQ000qJsbyZSEk1ZbLxbum3wr;zpQ*;yK+K5%akQ=s!sS9 z)8>>OO*KYH_txH2{{L?>Dr5icdKk%AD{14oBY4)8gevEfB<$Oc4EFy{sOlp_E3Gw> z9{$Bq)G@dds;q@h*fRPGz^GJpk2X3xf@f3`s+<{;aJIDHIiq$$RiA71P-QK24^`H5 z68^<<`2U?yrC%phnNGq;tSeU>olxb9qZ4DT-KcEsMwlLL{n^{@$_d@AQPUaKS`e1d zbdRd3)`HMG>&m|viRm%IoRTmv(_@4x(_tVdDKV~^Rp=+mj zNwitUnWz)0`u@XOT=^4}cAU?snyM3Rbar%24+&M4DG6swRnnbM5Qro{(mP_t=yv%{>8L@8C6qtV#D(G`+f1GYv$)a z`Hb1~FSvB$j-TIld9Rz_Yv%u_hkubU-3e*a*Ot(|rPHIS{Qo43_eUrF@$3&D@t^b4 zjyrv@t|UyShkx1rlGn_xdGWXAH+cAgGyi{#kWRwC-0D7WpWX40ug}--a^y$+|1m;3 z3IDRkDX*UG`T1|nuYc=%EcyRqgme=A<=#)&b@t*f|L6SRBi^#)|Bn&UN%)s*&V2p8 zZ`x*yao% zC;wk}-*4kxu9=^H)EgIWau}8APyX438-H~FZI`cl)cOU&{(Sznr_Wxv`=~=TXG=KV;pV}v*a+JCI|9biCao@doe)JReo$YwLm(N~y&BgO;uCsXa%(3s& zcinY%$@%{|KYP{dj)F3c8Pj+d^yXyXl@ zzU}gvkG|7VM)lzJcii~*%hy>x?AW7%FsCH;xXaC#uK33FmS1r78}_y3-~1CdU;5y; zueZG8*8h9oB>w$pf3V-3j=g67jk8aRzP;OBzrOF=K7Y-8!7M0L{MA6)l_H~f5l z>29wN!kp6M$|v4@cFCiEJb&M&lVXf|=8bPQ`_}P4oXE0;dXG|d-ui>~yW#DxnIH3|%@O7$@fY9RW510x`dg)e{qaT^bP z>Q>7eZhGWqdXy?hqV(_?`1~DLI-$x@DG7U&vrnJZBvjeE(!;v4*M8}^ZI-V4*{zqa z`s>9NfP|_&A916l+ud`U<-K;e*Fx?3)NC1>zHr^y&Y!<@e)g|!=4Mn9s*X5mo7tb= z@Q3pwe!92-KJ8CICBvk#!d;V_2g`c=;zTq8;KKQEn<)2@i4J1@; z_{v*19{v3-mp5#`*xGGz&V5__*UM&?-R~RoGcQ}52PIT}^9v`=KCtVB^S#cw_eZjJ zZ6&tK8+>~IrL(rW-ts^F<(n2(OA@MjyK+_J)9g=BLe+NHf69g@KJ%*iH{QMY%&i}L z?bHAM=-HF@{^op-0~gmm5~{4XtSi@1&H%T4{L!=fPT!n==*mS8303Dk=jhpY_WS01 zr|&Kjj%2o&-by4?J>i&bmR|Fjt(RYS)Z)nC>c!FdihKXny4m{6=BGY&vF%Ez+Gf80 zhW&4I)%=*xE)uTK9pOInqw6l+=0{sE|HI!buJk2T-S@0*mM;F>*2}kj$KDGggL9vA z)E2k7;p_nqx^#Zpv8ONeS_xG;!9_DIqw_u`Syz| zWC>Mo{PZ!iQ#M^RKl%~-E_gUEIbZF*bxBZJdEq(FP*IoXDPhPw*!kMqVtM@1gRr+N)92v&OD+$M|K9)G1yR!J= zPj0;Q$tQ2K{IkDZztBn~R5{LPIV`y|L!S*KRQ0-Yt!~TjtzAOZBY%F~*@yr2()q#9 ze9J<~joq{nX33kX6Rt8EogNaAZ(Wx-FS%p+!IKW0?eNQQ&+qsA#ZvI#&aI1t6j7BZs_r-Dh9*U-L_EyEwwRR(F)> zBZGvhUEcN=OFKODy35BcpRzEPxW;vc>$94KD%ZYQSI$fBAkW<8q}gL{eBu1KGe5p? z)+eFL`6@l!H9EufSxrLKx*tAwwshG=^J}h{&ChCLk303ZK)5B5U zmSKzOgsQ$Sv1Pbgay5`15~})I)Rmq)uU>KqRWY~6Gk276^ywpmM5yZM;|So$;EFu! zNLDHJYf0$d(%DLy%KuNoc*k0MBZ7{S` zkD?N$)5E{4d^V6UogV&W<+GZE>Gbd~E1#VuOs9u`S@|kM!gPB07kiQaKSoF=5r1JE zjQ@`w6@P)SZ?3&ZxD!>Z&TBa=lRJPchkel9gu9F+92wlPxRXi3@yc^1Prs6IzhtX) zf0cxLG6{1^!hN5u&wXMN?k(+)?nRSu|0-clNw`OLTyamGgeqzG?MXN$dIsQ$MG`(| zTZZQkN!a>4%jhl9s5~#}QQ2NChi5ezl|9OHpWdTXdA8IE|03bJRz_uA8KF6KLRI&$ zh3nU!hlHx$KWyRlLHlNwLqb*WQMPtzds=!(sOmk+mMA^eqg2_BGb%?>$6$L`Csf%t zlW^SCZ)~Ji<*1z=wpac7*e;>Uo|YcAS6jGmXgZK^;}<(NvLuilpgj8%haQiP-S0959dB-pFZPAsB%tB51)Z6j=su} zQ022p59dDDbA3f7p~^WiJ=_nts_82k303YB(!+k|&dX0ebwZW>F$qToXP-XfNT}*# ziDyV-a=1?L#KTY2bwZWvha{Xc9JT#iNGDV|mn7l7##-nlmr&(?Cp~OS z?s5ArRYH|*EInM8xSQ-dNC{PaUE*_g?b}zc5~_UuSq}F#?%Vp#OG1_Vo%FC>jm?7+ z&V#*Oxw3GC>#J~yP}OybEzVxsXB-Jt&R1Dij%2nzTV*Fy^}2F&_Nh7hq=$s6-ga#n z?i%~7CZS5djLKQq9b~Uv303-~hckdXmc9y?Q04Pa4_8R;=-h>MLY00=I085_*b6$L zO1~srrH=JliD<9258A@VMo|gdYxu>vkA$li{j%i#MZ$j52~|dzgma&xk7G;{u2^i9 zy(Rk89Eo~exhl2~+S7VemZL{yzjFuCTf03l{@e@wz2C1FlU*xFq!#q1L; z(e=4~&>ox~&V3T*l!P;YV{jjL9nXEveH682*j^ojGb&r6tF(iQ z#t2oW%_*buFDoacZDZ--Usg^?+s4u({=#@3{~xX0znHdnrH6l6IU(&RkskhK<%Dz} z;rz?W3F+RW{L9J->E6%%%gPDqKKl5Vl@rqTu6*YHWsI;FwRCzkmH(fFQML57C3H8f zU-uXz+-G`5W;#6@;r}OLUhY^;j}hjSgn1c>=`q5b#)ux3PpYNUqpAG=Bs|Ng=@{Fq zio1&Gq5D|7Qf1nlx`!&$*OoAnmQIhR^8b@C-m&pYm1%QI4^?C1t}4^!lpdUILoYF(p*!oJ9X>&>sRbwklRi-1S9DP`S zMla(3Gb&Z4_3MPaDCmevmFZ6SmoY+>X>-bQtb8A}^1a&1_k%0nbFO?}yYjuWyJ}ls z-=DAi3}EHw44y#P@A_HB%FjzyekS8-)Y!St%FmW;iDTziMm2Uuw(@g0d(_xjpTyXC z;B|Z;{K8i}HihdKLsC~^oXv@#2BvjeclCbAU^qEsal|3yz>`@YZ z1t6iycAOrLyRKgPYDq$s=YHwoNG;J6b_v^YCsf%VlW1?>q8&#|lu+eJ zlpbvy^x1Smm8YsnIIG1;b6xvL*az)h>CxWmS__>}<(uCm+*R8$Yzv)G)py(NEqfms zBvkn(KBH>41#Fd_2vyxXw_6Xk(DaZ{<;{pB+UKZ23tuV;UrXf_bWZxZ8O(FolxbC zv?SWCIoI=@Q02|KBs`&Wj&e@ygeq?cCedy&_SsoNl{X#Jqus`I#nA~>-q=i{-74+x zYb8{9^E5rIMQhhtv=geVktEtY*jKL-s=Pa#9`+o2RIHq1T_T}M@APQnm7_!_RQ0i> z-X5>(5((EOF-t_r9rc|Jtc5IxgsNUwwrX3uYxwk#P~~R?k}xXEVS0=(r!gW{9JO{s zRo5l$QzCuGBB9FJF3ZtA?b27T5~`xTMaiA%T>-f3%cvw&`B|VO9042|VhtZrNvQHO zP3ht6<*eooxf7~f2Pe@!4d&@iCsa9iCE@Jltme+L6RP~IToU$9d#&dIols?aOTwLr zJC?pnl~Cm>Dm{D?XRmdQ(g{_raFVbVtzGwRolxbjF9}<Rd=w0de6G#bF z?swC}R$|NOvw?)FzAkYE;9AS}mQhKl>T6L;=q}9FV0uWX>T3o^07s%e`bdPTt{Ggn zJFfJRK|+<@S#oR9p6a~P2~~P0;rwAw?JF4xRknrnaAvW0^&TamO7HZrg*)qeD$)s6 zdM9CvbLH&{rxU7rA9PG~RI?YPhlDDlO~QN~iKLUTPe_TY~i;2 zBDt-P**se_Xc4hxC-TR06N<=zVmW_!2A0mE} z9<~f4F`c#B2>(9`TZSsrV}v;+VV_WCdWFu|mFZ55MWxDgMEe`8-N=_($Z{A-OJ7^UNTN^Fs8pFYr;JLKecS&}LY3(; z!ZAuZJye+_yV)p~`e8;xF{=`2V31t`o7^#={7+9Ioii*K`u@8r)6v9_7x&9ZT=$ zo^p+yExBK^ExEtSa`+bs_i#zL@3SqrPfWtSrM<|#XcGQK!u@U%?vWi0+*2pvK3tV_ z67Kaq1Lz~1=LxnLpMQGT5Tv_F_3~#~GD9%5$IIqa-|A3Of3^=Ts8r z)T1(zURUO&O7HaWc}UpuJE6*clZ1WEK4{DDgerSl680R4-r6Np+0)X)9wpIRqJ%2j zaeCN`9Bcb1Dxs?PK}TwdzUq@u<*1$IaBP>b9d|-i?@_*qv4z`?(?dd)BT*8LA3i~! zO(#_K(Z^ZMS;pR#9ulhTT}k+c&(X(v>x3%b{3YR=N_$uDQ4*?rgP9(&8)fa|$kz!~ zzKKo3H^%YQ7H*;+PiLdmDveZ-oQ`7m6t0r*Fl|7CQAPN;H5O~Mt6D;f6*olxa2BMCp@7zCsg%yiK|psuRj0ukWgjoNx~hLJ1_S;olxc8 zCkbaY=RsRcCsg%zuNe^cn*ILo?GOA-Pr`B6c5I7D!j@>d zw?uoC>E5HvSHg6UN|iZ{5z=Es_pmLgGMyfc@c)yrErqI%)QyP$kJfH4Qe`@$YJ~rv zgrk8f(_@4=CE*CC%JdjvPD$9KRGA(l%qa=`xhm6RggGVQ=%dQ?7-3FH*w<8<9wW>t z3Hw@0Ut2==mQIhR^8Y(ARu1!$@c+}pb1~EIKRcnybQ11NP3zYQRi=~B-L!t4P-Qv^ z-N!tvchmpyhFfpk?c^)w=Rf(3S$Z^8Cp^C}z2_b;+W6qVyKH{`6Bj+2suMUgqw{$*)$hKC?bX%GdvEidb%#IlyYthIJAE@_qN?6M z%vZlLLY02`1obdIMyN8~J=UTeu2{FZ`w_DT9{;uZ4IX~r!t=1Ss(LxhS9*+4rC-*S z9;U|#Ri?YgT9m_G%1vMK>e+D*`O5sRyBztE9+j$I4)YzWD+%+}FQbw$Jw~W9-96T# z9PUgHde>8C$9~~!^XuRG9!otcRlOYMJ62Z`=Br;uC1HAuP-VJ%tVKCIjd=3EZd!N# zx!;{1e8gK8o`ldDfrB&6-VZPF1 zgev{AuJkZHMyN8~J=UTeo;u(0f`=~MdAn_wKlb9&_U%!r>g6!svAU8lU;Q#F3DaYQ zD%0I#Ey~e$VLdAMXT2QeJ62Z`=Br;uC9xJBWBU)Iav#;pVZPF1gev{Au6|cMd=F@u zddVeJUGcP?Hg3P~74y@NdgH9umGl^)O272*4d~c2m++ZicJeKkPX5w$mVf!E^$X=N zU+FPIm3~G$SGT>gdy!hFZhG9)~c(J#wkzDC%il2D~zdUz{jY|S9y+ULjD?7H;9pI&GAA0BJpoky|bu@#X6*-{$naH$$k>FFnlH2zyi#s`N__%i+D8&;9Yz##5hv_5847 zkJ?N*RP}P0uMzgBBvk2_<%rux+}?2I)CpC49(doSM?HR<NjkLP^Di+WxkfFMw>wa+RkLQ=}_WI3~ zTvacJ`5Ix5NSq|?Aj-4||cwX}HTm1XFJHGUL^9Q|fvE=6KUB&Z0d+xeBJ?4A! z_ib8y<`Sy(%W{~n5%#DgROy!<-oTU|BUJtKA$!bjcfE_}_j|@+3$4VG>o-QI>UHJa zQ}3kS|FwtCUb_D4^F7w>vk;Ypst-Kr(AhTk`1*Y3O^bwQh+`fSdU!^f&%>ys#|Txu z9Nz35TklG^-u=o>N6ilT)&=vW-M+dm%VEB*GW*I>LY02$VZKJ#qmodiUwXKA@;>#; zUUkc*Tm5S1b>a&acU}^z+*758`MPiGJ1+@U`lW~Y8exx0LY02$VcYe#`c?-YIy>y1 zU!Q;ICyPBwLRD|O<~!CuB%Zqj^Gzx41^G}2>)s+WA} zW*cwx=qu)ze|d8y*Kdqa)$7U;Zp=eMk3J^4uNfP6B^-C%@8t7vKj3b%?;s^qxlc$B z*PpJ)$5!F0T$iSYBlXxiQNnd%AKN{}89ToUg7d3>?qk0jJNJ?B+{gZyb>$~Z$J(xh zZFjq0d}ZSf7hN;oaML3fMrv0X(qn|GzAo{s$K8G3(MzcE3@Gc$Pb`o1K?(cdx3;~@ z(tGc{h`5Ix5NSy$drx8?WN zE}`nxr@m|5lfLln`Pmn2?zGl>^B?}>x37Et_Wv z>ES2($HrX=$KAJW{iBVaf5w&b!(O&{0_jNY_W|7O4lmib$EPlvZ+OSzIfI0%KDL{$ z5%#DgROy#>&<A&lzm5V;&NE*p9Ote)E&7{=U+eP_^^_K4|uqyIwH= z(3OiNZ{HQwPopGMed>yXX8X)8nD6x6&3PE%SX8RIhwFB~W9wO0@4d9|uU4-o9<{i7 zl~C2!?dI#K-A8H(Rr=*K_gl-{A@yC6gsQE-|A2j7b@%_ApZe6rdC>CSyz6`;@CSD z3E#1N=gddWUVi?0^QG+<-xc{*#_wo6{(yVWzJJkI=SM$c--VM2303_)j`!mlu`mHbtRkuF)#j_9J_uuEY+~s)-D@)&b_??-Lnmu^-flr=4-)@h^&*Mm_ z@?A)l!+edfMKYD-)H+BrvH8ETh~45 zA^$Od-zAG{I0;q0=gxANuMzgBBvk2_9zFwmaPPGes(d!-;igUwvJ9d60;rW$*Sq}4c#nD$85~}n|4_AF->s<-gyL}DkH#2*0t=~M8 zP<6?XcVBwSkG5R?ufJXV>d(XbC4SHLYrprdb=!UB z+w+5;x%inl303`miTOI(^$|`&m3~=Q){kjxp%bc1C*g^gCsX}2N1`KhX9-olD@+gb^(3U9 zen_a&FFkw*)ysPf%ZdYG>>T%Xk>ROy!boKd zRi5vshrbfy?X7-eOG4Ft?>TDr!5z+>A9v=*7e4>tT3vdKP~{pv%i()qe=FtB54+dw zMwfqee$5rLh5d(ws{Sbm^L1y@cM}q-^viPCx1D|Zj3c4SKA#?*U--tXza^7U<+(EYbx z?xpWoBvd&kribs+#-6!^&)j!x>EZs&nW4`H5~|#{rHAjiJjLrLcM__6|CJuL66f|l zJ4>kQ?aF-JC-jxRgev{A9OgULKP2oQ`lW~av$1u%gzI+qZQWz659(nb>>lRp?!NEn zB~)zia#?OnY`NvP5LP z5{`*|t~FmH>`_Um(l6`EGpn(=R>HZ~v#j)Rb{;E-gyrb-x%nDlk4i$7epwEClrwdo zoh4NDK4`vUYf%Z;qWWby%y+D=B&;j_(!+eMgu6UseI+BI%Jp@6c+N03 z?n*fB_HzdFHNqa1gev{A9PTE@)>jg)uloMO71>xhBrHc?!6dloYRR2< z-)&2%>gy}-l#G=_!g6@mB+FsGM%bg0P^DjbI6Hef+)sohRQ37Xa~M~dePt=3%JZ2l zhxr;|k4i$7e(B-t><+2#iX>F^`P_WR)>jg)uk_1uSU*PCqmoc%y`_gyjqTMW+^h9+ zNVq!dD>4aH-NTXEH%qJ%2b>0zHRZOiY3D$_|A)tHBb`Tlmx5wQK^86S~|3o6`rpE|Xrhhj*Y_DxKkR_LJ4)3kqe5J<-Rr+OAdYB#~ zRGID`Yf%nYfMcsZRlOYMJ62Z`=Br=Um4xXrLY3+6u@>cUXEC;$P}R#}zGHPIVZQoh zT}hZ8BUG909&1q!cdTQ(R8_qk<~vqb66UL4)|G_mF+!E;?y(l-Xs0bbD$huIIm~yg zt|ZJ?zl=)4^cbPaboW?`aEWx9K;MLF8(aF5C} z-Chp!9jhw|^VKh-k}y3+s50F>)}kD~5g2<5psJU{e8=ia!hH41x{@$GMyN8~J=UTe zV|z7A>;9~l!+fR32vz!JU9Gjp*#5()+(-3tM7~4&a|!d+FY8Le2&Frr>bL9Rjg%-; z{jGQjWAArZ%vXAhP^Dkiu6M`Ao`;0b!#ic^VZPF1gev{g!`ryt^6xkMB~ukP8*rSqA<=yHmhc|x5)>jg)ug2~OuhwVc%-0BeR1&KE3|`ii zH_6BLI1=vF#_pc4)_qa)HNqa1gevc+W?fl7tM$1F^EJXAm4qrkr;+9GlNMv=3=*D~ zjD0p^weBICuMzgBBvg6tIqS;L5=oB{s{D*mdiV_V8zWTtY|_K~=6VDmcUicOc9q#zmJ+I5UuRvJuVw1rupyyJzw~hL`_Um(l0$cBU!D_6`8N+GyQ}`LY1Fm%5s>m zW%5L)6RPw}!hBcjb6)0aE%cI0sPc1VSq?vICp|`}@-ukpVO{ArMyT@Hq=zHin1_TO zeN1#;GdAu@IPSXN$#S?Ka5vd^kP@ogC!~k#Pgmq)t8i7WOVh)VdTgC2;X1L8?VjR{ zonJ|Ke$~%?>~~}5J`$e$*dMd5{I2t{wku)V^*hkh!&Qd#7@?}KOFZjwci(sP5~@4{ z%5tp5cdPphR_nJ?%-1sYZ+w?f?_xDy zBkbSIEuqTqe$Be_S9@HM_f@!rs{T7a?zUI!J2TDK2zyi#s{D@5tSi6ubZq{RaQ^7u zlj@AKTEA&wzDC%il2GMuXk=aaZJ}dp9|_k!{rg2-Ev?q?_L;8{_Namw`t3~L#I4qE zbC~bgxGUkf>u-VNGxrw@$JU|}u0{KA9J+d4t>4EnUnA^MNvQJoc(ShiHg?af`>C^p zs{Z}$zGGRf@7XqABkWO0sPcQgv#$K*6Hit9NvedZ{#z;bZRs&WRUa8VH(ssZl`&su zxIU{%sPcDg@_G0RM`Q1uC4BGPe-p{Gh1L3q*p{-#Nm z!_%nM`fWh-b!X9c6B4TYEy65^zl0elp1;B0kaoSJU;njD301Cx*1jCZZrTWYRH{rT z;V);6z4Maromc;@E!PRsV}vT#4_OY+zgO$`&&_x2c}VyS{Jr%ohj*@aJ>*Z8UU|ZH z%O893Y5V%iw4G4pU92R`*O91?J`$?*OAp^D`fIEG7gi-y^>>hux$K15`wzHezVoJi zmVDdW-|$MP@^?+M91>&Ykgy!x!(S%#T)dxbOQ`C59wSusw(D9%zcE6UYm|H* z{(_{pfBTJJ303_!C%u8aTEB5!)fGs@#jDhwpAYrRXOU5~_Ts zlOD_8f854HpSsoZhMOKa8+&&r;k&!-e({x!J6v?l{Qu1oAa|Se)Gl)&V9~o$A?|;>vTpXp~^cmNto~093|l#rC)mN zcGs`(`?k;j>Pek>(volZ`&)epRj)pM_l>{!!WF;HDLvc+N{o!+4eI+BI z%Jp1&_&&m#?zJnS%J&rMVW03#Y=8SIp~}9J9`1^KE7#w|NvP`kD4(;ZE&YT=LY2=y z%i)=>r}c*&y=mQ@FZk~K>`kXEj6M>oJnKylTglkkN5ZvFZ&%J4W9xPa*X_mL&K5B<`^d_BwRr%@8B^h*!NL024ol_8+Hzq;wF14?z9nZ(gAz?Wj=d&E^Gzx43Ta_lTz!n5#xX6c?|Y}}P_ z+;uOK`_Um(l0%H=ixh@{#HgpmG45*!}E)=a!6PX&o$D+e2uV2C80{c^l-H_ zw%(O+z1!DUu8_ydAz?ZCn%aDgutz1KO24ctXXn*gQ+sCVDzmRFB~OKk<@K)ypB_>a4HGBvf?| zM`}-|`e~Gesy?W|iKcr? zY^qM!CrsP&GpeTQgi$p*JtWNcw_6TlH^Q-~R9U7h`C51wyJ;iLsG6!1mc#T|$yJ%o zsLZ!T*a>6r9_H&T+GkD)Rr+OA=4)EI6RJ#i!jY(tK1QOddzi0j>5QtW#t294v5{I; z_b^}6(piqC8Y7(H#%48D-NSrMOJ_NnYK(9-Ft!3v)jiDDv~-rEsm6%0GXOn21Lz*+ zYg#(X(NtrErxg8ULJw8l!+cFkXE~Z`jPUfUpLpq^s(YBPY3VFSQ;iYMSAE9OLsj>1 z#xdPzoTeHh8l833RNce+F>NiRM^lXvjlQ-XmVC@Z!q|IV`I#Tn{oD7NsuO-b%5?uU zc~gxMEy8>rdKi26@Uujw{lrjuG*u^zz0v6*VZN3r3G=lU#!9YAzx1&6nf4RFols?Z zwFn~|^XT8OVP2~Gd5MIj?canTp~~}_UJgft{w*GQs4~LtVQI~&6RM1`6V7DPols@E z6P9DFb|uCx9q1bSI2`<-}OKQf0bFrOI~P2~}g|(8E68Jyh9Z zI-!Ssz7wi!F`e)UnjRy@>dMlt++&O|k}<+QF-EMs97bij*RE07gFB&W?3r7RPT2Zv zySA~6suBKwCsdgpBaCp2u+MiwmFZ5n3hS#SOQ>qBcJ(l)9+fK7W5ig=&DZpA(!=Mx za*r{>(vA_=?s-_IUROqCx)Vla8|#Fsv6g7;op5}09Q6M)D*qy3x)ai-J7K=2$A~cx zRV(-Cgnu!uUzS5vODADorlmWf%JdlF`}$6(a)#@KD$A6F@wRj)#_m(Me5JdGbRQX- zYK$7Vr0L)Dmv<#3efC0AvHN!WgT+vER_5&b5)d8z8Ryd~V-jS-f^^jJCkTyiH=*&mb8 z-FJfi{}|D4d7GE2erH?4=P^cD4%1`h=)at0RI04$^w8Zm$Nv8q(LeKJUaI;hcqDut zV}#`}Jys6C1EdqGtm!0l_kMx@KSp>1zZ0tZ9eUlpHR-eI9_B0E2}^EzjPSeSI-$yE zlZ5WxTk-$L2ygj!LRG(~ue&!)#|WRfbdSoCn;s+lou^Ky^4TPzyZ52||1rW(5_CdU z|15y+-o_mxeCE&j!cVn!LRJ3^ukPMl9wU6_(mg6m zZhDMxUD^p%KAR+Tx2{}YC*dx|oBjQSUP4trhm%m{ey2y}KGO(0p~_#x=!B}Vs8qQt z?jEWvQxfhud}@9(VkcC&7wLp5PY^nx%9D^J+A7R%(C&n){u@<#xQ_N4Z@P#1`kkqr zP^DidjKtr>>VzsIOu}`o-WD=_Sw~I-r^4avLRP}Fe(Lpmc&p(M4ySJV?p{noC-NpNkMLInsRIS{@alU(~axO{2dD|J;Z`SREs{T7B zdN_0XE%x0*RrfGaBcqZ~rC$=}8#~DF++_B+t2fQ> z_x!__I^pj}>6e80dTP*raY{m!e(B)}#MnG2;XLT~XLb)&M%W2e)`eW;M3D@n;CFxPB;U&$L)lw?%`i7xiy_05~}+1kWl5`uzRTLZ~MZ7?+Ew*yNCIj z9wYw$<6$JGdpT6~lDo$BjgqV7^pH?xdW)q$!N;U~stk;g{ zZ+`Sp)mQrN!+nCi*Gjlc^=W3wB~i zsao%~s?4`XC828N9?rX24)e9On;s*~DGBqkwVNIz%qa=?`sUmB=Ms(|jw{_mm3~R+ zZr|=i+dHbPrX%U9}qZytI3$ zGQv(6iRX-+P}S?oR^7^!9uns3ZTwES7BxLact5!ls>Y&HVzuWSSO6cQKA#7x`%Dl{fGab9{xq5?t4XMtF<86RLbRN$Bp1%>N%F#!l5#c}Ctn zJe40KeCE>K!;+gGBV3nuLY2=Z3EizL*Vjq7&+rYOZ~i)=%KI~&Q00E76Yf%tuoLF% z9i2|7TDgb2;_ji!G9}@j!>8s=$4;nnFVYEBo*;BWRX=BN<>iXZyY=ZIp~}15o$!sa zYm`o?^441?RE~*DW$J_~BkY7KYat0^cZc0?#7n5MOzC01?&!UZ-w9RvCDHPAobQAx=aM9xx4lpA zE%r{R>hrlC&Ya#u?;fg*FbUmVWq7x~6RIpz5-nfvy>~)YU%fgnjn$Qeb>)1OQR(ig z*t_+eP-U5tFke^U-jwfzD*ci$U-xI;l<$Np{gNo^ z33sNR6!hEb5~}n|k2dRj8^05(JlE)ibGUPRCseuONJ4j`vKBg_%9>8X)t#$8@1b`> zmG8fjaMX9@?TWk;s+>!bFke^RuE;x~O1~t`*V0-GolvD;61uB$ztag-y{^pHHyFOn zN)HKD`gOvR+r~Pf$_SHiv~*PKXG;>Q`ss&+sy-XENcxy)jhgPYtID?|rn9ajtWncr zggJFW)yh4_2t7<^RMxxCyw4y0#Zk1MAxo&zFQanT=(~cxE0S=|a4s41Fv3o#(k}_) z9ot7WqQ8%lP-VSkRIWc=tM^ki300oSbr09LooJEtmA?Bv_h-GIOQ^EmvK$gV!A`VD z+)buOi==xwk7bVk(( z|33-yGAjQ+2}?NkJXD!Br}XgtRXc}G!rwkK?JuWwLY3)G_{;D5b;5j2j}a^P*!6Bl zZ`kS;SIy6V;u*8cSJ02-T2>bVRu87W}KK>S4V<^~?h{{CLl+ z=BGdHjkAa_JS0?E3t77o=dC|zzZ>5En)xwbI(4?qeE$vm-{z|MF`r$mD+yIb*gb5o zw&PBy>K^BBd;08!yKkBwa?bjXeDez@&OWf~h4a16x%Wq&{`W`Ep0xKj=X)Hu&r*6w zsCw^qFPq)@ZkNpWShq;L=>ezDZg;ay^QWKrseSJLzg|9j+;=aYAN_=VXK(!UF|$)P zT{J)X5&JH*D+yI+?sC%Xu{XYOe%zTK-zTGTH2BhS+bmu8vs*7;_1EiX{(mP_Nske| z?V4|>YTNzI5s_20c7JEm-=Fl?E3@Rr9&M?XL!zc5s-_CRAS|Kj$f**cs^xI>F&%Vx zRMn7&`8pEi^Uy<8AFm|zhZ^pQ~I*wsB;ExFd}geoKKgr&6>I-#n2xMFc7<2ookBviR3O2T!U ztFyi$lThV)EmoC7t`jH9!)hyNcYy> zRQ~^OGAd*L?Rpr=SSxAcxg&Vim4qtik|gZgjtutyPN?c5Lo2N{k{(thWR+6h&CuGJ${wXP(rtElN%!}%A>;s0kj z{EKP*IuWW`4kNLyTydm_geq4YofvEFdf3{HFg@D(v$x%q6X9-+nvQ3_7K9}<9nU|a zs;XL!-y9J+{Xgv8d))R_S?7HPcR9EzYiy&1Iy!C4Ns|e<&+|4aAk5Mf9VMd;JsE0> zL)wVDOf;di0ud|)%-n>HHr6C?UuuAsPm`N0cOe>T?suTfP+~myaq&8z*ZWxby;$e# z_PTstK3#vU*YEs()^V)$TbpC8-@1OkZSpGHXmvtq=Ll7%drM3IpQm6!@N>|>|VJ;mv-?wk!BfZqD-jD>kn&j{W~b##`)MvRGIGONTai(YbI2+ zGBvN9EmcW(8xpG8j2dms)s;4^pKQYj(pK4GqUM!^Dr3}MawAA1OXJc^s4}YUMDpEQ z$V5}c(MKC|y*AC^-v92Egev2QOz5HaCKIZ%jrAk_^$APs{V?6ju@i*#qbH`jSBda? zCRD9&BNP5&T6^8Aq{_rU9rJ^)`^LXI;PpRn$~lWa`{PG$roZr@BNkroHvC1xbS9)t z?<=A8l-hI!!!0U5_bSM1fu#f)bW;u@d_)!~PpChC@;V+MP)2%k|$CVFy@rKvu z29f93kBaf3X*NeU6asgumDyy*@`scfw!H*XwhHbSM19e7!zLNO!_tOka7}eKy3d!9h)v=RXD$cA{`@^x{mJH7^5ftByBGY?Y1gGsz2?>zbgvv`E`9D> zHb;iVIR`CXa^PDx(--d=&zCs%o%N3E`uV^6mopaE{?4s8z53hRojG`Q>>IE1_qazK zx?u~y%WwSBCUN=Q##1lX{-+0S{_=_=PHKcX^>RG&JAQSu7d`Q(e(m7a!hhnr>;H7{ z(T8;6kH6)*x%NrVd4B8LkH6V<6wi%r%rs!3AfoK9&pj~ z+Zgqx4_iGQ@8~@?r>(USW!^J>& zpKTexbJRtf^beo@wq3m(w!Y*3{JOjO;0u-1C$}8Ke<@l!Wc>ShV z2cGb4sN(Q!ZZ-J3Q2FTZ`}BKIg&d;a5f{qC|)ZAF-u#LNHeI{KlX9b3C~rHAI! z%ORmE+pvX8=ldbiRIz_J-`Q*Jo4p(os&bFAwM*O6x(x|cxkuR&rRRE-D%)}Q%2CuY z*xr>1RrbwJIPPk1ZlqS_sNHSYUbUCUb_rGXv~I)pY72kw(GS_|!B2j`V;7lFC z?}7K~47KZBvt``cr98=^j2iLLUY z*FNcWMLgq#=L~0rB~;~hWmM$d%y&>i)nPBYu69p4ZM^5!kG=L@fARLsx;p-VG4_#A zWxe%2XdLAX@aB(SXXEo1k2WM!{rIKV5f6OTNH~(&Vsa~yQ1yR)=(>^N!LJ@i2BQ~8 z=c_;Z-J31(i>HolS3=bT_QpQd65s#PkuW}YgggIs$8Qpsf8QACOQ`z%1FkE_agRH6 z7#W=VoTFa&sRK4`T>JZH54~1G)yt3g%gt8u)>Fp+u5lJND!$<7FWw}6_-SK=ETQVA zuXyVwu{h(P!7Jw_=d1s+|I0Uf?MqKRYv{ESsvdQtH*eZF;`2uhb>#}e2-#>cup5OEP4$_<^>?KdCOc-S(y4#R2-yD}XFS%lQz}cVQ z92rhMeB3pVQ008py|OL2R{P-nerU5@z5Il8huW1;WgF`@T+tZ~&kh_73AN&@_%g3A5VYs z*AMfDgevE&Zo^fhGhCk4Bvk#)A#dAki5K5<+)uPE*@JVhl~83H>*X-salMmQEE1}W zgSri45qog%wGyg~QMwJ+XV!GCT?tjL+qw&d-JQJ$4 z*9m6;dupy-302zbHjG&8sm=_UQ04vaggwU|m1i6YRod$|-0^VMx8Gz!mG(Me^kU?l zTcU)j+y`ypj%v9jN~qFaFNcJqkM=s@FB0~fOsLYsPPm8U=;IjE30HBp z=P&CM()O-?&;4bNuotCtw~}XB*O{ z_m$9kN_QJc<@HYJ?_6t7gmkyzFYEVO3Dez%zpOv{NSN+6{AK-7RKj$(;V9dov3GfWj*y-szLGYyZrbQP zd!;9?06JkGbT#2BqZ5t{u2@{jbi(njPBd zWqY+8?$va!>_zVTkWiKThb`PbXy5GR zkWiI-l&xLbp4M$hsLDOcmMA^fqg2_ByH}2)j=}b>OsKMNcEWL2dvhbTDo5>Z!}hAZ zJhn@yvZr+$wpUxYPiQis%8{rO-UIKy<4Pt}IVyF+9_8$lXEg~`_O5Qj`f>E}$zLW^ z`4q4d_8fbZPcSo~%BPu~a8$F!>Jz13rRi3iygngn# zFPuEhgy+cYE1hs0G`jbsTP9RFK6b)U-!_(8iG(Uo?{ynSERK(Pgp*L^Da3BWzU|!Z z3Cc{Uvd?$Id`(MdLY3)G*td-tJkgp7RrdK#79(8KJNgev<=x8dC9?2~64302OC-G=wTh$BZC5~{p6-G+0Y@m!9`Bvd&k zb{nn-jDvC{BcaN5LbqYRbLHiKK4n6c{jn2{CC)y1#*t8!#}fCD=Ef@t$1DFUt(U_% z!5t6(qb?Jwj2}AToZ+aQM`{UG&L!Q3>l$kzms~=X>z!`Hw&WT&uTmvc*~YpJ;}Tbs zc?BtXJg+Sy-KL^{`YdYu5sO#S6&jTT<>%nwvxGdP{MgIw<{wHN4Ok?OQ^CP z_j1_c?6rBukx=D))onPE+4^jinNXGM%F)@o=Ik@r;H?^}a@)0KxN6L^nuIFtb+4R- zT|wsBl~ARl4zp zv2Md()+eNGW8H?otWQYWySfd3S)Y)0l;}46Wqm?Ak8u97J|UfZl)tP`Naud;FY6Q1 zdGzs@^$BTvSHI`}GDp~pQo7qnDzA4!uTpwn39XyfUbZnuxXyHs%yhSr2(Nd-yj-!G zo+Heu6XvBSrsoKAnj^AT-l>%CHj>Kgop3KBrT3N4`dqtGW!jv&SN@XH`%36ZN_QJc z<@HYJ``mb?%CtFk8>;5UT~(&dsoPLBH-D%yZBE^Ws=0Ykm1%S8HdM{c=c-JbQ@5dN zE-q1J+MK!#Rdex`D%0lFZK#@y6IGcur*1=)y~yjGP-S|Kuop>p8>&oa!e8bHRi@3U zd$s;LYW=m^`s=~<*PQFGYu8^pyQ;SJ<@NdcdjRY2Gq?j`zsq|W>+hGWzbE5v)ZD($ z`g=>Z#JT+|y_(x2TYn$U9yPbuCo#95xNm!w>yHeMAKLC?iT6VykD?My6;D9e*X)C~ z{O*;6DtlTd>^Ty7=9ExnPwO`9Q4%=^2hRkba{wvY)`TdcNu54EF29vOTtugWL!-K%t3z*drZb_+ zlM$Urr#Es-lu+dfk8UHKLb1hULRD^8-YM^(QB=1fp~^Elok*vGj3qLm$~x>sI^AT1 zlL=Lxfa-*;!B&z-I0;pr{pvQ-X*1)XOsMiiS|`$}Ipg_EsPbf8C)}ZPj&e@Sgep%6 zb|Rf(%(JtEDo;9g8|gHr5l1Fed1A8@=~QWcUMr!>lc(K=wP@`+i)KQVHPVSR54!5h zgeuPtcfy`yk1}%3getx5L>jLgB{HEZk0t5!xDiLUA)(4yq7#n#&IZ;(CRF9RvQ^vK zjp4fu303}&Kqpc;EYloePIH72MsB*UJUZwwb$cEXj3D;9S}GNH;|&f^yU&;jRjyt;;Yj8P zmv_7*R2kcK8%Fd-`gsRZLX|7zZo_tE%gD2VgsL2u7y%e-+1|QW5~^}6Y6)G189#O# z5~^~{;0WMIlt&*4RkqOXm2tb{N*);`RB5~0uoms9&MTQvrR`2Qf7nxVBqO29w$N=j zv)H?GkCITO?QX*s?yT>wNG4QiyA!rJBkvXywLMD-Rk;t^;vCiN1>GwNRod=^`8pCw zcfvj)VNRXUE9p!)emMH%y(L?mEitd|tP{r)?`AKDy~sY8qdvW|9N8;dwezn1v3sRT z+P=~W`<*S^HA5%-MZ&()31=KzxGldE_EdYYD}YY;i-dDxCmj2{n~vI@aNM<3djC6N zyE2{ImHoqX?jPnWVLE%I%ADp1={X|Xuw|$+-EAbo>z%M=s4_iAm{TY06RJ$l5$4nh z`?f06bA&l{!tqL#={drjI$@7eWqOVao}_fQkyKvq zg#Owmyxs{_rsoLzwsg0l%JdxJ7$x0ps4_iAIMzyc8>&pt5zZOX-G(aDbA)rBbhn|( z^c>+FF5PXYGCfBai%54Hs!Y!j_9E$SLzU?{!d@iZZKyJx34fU*RGBuX?v>HDgy~Kg z(VMU7PPl4tHIaLiD-%~Nxu3hsHMh6qddarr`l^@1UnE?^b;5O@ZOL_FCtO?Fi(HF# z!e1m@?{>m9vZH}(>Q1;0S0&vE*ZS@OqXj z+m)>+6RLc|-w977I7;M^K|+-$6}pXd&mC7Xp~@2%o$!Q+V^LepfZLxmdJ!EPgZpzoe{OKWI~la ztrMPTGwOGI%!De(%1+oCY$bU%kWl4`y>7#i%n>d}G7_phdDv}u0@K;qaXu5O95FJX z4byXk-gd&1tHuoW`An#C#OQ?9_1$^9P~7BZnK*Ok9Wx!uP-XPd z2}fi{YUh$nsB$LigeA1(u6Ht_%GFXQT#dR)wT)#$l`XUrt`uEC8gXPoRgO!HQjK1{ z|J{a!DqBw{Tw%HLa=nuYRjz$H;jHF7Xp6~&s@$%OwjCdhSGo-eRrZ2TIJ4M;b4!#^ zD;5tSHg7mN|iay5z=!+wqaXRWxCr)gx5P^ zTT*3ujxeWA*o#z|o+Heu6OIO|OwSSK)CtF3Ri@_%bLxaWN|ot3!kju`KUZaXjxeWA zIQpnEJx7>RC+usgOwSSK)Cv1qO7AP7^_1>5lFI9um@9{QNqD{6a9_-{>(5N6GTjMR zrlz%*300;$p>@;R%Y-V^ozVK64eNbOzxdm3@%n>LIcL#rBvoDi@`U>rWBSOeZuNFTy$9Jw@+1?M zV@!Yfx=N~C4vFMpFZoFPf1|EO-?!s_e685<4vy*VXjlJcqeiu=g)d*bwkxBTN*8hM zxo2-NCaS8OR{gQ}TJzQ3Jh94Gd)bEe=808l=ZKvshY@R~%fq*xe%Nq7EUl_s4)c|s zCsuW(y?#HmF{ZzKIg)D5#!i&ORZ6AL*?T|EyYHD73L#RquT5DLmOlI%aVL4Rga+t65Jh7@#?e+VijWPY@ z%aK%bHg=*M?lf1rM(zK4!f6NRlB>$)FyB-Q-Ns0)YE*lfke(-2rJW;oq8xK;HB0Nd zCYQr}rRRxNU1_i1kDazLxBk#8S6jIp<|{oy_UB^s^SZuB7LQRleCPpMcKY^O0EH^SWN?H=MZ~%3;3J^TaCO+!B5E>oeg>R}a45 zxBu?2RvU?xs;*c1)N5`%5av4@!U z&N*nfgCnt0)%8kWyz7vGFyFcL;7C-ywd0Y!GG9H+UX8@c_qx8a*Xzo23!Z1FbP)&q z%Naw-M`ER_>y>`T?amws^PStv7>UZacI&cN=BtO+Uo=&NB5yw5^&>J9Ds>*3)uiG$RJ#LD+MuRNDi>7rk8#7SF} zLsc$^`RZZzY9v;5rM+HP>6~4TuSTNAsLjZG@42N)fB4{|59v0H+dTDD=^{@0sb3r7 z_K{ervRCG-hn4>FUX8>`mA&#T*W4OsBzT4j7je>aZirZ^ z@(R*?^)P!i5-Z>5yz(?%rHel0re|(Za#gt;=BtOy`f8FN`HOU(YI5x`_Kt*kUPJi33-geE`C^JWvBT@NQRGg!5^VP%b)kv&-&tAE9 z@;r5=i+=D0yoT%wE}cJ*{5pqCbEB z*rP^brOIvBeCPVdNR*B0AK5F<PjHq2KKvsWXr@;!Uy9*O@2QR$+;_}k;kYa~{xyu)I?bNfCcQ8sGdCwpbSdYHW$ ziLz1qNqOJLeElDjN*De9Pi<9Ks`|BFU?o>-N(a;o&hKD$-P zwKq?!^38SS2sdYABvv-^nCQA@ZrmM-vaz|p7gt5D2V70&734^ijavQY)r9e<5&2vc zt|~{bj?{B;;z(3ED*nu4ySq4Z`&T1THfrxJ@B7&A=JtI?qHNTjRqnO^XX#wq9f`70 z{jRQ8`pFM??4tK`qYUYJVwG=>OWf;mb)Q%CBT@5U?MCFCBL8D~t`Ckxm81G#U9Ypw zZ+Ola^*J*5|IU>z;tNNOXPrl4rOIQ8`RZZzY9v;^XRkb=Zp+WDeI%-`YKOM2m%Rh7 z+G@AfbMuuh;vKghX9IhYs=SkGzIvFw8i|$f+Uxzp|LC6^cSoY?s>a>AUiMz`fN=-X zk=pkGRJw?VK6l(_7>SiCkL~8GhuN!*rOcJeP2eUi+J)8 z<7_YzD^;Ei%vTSyS0l0VJ$vOFNaof!BT;o#>$AFE_6|IGT&uY%^8F>1K7226KkRoN@|rhWHLrOU&UP8-MFkyxqn&bIkFqUKR_B+5q30NE>d z4SW+@rHelPfN@V|Bvz`t&tQ9kKk9nfyY~0b9@fsDdhwlrl`i6~r;PtsG!iRSJ||_l#vEs;=t!P+c#3Km4@uS&>g=d`DxYi&&g7o{bubl`22SF<(8*UX8@c_w1E# zz4U}wJ{2|+6<^g^wYpyM)k{x3Yltj;=HWXtD_tHQ@%i!paYkaL%1;!{R}ZsSBeC*5 zd*xe5-CNANgCkMxs-DZ&^(xcnjvt@V`#jrsm{z)o`~T$_!;QpBm7j*2uO4QvMq=fA z_R4!;56-=IB+5pOIQb5`>+E}LD_wW<)93%>a6)J#{$Ewxr#9cY{i~6va@5{i_R4&X zIC7LR5@n+zq`a$U)HfIJjzrn0$UDbyzM0wcclqSmNL2r*bA5Ha?A`PG$NzkB?Ib-< ztnzhD)z_TXuB))T@*0Vs|0ixFR;qly#C#p?@(4E)Wur#t z?A2I5H@tQpiB(h;cr`&R!okwD&@>yZ8EAw?HB=3HVMA@jFo9va(p!^m@rE7-!=GQ)9*zp>P zl`22MGGAx7JgbewYOc**xsUG(DX)q~qUx$vbonWOUkUN_Rz9&c5*6>(IkLK5W%~6e zfBg{e8mmjs6RUi4r0;WJzm-zys)dX1IsT``NUT)(zaY%ll|^1nj6~U}l~DG|zU}Oj zXPl8J8#PIq-o{ctVo%#de;ktiEAN9C2U&jhXMT)QJtHmYUh zCyMT0*n@Mg9f{Rgl6ODs6Fw2hPXR`vY}6Bs+@qZPT)pHK%ScpR)oLrxoIXpNyXPaZ zy65>xoa-}ZhCCaLMA@jBEU)f-&gCv%-nkozvQfK^`6-#L#JN4s&LdGaYCVwKmHE0( z$dUd?RJ*F@L)k0yo$DVXvFab$E7xapar;PAT~*v(*DHPfCngeeeQ+dJeK6ZFUsv~e zML!Z{qxO^X$Y8#6bu|*Jy2@U;CZAgyj6~U}^bZ^N#RHl#P0BkmqyvVT>|! zWH}NwmQ=i(cX7;D53^SzQRS%ECwt}W>jiSnfS$9#{vVH=jie80Nou;h9;=ankU)bHR<*cg4^j^~4xV@$7FOfI?gV>%OK znZA7g(383QVZP3ydFC96)qJkKUXC%}^Y=bWea7AhAln#;m5n*VktmNo+EA5kjQLi( zs)$UwmwY7ZuFVsU+H)hds_fO6ZaOqx-k*IRa6V7mRvzn^x)tGOUqgGzhy&NM^ z<(Mao2Ie9FRoSaC-zrC~Ql)!2Mxx3wPt5HBXv3X??A4fW_4C?EmG0#jiPb*C9N{iS z-kH#bs%&G-x9&&nMoIT_j6~g!dBWYVyyK+}RoSaC-zrD#KuY&=j6{`Vo^bZbvzj(k zWv`rZ#&mi3m539^e5XBY9vuSNHtYSn?Y#$4IQok?U%_2RCeEBvv-+`YN68pdMzgl4_1f9%irfJ=+-b z-Hy8YHyh&^<-N&wFtwPiiE6Lwjns9|*CYP_W5c%VOr2-vk*K=b`Z=fhO6OKG600#P zdnGZZS046ul~i*!cA^|cfOAows$359O|{VLY9v-Qs=Z7|&l6?is~=PAyDN*i)r6{C za`V;RJh7@P?e+VijWNAig>6l&RC6|Vq8zSR=T@nzayiU*uC7L6Rae^UwJR~E>u%QS zersZ-nzOMJ;2VcaqPrn6LCa zQ8ug3rxQ8uR~=rJQ8JN>$A?DH}`~bK2ABoDh-uIdR8_;~`_GCt)Y}7tn_R4(qFncu;Wn=6A%^3fq zs`~lXQQt_EjUwj$2Wz{|s+q4IX0Jw~%2Dr%s_T`${#&4oskZB^v-#>__G%;G5$c#^#O$JQrnMxyRV5p!qH zx9hy9`RZZzY9y*0Tfd{xwd{8N--P+%Y%(Bvz`rUg_(<(P8{gi@N7qpF$dm z%D0HQ|IOI0bI9hahuN!=8O|DQ;8Kk7e`=83Yg_5Ut; z-rWCc&Hu?d5@loScOkilIA>!dRyOK-rLVv1tXI>V7BQWnmm`l$j&Tk*IRa z|8JjV%5T`v6IIzOS6==HaQ?5~k*IQP{T?s#om&r%MA@i4fb5m|>fu*%J!sqYKLhiB z1CB(Mquz&`+phV}^^cJ#8`bZ!SL0iOYrLwFA&(^^Q8tRm?-6$VaK)NesUuPGPOY7@ zSLUmS*{hMLa@2dsvsdnsY}fx4nXmgZdEaLwsvNcNlmFYvd@a*ga^J^%x9k7B%-34T zB_D~ZtNK5$x?bt)zj@gIwUeGFR{7>PAy`-1nDuqle%8^Y7Fjs8Ln&Q z#@&&qe7Anrr0W4!lX(R>5@nlKpOO-)AJs#@6qq@?Gb1ZFeNfM)i;RH>KA+SZ_+7C(6dw?^buO$JKpa z(T~Jx-Jf@gcH-UY-h=J>R*Lyrru@eDk*Iex*4O>&dZn-by}R*kh;`3vg>4V+Biu;T zJuf1^Khd#dyWY8MzIvFw8i}f(`qFfM6ZCi@zUpc`ncM4XB&x28$Yz$eAYnk%bheo2_%UR!+&2RD?-+Eem;kV!WZqz(~jKpgG$nQyY#@ViK zT9~gMX0Jw~>T2un4~}mOtv*-#w7tB2XEk*K=b`dg;s z+t{nFwtj9f5>;14D_V4k_CskLqTbp-MN22Pg zi2SV-`?mBvQRP^ibI=y!mHWoq^<5eBb%x8c+DNS4PhHomO!Xa`9$AiGII8UyQ?yQaOa#xI6c%2bG7>9QjsV6l{nb6M=coBu>PXZ*FCu>%(6y8FJW==H zq~|<;i}j%AGPdg*Bj)R9mq)mfsB+Zz4f2;y+>P3O#!*54){zl2%s zYU}^!9EoaIMdWX18t+KY6IG5cJm`om*3RbZ&g~YvwSWA`*L~w(9k7|Ns_d0E#`Ma= zKKdk8wlRJ=tM13vZzCCrx*tX4Z*3VTNY4{h+9N)G)E2Ri``_F3{d4n8HQn#WNUZK! zU9WU~Z@m+qY2U8z*P5>*Q67CpqI|FKF6FQ4`b5#Mt>!PRjzrlwe!s1rjMg_xt4FQA zMp|vR-td>7@Qy^a-6HbWPbKEcF%qkCWET@{hPIq3=P?fS-{`RZZzY9y+z-g^3B zi~Qw9_YmEI&b!VdQ8w!PgLwzi@mvqHS0hoeXnhYd&jzl~T)pHK%Sco?YF(38cisbc zLh|m%NR*BGUTVIBt~uOct8~%zW!gR)jKoTnS1dleahIafMc0>tyN!`psq&Mb_a6O_ z%}D0S54i4mUG>4OpWTf_^}!t@-((}YB-#o%OQX9!uy6F0ncK2!|R;oO< zdp72qzT@?q^VNYTylhXUi@wv|bhj}QD^)&OW4?29)JT+#nrpLHkH6VroA>--r#yf2 zNq*h)I`NyI>W@U-^CIec*?Z{ToO;P!14_>mt9j&2b4=~? z2x~gm?ntced48H8RWuus^G=NZR* z=laJ;top~?YQmkVyc?w_s&cP&95mv{QN~DAUDcYiu2;Io$39+3*n@Mg9f_)|dIp(o znD1PCH4>}%Dtl$Vb9FTmtGdcjpYypbKezUgC>ymWljn2uof{KJqHNTdn7uMKbd<;= z!$_2kdRCMpOXD_2)Jhj!ry+Vv9Ep`GM}6k2huN!s^;X0MEpecqCvrH(||sAs7; zrgl7^E5}Hz%8_RS^VP%b)ku_$TC3&SwMWf8#~F$0=k=Y@+y~9q6;fUmjYO5Bo_Xb` zOXfRQS0k~itL&BeS_`@4Be5D2^Br{0a&9ktB&r;>7oMMty5^W0cSoXZ)Et#pEat0+ z*{hK#8}+=luCMI%(Z^>VKGVtP;YOnJt@CjCiHZ9cbLAL`RXOtRhxzJZ_G%=`Mm@*L zwQICA7w?Wl*{FCo$5%$kbLAL`RXK7@ZN7S#y&8$KQL$*QU1#U*imBbRG|J48@BrXU?R`ZFlSw_5AAx z_O|xiKSrWFss51(y-IX0hxvYW%c1X zT1>Z*RCWD^iR59nq3_v-`8tc{nR6so^SSoAS7W~4u(uCBZf^vTZH&aq#vI{Dlt&+J zsLD3Re5+kmL?+!!J`#1G=Ltvcxsh5`_G-+x%JGW5QMhz3$4FE;DsG=6oZ;qXHC5Th zm~WNi%DpQu>0XYJsB+X=ZH_P+n2P{ZWgBC@RgPMvO80V%M3rNnnA-!;hC2n>t1;i| z=k=sox|d@lR{IQdgu4`ZXF?mQvW+p{x*xS0CEd$05_Lc333tEpj+Zu6Wv|A3s~rEj zcL!3smt!QV9JOaTN2D1p+i+gWHk@(Bbb0udh!e+rtF9{FJgbew%9Bit^;0&!(y{1H z*s$cIy{d)l@3)V{N|o!%|M?ly^<=!>_Sf50Qq}b@Px${)W4efXYhkyMRCWD^iR59n zq3_wo_V06=}wrJY3WR;GCfD+ljM4$${8-(P-RY?(BG8Kg#U3Soe5Rab3`5) z%vY7^Y(r1BKKHuXtux>J{$0INrM>Kxp5*@?(1t2o%$$wX#Hq6)gDif;aY}n78ud)qQde{kT)O3E%smd1a^=!j@O=m(I z`BYf5&i}j4*|3%5Z=Y#H)tn8>;V6+yu1XI(Vf*oEkJsmje3IO}RBe6AyGGG^KIH0d zj<6i2=gN`4S7bR<*&n+Nt@}*S>vKdty60BBK1byL1u!pF zTc6Xf`%x!dJcl|*c+aKh%3&?c5&2ucmRyzhrrXfE=b^klN96ynH7`|L|1Ut@k2*o+ zncX?UdoDd!4r^hK$Y1WYr6e&gsS}A3~i{I^GcPg z;_Q_w%hU%6pT&QkCD_q77A+soSvLT}S2jY)h!} zZe|;*yf>Lp<^AtO@;#pllTek{=dR*?#vVd|g52??*|f(q6Y= zzU~_2FHT9Q(q6aW4#eC%DB(P~^;cG=Jq)7dLk*(+mQpC}nEcN-F_OwSSKlnGZX`KgR~sj^JjhV&ev%5=A3ov1QB zN0`$xDKSRbZZ00>n<62dDS4~1yj>3&OwUOgQM+0XW$CX}k3FkiPOxPz(cfvTq z6@88wRQa4MKVy+lwZ4tKUy|kwM~Pl?3GcbrGodQm@E6nhek4^UY;m?kS1-LB*1Pv3 z$Gb+X`$ps^KiW{0BYoH5-oe~!C0wO?H+x-4sM2019Qz!pO=rUUY)+X_m2LQoz1DQM zkqEDM!hJD!)pD;@Wxm-f303RcaNh0ZFkf4{={drjI$>V6cGGi&Id#IdzWL_$xrF0~ z<4U%nN_(Bqx_vtnX$4^q?l$b5dgb*@sIt#@LVvYkIunWRgrh-9cS7ry!|R<$Gmh6g zkvz$Sza+ZbkT5O1U4-_!4XvkiCd}9L9HG5Ts9N8~9I+EN^dv{fdZNn6IoFjcbLxbi zr1Tu&?oK9DX)hD1JP(@*Ri-ncSMIjV5kA4rgsM5O^u!&VY(teTCKGz%?q?=c>0u`H z#C^t0sM5nu*sAARiG=xj8b8}G7BxLbcoH}hs^+{><(ZIdLzSaMC#+G^Ic890kMer9 zVZNs42&1S>sM5ns=!vJ3GNH;gmI*y^l*ojtY{NF{`ortphQCPU)r5p9_vEq-_r>N2 zy)vC`sPf!0<*rC3+}D^Ryyw!{h9x&WM|k=>6RNy7ozVK+ zK0_jW_L6O=^4Uu!+!voCyyw!{h9x&WN8~f~)|D#nO}C+SBQmeg5p%n0s@x;bHr$n; zBfRI**@h)IJx3UqW}ZDsyxe`37;q% zqhvyrr`|H5YR)TFo~Fw-R9U7@Sfj2Wd={1oYu8iknNX#@Oz25It!_C~>0!5_b=Ogz zV$Xys%ajRKdYB1S)x$mf_?b|py-uWj9p^Kl%DJQy&fA`j z_Y`|3ROR_x8_t}bL(eu;>0u|dZj|9k`An#?Or1#idWt<0s&e$|yfjx=64sUTRrgBk zM#Y|#&x9(=)Cu!73iqUZCRAy!6Xxss%#-q&P^G<2n6E23Ps(RPmG(MezOEoWDW3^d z+Utb*x>Jx(t4pZTUbm5EeNW?OLY4a(nQ#twZqI}&BaTjJU9YT#OsKM^J7IKZ)aN<$ zOsMktS0^0xjl7M>Goi}4q!Z?A0G<2d`e=v*Oi1d zYI=?^r%b3?-^LuF4b$B#>)m^v=MR5z6wQ0c5~{Siv)vx2-Tl5oy&E}65ThnY~N zy-w)w+&U@|`FWItD(kI#W&CNZo_EzGRJkXYZ5ZQbB6*S{eb;@i&vHMPP-VUKa!7aw zGm$)THQ8+>PqGaO)0wcZIZ~VMUP;*6y`BkG*@nN^u1t3uiST+SY(M7fYO)jl;@ThKnb*4we=+Sy)CupJ>Fkv=y*6^&bwsut zrn^^(@OmfAORv1%2}?M4KUA4Er*6aZS7{%%6P1@=lUt?B-s%f*nNVdq6V;0<-#47O z-28go%3dbS*Yq4=`R0f^6thVQGHm#R9^Si~2Ox}h&ZuS&aqIp&DD4>Q;57YR-nWJ4dK8-OFJu)|r6v zegEg4wM983EQhs_ZPZ>?)zxL6I&)F`b7k-SA06vT8>;j$+pxXbjx(Vu+o;uceY5t* zzx{VFsHfKF@Adu2V^6!F_Li$0#~-lR=r$x&)r?xTaQuEFQR~{Waq2azFg-`;Z6+L1=Ll7%vkg_+>x3mt={X{| zcJo!`^=!jCXnKyAv!QBz8#_VhVK2GXP0!5+s-&|G<6WcWOsL9zQ0uCUAF>Tq#t)sa z_1P!#=p&)Zu`Am!S~AwkgepDEgr&6>GNCHlFk&&1F%IfBBvcs_b;7vK=qyKM5~_^n zx(&xc+m-1#!kjXpYJD4-@R#-1wd)hwvrN6@NtFqIF>OxWMpDfY(z&%KmDj(LUg`T+ z*M^?VwURWRJA(JRl2GMb(h2*vBZK`v6RPsakV-gsME(YD1N^kZq{4raR#;mc#3rP^G<0s50FNJ+ZEg zI5MHih$9notzEBd?Rwa4r1&$p-Sr8rTcf7CS35yiLetr+q}mBW+r6&*MNdr65$4nh z^D;e0s4_ig!?utKReG2SJuy8;e06Q;Z|aY|9PZGiUA#`DS;mt-bD5Qf1D4N?B2M``8VEu=Wo03 z!t33Jzet$QgtX~>CA6N>-9}P*y%YNTx4(VhU9gmfqT#a`s~IYPP< z{$hXh`WzwM34bwPug?+Eo$wd)_4*tk-3fm&{rVT3u;<;kx@!4?C;Z-Ul0)mJKk;)1 zZCrEcjdy?g*uw_G{`~I0_|?T9-01I?*Ishi`Ntjo-xd%2+&h+cJo`-tcdr~}PX74) z7EgWMyO)pp#X}Z%z0Lg=FMH>^msjjJo;-8x`^^u%de^mo{Nd$GE<1Dar;mB%uG@X; z!^`9U&sl?4Kl_Y>Hr}nd-r}HI;0bS_va_=x&MQ& zT7Kw{p4a;Joj>yHdoFp&Rm+QB`nuRr^jM=gHx z-@j*h=&pk|-uR-U7H|L7_bl&n*}((heYR!%(|vztap3=a=knY;y?a+Lhpq3a4?Ae% zlDOYBkKgm&@4ssKxp%x^(Y>;+ z^w69#p(@+3g=;V04+&Mde>mUSYwer891^N>kFvE(+ta!Y301jA*%GDadXy^Lareqm z)G^rJl?he$%}zM(YHx0&R^_POZP;G5m&bMqRra)Q!}e+m|J5UJyz#!*T(dmy_9t%9 zqf|K(bsOFT@4w?pCR8~pb;2Iy?2~6T303y4Zo|5=*Z#!fz>UX$X20E^K6;D*Bviff zcHgz}!bk1D`_5l~-=TKBYqpHn-{T*4U2(gQEno7U@ieZ4s(-rRox5)FppPw&zjBNK z9042|p7Pi??0LwA|GGTslqU>xl!U5(Jm{W#UVF=bUS4$GI6K=C9Yv41`5pHB^{-sL z{JqDG@s)(Co89BP_Wa}DUcG$VBVIhzm95XV`?3G)PJ905_pV-k?k(eNAff8QAAZlC zj~;NN-RIqEZ0)u<=f2;##}gNCKjS^iORgH{K?zldp7^ZAnU8qa@&lLr*!j6#*(!hf zDaUMl^MAbA?oS`~!Xa9+U8&0L%BaY@neU*4suK>p+nyiUxO(}aSB>}F`mxvk^Cyp8 z9PyM(mxmrc#y%3NthZiQ#!=1yuln!DF23{ROP6o_;%Gxc)z4jW?Be35U%I@@M@Pbu z%odYdiG->TegA$9&|p7xlr?MkTnPjC6wJ$HQc z)yv2KuaPi5cZBgdtx4Xl8m*?JUjF2T% z-SIX*zc}$fT)KSJ!w(s3I4?P0J>&f6E{;6?@0Wjlx2FufRzlVPzS*M}%Uk}#^7eN> zVyG)u5JpIMc*k3IefHvyEg$x*as45o>ieGl_FY%L@MFvKKREV5eYd7_?MkS!OuY{p zrOwTR63&AU{lrgf{NOWgy8DMOzI+(r%-7zPdz6GK?e%gvGR%!v5{_4SEO9(HvN-CG zzGvfJzq9}D{U3eU&`Km!InMWTSaN5EJR3-;%5`O|Zp+WDT|(7gJ>?&EJ?}OjTORZH zbB2=ZyJ33r+)bI<-?!)$f4vCs+_O7SGFbB zY9IdCA$yMc(SKgv_Bm${HY8Np#<~qxbVdU?0+3Mk?6-Z}u2;PCv&$>*HjZ${>W&h5 zWROtxOUM4i#xtID)7`JyICB_FjB%ae@~kGI%GkHpmGhD-$Q!-xS&JX}FYj93{p`OO z_WC4LIbU@ft{R=;@~kGI>XLi^+~Ng`OP5z%y;yV`wk3OT?zIxCY-8Pq@s6vPyke11 zWgOIP7>n40bFY<9WsK5oxIVL{bL~o~a^2Q#7`NH-b8DASWjxny*j`<|5Ie<$oY_NYALNT||Yx8aV5v%dW%6RNb=38NPy z@7xk4ROLQs3wKn@Em1<1_If!a9DTIc34f8W-(*6S9(KY#Bu5{|m`=EgvmNJ_=v{MM z$#rFPXCJhuWv?tp_R4;>_sZzr7H)s+ggr{aoH}7^w}m_Vbi!EOK4=f_ zgma&SId#I(*}ImJCrrYqB^$BVFO1I%J>l4!U`EJ8s)+eO%c;zqa6Vkay`OEr*bnfT=vOXc5 zV>o|VpOCh9^?U9wbA-JprMr!!@_HxqDy8?8(7I{uWgBxuTA5}WiSYWqdZl&CVLIE8 zHodQe)>FFMNGh*)LVxF4dm^N}4S!j`*Gib~HvDD%(MQ5`x8X1AkD?N$yA6L?e>RXX z-EH{G`m>sZ>2AYc)}NgvOm`dpvVN2yVY=J!7kiP{=LqRe_=~;E>vM$k9PyR3p>@+n z=h-VgaRtx``=F}{R~emfWN^jeN~RNzSME%?`_&28OSVeaSDkQ8CSgvUaNTF?bDh`; z*OvB2*P@+p{VHKjop6opxZ;|+6RM~g&XrK? z*(=+t<#4a2du1 z?d7puLX|zO+pxXb!hJ%M3000no$wxb{~cE{p~_LI6ZR-)pFFEcsIqr;8`h7bk5B$G zp~|O#ov`QFqkMvy2~|GL?1ZD5Ehev0B~bsPZ&SC%gyF0G{N@gevb%CyefF$GIg+sPc4Dx8baBtnSIGOsMjdRVVBd zMlYTa%Y-WXN+%o#jqW|^mI+mkkDYMTw~ggiBB9FDd)2@1N8Mc_*N+eWy+PK?r z6=$^MN$E_e^3-%EEV+@MC%!YGD%X|chxX0>gS{XVsF$+LkuBW**a>@-ggJG>)@}=T_UVN2xqZ+c+zIDC33KX% zGk{}o9(Ntjz0Y|RwPn~|9fP}9wnST>=}y=O?M0?L;V-7M4S$&pOChVbsPS&J|S)I>NfmkeL~t%qTBG7^$F=b!uiYkgmmsv{<1zHo%^}JtWQYi z(Z^rbC#3CN{hs^F9APg?>24#byxs}DO6h$iv~F5^*~T2Z(T zjxeWAn3tZIo+Hd@j>uklr&7AxNGh*)!o7@?-d95FbL~o%X>;mc`AbUgE1@ST-EAb5 z*E^x_bK{jN)8^D|sG1vhRhc%YZbQ}F{GrOUIdvPV=H@|Frp>9_P&GH7t1@j)-G-{U zxI~p{bLuu!&Ba%$Oq)};p=vHpRAt(nx(!wKBCmHsmFYRcUL@UZs4|@if0-jxnKq~H z)%xqG_19|auLsv(bFROxU4QNDs@m3<*XQf+0j$5z;0}cSF7IWmzhAQco{YOubNfE) z?=9I9=k~AkYHp8g{e3ul)ZAX5#N2-3zU^7AKQcIeXuFRk-Vcd9ib|-m-*g-HHT$3~ zKNG6#X`Qg=NaUGQLX|zO+ptGT9OaNIR|$b9cMz7{jn42>6`616RI4EI+4af?@cCDxvSa8hNT-5~B{HGPI_yL`-DHH5300ne>V&Pq zR+2|J300o`>Ne78GvlC4sPaTwC(@}o2n+@W)ha!$;IDo+S@BAsH)v$KRM zPdauR=`^MhM$4SS9~ z%E&nrsbCmi*i4XlMssLFL^tG2Zp!*?4Js{9{; zPNZ^Jra8i#<_IH>OsL9nN%}95yke11ncfJ%RbM7l`M*G& za0GBp~^V86Y0Of+}+89D(9|FID0v(xw6cJD*som6ZTGf zt@{C)P-T1TgewzQEP0hGp~@(#+pu@qYmHGdp~?uS6V{@&>$)uys$BJT!d7i-cfFel zRYrrIaAo3(#a)q1sInJy!idF4#@(GvsB*0AM7rnhGiE}StJh9Ak~zZV9WM!0#&+F? z5xtRq-hq@*2MSH6AN+wikyA#eI_S787NT{+cbQ{hr_O9HcBvfg; z+pvW@>$@wG302zege}g<+n6X5s&XH+#W||k3%U&nReIYA^K~SW?u30p!kjvxSJIhq z{BZQidrP)BTVh__StpJq-pyVPdy#!GM}2x_IkH!_YUf@1WA{pxw0)%$_B&g+Ylcqv zi-dip6V5oca9e&S?5Xx(R{)*x7YXOYPB`{?HyyP*;kaw7^!|6kc4a!ZEBlA(+&|1$ z!gTgZl{w83(sM+%Varfuy4y&E*E?a$P-S|KFsDw~Csdi9Bh0B2_H9+B=LmD^gyWSe z({qG5b;2H{%JdvzPMxryt1>-Dm{TW=)l`|DBh0B2_BB^)TB|WqMx;JxS?qBdNUJ3H`NCc)b&25=n={dqVT)Nv(WqOV< z7Lo2YRGFS5>_yVuhAPu@guO_*+fZdX6aF$ss4{I%-7BMQ3Dcc0qBmdDop9CQY9jY2 zS0=7lazA&MYi@7J^^$GL^;Iv2zeu=->xAn*+mh?VPPn$T7r7Sgguh6*-tB~IWJd$n z)SYl0u1dNSuJzpm$RnKl3APyTf45;vbT1>fM7?sqBzxuFi{-E#cdzVG?)&5(CE?zZ zbhqI?m4rEU!e8_x*Oi1SZFd{q4+&d-CREvPI$>Y458CoGp~{}t344x2ZtW7P>}lPG zJxU_CL4<8H%V|LGk37?~n^_B@$KKbi}Pb%$QxkpK;@(E_Q z;cC=Y;>ec?RX&OBgins0Iql7vP~{WlPWS}emSJzsgesqmcfu$6wkumtCRF)^zZ0HF zaFoa+gM=ziDs&s^o;$8&LX{^jI^hWs$F4m3NT~AUNVnlh7v~S>t4yf!1WYG9vEvNj zy~%_s?^7piakfOG!Az*~L{cX_L1heYERhLSo~-JGC&7$f>?@g2Wl!sbC)$kq9Un8H z%CWK&wgy{Co(&{ad19~Ia3pht%aM$PDo-AE8=k;)c6OZ4gepgjOlZUO9HF=&Xp3j6TPv&;Q)?h0!;>d(5PZ)Q?|JpLva@Cg!Ri2dYgeSX=^sI$U zsLFNaFA|O`-G+oJN2N}9(%!h;C^HkPJb~W{BQGN|R9Wq00Y6>xBO~=Z=T*Lnc%ieRRSR*^%10BonHf zi8^5kExGHROsI0T)CpIku2OAdnNVd5?Sv~uSCB>=nNXGE5~Eb3SMPtfA)(6F(+O8t zuDo3DWI~l|pH4WdIS<-mGNCHBE2C}4N8^=lLqe6kpcBq4_TbzSB~&@vbsM%_TVEdG zBvj?Pa*pz@Ip%k-Bvj?L>+I#K(e~DDNT||wC#*%+pLuqcP^Il|!)V*pi}Ok*RM{3f z;mXSuopDztROxLeY~hX)d1R1KrMKONEzUllqgM%4xewan9E0rz-75)I+U|t;8ofw& z!agBkPMy#z=}b6&IQlrFb{n=hTVjr6tP{r)?`F4QFR~9BYj;AgEJyarR_zMH{@87( zlD4mO!hUB9&(V^V&;W zmFYRcoI2rXpvv?dVNRWJgi~dDjxeWA*rQaLo+Heu6ZUgersoKA>V%_@D${d>Id#Im zrpojjVNRW}uch?95?W8`ZX>C@o{70~n3sgtyAAinOuPQfgeudWaAj&*dznyWx)WMA zt-VaBGTjNS&)KlvO&|BUV=sKc@z*Th_UIQcx{ajDg!>n!k9o&O4teg|uU)?F5u=Ty z%7k9ouI~NX12&%c12^4$mv6i87UfWt%VECSnt1QY^cTb9$>lL8oHN`HZK%rSFkk68LY4MH=U3in-pE>rhp&aHbJx8e0UN48wetjnVgcm<=@qlaJ zyS(O-!-ln*gsR0SpSQTtXWzTL!`W{-xZ5z_xtLnQm|Ai9!$zPRWUpIjdQf6f{R^PStvkZ?~%d%Yay ztB2Vu302zbHarzF7c)p0`@H2xZ??2`(^^ecF*Tx<1fAih<+3!O`6mGtrdb;Aq_uBZmBX7R@mDilL1))lNy&UGN zhuJF$Rod$|Jj*q=#*uKXcI(?d?ZU_X!8OZ^Ui!SDm6)$*%D(Qx2VVH42VJxL&>w9@ zsM209hxzJZ_DVvP_PPzr;c2?pocpX@$Nlo>msj5Dj4ev8Dwo52^)P!Sp-Owb9G(%J z+h>q)zvP*R@4tBEF`rsK?CE34&DXPvw|UV`7O%Ybr1OzAm7)yHmij-6#Fxvlnmu z)AudUzSFyR^>Uc6QD%-TB~)pz+b~}}%w9>T(q6aW+R5|OkAC<58<$>ilier(+PLzP zQ01Dc+c00(ZF%J-p-Ow*hWYAY_DVvP_PPz*uBX+%>y}SieCXRguzcecV~>(hmD{fQ z&h-xo`-k>=IXsU)*Uu&F=TCdnsf*wG-uErvc=_3Tx()Mn#hO>C5~{SWHFM*xgyXL3oqj)D54f7lD@X}dt`oWq<4+^JzMS8dT##aKXZr0pa0}%m&ac@?$+9iRM}U0Im}lNvsV(TwAXF;AN_OV zu7u<6dG9)S&#%94zuhODGVVY+Qu{uD-#qbuOw7y zuh*4tSTGLCv$KS%5A6S)hyJ${uU-D$DN{<2fP;n0&_aP9JQZyDn&$8+D8aqp)+a?kGH+JEy z3002sy&UGNhuJF$Rod$|d;`hc8b`u4&ix*G#^R~Z`@r&&tH!mOt0LcD@nQWp!|auWD(&^U@{KU=@8=zQ2~|J##qZpB%*(!R_h$}!;qVl| zSRV1P6+jpI| z>&?%)dU@Jo#(B{F3*UM6qo2C_p>KZVwadr0@uU``+h`=Kv&B`JB6#!+iBH zdnKVtd)1l=fyvq`I+VOpEmy2mZx5PC*aTC=64pad&562zx06d z|B573<#RITtB2Vu302zbb>&+Y=bo`h_>ASg|9slwqHq1j<+-;SpB4F3#&!l~e@~JQhRloJbmoKjR-Ak8$b+@Ms zk)_W(d}rn}-uT?bCvS22^7eNh{~t#}mCr(YIa3_fc>%E?2uOw7yuiNk~ zr0y-|-9ZUecYf42FJ5%fXO|Cq*7%Ix=h?o)^c5F>%i`SM|LpSo4~{XMgesqN_i~u8 z9%ip3RB5lbauJ<)gV^>X;O`nhLC5=J!8* z@q0h?vE{>``p6;Pb$up1N2qe$*303W{XKt|PtZxIdi(!A?2sqj-IC9qqmsec97}g&W zs(e1&ZJ4hsi@chUP^G8CBvGI&2-E{Y>_KqL9*Gi~zU!&WwPxwS2KLwCbWnbww zocmn84_7_gunz?lZP-!}XanL!J#JRJm^JHhj+IE?(ZblThXJuWrLu z;@qBRX9-ogU74@zgdFKhsM209hxt0g+43`?N_(AfeKr@jOBlDiZp$|2`k*%KgV~1p zy1LIRdI?q9>*X-txw?|DuC&)}xF(-l8%Ve|aIM~Ln6JGn_b3Tf+Uqu)apvYh3FpB) zM>%uOjfoPDiFvLyUp>rTNvP6ZuPgVg=H^-n=UVr&x(#RNxpGKYjy#{6uO4QvBvfgy zm%|?AOr2+E301idn(tgJDq$?Dyq>jwhWT0xx#SY6wAXDIM;q1TNJc`H z@pZT1KEvF&E8)1C_ZiGr53^SisO_f*@pQ#i{_bALY4NqSLSP4Iuoi)XTp&vk3M>$ zD%&t$)6(6mq?#ifwdY1^RoRC5nwIY6NUAx)8E$S?Q zY3W{$q?#j~ukwte4OQ8OGmhyz<0RD_k?3AmNtJC_Kc=mPZX>DYh(zzJ4NE>}Lqgwk zU3r>n&W40GzPjhHEcskHBrHd+EAuri{gu4udYHXRsySk=9NMt7*@pR=mfqL8GGEjG zX5;!_OY;tz&UY}W=7>c1d!AHZ`!;N^&eVB!mQa;jyZK7b5vsJ;@1QnJ&k?FjXB#_F z4kN(1s83ZchxyLcm4x|fuh*4?={Z7`>1<;s%HhglZZ)AQm&1JL>Po_Vwb$!P!t@-W z%5=7|6Xi&&;_Q{{4QSwP9KhdpVM7lGuTAq}`TW4);iMIm|cJLboAdzS_%# z^c+nayiU*uC64^S9{$n3Da|gD%07vbhzdX7+KI@{QZa->yQ_R95H zE{FNf)s=+#YOi}Gu@g4t)*pK1Ix3gLe5L0IRod%y^|fNd6DgJ{ms~o=bSoJyX{2x%o=Z5vsJ;ZFm~jQ~vp6zl5rMe%E~G)=?6!IkeZyVZM5py^>I+ zy>7$v=AJOlr-~(1c@DkXFyFcLpoHsM?R6XGtB2Vu302zbHvB&ZPsisI@e->1|AubE zeCPHuB;1qHUbkVsdYHYEP^G>nXevZuOw7?Hm=u|^|M`P zoy}JdvsV(TJX_t%;fde5*hj+nYVM5icKuJB`RZZzN%6G>>S6XuLY3!JdtF&S+x34F=BtOlue~F~$2vz=%QMche(B2%O%6rpoc;4LqYR&)2Dxu1A=-q~Uh;ue1wBa6U zx1m?kbA+l~j-7ayg>ke|W{xZ+R2g6Qx-wtOl;5x+p-Ow*hHEGP133TJuY@YsRNaR8 zx^BxWF9}uJ>o&|+53^SisC5~{S<%VEBHn7xuvrM+&$9hU9-zasN(`!nzO$RW>t`?bsKpL&sS-^c%B z>g8zpZqvSxb+ujp=ViXuLN2+4D*vyn*OmWkCp|}~@_+EU4eLsKbA+nA*XIZ~XG208 zc}#R&GdJ!^IPSXM>2>9Lz|~}4K}x7{ozQIbW>k!Ze6Yo~{ z9&FdQQq0#f$na=k^&S+-Jz|z;@2qu6MDTuO4QvBvkqC z*IrkCwa17&N8u8x@^^k*ZEx2*GtE~IvsV(Td`D-mE8lwR?r`1_mQa=7ljrTNvQH08ojQ3Tj*TuBVp{5-!E#kv|ZorGhaQ-UP-9(JAu8fd=j@^-{vsixp7y* zao2Bw^m6#c!ns&f!dNtaGJ6{fcH?4=+D^;dD;g_@Ko_R_5 z%qxFu%Q!)Lj!+=>f9&Zm8on6f6Ggwan!m6rp(;Ov^!ueg?afbkB~^9nXe%F&1N8kN#m!H1diRWjpBvd)hcN?zHT)pHKi-aoI zZQX|Vz@3o1`yrvqd(&;W=5U8C@2W|taxKzr`0U1Aio7!+p~`1E-NvsTdE<@uz2=(b zdAC1tG573F!e@7XbLu&J-uRvy@2;G>4aX?yIYO0VSGVCvZ6uqcR|!>lZ1-%;XKwKe zi}&B-oy$uuKYP#9-gN5Xx4!p%%Qx;#cdsN=d1j^)<~uh>NjOJouiLouM}B?JB`>*Z zdC^Osx8W21{8V2;)va&)vu!DC zp_fqQUT?QyE18RZB#eD>yK>H$i`ylP+y9^T?my<+v#jqp!;~3=RmNsA9RarK0OwXD zjcvJ~YRy_xWd_TS`76{Z4gs|#T2ROYBU5XvLJ4g_(-!A&@ds=cklF_ab)c~#%q?O@ zBAff8fZ&e_qnc{>9QTuJc^&7|&$q`nPw%^n@&4<~b${OHah&IM{y5I_I=|QDT++*7 zzO()zVgJxxw_(2SW#!!{302zbHXH|yIC7LBp~~?w+pq`cUaJjN*@pQV0U9}VuOw7y zuM_4wt1AiXN_*Xg^SLcQw{{6tc|JGa*_bHdn5ey84r4<{i99k$s4`~kHjLXGQS&G& zp~`r!+b~}}%w9>T(q6Y=gzWQ{{KP~;RgS40&u8V3upEx_y&UGNhuJF$Rod$|>`}Al zI1)a`$$ij#T_NRFk%TJk^>Uc+tga-iEA4d~=4&nFl1r%4Ubo?%T(q6aWGY_BXdm7jCXTehndj#Y{Pt=Mf1!lp-Ow*EAurioe5Q@GvP>-M;|>=m2H@>Y3c4&Qq2fQ z?b%4JD%&t$)6%^hNi`#!;bya%s%*o2O-uK3B-M;C8kj`@sHKT*q?!@Q!+t-sq3_v-za=v5FNV5}q{@W8C%W5^Fkj2m3G=lUW+hjpy>7$SXWCx? zXF`?fqayThW+T60!@N}G{SpaFo8N>Wq00T4Tn*~T)VYSt3&~^4d3vX%b`kpnb4E`>jAw|Ws8~F@E4AmP&KPvRlc=whmC9h)JV7$Llk~Gvb+0l}~wV-PPTU zupFkdSE~G7awb&SA3I^bJ`?o%jL4_F%}Z52vn}EMm=Tu4^sF5D%UOD*%9`#rwC>Ys zug{45n;-L1m4Cq_;r*Bqmc#U{9KHi26RND~PH5fp1zw*Kp1{w9s(glC>zz-Tj`i$_De-yxHJ>0yf>ZDx^-oI-3eDIp6t&%^b)G_KAeOq*E`uO*O_{l z2~~a(BNM7-Ua87!XG^ZiGIg(9b9mQ$Gh!xGxfaQUDt8bvp~{_*PNXQzH)v-Xx6RNbA2|e+fSea0zhn+C4^?eyRB9l;+-!3Mh%6pT&QkCD_q77A+ zsoSvLT}Sx_?M$fhZe~K2_a+mny#Jj@zI*B^6RPt1+*Q2KSfslR303>saGcLJR5_P) z!g<>n**EKELY3dL&4e?jZ?VsWs%%5wjWYa7P`4qW$})Are2qVS15qYaX|EH`OS8I? zu&$i1x($6dD)!B~nNVe!I$^#>;eKf)6RNb=iIi{tQj>(Lyt?y0uIT)-NB2rXmG(Me zzOEqsGE^p1X|EIJ>#jlm;*^9c?R6XOK+NVr3FkrIpP6l_(!)%svKBg_@6MLaqM1-- znL1%~XVm8#>@uOs=U<(0)St!e62|S$CEbSk8hIO$XF`?sI$^$+)>_DfD(!W`Sly=! zrZeFkoDr%_XB(noKO|JSHq16u<)?kxNa<|Dd`-^? z?ahdzvY{uYvsbFJSH`$LQ8HTYHY8M;o)PAh30Exnsf>B4vP{{A^o&qty4$c$RGFR; z=F|zJR3p8-XDOj7@2W|t%29aoB*%%42F@~$E4}0r&VACEuuquogmHo^`W!Q;@;O(2 z#v-9=e;awfB+VI)620US-gB>KLRGfme@y55kyM$m#o5|jz4UTe@7@n1*-jX-o*j{& z{Afc}j`UrJdk1r`m2j2n-RvcoP^G<2IQBVGo6dyy*_<+=D%q^M$Ga|3<%uAK)#B9U+F(WL8>1;!lr`R*0%9`$k z`ML)5`iyWtC=;sO(aVJQV@6mG)7ge9PqAl0l{MW7^L4G}^%>zBHxsJd3Co1{Bk$bl zud101OKv)QrOH$6nNa1u>4f>Zmi79Ka9<-6s@xUHg!>vZ!h0^AZCG;CGs08snNa1u z>4es2`wWTj*-N&e%4aW`a9?~zc+aJ?4NGo%MtF)n6RNy7ozS`wnb&8;Y*$T{d*s=M zyYe%_doG=ASaQ=d!niaOs=POy(7JVHeBBAx89w3j$zLW^d447ns$B16!d0psX2N_u zqmv0$``d6;oNcJGOr3De;a&5jVy)?G(= z?mZK#EK?>_>0u^RSqq)ecURauPQ-u1wFh7E)ZahxC-)p2sIpAmhWWap_cVScRB5jh zDPPC=OsH}$>4fukI6RL9b z>bx|oD+%k$`Ko)Rb)#a>)@MSMW$J|a8ijjOJ`<|6*9r4=edbB|OsLXcC(PFsooDMa zp-Ow5Fke@Yo|MmoD(!W`eBCL?r`07?X|LNzv%aVCGoi|TjZ8R)JGW;-l@UiLw60gy zLMBvM)15H7GwSmkdL~r){Hqg=`bOSH z)>XOQ$%Lw0SLW*z44-Co8xpFtmkCR58_R?$J?w;|rK4KjTar+fcRwUl<=G&4lE*}A z)O4<0RX!y#-Rnxi8Z|v5%qbJ9_O~%3v|+k?WxadP^Zem|97XdUvVl>Umd9LX~@R*@iK0CXy#P(s$kG z`YiWz302lxFNcJ8FcZlWSCidF@+8}kFr5kenj^L8?v;eC-Rqf9m2LPR+m-2VBN1Nj zgzd+CT}^hv|G0L_HY8MOuiHp{B9HA7s@%WqHk^fL<R>1@M%r88mu%m{55&t)6tD?KA}1Yo|ZOwVkXQzpze6OK5}WM1z!{EulzqE2|% zOlPm0>9vvDt|PMLFx|aMgx5P^UV7#APFTX({ZM7voVpFqU!{H6PWbIJ(|$QE6RJ#S z!Y{vTFB9f#dPeMT;}QSliFf>u|8mcX_dV|o2ib@ejQCk{#txH7mKQ4}a_9@A%Ev z-*foR?OeO6{5E|jtdo?U5q_=xs0dq3wxP;&Cj1`#Js&)I(dYi~pPab_>J$r=fvw@^!fum%!DdyArt@hTdum}3FmzB#5M1K-N8S7 z@8{m}m~X%5#5H%Vb){FT^f247z1og5p(@+>z&l@Z@KX=_uP5&N)l1%f(Rcme!Hv)R zof9AW)vtQ{pZ&pe4leuGA3Sl+!D=KmFa9lmFXFw?N0ceNxwho z*DE`rb=#8HGoi}#jL=>uETQRaBdIds7-f1!s50Hlk@9sU%7m&sUL_mWNVcKMh$9o0 z*7S_f+e|p3&Inbevkg_+>x3mt=^2q*yZNf}dbZ&mG(96`HdO6z<2VRC>?PN_>Dg?c zN;=yx-ZfgzgsR*JwXVweA=^-8{Ll$opM4^aJ`$=NyRr?VC1b5jsM5nsSXyf#6RNTe zBNihWR1p~`r!+i)DTU74N{=9CFl``gHb|Ji?CyFZ~l%hXGr zRGIKUrp>9_NU9kjom+cSdHuokO5Yz^8+tNpC22f&1n+evp~|_W6ZUOK2K#>|ROOK& zmDU>RHvEsHsAF&@R9Op|uw~>3K(AC~8)vu2~~NnO*68!kZq{4raR$(EQi-Kp-Ow1P-VIkdSYD}ab!Z35l1FwtzEBd?Rwa4 zr1&$p-TeuzTcf7CSI0qELetr+q&f~l+r6&*kDi#G5$4nh^D;dnRGFUHuq|Xll^$k7 zPfX8%Z(Bmwxh`i=TY$EeF5+^Dj6|fA|@f9eBOl@IMl!Ga+sIYzeKWbhnXI zUhjnde&#nl^02P%x%K8@T}hbkHvG@EU-a&ib#Lx`%_Rq3pApiX@IM!R^xvKQic>be z=xI0J?)4cV-3kBm&+mTEVL7h&$mJ)!J|m<%;eWp4-Jf~-emvt1C%rx+q&wk%p7G90 z4sE>Yg}0vc`izk7g#UTXwJ$uhapH-Wp7i>RknV*4dCC{Q>>YK_pZkLAFYx+|knV*4 zu@`xLMo4$U|JWbBJ|m<%;eX86>oY>S6aL41y*?wPJK=v!-*d^A9=5T2ufJ(I$)R=A z7u^25L*lj@Pb`G}`Aa_e9}dTo_doX4x4-rJ)9F{;ckb=oD@U2Tf9$=7Bg1*OoPY4< z$Nkn}`fX2L&zCs%-Td39`}sM4{@R0cUVZMNS9d-7#>K1W{^!&FUiG|-PTIns_(R`z zNPPSY)>AL%oOi|Hf8KK0OB-QMy&P9R_9chC=-Z$5iVJMvU+~qZum9fp&%Ce`zxa1g z&$Zuw-3_g8zw8mG$CWFuz2Rgh9(VBhhhDwvoEsZqPMvt{)t_}pJo{ZYv@zl* zQ_sC|vEfK2{l)+0e;?kD8=m@|3*q?sw}0k+hpKBHesJ=97O4Ymm(dmBofsgGX%uC`I-g270;*ZwWZe8i2IrVZ#sLD2M z;nMkjNT|yF!}-o$Yv1hUkWiI-l&xLbp4M$hsLDOcmMA^zQL1dm-77~?$6$L`CREus zJK?yiz1c{u%2B)9u)S(8kL?nw>}lPG?bR0kp=Un(um@lL^s5dsp~{h{6W#;wzvD_K zR5>bj!XD-9lV>#vRraoK!@9E9zW35^d&kY6_vshkcHJ5QNT~Yi3txCheE9OqmfH2M z*)qQP%5x8ib3Xan9fYdaKjyOziM!5S$3#Z}M~2s*yyB2}%hO-F%uy1mKKnJ791>Ul z*g8Ag5*iiMB<{U#jjtqBz5MPkJ0za`%3GF_+xl#~Uw+AH;);*1C6`e3-T(91 zhc;ex+1lD|an5~Tb-{l+S@--)Ub)VL5~}{?xxaYw)>Fh?S6;tFeYO%?<*)tH%idAM zi(hcvazCMc&PP2PIT}#T!r8?klce@45A3uf6!4??0@oYaY4AJ`$>|x84Vh zqnrW$+ec2@xbM!@hJ>oG``~HfId57CM>1PXZY2_`{^H9|j|`W;c^w&yUL2kO^6n=c zw!}~T$l7)#R6YAt>{BiA19z{4@wp@1uRQwshs4LvTO)l5Re$#M)8)A8MHek2gL9vA z)K7ivk%udF;_5z;qZ_1TAY^{MBt z>kkQ4x7~YsY=8E9);_53)^x62300P<_d%o7**qxWJowB1{UwJZ+}&^enq@s`zV@!% zqa;*mub0D-VK!b#I9}zk#PQt7;t8Mg?T7dL#8WO^T8V@z$N63kOYY2&X9EdUxvq@W zZTY#iOQ?GI6{jPad%pG!OUd=!v>x`7CsihlG7{ZwNSJSqOPrTnv3%pre|k7F+;Hi- zYapS@`Ko(mTXL=T;irB1VY_bZo|0EmY-X@gev2?Zo~HK>LssOBvjdsyA8)dqnaGa zNT_mr>^59y&hEK{_uTbow_)7oXqQJg301~(-G*yfqy8M}OQ>><+if`N+cIo1nNXGE z5?hARlF>l7A)zYAqDFeIymHAUR5`bIuN-9@ee%d4p~^PaZ8!orG8mC(LY4M9;S69; z&9y6`N_*Xg5sN+5nIRLZy#JlB=h&n2j3c2+d)IkH#wJ7><^+U*~zq`Ox}_qK5RV<+rU66VwiTe~gX*{2i6>h?i^3 zrMr!!@_Hxqch=eyA>D2GpZ$BSgz0X>|Li~dNSN+6{LlWQsD$Zm!~g6*8%UV$HvG^2 zvzmnIZo~iVKRZj9?l%0-{!xa6>2AaS*o(YABcwawf9zdepAphC;=#0`b<;-Y*(*J9 z1<(ompsNX28J%!saK++ErW1}=?o7G+)d|;2wo2Dmop4PiVNRWJ-Dm4_o!AN2mi9;2 zqMdO4Dq&8YaEs-#`Fcfv8zJpgwsI^lh`Ww`&)30t3g8M!6umHQ>xE8DB( zaIdC&WiN8yC-*2-?k#1)|46v6)xEN=^w69#p(@+3g=;V04+&Mdf7rt9gZ9l{4hdDc zN7>q??P=YHgsR-5Y>Cpd9;M25+`V!Xbquz5WkQvGvlEWH+MA8ksvNbu4cn{s^4Kn+ z%AVG3*j{bnKB38kDo3JDcn`e)jw_i^<*3vNdz7&MZ@Cx4kx2~|E_?u0$nmf@4}OsMiHc_(Zc zwkx0TXF`=v`#a$X;K<-fg-odO)Iuj5KW6t_!h7!Ni*CbF#?i-RCyY|6UIBnb2%cDP-Ps{ zZP+I)Q}#+im3^h#aPD*V$uo|GD(A#*!+T)Fk)sR=Ro9vrnFJBvj?G#66_hcqQR@<*(9uIgAtB@$eUQ znNZc@hsQmPKj#cb?L1OTsB$jpUb(KZ7IMiYRJq>iHf&3-aq}uwLX~Z-+b}M1HJMkC z5~^}s;(a#u&C#obD(`GZF%J-q0046w_z)p&4UuogSlN9SvbPwC|p97?YNi2 z7H6-`GmeBR=c{hRk<8X-tIUL|Tvv|H-Zf{R#RhNHP?g)REyGn~p4B8&X|H?bEbIz0 z*RF&r?R6W@0Ipba6fU94``>LCA-Ut_Dl8MKwATqo07nLUK_*mbuM*bIzdeL4d{Evh^EfcEruoKRGjy{etoiJjtRpyrHU2`PLb!Aj+AGD`s zuPjIQ%6{hxBDZ#Xlq%`&l~GY^;c9wlK;-G;5*7VhlR3FCA7pgp(~&V3T*)Cp$* z$KX8fI-Yx<^C)V|u)R74cdu-Twm#FHun*dcOn1Wnn9er*&x}xI+MK#q{%3ze+BVj0 z_@Dg=Y1>%0;eYlgr0reZhX2{0kam>lHvG^2gmfO^{LlV`bna39XMaLE_jCWVKOvn* zAOEvIA#LyK_uT)?2zyaVcNZDdMC`w6|3nP zVNRVeFFi3mBg|<=WUstaDcx-(mDfArUPelvEur;UyHaJ^oVr*3C#BDp(36zzHj>Kg zozVB$c%{m;IdvPVX5+3Z)8^D|sG7|ms!W?xx1nk_52`Y4PThv8*?g|bv^jMfs%CMC zD%0lFZK#^XSE@{#Q@5dN7ALAQZBE^WDtnRFJE6+-jIb9;cN?lqXTtx?2vw%dse85m zI%@y5+Wzao{nwoPuWR>TJG-j3_2u>X{(AuX?=!dq(fVE7%h-RvWdA)GccW(eKKt)2 z*%D{_S9&$uBinx;&K@<}>yw!6C!XD&<^CgsdT%nJ%3ak?IIFqSZ12j1DtlKa(o6 z^4kel)wT@VLMBx0SZ(tjYDbAYGWc9xl~3ZkSLw8Xtuhm;TzPjQoqDiMXF`=HBRY{z zZ{(IJp~@2;-9|cvVvEUys@$%;Q{F+NsBS|-m1lH1kxm5}OJqWob=Zk?y2%J96RJD` z)d^dJtt5|d5~@7=)orBHX2wC8Q00lVPNY+F#`Bp_<;l8ExI^b0<(!xaRh|&+L^{Qo zXJ-jjo^DMkfdX-RRd+Row>5Twf^<_ep zzXj@qBY-1=F?=Re`J1LrID0v(8T)2Jm2q$<(yzhX-N}S1=dMmTdpWDQvdn}ke=FAs zd#AnD9fVA%vb}Y}m5D2syh@c&WfawI*gNgD#weLkWrWiSYtht0Ey%CV>=bQNa&*lkFt$}xi@ zfFn^JeI!)bLc3SS?T#yXWROs$?QX+bw5K|+WI~m;JK_9cPtB2xgeu!Yx8clU@5((& zLY20=4O_UgzPlosP^Ilo*y4=5jfpa$D)&KKoTHk(pxcm8rMI0hUq>S8PS__T%&8N4 zC7lV!4@aN8w`7a6CFa$gb>dj!-R$MC7ug4M)TdXLBYS15cHXr=cCS=P+gCbazq5tA zX6S_fk+83H!WqXFZp-h4J=GrU3ZN7IN5VO=6OMh}O-JocIPTgiz5kuCU7619%Kl+G z_Yd=xFrB?pWll3fdPZa$whUFKyNyJ6y%V+!Ri?nVu2m)CprXRiwbhn|(bSC`Ij8J9ToVr&=+Y+WbVMK4fraR%P!PP|W zQLao}vE+X4F4t^t$@P+L$@Nt)hyRgq4c7_ReYPdniJfq5X)kgu+6n(7;d-|du8|!L zTvK<#b+{_&PPo>04%F%6-t0S|Ue%5~>`vdpR82C2Yr;P?dX>PhxE0w&QL? zLX{&?CmcV#gWj7=sLG>{vzoJvy{p@hP-XAxgirV!eXO@ksPf5QC(>Dr+@mB^`2@4u za5ZWxapcQ{Dxbu5!Y9Yhoc88SsPc(&Cwu~K%dj_RLX}U(JK>Xj+m)>+6RLc|-w977 zI7;M^K|+-$6}pXd&mC7Xp~@2%o$!Q+V^LepfZLxmdJ!EPgZrplVC>o)LQ62~VyX zGuY=dp~?}X6I$1Ir&Yz?*&BaTd{@`Q0G{MDARmaD!@sPd$ACp_70q-QN; zLRGFS|0ChJ(rrkna#ZSsC+&^fjWRQ#$`kmVF!C}YGY-mxDx;`QXkD-DE16JbPwRv; zi!+XMVkT5Mqjth*!YIRglL=Mc*-jXF8Id_BWo#n=w!S>V zNvO(ou!TEH zUZl$Oj4-E8I2x!jJtNGi6OM4IOwR~&>V!Q?mFXE_PMxryt1>+!%&8NOKB`R52y^O$ zeNC0=8DUPHu&<@`*%De$>24#byq<|!Im}DK>)nR?Vy0bxW-SIS zmF=q1<<*CuaoLV?sLJIqU+v9_t$wt(yLK=AHDv%q{`)xNFMf* zuf+c!b+!6_6!+t=iVg4Jnm&ql_189PRI6IJ|Jt=(8NF1xhQjs&d-;WAC-* ztGzj~<*U7HLwj>#EA5Opj&c~WR=PaA^EH<&_rubv%H=R$={d30mG=7m(8ij+|8gYN z%*Jt)!&OS9?>KcozUXN;-kwXYDwo52Q!R8GE3wt6_A((oC$`egh~p@SE7M9>EnM-D z%TMN#tIFjt-&tL)#8y|@>t0E$>H9B7Qq61}M>*V$sC1R%o@cyaxgVB8RW675O3#U{ zMzz=Phc?#q{g)%DW;Twa9PU6>y2|mU7v8$u56ht{m&1Ie=fqZ{+Uxg28*BRh%aK$w z8^=)&cO5HT< ze?cy}s$359O|{T%ti)EM+RKFWoY+b`BaWjSv$dL~bzPIoVZPFHVyi3d_4{$$HfHM& zy>hjc%VECKbE4XAtvR*V>*}wH4W9#Ard;xssJg1%)4E>id#}IgAlH@joY?ZsUik!c zcF$L0yXSSi(zo4sVkw9DO3#Tc-`o;?_Ukj@N>>kl-(z3BtkqUxQ`PlKzv{ko7s7mJ zG4)DRzO`!1UYV~RX0KLa^S!Qb_IkVW$+}P6D_u2x-Yw@ZcW@;(Rb8+2+n##iLYVJt zJ-8B;Z|!(wugq5uvsWvz`CivId%dnax8Qk(N*8g?pTBk~`ATf6x?btK9)06NnD1;a zVQedR=)cWEL~5MCDsiab2(U$G_l$lih~-dOD@jMO^i~ ziyna3RF%_auiG$RJr*pSNiwPf98eVhWUExsnSJ!$Fp9s#O*7wsdCh3zIvFwT8Yi~?3HJ^W^0_4 zsQXc?__|){E3ds_X(i_CnX*b3@%`655V5K93etS_FnhHUo9{EPJWW^WqF;5+jXO%N zDwo52^)P$25?ftqueV*#2+sByR-$awPFh{B^rxP?mfU&mmIo=L5A(bqiuVDV}t zHdS4(^mBfCCEP=t*;t9qM&3!)E9p5=HfmQsm&232vv_wUYTT_@y{?zNC%xuFE+w#h5CCWza z0c5YtR}ZsSD^WIToteF|?Rr|h(na6*_O(Z?#HPw^*L-LFVZUDr2z-G=$Cx?qg^G}-kjL-&2{AnH?y%4n~gjsx~`dxyDL#PYTV7M zBG&`1Ci4n%CCWyve)DR=_|u4d7KN+I(W@i%EKXdBDo4eid2DwVXSRQ}5@nUo6l1;7U|Est?xnI_rGfb!*h;$l&jtD_z8gFJI3(uf(RxV~P3dVfJbzHs7;X zo=~^t=hnUwRadn`Ti46pBR;unx7Ks>l`i5p&s}E&dy%TVlWM+tn7vww&3Enf{^2kB zXXEZlR9)4$Ti46pTb{n|Ksr+UK7dLW@thx9_Ze1VQ{}PUeDyGUwGx}}*(=|$U>uZZ z=as0ss`#p|m%YEZ?xtnDG9pXAR_J$dB`V+gJES&6EvTA$VR zvUkBN*R`6fBHv$9>A!P|_>C*C-|^H^RbEG#uO4QvR$}vAd%d;$Mi_VN^A7z=)LdK7 z4eEN8{Ci$--SQM*oz-ssyiY%z`|^yl5><|xEobwf9%iqc2UTUS+?)2@JC!aEUw-{M z?ykh9$~)WU>xi01(UmA0H3MX?+%@n`Y?Uthnn$jCGApsE@;-y@b!KBFHXFIM`{pO3 z{v7GAMD_F9fvW3e@4h?NlBah?<=v>2C>ynJT-VFSbKbPeh91tmQk88OxBHH*N|#rc zzj=*bS7KA;xZQjmwev{55@n-ine3HsEpvsGS4AsP_oH^8>U!DxU!CG(#zLpMLw1B9gUSP;=I?c zXQNhPQ|0G4=BtOf@hS|UrIdHBxEN|%RM z{OS68oR!#A`H7S6Y3B{tu)SH6YRy~VscxDwT_>bZPfuQL6g=daJ`eV*+*OeYY z{iDwH)%CJ>;wkGdUtBv$&xtKx*HnGYY3;fS%PX&yC>ymN%=g3dCBA37(nWvjYuDez zt;D9v=S$4j(Jqg0D^WITbk1I__4B}M=aty%D%bA1<5f1Ed}_xl??$ae*_iKYe)gt*j3u3mp}Xy2Be6I;G{Wbm1w-#4gq(Leqf>+HM||4$W98=9{>A$j*>CCWza++?qO z2IaRPDqS<&U;gZimmRN_*i`ummiaovsdC=g8`MmFb6H{-`D1HCC6N6I;GH()T&A-%6=;)xz7Jy#CT+B{o(56@>Y^ zvdF86l_(pv63SlLx1D|RjI$DDqh`xIGPr-?6SMr3Y$eJ@J>ko{AFgGc8S-qf5@n<2 zsJs&PnV>bDYj-8eMzxImMA7{VdvNZxE3u6wdH2IU;S+)U6ksLFMm@pEJ<7Sy)k|Kn ztVGpSt+w*a>9e%iJzt6Kp64fVuFsqq@@%jYWus=Yyt?x_m%Dg*=WZp+M(sN0r)0Jg z=k`21uSD6X^+0Y{=Ic5kNBS#K?W&#+Wv|S4)<0Ha>mS)G*Jrc1eI=@{DsHdql|J){ ziNvfAuEf>{vkmigb)Q%CD^WITKPit4<~ysamDuVkd*zyZwl-LavQg{7yrMT>dspsJ zD^WJ;nYn9`9=AK=%;v$BC>u2o<~holb2cWfMA@i$FweE-tB2XEl_(pv|6SKNd%dpQ zvzpDdD^dB@T$^{ioSkRoSc$D1c|JE^Jd^j*3OISLQpbtCiU5Do4oXYc1rGuf#Sc<~wK{ZB&yZnU$z=)ZRjl!rf<>jk_yR zHfoN_`wZr*huN!@C>ymlsOy`(-p^f4%;Kw+sC+BF%Igm!vROG+Vk<|E;mlVLvsWuo zHfkRx*RIi$EAPD8UWu|%=LK_o<(ZOMIaXpTM?R5azIvFwT8Xkzv1qPcXJ>bZ^N#RJ zl#P0BkmqyvVT>|!WVsSGmQ=i(cX7;D53^S*QRS%ECwt}W>a0>8+a2L%djKm@Hfj$bkBN>9v;EzbC>yoEo5vEzsM)^gN|cS-7tJG_Epbg3 zT|KzZoJp12?%F3F_&Lr>lqc0c<~{0xZCDQTeQ3*J$@OsNl`6~B@8EIRSbaZ==Yy7G zO>ZqGm)!a>or$$f_uoJCWOhHy*I6{roGY=-=i2M#So6L0)U(tVpNasojg{DJ%m_!K zJo;!uRkpF_TkWbMGU;COm8iQmCmgkBBekmR)tYaW<4vccaOqx-m8f!5+&&|m;bya% zs%&G;x5{zPsg;*>FULw$IclvoBa8-S5rC>}W6ih9QL9wxUXGQha?FX@9)LF7Dac-} z`Bp!#omA;wj+NN<8D@mL6nSSt8>+I6HQ%})wHqbf%drx5KjwtHUwOw%8>+HbYrcp5 zUy31IrD2!`mTrBtCc7l)k^C6 zW^Z@NAGjQPV!ku4^iaBcl~kF~t2MpVLbs7rxf~M7!)!y}vyC<12fiOGvE66wb+1<9 zuWjVI@}95hqi9!uZDaSWX4`e9&a?AM)Lq;CoYQ=zb1PYitq*3eB-ZriVehp`HM4OX z7qy>=zmbluHb-R~wg)y&3mlq2o7hdMW6XwnWUQ8IF53pUA$a!_ug_j%s16Sx3LmijcP9w(sQD0 zJoI~(`kr=&bIIM)%_TQq?ahg;uC&*^(#D!Dx^}sD6Ps#g<2cIU6M@-N09Cmh<~ysa zmDuV^d%bog)^yE2XTE_Zsb)5gqa3rfn&l{>>Z;Z_xg6#zJtwvr)n30J$8BS_{?IE| zTe%$OD?KN+`=PyFSASJ(cp}9z<&v*N)m829<})njD?KO5#_n@Vo*kRr^Oe}{`TXQr z?UB{VvpG>VcAr)AG_I%o^U3~|sD8ftH$Ub(i>X&)TN`Ar%vTTdsp6F=8@tasd*0j= z#`#q7O4R+>{kuE!ovjB~qHNS2K=#Uf^{~=KpZOcbmDp5ueY4lcD}Tq}>G*siekCg3 zdf#XMHK6&<_GDI~Y}7tn_R4(qFnhHUWn=emGuB^JRX^W-0%9e~MiH~$U>((2HS^WO z?A1zCIqF?eb-mJOz6Hvd>Zr~-o39>buU2C7J)gi{Py9acd7qWo;;Y#i;iLLZocZcu z_G%@n9Q8(+x?brse-*!;B(MIl`z+2%)cvS+?drTtwfb$_d6P010L1y zCd^k4D_tI*`98~)*i?1B(r3QWVg02=-Sge&mR6$jtsSV@Z!?bS9J2Z9VfJbzsvPw` zs=8k3Gv74jZ;7Pm#Fp>;mq>L#>X%4!qHOH`?ULus{Z(uJCF@F*jot4;au0E4VC(6eBw|z4kj-q-}*UMgw!F?~obPT%|n&ZTksB%=Cn8$W^ac28hD^WIfzi-KYH{17FiL$Z#-BiBoeAaeX zqHI+En7=8#=D~VX`kW{myWg$uUXQE$yrN%;ZQY-DijL#m>fVE+`c{hhTBiKQ_m!w~ z+Vyq6x?brszjwF34YBU|t4_TM!5-X4xRt1Tez+!&?@x3rIjVOqo39>Lx=McLH>6i$ zQ{^{7uP5TGu6F<4U?r-qDlW-q=$!|T>U%}ztB2XEm8iO^^Z)rPO6wb~tFE4X>Q~r# zpJ64cu4?}xzXRJjE*1@BFyhKB{+Sny+QbUmseD zdM{^vTQfNY${#c1^{>bl1b;dcWZ(5kI9%iprqU!29p7n|g@|QN& zw}n<8tVkw5yIYCsgGJ=`iyAE*)pz^MR}ZsSD^dM!_qQ{B5_eSJ<}lycxVsYDxSPKi zvVO6!?)hD(-u9NCL9RsI^NL0DHx7+nkLvq4=BtOfwpE;?D)D_!3;?RB+&F{EOKdWxK%JFi5=3`OK`j5vo&&xyL{ zzjyvKFU%3Z`lY|R=dU>R)}j0?btUSa*Av(LZ9vyf(sQEj!S`Qx>I*k<<>k4Iqx!~( z`8wL=5pE@_9QA#J{N)pOqmJs^fadGUBCjS^qRO%R+vMw)Fsohd{#D~jRJ$r7e>>B7 zM|w_FIX?W1%XX}t&DWjV9lNzZ|AKd1`pI(+^Hr6-(#D$JJnW-SQe_+Km$T}A?0y@` zO4R)*B7bYkI6-<&RB5mH$mKg?ANRkH>ig&Bn`*k>kCoW&T3xSneQ&)Jo@qa-@7J2I zBT*iGR-$~b?=I!9>iR^{udU`Utgb}axaQ%zo{ZKvORGn1Un8xyTW|QwPk2|N+HMi~ z>!%X4a;(Hwj%;K7GHI2gp6cf(?kiE{C?bE$)RrhcC#oEm-+tmCw_Rfq?ahg@QGFsu zGV2#4tFCtcvUnw`u8PRtob&|tQGMgkeDyGUwGvfVcfRJ5gZ$-1_YmEI&b!VlQ8w!P zgLwzi@mvqHS1VDmXnhYd&jzl~T)pHK%Su!^YF(38cisbcLh|m%N|cTIUTVIBt~uOc z%e!hTQ8w!PwRy$jvm19QDqVDaDY(x#E3v8alb;Vg^Vx@y%+*gn{k*RF;O=L4D^Y#0 zh`L_(DyQz1W0drq*z(OIoFlc7Y^95?FKKtLR$^1-vE8#VkALj19L`r4T>Zv7DqZx2 zr_$ZVN^Gipvc`O8bJR+djhbt-S1)_SC5QL?%4=^pe3D=H{K`{bWzSFbSK@H4Jw?>@ zvR66vlDh_!o)cTXc?D_QW>k|SnU$#V>RVrM{RKIu_IZRgooja`wtJqRX4ofu5}TjC zu0+|WXSBITxsLLwTz(R_5@qAw>u=hzj`BXc+md%!R-$awK1^;U?&-Q)U+JRH`SaH< zqt8lgs=Om?E1AVUD^WHo_Q~zaIb#;LuSD6XxIND}<~!>jE3x&D*=oYQth^hgC#rI< zbsRL}$Wg{hR9)4Yv#wXV#>YNhN!WvPuU(0%t9k~RZJ6&YzFLVbzRF&i@2sv?Vymkh z^*Nv0@^foniLz09GI>5X-`SYB5{KnDH6~`Sj13(n^2o3fWuu-I<;c>w%@MWIMW6XL z#Ff}oIqEZCJfChlL%%CQn#Ir8p@`RZZzY9-1>J;%wlYqT_rcUPipRJ@zxDc>Xo}+dBP%`L6CD}c1IoKND^WJ;96%mR9HZP- z%{!?pQ8w!AM;_s9iEFy(>cM^HOsd>=*FI6tzpAz8{;?9}$!Dc`U4Zm!)!y}vkmig7R@u~N^JAF z_PSSVzF%>w4?g!)1dwg4#AahgI1=U2M;ofLjWyqDR~3;-_mZzf-RC*ss6890Rb{W% ze5)K4g-iExtVES#PB_EOW;IpWt2N&$N3Fc1dpTC3$}uO524)d}s_fO8ZweU3lyoo0O4R+B z6YhTH9WQOD%3iJc9`=t@JCM@794m3yKgL9w;j#_qrRo$_AuHQf5@1xdq z5%t!>ZX>Dc`U4Zm!)!y}vyJt4?RC~*_wR~UqI|F473E(d>3gDkIaXq`VVOE%zNw~n z6Wcw=<#5C?o!?)QRL4PRyO-Sm=t+J@iiE1XUm~H6{QeRNRe2Xj!qLDJ-?`+f^e_{a zR@<3SrH7etCX>#DD$|*;9JAV$nAuQeI(wxj`3bLisj}X(4ex>JOlWU^V%Dxyna*CR zvK?na)vO%au+L{3s%$Zt(1v|J6RK=6neYypo)NRUvb6i#m=St1BkU71V*hgJmFZl& zdSwsJgsRy+w;Y+U_1Si9W8JGncs&!UOwR~CoDuf*~T)VYSt33H7Rh|bWj9yJ=8|LeLl?hcd8}@VOt87D+9(KYSHJzVx zsrA_=yBT3QOwY=ZzgJ{ARM{W94Xyi3(CafIpYk>@RlCn@SMAnQMDNFpupFjm z<*46t*IRX-@S``~o(WafbSJd#lVh*Xi2R!$^HR0@7d+duX79(0upFjm<;Y);upFwa z>25>oo-gqFjL7e-F)vlS&(K%ftrH2Jt@Pexugq6E6PDccjL6^mwdAV2H{FKTJ-6cZ z8IgYr(DLFvPP@bdHfUpcnqs=PPdhSrU#ygnoRRcj_x?fwmK z-H$q%=lSLt;XRkmURiR}Gs3tu6RNy7ozS{&kqMKl6KvBvfgy+i+f* z)s=*G<$Tp`=(|yIe*dn7D$CSun6FWI{vL~jD(!U}Dc}61CJ9w}b?1Lv(dF-dNT||Y zFNgWMg3RBKl2E0+Zo_=tHQ4dRDZcZeN_*XgI}o#ZP{Mg|_ggk={jt55vsPR6mQ6j( zUa7JcI-&2*md>J?P-U4qVRUEIm%mXZp~~l9iw)k(H;dbyCyd*jOS%p7HS#th&x9)N zb;5itt+kK|Rod%>vARzeOlQJ7I3rY<&Nft;&V)07Yurqz$~OFuCAX%#4GC5Gen_Zt zZJ2GS%1`^Wk)+M5waWkXL)XRlOcuZ(ehqGYt(ZAhpxJtNF16RueDQyKG8 zWtp-K=^3HQbhlxhs4_hx%xNJ`UU9gaxqe|Y?^znxs>-`+5~^|(Zp^8T949&&ILkP$ z^pZ&%K@rRoRCBF`e&6 zQf0yxXG?VT(#v7Jdp~l#Ys7kXM1Jz44OKbPcOC8>%)M5^RjPNh*Oi1S?RCPj&ym`6 zCcMw)lnGVYhX1kGn(j6d;q^|qFXpaV?zO7SH+vBh0B2=4ER) zJtNGi6R!2mH?Pkn96uaavJF+*>x9798 zL|#ousB%v(+i+iOM(CC4Y(tgj-ZP=f{@4lgbtUBW8If0a=B3JYVz%M^m=Tu4bhe?& z^WvFMWleX&d|d;2eMYz+lnGVt=w-tDF(WL8>1;!lr@u3y%9`$k`MOr~`iyXmn+a9! zgk{3}k$3L&SJljhB{!YDQswFIOsMkSbi#aH%X)oAxUZ23Rql#p!hMYy;XRkmHY~a6 z8R6;gOsMkSbVBR1eTGE%>?PY!<+GPexGz2q?dPrrXfE5t-L# z#B5hhm3!pbhP(1J!h0^AZCG;CGs3tu6RNy7ozS{gD$CSun6E2(Pvd7omG(N3@^zfg zgevEfPB?FSI^I+4nNXGIb8R?tdJa9?P^E{R(7I8EC*?Dt$})8#x#~k@|jT8 z>^<&b{F$#SNYB=H8xpFt*9r4=ry!qJmr$j>ZX?b5p2p9FD)%)q;T-PVo(WY(9G%d* zURev7P-RVb!syPZ&vWRRQ04QlPB`itc^i>uLX~q#C(PH#+lV|9sMcsM200EV*qg6RPyE6OPV~YI$!-LRH@VkWiIpgXBpb z6RlCxxpr0gl*DweD+z1V^o%g4OsLx5#*EO0>F$;F?mf@*hyQUD&3niaspe;TXjT{Q_+?#X2v#<-bCp5#d1 zb)W0A+|MOcS#P}@65hc~Bu`vTb{ol)Y(v6yChTjD)TX;v61H})XF^rB;eTvbrn`+q zc)b(0AMh14MsB-~$dO^A^AHMv7 zz6iaQcK>qBi2ZA~#=-LH^4m`w)XqzlKSW!_(Ly$)bf^QM}sRX@n%(K$y4I>sK8RMXCLqe4? zQ74SsjLvdICZWoBuG?@Nv|X8=5$2Q$Rr}k>g#X!pUAsS_JZCeKc>y8+eoSz zA)Q-$QhEKs^h)0!S{r&YYb9wscLeWsC85f>q!adSM+WbzXcK0W=ZjGAmUL6Nv2~B6OlIl1J zZTGtJKYC(%MwnA4%**tQP-S{%!?utKReG2SJuy8a9$FjvoBCrfhdXp>7q1g(mT@M^ zgsQy$uom~fgVK)k-K(U^L>iqPUAqklRhFp}&X%gAGoi{kI1{NBGGYB>8%B_}N@JpK zLqe4?YA1{!jVz5zGoi|;wiC&BYatV=tm#b5dTpA+z5m^Ygev2QOr%=Kgm*C8*gw+W npRlyv57WII$3a*>dSbeJl?bnALe>5@GU0zrYp;8iRGs+0M5Kt1 literal 0 HcmV?d00001 diff --git a/gazebo_plugins/test/diff_drive/meshes/p3dx/back_sonar.stl b/gazebo_plugins/test/diff_drive/meshes/p3dx/back_sonar.stl new file mode 100644 index 0000000000000000000000000000000000000000..9153bc5ebf0a9adb93de31251286f3f6d832f377 GIT binary patch literal 12084 zcma)?eQcH09mi{6opccgTgrG@y?_KEky@p^KIakvu>;~3CUKnvEL{;=UYfpH7;7L) ztGEINX4Ot3F!2?(@G?ik^ISSmF>w)>g_lWg301cMqmT?3b8PJV9&f|v+~@wX>mTNt z^ZlIfd3v7T`90_NbNaHCWi_i~KYVm8ceK zh080Kt}3iqRa;nFS5{tCTUcLPey{z{d#kISEUc@kUs+XHeZL*G|GJ#x{$heeDR~I&|}V?Nxthrf~pIa5RYjQf+Hk~Pt6EDHhtWd=0Sq0X_gR= zry~SMNHpyq7<%0E_6unqB&ce&gm`>6LU4q{_KM@a?EkCdOqvG?s(xz;@%T%G;0TFv z6W$FyI$Dc5-2EWos4O8Kb9ST?F^rHH^4_0AkJ3GpJlBH+RsAg?9^Z=)93j!UctE%x zzxuc>&4UD0zqf>Vv_=TV9iuBdf-3sCPdWiP7+Dnws`~FaGxheG^X8=p@kLXMS9QAE$Y1gE(j{JFS<=WJWJgd%KleEvcUS-e?O%O1H=b{E2K{t`Dz4Tg?)&`Q!BhKk!|ZkA`JsN-wLb9^Hj~j$C#d3TU4n7P z=%SxaP{q}{#LabsOl{*kMq>MeWS)8X*c)NYw{O4b{k5RO$TR5#RekIFm?KAhb16b_ zghX{tmzUFZ$P9_(a}rd2I_sZavgL>w5Xt8pA+g|-zk3r`cgPu&mZL~eweTB#%#aa* zF%g0zBz`(R&vcf(8ODnHw<1B+TPO3)Yk9kjM4C%*ghcW25vIFfYZ&u7LDk~MLi66y zw(!Jo3678$(>=koA6p-uT{=NkY4Z%Twy`-p`CNh{Boce$CTDnqIS|R`B&fPNe~$5M zer6tw!1j zZL#@7XW&Ki`PmImneC%1J^3qMyl0s?+q1}%J;;urihk~M*l%_7#MMHt(Y_7m5>;lM zKg!#0-zEC#1XWzEOU$o$%G|kdg(vyEyzeq|^X_s_@;Uu z>ATn*EA8|opVLn#sN!l}BJt6a=EGs%3Qw6oR+gG|o4y&IQS{RZs<>L07~K7YS$f+P z6St@3sP*&A-L(@XihTO%1XWzEOAMX6&|KeGVy4?ufAod#o6egLnXlQLK|h_KimP>r zU+0#YQx8lwavR;fbDr6@bApk(Ed6wXDz4Tg7DJx^QzQADMCxx9s_5rF2j@-gTHxJ!$AXebb|#V9 z^H4=U*Msw>-o8L?*r~TNkZ)g|po)Gj!FeT$N*mtydt?4jrJcUSyiQ0A_NwUT5}a2~ zg8>Uy_`PS>ZKKNl=>438oF%;~`nevQH+8D{CAZFCzcrH2Nu~p9y@Y^G|R1#Ft&-GyBkf5JVP{r7E z3HfGdKlVl#os#tvty==AEy&kETX9=o`6Ozy6cAvb~ClOs0393qL zMXzb^ml^Uo=gsLD>0f%dU!u|OjZ`WA@oa8lY~-&?f-3r@JqPBMs>-4hxe0m4q}ozG zxNkzf8Fhjx`nd$>&H1X9m5^9@@vB-^hPRyrRrGT`IImQ>_C9_&kczofzurB2K10-! zpo)I32j`V)T|?}%@a&Q*UeoKz45tqXs_5r>a9*i8&bpoq9!b?m=QW)Q7DP@_5>m75 zRngD&;Ji{jtvuZw<`Su@Hk2hZWCIdZ(a-hZyix_ecHvSWcUh?p=hmDLbD~aAML(Bd zt79RVSRMDn?G8f3(N zJ3I5z&s`M>s-&|aD~#rI>6nN|v3-|FXGc&a9UAd?I`Ss|#&(g!BkUrS)L=BIT<C-IX`Ntyn6yr?+e=y}*t;dI6YRv2 z)(Q4iN$UhVqYmx-{|r=dH}xH4ckdnb%F;hUOyncidG`MOw05Z^u+8sM0%% z^q@-b50aot@Ai?PO7Go~pi1w=k)TTNtC65e?~;+AO5edfYe$-b*mg7b+}0v?vsmAE zcaQ{oQ>1l*ohH&c!A=utonYUHv`(<6Ls}=;p&_jk?6;8C33g0Ky98C-8+`}awfIl* za`S^NN z+euKRSIhGd$dka#oN73P{o>qPEf^4h)y^v)Ms=;s~YJ9YfG$txdcbJ JgWTCz?0=HXoMZq1 literal 0 HcmV?d00001 diff --git a/gazebo_plugins/test/diff_drive/meshes/p3dx/center_hubcap.stl b/gazebo_plugins/test/diff_drive/meshes/p3dx/center_hubcap.stl new file mode 100644 index 0000000000000000000000000000000000000000..c9400f7d232673f850e9af32d221b1686a2c1c6c GIT binary patch literal 1684 zcmaKsyGjE=7=>deHeRBIiev=^3w3n`iMj{|OG%?e2#5xaw`ex*#SpZ2vaTt@hMe=y_pvh*T298yj50Yjp%{*xa%^S6G}e^R zYCJg~H-{B<$ykf4t1C-}Y8sYm*|E8Vr8?GJ5C7SdN-V0j=_C_sO5^|XFU42!sTtIZ zo$IU+3F^}Yf0>?pqd9%+z+divIAgovHeHcR(7;sf{oM#P#?p@?awB-+FMrHz>fL;< zkGJVL&)1{IMCL*t__u+n!()G$8apm3WUoFQX2PkX@#*GoDr{g1on()CK3A}#lQ$JM zFxAJY{x(PUWOkdC`1|O1E^2lA0Y)AJ=LJos!Um?$Np8^Gcm57W-@7y<^rpfFrf5C# z7|(|)sF0o3xaJ;CB*>^u9$y=eH*P9niz#%H8#MQgd!+eXB=n}j2Bw6Qn+n;h`R*f+ zKs3i*kABXF6K-Q}Dq)K$bdnokK9@Y&`CKIQroslM8h+15+C_!zbOr^XIJItbY+wqV zWDlAv-UkwTQ(*&B;>^3Lko`r=>a=5;W-qlWU#m|0s^xaCVkcq)Q|Kgn&|EqnSEXhR y3B9SXfhk&#&c|nLsDcXFX$^5-2`cw3iVaMmlk7or#eIc@-c;DYlyLgFslNdQ?I~*j literal 0 HcmV?d00001 diff --git a/gazebo_plugins/test/diff_drive/meshes/p3dx/center_wheel.stl b/gazebo_plugins/test/diff_drive/meshes/p3dx/center_wheel.stl new file mode 100644 index 0000000000000000000000000000000000000000..c8b857dc5cc08bc87d1d651414b1b8f4a941637c GIT binary patch literal 11284 zcmaKxU5J%c6o%EbNlTn8(8%E2Nr??}l)%`BNc18xi4v-d5H-_u^yugjO$ROL2mi=F zG|dV#3n^ukF!czPKj~=ZT}5a{f@IwoloIyyuIU_k*7v!N7tYrGKKot!`@X&R+G|F? zIA?PE;=)rC#}~?#1#>G43NN&mXBS4dO`G%lwDyM!Ei=lqr?s@roKbFRFVAn8zhLsz zS@T;e^QR8iKZnnqHN9m)du7h7mbpds>aPnu{GYeBUQ8#OU#TveQc8WU?M^4u?x~-> zPPMh#|Gzt{=iWkr411Q&yV^~kA2)a}M~aEMYWDulp>6lopxfro^ z>%}VRn0RN!twkf2&AVD99TOdj--vDUARQC`UftbI+@X1pj)~10+up$jd6ABZC5qpO z8HfJtBpnkQYaXxZnURhOi^RT?-g+;jV`8D=H)4~%HPSJ$^Z2o%5exNwk&cNSir5EE_>bLsYyt=vcrP}A0mD51A=XQQt{9f&tpz3#x@Wa8wMH|Tz z9QS%Ornq3y*7)Q$r(IX(O1^0;et^WlSPpPKV)(b_RV75zNH zara#6yU#rSsSG)MY08#;16KV~yryMHtqg5%z%*lnE;TV77~9x7VrZED8^RdG}tw@c67 z_LV=hHukH1K|4e9*zC?gf-3s?s5tItdiI~oL(i}Dy4suWIa0h=?U z%-hxu9auW4sAfL>Q6^6$K@}tP9_%5Z6?NlGj0vhZCr>c;dW4pXF4p_#(mb9!Fe$TN zk)VnZdJp!H;As~VRB=w8VC>9sH|py+rFm@cJd>FR391;O_h1hRogp{Q_L!iGbMgdZ zXXZUh>*BQLv9zfvbJmieiV=Db_K@H?9}`q@PM%=wybqk+HEOTTW8Kh!nR$?)iV=Db z_K;v^5EE2!PM%=wyrqoSy0Cd@jb`RSf+|MnJ=jBn*+)!J#W{I`vGeYvy|QZSV{>kO zkf4eYdJp!HU``YhRB=w8VC=ln*?X}4D&_Vo5>zol@4+4t%)4TOD$dChE_U2f-_*LW z^JCoLRmGT~iV=E(JtQn6tLKa{K^5neArL!nzYFy~>^y%qcb@ZBO%)^b9_%5(Y%nIM z;+#Ce*m);ksdZsl#*SQ;L4qnq=snm&f;nnTP{lcUg0ZtwFhuL(xaOhrBa@4gpo$TC z5B88?RvQykaZa9K?5tl5Sn>>&|n;UuWyoV-V^TAR+c?P;xL zPDz#2RLcg_8r?-=f~r~`oF?vmxiu!Js?)D3zm5s2YEL?;0^NY1%Cacc{kkiWu?JO_ zuNbjdJ9$h{Wto8ys$VuBsH*KT>DGqxpsIF0Ce>jZcu-Y4SJKWm4mEfmRMmFyq)*A zKY62%E2$)?vOA}$U+K48MVLJ#>~>eL-1?|UP-SgpGU>#y_cOO`_K=7xsU)bfyPB$B z>CRk5m^~!oN-7De>>j1+SJJbr`kAwbL|jQFL6zNcR5eNiRKfEe>>&|XQb|x{_Yd>< zD_0R_4~e*vN`fl88<>ZlcxE2#ArV(nNl;~ZySbv!^sIs=LkISdanTjxbNW_&?5>#0(YU_Mdt|H7H68jt0ck5$O)g{7qrN6aK z{I|;<5^*J!1XY&9>9-C^Pdt-ZvWG-mNhLv*Lnzosy~r@ z_R4V?6;C@K6$z^7=Lxz~WwlJ*&(qtv`V|SPm@Rt`j?1WcqQ(SO^z#JWsj^z8?t^Jj zuAWGOD(3LsgX1zPp4u@%75zLxcdD$GY4gziJaamepo%vO@4;~y6*GgFpo)H;pgUDo z%d~k|%_b(O;(f;x9G6is`-lmu=;sN#Q>C5H>Q`x9ZherTinlQD!EqTCGn|;9ihiD; zJ5}2Gte%)2%$x_-|fCtQ)TPI&hzQH^PKTg#e1~(;JA#6`D08_ML$o_ohqwkT8^?y z=SWOY#oN6nI4+}N#u*b-(a#ffr^;%X`h8Sdo~tL4po%pI@4;~y6|>rypo)H;pgUDo z%d~mu_k)=XnFLj=gm@2*%cz)Z#{^aM^90?gvdW~*qmp|cB&cGo#(QvFM#bzrCa9vH gC$uNt$kmyV$5D}>D)wLxqhb#4qar~Sy$gl^0G+>$^8f$< literal 0 HcmV?d00001 diff --git a/gazebo_plugins/test/diff_drive/meshes/p3dx/chassis.stl b/gazebo_plugins/test/diff_drive/meshes/p3dx/chassis.stl new file mode 100644 index 0000000000000000000000000000000000000000..13aa2cf73d764137c73956909ece7214bd890cd3 GIT binary patch literal 11084 zcma)=Ux-yz6vit^Y{VfjqY~jJrqMXwyfp*q+@lNzDGFlci<->X*x0Bu%^&CwJs49^ zOcY2jgM=VNgpwpgkoQ2L5Lp_8f1(~t3hKciG9l@q^KJHL-#Y7@@%SLCv%lZ^*4}HK zeXjNH-q$z!Qtip7Hq?eE#z!W`Yr95=_ttv%4D5S$V02}zJ~+I0puXq%!QuMo@K}9p zyl>~wSbbt_XJ_`Gog+if)yGFC_6^lX8rf?0*H5?DKVNmWCrA5dr@!^x*KFP~H_gX8 z)*oxtYRVK5Q1Q7ZR_*UfHuW58!GkJ|s}WE!lP4DSbtMmU9BCjgRT@_#phBisYeyQM z_;TU$^oQf$wvJ7|(c)P_#Tb6}tVm#mvC`!PR3y~HS5?1#c{;WFV5o`&@-98vnZCAg ze>jois?-C7X0n8?XkNtAckOQ=lOSYX-W-2FC1^V8V2U~vUORjdLXO^CB-Jf|N z{$>4ih%f!vkv_g@y7iB%N(oi*c{zcVSDu)YZvA9dNiSHdt<9oLRCCNzw*q?RFFz%mF}n{XbRXrjt8b2aXeR5S1iHo^ly@*saK_ZndgzMg<+=HuE1@cO z(7#c7?j-V4X{%C#Re|8Q#CK3vlt9e4z#34$evu+p4G{K@|x+x5VJDhnrL(h9B}GJ|DejCa7Yj3Fo2isw(%$6ZFvi z*{XC!^CIS+W+td&rt;I}`H=%HL5`!6V+(p<3&qD@ri-RVksjwt8C+=3?#Hx3-B`GnEsnVkTczyu+J| zv%HM|%}zAl`Ax*ugDMcJ!t?CQ88Q_U$fb1rG9<`UPN<6UJ%zu0vTt946;(0z9`T!D z2&nkHnBX@9s@&H(WU~9vZwB+CLdM>MokE}WtjbR}Pp}7g&WW*ADWUI2`B^EUDy`6Z z)&vWx0N~lWxtcP}u_cgOG_QVfl(t0t(kEl__f__FHu3qg-&an>OrD6p z4HB#{elhbyoc*c^nQT>5k)R6CvtJw`lO^J>i5pcxg*}yfD4{BPqeAb&yt><&t0JK) z^(*$k&RHQ(0lh(7POx?o#b-5owmW@i?%7s0ea*>ro6}R@^qcst>+_n{qlkct&polK zr#(HqcTNfqsx+=fK*da+!23ZDs^G!68UYnEdE&M&x|6ARR#d@*aWw)eX7a?mRhyF& zu?JMagK;$iDrWM8_W3~jw)E}pWes>Ru0}w`OrFScB@eH?5c0ATcrdO;K*da+;4EQ9 zD=FWH5~}+BP;MjZjtZn)bAL>dgWVogek9MyP7_n)c+e>B9vcIzQ@H zjZjs#r=)ARRp6m3+O_hYE1@c_w($08X8%8dcS&>d|Ne)#oQP9m2xPKE>_LJR#_)*! zFYur$Ulj@O5ql6k7{h~^5ZkJ#%2!3gd&E-=XT=zKnaO)lg^)-Ff(K*dWhU=I z6>_PHvG-utu)-L5^$oUXMHL8DF~&_~3C?T02gBWk+kn;jeJG(S#(p212ISJ4fmQk2 zM+sGNf4(YC16K5AP(QnKB~+!}mf$qNLvIH4D<@Q?-j?7r#Iw3_d7JLV(!ZcjmwlUc~wY}Mw#S5-&NiKf&efU1ws+3R_Jql;{#y9-wYg^J= z7j%TY%ZE0mW6{rgaF&ouRh%n7ySk#p^#hG`UGLXnPu4>TRdKGohps5G_RqHT(MM({ zI5+E|gsM1K-a}WExN^BIX%u=Wp(@Um_s|t3K6t&6tSvk%B~-G1D_*qd~~c_^VO^|KzDH=cCxh`lNB zP(oGeXFW7;Jn698*qeg0QbJYgXFW7;>{{@M-7Gj=B~+z;) z&s5>87*`{pDxZ>g?%|utcv(iE=Y!ez9I>5ts_=zWRh*cDuX8;2@J(fmyv*c1sDg*8 z82hhtJon(i7TL;~2Az_d-icx7s?r literal 0 HcmV?d00001 diff --git a/gazebo_plugins/test/diff_drive/meshes/p3dx/front_rim.stl b/gazebo_plugins/test/diff_drive/meshes/p3dx/front_rim.stl new file mode 100644 index 0000000000000000000000000000000000000000..c3074d5c0873cfce0755b5c5cd21b411a7e533d8 GIT binary patch literal 1094884 zcmeF)d9baN22y8)$vC{7p@ zFKQ40uRyE8C^!%!AhHVqgNQ-&IuVD&0gMtML1GMvNNz9l&+GfRKD}zMP9J~EmHvZO z?C0~YPu26(@U41k@3Z@fuQ=|MGqyhD(EYbw_R74fK=cFHSXanf^_ zZ@>EL>i?X3*m~vP+22~wmS3|;JMaCgMmxWL@AkpPbK9lo?mP1Ht+y8OiiGJ*NSj_; zLid!e9!cfrm9V@!eSf;0w$U}?LvFad^Il1qt{z_b>^6(qw@=?|{Ho9GG4k_1Lb?)O zx%!*OwwZf29PfM5Q?K{)K0>+@UODL(3)|2CZr|}`pV(*Y=Y529CA{*iKc8-kZoh7P z+5A;wKkp-?E8&$Dmo8{CTW;14J?QeWpZ5{cmGH{|Liv7{g`v~buc*T7EypNEsgjdYh&-)1J zN_fTeej6XqZeF!a)RCy)R47F%9_ z$Nk5&mEYfLeC=-cUQ<0BWnO>c!Z!2ohmG&~%G}X6j$YW_{Di~DFI&FLz{9cc@83J! z4!`@l@iC9RXmsamSNHj&t{-o^-NggqPkwu!_TF=LXrJ1C-q;rYq3`Y8mhZJ=oB!aB z<65fQZrQIrZ?E5P>us^J5av{gdyd$%{p+9a(QZ3@!F*fz?Jr*a`Iq--r~l>E^DFVy ze^}6VeB~zX8K+)Y`u4SLL3`<ZHSlmGlm6-Rk7~QV^ljt2H#m7fIKKYt&I{Y}CmuDv z`3v*M@1D1?efxl;##b)ec|iD_Z5b~(b5Yyx&Z+S`AO6ZNwH~&_tmIa!~u};~p3v zaOK6L+@n-|ZO#Ghfj4d3w%_oQH4x?{vGAbHbO}*9<4Pt} zIVx4c9_8$lXEg~`_O9yTy|UN-?DzxQ5pUS3&D=7K03=lX_4f{F3s0KgE`80ugS+cf zvt^w4TgSAUe|p3Cn9VL)1EK1O?=EV$oz%wLK6@AeI085_9Q3q9+7A{ywXL_+%E25Z zq3VyHJl(cBVNP5BwBhV*OLP?d>Pge>%nxnaraw3wsU=iBW|!&q>>D?0=UujHP;*<2U`;Gq+wZQ8bf)UdT1pF6@m|BnxB&pu#ooA>aJgXbZkYQvWv*p51Xr}nAQ zyaD0d=NxtOA0N~H;Yrtz54ra8L9dlib;ui*wvWAg!T41-?lB;og^h|o|Ba>X$v@d^ z{LUW@BV-9xo7}#n?Z4Hc@jbWC9e6k|IbTifd}7;w!#U#@{CJx|ua!{s(9uiVo$DPm zzU;<*2KUMpgb~umuUOQ!eDqD@_Z&D}e@Li$_w|d~j@x{C{PU}ZebBOdr}N#FP-UHJ zA2dqsn+GMF2M@h^|Mt;u?bTkr;$wpm&V21%xkpK;(y!LTk)dzAl5o7rV~OLrk;Tb3 z9MHbC=g#fiM|K~y5(!m~^R*t<+?gTI1`?|By)ss}<>%Heq3W-G`9UJG>{_z2~{6>;HY-h=Kna}>4D*RWvuQfkw*pzRm(oP zdMx?KUhRu_?Kc=pjB%ae@~kGI%GkHwE9WIwkkboh+R=~Rc6`?XZ@D%Ts+_MX;i}OY zF3)Nbs$M!?(q6vHG2@pl9quREmh8c~*Gj0ejn#S>@3`K{D;5b=#zED?Si~Njd#!{j zW0dOQ`pi3>@2-R@*KO6qxXqTITf2lR&(7qF5xqG{aHPX+Z^rk2q&S+c&>W5mNn|nk-mf~*SOWgQQwwfi^+tl9GBQKjFyZB zs)vNC9E%$1x$??2mr&*0UQ6XDE}k@XF+)kA&&!;g!ipQ3=!4!z+`|1`?*LhgT+_)g(+;53fuBv(cY5e<+UPu&%96MOsDypc)r6~zN;opO zVsRx?3CAmUrriCigzF_+rR%FoxF(Y@r%Jf)v-P=7tb}Vz`=e{oO1OTNFsDkmMs{3r zOu)W&CeM6H8 zRgOfJ@EQ309al1;%2BBj_9$ncJgZ5lvUgPv?~kL8Z~ii&%C~@(u;4`zzrH z;K<-fg-odO)Iud3Kl+}zgwNd57uCa2#?i->Jz13rRi3h{gnh#3#S>zgP-S1KgyW#my(ir= zp~~^G5{~+|vD``|RC#)@dKj@dKIRclLY1cwtA~Btx!n_#nNVe)uY~!Umd=DK)0MDq z8#8#KH501r^OZ32?yHA{W%rbB^)Sk?#pG5Zp~}<7)x%Ys(UK>nGoi{;)0MF1MtYw3 z&V;IbuN*)0>myV-uGCT)iyF6k0zVU~JdIxo;~nF<9Fa+=G7hR9_6h5hOC_PozEVA$ z`<#99j3c4SIk9^942(E(lp&$YXHz|#`;6yuL?)riIk9@U9xx8dk&J{Y*9q0be&@=| zZ$4#0mHn|2jwQ}MdB%}YmB$kIkov|e3CAnHl~(IvoZya!->A!kD&vPrIA=I&=aE`M zm2*k;a9!hF$TgQx<$9-j*p^)5=2fbMD%)7~FfMU5nOBe!s&ZW7b2j$P(W`_ipMR}~ z>l)W>dF3Uc%Jok5u$A=9gA&ezxm_7qIKt&9Ttb!YxYolKXRpmOj)W@btLouMX6v(6 zWN%30VIWWKu+s`RTK&H%1hauhD1%I9A_ zjF4Q>xeCjKD*Y0tezhJFMlbqR!YdN? zn@p&(gq3jabM$eHse}=WtunVnpPD04zE?)Y_Cb4EE|vAjrLy0-g2=7i9;HgUmddEe z7H)s6ggr{aoGM{!w}m_VRKob&K4=fFgma&SIaR_Lz%e+FyN>5R=RAtqGHkDo!L?Mj zL|dQfO4tYOMW!p^71P%!@XF+bv~8?#<;ldsh#Uk^^c=A3+8JNeq#Rkf`z zug@pn1DJfD!5s+uUEa%>e7|J!JsEeS`u2S$-&?XJ_U&I;s=ht4$@k&xQGI)T5`FuL zYqw`P`N-h-p?4iid>#^c6qQhAzo{PfHT$3~KNG6#X_c_&NaUGQLX|zOdf1~Ras(iu z%642m9CwXga-jxYe_O42#x4PbiOsMkBZzWt++cInmnNXEi+vzQP9vLK5`6j-W zDxDUvRc1n!EAL9AQxCT3OsMi?L?zPcjocC?RC&UqdZbe*wwO$)%I(T0kYq+|6+r!kEF9KA}Y^6YT+u;02Us#Ui1~ z*{+rG7hdp z`Zk!mJDE`B+*Ju@FK0DZmYGoHcjYQ!@3hytACL)Ewzo>SGI7O{SE&-JjH0TCz0+Q6 zjFJgeMmUx5E_!!ew`D?=tG-Ius%`DAcQc{NXs{BlOkA2&)j{+OsI18S_wxoN4UJ>C85gLu6h{J8|mjANC{Q0kgJF7%9fF50|`|*E-?Zy z*0R0TQc0-Fv8Xk46=wWcJtS1+n86XiktmNo5~^&WwN%FKjw^X&kWi&}_3$p*Q=L~b zp-S&cIDgnvb0j07%C=BFoLTH$xkpK;(z|-t!kzWq70HAuy(?jhGx9bj%7m)i2W@eV zYW9NaA)(6BR>FK8iKHuGpO7%8N?0oCOgMfx`sBSOTbwO1ukO4jjwL?LS`T}XeK1FT zmdbkMQrW7VckPe0RH~%yE0wU{*}`2jRKhC~_LWLF*H-ECuY~Q&bZ%Gn57W7Sn6HHCTq;%O)JI775!u6*p~`giNQ9qP!j_@R zbRS_(m9S5!GTld*Qzh)%s!aD0=2QvCD^;fZ2y?20JxZ17KEj+TVLw-8x{olYN*JrD zGTld*Qzh(cs!aD0=2Qv$T1u}ip?gYKkEHVRO!U>myd?ZQd#G}kA`_}iXQHoEs!UtL z?4io^+7gx|rK?9$`FSNQuYJPLE1}ACA7S5?t{$pP_Ysa!($zzi={~}-R=RqqGTlcw zXGm8MRi^t0=RWD`p~`e0;T$eqJyeC0y&f2arcN_Y-U}KL6@rOLQ+I zw?s?jen~EsdoR|*c3ewkk8#_E{7k5_ z-&DfBW*@ZWXF`=dtrGSeiQL*HRN2$2hdoLnw?qk5w&UtyFLJESqo{p1I>nCRBOiq7t4EaqP;Y zkAx~uj#Lj%x;TG0Uu8m-Ctxb!i5+JEpG_uI`J5_Yi?byf4Q4`>Cz2}R2`XcFV~I?t z@?=#dJPBs>VqeLGDtlTbJke&<@A#MrRgRUFur=69@@ycX$`gCl!;#DpE=MvFsyumE zJv@Qw?Cdz730005nb5;@A7N=L;mK8F2K#&_R5@Z)LU+qFwINXB(SCRDk~sD$5eausHO%!Dd?b0wTvoN*i_GNCGuC272JJysPda= zmGGN$?synKWI~nEMh)O6RKSMRKi)!dC(S<301jW8Erd08n09j z303xjN;tFFgL6xiP~~h_J#4$SzC6N7sLJ=sIm)Nzm|sgJp(?jsXD?Tcwzujbp-S&c zco$uN=Gj?7mEP6EXxr6`^GYUE*%m6{%F7j!oP9n= zuM(RQqF& z+r1*;oLD^^UmbUy87kpu>GOA-uY}{S?bsGm30tD++!F0krgM)nUkTH>RI1FWkC5&o zvWIO+mFen{2tTicZAq2sKEj+TVJ}i;x{olYN;n#*GTld*QzabXRGIE0%&8LgC{?EW z2y?20{alslKEj+T;pn5vbRS_(m9Vd=GTld*Qzh(cDZRFY?kQb8lFH9B(N_=ilJN8D z;l7w@*PoeCWx5isOik;T300;mp}T4QGNH(bjwK&h653_gzChRONb@uYP@mD*f`^ z?ej<~{i=ucFr7=4RDDD$VJ?+r&mQJ$TDp29RUeV)|M6Jwm;Y=frt=d_sy-so^?4-K zZ@!1^)#zo>J}0*A-gLwGkQ**v!cua8irUwwjlnC>G~na&=wQ4b^5e>!)0 z+ik_c<5zudkHPb>wyJVH%vZXPP^DkJS9+N4BUG8r9u7~+b_Ytb}tM^I|(|v?0 z)7fJ->fsL5C679`-FnCt?a+fRA3P6ht18#Se5LycRr=L?rHAQ0LY3+4F&p)8*Kwnp zk7#F~xl23ur@IfHhqYCe>tVjqeS|9g>b=s#bRVJ0boQ8ydbrd4yvIzpL%*|M+hg(c z{9GzkxgO@*cdsPOSHD^+3DbRqD%06xHtLa9VYyVU&vHG?x9?s_n6G}dR1&k{(YOAv zRIa0PJ(PPyh1s!myPYRI zF5xr3;AKa)hd(*D&D^oupdRKc-AAa>uhzqNzrGV*y5A}7XP5r3@%dY9IasSnsCw|= zQ`&vsdiMC%-R`}ndYErtOfBKsK)>o?zLqeTN)x$UIzHz_u&SmXy9$Y-W=PPpu zPf$YDZ{4!2-M9JiS^N}r`y4I@7+Fo_<}VMs`RV%FkefUOC_O7zv|&xuD&&nglo0Gf7-Ea{GQF* z`pYgHv=a06Oxe$0bZk5SujaI8oVq4Lm43Az=4%OasU%eCS3RtUr|EY8+41dP{^Y*# zPCs5#Jyhj-n6D+wrIJvkU#*8{1pD?GB-}4K@T!@1=<-{}@7eENgJ*8Oo>iRt>6!NG z^KTyi`~$;#C80{cS`YKJgt=4_s`RTKp1_puBUEj5$;s`XzjNXE?hQ^}!@bh4k5HBG zm1j>qlltUU%i29#oG`xm3-br1l2CQarOVo``<^hqa@o!U!ac-34+%ZoBdyQFQc3p_ zs&YL%+1nTIN*M3Xd}n$4@NN5zFL?MXyVQD^uTf@>EG1OwS3S(v66R7#sM4=`xOVb9 z^_-KAY&%{%w>@(IaOEYT$~9H>FkjbgdF3UcO26u1zLqeTN)x);yY4z8>XIZ=R z_!Gu2dgHK1NvO(g*L?f>hlKq@zgiE^)x-x?;_% zR0&o3RS)yEgt=4_s`RTK?veN{i1W8NzJ2t9pN$W=a=7x6Q03l9^)TPQeIE(;Vf3pW z=4%OasU%eCS3S(v@0k4JmB+UIZ{4hIzu|B)x&(f3%TYJs`RTKev3xBk5F~a z!eiSOFP_srywRF!u3sOaD&H$dxIPaFJ@T07x~6a3m2lj3y;Glu>j77jc?Bt<%5_5Z zF#a?m?~B4!8JAWMN9w*fQNlPekL~W_^zC0sxPO)Ree8FA`#uux``91rz4Du-eQj65 zwtL;OquX8=KchYUoE4+$VU!`=N2tniiF-Y+?(>RXLX~?!wH|(BxvvjO*ax5T`QzHv zH_vY~w+y2`M+U#|eBa*3wd+4LYL~udc-C1$RUS*s*AnJZNvP7V-YZY2+wya3mr(VN zr7PN5D{dJdv)P(D)RSB9{A9i?e?3tAD{DQj~~3}lu(t|QRZt2 zbEza$=~wTSzX;?0e%_&%P}NRc)HeCi{%yhc4;j1#a8`5n$uo|Gs=j&966R7_5>@)u zd*$A=zrFLO)jPubjJIgdSUDVbB~-a*T|La#5jBsZ5~}p89_||WOKgAfpQpF$c3d{T z`L5xFFzUe=SW{d)$crfdVBAmE*rn-{NZj?`mLzE8zrG? z|6iQmo_5l*@s($+$-@%%l}c6iFmCrZw%)xudi|#x=eI}h9!9Scs&d?JzK+^?q?S;n zUw!8OYMCpfyeg7V^@LMSZcn)4KgI`KF^nwTzwkHD&i&d<`>i`SZ`(g=ICDyS`YKJgt=4_s`RTKek0ct7x|QhgsOW#u%cab(Wl3UTzmOo?d+)+e-rS` zAFXWLyz{8>t8N^AuSh~wJ||dQK0AKTfx~z7zR&hIOmBJ0iuMne-#Y&JRl^ugLY42iYdy@@66R7# zsM4=`_zdj9xz|dl^4U}mcb)y++OxJirfu+~7q&lH@v%XqFQF>$Q=4z!{*{FLSNhd@ zn6D8>jxr=v=~q3B`ugHs3FF-y!}-h1o?FW&&m>g6bL->Uyf;j>a~~Og^ToB3bRVJ0 zHC4S=-d$H=dF3UcDnAcT*7!Ty-sRX$&0zK(Wzgp*LEU%gk} zAJg82OsFzl33t5QnaaCS5~}+4$olGGNi4g5gL+)BxLtkP1>?KtoILJ(SLE;8`>yCO z9y+ZZ_mfM;-(Q_m^>BSA-AAZ$-BvyPWq;4#f!!{Z_M&rG6_}gYg7-{vd#>7Hjq%|8n=4*PS87@ z@2-R@-xXF5_b=?hxz|dla$lo**e84=klzAGsIsqA59dBtFL}ixp~^Y2diXA_@0m;Z z%zejJJzSqTGvwJoLY3>b>fw7Xck%MhorEgie^n1#iF13Noh4M|c4fY<6LO?4p-R76 z5A*Ho9}@Nt{i=uSv%a`p!noaaTlVPdgL>EpvxoV*x?jVJe$Sb9!fo4*-?aMeq$>St zJfwyjHxEiU59T?_nX_+9lyFSU zbFKMW!dxl|Rr=L?<(^gFTr1&R>t0s%aCYvihlKUW^SSw2!dxl|Rr=L>*rS}O^Xx34 zD)&M2?TbYvj79aU^)TPQdnMt$(yw}$uXiEWTtbz8)x$X2s3u1;5~_@^tB3mxedDf# z<8Iz(FkefUOC_O7zgiDh6MgZOgz;5ge;ASV)kDI1BKaz!u{PmmN-WB?TbpdFY4G; z+m$WRbZ&`Bl?nTVX-M;}X~Dtnl(Y3W+3 zr0OFawfjbDRoTOQO-t8$Bvl{b4A(cSsmdPaYg)S2BdPibqk+B%KvniIU(?dH9!b?l z^z8xY;T}NtFkjQswH`^;N4QIocP8{ul|9VYv~;aUQuPt;e&ro6Jyc~6^EEA9>ycD_ zg!5INar97?J)Ch&=NTud`iMl=dzDn#!~0{}yHGuns*gzY+Im>?J`V}Yp6`{Xsro!5 z^!W9jzq00i^^mY0`Cgf?Y3cvYXKo2|sgkOX=&Oew);4>XuW9MEy;tUI`hPrr^{=J* z1Wo5Bm{fg4qU$qHs^5GM+p9Bmo}DFB<<@S#(tU&~{pu6c!*m~^%5?Uajd~aX_CtViq_e#Qi^{b_lFx^L} zGMznUqaJA&FPF-_w_FeN?YmbJ=Br;Vm4xX&LY3+4F&p(ryTiFu?&;=wm~Y>`k}zNW zYN;el_Yta0XOG#ahi?S>-U6t~^)TPQdnIAM`qg_SVY-h{WjcGzMm^FhESJjlS+0lq z_T4K9^VP4GN@6xV`qm$o%5_w(hxtnP5vug7_v$yr!xJgiDc4*=RX)RFzS4b!D*fv7 z@a$OMGneq0d#0>pyRS)yEgt=4_s`RTKeviS^@%co&get$^P(94I zZ!bf_JsJI~hxuB*^e``C7tU zDhXAdd#?A&?-EJ(5vu%-QT6Z{=+{T6^4U}m&zt+L*8ELY300m$uO99p_IXI?;T~!A zuvF50gsNPR+4wCB<7lJI99c@JGQO_&%6zR;{)G(*Rr*yA*G_%|IDhL`LX~T(>S4aF z+w#gwLY02i!+b4aE|r8T{i=s;*KY>qZv#rG%5B$t`}&83{X@T64}TT7ub)fU&-3pH zo3AU@yh@c&rC+Uw`C7tUDhXBkRS)+_*46il%-8*yyu%`)%I`7NdYG?uaz`f{*{FLS9#yZ ze%H6}BjLV}{juIFf9t%j?Mm2o{SEZ$VU!`=N2tniiF-Y+?(>RXLX~?!wI2Rbdf#{@ z;dqsQTis`{uKrev`C7tUDhXBoZI*hk{58bBelB4@&%dAOShB8ubJ=_?VJ?+~Du3g- z-YZYU8$aX-KtfeML+?DeuKr$;`C7tUDhXBoeWZG?{6*`&eFh2l8S-ynJ7=t`-(odi zOPEU~p~~NWt@p~m+G9kXqi_jT`S1L=+Fn<`nQ6Y3FqcX~mA}zh@0GuL+BbhlIDh2d zNp;3qSAWyOd@W%vm4qt)hDN?2|9lYd{-XlY&j-9GcRgt=4_s{A{F^7;5~}=N?|QHN%O~!t=ABdtRrznF*tezo2vzp^S`YV) z*VW&ZF<)o6JgZ5l^6%KxdiWQP`rbQB_})4HO(gdg*45t`HD61ZOC_PoziV3Wm47kB z-Ilz=BB3h(jS=T?={`c0b9=3af9bF9U8;odQuE&ibnPVFN2qd5RqNrojCJ)lM$FgI zE{|{$s{ETKwI1$9t*gHcXuhs2@@hgtm4Az{*2BMq>5gaK<&;pB|8}PFj&vWP$~b8D z>tWeVTf$r_Ri-Q9U(V`#=Oy7gul%>Rj1#2$2vx=pwI1$&udBa*ZoYlbL&9g^-&?Qs z@XXb^`unx!>qwMG9|=|dz1&(4-zfUmR`Xw2l~9%6LB4z5!uIV0jv8ONZ0E6Wd-EG! z303}G(^?OSzIsSlkL=-JCUsvt?`%t`%74q$mMGmvsLE~ESVX@*LX|N}eIEV=Nl*Xg z6TcFw^52~F1opc68;9m=33I6=RQWd*>%DRh(H-c#>nx$ly~XO`cy0-EsU%c6&Q}lD zXRcoIibX<|>$d9QGjJy)?|w+A^4U}m*BtJ!`SR z&UaTrmG3F4hke2~vH9(*gev<=^>7{KTe%FGM=j*=4%OasU%eCS3Qi7eczJbm`JF~ zF}34)Up*wOhvR&$hxuB?n*fBx)!PRFkefUOC_O7zv|&T58vtJ zw=xo{d>2wZ+`s6nhlKTTU!!`MuO-Z-l2D~z^)OoMi+3fAcXNDYgxpsT3G0z#YV);( zxl|IW^sD#E*?C>X)b3dtW#-6ILRH?gG+*nKqYMdE`qg{o?7XhlYUbM)Ur88Wxi+lz z@cvk*9A!wT^1f9MOXcoY-tm%9mFpp4be1DB302v{k=mW9yc;ELFpi zzutOSc1ze-Dpl60)_gWREW2q-SWA^unXn$F`)aPrbS;(nrV?htvS$zTbr#Jtr-Ul~ zYN^cEv~(s^na+eGQ67CPiK^^jzNV#XsgkOXaMbP_sa0hU^EEA9>ycD_gfm>YBADq(#ddRX@C;dhBl z`;DRMkyM$m?1`=(66R~2Dq+6fg}$1r(yw~h`b_%`;7q78y{-sL*yoXdVZ*#s<^2)~ zYny)wK|+=LGr1m)2A*8cgepsz32SRaeD+Xfi^+r@_W4Yxvc+V=Cuq8l z=(|_ec5;tC!jkk6_K7}X@_JY*)A{aNDtmAyRP{Y`>yZgtpKaGRR!fx#KhK0J(|v>` z>?7>+nNVdq6GmYy+=6 zr81ofOJy6&gsQ%lXxTI2`06<5=e1N`kuaSJY15f7U(T2TvxjsZ8Ir1xuq651Yx3|H9&6ZyhlD|D*sZ`lw`aJxGVpMX|?<4XlZ}U=> z&umNhJo*UhVY;s#`7dW#DplU;>Y=-Dj{Urk$lv)fFID*)JQ6;SKEisK?yHBt0g?$- z-swu{?)d^g?;|{cp9xj@48882n)KOZ5A&7Igf%zaNBCQDnNa1ksf6yHTk-Qg!c+d4 zP?gW=>+T8DKEh`%ol9lSP4^N0ou^Ew^4U~Ech5ulc^~072{NH7e-}V^PviCxK6B|@ zDr;`KkMQr(WkQwDrV_dvQ~7xx;kQ~dp(=leS9eb?_YppG>0BynZn}>!F3p50pG_rn z_g)!aSHe|_C;Rgby@aZ~4=16@^-eC8>r6|S303~Wd?r-&l}eSX;_RWyI#t3ohfmF4 zM$CjN*CLrv&rB;!XB2Uw<<-6RPyfgeCEBVr4>= zC9H&Tt-qI%BQgn9`PaoHRQYUjsZ`}(ZqY-Pb*diTch^z=f_5fU`7|@3%4d@aRX+bp zq_TVJDHE#l`rK8#?^vX(hlHxhJsjt=hbrfiN;q#jBm2v`nNXGgj)@-5oc@Y^_E42Q zEW1&LeNO20~^eDhywl2DabcV2Nt=U?`yrIJvkUnR`f6{LR|Dif;otAzQwYmooql!Pk% zs)sueeeyy@J6k%7WA+S&V)~}k5FYgd#Ey<31% zN0?J3%*)npx{olYO1RcH-@HDTaQtvw$sVfotAy_M?M$QWS;9F>(LX~YS6PCnLA`_~z zhi%mLho4suuSn$8goG;h(y-^)Q`1RC$U$6RNz^l`vn|fPUUbxF3`WRqp6z!spRPSP#?LLzSo4Goi{mT?zAb zt>))_glpVPsB$N44a9$*-mcqm+4$zW=8yBvo#j>4=V8rF=TfQi6niFA`D`j-zOH5c zypM2SBNM9J70HDA8hwP%TsnJLbJKlFi<6P4^MTrI}FWv#Esc z-YetlO1RGO4WDoRGNH=zGnr83dM6XEQY~R7%-1tInNT&khpXc3p~^Z{!Zn9a&6AFq zP~}=A6RO-n$b_oA&tT+bMCRH0>LH=Zv)q~Rjj}OgCRBOqEfcExN~OxvblF3db*hAS z)D?v9!ZP9A_1t?VROy!qOOj8kTMt#1uzKk3I?8kJnNVe&GNH;6WGoi}4q!Q+9 zINJ zQ7!K+NvO)Z9}=qaY>-N_hB5I4$F|S?%ba%J`&W(n?yf4|l9;adO2Rv8x{olYOsJaN zqmR(TbS;(l-DjTX53e|i<~?KyRr=LZxoY%XL0%O}IA=JQ^m$mqOsLYY5|+1b9hHdu zK1xEB_pO%7_|sTD@2W|ta!)RM7~^Ill_W>{uKQe{<$f-q%KKL9A>k9uL@J4^$?B0x zl076$XTrYbNNu{7O2XFe=b2EIJ-lMOGF?3q;pdgG{g|(-$x3*|wNv(xP^Dk>NPQxY z?GmcozpNh4!hQ9yy!KSn)guvpUWqhk7{gb>+M3QaSC!J)!+fPP;r;0&^e~>w9_B0E zN8|{=d{vq5^Dw7Om~SQ=ah%Egyn1-Wv?Ea^d}^k1shsKc$ZgjV*?O3+rAmaKSHiq3 zm7iC_8umR8Ri@3UdU*aS?ZZ~WzkO!fznqo{Ri-oHUw+pw6Xt8WkC@!!F}qB+XWzJ4 zJMXepqwJyTufKOdTX@p^cIj*OT?1iBCa*^yG5Ov7(WCci-#&eZ_Q?IajZRr|V7u_O zJGDpd-fa!_P}Ns+Rrek~-Cpse&Dw|8zhn&_s{GsZ^?7(tQo4`uueGl$!WNS~RGH3% ze~FlA(bRVI2CHy;+{{2b+dSxYa zw=Ma3CRCa3BlN3;H8h<)k}4C9QKtI{Ri-AClsZoaDgJbU;AP4^Lf9;znym<_@b)|%^Xx^Fg6C7nHt zca4@ap(^)5-BlSsWDixw50$X>*(dVoBcaN%D|;9%8Ea)il_kuCwe>D!LRI!KVlk32 z4yqm!s*H&$VccePmLoC=RmOAG!*S4dWx9_rr%b4t+#?fSnS5P4Iia6*sx?olOnAk# zIaQCO>La9cYfmaa|L-i7W&ibhSdzY0lE!mK@OrN#R5_Pa!oKauVE@mAsys5J+ImN- zhgTd$9fLEW%Da#WTSkrmES0M4kw#}n@LDPfRn82RaJIDHIiqGmRi110P~~079;&?4 zmGFx7@bgTl(k~OLOjp8^c(05&GNH$klM}joM@`pK%?4o& zP3KZ2)oc)Y*L&p^OJcf@FsDkGm+3x2mFYeY+d?K(S;9iqPU8{$LD(h4UXG>MmnNZ~%oQZT7GU5Hn z9!8M1N@JqxA)(3`wGu{CmBJWSVm%m(58u_UHzsS@GmnNT&kM<%>tTEAMVq{_qxJ~`8_TDAGq zn^&LmUUbZH?RyJ$nY!rQeMf#?J-i}eIup{S*Ot&drK?9$`FSNQ?@KnHqKl!QG`*|NBT?wze z{hKS>58rac=zX8qXYA*Fgmfjmvfy|L$^~c6v@6#8-Kpt= zE+6}OA0b@{uWT^?xVHG2qp9x-rycii-RKkp-?E8!J; zk)QVw(v|Rv{n5|+2}E9a-)HKQ=>_v`;eYp>)t}$E&(wFXzIy(E_{>aOe(dH`&pP$O(zn0y?wNM^ z3%8isVA+L(QtkSRm$#39{r=IsA6Mcba;hFX%w5q&Ke}b~iw|5_#;6x=v!XrW^Pd@A zw%@x39*$(vPu%>K?c?(=8vSsClLv(3>l2@_vK{}~6GykMK1X!pmMhzBKRm=39=5)X?>??=`O(VBX#B{Zj65AUw3>|qO+&d)LH;j_b6MUbZ3vE-`S39sT@TegY8|JP-Wk&gyXJ$eIvChNA2oid(|(G z?Gmc&Y1PB_Y776w507u#?)A*6toLc+I}h7@YJ<~; zv$HMHQFP9c$F^r*yxG*Dt4}f4dnKW2?`Iv`{`SLjrr!K3-#4)J*><0?*Rk!X8?BxV ze&r0kgsSPok8TGa^o*&k&ll1)~$^Ox;0`o@!n^Pq&PpZ?J)?e|{)?9sMLX=C&(aFB`ohw+t!s^iXC*3SCF zrK69YF(e$xY%#f&NT~YQ>XBj3Wpk$<`ju~t7`-?;Kj%MQ+WzLzKO61;E8kF%P<7eD zW7|=$*=*{0zj99B_}me0`F2OPza7n++V$Zb2hT%7)gQn3$hOg|=T6-;nl~Vv`<$bG zvE7Mn?}t7$I_TQV2mM?^)!UXYZ)YEN(CCyK_ZSe)!bZg#-nP8mbJYt+Z~oCRLY7eV z!fThc-?{6!(RY94oWApt^VQO4p4v9L<#$J~xNn<5ua!`>{eH{ayKgvj^u8PS8Qd#Z z5JpJ%{n<=g@%@`emmN4@49MGbIWep64sh0 zRVIuw5?wtc%s0m+&P%Rqu7B;xZR3M49Q|<4aMwUWmGf0Cm2Jtj+D`v?O#AZ2TTD$) zUp~0I5~^%t)x#B?(Ljy>BvgI(?~ZR9|MQPW^Bx$ESH|j&5_x2hQ1$LBR*xm`*?a1J zU*B&qmKftY!{u2`LY1*^y;sglt{`8(^C|6#H$7|g&eMk9^O8{Id{sSMH9EuPSxrLK zst+w|fB3V-qoqrS`-!$CdvNZx5~^%twI0Shu3qwrMM9NvQ1vhtu?Oc~E1}95rFytN z^G@fxE1}ADTlFw*v*qX3E}_bJu6o#BUA^QLi-aoMarJNFQLjcZuM}~w`JI3GNCHRCAJKsC8L4rA)zYA zqDFeIymHMYR5`cTQaQ>v`s9&8LX~Z-dN=|&G8mC(LY01%a0ala=DRDQO26u1#9~i% zX2^sppMNFnIrgYL<4CB|uX?!S;jC}J$%HEXDq-|u+N_a)Wev=7Rmar1;AvyXu##F*pob5QbM4y`DO1@V{clJShS}v9K$fdI1IdkUL zZvRjvT}x$jZwt3SR>B@7VNR8>wcEm-eJWwBZXdJ4sIU$|LE3ZsWNar5qmB|U|+|Ru-IU${6IIm1jNZY&WGxtg#VJ}MQ>XB4_ zUI|N;(rZiTZd$+W(MP0}Y4%8jpRaAHbhjR+vxl_lwIy^<>FSYGeqIU7+t=C?AzeMZ zGI_6+FkL;oGWqBuVY+&FW%5x}!gTfU%H*?wgz4(xmC0u{3DecXE0fR85~iz%S0;}# zBurNiuh@(HypNEsgjeice%?n&_Ywb{9=e-0I?tuDB(4A|VIOog;VPpNjts6?T**|z z@yeYkcfTs(ddXJl`l=GH$t29F60ZAfeXbJ+1ZP$3kFG^4;rdk~uTotjJFd8`}hK%!Df6W>&&c%@&ha zsS>JuD_cG6sm`3fQO<-a-!50eo@&eR&3GnM`IfvAwhY^qZ}>Bz%D4TMa0GB<@T5W} zRC#Kl5{@5z&s@T1?&*u_;V9$iKRf}G300nksf5qK8Nib~nNa1k zsf5v;?KrnY300m>svgez#_FD|%7iLUSyjS5Vf5k&u}rA4uT;Wu(CFTiZkbT!_*e-? zecM=WB@(JUy;nVqSR5bo2q&S+Q;5~WzU|!Z3Cc{Uvd>q-d`(MdLY3)C*td-tJkgp7 zRrdKx7AL&CCqO1F9#W!PeJE0Iv;Y2)hQD$Z!hlhT<`<*DgPSaTyiPkd)WRlZk_ zANutXsvK8psfS4cg<>fb@GNH=; zSP91xXP-RdNT|wViF-(W?aQ$1`;u5t4!RYH|*ta=!ixSGr>NC{OrF7Y`V`{w9XLY2?I*28s;>$be| zl2GM(r+U~*`sP6i=fT{rj4T}CauhD1%644qVT-fZ<{3vqmGf2ga3r(!*(x)kD&H$d zXP=t0PxX*cmD{c@!&PIR)g)BuS4-tA>9r+v zH?3dx=p$Tbx<_WZdL+WnD`8%)SWWj4=2QvuvLvSa2y^Nqa;bb$DP291%Fip|UPel< zEuni~yHaJ^oNB4OlG1BSSdx^k9!cfrm9Xr65voj^Q!Ulx>!``sYLl-ACtq_;zOJ2o?d+=B)|c1klkWjc zzR%zeg#9k>WlX+bGWnj2yHS1nK9lb)*%JHquPjyH9@*skaQ3Lay*`P){lvA~vz&Zn zaQx7_jwL=1i9Cu*sIuQw5Br*Z(3YPGRra(>*mETE%qgMDo>o2VQ4%=z(`@g`gerSiCDL16??NV2`R2D0uBvSrwuMZn%B$`4mOYOQ5~_R?UrUuv3)m_% zp~{taCDN$}+jJ&Wc`~9B>GVc!i4v+j;ZZ%(DHK~wCRF8i<&*LW8bwtP300oasYE&z zWGs;hRo=r&q|;4CIGIr838+fg8f+zbgp*L^*{|x6PMaAAWkQuF(khWo%^A;ULX{`$ zD&Y>DbCh#pCRBMsuoCGMW1gKQRC&^|dZg2sMjV+?<%!Koq*JB&eXWElPo7o}@1l3t zSu_)>yd#xJ^I(o%B~*EKxO&)g>`_L}nNX#7CDM51D3J+Oc`Qk%$Bj6uhlDC;iAp%? zI~#ZxGNCHpD_ga#-59=lNT~8V0+mShuugr1IrR}n9GOs+)x+LtuQf)=geoJP zN_ZE&yRO?Zp~_WXC2ZBUcGtU^P-Qe&30EerSlkuKgerSMC5%{%WZd1!geu3%N~CA* zK4T_Sxq7XHBbg&y-tm%9Wo%bHjOdN@^A4nhDp$zW!**rMSi@|<@BgdHafuOtv6k(v zmP$fZjzz7Zt1#oo>LH;j#|(}DjzoF%kx*p|t)((0Jq1oRPONQ6^O7K4^<`RI?XU z4+&M4wi4#+NF-ee`-Fr!Rl-t9XTtHr(I@XM+2U-8d3EPKaV+s^)_T~B?1MS#vsBh2 zm&#V{yla1~rBWqrU#W!s&KB;Pp%Pw^u&)dVW*l3%Ex!`>RC};1fJ%5p!a1=Lj(t8& zN9{^D?%FDS{*|y@na=IX{$V=z5A&5UolB+4ocajqJ|cVAGE|wa9*OYtO4u?~neHRZ zsS@@HRi^t0bE<@WTb1cP!kjAMc%{m8A7M_Fut%vf-A9;HCG6*_O!pDyR0(4>Ri^t0 zbE<@WO_k|B!kj8$UrXt=C3H{e>XB4_o{7GCn3sf~XAf2GQe;Av=}h#MN|k9#m_1aP zUR%PFq;&O2DnGA;<+V@vc_mbt?j!8m($zzi={~|SO1gTeGTlcw)=F0oRi^t0=M3rU zp~`e0;oK)(JyevftA{GneT1=yboEeWx{t6INmmb5ruztck#zM?WjYgH=_6E` zHm6!DqiqS(l`x_=U(=Ov)!=F(_b68;u2^zEcbBVgZ^`wNZOQdjt%p}6T*Fnub)Rj? zz069uwzLijL*M%*b?2#$Su)Q zxnGh?<=%_+upQS@*`wU|$vsNKy(Q`D;XajwIaR_dmL%UR2~~Pm51)sGEk6^g>^GIL zuh|D}`I%5S2$P$SqMqmF>8C*oz!%^C&8zD)&K0YKa{6NvLwv zuJv$im#`gYLRIcjzKOAg+m5S;gepg(N;rP_1bsG{P?bj?XEkRTdsp?4P-X9`gm3s9 zeY|g(Q01GyO8BPI-j#clgeu=)Ru5OBwh~9aOsMirY$bei?96Fz&V(x8C|ANa;I<5V zb0$>zX1o%<$+unEdNQHPH~f|GM1rG49vLK5c~YTzq-XB9k_lCwxTu6DL>#;F=p&)Z zlOxr`lP=C5&R3aGC2S40k~|wosPe>K^>8F}gv*hPgep%SRu4~L zIy*bgXF`=DMke$y-A7p3N_cYBn87}u30005mC)U?8_#D#l_zs6VQa9J7;$7ml_!iV z;kUMowOsXOLX{__E8)p*BR%gzCRF8n&DUVZ-6Lqe6UrxLEPTzR?P$%HD`K9z7*a~`zCWI|PLS4P{8kH#z2Lqe6k zpc2k3_TbzSB~&@vRS(;)tuK#o5~}jOa*pz;Ip)_=NvO(g*V)TeqwTGFNT|}g65d7E zpLuqcP^EYEFxqzY;=GaxRknpnxbkvEXWW$uRhG6Aws1#@JTgeAvb5F17H6N&(W`{2 z+y`xOj=}bVS}F-udRM}Hjb5ZHVV{sNr%G5V=}b6&IQlrFRu5a8Eip$j-V?_XpJw&2 z7ug4mwJTw%tVb@Dt=biY{jqwelD4l@!hUB9&(V@sBLaB4i0oloQf0b&B*M=tVOvsVx{olYO4y53neHRZsS=I`s!aD0=2QtsI8~myd?a*dblrU+Vy89RGF@XD^t_@WkQwdO6YD{zf7nyT?yU$JiPCwUvtQGd-38; ze>p!^kEF_k`xmCS{>*~*>Q`(s_2$b~jjBgfMZ!|quIBy60qxw)=1pC6?!E&L301is z<}2MtsM4?2+~rmxl~EjNA%T056hlC%-6JZt$9-Q5sCgEkM(}}&nIX) zKf$ExBNAPoM^gRfd)Qu$UY>u{F>UTkuN@t9!{uuj6IJE@VZQqH5vug7Pf!ojeS|90 z*<&{9VZ?gt|5@6O`-6Q)r+jXY!Sk@Ts&YNdSGtc-rC+^QdYJAbRGH2mvr!LMDVH65 zV*CC@j~ShQ@>8$RrBapaVZMF$O2T~gtEDQ$b!W8&?|A$#%ic$*GMznUqaLnIXI{Cq z{mDCCG*U+-HVp-R76Dm_g15vojQkJ+e)yAj)NbxeEmHrJ0<&0jTm z9@bV>u7~+b_Ytb}tM^I|(|v?0)7fJ->fsL5Nn1|0{chQ8YWkqd2hYRWs><~+U+F$V zm45YJ>0!E$P-QxM%tk%jb^QCU9MCqrcizarMR$;kR zuFrBk%(w4eNtmyGwNw(b;nBDLuvD(2ay`sfx{pw$U%gkqDIUHDv`)F^5~}uo<8*uP zf=#9l{m3Pwe6OVY2vz!358r_HJ#z`4`OK>iY_EIfj#G>8*lkb`^Of!+ROwgi;k#eo z3IDH!GwtESw;tW_yIT&{Y7(lhTQbxBS4Z?FqcX~ zm44O3H|xG}Kl^)2+Wj9sV)Wgw%pE*I2~~gl)g^7yJB}PJUB1hJFyFrQpoHsM{c1hT z*AnJZNvP7VdU$TZ^9z%%zf0rC;^19-hnj%+80jIS>B+)cRYjTthun<$9Q}CCsIgP^Dk3 zho_DDVjl_Pt7|^DfBVYWdrp1t>Z=D)xcPeO=}TYQzkT(fJ*O_7Ua$s2m43Az=4%Oa zsU%eCS3Nw-)wjlxaIN;2J5IN~e`m9)4VGOvXeH+BnX)e}nr@GK+NM*_I(1EiD*b9b z%-0g;Qc0-NuX4G-8Y`=F6p1JvYR&nDmFKTZ(X*~MH2Zr}bLY02C9_DKabEza$=~q2GfhpZb zsQSrOi`#Ftw~v0f!O3g5SNio4s`9<^?5Sr`fAaAq?ceV>dUWd-<_}6Gq3T^%FKI_V zYT@Xk%XS_R?jiPhNa*1nX?-4+O1h6wmFwZj-oAKO!gzP|;L>*M>s~NA_u;SXQtM&9 zMwvOXlu)H#^)O#cm`f$0O26vi+R5|O_pDxdwWsem_0avpm6wDn*HqQRd|kKYm6wDn z{i=ugTEbi^303-458JM%)ermFl6Ln~7LE>l;;) z+W*So%1c6(dneVyeEar&B;1G5uX>oTCCsIgP^Dk>Fkio8^3`*v+n1le$<*^U9PUO* zsM4=`n6Gys*IYuCe$~To(Mb0Zs$Q~ux?OzZCR0~#w5FQt*GH(z_sS8j&qG3wJSMuX z=^J+?9Cuys)aT)Pz|~}4K}x7{olrfDKaI%yqHtBlrPafcx-U+YFiy;4ySq4j`&Sa~ zU*&xt`(59@kA(X^_Q!g!{AOui+m*2GzV-D7x6g0z^r@}SSuv^}Mj6t5gsL2uxYy(A zKCkE{RJjLK>)|(+`}&}Seef~&@7+H7{2iti-!hE)92xw+^EXc1yIu2pbEhtP&G4+V zgsME2n6D+wrIJvkU%gkJP`Bmh)-Ivy)o-3|ryX|PXyImS+O756{K+%Z?W{XKIojsg z!tVi@FqcX~m44O3Z}j(#yAqDOORwCoO+E6Isr9!S?m#+H`+ER~T(O{? z`uI(!HaKm#&mf^HkL~7b33I6=ROwgmmA|lHRFh|C303$0%Yyc$BR8Hp^n=5gp+scQ zI=21ohv)p)_ZYgqq2MJcM+e~eJ)^IkEP~|vZpSk&3!dxl|Rr*yAe}SZLjU(Y2XX=BC+8?}X zr_nc`G+e8>D)RSB?zrQacJ;YijsEI_#}D3fN~p@~DD$<1xl|IW^sD#EUxaagKkv{> zsJi&Ad$bLY+Gpzbzjw&sEr7F{vrnFJBvkdygO)It%95zkuih*7rv2@mQ|?^Q_S$Zf zsb{Smj=K`7+_SD8=Ie->M^On?`c)5i4g4jxcRp=lyXF^1jBdSaxGyT9D(^GcUi&;G z^spV*dicvvM*TU`mr(U@Pg&S5I`GKRf#(l*qtb6h<=rR=Rr9xA*q-qGBS#-SV@)2G zu&-3AvWIcIzp?eFtE1O@pSQ!*Lw66OR|!=)ZZ}^??L1OTsM4=KbAPqW6;fUmNvL}C zg$K1Q-~7*`{jV5CmhNBpn`f_jXhHkW-8P%u&w&L zLr34eeRwuXLREf`W4@L!mr6pFe)V4YtCyY-%csI5RK4JqW80;l+hFvH`?eWGmcH}w zH!~megN5z6x9l@|-;KlX<4CCTT}Z8m`C7tUDhXBkRS$oK)V;;LJ1C**ub(sBZd!7~ z=&}Qc@92G>?QfWV^pB_8J#YBb=oeQFV>k&_zUQv>FkefUOC_O7zv|&Lum|T}E1}9~ zQ$5^u_IGO!pSNdQ@2z`G{b2dW29dsms=QBazJ2>w67FB=SL)fgH9~pl0#kG@kAEC-MRlQf^y^j1ExC86r5uN~7q`q|$bU2@u6uB{%%>e79LDr5NS;d@~JR?5mf z7q*)pK5Vpf>F}Et5~_SZTs_R!l|^1nNT|}Udf2y}ee#SWp~^mAJ>0+Wjahz6CZWoG zjq2fA)|tUKG?`H48n+U@6ZB5!yDOo}cZJo%{R?|=?zIxC+}Efc_6gqz;L){r2qP_>oaGDJR3-;a@|(z;d?H3@$$}{ zgeu>ERS#Qm~Y>`lJH*XS3O*l_pJ>iTpPGnuO8-W@5((&LY02i!#S#N z9+Yq%%yX18XWy78;h31`TJyDpxl|HG71wI+S@q4e63(^mW!2~5?A%um3G0#PbMv)? zxl|IW^sDu-M>$jH*;ztW?t|vr7mG?5i|SYFVZMF$O2T`kU-d9w??SG*gev{2hjFw~ zO^#$FR2g4a5BC}R#$5@=-Mr6WzLqeTNwH~e}`r<1IFe2-#hlKUWF`W5Y z!dxl|Rr=L?WwhkVJFm7SROR@}GbMfXkgy(}HL3M5UrU%vC80{c>f!9{?r`1_mQa=F zbN6A4GIL}pq00T4S`YKJgt=4_s`RTK&d#oo@~TLpsN#HXzJ2kPgz;7JD{D3Hk0ms! zsWq2S<$bG!rRrO&Nw`+a^^h<+%MqD`s_fxN?VG0jc1c229@`z^`t|@M+ylsCq9a4! z{;q`kyLl{ejOyDLm2h9wv8%Q#TcYXQ5|b(u_6gIr{93B0iv8SDMLPCcOY-Zjhh?`; zeWfa@c&}!|!?K$$C5+EQmFe6{tdmbO6Gip^n|tLfnrBY$sH%Lg%-6JZy;n)qM>rDY z(MJze*~5HIOV@fNRUhG~-8WLJ${yxxTDsOFsrm?KxV~9URrWAn)6%sbN!3Rf4fI6- zs8d!d;5IGogp7>|ws9rE5Kss*iB@EAM#e zp(=ZruW9L8kEH4&`sP7BoCmXqGmhyz<0Mrdk?4A_k}7+6e@uH9sz+3H#SD=yu}@S* z4{ILj;vr#e^S$~_Ie%r%*VVl;U(?>{wQX0{!*qUvN!3U6)k6R|-f7xk&i^)O%S)JLe&uih&?O!pD0OlObTsD~?yzSV@PTo3c@yH^tCt6#lW z5~lkIRi?AYY}CUQYu_qWRj!Bm_T4K9^VP53D+$wmgeue7V>arMc3X0(+#|{LFyFp= zC1Jk$)lx~A?juy0&K|Q-kF<-IOXc2Mu7~;d-75+6)vuOH!gL>@%5?Uaje4Zr;an>B zbaOq-x9?s_n6G}dR1&892vw%D$86NYHv)Zc0aWFBm~Y>`k}zNW>b;UM-AAZ0ojqow z9%&VpOXd13*Ta1K?v;f3>Q_r8F&iFz>kmuiIx5%0e5Lz{qKfOm-xLo|q*y2KbZsRP zs`42Y^Of!+iYh)2&yMvya|xfhXUb~L%~!gQP^Dk>@HDQc{PW3v303+0uKD)G)Do@@ z^sDtSUrU%vC80{c>fw2FPZ;M@#S*GKhh9C*w{JZt;ks78>S4Z?FqcX~m44O3?=g5f zKA(t}Q04asNEl!Boe^GF--$C{OPEU~p~~;z)qCYh z^1d~Wglo0Fv*+vTyr}tF!dxl|Ri00+_saXTuD&;6zLqeTN;nXh%qzpx>pO26vi+R1MK z|3CKbH0rjZJommB6-=WdYCwEN?4uGO0cm`uC}G?8dSa89qammm1P?YKO(2NvQIIH& zzDgsAAh8z}SyK%$CMtmG5Rw6^t+btwMVV7eYRs{oaa-GHLri& zch#)suA24C+5hWTLX~T(v|+xk+xp5&LY4N?hWYAY_ew&Q_R@xJ*Z&Oc{|zXis<&P9 z9qS(w_7Cl4IeaVdSU;DrpZE6&o3AU@zDku)rM)bN`RZZ!N-B#{=Ij29 zJ38r=gew1!DGBqnOtGUACmTqT(Z@vBHDlwhgyXL3ovbU@1Fk0f3Q|Iq>x8sn{Aol!7KN)a zE=?Pb)MIg?gmGdY+ug+(+rN@<|Ell%*zd;neI(rXu|H;A`L6S^wku)V^&RMG!ze>~ zj8N6%68Cys-S-u}gev!dvK+oCeQdmvaJ=g8R`(vP*SAv4R}Z^a5~}<*OV*WdLmcbp z687`{{zS);^?K*B`RZZ!NfiZswY^^N%rsv;>|RNz@*SO7 zSHAUhZ2pjN{^;*Xb;en*Z(5kI9(J!JRQU~!tSjFZIu`p#82j}1iyAGh*LVBOR}Z^a z5~}=8;8I=v_QCUme)OTAeNkh*zRh92W8&h>mxU1TCQYBRN zZ>89`rN;GMget#dljZOWM`O>OC4BDOzlr4D!g_sY)O_`@ zdnKXD@0w;^`Na@-Tlx-*gsT3H5$AB}F+!DddzQm5{f#|KmGD_=|2Cj&C+RUlm20Xj zhvzcZ>l-8H>uA?UI0;pL(D&wFnUk-gYt%uz!Ri=~h%UNU3yd-?))xWi6oFF|$s4{-Ya=8D!Uf(}A-?954 z;XUws>sb!ZT&>slYt7e@sEgXVO2s^e+K#C+a5Xp?ezz*z2mTb z7e4LnPk1F%`CZd2hs0PpBrHd_;g?C>7wSc8x`}H%6#3M#=l( z7bHFX+fV#TsOsOG^aS>LedEx4^{{&-p~`P6W?i|5=niz>b(T=&-eTHtJlDhSm4qtC z`LyBs%+*U@u}G+L-Ig}I2kwOQ-46*>-kY@Hn!_EozN;po%C$(^@Y#*K6n$qxLY2>S z(#HG#(E!!@Av7@^9wVcIZmGpgy4 zjD#xVxwPT)2y436u7oO|Q=|?1gim7o(^m;q_La2ZI?AVV{YjjJs=ki$KD*n}cUUA; zdH=Hbp@AsM&PIimJaR6>>UT-q>SJ?vgdsM22A zFhcfuOMhY_p{mE!j^|_Lkgyz%^H~n_)x++UgevW&4SUqsa~uhuSFR1S9M+Fz>QRP-D(fw6=#{%)eaA~eRWFBx(OHkkBvf@9 zj@0f<_1!25RefxCgmZVe?+8n%>SLlKgL^=I7e_)>A4?pg+*R#6sS>IjyRu!`5>5A( z*i@acPnfplr&mqY3B77`+K@2cUv4?{T@S~+Qe~O4kVL42Xm0XqS z^vZmjhn>*(Zo_M?u07Sop2=TqmQ1b>Nd>Rv~+saRAYpr_Si_R zs@pJM)6!XvrWzxh;l^e)Ro#a9nwHLTG}RbkG%ywcsOmP%*R*t&qp8LScS2m_kjBuBt?@VYzRkvZjrlqqSO*KZi`_*^6w4tioFkjQsS&pU}Bb=}LjH3-z z-G(!c=|1B$)fmy}tgEK#Hmo1h)J%-1p{VZPSFSjkmsFKyWRO#5b% zPK2tyTcqZ@G7&k&`!eDR`Sv$#NT}-jB@&jrzX?G?mHRWjb{!2ox!#FTQ45{0w0heK zReIP7XENzds50FN%Q05F5@R-0neJZciO+63p~`yeg!jO7C-i-F!j|7{glecIN<=%Z z{an>pIkaJ)@8wWsi|K?m?DL&aWsB*AchK}0F;-WWc6A$Lgr1BM_K7iK^>XNy>0Y~f zWe@Izsdsz-uEuDmUnU?N^D$`?x&+9v(${DT`sw`6y z`rFc-7&}kh@|Es3r2ELwRAYpm^mDJxhHrT65N*KMG?lzovov%8fYRrcH-1(~82vse)HEOy)=Tv12 z_xJS5E9Psu6WZvf!nC1k%!aMRuWod&RE^oN9F7v*hAKTw!uI3S9)BMrJR{x-RsEE= z)?M9=5thSr_ez!jm)r?e_Qxd5*JpzMK1TFY-sYvMpV^l1evA>8!}M4=`j@lxN|iO8 zHni@OV}Bnb`v3fxm#Y3BJQCiIF~V|~9xI3M0O^D(YdQ(7d%nQm#|Tg0cS2P^L$7sD zO?q#-4fB=mge5mUM)F$*!H$6remv%yx_a+IgTUW-{Nw`Y!WPjhGmr&LB;UrYW zdMEZ1C0u9f;aE9*!+a-Hjd`WYRdKhW$}%P4n!~&1n-M#q%C$%*RJnuD303ZdB+;TU z-=N(IRs9=P+AxmxjW^wf`TEY(PN>peC-lT`Vs%259wuR2>-#c#L?)rCzg6`)1uvsOsM_(S|dpZ?W$-RCOErZj|9yg3^YBD$A6F`5J%v2BJ== z(q0nIOJj84 zq*oHEw3meWx`On}P@Pbvy(G-nU4#C`DG62DOB?P$jLm}*&V#-`v)fRmhn-MmEhM4u z&X&%iols?&k}$e6>hleDolxcTuOuAxjqZ)eJE6+CBnk61@-`yxgevVNVZN5uTIhr- z?ImHX?$ZU+o$wBh5vojg8>&oq!WqCdZYNZA8(y*G)^ysCP}T2;geupD-A1VD(>`sq zbhqJi&Pc~<+k92U??Z(2m{(&qRGIEJLRD+m7}qCCM$2hKLY3(;!kjwcilsl5F)vk? zsoRhqBUG7A8`g;`(_@4=C1I3mq}TT>B~AMd14)$Iv;VRX;nI)G{rM)B^`y8oFcfyuo zPMuKIZFt3AYdUQ-!rzl{U(8*#-fLBvZ}&<<)#^5!ce5PkYil<>Mwn9)=4ER)Jw}*Q z60Y^lx3AA796uaax(!v@OG4}R?M}26ggrQI*gN&g-#ekoKA(jCYQuCV8l8lrK~2Z} zp>@mQ?`fmWIPrVfXr6Q;t~isK4jU4t!}dB6VJ~cib$vHo3G+3b_d^?@8nRL8U!Yf8 z!iFX95wf1BGIH*<8>(8ndeYKqqp958>4YkC>VzuK+;&2h=}zdCyDejc&%8UKYRoG= zaYv`yP-Tnhgr2xx+6h&9*a09sPfrMC)^hwBfRI*-G(JMJw|wny%Vau zH%VySh|J%|h_PKYRql~@8}7=F5#Dp@Zo`tB9wUrPJE6*ZlZ4i-E92`VTxa-%&nJJK zQ00l6PN;Id(+O9pde{l`^^8s@RIP5qRdKhW$}%P4n!~&1Nykp8axKycRqh~kLRH^q zF!C}Y^K5kx1q{1C1H)ag78^bC#+r1y>~*D z_Bx>_{j|E}P^E`yL+h@iJonxSRhFp}s`RiEs;q@1^xYM9KM^mX$}*)5^L0h>qZ%#t?z^?%alaR*K_Zk zP}QSX=cTc_lCZ9vuhJ{68x?!Dz7wh}QxfKD6z)m+PN>pe66Wjr%#-q+P^G;j%-0p2 zXX`tmN_$C|uPaDT%6CGQ_L4APcMAGxbqQ74OB-$0_cVSdRJpIw3FmO<_D-lW;z&a4 zdSxwiLX|b0gwdT*pXbm!p~~l9NjT~oc^i>;LX~q#66R~&3Rj(`a^$CVgv(koyD(!W`lH0~Qp-K;vaI|z(>w8NQs`~DSgsMIpG*9}N zXpNfgwX4dfB&M^jB&<=>V}v<%Le=Uv#t3bgPOq$Y?|Gj;yy7U@_mCx2X)nEU)#$T= zzABP%&TuXnv!REbP^G;j^ml9>)rkH)NGVp%*6#0}P}OaC#dc*nZ8XB) zld%1mudB%!hW}2*1|8UW6^C+fZe?6Mm0=myHL`pStM})*kVeGnQ)C`cXAzLwbx* zWjf1Yz2EA|Pn&=1j9X4mxZw3`de{k7)wH#rFJEu>V$m{pLh7^J=b3M{8zp|y>c}8of}4+tq$W9-?}RGTV}$mSu!N?& zji%~^W0dJJLY3((N6Xids1vICc-3rJBi)87BaTj3TGL~M-gd$fb&ODDy4z5ty(BDQ zOOFw~wVSUhfA2QDgQmxbF&nB@x3MJ%Jy@ot57S1AKYQC&h#7VtR}) zrzFhF^cbPa^q39XLMK$|VJGy&^ceBWwV}VQKV~`Hp=-N%NwitUnWz)0`uf9KT>TD8 zJI<$9P1T7uIy<_i4GC42DG6swRnnbM#8|IwbGY|EZAhpxe&|H2g-&<}yN%T&{nZIe>-{jD<=7I0 z^`j@I)2l}KdnZ(_Zle=kF|EDys;N5hn)hx$|36=R+w|}koV9krD}QBv*nQ?{$NM~J z&EM08S0qe#LfZ7^5?XKRw9!=lo`nAXn(^Rel3TOYf(zmE~pNqEIxdJYvta zFFt9*e9K)=oUYw&pCucPeeeI_?(@rk__}GQYffD|;f&qqjSoF_`ua~#TOzLb$xY@T zUwi3vzmxY}*utOtrkl)-O{Pt%}FWlkuAj~Ok{NSFu&!@fZ)M@Q=r^Og`_P2MN*S>t}boG&^FWGP; zlm6g-YxCW1^{VMl4m@;;aD08h4-c46eesFY{`Xp2yy!az%+G(tiPOC=Ut1!)&$f&+ zPkP9F;4}YxI_391eZMS+t?x58z3Kd|f4+1&=9Fozw?w_V>fa8SPkYsg)8${=lvlP_ z%P~Fqq4RYgIDY!fN6x=Ay|NcQ_PuwR&v@K9(=CoUeXaK>Rd;>l4)cyrc-!>dFKmi1 zFNt^FY=?R0Q{FZm@}<+)(ktsq56!6)s=5tZxc2(}kWkh8hx47i*1nnLkWkfol&xLb zo|ZNwRP`QZOOzh#QL1dm>6N3XW3aue6RPZ+NjUCmZ)~Ji<*1!DY_HnuW4nYZds^DC zz1qT0d;f3D`#kl1(|PwiY!f|7l_OEw@E&;o9alP`%26o^dz7nGti+|kz(rL##KWM3Ty=%6N@8ADk^DS=i+G(eMI&~9- zs=xZ`Z_UR)?X}a_H!ep2jsT7f``v1r`M?X_H64HCQHw0OgsPjoBJ&zsI4dER@b^X|CZ+HG;peJ?uuf%Dhic^?tu9%*B&t) z{N&S@qCQ)Rt@41o{mWwaXI?gK-1>QY=lzgS)!UU(k$1D-K?zj{@3`&!j4Ljfo^k&2 zJ-2@Bwa4CnnsHDC6<*G^X-yS)C8Q1v^{{jK>9fA!jF?W*NIsPEQvuU!dMmMQz7 zQR>({DB(Q#*c*Ri@r@6Cbh_}e2QRG$&DY-5dz6GK?PWO}8OFvd3CF8GmN=dpS$yZ? zKUf_8=1Zqv{eulltwchV<9wFGk~=f>*+4>7uPbA9TYhit5~}{^hI`FV+UB*>R}ML6 zspR@@S`V}2P1Ombj7Fyo3G?l7iSv>xmhZoGkNKjzzw(CBXL;8^LY4DXdSzR3t+xF` zZZ$vSl^0I0JMrwL+Lcgc8%rCm=!^z>1R$a6cV2nN`Ek3yX?o!umq$2bbw`OlGDxV} zdiQTE-hKW@r>(#8sHJGh7}puD&uS8?jD54NoR?fdp7bjl<{y9fi0Na8@3=I7NT_nY zN*k^ko#FbdCZXzfXHWB~8%~(6z4`KfqHW0@+_xxjnsdlyUUwBZGu0+gRFg1aM?9BJYGM?Iqz1U{CF} zE1^nzX~T%cp6bld302$wa201e?k&-~=D5=9%IMBMXiw{2 zS&r_N{mz-Qw|4u7D(Upf=-w7?e@wz2C1FlU*xGI3&OS*PtJ??d!AUsxNtja-j?Uh- zJ|=pfZGDcYX~TA9TQZ%5J<2{|Itj0s?l!zKMyN7vPU)3bRwtxwF=@jqs}s_;n6%-Q z)d^|)O4{(s>V&j?K5ckqbwaw2S6*42knTOoE2|UIy`Ot!bwawwa9&xRkhXW_J@?8O zVJ~Xww9!=lo`hbt^yU&;H?6&HV~l7k({7^?{=T_hY29*|?lz=NZ!V$rmQEW@5mDNX43DarAE33~25~kCJS5}|ZBuu9budF^h zOPEd@URgcLkT9J#ykal%_c1~`39s0@{C$j&9wUB{HneWq=)8NSC$0dJun)SLaFvmS zBZDgzS29UBUb!>n?pG47mu!`;uaa<0CSguVxbCy{xlT;NwWa;hwP+HqUnR_GiQrkh zq)CpDHhAmus{eDQO>ixqOZXdL7W;rBO^&Vwwm$s*+4GC4f zN7)jk$9j}1+i`m3DC!t&@9Kmq`(_f3yV@HYsZ}{@rw!Yy_WIZ^p~{|?Hf*o9aG%h0 zLX{&?65a#vzvD_LR5>aoVUKe5>9d-IDtlMjuznnUeDc=`RXznw!k%M~@(E@qRQWVB z2}dUq!X$5dE&bh zs(M{HerRuuQ02IiUKxuTw|fG=6RJFopM>#_@m!C{Bvcs(r49RpW$Io@sIsr54d*^* zpFZPAsB%tB8{PvWjvi%5sPf*V4d*`NxgL>8sB%tB8?Fb8gL)(*p~`ha+OXfb^722Q zI-$z`n1o}AvrnIKBvkdW#66_3@k+w+%Ku8sau_GL z-PTuL5~^J9qzzli*gPoVJlNZnk%c2%kHRHX*^aXuwm5rjpK&BqIbWp>M>1QVt+ErU zdR;j>d)J(O(uRbp-ga#nt{VHSCZS4u>6NpvE685E5~{S9Hk<)mvGgchLY4PFZ5ScB zqH`72302xl!V$oc!Cue_RoY9!D0QsYO4wdE(Fb|o%GfAspRm1ZFUuie^rF2aydq)0 z>4YjhOv1U((Z?|+2_qI;Wp9bzHAkXeS4PG5L3>*F%5rqC?02ppdTX~wsgh2wjEZdG z_QxdbQ4;2qgst5c?(DNfls9JCgOhOXv)A@W-xCGkdq@~kFQ~7%m`aU*ZsWNR&X+zc6xU0&vIi(F%WAle9 z)8>>mRE^Dps!W?x+E6t%pQ|!$PH98cSX`pYv^k{>Rb%m$D%0kaHdKwpiK_yUPLzU@Hcx8-GW!jw5tJT+0tFP5oUk|Rn=3ISUyZYMMRkf|Jug_QC z16X~Z!5s+uUEj-CeZOS&JsEeS#`b+y-&?XJj_qIR)z}`{>icl^sIk31iLw2}&D*nF zePnR_&~}a`-VcdBib|-m-=q!untjlg-w9Rrv?S~~5`E^BP-Ra`8}=xP9sx+GvK^-l z$6ceB9xX|za^Ej)I8saWs82$byOC+bv0cJ;+zD0o$0XX*H`{S1R5=nQ(Z)gVO(#^j ztD1zfnmf(*u1=`3cO}uD>RJn(Q00@~BwSV7GHeT-P}Nu4?J0X786;HsBtE@rrv+@4 zolxb4d7@uDny;L8GX&A)(4MI!Uxs zLBVzs!q$SZ#%^A;kLX{`$ zl5mI4Im$V)6RJERm_$3p*k@-6Ri1QA8|^fv5l1Ifd15n(cB-^Lua!{c$jU>_L!5+OzsPgP^+OX%?ql}z8p-S6HwDHPOq7$n6Skg|98*!u!302M#NjT~| z8(0gSP}S?oR&8rHhEE$3s{9{;Bw9Hv(->h+V}ub$Csg&gr2UsjU$IE2a<)sa+JC$B z=v6|M?JaFM(;ES}>g$9m{}(6;M*v3#WB5*}@_(9=aQ1Rmb4}d|RmQ4Yk6Ct-^-@-`;wgsR>LZE=oj_JXt_p-OL) zFkeR^=_KqE66TbIUP*Vt@x#%l?=9KlY>9n!XPr2fcsH{g_9FXWkNWh=a&)h3)y}*2 z$Mi~-w0&iX;EOZ1aMuh;ctyg#l7uskE!>u$ggwHSZ_c4fM^EBlA(-apJ&!gTjal{t+O(qlxoVarfuI&Cz<-;=Oqs4_i9m{St=300=Y z2y;rpzOBmi7-3FHI9{nTJw}*Q680!nrpE|#O2U4w%JdjvPDvQ6sWLrAm{St=HC3j^ z2y;rpzSh#4OK82N(?(PIdnd-qVO|pc-fgIIm!cD@Om||;D^;fTu-i~&dUFXqY3a1l zRQ{fX{@N$}JqcB&#|ZnjblOm5dW>+4l1>|{Opg(cwbE%rmFY3UIYT;as4_i9IQL1X z4OOPc2^Ty>wM(e7r=<;hltgcd5~^&+X~SORSldTY301uhI#NsYs82$bqjr|Vv0cJ;+zC~^ zNBJbi7H&IE8xpDtA)(6Nm4r|D9DS^}PN?$9UlKm4 zw0CWyN9}+2`SRPJHN9|$(-*3If|)j4joL~a`8uJ>C$UNRC*ZaW zdvhmL`D8o^pXA%FY(1S&o9sPbgh z67jr^i-XR)Y&v`YhwWqZVqfWmDtlTIo@g`bcYN%GDo58OYz?-OJ{w4=^2A=+a3pht z>yeCvDo-A!4NqV?J3G#ILX{&%C$wRDjL_R8Jh^JjV4v@VDo2bYw65>Q^PN!T$=oDt z4Ym>^j!vlZgmDu7*OsxCtG-UC@}zVUp6oW#vlcp`s@IiQBpg@LhJ-55rzYV^d*gPa z%ucBC1bz}mUPffbL7h-#6qSV5!>dF0n|HnV_-UsdAGYX(J<6VzgfojXj&ousR5_z2 zVKiZs;l1gED(`F(MqWl_&WW8+<&2ty5sQ(G>x53Ia+Q&U|Ka2+%>LL3Rrcm2oLQW4 z93?uTs*fdYymHSeZAhr{Khcu#Kj+->Fn;KSDx;4i9FZNVol82Q%9$t$OK8bm?{q?y ztED7djk-#;jdenmEi?&Nimo7yI69%K$0bIoMz7xgv>~C&){}%QELUExcRHcUwNDbx zYR-eUm`uQ9_loUD~ki+WPtkC!wm>m2;GL%`rc{ zl2Fy#uCte`M%!E3kWi)VB&v3^fqnS;_UN1dX-Ss`=Bk(G1y*^UP-9Zb`s`m^dg;veL}*VlF%#ZPB?xz z`Z%Me4O^Tou}3o2iDQX(Gi}(5?1RSIN$8d3=w8{XT|w9%(}pT(`$`h_J6m{l5n*2{yWYm;kau%w#6i2OEleEqCLuV z?@{I}VY+*z%ACds=`o_)uq~-Foi-Zb?@8E}RGA(l%qadrl0ZB zZRQJiIe&Wi3(i_g8%@;-_b*JpDI44a}#5ts@^}$S9@cGD(&SR z)Q0IXLY3)mV@s67i1kI!+H1bkJ&vBf|ECXJx*wKSRWFD6N{wk1`Ht0o}Yag6`^|2e4?uVsS)yrYN(qn`w?PXnQ!}J)T z%5=A}CCcGW^O0*;F7|rMN2bqheeB-dD^2=cIBjXICZX!?C$G&nzu)20r@y!T5@Eh$F|~wi1MOuw%vTS) zR}!kUmo|K|?i2SrKW5K)`@j3M>A)lQT)Kl2s@`?)J?Fi)K5n{pyM2}j^Br3cO1Q4o zUY5gr^{{&-p-Ovc!*dIsXL$7|cb;$ah}TRzU32PE$t6@h`A>JAUw-Cmrmz3>v?aoP z$M!NL+>_B>mcxAYuzMw;N_%O;Qz2t9gM_iqNniWd#aI5~1Jj2dwf7=zn6IZ(9)Iq& zi6{z>S1xUsucw~&eaKad5B~3Yy5{O9Eye8; zsI+y|iIDJWY4`ZErU}>99Ba?Ekqtp{kd|eD$z6K>$ z$MzW{+%I|IWp|oCx!Wnz)kmJbbkEJ#vx=8rf2aA*KYh)#_POPIE}=?$Sq}5n!|s)Y zD($5WPhd)q5voqU^?l}d|H;dy0}niO6LqD%F+x?ZE6<*KCiQlo++#lAUB^xP-)n8j zD+yJP`RhIAtro{k_r83Ya1U|JhJ-fUBhCAvSJGpIs$LFH_KwB762`mFfBN3@SC4qn z^qSxM^!>6N=4+JMBTETY+DjYetB2hy302xl8?K!^PyMKOU%NPZ?+;AJoU**~l2GNE zDs7mr>$bk~l2E0+v|+w_*u9carMGH2F_b3Tfz3rOsSpSf) ze`qhu;d%VAelB4@-|jy9%-`Jp$mz2mIsekMVZN?d`zlpJmG;tx`RZZ!NUHG^H)caZ8+}Z4T{AZBN;vMi-pTvndcf6WUqMQ!a-EPij6aRY$D(jm#-(Y)k$Nmn zlrT>0W4pUJWBXSU?qBtNAN$?dzK?|aKK93~EB~`}tnEtJcE51z>lYt?_dBQa?s?eK zNNtoMJw~YNafy39uI~GaUP6_7Kv`G*$MRSol&}x}=h{_^eI9f1bj?A_QJ*7&|L=VC zLqEUxqto9%?Re+qv(6H#`dDJVdf2^^P^G=BD^IA~@_TESQ1z*8Z$H1`k58I*`ln6p z)_QLKjQ8v?-{Rtvrmt^Y-mSG4sj{zRIm}lNyH^sbw3jyayVW-Hffu}MI{wI`7GvYC zgyZh6AOH5^+dsbOXE~(}M{3^(@TDVeG#`D^dDDhBFYhx*sOn?8`RZZ!N`^Ymw)oy>2^nL>e*fUZ-w;A7|#D==rNr6>S6avLY4N?E8loAHnvMR zwtwtSTg`j?_}uA`E0^Od$8+D8@%!g|WATW;d;4_W9hYYV3002sSq}5n!|s)YD($5W z-#{|9#*uK1^SI~jKL5vt=TD!#VtK9Rs>t`3d~Duh{_tNMIz9Wzr*Gn^rK-M;GG9IH zUP-9ZUe=XwgmJgN@6b!A`tBz`zL+nar;S@bZ|Nz3vzoI{pK&Bqjm?94*uBydRoctC za&Ovq@9g)j8_nOl$9dB;&R-sPB~-a*oi@zZ5w(w^5~{S9HrzGvO>B?fc9;3FS06p? zzsK^PjD)Jb&tQ8Uvmv1k+i{k|H$NHm_eftt)wgc8%l!CnA3a@u-Ezs>yQ2DTl!U5R z{pv3BZ(Muybnh!SWkV0gyi(O|7`OY5t;_ejYO&R$FP@Hh*>dzMp{mF2=If~4M`{UG z+RJYHShY7bEofKu_;28`x;pe z^VP%dm4qtor48RA=L)H>iX>G1`e9#O9CYEu(}$k4d=9|p*}jABA9ufMam)X9@pQk} zFP{UDP~~&(EQk5(VfRWxmG;tx|B>s7i+;*NLe*_Pb^G~lm%Vzr^~;z4Ys*tFz7z0+ z@7#61&l8TAzW?;)|0|ME)z8V8uO4=&mw*jy+?M@EOZ{FW7DV#(6K8UbE}+ zvm&3$_>RV>pT5g{x9=Y{9r&E(vr!VN`g0uf)x++UgevW2UBz23pYiDVMgRW7=~FlS zzfwOHCZX!YS6av zLY4N?hHoKtZ?W$VN~n78&3Bj|^Wc-GtB+lNM(^`%-(h<5-`ion;I=1EYgaADa1yF~ z&Yk5jUp?$zNvP6Z+VCFOgL|))Q02Wz8}2&$-rAqM^^=PuZn>B)y6nMACxj$a^?hpd z9oxT>aQ{ksSq}3x;^&HkRh>nG?WRDJvQH|$J&<-F;2CoccTp?zC= zj8J8t&vN)o(C-^;^VD0+UwO$H(+lsoJUdIM@>yZpFkg2<`tFB>D($5WpF#O8i1%Lk z$;JJzzjWIAE00>*@sd#Gv#GRUzRqxcR+CVry|m#zzAL1@Dw0s;{(joL<1& zR2_8X9`kLVb?Edrhwr!)?;5L1j}fYj;jN7JW4#p-Ovc!@lk8(`OtBRrdL`;r@kB%=%L@303ZEqz%`y&J2AvkWl3sH*NS# z(3?>)*xzE*0U$IE2a!yPeK1&wr301vanXl`F9_dS{ z(q5Lse8>8Sg#AN%X~Xr|SlljQ-0r%q+ZgME+OQ9H8|LfkzOU#dRB12EVZLK^C1G7@ zFKxIcA6pwpxHfRDo;J+a-qm}QgevW&4QHIOc~HW6u+LG>oMU66gkxf#Yt2^=yH^sb zw3l_|p4He~E8$%0URK(0b{;E-gyrb-x%uj0_ew&Q_OcxIC}-+EJ4>kQeb9WzVo?cW zQSD_p%y+D=B&;j#r49477JA7gRB1157)Kk`^hicRmGO1jaGzmp+?8QuGM-uB#h2_L?)rC+i;}z zNmGBiB%!L0?T&C`djJyd0rWA^kzs6qSHk_>K9)E}jqQs{xG(D1mF>!wXu7w=rs{-! z!n7?vy=tmX=vAZBhJ^Y4a?7FbdN}5lD$A55-x4Nd>Rv~-rEsm2JSfw2fcRkvZjrlqqSO*KZ0?Ez@RJ%Db*d`(Md zIhtyWaF?R*OlU(@w_(1frL!DOHAcAm)pxwKp{m<3U(?cAj;0zToUi(fqYYKvhBJ=o zKI1gi7}4mgtETEUtRK_XLfUAmF{06%Yr~R{*^topURR!`8nYpxjbHBhD@#6B4hhTA z>&kphOaCJ8xgK_}nre&~D~C2LZMR{*rlmKxuFTi;|F!Y+UrX~2n(lY7sm6##=RI$# z|N1s;ug=tcc9u}pTf6y6j}fZ0mv>MbrpE|Xrn`+TQ4S-(v8Yc~FNgV#)s=+#YA@?b z!t@xS%5=A}CCcH-Vr(^`s+Yrj$LdPLe6^Q#C1HAuP-VK?*b?P%#X7c1Rn^O3zGHPI zVZPeSx{@$GMyN8~ZET5hwB45Om3t(;9OgS#R}$u{z4S`L^cbPabhoi3%F%Z5x>xSK z^>Uc+SY1h&ulCX_3DaYQD%0J@mMBNt9qwMar`yY6zGHPIVZPc+uOv*55vojg8(X3r zJ`osu3ZSZ&!+gi;O2T}#mvtp!dW=wIy4%y-%s{SsOsl;&37!OmT+yLy)1|M>S6avLY4N?hUd*aVcbs@OQ`Z3 zdfG7GvGt&Y>ssxl4fEB*?v;cp?WGO>kHORN{Y1QkD*wMBZJ6)aUWSBwGTKWU=BtO@ zD+yKFOB?=2)mY3RVeB*ZKUnK^R?U3%uzMw;%Cm7y%?v;cp|4$>!;eT3;?K4QYUo!T;8S8Zp*?je|dnKXDbI(~<{x6aA7@^Al zF-jZW1MQ6ws=PO8!}I3;S8M-IRtZ&}Lr)v-A&%LQ(1v@YX+y81#|Txu99!aD7RJ#= znLVmxLmWpYQS6RNbAg!!)5|9P3Owa`m0q00X& z%X0X?cG6>nD*p#BZCF>@8zWSCZ_0W4pUJWBXSU?qBtNAN$?dzK?|aKK93~E8lfK z)^;UqyS@WGZ5U-pj}fYRT;g7jtNXs9mr&&%P?lp$yj$ISuwLIvF<;Bn-}o+}%5Sq| zUHLY|v3@RLKkx5PbSzn~cP^W+9(J!JRQZnQtSe8%8$a|2KtfeNL+?DeUf(M+Up?$z zNvQJsNLg3D(Rys3K_d1U>X)sZGuG=}tmdnS{mtAGs(klr*438y&X23@^?GNf`C6v_ z^&tsWzN0hi%D0}5%^woZAN@V4&N%D!O$+nY!|s)YD!-wTb>-Va$6_A|W1s$hQKO~x z`fi{3>S6avLY3bM%)0VP+S6av zLY3b&U8<|+ZCo65-euF-`#)?ScU$@n%j(}4aSoU6UnP-H<=md-@JoMV&r&6PmfF7! z=-Nqoj8Ns8D$C)yjP?4)i1|9&^$|`&mESbUa=06{Uf%{ZUso1=H6fwOZxLoW{1T=+ zo_&{7LRJ5ErtyyS7@^8IXv>#F-%aab_ez!NB>ZyL*fTE)pLz9fZ5by>j}fYjAF>?o zf3Mf~&&_x2en@x^{N8$&!!uXw_5E7&btLMekAy0}mz(AAiK1Uy?O#}xP}QG7zUVs# z%+G(tiPOC=Ut9RJw?E;PP~~?`vm6p*<&dx(-G*N#bzi*iY)h!>-!io&N{z4R4}geuo-`xJl>9aSam%KXrf%Dhi zctv}j_O|h-BY)ppLv&yrVVd8ZQ&FC{#0K=)tS3(Gyk6>&i`3X zSq|5L(qn`w*M@1sxXq}hM=}zsjOWsZ&m*kqUb_;id`^)z>=QnT?N47NRM}V3hU+Mw z%JnC45~}(-%KPkYOW$FUQ04v4a=54KZhhaOmr&(iZ`!bxjKw|@#y-7WIcJQ;?GncA z&Lvq6^BwCS67~=6r493SXR7Z;NvP6Z+Hf2+;^$+~$bd zM^On?#&c=IeD$ziQ#+oIl|#aEIL>D|%vTS)R}!kUmp1HC zW6yCUe2&xmp!vE&>Z>9NRocsPnD1C!Nmy6fOB?2EE%cI0sM22AaL;mVFI>XC@V;m1 znqzF-m2lj3Et2IhUp?$zNvP6Z+VGi&&vg1z83|QB3rQR9UyPMQ!g9E;kv7a%54%?q zslIVGXK9q#BTETYeb3T- zEmMy&BvffH>&n@Az1C{xI~HF_7+<+I%yL*imZ?V>5~{4Xw4qn-e)SzM301uu5=Lh| zB9l zWlJ>OTVhjn!aiZzmY-fVRVVbS(P=}%e1Ey+(04r?^GcOv%93vh8~Scq57Vor>V)Mm zJyvp6rqe6)Z60<)-@6U-br$V2r-Ul)rB~)_TDlXeOn1VOsE|4U(?d*Ra1=- zj@n}*wW@Bzd`(MdIhtyWaE2S3)l_vG=4)Cy%h6P0gweoQ1fZ(hFkjQsS&pU}BgXas zwBa5=w_(1frL!DOHAc8g(RU`ap{m<3U(?cAj;0zT-2Li1UfNLAZJ4iV=`2T6jS$8?`@nre(_bkEG}Rc<=*_iZ$;WI+=zFg#|L4bafBRlj zb;AFTGTr~1ys5^B=3(9sZRmTq;r|ku_CJQwMpJb{-y5AaB+S<`C1JkS!dS^wX)kTq z`b_&Dz@1QKdc6oe9JA5iuwh=R`hJOorR{G*kWl6ROfQF{fhX5Hp-K-sVQKZY6RPyE z6V7DPols@E6P9DFb|uDas50HX(i5NEc0!f))(P){=}zeT>cm*PQf0b(rOI~P2~}g| z(1v}!+fZeT>4Y}y^PNy-i`fM6(rxAocR7E0_zTWj^KOn2V|8U|SGO@n=*bvipBN)n zFNa>4?zO8|_TWya8oTF~qZ771+pcXay=sKNcS4ovF+vZ=2>W~|RGIFCQCN?bETO8g z+SP_Rb+1&J9wWv|ZoZ~}kv6=~tJ@eOEbSPvdH2II^}5n4)1A;O+gK-5jkQF5?}X#4 zbRQX-YK+j6e(tr|@C}cfD974Wi+vt*@pR2W)0*}=p(p*n z2lPsnEoRJy|KZpPRb#cQ%C{DF8>)IaY}M9+_R@xgsy+`&7`>YAHq6)gsuQZlY}n78 zueuFYdYFVYYPvt?RAmeI_in>{O?N^Y{ZyFNRgKxOmH5?-?v<)B8PL$UaIc!9wWTx(%madZhDL`F71RW z?@ba~x2}w@lW>*d$^O1WFQKaM!%3)ez0p7>3y zPN>quB#di)Uq+9}Bvkdci%F>R-gK{2^*6U@LzQJp8`it)DBqyn302xg-haZD(ZPtlJ4y{W~VwaOU(a_T7f6 zZbRRVGW<$V+K^CXnUXMH<4@l})CpDEOTu|+tga-iE9a}Uq3=e;zFD^usw`6y=4%x0 zmsUEVN_$DPeEXN0Bvkd)omX7Z`DKsvNaAq z_R@y=x@*wCI3=M-PzJvv=gc`sqbhlx?rpE~FjS=f*Lr+Y1 zuT*ufjB$OUWVD<%BvhFmBh0B2u2}k08S_$QnYs<>F+!E;v|*j7GCf9^QxZn0MtXhE zQbJYVRg+NFqwwZQj}sjYoMjwWvg8uZebSw3%<&suQ+2Tf3{5EQj^({V)Jw?>gK& z*n6#nt5ol1mRv%W_L6YybEG!i3GcHxbwX9Q;T3zW>9o-Ze^0`FF?ZE^uT^Ef-75)I ztJ`qi&2pHpt=;q(VNOYym#y9O7-3FHxYjq{zCM?5{BT_9HdJXZ39Z|=JJD7U_TaQ( z@6;=Q?}RG*d=mPr4bz=ybP|pREuDncEr-7+(PkWfPojC!39mFdZAh4wUN1s>X+!HR z-3jwGJw|A+6RK9XF-B|&8+y_sWIa)3D$m?@LY3)F z5S*cRw`Gj*d2}aKjd`Uf?&x$Is%$Zx&=dDdJE2MsJE14;Gj>9i9wuR{9&05M=Id$v zZo^pA^cdm!^GcN`Rk{sTjuJ^&qo#YzplbDgZoa0wSE`JnI-yDrJE09vCv`%V zZLAY|;waGxRo#Yd)b)qIrwy-2^worfD);2N4fn;y2)#1hZK(3RcqdfZACoX&S3>?i zM)cL4d8u-p*ll<}#t6${y4z6YDfUjNvZj-Wd>^>${KD;yoOasrVGDmBBis+_gerIR zI^q2oBV5ax?lx3;ioFx6tm!1o*R`6zj}fkMJE6**uugbC`p%vHsv5Il$xU~!RC$WM z6RNy7Ntmx|S$`iR+}G%YDtARX;l9Qg;XRk`HY~a6F~U>qolxbyNkZ#m`wWfn*-N*f z%4aW~a9@0k@SaO|8G1 z%F}e+hAPXHgf;34!e?Qfuy#H7-U(IO>x7>4)9RK(l^&)It-Fr$+9p^it%DE&7=WWl^dy2gis``Ac4QEcz zp?4dq^e_pn8)bO5z7wh}QxYv-&%Jj-RgYesm&WQ!!n$(4O0TqTRP5RMPN=d>Ntmxu zxF_X1p-Ov6n6K+IPs(>fmG+V_UsrUVl<$Np?IjWUayH+S@|{qny(CL|kt4pZT zUfO80zNhg!p~`)YPB@1B{7|KC1H)49wW@D6RK9XF-B;^ zbb4jId(Zp);T1>GzK1NKN_***t45y{^i`3BbB1%tm<>JbgevVNp}%A6s7CbXQ4*@G zxAe;R(^$Rls!6DFPp;cA#_dG&q(}O$`&^&(elDTPddqT1cn3StJaILdHkv2hhJ@)( z*w-AXO{Z59wswE-gsN`CE4C}sX`>PTo`mhkd|gc@;T6|T-G+oJ?WK*@C;Hefq00Ts zwBal~Ru28Ora7X>z^;3jyYvo+vyD(7th~#$#l%i zrcIPX)mX_@ee;pq&ToC|1=Ar{p1uhis{A&6-Vf`frN;=r*1leZEvDO0Wx5l7kN)J1 zJI;4H_^jy`M=aN_^`mOchV&Sr%5;{)dVk2vww-_cf(xeG9d*W<9(F>Nwa|&XK5~b7 z$0xjP`tBD_Upw%1+s6w#nIEDJDhgO zx2`*2e#^B-Ob0*t^h=Jt|1|ITh7+d!_gGt`4GC2b_~8NbsV_cp+W%h5#G@CFn7?z% z(bGSCZ~IFge(Z+%xLck$9eBi^YcK!UG~ev*C*1H}oIRI%t%RzRer3b_;}0J(eeCcZ zFG;T)4IXp)#>GX~U-GlB75IB6R7sB!y(OBjD%0JDD$`?xwv+HXlYW2FuU96ab=#7^ zcS4ovF+zJuSVGg?MpJddG0OB9p~`fYqvh*J)CpC6ylOVAk#0kk5l1I1t?4mBZ#&_L zI!356-EFASUJ{nDrN@Zg+Ray$zjqtnLDOTzm9%jk4ZhCAsP$k`M81EV_ zcS2R~gIZT*{LpQvGJZ(H)@PsSqmP6t$F6R}XvtWs6RPyE6PDIm=!B|n!-&O5#yBW# zNT@O>m znre)Y?ybG4{QVc{mA?OSZRp8ZD{14oBY4)8gevEfB<$Oc4EFy{sOlp_E3Gw>HoW2} z>KNP!Rn|f$Y#BWQ&?{BlMjM?S!P6@VRn81aI9uB9oKZWWs?W9BP-QK28>+17B)nod z{Jj&ZwATq$rjyVU>&l3u6RM0jIx*JT^~%<+hiRk5pS|s_PH5d4HJx5<3BnSZ?p`(3 zmLRm9b>$U3F+E0@QxfK7dW=wIdd!Axp%bd~uoHS>dW`tx+R)$DAF~|p(6wE>B-$+F zOw_hjB1l;zFP~OP-RVbVyxG;Io$i7HY8LTKXjtiLMOa~-Nx#X z{_2FK^?sPna%>60`q2~9=~W~Ay%VZdx6ui&nATo;)l{AM*X_2NU-Rhm=EuI^thKGK z{QTl0Z+`!LkNZ4m&EM08S0qe#LfZ7^5?XKRw9!=lo`n8BVcQ+&m;L>#=j~s8=3re( zm`)pB`H%1KI)D4wN6cFt^1wBJA0wob@XF^tzSsQBe>!-+{{e5j#NWpV=_I`J%9rgr z?{%*u=k2zA*uvk(2t~+l&>A15O z{ys)XC*hTQ{Ku7xr{DMk^G_bLVd3v%gme;KIrWKuySVe;&hy%Bj@{ee#|Y^pykal% z_c1~`39r~6{e6s(PQokZ>+fTPbP`@MUw8GE))%=amojc#*xHFbca%kQ3 zi$8JI;thXr@%+8RmQRJ*pKo`QJ?B?kc|D2QNyIphY+LwN`!+h9B zPny5+)6DEKUFPln{pk6=H=HB-%wu+$ zKmF6A=bbKJK2zjRcva`F7~8_p;8mZ(?1 z`^X!-y4f-FN58fyuWYZDCoiJxZ1BIK6Tdbquz5bwZVWGYQ9C?TwAp zsvNb`hV4~*eQcLdWlu{RwpUyDmv{NM#XGKe`~1#(9=0@pNT_loN*mq-@4w?pCsa8q zC1HC~nBA)#vPyX`z*aFT>d z?9ctbfvwNB`@65*X8!cE&z}$fxij<Kzf_g(Y5?zr6AZE?w}1JWOZ{9z)jjUK*ZlO|kD0gn z;|DGg&ca5;ciD08d8ZSOnlIdSIYO3D^^Z5-bAIw?kDCwsxpVr?OU_p}{y+Dhzw*{Y z=Y3Cn(^9XMQ1$-z-gmz6JIBo1-SGb9Y`d-?jF9en%$?@zE{`6H}T-@%@K0N>R-#mC}J!ro6uHK_0 zRB12E;m9yHUP(A!^|8e9+{ohi$9{cr#zB|NKY7ZArB))L%5gr+Vac5t`fMPfs@IjV zx-GxAb_rEiymIIHxKF)i{=y;WER|f}P3vKnys0{2l+oz4Az{8fE^%IR#WG!f@AIk#C((I`QnK+Lcgc8%rCm=!^z>1R$a6s88Q^ ze$l_bVSer%mq$2bbw`OlGDxU;-=$w%Tz&9|=l^ltqn4s2V_avrKC4NnGWN~7a$a%; z`Ognon;*B~dGkewE&rdFgevE&wBf4J8LrQ25~}{_>wC0Y}Ms$93F4dXUjesAp(s*LB- zhV9kWOJA`_sIncW4aY&FnjXnWsB(Nv8?H0Q?zx2b-1TSLFm7|S>m!_mD&x7d;ab+H zzeoBKs$Aoy4M%-jhApNOs(M^v%P?9p8b})ws(LJHr02@3ms~=Xb9;K_DC6kUM+OO1 zwz0I~2;j(IMBWKi+DpP2z@FM`S3;Hc(uNU>J=K|^6RN!bN!WAjQGLddP^G=J;f{y1 zzWt^XsDj(4N-4vK-wj`<*jqZ|(LERnqB|(Y-C){+NV4O2V9yu(jL5 zoqdupR<{q@gOhOXlQ5?w9G$&ueN6N|+xi?)(}wNJwq!a9dz5{`bP`@M-EDYfj8J9T zoYE_=tWHSVV$z0JRwtxwF=@jqs}s`pm9*iN)d^|)eA@8J>V$M3ue`E4A>DhFS5_ya zdq4Ne>V$NU;k>dsA#LxCGjyZd!ZY#u(97rrky({C#u1 z(z@j^-EByl-dsZKEuA)+%HNaF-?7%-25mDNWd3DarAE31#95~kCJ zS5}`5Buu9budF_+NtjL>URiy1mN1<*ys~q-yJsS~QY4O_VO`u&hl)%%An z+&*aE%yLMm>OIQVE^SXs8xpE|kFq67kM$^3w&V25QPeTm-qi_J_RS<5ceOV*Qmb;* zP8+sY?e(!;LX|x&ZP;FI;Xa}1gepg(B)kXSf5(+hsB%s(cEVggwU|7=ybtZ%ID$*NAM@|0B) z_6egGPl$Cwm3<`%$3dfePr7wNmE&U)j{3H--by4?d3rBx7_m4$_7P4(m8THXhJD+) z-4m3ZP-UM_!hB6jcS4ovB<$P944!E1gev=d5=P!*<&e;KPwA!&qYPV2ZzU3{JZ+pd zT*VnJc~ZI)sysEFge5o9^Tc;2RQ0-Y{LtPQp~`V3y)qUxZubO!CscVFKMCU<sK7GcKQ01JMHoON$96idAQ02Wz8_s>kb3G!HQ01JM zHe3%F2lYrsLY3=;v|+z<<>h}qbwZW>F$u>KXP-XfNT}*#iF-(6xE_T|sInbr zIc#zE+CJk*sB*qa8;)eQK3iocRQ0-YboQ<}`=kvCRlV)nGF&zGSxrKf_R=e7VONm7 zb|qA4FKsvjxMJy1xP&V2f7&oYaz*DVtP`rVmxMEbBZIx56RNbAgi-2Pua&U9_C9C} z9~(s_Y_HnOa!44xXfFw`NZ4;Wp-K;vaPD*Tag0gAh{aafTcUT(k*L>|QL%l{p4Ppx z9NjDXohyjm+U-%Qq|+;-B3roqF$sH=ggGT)Yqy0v`y^p}ZXdJ%Z@XG3h zv~4VHcx81$+TN8mys|nW?I@8pys|nW-A6dDtWHSx9_5wQ3F+R?y|OwX-A5m>mRE>?hs!W?x+E6t%f2cBTPH98c*gUApv^k{>Rb%tHD%0kaHdKwpC8|uDQ`%59 z7GJ3{ZBA)J)mWUU%CtG94OR9ce@{Y{=`q4yB%L-?neK#F#t2oW%_+TFeI2#>T5a|9 z;OcA6)z`JFubo|0+xq(YeDytm)%O|Pfw158y^PiOOIF{LaW`sg-)Hr`C0pXy{*_*h z?UAj%4`+`W+v}4U+fUrQJ!{ULX{^3lW3C%9D<1qn*Yy;^>4b zPi!X9PL=lOwGyg4d73t?MQhhtv=geVktEtY*rQhoRh}JA8}=M~l#z2MRB1bjHeNYO zbV5}hOWNsiBaXBop~_hz2}gZr18boZs(M}7s%`DY@M%LrmH#7FjdLsZ=eVtI{{{ki92;j(I4BrV={!dd9 z&R))HuBkhrD&pWf-H5;T-(c?UqzwsG&Rt13dpWDQvh0K^|5q*vd#AnD{eVuWvb`nY z%ET2*U!_W@GKxwY_D*}PF-j*?8Q~;hEn2&-+d84jRbLXeYFoSO-A<@78cf2Ki7OU& zMLMC%Hl2hKi;;}GJDpJFSeZn-=k7ChLY1r6Bpk^c;rfo3geqgZv|&VVq~CWSB~-aW zP8+r>TSlJ^Bvkdd#0bDx%l4LDNvP_vs3mk2X8f2oBvkd7!4bfbsEl=ao*V(smNgANJH9$w;WOEu;-+7JFCkQ4*@Ooi=RY&id|(bV8N3 zld#1Zc^eaTLRIgBwm3&MdqLWeP^Gs?n6D#|bQ1On33Ez9ucSNS_~Gc&_m*sNw#2@= zvrZgKyqj4Ldy##xM}2x_Il5Q2YUf@1V|t}Z+P<t344?((_@4= zC1F2TWqOP-rzDKkRGA(l%qa=`nkv&{ggGT)Uu)^jCA8ksX``w9y%S^QFfR#z?>1Dq zOVJ5craLj_l`7ME*lnmXy}5**v~=2NDt}Kxf9(_go`fpXV}yNMI&G*jJw`Z2Nv91} zrpE}!TIsZ*%JdlFoFSbyRGA(locpBHhAPuzgmbub+E8VBj4&3FP8+IBj}i7F>9nEB z^cZ0;l1>|{On1U7V}vTx=9FF;ZA+L=!ie5{O()^1!PP|XQLao}vGjiKF4x%JlItbg zlIyE1hgT$A!zJOm&$i?`F$vd}_9EA!Nq9xVeWoN_BRd+nrcT0jxGL!+TL9jPUH)F+|JQ9H}w*e+o^?u4q|qkIx$3%4Do4GC3_L`gV)cn7^VolwFRsJxmW5YT#k&6nT(ocF46?@{jU7>@9P zF~_d!Kd*bvXZFurYuURh;Tt|jAL}g>s(kZT3Ex!OyK;|`P~{uU>fvtGR^rH)301y{ zt%PrmojL8znNZ~$~?^+@gs=QfM32%bAda(cBN-6WI|Q0EB{5paiw}l zsB%=Qgg5P7x4X*Bgeq^~SHhK-D>BzXnNZ~_suH>zm3<`>s_bc%aAtAFaZb#HDreM6 zxSDX4;j_tvDxYj6TzR=7b56{JDreM6xMC?QnQw10eCgq*4L4tQ;v^I9CfsFI!cRE4 z3$s6FLY2L_63#5nIF1sTP?g7$G+ue;R6QhA`AM`&q|ZLOe#nF>S09ydM0TWhF3E%{ zXQB~t!A`@fy;;u@X0Zq7 zmMEdh*{*unc5Qumgp*K}>&iLGr{hyGGkv^^j1dcO|Sv_n&!omQba4 z^>DTA?!|c}6RKdj!)2!vN7ug40YgfXk zEJu#YR_zYL{#ZR!N!wQ{VZXD5=hc$`B4J;t9?m$na9e&Q?5XzPyl(ejB%BkghvTc` zt}{a=94&qRj`Njp+_fFsVk%)vG@V9-fPtcK?|PRi-Q9 z&eXJinNVfA61tn#FB7UvS3>umhxKmyZp$_wHoEQf;n}bM@aW_&sWRdDh3UE1Y(31q z=&a${2aY|GDicO!yL#@UZkg@0$Ex8WJ3etOg6qS ze=^+q#t#fzzvn}18533I{$alQ^$1n^)hDQj=^mlVboN+`a=2o>{{tRB{AAxFhxJ~) zZ1g-Vt*TrO^Of!qs`RUMrHAPrp~`gjSd4PGOIh(r?z#C>XE`cWxg6%(t1Aif)vrb+VY)}C zGMzmZqa2<_eC^#k4CmkT*x`y-eq{7KEUl_s4)c}n5vug7b)|>t9-+!~_E?N^cmlQ0 zd+$Dc@%S@__a3og^gJxBs$359mF^L$^s9BHhv^=n%5?TvjBq-yPJwlb~?6DZ-@TB=i=lpPX;z9p9Ot(0EmmHOQ|$ZFx?|mna&=IQI50=%Tc*M%jGcNUR_C;uYNTuiN)~f?LUmleN--o`AYW) zRr=Ms`eX6%J)mXEC6`e3&Pz5Q_Ic~+!+no9X_D(ox<{zeuX^|fwD-&Ocdo>AF*FI(W@TG6QVL123 ztw)6U_SV!A?hW*-X5BaL7vE`ac*Ra{8xDBGV@FR=Le-!DU^@Kj zS#KL|y62J+VZOcnpoIHc{c1VP*9db|5~}p89^PB;CH4;f6IQj0p4Xon=UPCZk_1hxr;|j!Ht6e$~TUA-y$&glnJgKkvV0yY9Pk zID3y>X4S)dy`6IA*0;^Bzw^rBQSaMzErcrls)zX+VU9{dm44O3a(FN2sJ%8BKL4pt z46on+O=~HKs$359HNqT~gev`NIlOJuTl+}3zPjnXH_V>>#fyev`BRrv5A*fb)0x8! zvn@Y)(eR9)ykN9$mr$i&^)O!}%uz|G(yw}Wm#eqOk#Mi}lpF3o?6LfeVYgFG7_}1f z^-kH@FWG$f;x(ra`+j6?gev`NIn37xb5s(l^s640!`pOk_}aF^Cnu-;cI-WIEhSf# z%VEAon4^+VrC%+FcLaOq3=*D~Y=8K}h6n!U*x_4m9GBdDy{q`fPd{up{42)}JAZTh z%q3LmSIc3(Mwp|LP^Dk>@CK%Ik5IMiC65_4d-S`8=f7>Ydbo zJ!(1}cIw-Pop+dwqLNT`+u_sUg=f8OxZhXCglC974+%XyBdyQFsHA&@s$33l_V(7h z60Uc*z3;N&gnJw@oc?EDU0TawzOFL!%2Gm=e$~T#jW9S4a_+w#s!LY02i!+eb}McFnifKP2oQ`qgrHAHUbnCG6+3A1oVIeCqIFf z=BOl8=~q2GBk@xZUwiz0ha+G2;bDuz#-B2kQ03W4^)TPwxsSx%xHYF=^)O!}%uz|G z(yw}$ub(mb_?4Rv>mPFZaPt*wt1DIdRS)yE7IMiYROwee{1lCJk5Ki9<2D~Q`PAvd zL07G<%6&rhaQ*3uytfKh z<+`+bI8yi4i4v|8^VsewPVf9m!t<+q?qk2}o%=|5?qh$fb>$~Zdu><3w!78t8xPwb z@yX$zA9}!~dbrAv?h&f;y2P^{clUWmFQLjapjr+;vE1u}686EZF5GZ<;(oK?moFc$ z`WzYjyz^eWZ!rATIm7VK`;YHBOQ_0YiTN5~j!Ht6ezmT=p>E61tzAOZ|2}7@;f%wM zA0Bkw+D>b|H^0+!b{e+%>hZ%3|200XwHK+fuhepwuMy^`Bvk2FJ^Vy}Z`_q|+&$-Q zcN>1a<)?<%zj1s5=}7JO0UWZ`*26KUpEd0E(eXKhgsMEYo39b(s3cVBSL@1eSa2Pb zXJ-jjpSgAG;e_vKCMurat-I_81fp>a&mBIo?RYkjP~|vZ%VEAon4^+VrC;^%8%TP4 z90~V0H{9bX!=}@>4l6Gk@73HD`TZrY-skDVwO>4Jxcd1guH~(zs=SXfUn9&>NvP7V z)|KA~<7s_9p_fqg_&fb-cIh`iKkU23!K1eT&T7s+dB%}Y)td*6Fh^x1s`RUM<=M2~ zy>sPzwjS0jJ8RhYwDGtrp~^Gs>S4Z)sCg8XP^Dk>@YKL>V*B|gb|22$d2HR`TLqZSRaV>}6{N$=Xuk!_VaY6(^P)n{J5 zwd|gE88-XU-~H}9DesCTRIPdS1BO-K`pB@w8^-gX=NEqG**7-6_ptQaXa4_uOPYi# z&oyc}%-0BWR1&K6s~&!foI9kvE0R!kzvpi_9Q?W2aP|wv_W*pK?RU_<_52NoZ4a0Y zk2-mL4?sed@40I^%-0BWR1&K6s~&zL*Bcl4mW70>f7I-9gBqTSf2jmCl5#c^sU3` zj~u@%@~w>D(Rl05%ZCpichqpeYsPn@Bvj@1IOc1FIVuTN`qjGfTQ9vKmT!egsQQ!b zo;iH(H*XkrJ?fOv%F=fperM*j*FJIh(gWT;+;f}p=W!%d`7WfE!+eb}M&n`77nXNk5~}j^@P3Kkv;FJO>@@u1kmH9dUOoOyoP?@;zr=hU?eYjG zp-R76SJsbdYatV=Ojp7aFHffOX_SPj-Wgf197ba7`qgsyZS}o(MH0R%dgZN4hbJEY zH^W}98Q)uSeW)&`GFz*UGJi^=>_Dc<)i;PaN8}rF(=b`+O~j z?*#pQgOA<*py7rOe|R`>+wtryp~`oK)x&%}3CX7)5~}p89=?O}w;(py^1o)!y4UB1 zgMYHe=)_AxmG7pihxt0gN%%Jco|;je^vdn@1Al2CP*L!LfN zK6u!0)&bj$*1N9NrF(=b*YLF*z6bWVQjWRD@?ocQj~afp>G+ct5~_SZTs_R!okiYF zNT|}Udf2y}ee#SWp~^mAJv_hgjahz6CZWo6jq2fE)|nyC1`?{=<5myf30l*+b|qB# zuCRJ|eqj&Jy;ee%=Ni?+KH(dI{1!k$m3^gpIQO}G$vYMaRnCdk!*^-DXD;D0_Z?gH zaDV2^kY@u4RqorWhwr&O#mgsm5~_UvRXuDa&h2@2mQa=3mHE0)$SZvbRr=L(m~XFt zNZ3F0s~+ypdh2!x*X{1xvPZ8E>R}(u9_H)rKJVxyROwgCVZOb(lCZAys~+yjdwT;3 z_Xh6OtB3j8yK;|`P^Dk>aK`D)gA&ezd5&`C?2U;Mj){4$HD4plQAw!Muhx}kR=v4a z!nxM7tm@(H+$)EK<;e57`5Ix4NwH)>+XX-pVOQ_0y(0qGqQ3=&k%X!| zpPO%QeI?=gO21kT>&FOlR1&JJx9VY3y}g=*d$n8+30G%%MJAytdpJ`2rYXN&l2Db$ zc1O708GwXm0C`MwWaypmN_f7T#}db=-npoR=c0~XwO!c~P3M-FRGF|(n6~BDsFErZ zMwRI5Az{9M*m4-V5%!`|WtnQp7sJEYO&ej2DycGIIZXFTuF79^t6n8>v-g5A!uGUCWVFJ;E8T zH>;`29_DLWx|SoUdW5Ti-U>if_Ap=5(zP5()gyXm0D5=^kUh-Tv~(>;QuPQ=De}pL z9;&j3`I?rl-_`N!23~UF#~T zvWN9!+FGa{N!23~y{;aXyyqcd?76PIP1W;|(Blt#|H_j0${}Gna$T9PY3bkRGdIE< zRZ{hcUODuzwAsUaO-rw9U74@x|MB?0e=W@?XgWW^r0NlguFpKF{`fs?ug=tYc9u|; zTf6y6_Xt(`)hDQj=^mlVboN+`a<~HQt@>2ua+q(gt|ZJ?zgkxkrh9}c)7fJ&%HhtU zx0_Iv%VEB~x{@$o{c2rFnC=m(OlOb9D2F@N-Y!*DE{FN{>Po_V^{aIyVY)}CGMzmZ zqa5k9B}e5MNiK)^_UcN)eD$kQNto^ts!V5(#VAKQ#miB7_Lj?GzP-AVFkk&@R1&6p zgeue7V=>B+PKR?;p6TXtm~XGHB+OU88kL0U9-+!~_E?N^_(q`j7C==lhxzvEO2T~g zt92z|x<{xoojn$#9BCJpqjGK6CGsRS)x(?h&f=s~+CQ^_G9W*)O3g z-`_Rg-kMs%y@7tU9Oi3;IVuTN`c)6_n|s4J-zt_+)x-_RcaSJd@F{dYG>f=BOl8=~q4cL{)FiAmQ4l_Zh5( zx~pcsMwp|LQ03jYT36Q3Lfv&XUn9&>NvQH}buEWCetT;l3D;M>JHiX~nK<(`!W@-^ zDnEl)>&lzty*-YEd$r!(^M$%EYQ9F8qmoeN{nT1l*3UwHZo+(xFh?b!%Fk)ka`;J$ z-Z_JW=Ow+*W-Qb_Wb-w`9F>GB?>*PL^0P$JJwlbAF{&Ou1O0l0DxXdD@V>d9YR#Wy zl~Cn9^y=XmV$VZD56?)ehfzuQ2vxZpi}76+uA^OL=9Q&{D%aPwuFTgm zaPQPo3A_8yi1i(rC%+F`5Ix4N)x$HAh5B5P`FcK+Pgo>W z`8lRq4)e84p6Fykm41~l--Y^|m-$)?x#SY6{9IWrho7~R?h&f|3|{rHuJr2>s(d!p z!x66MA)!Ye6W!PJ#$5@=UH3b+9PS6)P39e>gevz5)x-6tEArkdT$St6>fuP;TPI4m zPRwJwr#QXyD+$l9^0|-wu6OPu;kl3fvDTH}b>3^c61H8x1HF2<%8>36s`9$TvmST% zc}Fjy$}^x^4!5;85$32QRQcO1wXXa lv_u%GAePjoC< zsP9}hUn9&>NvQHWo@-rsBi{8xUI9p`%6I6U2N&x1ipxx zbB6pK*v=UX^Ht4XNxcWi1o{Dq_5duIvXJLlg-@@!$DerMEtjW9W<=+@_4wvo`s+`+vIsB!+-n&!@-=&`YlFf%NUUT}e??=YJMC0B` zx<{yTPgTp|y^MwWjS=&8w96x$gerg2q?W_esD=7%K=XBHk#`dks{Ad&S`L2+(-Y5p z$|<2L|8}P99qAsS%5~7(6@l z@YOYM9`5&*$;`LC`3lZ1--=@fYkgtlIl^!v&u`ZPn}t%Z3%7I(%69+jNaeLX~%BDq+68 zIZDDgO26u{-Gz4;u0HwW!)~XXF!K$6eycB`>M1weeb{698NbV^dbkIa?h&fo8&(h3 zZLVtaN=8DJ>$&RT`v_|~*RF&r-&0f%`-E>|^V?SmRrZzY;XcZ@a`{c1gsQxc@;Q6j zl22G9RQddCIXu(#v_7BEOQ`a!w|dx0dTSpE*FL#jIcN0N?Gmorol9yt%(vG+BZc;>fEE3001d*~1>3d#xU-vWNNh)>jg)uk@?sFyCHXNmy6< zRS)NLTYhfs5~}iiZoa)SQNl4%zgiC0hK>?>WROthnz4GgZgWJfs96_bvI2iG-@Wrgl8qf=BOl8=~q2mE%nyB60Udi z`pOk@uN)GVBd@8=*9db|5~}p8b>-~5P-|+>EL~;hm8FELd}e9BmMO0?Bvk2F>&n@A zq4sL#+go2rxW004Sj%DkSf;$nkWgj4RS%=`^edluNvO)@kZ^UDS7Z{ZvWFwJCsX+} zNJhF6dMf}`*~5HIOV@HFRgdVM0qEfwK=v?S)6%sZ zN!24frN}1}dZ@}C=4)EImLsWpgr{Hm#7hrV*~5HIOV@HFRgZAK$}^50sHKN(r0NkV z!umY)F!t=>XNgSviJ|I|RGBdLL{|?9^R-NsFkfq-S8`SQRS#RAX+Hs+300;SiZH^S zNB)Kl^HP=1OC&69{$!YhD$i$fIUEhVxt#DD$|*;9KG6= z=y|9zoue`m-`!?HmGzbhpMmL282kK*Ub|9dI!C3-cAN=Sy>jSbpU)nuY%!V8!#0G-;We?7Ts@^lV9GS57*>-JXHL67T z_e`iV-6M>!N7&~xp~`e7T!rP;k|k8tt6e?JDMzKsbdTtj+CkD$_l}_w|`jHMBkl`Y)AXAko=oe4eituWnH^*n4P{^~}KN>$Ip zayUw44^>833EPivd;EKk@Q!#UROMUVy1TpU5thSrj!KoEOU{HU`(q`{*LQ;cy+`C* z-sYt$-`SS%dGrX&VY*k2{L5KJrOKMF9=iMH*uVFP{Fxu~Qk6f!BjNMt5thSruN-~{ zNG4QS)0NQO`vv~JM|cB26RPqZdfmM>>9ffm<|~~EOK!SH_+4?CQ023!gzny3@$Wst zTmG3)mG9~6?hVr(;WL-cQCV`+J;L94%7iMPO(k^qK9qm&5q^>&6RPrO0dzl`JG(u? zXD*$ivgD?FguhFd2~|FuO6cyI%D?vrKh>HERrxc#x_fiENBGR8b5xexbdPXdnh8}t zn@Z?zUAexlgu4`P_U99N303(VPC}LYog9_>Oe4&MD!*Yq6RLVqsd87GJycnyO1S6n zsrk)_nNa0kBonGULCAzEPeLk@R$=+qZX{IY->8ys9ql*XpB= zCR7<=C0y6~eHnR0CZQ^SyO@M3pG}TRRsQA{Jycny>S4XRkMbL|Goi|-nF&=sn@p(k z`Bx&v?yaXxsLK0uck#Ysk**#Rs^;(EIG;UKIhRzzdD|J;Z`RF(s{A`9#bdZ(w&f=; z8lLf!7wqD<*k=z_*~8dfW%w&W)k8v+WvYbvy8iSVh%%u{ze+eSxyrB>GNH=(suITT zs@QMV&4en;R0;ES749#sWI~mGl}P#KUuu$2m3Mdki#s}h*`r1!p-R6>n6EoXe;Fzh zs`RUb`Fd)Qe{o7em44O36NuhCDB(Ql_h)7gRYsTzRn|f!jNRGNSu_)>EK?<1-MQ-X z8|*Tn%J*NDaMbUu+a+ALJC{@s^L6FziaZml^s9vVT3Tx%6RPy9gll!*E||`QPq0U* zGMznCna+eWfP36bsLCGxizTquvqk*%G<4P^Lgma&CChQZYE8#l99erLisPa8me#auAYW^Pi zyd=#TjuN%x5esXonGatT%XRl>2)k=k@7e9q>S302v{f3eq^t{#c-@0IXe%u}`8YgL(V zj!Ht+{5_m^YdOr<)^55-m{TRp%hqnXN0?J3-0Pcf-k(c2emJgV4^{eALU;RiCejYV z9$Y=_okr!~Goi{pUkT&Y!*nJRT?t2nl&*yCmczeSBF#Ady%H&sO!zN}t{xJmr5B3O zuX^a7(wQ({(>+4JOsJZ_M~_$x4%1AuX$sVd~F_|zD&r36*$_O)IB%U*7LX{C#!dBgDB@*U) zw`H3T8{KyL@a)%rc#=I_i<<5c-cQbis$Nv8ys45sR5?mi!WuQ5*9@xcQT{!9n6K#` z;VLQ>o(WafbS2E!y_$dT5$Umgl(>W?t z-eS*$DxXay%-6lFfA0~VYh*%|ry`l~T%$+$%%!u3B{$t8yv3dgRX&?a=-xYLNQCcR zvWF_)y=203@gCtbm(CuR+;ord7JDXC`D`koyDKvP-XnUaYN|XV&mNx2_XwZ4boQ|1 zrhA0z(oCrG*;GPz>&o?YCEREDhR-*DnNa2ZnM|m1zmo}fsYaLy^YxBSCREMe!(DOq zP-U4a;hw{%=H19lsB$lo300mTWI|OwXK>}^ip;z9)k8v+ceyj+8)erhnNa1ew@j$& zMWxEyblF3dWvYZV>JGwpVVSUYz4x98Rr+PZNb+rU%c05$tB3CHqrCT?300OU6RM0b z6RNC*N*KF4?0h3$LX~Bz9_H(g-rM+@P^DibQofG!nNa0iQVHkna-V+d+h*6_dFAk^ z_w72%gm>NZe6EKxr}xm;;=#>+Bdmn(t}?t^p9xi#sS+t)@4aV2RbIV1FZJq5!WwnH zs!{3gs@S{rnNVe!Dq+5^!o4Y<303-4!hGGIc~d?Us`RUb`MRU?ZhaXBxBZ{ue|mFF6na1M8F&x9&h9F@@BsH}xdsIsOj;p)y+ zpZCx+q00APm2lK|~6RlCxxpr0gmc(?e zD+z1VbdNBnOsJZ_M~~3MbdAb-_nGJU!+&uU&1c9Gs`RT-xoh-YLEaTfIA=JQ^gN6( z6RPy9gz@(FQHjX!qa;*WZ#62{pRU#OshWf;&*ZX)YurqvNb*YGeV_ZY+|MOcS#Px* z5@Lz0KrmIIH{Cg#AKj!OhvJ(D_ zd#CInp-R8%k@`d)+a*+aepx-v4&Rt$gq1K}d#dRkVNR7ubB1g9N?2Ocx#X%+I(wL} zbSA8y9-)Wpx$I%S(mf)t0L)jF>7IuW?|y&4`1U5lmmYrFaPwv3x-u$NMwmTp zueRe%sLCE&Zur>YooDPnT({K;7d+unli{`l4jxuN|HKQv@bkh@P@}uzP9OOhd=+u z5yJtmdF-gyN~rqolPAM-mLEKvb-*?k)RH?IT($j=XO~_0nc91E-LU-Gef6s&}(>+4JN?1bE*(0ek;TUDQN2oGg%aQVR zB+7)UJYFRaYb1N9a>bDeOKZAE7;PpTQG0|c)7e9newDCs?pNnNXGcpzf+%KV%P8t{*C4>$6Yf(MLj+V^{WY zwd7hW6RM0b6PDIm$b_ow;flqTjO(E4A)(4OQ6*frxjM@$G6_|#=cIZAS+CeUJwlb~o`-EA z6RM0b6GmdXNBm(uj5qbiS`JU>(kWgg(k$allnGUN|6wi8{|QPv&ey1tDidjRc66;C z5~?gyC7dl)NoPWpb8se7Eo8#_$sVpCZI!Nxs)vLs*Qk|n1?kGtb!jG4xvH&1irres zgeq$~6TM!W=5U{X^^j2I`XLjk7Bb-z%pUV!>Cd0Av_22hwH%8d^Oh}tv zS3>uct{zF{-z#Cf*B|%5+5g)6=hIV;TG6a43DecXf7#^v_s`b6{o3iB|6=*XzxN2~ zO877LdHU(INAGpjblKJipYPv$gmfkRmxrAAzS+YsxpsP|`#*8!-+P2~CH$Aizhb-D z+rRPi>7ECEc;?@GgmfkRmv?;p_zOO`(H-VCJ?29*|K1~{E8)LfyTLD(p81Q7=l=60 z%Vz$)M@Uz~e|h*`51G8@m`&$C`}R|o`1c+mT?zliUgY0)(5XbS3;3)91Y42`g7UZq0PXA%8cz$)UUH)epOJ>Cc|I@!ah%S~eo= z&tKno#q6y+te)<5-m>#geAbHDBkr?$deEs~*ri70C^P;0cgor@r_xvl`XW%O1V-V;8PJx8D(m6vCV;vE*^vOm4Y)ler^* zbJ7x9_(MOp&E(wgZ8EpThTmOMi7o&0-YXw-#G2{VXB=1h_L-BbF8GH_)=XEPdfX_g z!*@P+)k!D)YI?)=#}~q!s>lEH%57%Xzxc-KIjfH=W7K|kzt8O2qi>u(=dH(&JRHfS zFS%l$+0`5WXu8Mi_8Sq7uV-!g&e>xhfBkgFrMt}bzUi3RCwISoy5;I!MugAVmhrQT zkDYD!o$pL%+kb0mEr+e|PWQfY=^?*ueIGb-*(A3_qdNL0$IP~V()H6%|6*-X*|qPnFFy|nRk?pS-`Q*Jo3$Jgs&bFAwM*O6 zs)vNC+@oxX(!Cy~%643%aujt8ws&Phm3^}kj=TExMru`#+SSAMs$U-4B~;nds)y~> z7XI@ezijEg`)@F}a;t;a(xX&45>*eMfzRJ@B@?O~l`3J6a`wrynuIEQSM{*2?6p@P zwbA5`7jHOs`*X)D00~t~_P*=n5l`M|Zl}$kI;vftnl0lSm)>!<&ngsLZe;%|1n@^iOOSDrbZ zoo$JZq7ON3_g!y%`0djbuN|+iBvgI-@rUlZ!MkstUjE9Dj7o0nv+eG?-3xcUanIYQ zH(xTI4J1^3WjNgPTOFUxqtby4~|w#5~^~$a#iHh%ui54)xX~9HM_p@^|w#2 zK70Jktsi^st3Gz<>{UB|XS(C^@!Cg1mGxHZ%5{`8zvKoA%YSyq$%B5j;oQgna=g-)Q1$1hZ8W*=D;v%|_{^P0 zBZG6FbJR@_yvyua&$(fG$`LC@y;efi=4T!?+v!cKr+0q#@)6-I?5g-z$G&6sxNlxP z9X1=UkR?>T|BHvsPF#J>^w7PRj69r|oUb0V#Rq3EfBF^E?YBOB)N3VFJ^ICOpY8j( z)zeLPeB!9C+(EcPI()@?vwI%))9G{mdc6OTP<6*!)}L*;*H5SCTtDuE#%@jL+Lcgc znQ9+&mD-yJC7cKE@u!cOeD%e5oV)kuuN#eU=4gsPo>`sUf}rfa5aZkbG~hi%CooO`W=D%)7~aJ}R1CGS`y zRJjhS9Z>;RIWi z?YMe44!WwzD;Wt@j*r#DeP-{OOZd#)e^!sOZsP)UTKp-R6> zxO#Erom--Ws@w-{;f`v#B}%B$ua-l?(MP{Z_%9Onn@p%O!b*6CV&|%lrvx`%3ljU*=Cp+vlr?|1y6`7h^6!-}-d<}@gmm@rU*_LyB}`Wj|7HH8kA&&!;lIp(6qPVt zJ^YvX&ju2vtB3zG|5;7KboKCG=07`2n64iF%luav5~iz%|6(ul?>$1g68?+5%fI&s z=^pX>^w8b3tMeR{k+=h>gniK6gu9GNI5N0naVJv=$16{!JpHPK`z2eY`>RU0CzCLz zO1ST{^|?>1gnLW-qkGXxxPO%}r%Jd-c3g2!T?tjv?%ONjnCKaRCl;0PIomQkf2f45 z&$Eo&5{=69k{p%o)pB@NQ=_sMdG3>Ylq%1bGU2~Sc&=5WvaXEKoHC&*d)UJD%g;kX zRqh|QaQmQrvz9|bRqj!?c4>QB^^j1Ndz39vy4Ryr*^X;ej-rmi_O48*vTs(xaaX_I zNUh3IyL#AO^~+%L6Z)K~8J=K}hH_Dk%<=f>- z*i&s8z8TMiD&MPD!j@sX@(q6`RQa~Q5{>|l4Bk}8geq?>RKoG2_sk`H=H9-j9*#1O zKHeP3geq^5RKj`5`NJD9nNa0zm`eBzoB_PalL=Kmn@YI4vmNJ_D51*RN!7zy-?h3o zt1_X=TUM2@Pq=#VhFB(4*;gvzIOyu$n{Js<<@i_$M}6B^ZY2_`yuDXFT(LMl<`GUp zmA4S9hke_*-5ZpdP-UO5g!!75&V(w{m9THSX7EO9CREwyE8)t!R}Kkd_m*z;aFt<; z$*n{}mA8$nhr2jeOWu^ugeq@MSHhCJ((}f5CRF9Ra{SP*N2qdKsZqHWb=~d_{7k6w zHhv{s@3@}JD>4aHu7j$FeZn&3s3cU`SE`3|pR-S%aU@hZCsq%ifh&%@%8*dyv#B0s z?&A|Rc||6n$~m!mxF2vGlvgqms@x}35Br@vFF*N|303yTN;sA{`{Wr%LRB71JVWY@ zR}zj_ek!e&!*zlu9)6-O6RKQ4RKhvKQ9F;+5~`d_s)zd;Yay3hLY4cS>S0@QkDGU? z5~^%t)x&j(yUDzRlu(t|B|c}@zIpX3p~~l9%i+GpeOumnNvLwaQ$1`Yy?Ic=c`&yt zR~C+Nc@-|9%644KVT-fZ<{3vqmGf2ga3r(!*(x)kD%X{xvro<0r+P@J%5B${;jS^y zY7(mSt5G=%yMxTNE1^oi>fsFFjwP?cB~IGS~|;p-R6> zxJvEyS_#{0?t`}Q-Y6iD;8U2ZizlM zN1|L;u8Qr0_Ou+8<;YRl@7zJ;)^3kdC0(O(Rb&gdKUTsXC1FmLu(jL5oqa0d`rJNf z53YoBpM*J8!WqCZIFGxI=RW5=irO-4ua3brDqEth&vYg1gZ3iRmGECoXAl3SN2oGw zPBkk3W&VV;ZLE6uFY_m)ZDZBLf0;iaZSSfc{>%IcX-A3b;lIqEkj^8V|1y6q_X}YgejFn^TR- ze@W?eC5$Act4C7#_evOhZ@f}v+MKF~s@}M(%CtFE4^_SSLzQWBsvfF(^Pnoz=2SgY z_2zR`rp>8(sOqgtRGBuX>Y=K)zEWk{oT`Va-a1j0X>+O`s_aGny%MTS_XvBDboEeW zIurg&k5FaWoN83_-$%`VuQvbv;QaTT^WWFbfA8$B+SZr%=kq@UnEyG0ClL0#e3mi) z^OE_W$#@#oJNKFY*^(`>cYbA5y)&};pTpUsdS`tSz4OF%JF}es$l&;)cO6T79uj#J zl~85BsUG$<`=Bj96RPZKm9XbXXT6AX=L?qY?rVdXF`?zu@dR+o9#FgsvLbVltsBw=18NPta9V^^j2I9i2*~TS2ZRGNH;ktVFuq z|l46flbp~}xRRl?cJSDtE7y za3pht%O_qEs$APu4_EZA^z#X%gerH))x&mW%gD2VgsQwQaRuO7%l1~Il2Db`qL$EI znCr*tA)zX-85{u|iSpb#N(ReD##`NN)?S27Z+ zYzx)HnZ@3fdz6GKy{m^U+*#jKkxZ!4yArlISKh9PGNCH>L0g=on!TWUNT@Q}N|>)B zk#r^O6B6cB38RwEgyV;!Pd;0+#n}?`?#?=KEb(d9a@dRPgL&0wRF)%0Wvh1HwLjLV zR7u-cDq+8~g}Y~{g#RL8U#WyMjxF4lUkQ7vJ=h&UCHxl&=fp}l_W3j&wJYJcYpeA6 zSHgB>I=3tPhw0ou%vZv6j!Km|^$6)6kv(h~s!UgpMELhg*fLa^?h)ox3HyX9(>=nR zDq-JNWx7Y0Qzaa)RGID(=2QuLlq%Cb!kj8$KUZbCN0?J3T&t-v-6PDY681G!rh9}r zRl>fO((6j-p3>DLsr-8;dgU-L3ICowRC!8~300;u(ThrzX(P-Ys!XpdVI(PCJ(9}5 zSHgJh6aKvts!aC?`?hrTP-VJDI7UfV4^^gngk!CA^-yKHM>uClR}WRDdxUeJboEeW zx<@#NOIHt7rh9~I5$Woo%5;yg7fDwSRi=A{y-2!xs4|@i|D{K$GHp&ZDp%VQrYqr! z-h540!d-*AiQJ>ynYd%g{oGTo-r17-CEJqwt6C2KMZ!H?CEWMfmfR;+!o8)v$h~MK z{1*xLyOnT{>}cSgx)ScgRY_OEy}oAvd4%&k!4~85uO7BU&oXjLG%C+aa#WtZSPt89 zjmjS7xlis<5}qwdR}asrB+RK2{)>_1x{^?(clGdjNZ9f-p~`+!3HzFT(3YPGRra(> z*mERuYnM=EPpcmGD2dz>B~;mttB1YFu{MvQ5~^|^bflKZt3C-;j@q>xj_ne*<4mZ^ zJ<2yRws6~V^^j2INK^^O51*jVCKIai=;N&BEMxDg9ulhTU6t?+pQDfUmI+n9`KyF) zD(zjlM@gvi4QBOlH)< zo>mEOw7Kece9VL@$I43B8f+zbHjq%|jlJsONahHaS27Z+ym?qXyn*TL>^PqZRgM^$ z(8F|(FxpCZbJaD2eLfSa95E`PyRo~T&x9&(=2pViU@LLOkqK4aFs_83+H$Sst}hd+ zyeVA?Z+5%VvlcR;D%X|&BH_4FJtR~)DpkUp_O9DqWoAN^H}EUr%F7j*>!3`iaurnx z-Hpn=k_lDzv`RR$IO8}cWOERI#nWz$$(2~2~$%HC*OOMqqbmI+n1&`P*dbO-5*BNM9fy2Mqg zt5=_Y^^j0y>#2l0EO%b+cQT>My-y{a)tm=yF_}=6+m)+r$4A#I)k8v+y`U1#EcW2s z5+zhQ+f@(SuB|VRa1yF=T{%bj)Ex6`R1&Ik+jaJG*JyjI9ulhbu7tJd{xi?c5~}pB z9%nAh$8i-dDx^>BQ3 z+;wKCgrlX;-*LVYj=Q#FTTCTviKcT)v`3lFJ<5C~Oy{UnnNyFD?h)C;wxr5*^+<$& zuY_$$mFXT~PL;41sWROo%&8KN2C7W=2y?20Bb+MJJ;IzSVUJQ}x<{B(CG6*_O!o+L zs)VDDD$_l}oGM{oQ)RkGm{TR}Ybm|1gzhO_J(9}5XQEdQ^OErI)x&c!)9ybrp~`e6 z+?krzFB7UvS3-Bw`ej0u=}PF{^RV7cf8(V4FWvrxTc(%4`XiI-kyM%R{KE9^JHBn{ z^Zw?h>E*8+dn8pRjLLTPqWvE-x$qeq%sqOWr>>bc7>Jcfz9F?(W5A!uGT|JVjMgDer z^n+Cg?DNCvDMzhX%b2Jt_Yd>cuSclTuRcLNO!o*?rnARll*1M4ZOaavJ^ScyPw)H} z%SX?{(yGekFkk5&p-R76S9+N45vojQkHsj5yOevp`+c*cp8xODxvdXAKS!l1m&1H} zbtPfG`qii;O!o*?rnARll*66r^dDa{JL1xBPw#a9C(d$Is&YBZw^vsZ=Br0!D@s4|^B7NZ=VG|ye}g2`hy-FWV^Z$D*8j!IQ7 zhxzvEO2T~gt5Hdq?h&d?XOG1wN7{wusNA3Ba+q(gt|ZJ?zZ#XqVtDlSA4cUqDwo52 zrF(=b{c2tPv3U3%&@$zcOQ?GALHAm^*^*nPD-QX)NvfsyE-ZPi*nXh-^ zwv!XSw*K6&U$ksg4)c}n5vug7KAsY9_HIyQ%krv(64%!uMy^`Bvk2FJ$$q78~49F_hqy5estyZ&{r%OJwXXo zce?VWv#aiK)pX7J(-C35z5Sqs`&#{KIn37xb5s(l^s64;Tkt-^^A26T>akb+aC*k- z6GtVNP<8l*-(L09t$s8;@aB_7g!%T)G9)~c(XW=ne2p+iC80{c>fx=B-kL$ewa;y9 zwws*$%k}3z{^G~Xs)zY{JLS#iZ#y~g8|%+KXvNNJAynyCJ5$32QROwgC;ccVd+DF3m)sp4Un7rua8_jL8;de)? zaP#%n)6*_^`s5?~Y&7@Q-<-4-LY02C9Oi3;IVuTN`c)6_a`pB&67JQ86+0}w_0n6W zD^ERc)Jn|PJ7v#!_x+as^^vzsuRddKgev`NIn37xb5s(l^s640!`pOc{O|j(`pL;R zPH))$__dT=RW6758ext~LY02C9NrP^oij*yUUJrbzrO0x@A%R5Id2`8+N!+eb}M>ioTSoIPTb>!y3WZog40vE=&o z2vxbRynE`M)a8G>@9Z7RuA1(+beB<75~|L4)xNWho_N)C%hhATGsK>UgdU!e*5_eV z(mg^|E{8XJd+S{Z*SlZ7*CDgLuDxP9+x}ZiYdOrf=BOl8=~q46 zJ9(e_`1fu*x!-ShUdJ9e-g!x=a!*w~%-4Ne-g!x=(yw}$uMy^`Bvk2FJ#4$)R=@Q* z`_A@z@>SDM|6<&uBvj?LYrehyAz}Z}ua?96_`QBEVL!k0vNz4Pc;OY()2{o_s_J3B z?pX6KRYH}1)x&&^Fh?b!O26vi8Ht~Q*k|J}ul$Rr|6+Rb!6#2Lp~|z9N|m zgsOADcAuqpeeo^Ro4>KPlIzzaROPyIgzI@o=#j@n_cgt7SHf}E{Z4%z?g!jW<{hMj zD)$N1!}X^t^4=<3mFv>#;Yi(ECrY?Z%wxN!IKA^L3D2+cxsUy>ckUzMxsUy^)|HBf(neBx>A&Hehh<5iy{gP(VP$~C)9zV&bG%{_Ya@m*&LRe3BiUn9&>NvP7V)|EHZ zZTY#iOQ?F&$^W?ORhNEmdd3xNJFWHJ{6`;n`l`2V_WkLBzZ{>|+KW`#S86%T*9db| z5~}p89)6;~H||O}?q0az&z4^Dy4$Avy>om5=}7JO0qpdszh1iI7jBxaJac@`AfYOc z?dEHQIVuTN`qjGf8x~v#<=I(6)x|r#Vd=6*-ZWkD+O@sAOP^M#QMrcma}0S6XTC<5 zqmodiUyaIdyy%VX5{~Wrz5lSK%bs`B^yW*(>nq1|zc1s>KV5&a{NHb%uH0-q8%PvY zoIlLh2y;{ts*2xs|9!LlzyI$yko5LA67F%n|FW0Pp7gA5PEWgOyjOEqXV%rjd>v8q zC@P^!zv|(sf#1Zo=k`yYt$f@S(;b(O&txQ2<#PtxYtKVM58H7qhu{3ZcFh*XCh_y{J@W57+H}$JX0_v-4!h zZ&$C!9x`6NN~p@~cJp=A&Lg#iD*fs+_gl-{A?00>gsQEtdDi*weZtSDC+|O=2R*;= zJI~I!`SnZx_Q5wz54(76geuQ9YB|i;2y;{ts`RTKev6ztq`WJVQ1zn&mP{^v&3bbm z`^(3S-kA74+wY+J%ZEO0@~&sBH}{}3$M*mvRQaB}mcx9FFh?b!O26viCvv@Uk#AW@ zsQS|K^HzOy&+ko7Ib!^&EpNT}oq%`!#`9-Kzxnd%U7kJuT#b3d{N~rqUWW}mi9RB_3bN+h#j^6j#euwGz zZ}{k{S3Upx({rvLui+$A`JTI$!+eb}M(X0ouqq&D)&^iuB=^mVR`2zp(;NQ@0a*J+lT$= zA6ISqsQO+x7GLF6-oH6=;9yTbM~^|te)=imhrtM_h-^QLY4crS`NS2-}`s@2Azbe z**o90^rH2DHQn?*<4+vgx21c8D*JpbhwlXaeS_~m<*h5Xeg99VH#~SeJ4>kYU19Yw zUr$2v>4$_W{i=uWp!_X}ogen{$*Lgev{2hv)e2 zkn*lbLY3$H)x%#2@%C1}u_dAEQU83%?A*;Sn;v!QMWfGuxK@|$5vp9n*K+tC*xyRo ze!r*A?t1gLrfY7QjP@TAs(e3OJfv72nIX>x5~|$eRuA6^TGP39B~VP?g)2`MOWYD}4!7`qgrnZ?Aty*gy2E9`4V2>vjp(?e5#MN3Rd+VIRyM z=Iic0@8~5|=~v5PzP-AVu&(s09`4C|djkpg2JY3XhxyvOa*vWwrC;@M#_7$263&Bp zj&kPgjfoPDiFvLyUn9&>NvP7V)|F>gy}4Gxxz@9+>f!9%D~E*T$n&}R8ext~LY02C z9QG(@>O4D3sLFlNe0ytA3D=_f)pD3`udXDlEB&g6`C1FPducj=Y95Un9&>NvP7V z)|IOzciwroEuku}ue?*zD~E*T@UBTMhxr;|j!Ht6e$~U-+0)^CA}paQ&*z@QxXR2c zO9@q;&(w05uMy^`Bvk2FJ)E80A?00>gsMEBn{RJ@CE@x?zgiCK#|U#&5~{4X>S0v9 zy_$r3wOkGfS7&)eCZQ^OI8ytjDZgEkP?g7aN4VY@fP`lNc}#R<=$-FMc)pv*633|C zxu}HaqK;j)UD*;%=a!gMnXpfow&mBTk}4BMmFVgrVZMLZau~Z2_M%c{nQF-w!^7B3 z8)1zqsWM?XO!rEz%5;s&d{cy(F!tnf?ThxKFHTBsgL)guzUt{#@W=OJP2xvsoT z)$@?h;}3iP%98iWAz?XkU74?G>EGuwH^LlMQuT;lIrOl!*~5HIORsBPnXl>p@%X=g zEzKurIzPdr>Jf>q&pfIA_&sc|&eVB!mQa;jyZK7@2vz#kC#Z+%9-+!~_E?N^xB~30 z`c&m|m~XGHB+OU8T2~ULdxR>}*<&%v;m)GBn^2X@VZOb(k}zNWYF$Z~?h&d?XOG1w zhdb8ZE>%@7hxzvEO2T~gt92z|x<{xoojn$#9O<+rN97qwE{FN{>Po_V^{Y`ynC=m( zOlOb9C`UTQ%Tamumdjzjy}FVxU;S!S5~h2ED%06xG0KrnhjUb(>E?2nZ?CQ-%vZk} zm4xXYp~`gjSd4P`Mxgf=Kvgb>`S$8c!hH3sbtPfCN2oHLJr<)JX&08Ga(|Y~VZOb( zk}zNWYE%-7;nCZF7?t~|Tn_Vm-!GBKZ956%A4f9J&uHXwcg$Hg}N_lzDAg% zl2GOS)LK{8&q94}!hDS|Mfsq;&qG2F&q%9>QAzg*Rk<9C z@m&_Kqg`d@m8FC#*VnbK%-1sIZ`hDfrC;@M@8l*rB`MI)M4nJ!r-6K@_8NBLYUFp{&RQYVGha+6iLqd-{ zCc3Zbjk^+#yY6>tIouDpo6I{%303YBs)y@OSLD4_xGLAB)x(jxw@#FBotVdVPjPzZ zR}!9I<#QkVUGLmS!gC+{W34N{>%7-?C2YHX2YU5zl_A|DRONMvXFcxj^NwCZm1jV; z9EOO;o`mGf6wM_XN-z8M}+bp%N{5Hg9Ki~P>RVSVFtKa=9v;91Of1+c_LVf45 z`C6tNm4qt4%l&_s$t4JZH$? zf$f~JP~XLBzDAg%l2GM$zt+0)S9@HM=T*3bs{A`Y?zR`|J2TDK2y;{ts{D@5T33GS zX>b0JaQ?{Olj@AKP`_zmzDAg%l2GMuXwP`}M#zP)i*!g1H%0;%Qj7Ylo9Q3=b-ZC@V#^XO(f427V39K&DRKXR1&KEUDH}u z{$hxyE%}5+LRJ2a5$AB}9-+#)y_UmY`s=+*mGE6^{%t__PSQO>m3yjM4)0|w)NhQK zucKWa;UrY~ni28S*O4fXJ`$?@z1&(3-zfTPtN9mJB~;~ikbB>B%uG&H zp_fqQS#R~QmGss=60Uu6yK>Ixt=lDBw>y{Aa+q(ge@NIr^s64`>seMljgnBMU-fVt zbj6WZ84{`-AG3!&IQLpTRAmqI?X9mQTwm!|%VEB~x{|Q2^s64u=eGRZ+9g!w`P_Va zW1@s(qJFg;t_>X}^2i{e$~9y4aNXvJnnzIyRj%i%hxr;|j!Ht6e$~SjvhQ2+8xsjt zc}?wj-YbWM<#3#@EJt2bo39b( zs3cVBSL@2zd7;+So>{ue%qvR?Rr$=)d@WO6Wk{&fuhx~b^Fr;_%(u6`l5l^;G@sd!L%OT9^t6n8>v-g5A!uGUCWVFJ;E8TH>;`29_DLW zx|SoUdW5Ti-U>if_Ap=5(zP5()gyXm0D5=^kUh-Tv~(>;QuPQ=De}pL9;&j3`I?rl z-_`N!23~UF#~TvWN9!+FGa{ zN!23~y{;aXyyqcd?76P|%#Z2(?R!a;2|piYI)9oxsd_|;us#nxj6Hk!St8SZVyJo~ zRVIu*(bYr3d@WNY%-34zm0Xp6)x*|j+D`yyLY3)-B8;%-k-uTXyj11$5(!JYmTy8h z>HbT%KjD_?<*)w8M3v_=xg3rL-dxXwDkIE zGM%F`65risLY4KF37>)KOc?w8iC()>WjaTt%66OyRlRcPVV}<)s%$Zt(8E5T301b3 zO!x#%_lRCyS=#w~^avyA5%!55G5>NHmFZl&Mr9AqgsR>%w;Y+U_1Si9V>PNo`1eex zGTkGLut(VEGoi|KCR~N()siJt)vH}S%qd5u%5;zDmE3$y|2{o@&hz)^5tg<`tlRUj zOu4R%%5)}-$~Kk>RlSyI?3r+UbsY5XH7fr_!gMC2O=rSzqdH4;F zxg4tW%S4GJetN*D{5*~=rsv@&95bP+SG%hG*23(eDwo4nZ7t|mJtS1+c~HXDtLf}v zzRp*fP}TFWpF3Y=4^>8332W4Je$T1O7Vh7(hxwY$gdX`;nC_~29<~yHbt6Zms^?)j z93`@cDkH3f?Z>x0{=G+dM?4d%@-1)O-QD#F%V9c4rOMAGXF`?zu@dI%J3;^6Bl0b8 z^HP=XY)kk&dW7XL-781_!WEV=0(;qN?ULY2>^61saI%D?vrKS_`YRr#|3x_cY9NBGR8 zb5xexbdT_N=`x|pXHyB?T~qn@9^t22GodPfhF5oQF82tZxpa=olAG=ku1hna%4bsv z-K{Iv*OhRW;?4eiLNB2zpTkM0a=(+Ka-V60nNZ~~Vq`*9FDg~;inE6*%Tx*X96mL_ z88H*8+>2yFl_v<9P*qMswz~uWrB#^Upj|yAROR2Ol5idEH{N6q^YuGZGoebqOc;s3 ziIoXeMpy~gwSHelUXe+t%HJ*~p~`2Iqf(W>xkV3EmZ^GJ@9v}g2JK9!@@Zy5mCq&< zs(k*HNU?kCDHE#l{@h)>?^vX(hlHy6dpOQ#4^_@3m2lp6M)sR^GodQ~j)@-5oPLXa z_E42QjNMg+zY+fPrrdE6RPy9g!59bt|Y80=d0>r?5>LaX5CDvvP_jQ zUsvJ&(n=;&=~s!AZ~mnw2~~M_=fAk4^OrqpR1&K6tAzQwgY=i7GNDSpN|>*w2Kg7K zBvk2FJv@Qv&4UuogMNQz_E2SnnNVddRKnPuEuBR(p~^B{VHL;zU?cuQ0huTm44O3wYqN?OlQI;*dtV#&K{~v zXTll4J#HpcWe@+wl3UZ&Lqb)49ulhD8)gqx`E8#bDV;sc*L082uSYDDhmn}hQK`yN zxyJR4lB?zFA)(52k1(f9xMRt0Wz0*JWy&7XJwlb~>S3LzGTkH0sS>VIUFqdBO9@r^ zR82xvUWKPf@;cGcz*)v|rIuX6xlcM1_6gIKaGl_eKCc;6`JOAkW06obe~)}#lI9FY ziCS_ApSgd}gsSY}znIR?BdIcBi?g-6d#UBH-hCdfWGmr{b=`>k=0^`zd8O|@+$WfO zt%SQ&pJpw&gev_i;n?R$Z8{S^XLHJgs_fyv*lSH!k3{(QN_Z~jsao!}s?0Y>C828m z9?rY99Oi3lH{B!5sS@U8Yd75^%&8LY_02c$&m|l`99Oc3D*Y;WP-UO5gz@TOIunVmgrh-9S3-Bo;omEfW*q-si4;jD{Fg*m4++!K3q|NxJ#nM!k*IRzoa;)JIaR_)Qo2WYx|0c2`ej0ucWyJG%5)}- z%F~t};XCh4sOm*!B%bJG4^_69Oc;r$pP5i)gqbiBPYpAn$_OiAtM0WD3G?+fe)e!J zYPv^wKRFYsdQqwJrb_lu%c*a+uB@s=URX302l~CCt}7pnvZXo(E+@l_z?c@OktI%V9cusPYzj zCRAC|l`voTYW}@PxW~9y+?Si zkqK3vimZjW;iIcw_5ANo&$<4%SUdn%GWEV=0(;Vt$|sPfrVLigS|Ln3_ll08)U z?j;kRi}whhxpelhhbqfd z3HKa6HE%j*LX~@wOsMh%Arq?dIfE-NS7hF;uO1Sryvv;l-zd9A$%HCzy=6jGFDg~u zrpq3xEK?<{QFjo&3(JJH>%I3(sM0SJMv`xeYFvS633&mGf1NN_SVq-mTAsD$7&}^K}*O zP5Dfy(ytQc>;BA}@|jSjUnR`f9i4aUGoebqN|>)ZNN>t#LY01%Fkep!@@;hqRr*zr zH0ygCKNG4v*T{r(xO00ZRJr1)gziRVEo4HKHC+i;cdq)phn@*lzW=I(qrNL|SLB&c zYxR7pCZWnRx$NN@Hxnt6ywZ2y=l(4Ba|u<}TP=r#PcRcH5_gl;BSn%u zBur<*zUD}6x<(~oYxnP&P?bIW7u%KT>X8WlUJ2We`MR5|g#Y5+DSJq$(yw}?K9R?E z300n7Ru5<4UO9}{o@%;!B*MQ}BF!1D;VWTjP3MxUO6lxjzS5bnetLu+uII9c`AYYQ zyaF&^Ri=9$=9CHZ&4eS4Gns#{9{!7IN1{sj)J*57oayz*ZPyXma+t1BCBnZ~!n};i zzgNN%_MV3-)8VJqQppPBZT(=wsTbSC`ecl|P9zNUM`{5`(?_(ONy;N7=R zFMs7nCfP&PlD+RbdBl@9n%imfr>=!ClKGdTN6f!=?{x1gmmc!Wjpsga@hix=@ z|CcwM`@kW~)=~~ty^^cice@wvdgGqAPj9~Dh^RP}*x<~kH?F&WNVzP%S z)0y!1=r4ZSQ&vru-84OU|8ebFKdO2j(mg_z=~@o!{UN9AzUz$-zkRymwSPA;!c3^L z7BX?4Wlvgpmq)Id9`?19Cl?O8?Yi58Zl4}@@wl#xN|h0258JElI1{R}$JaMrF?;I{ ztEYRNx9t2YFFkhlsqL_$>jZCJZyI2>T9Nl?!9E(Yavvv*!sBH zOYeT=^r%xW+NDP2XmIsW8%^$b@rHA^KX=*0zh^>~bdSg_(R@{z&K{~v_Xxc!;qOfP z`;-29WhHdCE&2CMs50Fn^s9s=G@U(?Die-Trh9}c)3qEaUq_-$sLJD2@~}p-hbmVb znXt5`dxX(u!V$Gcs4|^BROwd_%4R5^BK4_8aBwKAc~2s2@6t%Xde z${wy*T*zIVc&LSu>WU5 zRUR2qX|0j!;lDVFItFJ#m9>xwTSi_17?rB*kw#}n@EVnbDrbgDI9uB9oKZ8OD$lig zsInHahbn8j68?+j@b8&WrC%mgnXZJ9SXZt%GNH;9M<#l$-KcEsMp!-4`ZKrP`4hTZ zqo!+Ai$PdI(>bdDm%V!r*S{+3eJ|iCL$PcxOEXVfJ6S6a6!4p9SU}jp6S_TR2TMat zG13apNiLxyzvzkSIl`PeVP2-^2vw%%Y}ghu zp-K-kp(m#2h_9{<{Z0L`m%|;pw2Rk?G|M;>WkOY6e^`s_-$7}|`R-LxWg?Bvj;`H? zgeuF_31>@H(wR`@9Gr<%3z@KfvJE3hTct5kw;`d*7_}2dkVclqrI}D=RNINUXF}Eb zHZtKarnT3-N~%nJ{82x6=r{f4{`dWXlg`?B%O5^*GyVC89vM#3C;a7!e}2o&`*GDnU$gM~93kBa ze|gZ`4%@Wx#$(T3czuqL?u5U*_@rYuZQS!a4_|nFj*#wzzkJ_!KjYB4=TANE)PuY} zM@V4y%4{rR3(|MBKn^5ft3lJnkl>UHUpuesHE-780#%U|@)&5>c_ ztOGZG|4x@|rZ3txo-c9i`Gbu`;uF2di6KAIeqZzXD+_Z-^V@SkcBP$ z?*Huqq-PkQ;H6L;9S+oo41-{ka0m{TXd^|%8zi3eTy@-{}j zHRDQnmZruj_Z0eQGPhyd=)~)9dI*TrswG>q-yJsh2}S zRkmRZm(KS?LRIb`&Uf}&`(`hPgsR-5Z0*wav~EK}Rqj!?MCrL6rOI~Py>b+F47PV= zLX~~96OOyun;WTBIcj$swpZ=tv0XxyJ+0fYz1qSrJ^JCBJ@}aqddfy7R5=oL!h7KT zcU;MYDo3SG*rS|%@~kGI%HGv&SXcJizdHOE4n6Q2?6n8I?cJMob?5Jddh31A zILaB|l7GF<#^){?ZAhqk=m)PO9`Z*c;YenS$*n{})fex7-N^8;H;p5M(Tk(=mp=Ag zn=SE6Cy#AcLe)d}#6Hy$KlHJYFg|yLyWlp*Y!a7$-x%pjsQO$l%=P z9Cg;G_TRK|?eCm9^jZm3|NV%+*lZ=2oHYJ-jkB;(@mqiPqD|t5o-;byOc;&q0eD#z2yZvz+$EJ-VK6lhmSFRw8kbd$hx8AI)&mJ?b zKO|KB@io_t?GOBmu@CCIHJxi$LX~Ceeb6X%ZXT3y9(>C`{L1DC_nC7aJgf)J*WQ(T zl!Pkn^>R2e%#BwPj#qgsaXdG&xZ8hu(&jzC=e-XfT8V@z$N63kOYY2&X9EdUxvq@W zZTY#iOQ^c`!PiAHS3cr3L&^2sv>x`7CsihlG7{ZwNSJSqOPrTnu{`|D&uxwjCm%lU z8c3*ezUp4tmRzg-^N-zsvt7O6xU+}al~83H>o#1`84ctJKtk1<-*LB1uTHwfIKmmL zJ4)n{K|7vtR|t#*tge}^O7sb5B$OZ*qlF}_snk?<_`%~ z&R5-rt43$IJgZ5lde6b{+H8pz-E-Vev@O|#bFY<9WgF||Fy3*!lUFPfs*HoW4Py~| zaPGAds*F*(4cBMZbgo?qRj%8*4dXUjes1j&s*LBl4cn`$m%L(;P-Q#rHXH|yYH}na zp~~^G+i;yZch4of=dM4y4dXUPyF9{4s4|}GHeAaZ_2)=mLX~UWZo^UEmSKy@gsL2u z*fNZkj0U<52~{~3HPUnCl}j$6%DKIJo(l+aMriqWI~npI$`u;kk0+wU)Cq2a}4J%>l4!U zu71z`Wsa~HrF6HER9^3dUZwQj5?VK{y=-HSNGsE9BN1NTTd%ZkIZS68(x&&8(0WRD z8%gE$PU!DkYfpr9x8X1A_gV?l-G;xcKl(_R?l$~o{ZUlHbhqIz>(2%frn?P)S$|fO zFx_qV%lfmkgz0X>U)GN@BusZ3{$elk`WzwM34gJ7d3}zMo+G}JHneWq=sbI+C$0cG zVIOog;VPpOjts6?T*-98@yeYkcfUH}ddXJl`l=JI$t29F6R!JgeXbKb;o8#v=vuTB zu3sh0sS~b|9amgacS4o4>-J7KCb|dUjzuTD&$bNrA39;{b1x&eM7?sqBzt9hwH)r% zbg%41?)&5(rOLgfO!$k0`&!*A>q-yJDHE!)4O_VO^8JudmHUS++&*aE?B$S9m3x$} zUD}@3ZAhrfJ<66SJ=ddD*^awcj-rmi_O48*vTt_6aaVhDBeg0=?QX;Ns=Yk6OQ^D^ zbsM%IPnSDkPqk(EWIPk9d`jL4TZZk* zC;XXE<4f*d8Nib~nNa1u>4edp?KrnY300m>>NcG9jnzF_l?hdzvg(9=!sx{lVwq57 zU+IM7pwYc2-7=xd@v#$*`nIv$N+eWydav6sVsU)TBbl$kzms~=X>z!`Hw&WT&uTmvc*~YpJ;}Tbsc?Bt zXJg+Sy-KL^{`YdYu5sO#S6&jTT<>%nwvxGdP{MgIw<{wHN4Ok?OQ^CP_j1_c?6rBu zkx=D))onPE+4^jinNXGM%F)@o=Ik@r;H?^}a@)0KxN6L^nuIFtb+4R-T|wsBl~AR< zZo?VC6-$o7B~*F;yA2~GS9GqzGNDR)op1zjWUv=xLY4M9VU#-8Yb9*2xewaH=SEQp z+pG3^IV6l;wATrLk+7#_LX{qN!nx1U$1$c8Ml80<+!DQOjzqbxjEe1p_O$Gk<;Y&y z?_5FT)^3kdCEdL;Dzb&!A3I@>k}#)E*xGI3&OV(mKDQ6rgFE5eCt*&Va0YM;&f~7* zx%WAbqP7g%t7CBY%9d#BGu;XMpuNa+C;Y{9w&5>xgeueK)V=bT^$BU)ShwLX>l4zp zv2Md()+eOxUEPMitWQWgN^~3kvOXc5M>v03pODTy%3szeq;o&_m-PwhJo@;{`h>K- ztKV~fnIr5)Dcx-(mDf9=S1G->gw{=KFWZnu z({qG5%@Nrv?^H^68%gE$PPmtm(tAs2eXd=pGHp)XD}PDpy(RP{rMr!!@_HxqeQvx` zW!jv&4OMgFt}4^!)NQDmn?F>UHm7bw)!aO&%CtFk8>;5!b5*9zsoPLB7ni6qZBE^W zs=4?|m1%S8HdM{UiK1t-lXvkDA--lbG92+`B!?^+yKB4{i6c#QPzUM^On?_M2|QzGfe^o;jHFPv%M=5s_b2zNKbXGg-odO$!{lIRogOb z3z<;0#cG@PP&=;Vk-_Kks(cdPy-KGAY?YZ%<;uGg>C}U5Iuoiq8PSP!dLy?)300o( z=r+$4YLsjL;({96Bw050EGoi{F=|q|bbMz{q%Cp1WhCRm~ zW#pU*Rod=E8m}BBGNCGuCF%6I5l6Qnp~_jJ6OQ`M2G&9*ROPy|RomK);kyk9RsN4a zCsH{q(;Q(=bA%B`CRF9PB>k62Ua?51a<=PUrT=!x(W`_i+grEcOm76>sxK3&{9m9> zI085_7{g~mmH*S!31=^7HDljQs4@=jMEY+qcXu+O%DJl(&R))Ht}HX5%Kw$?guT;V z>wZ8cRN3A-;mX7nOJ1c)s4|M`Hte1DT4R( zmC;})T$#9HaaSZ0s_X@wFk&&1ad#&ZsvIjjk?y(sjG0j7>a`P&WR7sgbD2_&>yRv2E*+4>7j!TRHjJ0fU-75)IITp2quELBTyA26dIc9JKa3sp3 zkAx~)X!pvv-Ek$43=*oe-ECNl_EhJUOsLX!C!9a*sX3C7P-R=_Hk?`PkGV%lsM2<~ zVGDQGcUL46sqsQs3HyYEIdwv>q%-08 z;pmh1mTYmh#Jsw*P8>_To4p+NBKu&D`t-_jWUp-1&b#)7 z3HwSXoN;X7w){@mQ|-a706O6>63&U8aP0GLI%;>qao1Mq{qKbB%5-j5_7Bs!f0(a? z>Fkv%bDATh=ZI{>mZ8dYw~+|1cfyvT%JdvzPMxq%s4_iAm{TY0+p0{@5$4nh$17E) z=LmD^ggr`?={drjI$=LoWqOVzSAl^xhJBlG5EqQhB`-`fH!?dM8wwo+Iqr z(%ptC({qGllytYD%JdxJSS#Ias4_iAIA=(A8>&pt5zc+m-G(aDbA)rabhn|(^c-O< zBHeAMGCfDwi=?{^Ri@_%dy#avp~`e7{AG?%W!jv&S4P_sraNIoZ@#8G;i|#aMD9_p zOkA<#e(o;U+}@JwCEJqgt6mO&k#G&y3Dgmjs~u& zJK;K9m2@Xu>$?Y#M>zKrY%$*dZo`)7UPf+-dgXpe_R75%%V9h2UfHAE_sKml33KX%zvxM>D+yKF?l!z161MzIsIuR5!oFr7wB=_)l|8K!_8f`a+9g!k)4C0N zltgZc5~^&+-G;r$u{MvQ5~^|^bflKZQJ;h=N9|q?$94(ZaVAvd9_5o5Te$7G+mKM@ zNYn|(5AUG&CKIai=;N&BEMxELHY8NpyE@?$K1UzxEfcDI^4AHURNA|8kCIU36U=VI z)u^q+kuMXfd=lFUpBy`L+M6?>$|uU5@Cmpr!`_?;RX!Q-girEqSGJx^sPYMaCp?kh zD3M17300m{=r+7sLnNa14q)vE(${5~QA`_}SS=9+of*HNoS2Cf>p4JIZv>Ej~K4wCd zV`V374Yraz8%U_~#9p`INahHaBN+)*o;>U}Jb~%#>^PqZRgM^$(1z(bLT@|a$yH+p z`+O!;Ibw7|>-ugyp9xi-%YuyW+qg50>2YRUPffbL77lx6x9i>>y>>a6RPZKop5Gx z#&J%}geqs$P8dxXWq5Bgp~^ek2_r8fGUvohsB%W_gb|C8jO&C3IB7>9S`G&OsF#Y=!7G(Beio$CR8~S zb;1%_a@RYVP~~c=6Rt*GrP{_ap~@E830I1)AdNUOp(@8EMyW=x-v4exLY1wj6Rxma zdAZ)ngeuoQop4rj9<;?|LRD^8M%#{$#w*>1gerSMC!AUA!MP<$sB*UJHf+1LzC6N7 zsLFNa9OYee%=ao#TvMqGN zm6t0zV$nw zmFYRcoH}7&OXGNHDS!;=J!45q_Z}?l=AA{`dWXJ@+8nNS?I$G|98~Y==*lukFOOQ-oY`w9qsBrZPch%weaO@*LG#}Qt2YDJ^Rcp#za+>)2cuA zUTePEnmMm1(7` z7LNGWqZYa3s&YBZcdo8RVpUh#>t0EW=`UZ7q?)s_6XkF>qS954DSRkxm9BEU;n=f>`(Zg$<#L#>^gOYuQSJ5np^Y*9 z<;#&&b2fIO9PT<+y2^3S?>v0CAC^N^E{FL_&l9T})n30J+8EPcz8pz4XJaSI;ZAd< zYt;Uw<4!#&mt0jYhxw*j=r%@TRioO=g!DYID(xJx6Xlp&t65stHMtz-D?LxF>Pmb4 ze(bc3x%G!$x!TI*Fkk6;qS|h)Ikng8>TAV@&jBq{F8N4QUDfVsU9a?hFFj);*Ol}< zvC21l>m5v2T0HuvQz1m8z~+ z`s8bFH4x@I7gLW!%ozzd~3%edu6_Qn7tZ_mG5UZaqT;$<>C5ka(4yNg zUr(o0x`@X;;gB03R;tQrWv|;XUp>rTjl|0LIj=mIQ|Y4Lc*F@?ltWc6hxzJZ_G%w2X>deG4acN^yGsi#U8al)gYJH+iHu~Oxz&wTYTdo>a( z-?LYq<(gaLj6~g!TE*A(Ns;*c1A-^{g?jg?E7>SjQypyU|((^>ws9pJ74o~*Z#k(U><8H<3 zb-nD}|E2HR)!VN58fE6lawIC>ii&d-ZoYb$y&8#?@7XKYPM)W(bkPqxZ(Mne#7dP{ zUgqn%Ew8*rqHNS2K=#Uf^)P!i5@n;-nb|AbuBX*2UG(S99edPBtW>$}n(tiy7>Tk` z{UdwjdHlJ4J`!c4Mx44{={q0y`U`shFke@!d6ha6)dy>ZUDsFkx()Ny!|c^atbEU2 zxkuuEK~%cvFa7qo@*0ViD(|qE@7%u6NR*A*_sL$FuO4QvMxt!gep24|F<<}3q|!xy z=u=zOm8$HO`C1FPOWf;mb)Q%CBeB|N$U8;;$MRes9EmDN^})JcXPw`A${6)IGWh?_l`i7*M~!El zM`ESQV~P3dVfJbyR=#JiJfUvO&#iqVs;+8>wyu}G{jc6?x7Ks>l`i5vw;E>ydy%TV zlWM+tn7tZ_mG9c?{low0pBr~aqUx%~-MU`(-uR$#2hx$+_W@M8h)29=+-Dexl`4hUtBSAcdfD6WMO!_)OaHCV@1QZ9|HqJHIP=xR?A1uD zeAiz0$~RujjqM{*byZ_~T`zl&{KVL!9M64UMx~2*<`Ls;FcK?Oo(;@b53^SzvGP58 zo4^KFC9Ct@zrOG?o=Ie->N70ce8#M!DuiQ28O>C7e`k4L4J(-bMsq#L9 z?RCz^NUUt+*6y31jQVq=KN8i?YX_>Xm%YzjG?qNQD=P0sjYQd~edD@bHXiavTiMXV zIj>Y@8^-OvW2@5T)x+L2Mz15WQsubad>ys(NIeo|qh^`xm2WL`g_KuCBT@IGcA)Bd z+56JT<2>m8h3`D8bP*r=*bNaYRo?wDUp>rTjl|0L?3HhkbA^;wMI%vlRnPwGdfB`D z`^IwsKF{_Ybd@gRaZer30gS{-m7j*2uO4QvMq=fA_R9ar^~6O!Wib*}S9SKIu9v-Q zf9K3$?d+)+-w9ahA}%>;{J)}+SgGlI(U z?&LFu$kJyXzB9AZ<>3*Z8~-0?Bvz{YMA3ZpFncu;E8nwMzJ=7i#k@N>64kEixqMx( zGJW=#@fp3(vweqYrHgprUyL!_NUT)(X}J07VfJbyR=#Jiya)E++-paoY}AO8@1VQR zzPGm0bvHkK?t_ODLL;$K<$Y@No!h?}i7H3!y=AY=*N7uW86#0PDniP;YDRr?@$N{J zjf%W;4CkAfJ%5)^o{dEHk2=>^*UR2L?>+wKi)$z8d195XYpTBHw02#E<(1b+l#N;s z=KJCK65q33>7uWE#P~mPBe7ED^Cjl%XqQL0ktiEAI%luO`nln?^GK}fD%b9~<5f25 zT_1TjY9z`=5p#QF$-~}C+#}QX?3Hh;pLVE0xa* zdtI5YJ0W@ZVBElKNUT)(36}Xf!{u3RBvx~6_R4*H zS4eqPG!j);wW7;U0sKmcr?>Knt&ymBx6YB(^(xb^Kl2-gc-L56dY)M2n?phbkFfWEk#KY}869du88t_Q^BONR*A5E%V6W{)JD> z@>8;rC>!;JFYkW1mUU*xv%yG|jhdtKO4w(D)^x7jktiG0GV&8e_b=?hxz~=wYAngS zANC2K2;`>#BT+W$2}bTw&V8<4@``07s;+9am1j<$rOn;*kyzdH{3OoxnKMJ44Mw7D z)J&FFcRuHG7ccMJjYQd~UB~>C%vR#uo@eKgC>ymN$nDB}T_@y7eOIX5Pb zMA@i$FweE-tB2XEktiFr|6SKt_Ih2pXEit1jzr~Kb8X)7a(138$4IQok>_*s)x+%7 zNR*A*hs(8Vk8-BYv-3z)Kd(4C_d)ZWi$zDG%2Bas_R4(c>S`obb(JGz^R*Uo$wy)} zCgwY69BovSBbkw?a@5{Jj>6q%m>YLTqHNS0mG>FUR}ZsSBT+VLZBW-&_If{eH8B@o zjYQ>J@l{@b7?I7DVS6Y3B+5qZ!{pjET5{!`SKA{|HtM`!j;}mZGFOg~ zSd}B6NHJeM%wCN|*{E1F*RHd(yTf@$cqGb3JvYenx%)6inK`l?i5g2P-p#u>=BtO< ztC6U3RP2+za&~rwlvhO~Q8wyqWuDK?cP_pfiLz1gRrbpI(ZlT3NR*9=eRA#U)!bTb zB+5pu)p9u`jLvdIHWFo{&aGq{j?_MB%1@U@qHNSzr98Gf!p-dgj6~U}J%BtWIx@`d z?~X*-sQukMmN-Vu?Td~?*{FTdJi^%$$8^!vgZs>xRJrYred30n-EvrRJ)HAOm1XL8a3^exzHi6#LCZ0wS1l%&-1;${iLp#yzJKV+-2E_LXVE-! zj>Kv{*IqBjnD4oJo~1s2PXv%{jKs>u9N|cmM;~pd$~MM)t6fz@Cf!Ru5_Q++2}kX@ zky=&uYRtFFQBk;bFULq!IpzsxxVc$PRrYGkx5`l~FX>*6k*IRa6Gj7b5rC@f)tGOU zqgJWXy&NM^<(Mbt_5ifuPC@o+%(wb^?W9Wga*V`kpJ9%0mm=>>XhT)DG3HzMqjsaD zdpSm;?#Ddg?pNON(uS(+)tGOUqjn&rdpSm;$}vwk`{Y?o8>+Hb&NySbJp4+;iDSN1 zSCwy`)kb3FNhZenDH~fymLpL%)+4sVh9yrP_I5QAt8(PJ8c$Q*a5+X|W#g-R{%S1w z4VPmiR^`ZbHQs|8wlNYb8+Cn^&Ua7`vsXzqM&c;rZBkd07lDnsy%VEB$7P^g*SkMPL7a6h~sEs_QFzeZ2Dj7(5-H zPsEQzNB&zH@srBT+VLA1-@kzIvFw8i}&8_5Ws!|4~)_eCw!hB+5n+bN_?2 zU1!zIR}ZsSBT?n3cSY6pN?-piP{vf-b=KK@^)P!i5-Z>H3GDI2?+u^#8HrVVHFrjM zyZ$H6eDyGUH4;^hdLv9-uk`i*D}FplUj1Y1lQknz_oIlpv*+7&UetW`Fncu;RgSIS z(db%syZ&#&eD$!><>B?;XE_opRb8+2_21|){-;IV^Q})IjYQ>J#N7X8Y}Ywt^VP%b z)ksu1>U~soz0%iz)0F>9Bt1{8@}2)rq`DvVpGfmW+1UDjmppIof3@cSWF3jJvGu!< z+(VqRF%l~qb-mKp-*wh2>3L$6Z$5Ru6YsJxjyB57k>yBKIp+Vj&obpVZ0L!q?3F7o z{{uMx*Y8MFIktX}m-)`E2S=i8)E+?g%6#?kE4d!D?fRdA`M&{2qRLV4!_95ieCPVd zNR*B0ciF4)t-v*2)yR;?l94DIMdbGgJASxg&8yUrsCcK=&e<#T)x+%7NK`rMJ>=Od z_ei$u|BB4l{h7S)GZIyf+V{!-?PR`|=_|SKW4_z8)_Sh8L3TsB`l z%wCN|)lYqCI==~eJP}`YHJ;4vbu|)IS4HGA^v;9Z^}QnV)x+%7NK{?bcP;Z*l*Tt& zS6$U^MBZl@iK?q2@;k7dGq&qptmdnS*{hMLx~i{o*Y!$Y|2x$?@tq%6+uQZdO!Ku& z`RhX?QSarfZ_DO4d5&*At-bKu?s+$Ao#LD;lH9?=m zZP&Ls%y(|w9f>MOjl21aA>$VdYuv5KGCzYHiMr=S{VA=KQ|bO^1X=s{&t_SY}b3X%~ubz zS0hn%b>%}}v&e7%9>082byd5yc_(!ws;-L2-%7D>OV1Nkj*YVp++w_P-*~&eD`URS zaCuf6iPih5>w1-`zGKrP%kc|GbwBC}cz*6Y5_La{$lpYAZ(+N>Gittin7tZ_s-MR_ z;gCgLuXKIawAa=6#gK{_KC|a3a(?bS5*0JlbLaex5$AB}d7|$5M-Mvs;2Z&rU;3+i zUe8bSv(%BOdtOBTHlS-K>3O2=!3n3le2ev<=Q6hI8zbiHXqQL0k*IRi_YLxwPuz{# zu5SaHuPckZniz>H$JXB_AHRfI?P}})=NyS@S4HG+XBzKF&l6RS&p-5tE!NKF>(1>K zyS0D##LeiX@>P|+(#DuxdDusvq{=qNFK5;L*!pcGBT@IGi2SWB;{@q>qDp(jzaF(k z?Bo9Tc76Zcd{a&L`!N!$yH?jLUEf>pglF2f>-)9l>qwMGpOGlv>$^+&tGYf>^lPj6 z3#%hhHjdeMt0$xN&C*&AtiDECZMWX=m!I&CM77-_^4Cu#=E^Y=t8!!;3O2c@v!p_-N0ulB%Zu(Ix&xheokyZ<)b|JT4y5C`9%ipbqGHkd9%P;k zT%Wmm$t#wTsB+Z0Ca>YnGP8TJXE#O9~3BT+W$8Ex)SuA_V^m!HIqMA_Ky zrDtrhj`BXc+md%!Mxt!gK1^;U?&-Q)pLgg-qHOH{#nZOfr?!>M#XciZHY)bX?aDc0 zE^Z%*vQcq+o^i}~u78Zgs(;L_Cfv))yHR?gD)(B)K_iYFWsF4CRjoPedZlZ8?BkV$ zJvjH;k*K<=XOP*3`Od{xBe9CFvRCFiS63sks;eCJIiK6|b88=ovQc{{c|JGaxiN7h z%0`We*(+m1M~VEbXe7!;JuAwQrE!}hYNd;=r*gd|j>JlpqdxQ1!|c^al#PmavsXsQ zK5xm-Qb(d})N|k*Q#+o|m187U<;b&v`RZZzY9z`=t<`ev+N0*4<}c3(rqRU31Kh zyCYFHYL3b)7W37^?A1t=je1^N*H`xX=;JdFpXucDa3fLq)_J)6#KirJxpIuesvLRu z!+iBHdo>beqn_jB+BI65i+4w&Y*f6P<0~WNxpIuesvJ3{HeWr=UX4WAs8}@DuCw!Y z#nkRu8fE6lawKXjsr6vqvov4Jl%tH1sJg1yCwt}Wyj^QG^PP*YMxx45@l{?yT0fR4 zM;RkgHY!5OwX0X|e&rpnktiE=);X6$!sskVWFt{FY9B7!aHMu;D(^;(MA@jPCwXjl zgmZT|?+A}X*{CzAc}#R0$S3Bv!326P9C4uUbsEkyLg4hKb~1wxRFYhWR>+=9zOOR`a>`x>sYq-?-;~Jatb5 zkZp{_%ElbwNR&q(ZK%pN#(b+?RYWG;OFj~HpXUij?YWU!RrYGkx61LxJyE!HFULq! zIVx_SBb?#pW;IpW#+YxF*6k*IRiT5XOn8kma!RAn1uzEzG|rAqg5j6{`V zo|xMM(1tq&*{d<%>gV;OTDq5GBv$(jbA-DTd1pc!snf7F;RqTX8AZ6sA)zhNSI zm~H5LwlV%+d!04d`u~bXqI}<)(Dy|5a*V{vhGpu6`KFrQnpoX~%4ub<6Siv8`TZqH zwG)K4b2;=Rzaz!GROS5=32o&643kimcX1?YTv?6hp7_osSEYxUu(aCFgepDEgfp3R zCRCZugyop4U5Pmxs!V6E^dvvwH7`}xTejgnFr5kQtxwFgD^;emSE_8snNT%X4sF=y zvkg_Ym`rHHKA#CywwO$K2Tjirb9H5D*S9f8=*b*mpO_=oFNa>4&b6yo_TWsYn!D$g zBNMhh+pcY_dzA>UXF`?fIYJNT2>W~{RGH3%QCN)V(kEbSbzclX0G<+{=<)0xmK+gK)4&9y{*&xGTv-By6Z)Idneac(q%)yP zdXC5=gZZj5oo(pJ*5_Wo+&VMAe^;+mX)k-FC;7hzw4urtGiM|JAEY)^&DE|d-?*8* zQkBbLtJXv9bsG|@@;oSE^lCcWFkk1ZOsJZ(VLx}i$~IK#VJECn)A>24DqFbMvkmh# zoe6E^Q(;-mtY zyE(#gn4T*~{$7#gP-TDYHni?DL9fpd`INVLsoMI?cGYe@Mf85m5thUBTsi9hx$CVu zuRHmS^D?2zn(lZ~az;@5`W%t}zt+4|ZT-IhbwBC^ zm1lP62=BS{Tsf?TIU;|#*OII9-gFyUH>UFX9N~YpW@0?Z(?Oal^%A&xYoC7i(08MZ{4F5~RhFsOmH8Tf=JymysM21y;k-0gR}$8h^HsN@??%P> z{ksyXEK|2(zDD8sdn^*FwAXEU zQzwk>jQa97sw7nT{A;kmcMRs@cIOG>cIT3A!+edrjmR^hN_(9!UrTE(WI~npI$^Bt z(*@I+@D9!qs!V4as!V6X8NfAeCRAk`{$k0k>25;ftK5e9Qwqd@e z=Lqf15!+=$PfTa8RAsM>aebm>wA^h-s4_iAm{TTPvE-*R=B3IqWgF6SgeueBhIOLK z^c-PM1F?9>=6{BaU)aohmd3TJ@~)bMsvLzIb7~{UiH-)&GL9>~p(@+(7kjPg zZX*$1?}Yne?yBWptIB+{R}!k$x8c0o%VEB@cGGi&Id#IkZ0)A!2y^O$Ykl+0>vIXm z566{kLzVVAp>_LqCejMR9^7r%JN3%znNVe)?}Ywp!*nJR-3dp7l~!rh%rsM200RCyjY6RJ#SLa*FynIn9Hoe5QQUg?QDI@yLQTTCYO#NE$KsM5ns z=!yG`nNX#Nov>BUwGs*Q^)!CAVJvETj_@RKCRELNrOGoQ*@h}diB4FfrgO}o${ywQ zY{Ps_&k;sZnNX#Nna~qYCuKsFZ7dUd;wX^`RoRAZ)b)qgyA6Ml$g2qnRqn}U8}5tE z5qf1h+fe1X_e`j=KX$@=T?u)8j>xM!^HSwHG28Hd%n_Eubhe?&^WvFMWleX&d|d;2 zeU5NHC=;sO(aVJQV~(&Krn3!Ip8n2+Dr>qE=IdI`>vM!_+)Su)CoB`*kGylIzpCbJ zSaQ?ZD^;HU&V(xOO()FPwXE0Y2=_HIp~_v6Ot`NxM|jVrvkgmbdXDh)cP3PMZ#tp% zxqXI2`0ORyQ023iOt>#TM|jVrvkgmbdXC6v=&dVN-kWYi>qcZ=pCjgW)l|7ho^7}* zKSy}arLzr7ZhDR|F3p50?@cGPZe1B)cfxgsPxyTDmkCv#pUH$O*E^YTm8yrCFkjE; zWJ1;YHe3~F8>%c*CtP!Q*F5Q%301B|GNH;HgiNT)`wT{2Mr59Q?=~bNd>R6}_kNGoea*ok;mQ&SyfEb4e$hw>=&2 zDfUdL%JaE4oH;#*o^7bo!%k@3D8rNTnNVe!I+61A6niFA<>=LUX|Ap$tSjfM?v>V! ziajZx300P<6Xt6a?n(JfsM201%-8jqC*?DtN_(9!UsrUVl+T1J?RCO@T|s(MJ`<|6 z*9r4=ry!qJmr$j>ZX?b5p2p9FD)%)q;T-PVo(WY(9G%d*URev7P-RVb!syPZ&vWRR zQ04QlPB`itc^i>uLX~q#C(PH#+lV|9sMcsM200EV*qg6RPyE6ONXSYI$!-LRH@VkWiIpgXBpb6RlCxxpr0gl*DweD+z1V z^c-PMnNYR9jX6Rarn^_xyZ1cLAO7Mfn)i?;RB5k!<*LzV1$k8@;hf=IGG{{%Goea* zozUO8byOnq^C$^b)?4?=_|sTD@2W|ta!)SXFviV9@+3$4uKQe{<$f-q%6jYNknj#> zB6;F!vfD_WWE&EuGhttIq&D5XlCZUVJrk<34S%s+neH|c;q^|~e$3a^WGDQ^wNths zp-Ow*M(PuJY?n~w{$;n}EId~Z{k5l>?luzP^-iQY!x+93mezDGxvG@THq2K#6V}fh zp$+4?Y{Pt|=ZG8un6E0+b2iK=6Xu%NVs4{I%-G=9{(mre_Dlfk#w@R12O4rwjGoi|KCaM=zzHdGK z(9JL2uIy#Pd`-_0mT!)zQ=#>ZvV%_Cs^qHbL}L-3KkA0Q2)!!p`sJ7-*00?f2g|F6 zopd~x47r3=1Q)rb}Y)qBR_Ha7B*DXO20gO#GWtB_d95vr1Tt7>)Nuh z?@P~^w6Q*6i^*Q8GM$OqRVm-UxaaHgwXapR`=yh&sw-7^l;*t}PoUUvsPTYUNd`jk6Bi zsOO($W8<^~hyEd%AQQVp@8R$Su))RhiB< zRGFS5wB3oC!>fMkTc7pJv(oh?%$nybef{r@YTdTv^=`voQhJWiUMDP}Y3*e~mFZ46 zMwy-?RGH2;R5=o5LRB8GlJC}AwxKG=KFNk<$~IJ)Qzjfy=Ll7%vkg_+>x90i^c;~} zyZNf}dbZ&mG(AVm*-*8V^pQ~I*p+PD=0r%IjZAuk`(^ zYeP@wT1gtu9l?8DNvLuz>4bgTk-`3-2~~MyNTsz#x($DE6m<;Fgeq$x6Sj;T0qB*g zY$J`%j^N!Z302Mvop83a-#MdZLRFq?wV}#d$Tn12)1B}a%i;A*sM200RGIFCo>*5# z9GOsM#F2@))~;8!c0KGiQv8|Q?)rq*tx?n6tDPV$q3P^ZQtbqx?Os>@q9>;32y^O$ zd6}LgRGFT$VOz+ADm~1Eo|v8^zPdK_H}%I}4tMC%E?y_nEaOa+2~~OhVJ)tI2c;e7 zyH`n-i8MMpx^^29sw`6{oGn#JXF`>8a3)eMWWxH%HjE%`mBvKfhJ-3()J_;d8d(~b zW$PbP_x^Vq5~_?JGLdQ_6W+mWWBo{feZtauKTP*>>;z%` z=!xm>RU*8e303Rc$b`R`)?W82sWNfs#n0OH$tPaDeAkm+v$5;o`)=Iz+&7%(A@cJAf-3fnL-0j50(f9b9<)`m{{JCDABcwawFHgAq;fn{n_Pxt*zRM8{ zug?+Eo$!})?)AqPob|?!E+2K=Sqrbv5z?LTms9Td-*!Fx@~fA(e&Lx5ug?+Eo$!|r zUh;z*=U%$c?)`rDu!Yy>2HmM{=8Qo?DaW9x)c6lFY@{vA>9do zu|Il!j*#wzznHJr=LqRe_>1{^eU6asguj@6$*Ygs^}btPy?n*fUpbuQ(7Ndl{mg+I z*Br9X?oU7Vuz|2YU;egVS^Vye{&soo+YdYU7mxnc#X~;(p5-0R{PTmlSB^3-`qv*@ zJbU@R<*|=Hc;mkw@MDYD|NZ-xSMEEWJagCfoxbsoC;!0(xB1jZ zm&g3?GX}4Ye*S?Q@7>sE_km}B|H2mjw}15m8{hJi`|SSHpSsVYdv(V7|GMjO5B<{e zgy%fH5$4p1|NZ>OZG7E3_S=2VHD?@b3;);wKezE0@7izoefRyy!JT;hpPjJlfe*WS z`H|m$S?k+h|1ZD3>+P?-dU@gNUN(4j<}bbI{QvpnPc5&y^J$GRr*7jm-~ajx{{6sz zUY>i|%i0+AA1`?A1^@o&e_lTFd8Z9F9Lc0V{G0#B;@>X4Z26d9e&RqlzCQ6Uj#@nU zzr25W$gYDHm%RF@#k+6!{^eaSJ7^%h&$f&!9`I9(JN?hUU!HyE_wDNCu=V}YBM;m- zaZl@e{&Nr8$SqN??)bu^76;zp{mYB5+{!E4tL6B@&3|t3q3`|s<=Zbm^MdY`z36_= zyu*2SyYpw3ryl=`johPDJ^gV{+4UVaxqA7ucW*_Qm&6^fdD^ZI-23X~XW#RRjqa6o zrHAH}302vKEnIu~en_aw{lodrUTfd%<&aR7dz7tR+Md>JNT|v^%9bcS*P~R~j=NWm zqK?7#u1u)1Z+60QS9^0KwJJyLZo~Gfy*#!{sIsSZ8@5+l__0UsvvKL0uUVdR+vB(B zQK}q?x()Av_up|P6RI4QI$@7;_Q|uFgerSiw_#n`Ywy2tr;Vq5X5ZbPK6;D*BvhSs zoA2Cs)v^2SzRNc}V5nX1nl0mRzxN+5xbik1U%vMJ<7r$8RbM>s?=QIdLqEPe=BhCQ za0GBGh|y6fTR|Htx#lb$}zQ4*@&eBeEIz2z4Fw!HA1adx&PI*Q)oX5YH&uYUOp z%U3>ijIShAz5jc@bJsup%@>yMdctdly0Z1zcF+I$JMa3xU-^aQXWu!_1`?`X`$O;F z^|AeLwELV}j;-Am=iK*_@BOvKyH9)n^6giT^Pq&P`yKy+#pzFY@A4nt{-fvSc4e!4 z@UtGZ@y*|U)7_sw>{UawWV=$8+m%s~cQfBX303#M)7^Ld@Zt;0kNnYi&#fPO?S7wn z?Ba-LeQ1?f#Qv#zikzdd2zsnA6WkTgAN}EXJMn_7asS^i`#tb2bO2wa*U8A zRDJsaKf5^o|NP+c*vA|^*l=EQzIx%gFIpV=(!X2&)!m;p^jZm3H@exA7Kh&AAC|Yh z#}Pwaxq>i4y61b|dBGJIeSG=I7mVu<2~~$b@7)(%^{S6A&;9V&2ld^W&b2F{$};sn zXp}lP4@x)>{@5peV&ey&f0NxmbkXI*2xq?buH2&}RB5l5!;xWbypnLd%43P+xsk=8 zzyIAE_j>VuyZ3w2VM8mCP~|w^%VEi#8S-o(p(@vvvAQikw{{6tmp3*LTx;*h`*NnJ~&obhja4zBw*&UUJ3qjQ{ani%gV&nPGxXJE6TAV(NCC0eUaCugfP-X1f>&kh_7358pFIfEW|MlMGJaMkDxmuEEzRqwpd&n#ZC@xkSlU)b2_Hf&4w;M{8^RN2P54dWeGFL}ix zp~^U@+b|Zf2j^ZZp~@Jg+i-nmP3PK`Q02O<+c0jk<>%Hep~`r!+pxX5ddVvm301b^ zZo_fVs3u1;5~>^@yA9WwbN5`rd+z$P+c0i(w96x$gev2?Zo{>#QGbr~B~-b_?KT|s zZ5g(hOsL9ni7mru$!MV4kWiIlQ6oK9Ub*BFs+`-qSB^4{K6zx2P-PqIHXH#Q8H~s? zp-Ow5a0ala=Gv7|rM+&$h{c}j%#aCH-v3V6bL>%h#*t8^y>7!D4`+S*O(s-nuMF$-$y)E4S*a>@-ggJG>)@}=T_UVMNx_!_d z+zIDC33KX%qqBD{kBQ!ATc0Cpw_&@oEt&3wJ<2{|x)c6lI@|Dm-Pv0TTHj%FY6Q1_LXkKU)Cq2?epD+zpPJ4=kdy4)+eNMkMfuG3F+L={bhYZ zI>&JSvOXbg@9Ov5U*-sVQA&3kN#*rU=v7MZEunSO+RHZPh_o`zHWK0Wz4c1#mcw+m zA#HkZ39YAew~2AYc)*nSBOm`dpvi@u! zVY=J!m-S~g3Dez%zpOtyOPKC9{AK+pL&9{o;VS}+bthCwyKe78DNcz&M<1X3 zWkQut0Xt#Ou}Ap?GZU(On%N0QHCs$xrAnysscg4lPj%+>iE<`X`E0>gC`X-p~_PWopAh^yXO+#b5CD%8;&xLKAs%Ogep&w zbi#Sb`NI=1nNa0vm`->PoB=$^lL=Mcn@$+r*^YBdlu+gAq;A7m-&oy~RhdxbDXUJ{ zCyZV^A(jbM_LWXJ4jSEi(k&CJ93MO3sBas~twchVr}w%IBNoTUJiv@c#_gWK&x9&Z<9EV%$9OJB zWD=^3gSrj-gk{QJNvN`~bQ{in&OUj@kx=EF*llCv+S3J6B%*=TjzB*&jRMSmNxHXB-Jtc`R`cX>PodaJ=%r(t0_J z6WsCeKk71}%J`uZ&KZu{d8C$5*rBx!&nEY)h_j^D0$Bm2IrsFfMU5 znOBe!s&ZW7eKz*Z(W`_i?|(0c>l)W>dF3Uc%JojSVJn%N2PK>bbGtIKaD>ZIxP&U( zaW980&R&~m90^sozc5NB18uP3sp-Ow*D`#O> zkhyjxRB5l;;)nrM*rVrOx$Q3EOM# zgSPOwQB=bAs=Zzg38NS7b;4gH>^GTErH7qx?sN2UjOm0Ci>)%ZMDLm-QLZbaV*8*y zEqi4-vRC#yR}i_i+oM!Tcdv|!Y~l9DPS~R)%&8N$c3Zf!PbZAe?SuBy!6EM9AQp#ME1%%mD1ftQhB`-?q#I(-V$1$YgejFn^X77Us8H+2|Y>aZX>C@ z-U)r58?RKEHm7bw)!ewN%CtFk8>;5!4^^hksoPLBHxH^ZZBE^Ws=4`Gm1%S8HdM{U zC8|uDQ@5dNF1}J_+MK!#RdaEoD%0lFZK$#rdA$>=OwSSaBI#~JmFZ0Q%N(J~v^jOJ z)?Y`hzgAm+J-GgwbNzMg`fF!b)waI8K3{(iVEugtcOdL{c`sxA{gU>grrWTu*#~X; znNVd<>x4Z=BF~%>s_bdqhCNCmM*tG4Y{%V(cKXh300nq=tMfbkz1mK zDo=QH8|f5^EhZDHa=Y?Qc?XT6x(x|cp3&(X~tVL_rGG#)Q=Tke8=D{4jN~rSeaJOO4 zu}2v>XF`>>JCVjKM~O_R%4117J#NI&ZAhqcmgt0|zO#X~kO@_}u58t|c4PQ%Lqe7R zBhZOd4$Cx0nA03##E}VAIW9^6C6ZSx5~`f-x>xDHU2^m)q008wZ8*~#0l4bRgew0R zs1uF=jts`|nNa2bG zp-S7`hArG#-(8VRsM2;PY;i{3#zdJ=mHVJA&QZ-?&}~Sl(%Vj$uOpFkC+rgv=F|zj zlFo$Vhoev4Te8L367%ZLI&m!VZuWB6i|m6r>eDOBk-f52JMY>byH~2D?JJ$I-`T=l zGjzgVB8RZa$6Z^c_rDXiE7Q4M**{F@ z{$ainrn6V7%xR90o+GjiTZSsr-9{q3-U(ZVD${d>Id#H5q000eVNRW}Z>usrN0?J5 z9IsTFo+Heu6ZR-orsoKA>V*AVmFYRcoH}8wrpojjVNRW}uc}x5#w}jSH zy4y%9uV-Se9OfnA^=w0xyA+vFWjYgcUa2yzhuMZI(|b$kNlJGcN#*rU=&yak>zz<# zdXBJfOLrTpOwSRHQPSOpD${d>W36zLMcN?lq&k@ey(%ptC z({qHeh;+B1%JdvzFOu#yRGFS5>_yVuhAPvU@RvD4m1%S8UKwpmnC^rTz4@B%gsTQu z6S+sZGI7O{`?XrK?*(>*6EQjs5du5Mu-zWDd z3HO$yyAAiLB+RK3{-P(jt|U}xyW8-7NZ9f-p~`;K3HzFT(3YPGRra(_*mERuYnM=E zPwO`9Q4+Z&N~p3OcN_L1$J#uKN~p?x(2-gqM|~2i9JPBn9NQ&q$C*%-dz4RNY~i-! zZbL$qBT*+DKfHt9n@p(6qmQ$ivy8o~+mKLY@9Kn4_#Az#w@j$=$zLaYQfcqXJxW5A zPcXX;SEIHPN4`v`@=0tbd~)o}X>ZPiDxWBK!YAOi4104XRQY7Q6F$keUDSWK}0T31;+SU&(|jds-(v(Pq@| z_?QV*j+LFTHP}k>Y#^b^6MNl;Bbg&yj$|ZMdGfH^@C2r_v*UawR5@Z~LK~*%2)*ru zCs&Ob?DLsW<%rP_Qvf-nVC@K3H(kNc^Q!z2W3K)QB)_iu2=SzOsKM_ zb;6m&8OJ#>6RMn1J7F|ol;OR}gevcBCycy|$ea^1p~@Mx6GkjXGOiOcp~_W8C;Sg5 zS7G+YOsKLqcfy&)8OKo~6RPr9lEy3doVpDORsJVhC;ZPjcRY+AGNH=oqZ5wEj?~U2 znNa0S)Co&y$zAVcLY1qfPPiI%m1-NygeqHTCtNAIf;8gDgsL2u7^ND$djGo(301b9 zPPoEy<>h)O6RKSMbi!H9dC(S<301jW8Erd08n1L45~}P4op5Hc2j`Y3p~~5=+pz80 z`nDM1u6f$758V6ea^=F=) zB~)p<+c4U8_2Rse301a*PPp=NMQ7ZV2~~RA30t_ML>?I=ROxNEVT-fR=jc^JRqlhf zILBc7P4`Mdm9{%!zD6(7ov=?xm{TY8N;(scAC5lGsNIGw&X$-X8SBKc#JkyT*o*9g z#@e0GE6b6+vQ@i+us?Pis-*2Jov`28!gI9bFB0~ZZo?VJ7H-S$ggw>%nB#VTk#J7z zHXL6acbyqJ;b`gocbxBpV*AV zmFYRcoI2s?qssIgVNRW}uc}x5#w}jSHy4y%9uV-Se9OfnA^=`v`G1IO; zGoi|KCtR7D)?Ox5neK$vO=~X`s!Vr6>vJ}&chl!w@z_INam+Q#cRlGf8{I}yWy1Xn z(|_}~|8m}o-hJ)zT~8QoBvmH#%69b~Z`pt2*M8t8yYG7Y2W(LeRk3j#1YK};BzaL5U zwQs}rYV>mRzq;k(W8e45<;zbzdy6qqRqh|=tGziwmG=4_)Q0IfLY3)kV<*aC#Cpav ze`E2EM_ji2_eUK*+z(5uDwo52rRNA$+Us?t4byXkD%07QzF{r=@A z?|%Hb*(+7K9OgS$R}$u{z3!ES={Z7`>1<;s%Hhg%anBPMUwF-B%iG@Nh(-2FRW3)% z_iiUHj=sm=EI+;HKF<-VwAa1ThUqy%mFa9_?w;$(`sHvp;^%+o>lQ!$hEFaZb=+CQ z{m_Q0Tn_VpjW=Ll7%vyGi7 zM_PqtuUwzya+vR2T}ha)_PSRRJ7HsP{h?Q`qjEXSS9*?6rM+HPUn@3z4rrNj$t6_% z+tJ^3$Zza_&GHpbf8|E5E9p5xmG-&~pMcKYa|!SHU;pEN8xKAI#=AfB*u#c$n6LC4 zp-Owb96tN?nea1S^RmT*uKmFBnztV|tkon`9rnqWEpBwh2bSM@=AR$bZJ6&|Of6wd zt-Wr;eDyGUC80`t-G)!red2!BvCm%IWAUNov5!A^xPua^e)uKNS{(g%A6j0y@1X-> zzH{qA3D>pS>*X+CJU$^+fKltSGwU?bflw3m9FCTof#f6{v z*X+CJBe!F-gx(){M3CG z-G=#kI_0o^ZoF}i#f^90=j`v_f>5QsZo_=_FncATN_*XgZ1*2rbH)~gD(&@hn6DmYuOw7yuiNk}*W4OM!nNAN4tUNXPyO9%mKVP6WkV}5 zU(b|1=)8v<^6wA5X8Dod--=MBydvQa zQF2we9OkQs*((WE+Uw=;jNsfpgM|AfFFt&~#UDKCQ_Dv_Z!Ed_dRFmvufFl(5AO4+ z<++!Q_gq4i_If$YR}ZsS5~{SZdlf85Cu7vUKb07c0#U+1o>GI4wzi(GBhxr<1=Ezb)mG-&~^VP%bm4qtobsMgo zJWu`8@7r(VgXi6N_leIPS6&jTTvK%$=IgpGue>BwX|LNbUp>rTNvP6Zw_)4$wEDeo z@r=bsZvT(Vi?1Ael!U6>cFlLLe@NIrwAahwdHlJ4E@3}E@y}0Q{MPqey1e-EGcV{i z%-0obUZqN?(q6Y=zIvFwl2E0+Zo@qi{|n+K|Mb8M{`QVnFHb%G6&smQ<=#mr%y(|z zN5Xv=?R6XGYnifF5~{S4cir!h-?-N`oB#RNZCF>@nUHIR zEYI~p3H#tr|MF$K-ua6+-Tj%P$EeSd!T)!@-CeKP_4!ZSboX7qVLa9h=BtO< zD+yKF>viP`bz6RJ?Gmb<{fci}ob`=YEMNQnt#)fYH~)=4^{tCPd+-&@W3C!^Ywbm< z>?^$-=BtOo)w4{<(2i!g2S6_a3zC*Du|7_X#JBJCKglz7Jr((_Vkxn{Rxh z-4~uS?lVZJ%456v>S6XuLY4MthhA;)m$tB2Vu302zbUirq0xv^csvHfRG{>Jl9c*V8L&%SevEFI5%U&f7}^Tb{E z{jL3WpL5G`Hjq%|IN!@*zIvFwl2E0+Zo@Z_%&l=GT;n|W5vMJl{jz^tzWwTPt>&u8 z_m>=bpR*Q+KmM}iWpDq{;W?*-s=SUeUp>rTNvP6ZuPfgOS6XuPgH5I*OhzIzI*34e(>7!t~&qPI^}Vf?K*l-^m^jc$LLi;RgT-u*HJr<)Do(+*YCM+EpvsGS49%44%~I>1@Cyl z7nY|!d7KB`zwn)B`+fQz=fC5L*Djy-*IN;)+}G&kFkd~)UP-9ZUbo>}+>UP-9ZUbo>}NZnh^ zyMq#{zUSC)UcCCkE0&LZ!T5~c=h?o)^iMAOmc`k>bH(!9505dNgesqN_i~u89%ip3 zRB5lCqBRHn8hpa z|MBHxo;{vha(yN}N2qe$*303W{XKt|PtZxIdfUGpcHT4YdZXR9K4tumL;JS$9HGiS z-^<}MLBDUX->3fgf`|Xz|5{#k_i=WXQ0242Zo_=t3CX)35~{S9=lt z;EOlk{iem~!#pUV%4bvEhWR?f8*TXOG4GH zpMBQis)t>+{N6L)HvIpGvAXmep~@J(m&514ekO!x<@4cg z!+c#?x5~^I|b{jqu zw5D_IN~rQ#VYlJ_g*`a;S_xI|Yjhj-37-h$rvMVF>?_@dbDyi1yke11<($}U_$+Pi zo=bSoea6;pxIS}c$g_cjD%Wk@hR?a&#mhT)5~_Uu)os{HoZIv4ETJm5EAw@okRyEw zRod(2FyFcUAz}Z}Ubo@;Y%Xq>Fm89Po`8 z(q6aWntX0;AmQ4;wR*QT(q1oztBJYzO2YUmuRn~)=E@;qIdTkVzIvFwl2E0+UROp-uDtVVTS8TiuRK#S zR}Klw;aQVj4)fK+?3IKn?R6W@&h8H99bpMoc|La^#wasKmJ+JmpXuc=Up>rTNvP6Z zx8dyU3MsFOBvj@3+-E!!=9?p5C$};tm?}QC~H?4==tE9?=p=qI+E>RkmUMn6?(Wjij0*61}%JEcu)b34PCXYl%{ zd`D+%+}Uau<&({qF>)7i#Olq0Q*vsbQV zb2(bR+q8DphG{+Q*~U(kBkkg4 zuiSgf_j`Oejqg!yW(*Oi3nIYO1`Y-1ssw~8|JHr*((WE+Uqv_KL$_7=M(V~s{H?kZo_=%_A(^glhIzc zVZM5py^>I+y>7$*sG5rzB#eFL{s(Kj&Z?QO9%ip3RCzY8*Om3NU1y!mR}ZsS5~@5~ z-OJ&L-?`XF!uV?LjPQ2-Pn`McVfIQwmH&g+>&lblb88$4*J^WT&$sKmsQKz)_DVvP z=Tm!KSwGwLe-q}bhuJF$RsNqwFNgnWF}Kek;eN^7|7L90Ib`$I!|auWD$hOly7GUC zq~{1#{*O_&;XTma9HGj4(`|U(-2ZCL|H&$$%5&)5hI@!}HYBv+9%;9sSJHEYs$7np zc$bB7v{7b`EG1MKU-!B)U(1x=upyyJd)C;tOD|JSdCD%VuqhWWZ~%PTJlRod$| z%vTSyR}!kU*KOE#{m;Ps-+&UTa@#fEx&9$x|Il79hi?U*>*o^o^ZXuR^L53VSE&-J zwAagFzIvFwl2E0+Zo@s2?fSnW^L2kF@32Uy^8c86In38GxucT_Rod%>`EJ+$d6}=Z zkV`J1%Kt0t`&W72$9^}r?<3*9 zkNvUNmG3&AYr7J*UEhJ;Z5U-p&k?F}T;g7jtNXm7mr&&%P%p<$yj$ISuwCCuF<;A+ z-}o+}%5Ssuy7Fy^bNyVxexBc-=vcB{?_4%tJz$eAtB2Vu301zMv)7exJ)N6BB%D9;ds3Zow(FY~=BtO)RaWJ2&o1IPUr_kX{bISU4Ap zN*IggZyXxEZrAs5%vTSyR}!lH9#5|;-^T8qb>4NBP?g``?lYF{de64`>S6XuLY43J z?sestPux|_JE;$aB+-=D_EE1~nH%6SprRNA$&h5P%e(7)SS*nE3 zQuDU~T{}t75vp8M^>TPFW4pdFV!n=cd4!WtBC;W2O+%qo;pLykPZ5by>&k?GO zA9^|5|K6_epPTR8{gCh;_`UUB4$oX|*Y|79*O4fXJ`$?@UT!akPZa&yYW~8igsS`u z@{(5{wRrdK-oL!-Wd|*M+MA#7N~rR?ro9{zbLEh*9NC6nCUsvt?`%t`%HJ}zB}&f` zs&d;k7SY}up~@Jg-w(ea>FM8m;#Wdd{^q17u(#_Qhvut?*((WEep9j6m3xTpK<8a& z303Ybb{lOxzuSq6qwn!I%TM3^_;a&Y5~>{MyA9W8u3qwrMM9P9wr<0F;7&;1{g6=Q zz3DbwbGXBnchw|Rxfba*e0Jk5Mc$c^P~|h7ZsXV^_u07g&DSi?x$W^AbIkRzkK`UXI^mP zpP#(=t?#*XdGVff_ew&QXJ$HKzH@VwgmaYkx{a^@mtWuY_Sas$yzq4|Tlj=OKh>8| z^{@k;bI4PF_nOU|x((NW(sP6=*M{AOahp+1j$|ZM8P9baK98`bbL~o~@;ODfVW03x zY<~JGp~}9}ZMcr|sa$>%C!s2@qrA`Vw&Wca302+LpdC3CTl zgt1RGx)fReQ%&06}sgEh9MlPu6e=jNoFL(w9( z%oT}l10z#QES3`5s!LlIhnrJyf=K0V#j4DR0&|JDVccA@6bMW=sdTKfyRYw^ck;N- z-PfnrCwIT!F2?^(-k;a=T-Q18^W!?_eLnBEbH*%gmoRR3F6rek-&y~VuzzT;+c01E zvhr?}gevWI8;*lU968F6Q04fTZP?I=R2egN8^&#psCg8XP-Q&VZJ4hfX0Iev zX|LNbLiTw}eqth_D#z50=d*H1SPsYeUJmos!|auWD(!U}_Ndu&90{M}3ZJRXz*pHr&6Ml|#aExUbP|n6DmYuOw7yuiG$Mn#H>k#=AMb zGD4n}L&9?8nA&{xFncATN_)MooSly9A!wT(q6ACXXm3@ ztC{aCzLGG$a&6elVf|R99A!wTvfjE4y>j;}?|4b5%H@zSI?EB6gsN=Ak=mW9yc;E< zDv#}saPAK09bpMoc}#RM&V(w{nQ$b^qmQ1b$~Mf`v~>3>sb++u_H3k9m2H@>Y3W{$ zq?!@VaI;xWRkmTirlos1l4?d64a_0{RoRC5nwIY6NU9-mM9-bwTQKboLN7;B%?NiX z^3H@dRAn3HYg)ROBdKPDyI*<7OB<@P4f8cE-OG_wGs5{Q&p6spm2EiVn9eg!Qq71& z_qs}|Y{U97Z7p;gNi`!9eYQ3%`OJodzUR8~KR>4P+xL6<y|M>qLe=b^TaHZF`fR(lvF=qOyq*bF zre}m6&ItQ_CRCZugi%FHEW6bo(acS$3d@mulz;AbS9)tXTp3<&xn}~Rr}k>guj^9UN48L zlCkD$_H<=k=LTjP{Qcdbhcr>&R3aGHM3zqcfQItROw+StWneXIj1UHxYx4{^EI6bZRAs7T30o* zVJq>g8`&#WGaHt}Q6k$=rH7rc{rI%U>odYL;+asDPkC$I)!mG+9Hz5Zs{CJaCREuU zJ7K;)6ZHCw$fvx`OI1FzE#du`5thUBtQ`5vS$d_)n(j8V?$c?n&xrh=AM;X`|G^{S z{g@G!!}P2iz5^r^s;ucwXx;M#UY`-3z|VxLe1=}@o|^RDWE z-gH9io?G$yjPR6yCRF8f`dasd>5TB6OJ}bvx#=0odZC5@bSE z{x5*mJ&ijfyyw!{D@$&AM)+O2OsMkSbVBRKR9>GE{;M?;s`7t$weHE~8R0#b&R$t^ z(=)=jG!v@4H=WSBb!B|r30Ene?9V&&5~}h(oP;XZJJ~DOnR=KBRelj86RKujsd81E zZK$$Lop88x>j`oc=*@pT0&eTk( z(q1O?#BX9{LX{qN!noG=W#ouVLREgdn1m|tP4-Gvesha9R9U8O!+Lie?%OsKLhleDnNa2P zuTD7X&*F9o<96qgZo_T`(=$SQGvcUh=!xmre}mXb;2msNH6bMN~p@a zY7(k)6rMcEaiXJvvy9_PFS&$spL8bd6Q(<1oZyN+#|)}`&Xu3BNT}N1M&2(;bB3ct zFS&&G-0PW8m2LQo>3lzuDigLiTf3{5UJmQs`(Y&82_x3CBl43UZK%qTzUy%BVD7aN zu2Q|5z2p+AwATs8K1XWPneaZFQzlep8~$RiHQj9_!t0%IU(8*#+-p^tZ}v(;)&4e| zcY8U^*Vb-&MwnA4%*)npdPbO2CtT~BZ(g5EIDR;;WE-lq*9ooLw=78JBCjSSRJkXYZMZKsBlOC2wxP=N;+arof9!<$x)SpGjL54y z^HSwHG28Hd%m~Y2I@?g?DfUdLvZgy>zODhiJ|o-@%7iL+^fKZ7m=Tu4bhe?&Q|y^g zWleX&d|j(~eMY#(&4emM$lAF$6sqz$iCRBNEI$^%9WxYNl+}Fs2 zDtARP;l9R<@SaO&8@|Ou!o}bBtD%U%iaFwcunJ{0^=ww3G{x)0{XB(<4Qzu+=c-K7Xm0!5_b=Ogzd(VU_%ajRKdYB1S)x$mf_?b|py-uWj9p^Kl%DJQy&fA`+_Y`|3ROR_x8_t}bL(eu;>0u|dZj|BK`b?;@ zOr1#idhR_Fs&e$|yfmvT3G2%Fs(Yn%qhin2XF`=_>V)|kg?myy6RNb=3G;P*=1KWX zsM201%-0p2XX`VeN_(9!UssTxl+T1J?RCO@-6_bY)g@GEuiHqozNhgsp~`)YOgM)- zw`W3?5l1Jqu2hm0WCRF+Ss}qj;M&3r`nNa0i(h2i5@-`yRgevWI z!h9{QwU7x_+UtbYRk_~DgsNOu=Iav-pJsI%5~{S92}^Dp%Y-UD?1ZDGqgviul2Da* zKO|J;*&un6$3$z?bgo@hJ|!{T>q^2JH9aHDDHE#pw=pBMVY+)|y?f8|{NXQ-qInNl zLY4NqSFRd;R*+Xk63!XUB{Lg(mxBN!)=`Pb&!Z$%S#RAd<4vs^v0Xxy`zzn*hB166EUoEWa#bmvZJ4igCaj+sp$+4?Y{Pt|XGD$w%vY7^nGJKwg!yK|5yzR# z>)nRGn06%Ugm=w!_R5)F8@cT|B3lmA-K#`+y%Xl8S6=UgC7j(4Ri@3U+wlBV+K26g z-##<#m(wz#%5*0D^1JpjVZNqk#QrvZ``ORD zT-DEi(_gye*MH=}d;Z`zet3rsReqbk-w*2~rDud+YdzZ0KkGl< z^YAZS`tIj_$M>DQ?Pb4p&wu%)uYLCuKJk*1m;cj`-1FSaE;`w5NT|B@eOH`(;TQb& zJ&k5}M97k}4C9QKn~vD$~6jDPKpT zOsLA^RkC4?WE-lCI5J^rP0t9u&4eTBj8J7d+fb#wPFTW}o)NjVo3ARbXB*x@(=%db zL)HE^j)Ty{UUIFQp3MfTq_Yj#B?&vJF+n51p{}*(dVoBcaN%E88$y zGS!nn=oEJtJ#s*LBl4aY&-mFXE_PMJ`( zzl}`z%l_-y{R!<^re5--%7nj|Hm7bQsb++9ZtY3s^~cgHeSdsy=*g^=r19Jlyw{b4 zD(8|;*tZ=S?Ejfil}CnDT5F`+@E1o>$KXt;vKBI7%g7OcUa879(&+36-o27g<;>6t zXG{B?GioMO<+)ZHs;q@3A z%!Hnpo)M3)4gF31v6sUgy0nYei8RYN6JF-ZiTJMMHUXJ4+tRFow-MvbL*E6ANe;b+b z7t`A7UL{o~KKggR^2B8ioO|yDH{N;hb3b|QVfv$2U4G#8Zo^+BOlLyc^w|vPknV)Pyx_f; z9@==@%kMhr^%)`E34giy#+M)3IPuKOPI`StNO!_t&j0f7K2i7lrPtnak=JL0bSM19 zUgY%|A>9dou|Il!Mo4$UU(DC*GeWu({$jpfpApiX@E6k$T>8AjHulghw=X9-v~K#s zyI*!lyyLbL3t@l0;DMh$97{g*^f%u9_FGP;Z+`f^ySrD8GQanR-#8o@&cE}*gMahI zA2>|E>yq_+iDTc7{^sd^e(q=9d~nVi&pY(${ZF}V@#-J^@@apsd)aeO+QPr|`@iLo z_~e(ar(VuE|H{K(-g)_VHNu>FIbQMf?>y{9-}d6yUStdZD}U?s_20elMHhGC?SJv~ zT>CvY-P-#0t3T)TxN^;nx1Q|8(+;8s z(=WYkvEfK2{T2WGe;wYBTQB+cg>Za5@5euQsJh{tgOks??{xYlKX$MX-e+6Jtylcw zA^og3eejZA4qM+hzw7kfyyET?2e~Ec)sKDXv?mY0dzV+XSIhC#r{8<%)kW98^*!Ay zd(k(4{j&~h`cpr2+d=M8s_y-=)BWzlpV&p1m&8xJ^E7?s@2{=hy3#{)>gAA7m2KF< zrStueP?h_K^PRoczS+wmp(^(%Tf4M9t=o`Lm3x#eQF_*+RN0QZSB|2N!S=39sIqT% z!f{u7vyobrqjtApd(~bZ+a*-l)4C1Yt1bL@UUc|Nc4b!D&pwadQc#2rul+d#IQ4*>??@gB;64$(8 zotTbBrbT}olD7WeYV}_U3!|h^5bjCB~-og_pUy) z@yg5B)^3Y)?z`-wUpT0H{x@E?&Vv%FzW%(QIk@W-@&0RWS)x8$iLLUNf9|_a6!EHS zZ(7a>OQ_22%BaY@neU*4s;_?Q>DqnmE$cnEe(bduz4t?hb#=p&*4RfvmG##9pmCHl zzz05d+Q!58tu`c7UGXArPoTR`l-v`ci2kqyK((@jkB;(@h^PieTT%Czh;e)B~<;nfBk_&;`}#X zxOnBf$?7s zQ1#OfogUk-e&5;$_1&7zwJV{@GW9-alscOSC7cI;?mvI$;RtvC&%SV351OyNEB7b~ zRod(2aAcT`R}zj_c`R`}H?sJWFZi~@dw$|Mmo2SCLY3ouFNY;}X2`RFgsNOu#_G2G z+}b5nJ^aGck<5c%_m-vP`fgeed&!e36Gj<{?lvUMH^(K;ORiY1zT*!MM}}K3TXzj4 zR5@RDuWU=M)&BeQzv{4Ez3uwDmfDq2WgF`@T+tZ~geqg-URTabt|0%@zx{WI^T+qU=E=+aA)(6o zs@rhY=nR)g^_exDYga;*>$YygxXqTITf2lRq*p#&g|e>*bJe^wC}?{6)fklL=LN*a`QL9DN*PI^inLcAQ(Hcg=An*Ok$oebAnk zy|NtHEBl=@XKwBG4^`6LE2Dc`xc#vc_9zK+>V&P`7VhlR31fBppgp(~&V3T*)Cost z?^+%cz0bBjN7Qb^c4b>K-3fb?eZq7n{Ka&(;V&~nm1%S8Uir)ZgtRTD+whnD329qQ zx8X1Q6VmpTZo^;pC#3E3-G;yHPe|wS%3t;;q;rq*m;DLp+|T`Ge?mIPaQ?DCA#LyK z_uOA*guN)GyN#ssdMETMrO%eox@qlY8#5xUOtXzdc>Qd>(z@j^ooz^)K3hWTDcx-( zmDf9=zq8h!29dov3GfWMo7nHL~N1YwAv@l6KwR3CBeD0NkG>Y<=!!QK=L5C}*EMt4XM`cXb=qkE4%I{xYG;r+}TX=h&ls zf|&_bKF#ceqna%yuTmvc`Bb*su%|k6`b0Sss(iZK345w7!zbgJP~}tdPS`SRS3cp- zgesr*cft|Ck-?J+nNa1ag-$qr%|d{!-&Q4F^_N(syv0*ZP>S++dV;<303y_PMEK0 z=}f3H-3j})F@q;sGoi{p-w7k{Sve&1-BY^VhEawsCbtp^Rh~BPHeAISEqPKp6RJEl z-3d!>r00q6OsL9r<@lk!8KKH?rF&&8YTWJ#{7k6wG=3+HcZ}z9L?)riIH=pOPgthx zm4qt$O1I(M=j@Yb90^s*rBx!&nEY)h_j^D0$Bm2IrsFfMU5nOBe!s&ZW7eKz*Z z(W`_i?|(0c>l)W>dF3Uc%JojSVJn%bsNqA zu2^yuE}_c%-)$HnxuSCwmI+nb>x3hKBZIvl6RNb=38U0mua&U9=00c(pN*mtwpZ=- za!44xXs;9gB4JOgeueK)V=bT{RwH?ShwLX`xDZ(v2Md( z_9vw6UEPMi>`zELN^~3kvOgi6M>v1kpODTy%3t;;q;o&_m;DLpJo@;{{)Du>tKV~f znGyD)lz&Z|*?6VOv^jMf zs%GP^D%0lFZK#^fAF527Q@5dNHV>*YZBE^Ws@Z(5%CtFk8>(h;i7M0P)NQDm#aF6K zn^U);Y8EG|GHp)XhAMlJ*E^xg^o+0E(pe;WWs_bcl7uSv{kjcDYKa{6NvLu+vfFTMm#`gYLY4io6Y1%j z?Kl&v9Em!S#zF5*CRDkr+6iYhcbe^8nNVf#>O^{~Yb|6#l}~;<;i}q}VOz+AsvWCs z-b3xUl1B!g%d7H9eD^Ay7O+)jLX|7;PNY*0w&_f$@?=CO(&>%d5+zi5!lT<3=3ahJ-3-iB34`I~!OFnNXGM%2sV_H-_&vBvknyflj1ySf&|a zPBX%YBNM7}T$27Jl20>gE4$2 zRQaE#PB?oxs~P)dLX~lFC(^&c+}+89D(9|FID0v(xw6cJD*r3j345o#*8PA?sIt9v z!j*|Dmb^-pP-PU=ZP+{QwZr2s^hWx52U0?n zE97p&c4f=Rvw?)F9G4gY7;D+yx>pjaax7{IU4h886;F`yW6l9?WxWynNX$ePB?$qQ*$IEp~|+oA9IhAP^Il|!xrwW@2*HD zRB5{twm2hiW1>u`%6-rl=cr~c=r$x&>1`*>*O5rN6ZQ!SbLxa%NoT_G!_g=2E!pC1 ziFtKroj8_wH+wnkMfSlQ_34%6$X?m1op>s9c|1e(()7dLk<}@Ru zXGFGP%TQ&y+en1hJ7LREWqL-KQzz^bs!Y!abLxbBTb1b`|&r z&j@qsg#BEV=^0^8oiJ8YWqL-KQzz_es!Y!abLxbBEv3(v(0WRD8%gE$Ow7t*UJ_o< zHdMJwkqK3%Gcof@m1#Z9HdL8DTS8A#y4y%9uXjR!?Gs+_geucB!oDrtZKyImBOIfo zyA4&QXM|&|bhn|(^o(%MknT2AnVu2OebU{AD$_Hh`E8>&pt2z!xq zx1q}PjIb9;cN?lqXTo1*geueK)V(s=mN4B3BYN{S-3eC>t|oGia%JL*CHHf8xn_Gy zu9s{}uCIDI{6)ewTqj)j*_K=CzBRd+nrtXC6a8=TsaINni zKpx@TPq4*!|GN!aqI(&+CF+&?CD|+YUMz?0xO-)fa^ENSC<*tLq`M9GsU*y)6aJzn zxvnHsX}jC-en{BzGoi|U(+T^UebAPl303yAPS|rKa%-1RWl!ri>`@ZAB}%BW9d{e{ zBFEZ1ib|-;ebA9wB1e4^svNa@IUL(1Y{!{Um3x#=Vr=2I<8DJjl_OCn96!8+-kVIQ z%A=37nzM|(tJ{!JW$)^QPxu^tthY?4^2uK(d{Sxe$~{U#l}|9c4OgSK5=XvFsPai{ zCwy}3%xQ1VgespXcfu#&whVi7CRF)kyc0gjw_VwKGNH;R{GISbf}=zp86;GBQlZ;O z_uO$M6RJFM(Fsq8ICkaHM?#e+N4gD9x;TG0Uu8m-Cty0^i5+JE?@cCDd7nCAi?byf z4Q4`>Cz3kh2`XcFV~I?t@?=#fJPBs>VqeLGDtlTdJke&<@A#MrRgRUNur=69@@ycX z$`gCth9j9HT#jTURC)5S+wcUYv$NxTCR90MWI`LJXN2B%!jr4U4EFg)VJek`GTZ65{h$9oKJYn1k|FvbT<*F|esyr#(2~Tz#=~)YzP?hV-UnCq?x(x|c zj!K>Iq`h&wQD!Doc>=!^MqWl_#zC1-Wfav3t?QM2B@?RbX`OIpamH~@%!Ddu)J_;p z7-e{GGNH;l+X*8tBQod2OsH~3?Sv7Fk&NqvOsH~|(FyR9Wp~`=vb;5tnx#MB{kO@^rADwVScBFPL$%HCrqE1*sOYV9n6RKP- zb;8xCt5n-qCREu%JK;*v6{HbICRF9P#3(301kSoTI#Jj``gy z301l6I(xZlw7qp35~{S_32V{yXP%uURB5~0FxqzY;=GaxRknprxbkvEXWW$uReIYA zTezb{9vLK5>20@Ri?h$?=v6{h?t`{C$6$Lw_ew&QwmV_IMlaHxuun*sQz!IFIuni` zjy}$)-G(jBmY5?M>%_6dyV-5ni|m8O+MUoV%aOgZRl9<)KXx0cr0px6u;1CjbF}0y z684pD!x_gGZp-h4J=Ol0<92_Ma8B$t9A6!Gof$geXzBfTobQC=uI<D&_S zQKoZ`GG7VP*(+7%G$W*EM7CjDQf0c^NQBorVOvsVdPbO2C+tP4OwR~&>V%_#D$_H< zoI2qMr^@t5lFI9un3co4B)r~jxG!ef^=BqjneK!uQ`6eZgeudW(7I{uWkQwdPH25*!+KxS zci;8IdoQ^0&Vz0vsp|S86YgKE>8JerC*AwkU$@#ws=EHjgkIUMDqUWE^s38ultWc6 zhxux6PHgp~z1_8Y*#qYu-h*r-d6Egsv8Ernu97O3Ln3+DOTH5Sf7I3L`%&DFKPfi6 zgKPRI+SQ-hs8OwI;gM_Cc4hQZ=^`Gy>y8~`qN>Vi>yN$Hny>ce#Fnr2vJLIciLJCV z;yB7-#9Ha{@V+-)y4(*-t16ene5L2aR#)2V_d^?N`jN|#R5KgLQ4Uurm445u`|<4O z-*$H{xvE?a^G&tTZLGvrquR@a^qkmAJ0p&x9Ii|&UA1uK$F4Y;ORg%H!+d9TwGvxh zX|H=Fv8Ern97#2^aUA7vH=@#2jt8%L%W^*~hpJo-^Oc?xTa9Y3-w$o9=|?U{Qq61} zM>*Vqs&tj(Z7;uTxgVB8RW675O3#U{Mzz=Phc?#qBbOtoW;Twa9PT<+y2^3lnU^j1 z!*Zy~4$E){UFzs^qko8&0hHgbau~IV!P*cz0&Wv?Zi?J z^Oc?xTfVs^`s~+d!j-Nb{K2QcaapUa#HOn2mA?7m^A^HfL-*i@C%X0O{YUp>rT zt;FX0%q!33RJ!PQUjAJ>%AqQk!+iBHd$kf1%Ghb!jE$ z>zT4j7x6teJsPp8@(R*?^)P$25}WTcuRKjx>7s8w_qH7+SCz|QzIvFwT8XW$wAb6N zX9Q>a3@cGKYA3C(SNhX0T}y7ho>i=L5m*1>de2v4Q`Pm&UN49F>S6Y3B{tu)SDwI> zo)cR+DyK@{ddasht;CXRZ%%Ca=DPCisb^9vUGxp-94ua~#HOn2m43;Ot%Q4sGaD`B1R-$aw-do=HvER-1eO97u)SgxDwf?ho z)^=B-Y*fFi>y^I#1=k()er}W@Jtww&b6nzHkE{EpWNdYHXhiOu)yl_%6~`MI^PMAcR8(AM>` z_c;&j+O756e5H%{_4C%*z+R*(@1&Zq9%iprV)I>ly?^+R{@J*@5>;0psIuY^prAo39>buU2C7J$vOF7L0@P?7R|HR~28?^|JSWZ@PUM zuZ+mjzZLo&T#3rJ{`rt&IP=xR?A1zazUTi<_{NLb*uD}~S2ecR^|E)>$JZX^c<%c$ zDqX}YFJEVamDp5yHZWg3%wDa;=6m+aH;~NMI4eYp=I<-w5Mwecqv8iJEKcxj|j8lE3=eo0g{l>#TOi zQ$P1`?#naIN>n*&ww%p_dYHX(9#oaRa&Ovq?^L=xe9bNExVsXYD(`HYuOn(6MOUJ1 z)C`cla@W8&u~oY08=kc8$*jbt%KHqq*O`r#*lgt1?wg;C`g5ee64lRZ2db`@y@&5x zOP<~pm3O07qHNT@aa}JPFL~Q88+thNN>#RD-0nNJDqUV(@%A-(U5QPV<973P)XpRI zN|cS7WwKYkwagV#UKOoG-H+OVs_SL%f!D9|p!*lT^Q_WE{Pz8iMr^9Q`(eI%n7vww z&G+n;Z;^9_lvhP7QFT?%{_A?#`{eoSIRKw$`wqHF7jfMy*K+_Xv8nRYaP!r}?A1za zzGtueN3JI>@+pgzsJg1NA9cO#{qG;WV~IsQ_2N4LD_z8WH?IFHT8T}S&&imt9%ipr zV)H$FZ+a()%CLXtk*vwe&MUFiRj%E2$E$2yaB9aZ??$ae*_iKYe)gt*k!u3!IgXy2Be6I;G{ zWbm1w-#4gq(LeOWb#`8fP35z~URUPpPDtMUSc$SxJ2%-YpF#O8h)UNC_s2i+s%6J( zB{o%lf@QwWaCuf+iEXaUUb&C&3MsFOR-)>vR&@C(fL{sm^j1ExwGtKY);Y4eUS;}$ z*F1TNca7Dh=fsw8j`V#F?6*=XUA6G83)X*Hti-0u|AH`IR~C6Su@YsYRzle;`?j-B zo^e*9Y}9O-M+WyVd}5ZLlC4D9s3&}R_rtZUGee#YR-$aw9F@7%3K*{EH|{FKaA;@qBR=ancMwI0ar%6wfX zJu`PL(qmC)oY_3M z5@n<2!8}JfbI!)Zl_(oE59YbneDyGUwGw5c_P^`;X0O+kdsef#b|os`nrripm$UP% z94oPvBhTmNtB2XEl_(pv50`7#9_37(XXllueqM2O?t|t#i$zzW%2Bas_R4%`b+r;( zUF8Vbe659C@|D=e#C!*hqm62EB(oA#j@nzuQMmgIvvGGN%0|smd7r_2^)P$25@n;- z26cV2*ZaAviCKKL5|wYoS9$$mL^dnON^IrGF`W78VfJbz%0}(Obzi%uRK#SE5}M~<;W*e%vTSyS1VCADi+PP>+J0AaNZGKiLz184f1^MK8#Ujjx1NA z#*&J6^Dd70>S6Y3C8``1`(&@2on0a2Rnbb6jXGPI=X3L&#aAm)HY&c#URghSn7vww zvQe>5u3f#Ft<_edY}8sUmqWtnEJtK3Q8wz_O19xh?USbbbZI5ZMx9m4W4j~VY!6^1 z%0}%0{a}@qEy7tm&=A5=<~ zp3Lrt`8tc{nR6w!`CNOw9BaOJoqCq~s#6g_wy_eMjTzxclt&+JsLD3he5+kmL?+!! zz7loU=7gj6Y@}9|y;}3Fa=h(S6fWJ%u@Y5|irZ&|Gu&)eQ>XhT)DvF2O%qjsaDdpTC3?#G;P_bcysX+u@^YR&hsf1KKZlCDwjhdd6;eJd$zIW`{?&$CARylz3$aY{HcvxSKjkAeH88LPi^d; z)oi=Y)OmJZiMngMpL3e8bZ#XpvGu|1mBgCfJnX$Tsb)5gqZ~$nv#3v1E{FN1TIhAP z5?hUGFB8&pqHH|=F}1$CvY4$VROOPJulDA|R#)2V_d^?NdRv9piw zy67|iE0a_+8^=+Ow2PNZ?%rE2hxw*j=r&ejt5NM`LV8Y=jmLk_Qs2|=a4xxfy1C@$ ztGzj~)s^N!+d9TwGvxhX|LC=#G0*`O6 z4Ns(4rd;xssJg2C-F$|{e5L0^+1Pz<$+Kg#d%hCeJ)fUEt39$hc{V4?#_qFfp2qc* ze?Hm264lRl|L4biXEF6kY-@w;mHFynK2^LDWn=eQXV05^!Z@EQUWvLNyZ?7*zO(h< zN|cS-1IS*PuO3#q=rjMLxDuPHu5b4Gc;){XJRP4;#IHon7vwwDo4F5s;*c1%(p-p zQytY=XYxth-Kku^=TYNP;BYafva$PJNbVudY^=m)qpnx_%w1=_lAaS=zWLPs zalFgIINB&PN0uv5<(U7s&obpVZ0L!q?3F7o{{fu;^}7;Pj@|F^GT+&Ha3#t{?a5@X z%vTQ|%k`jb*MA1)e*>;Wm80H=o7=AW&icnnl#S|l*{k)fz%^dg$dJd9l_(oUwa!|c^cR5|KB6tNRH}%Mds`NOy2idi7H3!`{aK+nXhGf zEcboP_o)8oWxm!zF8NASUDf}*>UyQmeDkpXwUeF`TfX^C2-cPM=0w?;|F>^u!%U!C$F}Uw#xUQLvyDL%o?ta&#>j77jc?G!=WuyMXn^zOYpGM@fC|p&JULC28OLLsK z5><|h6Z6>aF3xQKY9-3X?)NR(?`HcxD^WIfznjW;ozL3tN|cT2AM-b**F0EnN}m&D zWB0q&-Rp66pI7uNv90^_PSJ6^TittbRNqQ5U(1x=_`VW#PP@MDSJx|j=J)Q_w;|R& zzxmXg5bVKygjbuU4Y!r@l0u-vqs$h_AZZ{eOd%sJg1S zB%h&o9z3e=6`8LdX0KLaJO7`*qO`uzy6WocQ~$!w`wS~lbyfQp`5oBK8AtUlR`b=v z?A1zCUDa2)>w2Zn{7&_8eCNm2_EEhv(|j#c{`$~L)O$JW+p_shp6gprYcKqvQ}0I2 z^T$eT^GAM9sx!_}ebd5x^)P$25>;2Pc=2m5%3s=8-xgYZup*iK>~1Bh4;GQ%FKV=O zRNw70Up>rTtwi;^-QUjiN!(F=o5Or(?y65jd^|rVC400vvo>wfI zzj0{vdQ{)XF<(8*Uadsc)$VVZu5V+ny4wBRU?r-q>bXIFf4k3Ej_N(z=BtOOV5ca$N6_&xMRF>-}tD$D`URSaCuf+ ziS7N=b-l_|-?8aY`1*yTx*zofJU@3{iMk&}t`Zv(n^lAaTF55DK7TX(DnJ(qD*-xx7pN4q@2twfchzHgAfeBy4@QGFZG zd|g@O)x=6vId*@WeEkw;wX5C#YFvqGS4HG+XBzKF&xtC>N3Xhk$J*I^-MQVdTlVE8g8_7!4{U{=TYs)x6dQMbnul(2*J7ORA zzmMws=jNMgy5EnL*zQ_guXKHHy%U~kKdSH7ny({K9(`7#e6Q~=<*(}cMA5IU<}a+S zMA^9EoLx^w>zk$3qqeV+R@<#N{N*RSD^YE?i2U_aiCH;TVk<|sv3{Af%27}C^Aq=# zsB#pMzh!Dml%5k+jw|jyagf`tv55BOMA@i5kt3P)3zAh=yZ>3d5>;149<@}hf)?m*{V=ancM_5H!T1L=6KhuN!@s93bV2bpIB*JrL? z@``08svNbh$*Vi>fjc32_hTi>Mtv_e-$Bh!${`(7o2`xSAB5zv%8h3K3GIuFME|!_sTI!dQNQl<`K@3+DNw2 zMc0?KyH_i*sq)zF*_fw3{ocd*>Z0r4`kqP`eetPux3LnNDxa(|-`O0s5@n<2+U(V< zKj+fJdw$K0w;n#puX}#YsjsrU!C$oO;P!14_?{E#JI?G;TAh$&t)T z)Ohtx*WPkbj;VbfVNK`SU5V|U=cgI=37^E~r>`qfHtHE|?oqCzd@7fp#H~cxc<7ef zcdVnl&+fM59hQ|S8?_IUTZwzR?$%ej=yN~w=4JF*iA|Mvgl#3W*k>imM#Vn4T{&mW z;`Ws&8x^YELH5=jJ;b6IbG} z9H+*_?3J;hqeLDVR-$awv!WbX8n-#3R=Vgj--fsnn<__r=BtOta?~@g{B+5DXLYp_TU}+Z%-34TC0~hcOw4!CJ!;>wytmX`aI|}51;Ae^KdIs`PO;3{KUlli&;5VVk<}9{V-oW z%wDZT*{J6@xps|~X7TPyl#Pmab9`llJS)daY~{!?wfX8{_G%@{M#ZAJcAcG%DyDYN z(kL@WmMc+XNv#L-o~8L(rW|FgMAcQrKG`d0=c8JyneQyVT8S!0tvT}w()zJXIm%dx zvQZIIu3f!y_bcystwhX# zc}I99%0`_@&10e?gL^=E7iT5PMx6u5V~JywyQ+C7btTG1o&CrooGo!p7hOHL&zwn> z+wR&Y>iJi-_S`>KqCDA6=vAU~IrQZ5Er-5arkPi&EK@J}aoAXW-%b(eVfSh!w$_&k z%dw`n7SnAcRb78{B6*l?=zF$dzRsd~=3I$wKG$CNYR&g+PW8c;o{9jnjg{DJ%m_!K zJo;!uRkpF_TkWbMGU;COm8knXCmgkBBekmR)tYaWqoQ!>UXGQha?A;5xY?|xDtoo& zTji*gmvk@3N>n-Kgwen(0#KE`TJx=P)GAfFmt!TW9CKo}2cQji3bI#gzSYm`Nwst@ z$4YGb3^T%Aio7$S4OQ93ns42Y+KrO#4OQ8zHQ&SjacT!rx|d@m z4*SQLNHbiv;k=Z+a>iNH<>6xyC$9NcT~)q$R$Gb9lT57jQ#N)-mMc*<_9KqMh9yrP z_Ith(TRC!F`JbOPT~EgAZGYWHQq}cGCj5WYnl7T=TG(wQRb78{B6*l?=zF%Y{;$2x z8tndG(Mpu>^}nM0Pb7U$bT7wBY&I-YC(Jk1^loCi2e}-MIHvRaOOon12yOS0`-`6B zcce(D%KIe}+Q{!Okx-R)aU>iKJn@}Nu1XIxVQICU2~~QS31>3tOsF!Q3Cl67U5S|u zRi?97dXk^;nwKi;E!*%On9hXu_9tfTN|ovCl`7kDCREMJp$+?dwxP-vlL>9u=QE+o z7Ly6@py?Sgt1C;pzl|B8Co{r6F(dXbhhCY^wX0Y5;7q8R-E+&430t3S*EZI@N`%)l zq001((8C#FpU;FU)0r>|%h8f0R5hzzZJ1N`N|osuF)O+Gntm*8c%S#TF(WMPj5xdd zVVQDW>6PhB=#_0O6RKt{QQtG+`06<5_3o9wNSMxqwCPNkujv^vv!QB#8=3GI)7tCh zP?gf1FfY^6nNVeVM&y&^dZNl1F56ILPMy%-l+J|zIFrtVD(M-KM+Wm%Wjfo?lilZD zx7|AP&F|mUD^=RdUg=5x_kcE3*%}l7u<*-%jq4v5B2~~L> zlrVZVoo$$}^HnBP&1~4uov*SDReIP7Yt(dp&Z){4?)7ZLd`)LU8~Ie2)>X}H*h=!Z z&$OXxX2WtgO5~EO(!);Jetg>F^%;>*lAD*R-KV^36s^8LSGeNJoX$1v&Ots?LI?aZMRM& zc(&4elf5!u=}cI1(=#G}>(`R2^4@eCTKC+F*JnijFMxTe+I>#{aHKwUHpX+PGs1f= zJu8Q`FeCDpdo8&t?@hO%b#6S~H<)_kVcne$>f4&o|Eq@40mL%95L&5yqvNQ02Yp zgx0MqCUb)WH!%V2k-_6h~RWq+txhl?Hsj^I+aLwUe z%kNK=P~}=A+fd~WLMBwX6Vizkh2^i^NT|wRij^>q&hN#MFyH)URS8wv%O%$nzloIz zReIP7<67UUks~q*Rrw1?5~{p6*(+7~%`MtcWtqAS>)mx!e$TdqD(_~tp~`!c302Mfgkn7vYEEp$TPoh_Y3Goi{db;9V*s4st`NKa&4GxsLD_Kw2{);hWVPF5!#y(M`c4#OlPlD zWv`5JeWGNv+-*pxGCd>ADHE<(@>3b}Qe~O44e1%7%5=A3ov1QBBg|iIQL0s!aiZT6UGUy=yS}V%I9498HAMd14(47f;VRX;+3QL|mG(N}*yl)XIuqV!bIOFOY{OsdwWhm`M0mXu?u)srmV2!# z^UYpKsM_C#^KLJP`P$k|&j@qsgn8N8P0t8(>V#{3^UdpX3C9n|m25+m_Bx?;`*tSM z3c?=TZP+{Y%Ild>WuNba{%XT?CKBBVM}w5^gw`#G*E^AB9ItmGd6Ef#Np!a%VOsjA z2<>$nT2JXrn6K#>p}kC~+TX^EI1U?nk|Sh2QDx+u>q?b5bwW>4dPcarlL=MY%Y-V= z!)8L2=}hRAyDc-qC)k-#HSJ#m!CgsN=AHtPDr>)nRGNaWRogev#svJLmeW`tgu&Nfte?mZK#?2nx= zUspn2pAmU=XI`pYCuSSoj~QV(OlKRaJTIOJRn~MT%-1!b*Jp(LL77nHj$S6bA2Y&o zn9ep-dHOpOs;ucwn6GO!ug?hAxS3GpPFNwqQ01;jCfwJU5#Dp@Y{Qb9o)Mn@&V(xOO((QI+h<6G&t9?(RX%&kg!|$% z!h0^AZCG;CGa{d%x2{xqZ@LYw8R~3#*E2eqP_@4eSH;4fvPr{g`vo(WZXKG%jbr{~bK4OM#B z39TDtcv3zSsw`6{Qof#I&xEQRy*e+=>Po`8a=z+bY2B#Ulk%BRWtlo*zDD7ml+T1J z?RCO@U7vYUJ`<|6*9r4=MdwNROsLXcC(PFsq$lMwp-Ow5Fkg2H@@aJmRod$|(yZ@k z{7k5FUn3LF;m+-uP-Vo?39ajuwU7x_)^sO~?u`08hn@*lKL6^3qrQ>15qTz5IhS<8 ze2u(~$TOizdz~;}OKUA;LY4M9p>*bK}4rU^G;%c(nNS2 zD~JBtQ%!dpiST+S(wt!o-w8`=I+t8kN@pA9E1e1JXGUnlcrM#8U+Ec!-chqaJx)LvHA)rUWE+d=Km zmAwz&zt)vDROw;1VSBY5XF^rBQLF9xX6=Wb{>HoOsr6l_e1HEnx7=NO%TKFI@VEgsOVRS{{D?Yo5Hk zrP~@3`s2f!BLE{Kd5NjL0p~d{vpwHdL9O5!&uV&EZu)=brl3XFc<*bbULs z=J`rLc*b`|wQgJTdbi;(DLo^!*9l8#T6>vLWx5lNQKn~vD%079Do3JBsLJD2^4)sN zHdGm}WWsWoo)LPR2}jf!p~`f&p-Ow5u!Jc+BXVmuUsYbuHoSwTXT;2gs{L&o2cd_( z);SI$`UxPvp@@LX~4zwqdkntd$8>dYB1I zYb|6#RkmToVkBc6)NM$pGA8PTahuUuj>sfb8P9baj)S%<(=)=HGNEdJ8=3Hz{nxeo z6WX&(z2r%i34bwdPTfXQ%?Rn-+LOxbkEK`o{`lI^lUXZCqE{$e@2o(Waj%Y-V^ozN5O%7`Nqs*E@?F>CF5Woy^NZX?B?x$W*x zXx$n$-Mu;v!V;R!UM1CW5Zdl_UXF}EfHZtKarnT3-N~%u$ EKP^P9ssI20 literal 0 HcmV?d00001 diff --git a/gazebo_plugins/test/diff_drive/meshes/p3dx/front_sonar.stl b/gazebo_plugins/test/diff_drive/meshes/p3dx/front_sonar.stl new file mode 100644 index 0000000000000000000000000000000000000000..82b9ce0d98d523138c7391851e7ebe82ecca0b37 GIT binary patch literal 12084 zcma)?eQZ|M9mi{6opccgTgrG@eEukQYOB_7GCCTxaXk*6%!Y6S$LV`DWS?1U=)%8V~&lT-{pCd&*Ao$JpN&h zx!=$Eo<8^f&hMPx&*{romDQ|?J^sYJ*y{Sa>iW9a(wfyPW7Ahutolhs&6HUFvehdq z@>f(YTb*CCx;DSIuB^POHov~M{C@kN_g7avlV4X;zp5&~`a%2D{_Ap<`_JN*>3;FV zyW-z__a$%S^OfPacY^PYvE$8~2E>oqaV$oL1XU~TOhSyaGr0ukJ$298(4%mB!Q|6+ zEW|h43nuFXRrGTS&U@iURiVeUJ;lLUI~I?*cB~Ur(a$9~@3fOMLXYP@YYPhPSUfh{ zu})A$KbPRVt@{Uu9^ZfWrJ%`<#p6CZ)(NWU=MtRvcNNDxdH;W%;0GOcEFQ1hu})A$ zKbPRVu}SZT9&>jTbWX5i@#t(V=+p_S=;sogci@MA2|a$Wr`Q{2$Kp|H$2vh3{ak|c z{$a_W@O`v?*5-BDv3UI2j&*`6`nd$>WpwEgMS?2&xgK+751hJw@7KOW=fFLu58PRE z-haiey7Y7Y}(fE8b%FCHiGX zP}M#>Z|bB`|M7nr+2O8y(mQq0@$9(lwe-u3plZsUdESzt{o`{YJN3;EMti-F42YN5 zoI$_L2&%dl&hf6kkR8vBWSj@SIyd#?zU;WN(e(~dx%*n{1UuFbV zj7>M9Zmu6JGBH5BkF*zb*-?=;wNH-bLT)XNHY3zHh&bg`fW;C|c7gd(3S) ziUd{kb3HikJ126@8#%jtiItyD%rRYMZ};)Flc0)zt_SB`(wJ{PJkl1%eBrTCrYCP( zA5lw!D*Cw|oVT=jhFRCx9PVA?dM26nqZ|6zeMnG6Ki7lvUR^NP1T{bNAGUjNVsG4J zjco8gi0q;ysG^_i!Fl`j&M>3$nti()c%RNGG9~pJ{2xd1ISH!h=X!A7eIp+*?S*Y& z?pxP1+H|#U4RfMSP(?qNVC0aXpH5K4*mQ~WJ4?;whwl!fbJnya=8s**lYBmTd$k#v zJmf|5ISHz|vzM6>pAQOm4A&>DxNiWFIwqIKBr%11Xbnzmz$gSmV42BKD46RJlXJ( zC;6OynGsZt>%YVtE$#B6`8@IIGv<>K-wF39`ejB?^{2{Gvwrip``F=z_B?Ht-7&?F z+x?Y(nGsZt*)ZSSTU#U%eA`YveBL5+eN%})-R2DXWkygn_QDTM*Uee}HzFD5H`!(8 z;RTjG$`w&iQ8h&PjgsCd|m8UuFbVj7>M9QXXcA&F3kVv3Ec(MVn|oCqY$e zg+_i6*Mm<;q<#f6Rr__rxu{D0(x%D2S=WP4NTlATDbc=?a4xD+UySi01fP&deU&D9 zdL=dQBByLbN%J|Q8|_2<&AK#kgPE~+GgOB+54WXEtl_=JS)5_gO#5BlDH(sNNI zJJFy;D+Ag2Tn|1Wk=pHo);@R9^juV>cGRH6?%=KmpOBDU`*iQ(K+Oi>TvSPBcztYT zAU8|bgHK3E_Bq&Pg7(P0mIPIj;qFNu3gmY0dNA%7U6~P7(a-geGei5)x5C(#tY6dk zo{^fIoCewo+e~y-B&d=TN1}DBiB=}$?30(*Y~)vz)64Ao4MuL*nGsac&;2`6Co*%h z%0=txL$sa&2R?k*``#lWj*g7>Nlv^`-rEf-0`oCFF#B=t!H9 z=#*1&ciuK*YbTDMPH z^8Uj+PTEf=+akVZXH?J>R4x9Yc9q@n2gJ!AC8FDL1`{@K#T&+vU4f4>DAj~+a zTceSTEVsL~pH5K4)w%@Zj?qOwouG=VbqT2f6y;_mB(_UzO|EJ0*I~@d?LK*}Um|+S zAVHN>5KipNPDJxLpOBC$#Xn!jPK=M_a}rc#b&mEfJu)EC7|G{+LPDx7kPJ$|_c+S3_G>=B| zIiHY_s%k@7GR!6Jsz^|E)>euuPxXYE$R+rMgj9#KYtHwP)ksh!73gahE}7^}m~qGG z%8a0je(v8PRrxKO1|+23E;aq4iFYN2*|AjaWo&yzx1BObP$e~enMrQ1q)Q?m&)IKDx*2IdouG=Vb&22FzLR)J4{C=U%e=4I zG5vJHQDL<%Aw4hg7-6GRx>lR4hvakm=>%0=txHJnPCR6nk?xy#Y_Vhd=>%0=txHG` zQasYVP5G@PpVLn#sN!l}LVBseUoj@kgjFgPbaA2YF$El!s2n!=5Xoi z%Dj^M=%*7@akVbNxMOtDPbaA2YF%Q@YvaS7VDYc&3fQ}2&s!&`Vh39%sA4}_C#Yg~ zStqDsuUIFjayq}x|2e?l%2C;<^OG33mHP>jZmuq;-OwIMO=7z8Yzr z;2R|E5_{H-HhHlQf8M!m1+2=mv!xSMu}`HFRI%%%6I8K>q!U!JBcu~lvA?4eRIyv5 z6I8KVqZ3rId*Tv&!Z*k_>k{mFu-5zE!WHJRUtIC7ZP|KTobw608Jx-WAd#wjd8*={|`dr+Rk*>*B2Y=>)6Xq;-N7 zXwo{t3N&e*U_F_%POv&mS|?IpUs$;%trM)nlGX`UQc1f6RjKF--=_WsSGebsRO!`05>)B6J`z;vl{*qt>2){~ROwYU5>&CG z=e|KXkqw@C-CSvCFJSeH^#Gk&xtrM&>k=6;;fJo~ED>|fgg7q2F zI>8DHX_ugiZ&QDRa-x?zXV34O3wWdBJ<;_b!FwiYo#5@4v`+9&PFg2eDIl#AtY47U d3D$r}>x5J5QEM{%by0Yk{vXJk3UdGe literal 0 HcmV?d00001 diff --git a/gazebo_plugins/test/diff_drive/meshes/p3dx/left_hubcap.stl b/gazebo_plugins/test/diff_drive/meshes/p3dx/left_hubcap.stl new file mode 100644 index 0000000000000000000000000000000000000000..01bd2f93f2d579c977a534930c04280626b268ce GIT binary patch literal 7284 zcmaKxPi$0G6vhP;SzugQ&>%1x+bXn-bP787Ucg2u#zGtav=CZig_f2At^F54S&*0* zMFEWq#UO#0s6Z>VDo|#yVigc{Vp zucCEZW9Rn7)6Y~T+IzY?db$(Mo$Xr_6}>B!c6B#y zXzNP$bZscrucaMro08p~J=@xn9cB8f{@Ztp|2J57SG@iBj?BWwnekgwe~Dhu&;8Bm z`1k$&(eb};&m?vi#EAqM5>!2U?|;#kB2wkoBTq2yvYm}lhdj<*SREfYxOl>&`eIRz z2MMa^=RFwrpQF8L^H^F?8(;aTH*NEHudgJ>g9KId^B#=*=ee~R^Y~fwXc}9PF^_e5 z^B_SL{k#X`{#!hnvGw>~>rp*?WWwX&y!9YK75%&ijUJ08itD36wc%h8+f+?h3x(|yNs-Vzd2(a(D@ZtAC16Z055+z_1{EzAze zWA(RXIUXdaqM!F*+#_?I&Gu*>wc`V+DJSM2T*{}8Jmq%CMu^bN)RMF3SFzx}pSN5!CHO|c5 zRym$Ak7?!Cb38~;ML+MsxJ3_)X6#vg+MLer(Ej*D9=dYn@IFps%;>p_Al`gsq=J*j$S@6}wrSC6aW z*n4$H-g`xYD*Aa3#(lD3No@P0Q2S$O*NoUacINF55>(O8dob?0;p*6)Rju~753Ji)q6f{{GIDolcrJi(eu zf{{GI3Q2;IJi&TLf{{GI>P3Q)Ji%H-f{{GI%0hyXJi$6af{{GItWScGJi#1Jf{{GI zj7)-&Ji+`*f{{GIY)OKVJi**Yf{{GIOh$r{Ji)v~f{{GIEJK2kJi#nOf{{ECsiwA0 zxiwm>Gr%e3+BW@sRJqpZ&AX;Vmb*Mb)fH8M77_DR0m1}TMXF1iL=>tng$b&z%A-ld zc~#3WLDgx^<9QKQ!@~qs%k>mCh_KEgOi(qXey2f%^*dpLs%7fJ){A&TU09f)%65?v zZ>Tp66I7j5$7+OidSQa9m()kwJmyR~52~iAySHa$-F?`Ds`qu~u=TJrN0^{$SSKM{ z^TA2ioT{lhpV@n5=d-W}RYUTycei@dcbBR?nuqO=Gn4KQs;=pTZM$owc2{^*RMlub zHi}5=+#e>WYSx{lWxQ^!CIL>ec7T zW1o6;W;^Zpx4`N{zc4`+N9&0X zbZ2EA_vpQ1y`o>3po*jQ!~)&D+5VWN{lN-Hzc4`+N9&1LcZjwgvGzG@BK^VyRUEA+ zj_NMd^3@vMg|dp$FHBIy(R!j>^RT@8m+~&_F8#s;RUEA+=4w5xezdB7uu{`6Oi;zq zdg3*`S5{eKRTkEE`h^LqI9gAP>svvwJTB^60lNYEg$b%ST2GAWn@5p6F6)~I`v>}k z392|+PuLfeLU}xPRUd7s~U*4oeB z-`?Namv7*tspF=9t8V0l=hsbZnsIH@jJm6)PrIgW;FJkdubeQwUtQ0M)2^A&bIR2d zr}dmZt+8k0jB(>9H}-659Dj=Z^OS2Rf3xR|=}l87_q?{ZRONqX?fVJy-;2x6tc>rz zHcYs)QmOkz=X`nX#=|S`&-i_~QvQD7qkSrMbwo%|_1ZxPRK6iZYrlyR@g-?@T=%uv z-O}z(@w;4>FB8AXLg@0ddnKp}(kmryalN8Dd-e8)G5I)NnlU8m6$z>~WH$$0BSYuS zjfl3ko5NUfzd|Y#*N@42@}abw?iC5DI3~Mx>CRsDe!aO~^XR*%A?g(gs``G=T(9HU z)p=n=Y@5?uuj9~4$A!)Hx<>1zU5?52iUd_0pC#zdUcLG8h)l<^e^ZTLr&bT zpBUzo;W?@3I4+dRp<54Y*M0s6(k{o8CaB{0EJ1hn>bcFm^Iol9v@N6wsydzh%{-5u zT{=eNsF%tW()TwycdWcC?dmxCNV^=9^&mkN*N7$P&Rz|hJvbl7sV|O-#zBIrgASOR zk7Llb#Szg(Dmsq!&nym?OS?Lb3%8F79Fy%8392|gOVFLYTD@dg-m8x%HS|prRNcFB zf%Y)$xphuNR4dtK*|3MJM7FkJMK6xYdXS)s)LhK9#~5cUB@| zOYupTT`H9h(yn|CW-Fy#j>-0l1XWxkmY_R(b!PE7AFg|}Pt+?CRNYVe=MkM@qzt{h*p??}5GlkF7=syIGN(4D=yxH#^HH)afpdPRb&XNseC7&UKhL@bet zW^jpAKG>6=CuDC)yBw446$z?1K14({F{~$rt^~JY|pnHM!m{oi;$=akcrubfyHA}l3 zQ<|WP=Ff`@AAS)!W54q%gvLUcFU(SIZit(xdpUmMxKXIVRgH5>#=G zSc2~C)sa8%ThXKQXXSG~O;9zu*QAP^eS{tE_%Tu{aweSpMJk6&ySk0lNxK}A^&mkN z$7cz;vsd#cpOcTHqa1hB1XXK3o|}(ji94R(BNZLTPoy$i+SPHKBJFZa)`J999G@lV z&R%Vqb%A<^ulKvXPnw{r*Y_9ZBfomwgAq}!WdCU0+Pg|*o#!9ehhwrHB&g!}Y7#-+ z*{hkwdJ8M%yfjTvb$7AW!|QTZYYBDN^WEP{MSG>^yN%K=$CM_h;`l5qz4mSH_c6njp#TPnJh=s3P2?Q%?Mf-0^NOVFLY;v7(#po;TBmf&0u z=YTk-G(i>DNSdH(_>;>k^Iuz#wGHSItzGttsw2MGSZNVrX|L%K!3v4L55A|e{HPAu z?C1OKg`ny)D_U~mg`-;{f)x_G#bf1B9l|{4L4v9y#6yYg&Vv;aLuFkpdTl}2>edwr zsy;32YM~IT+`3|g1h+SvAPK6{+ZA)>7T)d6HF<(#&VMNSWFSEm)3m)}g+zycZm8TQ znLj>xQ{+K{DyC^YSRtXyDZPrfuk z74x?QbLOY>o@`xZf$X)thwUF7)ksjqG_40KB(jFKJoA^XJv;IsK^4=q9;}eiWli&t zHS;2mm?%|D(|eR#2HUNUDZKTEyJ~op1XWDaday#GnoWM3PZLxze@ifDO|v}LIbw6))9wew@n%<+_GDsX>d?tp^ zyLH93sA8JdgB21yM@?}dPhX4R*O;E-BEy0}m zjjn5PmXWpgnjYmrf-0sNd0@*Rag3ZTY39!z-4c6%po(c)4^~KYEj)tvAVC$=tm%<& z86=J^&R)Y}x31V0RZP=*utI`!18IUP=5Gn+%-M(`XPm6(qS+@eSRUm;f-0tIJy;>p z;mQ7bhMYa{)auBC1XWDaday!bU-4Kf$MYXM4-!-{P3yr5iLc7KnkVOQpK|Mp1XWDa zday!*vs7tK^4=q9;}cUw5YjWGjDqSvB-l2RZP=*utH*)+)JUEH#-j!R54BK!3v2B zW;WMr=8w5`MS?1(X+2mW!K(vlf-2^33Fgcz7^htRv#j=hPJ$|?X+2mWanI~uWt#a$ zw-1t_ifLL8R!FQD56wJt9wew@n%09A5@TguY36@)`#A}!n5Ok$g#@o`r3tE-za^M6 zufFv-@1%Ck{9t#CB0&|?v>vRGc<<%wjIl%q?ZNxN}hwR54BK!3v4x zch7Iv^Y(4-yqyG9Ow)R>LSkj%5uS7&B&cGV)`Jxi$H=;pS?6$_TUR8gVw%>26%xFo zAWcxk{4K$pdDp`e&o9gObN~IE1XWDaday!b((u)J=HuOWXA)E~P3yr5iE9dvFxq*L zpo(c)4^~J#E9*+GO@t-x`#A}!n5Ok$g#_=0NfT5te@ifD-m!DrJNM=LdA*xkB0&|? zv>vRGnDx>6cAamZ>E_5tP{lN@2P-6Ii-*pgjd30%sA8JdgB231WL@cg{_1)0oFxgW zn5Ok$g#_>9N)uEue@ifD-fhzpN z>%p9Pr{vON+s($hYegieVw%>26%tL={rpXL&5Hz8Ow)R>LgKpWetzhb_}U!_s+gwt zD7Op}t;J_z*yh$1+oFnTS`Su8@SfW=K^6121aszHzFUf8RJO@o6DC0w)3hF}khn~a zwR-)0xx2Paf-0tIJy;=ek$A{=gYYBgL4qo#X+2mW@o;hO6Fze5iUd_m(|WK%f_HzX z396XCC73hsQ2)4JelD7=a98z7P{lN@2P-7z$~m=W{+hcxfdo}d(|WK%;s){1%y&5t z5>zow>%j_%-^jYs>*rhDx*|ap)3hF}kl?cpX@V-|Zwcnirz?&xzH?@sE?8bk6I3xx zORz%XiQ>C+w&AJOkp~H?n5Ok$g~U&bIe^gSJV;Q*G_40KBwju@pUVixxphT?DyC^Y zSRujZbkYP>%-<5snNJ2CRm?4AGKU=HL4qo#X+2mWab__`mi?xAL*zk%DyC^YSRt`* zG1nKiIu8<5F-_~i3W=t%`J83g>DCnqs+gwrV1)#q7)ujWF@H-iXFk<-372fqYhKx-ZXYB; z71OjHtdN)_9(p`q?>tCQ#Wbx4Dxz2~ z!tRFn+=m2JOw)R>LV{13rwOW$h!xF@|?A#sR!EZo)7|6J!mf~qfw$4x@q=R8;;(Mde!?rLfKn)4t*Rd?~YQHXn; z2P-5F6_2yV4bBES4-!~!C;vVP03JE@~U=t)k6`zQ(1asyyCc6ApN$^(ptV~Rl zDyC^YSYf-f#G^$LY;_(asA8JdgB23GJYN!==R9JfR54BK!3x{er(gz1g5P!?B&cGV z)`Jxie9~s`MEN8V^S1JJN|x$u}MRZP=+lv@Vd)#Z7T`HokvuHiwV zR54BK!3x{esHf)M5$t$)`JzctDn^6bIECfs`OUEocT<(F3VX)Sn8e)kBL&n zG;Oa~VY|9KQ!@7+F;S|RruQhf47RIJWPevOztxR{ZBfNEtp_V4bXm?`^XJ|7PLwL9 z>4|d7V7t1knd>w4dnZa2^S6YqAAMe4pZn+g07l6B2P%^JO!r;`5>)Bm%ly4ZSs^h} zJiaBF>pMWw1XWDa6XljcVw8B?EScX>)1xG)Vw%>26%wPx<7<-n_0EF?Rr>cbfA3LN zNbs!)X@V;CDD$@jbLP7&Zj^UtXy&`zJ2^;D#Wbx4DpNM}1XWDa60DH8Q#>^Doz8;LSpmc=6cQCdyt@tX<83fNPJuF)7Q*vRG_>_2P=KiyU1XWDaday!*Z&FGVR55=`FlWC1s*k*HOEYhD$0!n1 zF-_~i3W*cNLyzZ8&VvM1Ow)R>LZYj9=<(biYe`VWG_40KBn}e~&D@_ekf4fbS`Su8 z@GW3zf-2^33Fgdqw|!3D6{qK2E$%##1XWDaday#Gi+Jc+RIBqKK^4=q9;}e)Bp!O+ zev9)UK^4=q9;}e~qV9tE+;2wF;q0AwN_ucyw zNl?W!tp_V4J`fN6ezl+bUQ2>1rfEG`A@QDg==bx(oCgW2n5Ok$g~U7Jq2I5Lbsi+B zVw%>26%u^=Vw#|e`CEcH^PQPj$vZk_4moUc@BSn~71OjHtdJNl9y$;Ay7M4G71OjH ztdO`uJaks1gPS8GK^4=q9;}cUD;_!z*WG!Lpo(c)4^~L<&7o<6D&}tq=FIoK>axy$ z&VHqPbkBB7lq#laJy>D8x~#LGAO7S&(HwG2lq#laJy>D8x~#LGAFf&#dBjAiVw%>2 z6}GF(I{W!vr_&;jm?%|D(|WMNc6C{2QuTemdnZa2^S4B`t)TBD=DUhV6x(j-;NH1R zf~sl{YiuQ~khn-Z^t##$?s^mns`T$={(c-~g~TP|p;tK`tm#n_RH;XqzxOCBBy{cS zb+tJ)JxYSA>iBAGC9II(8=HNv%58}%ZK=%P63m(J+1_3pqr%PZnlK5fn5Ok$g~Xr5 zL$9AVIS&$4F-_~i3W=TKq1VqZcOE3DVw%>26%zU>s@Km4I}Z|6F-_~i3JJb#JWWu= z{4K$p`HuKE26%y}=hu#74OXop?DyC^YSRwJDc<3DvKXo1? zsA8JdgB23{7vFI*e2MMZ}ruATj#9zfj<^bB3Iu8<5F-_~i3JLvI6l6dDuJa&471OjH ztdQWZJfsP#n7<{MGk=@oBzbqZX1LgHWIp_%{Nd61xrX<83fNbna*(gan^-xAE3zbmuk+2(r9 z{51FbI3%cIn%09A61&^;x$yp{I}Z|6F-_~i3W@X9<#YON-h%{HOw)R>LgImy&Gnl3 z8EzaTsA8JdgB23|b)7Up74x?QbLMX%{ZM|}Ni%=dZM!6>Vw%>26%q~Nq5HYaQ%CpE zlc0)eS`Su8>?a<&pUXS@BM%Z(F-_~i3W;CHbv4age;+ALP{lMY!3qifGE|zNiuqfD zIlnL0f%P||^tZ^Km1WKR3U`bmK^4=q9;}d9S6tifKgD^Fpo(c)4^~Jl5)VC|%Y1v3 z2MMZ}ruATj#5nQL%;!4~5>zow>%j^M{;FA;po;lhf_KD{pkJDxiuc@Ff)x^6Z)t)m v-aBjwR!A`aG(i>bfwlxIB)A=?395K6wTWnNS6oy4b6XDU8i?^vrLEAb_i=|U$9;m<*r6eLmp_ocrYVY(Giuiz#T0p8K znut*}Sd0o*6w6)cVDuvzj0H0 zUvi*tQuKMd?C%}uYESl5s8#>HdW-)za_+oxVRe)d%iS5IKDf8GlUYduXs^i7MlzBW{ur@%jg+$_=s&2 z^~1Fw?)aM@r7cfI%99^m5>(aS`fb{>c~aT*1Y1ZfEBYsG87vHCkOWnqtec&&Su#H~ zOV~o<%#Om$33-f;9}UbB5>%~hsL5=V$Kryk0fH?ge*J7g#>VQ;^A&jrs+!K#XZqza ze5fHnu!Tf&tSoa>9ydk~RQS;)LDhFN)@3Xo`=>7r5NsiFL;W}+k2#-Lq`e0Ts@l|# zPI(+Y)DR%pLSj<+7?a15@uPVNs@5wX?eeHAxEdhXLZV6Y>OFbXEV`M8pz5OLRfjwl zH_poVp0kC-HLZ`g<&oN3kcXgZSnI>a>dTf?fM5%WA+5)^7n>>EJ zuqr^Xg~Z^&!IXI%9xu&9P*wcyV9Gq|3)Th*wvgDZeb+pm4fTTrRdclOn#T{Jez1i^ zgU%JpM@=XnB&eFM)5r30F_aIskhrcB$L7`Jp?O7us+V=D*}SR=%`3K$7=7*ToUM=J zq4hz6st0yMNSwSfKWFQ_EVRx^P*ricE@xwPCbZ7kLgL@kB{|!#PLt&Q}tQtu5A*Gdj6&hX9pJX3cLqg!@N%YLoB z%M(=nrplsKM73^0(F9f2Tl3hZ`&l$WRgtPN^H`DBgQ|~J8=A+wy#1hRx#pOS!fcH~ z^tYnwtmfXMU-BO|ns>*feXcF;rWvWOV!OO?pmc6O;EK@{jl}%eBSjz6?dX&537*N&-Y{>RCPC^>b^ps@6~GrX}@2Q zpo-p}C{>kOqPQDWrILytfIU}o$_JlN43rm z`o$xtVr}O=KGnRkb7_IjCHlo9sA9$GJyvUdJR*-%dhMrQJc26L!QSJ2{c2Grk2Ct! zf`0J`s#u+Sj}HC%QYw!U{rW<`cm!3vGkA|t?Yq{Gi_@% literal 0 HcmV?d00001 diff --git a/gazebo_plugins/test/diff_drive/meshes/p3dx/right_wheel.stl b/gazebo_plugins/test/diff_drive/meshes/p3dx/right_wheel.stl new file mode 100644 index 0000000000000000000000000000000000000000..c518c0eb5da524069efdd0f7a6e1942c79563e22 GIT binary patch literal 28884 zcma)_X^<7=6^0W;RNO_QiJ}9Qs1XUWL>=hsf)Z3zU{C}lDnS$kM3xy%0_Y6GGDrvl zD!3%L08uQJ#d4YN;(|sk7u2#U;uv-G{) z{q=JDj=g5&ZMIYWj{Jea#i4C-<)DHfG|4(cQ*hJ!WFJ z$rGn^n=*CesBu%e)lV7KRsOT4DU;r&z``uFnx>!2!;tkkiO6YQ8Ca-; z4yss_o#3V$8)~%ev5)2ktz(45Wrp#r=%DJP|JDVq<4ZRW=Dv5w-OKv<_)@mx+m^kW*0IWMiF8oK znrt1TC9cw+mAK3>o)sNbb-Ml4w2o$%6?6aH%A~BH`CH{P`0fh_mi1Hniu(-GK^1GV zb-XEYwT=rUE;Ed0MF&*}ym(+5=?<3_bKn2WtIE19bvw9S>CI<*q;=doXH#OcqJt{d zWF55<_g7NKryV3NYKmt?2UTB6R@#y7E-U8Vevihqj{ckGCFjrRpVraqouP#~=%9)< z**XrExLU`W*XAY6FrF11R9z-nX&r+$7qepS_b#24=Dy{`re1e1JYU)2)pd2fY*uto z#hR?65;uA9!N#@BFrF11R8@G%x}lGsP?Zrh_rtTkg#oY43uCg%!yRuA4RHrmtjRhK z%I2IrEpeG)JS#e=x+L35l5;l~vtsV&W!rJs(4k$q&Bk;~3|nV!D%3#-RjkR@(IflZ z$^N5VnHk2jqJye`WS`ok>zra%%zaDt84M@iRVp8qeKUj`HXKy8S_ ziNs}w@vP{es;y)t-=fJHee4 z4C7hRK~+1+O1@E(-@2@r`)k>LHwsTmpakxcm6oPmWpi~}mxWb3%-mh`wS*(!0F zVLU53sQO$Y={$~eSuywLvg6WliS(@@JF*PhUFCF86|ejgiL3J%CUKczJS#e=S}9p+ z9eyie?$>4K2|>5xf!UFMnBlfWI;dhzb{^BS^O~ep;xfZ{R&-ExaduXeG`Oso`{&s? zPNoUqt^Gtfa5YqE8GmYu6510^mqjAum$Rb8@kwPc~oin(9ZeQZhh;qS@! z{0|=KRgxosaH#vHrh_WhWa}6radm6>o5W>?@vP{e>LAHVzjr&kteE?XosDT73uM1L zbKKc!9c|p6lMbp_lda<(iK}%yEOD7(JS#e=nju+f9o=14%)R^XXR9i--?6sm#_5C7 zns&Z=XHT0I9aOO<>!`#{CiQu!2Q!RkMF&+CUb5=3uX5|u zWXWo&VojEzx$E)na}t*s#Dy>P6cPF~6nETn;b{xKy^Mv-6jka9G>q?ZcZ(|LlKH=}!53##_A$ z3_7SH&oUVI5MeAhtZmpVjP1f$D1SFO1|3w9XBmvUpY*Ef<5}T$>D77CtC{lmi>_C6 zP(_|)FfO+%v-Ox&H9Dvw&oVeEcJ!#ND@wCv=EKfiQ#f~~gDQTyEQ4{`(YF6uUs@(J ze|p?A1qK~dk!Kl<%Z{phvCRA>$Do5M@+^aK+0l;Dt7e(`wXRomP(_|)FfJ#^Z@##L zD)Ov@lVV4Ah1I25GV@2q?kMbW=%9*w2FqYvb~FrZE!`qBA9&)ag?%j@RFP*HjLVLy z`!<>RkiN4D3_7SH&olDvhaIhC6>jakq6#B-P(_|)FfJ#^y>{F|6?xXdNwK4yvTZl% zeeRmVS_U0d>9&v`saXc&vZL+fGkCMie9^dP3Jf}^BF{1ymmS?x7t(< zeEVTXJ7?dCp_A(sGogw+%V1nikVl+x2UX-*2Pegj4$1aW$=cJq6wc4-po&N2mch8} z=$EpumHlpV&0D<-3_7SH&oUU79aXm+KZNm)K?hajSq9^>qZek!KH)*vD>|qm&oUU7 z6Xe-a+(8w2*1<`!qaCwjl%%EmA_ESp$g>Q_ zWk)*-W4X-SGw7g-JkQ9tA9nP}?ASNVbG>3FRFP*HjLQk~Oh4|RiahJ!q}b83PCmZW zDEsrh1}!PfgAS^=5@H#Q%Z|2vxlcOt`(L@cz@URF@+^aK+0iz_&^bNe7<5oYo@FpD zJ9>unO3rt}KCV}EP(_|)FfJ#^RjRmyD)Ov@lVV4&u3eYt%r}o)QJ4oERB;v0G8mT~ zeR|l&WS)G^pK4fNV9-God6vPr?C4{{_@m7HX~&?0D)KCYaoN$q(kq?$d#+b>P(_|) zFfJ#^71p?eD)Ov@lVV4oSkO?bGk^T_)rEP`K^0e^ErW5{(SCCpYIWxIZ#`LH&_NY> zmch8}=wi8-LTBFK7<5oYo@FpDJ344uL#@vIN!Kens3OlY7?%^|)q%K!D)Ov@lVV4^ zUiNUP{yC?EDqcOY48~ybmqIcUeQ4nd6vProFK2t#vN3V zXC0grJGxt+No84EOcuCfQ97vNRcp&&Ty}KH@|o$(H@V|>I;bMgG8mT~T`CMcZhzS^ z=%9)`%V1n~^a$yd&U}*V6&+NOXBmvk32wpt6LAMsE z4ywqr494XIc{fblK^1w{!AY^Bw|sG5S=Nw~K5lJ^4yt&!kYz9~J39T_b!A)@o=(J6m^psefdryaRpVGWrMs(7cHWiT!~dcyE0%evnF&B_l73_7SH&oUU79aXok zm#!X}6z)%?gDUbYgK^o>N><^eo31M`=%9)`%V1niu+rW1zQwqMD)Ov@lVV2~W!r8t z!d)w(gDT#oX&H>mj@HX(P_L_Pb=SP;po%=pU|e=|k}&it#{uJu*Y4<`iagKAw;y)2 zCHqbcZ@6AD6ROCw494XIc?WIWK^1w{!AY^B8?${>@{GGCOb1oG!`LzymmM7;`&zwz zzQkSIrh_W-EQ4{`(F=v4*U$gt7<5oYo@FpDJGwSI_6gs*UeQ4nd6vProFMQ1jytF# z&pJ3Mc64X&^jI{xP3{SZJE-E_@z%k(>}aDLQ_JyH^0D085O+{To^>!TJ9@n^^!|x$ zjzI@ixldffiPOevU zP(_|)FfJ#^r*7g7s>rhrPKq5pEL&SjWDU764?3vgQ%RP=xa{aD*&12$e8c(zgAS_5 zvkb;%N84m;ec?sNpo1#%EQ4{`(fUi%HOsKY^@Q_}RpeO)<8p$0+A;2+iahJ!q}b8v*|nlX){qOmqJt_v4Qd&T%Z~nJ zczVq%dBS}L>7a@{%V1n~bgVFBO+T!23_7SH&oUU79Zhj}I$g>VkiX9!CT@z0Bbo(eesN$2%mch8}X!q>ec5;{7*U~{1d6vPr?C9X^8hu#o z7<5oYo@FpDJGwf%*C1>kSUmQjgDUbYgK;@QK4l(vP(_|~@P1m}HT&HhRFP*HjLW-! z{ww?sOqBF{1ymv^qmKT&j0MV@8wPW#SsZ~mzx z`-c_sRP^3*Xa8yP_xbM5emba%->1*GKNiNcZOyIs31eSj%#gp&aSS@BBG1;rxJL`) zrftoA?iI%V!f27df8iK(P(_|)FzzwJ=r^)|a*r?$5XPDEcVEY#gDUbYgK_y(0-sih zJE$VhIyfnIRG%0*U1q-6J(WTSRq!s5)lJ%%|1&FEHq!iagKAw;y)2l2x*5 zOs6W0+(8w2mch83pgQy!GyjAe9aNEL85LDJsfsF{b5pkMhJD@BfOJq5f3}Zt_3wPn zjlyV_nIBS>k#BKSk>~5k<0>Pcv$|)>%#Wc;fL-?RywGPKbOk5?5MhD%gm2(3_7SH z&(^`X>}UsJG|J37ItCq7k>?rt_QQ^|5oDbvmdb&olDvhZ9tX&fGs0U(6&|k!KlNp*}CK&;7Hbb@B!Qo%uBP1_3&# zioXwlarN(f&O?MDd%ITu77se8BG1>6$7M%{3Pa8f`dnYNj@&^Nd6vPr?C3?pI7McD zonz2J6?vYKZ$IkJC&;%V#2r+TXC0grJ9>k>{X%EH&ApR@4yxksvS3_xv_%-&_0JuH z4ywqrbucbFx>y*~W#;~^Ds)gqo@FpDJ9?Kebmm)J9duAdo@FpDC&)K?#2r+TXC0gr zJKA3EJ=d8Z@7~-(2UYR+urMw=`s_Se)0dfh1|3w9XX{{GcJw!LpT5q#i>reUs>rhp z#$`u)ENZCLnR^BuRFP*HjLQk~Z8UKQRpeO*C&iBTlXoHM%>7$#=%6b84k5;6N4pC{ zXYM~AbWlZ}t%Gsd(W8W+GxrQSs3OlY7?&O0M;JPD|6M`{RpeO)<8p$0lTzG46?xXd zNwK3n<&9Z7^C@m0MF&;!_g^tCJ9>;Tbbns&7<5oYo~?s%+0l-|(EYjJ*U~{1d6vPr z?C62Q(3$&V20ExB&oUU76XaXK;ts0Fvkp#*9sQBK?M-Li?2dQopep|EHpXQ~4;F@g z&RZOV4ywqrbucbFy1y{=xP5_R&_NY>mch8}=w8Cm<1YXHD>|qm&oUU76XYB0;ts0F zvkp#*9o;2wDwH+k@RfUiA{|u4-#f^-?C96R(DU=%+<7e>RFP-vU|e=|hcNW~{6NQ` zgDUbYgK^o>FNC4zS4TPq9aNEL8H~#b^6iUp2UX-*2PegjUMX+clr`k=lzaCl9aP2N znaQ~9=qO?6I@~9YK?haj**X}P9lcx_x+>Dvt&!0|6?vAyxa{bq!q9cNV;zGIs>rhp z#^nV0=FqrzIyD_s zk!R~*Ty|6)y83zAIS&*VbWlZ}WiT!~TFEN8qf6HUgAS_5vkb=N1l6JIu>Os`bWlZ} zWmHt@q$;X(&UM+g8``;dF4I9({9VP2tAFQnzEBu?UF~gmJ&F#h$n$mNan+sAdAKn2 zD#wFW8M%Wh@;oDtTgfn;v(84ZtIep&$Q@LX=NY=*uG^0?@;UR3&2a}+mch83Am26~cTh#1b#PMb=%@0od%b_+S-B@5?w~6Ej(EmpN52q;-aqlU zW6(hrc{VG?Wkmch8}=)Z-bGyjic&_NY>mch83 zAb+7G?x2c1>)@o=(S@%!)auNC?tY1e4yxk6E5o?#==O5D7T$W2W6(hrdA1J5Wk(0B zPS^DNcm^F*k!Kl<%Z}c^w4qjKezL2B4ywqr494XI`Rh7y2UX-*2Pegj-YLKBq%;4> zZM$?(75_~n#$`tb3PV5VvQAyt57I#udA1J5Wk+`thJMcFo&5y{9aNEL8H~%0J}TGM zbmsmSj_9C@Jj-BQPLRJ06?afYo^^0i?C9%qO;~4sx!Xt4LDg5{kavGp`6V;PWk*+M z*S1^7I|dz8k!R~*Ty}JhFm!(|>+OY^(?Jz^mch8}=tyDc%x5|V9aNEL8H~#b@>k8` z4ywqr4&G18`)t3PgDUbYgK>HHZ@h9ks3OlY7?*c3$0tY!RpeO)rLU`5(!0>3IME literal 0 HcmV?d00001 diff --git a/gazebo_plugins/test/diff_drive/meshes/p3dx/swivel.stl b/gazebo_plugins/test/diff_drive/meshes/p3dx/swivel.stl new file mode 100644 index 0000000000000000000000000000000000000000..03797a26a294cd6ba05601c79293de2a4f4499bd GIT binary patch literal 10284 zcma)>Yiv|i7ltvGC@Lc2E#PJB4-Lj9*GP zbbCkLoTm14XZxHA`Ok{^O%J9!+B#dB((}hjtNh>2A@+YCRD2hVsLh7?o=<{TZ|bV7 zdFJu!H2$V`cvZ#gQ=)%QrO2>_M5g|Ns$lFB5p9XGEBA$ye_s`~keF~!K1}vt3yIdJ zyTjopM{7OULgKeuhK93;9ZmQSwvc#MW?|QovR<79>%kThO{YE;+?VJ<6=&K{WDEHS zTn?^zeM%IwCp#+5hf^^JeuDX!BsoxecTxVW2UQ(~91KEGb)=AkK?tf=7IIKE@TjP2 zEaYI2xuS|Q?Vl^oC}&#Z8kx7tChsX)CB=kO>Ax=-y70O~N9ZTGy8_vdx68VUe+NfJ6-Q`2xX(3S{c&fs`bhf;syISRaJ6%V z>q?v`_eXrSlb}lDeZd{0H-t3{mshTtbU65}BOPw~b4BG#5`Qjnb#1=rydptWxAZhg zdWsY5_r$lSR84#F?#)Af=&ICs#(B#&4^B80{p@#)7~Y+|aCU$A<3%IGS3YU4d~f`| z@OaPH@9+Aw5XU`8P_^=`?rgdA6erm4(#CngT|4vP`~TEcZj-o1;(-aO=x4u!{c@)L zvrB?1`dN>2zV8oCuO1VP_cV#iCDxp0_P-T0HFrg8!B0^2ju3N%h=2K(JAMf+K5cQ7 zTaCZ?W@eOg`dJSWRB^PHI9xR=+kLbzaz9W#E88Wp=70T!Vd2~58^brRUmp4is#=BE zE=2r0dSv}P_T`SKWu2@mjdfko&w7xcileo}!ErN#-iB=C@utK(B-V9BKR-bgy)D5^ zF@yB;6I5}umZ+<`JbX02C7O5cF8_`qK~;tH^pf-x&j&Nb43_`Y99EsNFh5bT2SUWXq=ZY;PK3(@& zIHNxkN_|!2L4vBKR}Am16Jo`TD~brVkhuEbiEwXUCVc+g-2)I*)l9vh_ka+m&RA4L zu!Tg=^0Rvv^kl*#Beo7eP$hL&L~PB!Rz&nj%NrxNhT5Wp#@es?mF>zUd>j@_ z;X6oBrPYTLd*$}A9&90@6+q+8e1ZoFsV5T8b5|7WeYHknT?>AKDy<({q^Ee!S!K|#G=eHtHFi{b(}zd!CM&=F1&Xzw|4Opi1ip?J2&l^rqj_kc~X(R~kW;)(?M3 zPw{odJkzfSN*Qtl?#?iWue4X634%T1Xb#%JtdFI^{eyt!kS!gTq+Cw z^3@W@^TCl&ML+u;>{s)DY+5d8mC8ahxL@M%nzJztRbJKJo;&9$c^VE1huX zJnt3j!Bx%G?!R$JP^JH#e5-MHk}jQa{c2Ciw;E3w`jt+&ezm9MTaBkB{YocXzuHss zt;T&$ztRcUFHZ@ZbLN@V3oAfBK^60F3I096zo}R)`w6Q2H5~ipUt9hvoCH<$v!mkt z@Gm4*vVMXp&YLCJFaIiHCF>`sqMs$$FLUFcT@qB$&wA(??p{(fcCSNvGw7`rf5jm| z6>m}Np=Y>zNzvH7a?#IEP(^P`=o#+bOEh+`b@cNSRB^PH&@9qH#MsN!fX z!Iu=h0o-dg{rm(~9IYkv40mrx8oT#aJsFbU+euKRr;>XqwjO$hySFfn-Fqwj`~+3> zwuGMH?oCf)_ufiBKS32oYY9EW-P@$b?!A?Meu65F))IQ^ySGq{T{THRKS32oYYAqG z8PpTb{odjysM6Eb{X%C6J)_)jkb3&K-=OrQcE3&e399tWcfVj-f-NL?Gx!Orc>7p_ xU&t@lliK}m$2 literal 0 HcmV?d00001 diff --git a/gazebo_plugins/test/diff_drive/meshes/p3dx/top.stl b/gazebo_plugins/test/diff_drive/meshes/p3dx/top.stl new file mode 100644 index 0000000000000000000000000000000000000000..302ed5f4b6c10a849e2374bb2820a6fd85fef780 GIT binary patch literal 30684 zcma)_eT=46dB%YdZ4GKefRggzxX{LsZU;&+sk`$Q*u;vON}&o$n{|;b=(6m(?8u_F zX2mtGSUwD`E@)Gu2q{rxZ2hP+Q!Pj^jaspUwM5-+*e3m>p-F=ZQRs7>d9Kaxe(!VM zIsRd8-gEu#>zwmE=gf2NdFR=S-+sfET{q8N{wJ@W+p)NN=i=_UKi{=u``pFXZ@uA~ zt-D?|H-FuZ?OW%sf5&w@=6CJ5asI~LTi&+q#`(n?-?o1E&-FXET|2*f*WwM^=6C+? z&^r9zHOCzj{_&NL7LL0! z#(jiT&QvFMKfW)G!@bIDP`OM{r8!(2GsU@L3&-JJWe*Zm(K~uDFKNelK%i}|Gf6}-&dyS zam~>3;$Iv}^TT;-6I7jc`=*uWE;&+}tLf4D8r-^h|I(8$cyJAZs(S|Fw(ak)5F9Q2 z`UtB2X7Jei;bSTuoJIQe5mbF<@Hl(R{&HpG?A|%F+;ie_E4oq~x%@nDZGtLZ*+_hH za@k~YTodP=bk*d|<68ON2cB41fBpw6BPT)C_XpzEmp@V={%vSEu=n`VLV|v=S0sY! z<}H(l#+82C zkoe^ro?E!`dmAbQTS%<`*?%v5?Ba`7-kvH{T{SQ^{ zT@qCN=|KG6t+!MNu5HqN1XX`FczpZI$5uSJXV9;YplZwDaovyiRqpmHhL(Rkyt?~v zm#W8_rc?) z*R9^6u~!j*1m(Ra+eTn-Bs{Y`ocXxPDrDwPUM_y6s)n7k; z!pd(wxY|M$={UP2sJeU8XO`=8#V1IqeD{g=T^T({P{sSWO;E*qIubl*Tz}w!W#8fK zVVj_ey^RFtit{7C`khFEs;58l>nmUR`EORPAX`Z6|Ig1Yd#%J?k)Z0?Q|DIB{ZQ@R zz3r{@E8iTSEU6+LJxEZcxV|&+dQ8)66U@u28GFTbxA}=zE#>C^7U|D(yof+Ua==s>2sX`>V#WJxW;jC7*p<*394M<_^+7+RjzT&Ca7|a zpb>_8>u(KKG6qJvcv{H}%##Uz?!HRf^mzwvbTleJpxR z6IQuOkv-VLyc&6Z_aQ-*s~_3J^$WH5JgAtfAdbWJ3#mxA3D++~ke*HGOeTVKA3+t@ zW*mnr9`c~dbq#$U3AT`MT_Y20A>q?RCVcWziz^|D+k|Ti(<;(&93-gHGn}d)xmP5p z@+o{aL6uMUbwX7RwYX}axcy|w`_E zbKcqnRjycwj)ZFqYN1~rL6z$ovpuMCm0~tQm8%qWLRAj6kl;+`k(;23S2Gf>Wypi; zvAqUe%Md}jO}LgJf^?g3%|Zm}Ho+F%;eN7UrZ{r$Q6!it5~@aAb7<>=et)i$@cIk4 zlvpRrci(k?iQ5EKV{M>9s2cIup2PZ%*9H;qea(+bOuy(sf-3el60Tp!Lucn>E;;>c z(|&D&y`py{v;(~47l+CSbtgW5$&nJbM^1t&&Rg{0XmvKYef#^1hpz0A7d%**T@qBa zJvdsO4Q~1HF(ohkVy{S0Mej)P8u&D!>p{OZK^46tp*#E&CmuIZUe0uT4_2-R393}%c+0rbqKBUJ?%8|%YObMv@yNJV`UtA% zmp%NuM%OPiZ>oyeuT4-DdK-Q-%K70es7mqdxK^SE392-XQyx9OT-ivB)ris})ul^k zAC3D6tLWGEP%Y!5d)H0$eGmFYf~v8WG12!u+5}Z&En^LWs)e`;OwYZL4hy(6Kj%JAKf zqj`s?jO5oQ*emun5~@bra-e*(yWFqdIl2DZ1XX`Ld~@Un>%LpL6IG2^eCnC%)H0g9KIVZ6sX3(7hxN@2K>fO?aQDcO=vk?@><--+K7F%?Hb9+ao7I73VE`cn5fF z@YwUZ)q9M7ZGtL#M}j+!YA3_DH_ClQzcxXYJlxwz@D3kq8O1}-yTe(u#BDtn1~tkEl~=odYhS3UXZ!s#V1={BL!?z#76N5^5hO=w21cz&)B zq$8nf#Nw;ZDJl}h?BzPaU?_Cm9(J%IjrxeodUXh?mr){s5=)u_?>kMVIBI@@Q395K)q6hP; zGI8FAR`0~IDpHwU5>(MIdZ-%lFIT>__sU5445*rn&$5bL zXHeWGsPgGP6U^(gtW>Tu%qFO!U+$HvD$3=Pw_;aCvIl!Y75$4-!;y z)}x26l&T!c<)?eaoWnLjRojDk{d6x6&PeniK^6TX!Mr|;$b;9WO;AO@NN|LlUH>eR zUz?zcvmOaOdFk0%x%@P#*r)sKL4qn*DKZgS#%hFAex{yHP(|-J4%aV~%heBi)PDy_ zLLOGpFM7D1pu8M8=dDdprE%y4kVnoI#d<21%2kos1Xb*9>=pC6uA%RHkZu#MYv|h> zq}zn+8X`!y3D-6BeGk%YLRW40jcb`J(vk2vT(jspgJQ1#HbGT;MKQ1I4D#TP5j{vy zMZZWeud5&O;7-&gsG?saI703vt~1E5O;E*Ij|B6|Pt_0Qb)6x5kf4fw(SvzK>xn~o zT@}e5B&eca^q5}T-cem=$Q~r9^65SkTD8v0J<4^4*#uSei{oHk#oiNL70DhXsG?u= z;0Rr3P+snQZGtM!VI-K>bq0HQM{N__AL$nf=5_T$9-0yFL~Vj9`bEMu3+3gVuV-JK zKlOPeT(g)am?;vjKui(Srn4 zu6|?>x|87PC3=vc%4hZL;d+8vTys#&d5az-sN%JWgzFc|>)OJ!Uz=dB=-nn9!Gkjj}d;k#OS9ZS{ruxh#|CiHo{g4zdNWB1PwsT}8C)d;Elc_y4o zdA;r!_YqP#Qzn?#D^(tj`v|F=DHG16yk6Uk`v|F=DHF`=-9R3W`v|F=DHG16yxvO~ z_YqP#Qzn?#JB~aY_YqP#Qzrb=Qh9y%G43Oza;8l9r={{*#kh}<%9%3ZpODIH72`fa zDrd?BcLuBE;kb{G%9%3ZYpc9I2{Y~^q;jTA_%y1#J{vObBcyVsO!#D~ypE-6pHi(7 zL6u{D9`{}L-RDoK9Oquu2&w#eCY(!oeg0(JM@Z#NnP6U@Yvtj%kC4ilGU2BQ<#jBT zo&fX?vDpMwj+H5154Jc~UOfTmU1-_ED&?ZearR(~W91#604g3qK$hn@}mUJS>|>rC0hD&?ZearW>Ipcco<>rC0hDiKsU&K|r{ zj+K{xsTt?WD&?ZearV&Dgx_Z6SRVY_#pq#`a#7_tdrUuN_{~;&%21r=szylV&oe=H z$5QF%x!xN!o1n^Z9tT?-D=+`rG0v4$%0-pq?7zVU==GP<-rPD^sq{~sB)Y=*y30ozIzl$537`mD#vvXeA3Du zQ_0)4{@5#xe0b+o`7W1Lo`rrwJ#nVoD_uc<578=lIPN2)a;8l9_fE=d72`faDrd@s zR+Pu#db>Ov_YqP#Qzo=C_awF1hPE`|WFv`v|F= zDHG1+kq@(5o=e9WsXSBH2<7$X*<-v?C9l??$Kkk-kjj}d;anPr_77hV$9;rU&Xft~ zl85$0Uw_AagjCLy3FnfB?+nI$gjCK{Cx$(|To0)fOC>+YaYb3>o>*l$g#Acl4_zt00nxDt`b7_`l#43I*<;*&%3L{?hchXT9#$zARgSZVR*uKvSb4|Y zr?Cc2P~|v#Xb151a4Zk{#c^1rTvR#E9=sbI%fp%STv?@DR5{KboL$AJ?Rv^ElYzSRUiIKUea*!mLs*svOULx4KpMPU=d2gPB#zHGXros2t~a#IwaQ zzA0Pruu8ec@6#5Q07>5IiCG?e5>%S;>tC*O1Z}G9v7A4 ze9hV7Sl3+Nb=JR0kncpRl#43I*+bv#*1rXzT#iZ4CYaZ8?v=iKe)L~(7-Ol1--VQa zbzwF^mE+tiee2%;j)mir7k+V_TUByV#omVZ0dRgC%fp%SIINNfRgUvGR4MXbmX0fU zB9-IpQ6r@C=b3OW#GmJ0IhS_^zdJ#zS#fhODLtXenX-pf*cr-9kE?l-XSYU3<aGX7AgjD`K6V4?M-T8jch~qv&Drd@s|Bgi-+zlM}5k)nubG<1f6Iz`|-)u6f z$I9EjudEV5mE-K8k^9Xyj^*J@c^p$EzOy E4=m-BU;qFB literal 0 HcmV?d00001 diff --git a/gazebo_plugins/test/diff_drive/xacro/laser/hokuyo.xacro b/gazebo_plugins/test/diff_drive/xacro/laser/hokuyo.xacro new file mode 100644 index 000000000..13a15266e --- /dev/null +++ b/gazebo_plugins/test/diff_drive/xacro/laser/hokuyo.xacro @@ -0,0 +1,68 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0 0 0 0 0 0 + + + + + 1 + 2.0944 + -2.0944 + 683 + + + + 0.08 + 5 + 0.01 + + + + + + front_laser/scan + front_laser + + + + 1 + 30 + true + + + + + diff --git a/gazebo_plugins/test/diff_drive/xacro/p3dx/pioneer3dx.xacro b/gazebo_plugins/test/diff_drive/xacro/p3dx/pioneer3dx.xacro new file mode 100644 index 000000000..7be97db89 --- /dev/null +++ b/gazebo_plugins/test/diff_drive/xacro/p3dx/pioneer3dx.xacro @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/gazebo_plugins/test/diff_drive/xacro/p3dx/pioneer3dx_body.xacro b/gazebo_plugins/test/diff_drive/xacro/p3dx/pioneer3dx_body.xacro new file mode 100644 index 000000000..f0e6712a9 --- /dev/null +++ b/gazebo_plugins/test/diff_drive/xacro/p3dx/pioneer3dx_body.xacro @@ -0,0 +1,373 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + chassis_swivel_joint, swivel_wheel_joint, left_hub_joint, right_hub_joint + 10.0 + true + + + + + + + false + true + true + left_hub_joint + right_hub_joint + 0.5380 + 0.18 + 20 + cmd_vel + odom + odom + base_link + 100.0 + + + + diff --git a/gazebo_plugins/test/diff_drive/xacro/p3dx/pioneer3dx_drive.xacro b/gazebo_plugins/test/diff_drive/xacro/p3dx/pioneer3dx_drive.xacro new file mode 100644 index 000000000..8fcf6a254 --- /dev/null +++ b/gazebo_plugins/test/diff_drive/xacro/p3dx/pioneer3dx_drive.xacro @@ -0,0 +1,35 @@ + + + + + + + + + + chassis_swivel_joint, swivel_wheel_joint, left_hub_joint, right_hub_joint + 10.0 + true + + + + + + + false + true + true + left_hub_joint + right_hub_joint + 0.5380 + 0.18 + 20 + cmd_vel + odom + odom + base_link + 100.0 + + + + From f77f42b4fa4da8ff555ed36a5ca78d2517f325f5 Mon Sep 17 00:00:00 2001 From: Markus Bader Date: Thu, 20 Mar 2014 16:14:56 +0100 Subject: [PATCH 044/153] Laser colour in rviz changed --- gazebo_plugins/test/diff_drive/launch/pioneer3dx.rviz | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/gazebo_plugins/test/diff_drive/launch/pioneer3dx.rviz b/gazebo_plugins/test/diff_drive/launch/pioneer3dx.rviz index 0d72dde2a..95d0f3037 100644 --- a/gazebo_plugins/test/diff_drive/launch/pioneer3dx.rviz +++ b/gazebo_plugins/test/diff_drive/launch/pioneer3dx.rviz @@ -317,7 +317,7 @@ Visualization Manager: Axis: Z Channel Name: intensity Class: rviz/LaserScan - Color: 85; 255; 0 + Color: 255; 85; 255 Color Transformer: FlatColor Decay Time: 0 Enabled: true @@ -346,7 +346,7 @@ Visualization Manager: Axis: Z Channel Name: intensity Class: rviz/LaserScan - Color: 170; 255; 0 + Color: 255; 85; 255 Color Transformer: FlatColor Decay Time: 0 Enabled: true @@ -390,7 +390,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 16.9585 + Distance: 3.79949 Enable Stereo Rendering: Stereo Eye Separation: 0.06 Stereo Focal Distance: 1 @@ -402,10 +402,10 @@ Visualization Manager: Z: -0.109698 Name: Current View Near Clip Distance: 0.01 - Pitch: 0.845398 + Pitch: 0.585398 Target Frame: Value: Orbit (rviz) - Yaw: 0.555372 + Yaw: 0.665372 Saved: ~ Window Geometry: Displays: From 6291a10a54cb98e9d80d8cdf45d1214b9d462a2a Mon Sep 17 00:00:00 2001 From: Markus Bader Date: Thu, 20 Mar 2014 20:03:56 +0100 Subject: [PATCH 045/153] fix in pioneer xacro model for diff_drive --- .../diff_drive/xacro/p3dx/pioneer3dx.xacro | 3 -- .../xacro/p3dx/pioneer3dx_drive.xacro | 35 ------------------- 2 files changed, 38 deletions(-) delete mode 100644 gazebo_plugins/test/diff_drive/xacro/p3dx/pioneer3dx_drive.xacro diff --git a/gazebo_plugins/test/diff_drive/xacro/p3dx/pioneer3dx.xacro b/gazebo_plugins/test/diff_drive/xacro/p3dx/pioneer3dx.xacro index 7be97db89..6a52153ca 100644 --- a/gazebo_plugins/test/diff_drive/xacro/p3dx/pioneer3dx.xacro +++ b/gazebo_plugins/test/diff_drive/xacro/p3dx/pioneer3dx.xacro @@ -4,9 +4,6 @@ - - - diff --git a/gazebo_plugins/test/diff_drive/xacro/p3dx/pioneer3dx_drive.xacro b/gazebo_plugins/test/diff_drive/xacro/p3dx/pioneer3dx_drive.xacro deleted file mode 100644 index 8fcf6a254..000000000 --- a/gazebo_plugins/test/diff_drive/xacro/p3dx/pioneer3dx_drive.xacro +++ /dev/null @@ -1,35 +0,0 @@ - - - - - - - - - - chassis_swivel_joint, swivel_wheel_joint, left_hub_joint, right_hub_joint - 10.0 - true - - - - - - - false - true - true - left_hub_joint - right_hub_joint - 0.5380 - 0.18 - 20 - cmd_vel - odom - odom - base_link - 100.0 - - - - From 7e1999ac3e817887ab2b9962406c4a64769844f8 Mon Sep 17 00:00:00 2001 From: Markus Bader Date: Tue, 25 Mar 2014 10:40:12 +0100 Subject: [PATCH 046/153] working camera added --- .../test/diff_drive/launch/diff_drive.launch | 4 +- .../xacro/camera/.camera.xacro.kate-swp | Bin 0 -> 143 bytes .../test/diff_drive/xacro/camera/camera.xacro | 81 +++++++++++ .../test/diff_drive/xacro/laser/hokuyo.xacro | 128 +++++++++--------- .../test/diff_drive/xacro/materials.xacro | 36 +++++ .../diff_drive/xacro/p3dx/pioneer3dx.xacro | 9 +- .../xacro/p3dx/pioneer3dx_body.xacro | 2 + 7 files changed, 194 insertions(+), 66 deletions(-) create mode 100644 gazebo_plugins/test/diff_drive/xacro/camera/.camera.xacro.kate-swp create mode 100644 gazebo_plugins/test/diff_drive/xacro/camera/camera.xacro create mode 100644 gazebo_plugins/test/diff_drive/xacro/materials.xacro diff --git a/gazebo_plugins/test/diff_drive/launch/diff_drive.launch b/gazebo_plugins/test/diff_drive/launch/diff_drive.launch index abf99065d..9c11740bb 100644 --- a/gazebo_plugins/test/diff_drive/launch/diff_drive.launch +++ b/gazebo_plugins/test/diff_drive/launch/diff_drive.launch @@ -7,12 +7,12 @@ - + - + diff --git a/gazebo_plugins/test/diff_drive/xacro/camera/.camera.xacro.kate-swp b/gazebo_plugins/test/diff_drive/xacro/camera/.camera.xacro.kate-swp new file mode 100644 index 0000000000000000000000000000000000000000..dc9f4f3fb4717321e13adc312b7090f453b90820 GIT binary patch literal 143 zcmZQzU=Z?7EJ;-eE>A2_aLdd|RWQ;sU|?VnF!^(Dxx>~>_KaIU+lrc+wS$9zqEbLC w2E;O+NF0{*qSQ)Pu)GRL4v3XN@-PmPJTFj~5r~-}%0XNZU~~-*1ya1O0E*Ta761SM literal 0 HcmV?d00001 diff --git a/gazebo_plugins/test/diff_drive/xacro/camera/camera.xacro b/gazebo_plugins/test/diff_drive/xacro/camera/camera.xacro new file mode 100644 index 000000000..73c2e6b52 --- /dev/null +++ b/gazebo_plugins/test/diff_drive/xacro/camera/camera.xacro @@ -0,0 +1,81 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 30.0 + + 1.3962634 + + 800 + 800 + R8G8B8 + + + 0.02 + 300 + + + gaussian + + 0.0 + 0.007 + + + + true + 0.0 + camera + image_raw + camera_info + front_camera + 0.07 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + + + + + diff --git a/gazebo_plugins/test/diff_drive/xacro/laser/hokuyo.xacro b/gazebo_plugins/test/diff_drive/xacro/laser/hokuyo.xacro index 13a15266e..d1d10648f 100644 --- a/gazebo_plugins/test/diff_drive/xacro/laser/hokuyo.xacro +++ b/gazebo_plugins/test/diff_drive/xacro/laser/hokuyo.xacro @@ -1,68 +1,70 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 0 0 0 0 0 0 - - - - - 1 - 2.0944 - -2.0944 - 683 - - - - 0.08 - 5 - 0.01 - - - - - - front_laser/scan - front_laser - - - - 1 - 30 - true - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0 0 0 0 0 0 + + + + + 1 + 2.0944 + -2.0944 + 683 + + + + 0.08 + 5 + 0.01 + + + + + + front_laser/scan + front_laser + + + + 1 + 30 + true + + + + diff --git a/gazebo_plugins/test/diff_drive/xacro/materials.xacro b/gazebo_plugins/test/diff_drive/xacro/materials.xacro new file mode 100644 index 000000000..311c3cdd8 --- /dev/null +++ b/gazebo_plugins/test/diff_drive/xacro/materials.xacro @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gazebo_plugins/test/diff_drive/xacro/p3dx/pioneer3dx.xacro b/gazebo_plugins/test/diff_drive/xacro/p3dx/pioneer3dx.xacro index 6a52153ca..1b7fb81f5 100644 --- a/gazebo_plugins/test/diff_drive/xacro/p3dx/pioneer3dx.xacro +++ b/gazebo_plugins/test/diff_drive/xacro/p3dx/pioneer3dx.xacro @@ -1,10 +1,17 @@ + + + + - + + + + diff --git a/gazebo_plugins/test/diff_drive/xacro/p3dx/pioneer3dx_body.xacro b/gazebo_plugins/test/diff_drive/xacro/p3dx/pioneer3dx_body.xacro index f0e6712a9..780e901bb 100644 --- a/gazebo_plugins/test/diff_drive/xacro/p3dx/pioneer3dx_body.xacro +++ b/gazebo_plugins/test/diff_drive/xacro/p3dx/pioneer3dx_body.xacro @@ -369,5 +369,7 @@ 100.0 + + From e2f70ff87b9b20185a65807f9960187f31eb4892 Mon Sep 17 00:00:00 2001 From: Markus Bader Date: Tue, 25 Mar 2014 10:42:14 +0100 Subject: [PATCH 047/153] diff drive name changed to multi robot scenario --- .../launch/diff_drive.launch | 0 .../launch/pioneer3dx.gazebo.launch | 0 .../launch/pioneer3dx.rviz | 0 .../launch/pioneer3dx.urdf.launch | 0 .../meshes/laser/hokuyo.dae | 0 .../meshes/p3dx/Coordinates | 0 .../meshes/p3dx/back_rim.stl | Bin .../meshes/p3dx/back_sonar.stl | Bin .../meshes/p3dx/center_hubcap.stl | Bin .../meshes/p3dx/center_wheel.stl | Bin .../meshes/p3dx/chassis.stl | Bin .../meshes/p3dx/front_rim.stl | Bin .../meshes/p3dx/front_sonar.stl | Bin .../meshes/p3dx/left_hubcap.stl | Bin .../meshes/p3dx/left_wheel.stl | Bin .../meshes/p3dx/right_hubcap.stl | Bin .../meshes/p3dx/right_wheel.stl | Bin .../meshes/p3dx/swivel.stl | Bin .../meshes/p3dx/top.stl | Bin .../xacro/camera/.camera.xacro.kate-swp | Bin .../xacro/camera/camera.xacro | 0 .../xacro/laser/hokuyo.xacro | 0 .../xacro/materials.xacro | 0 .../xacro/p3dx/pioneer3dx.xacro | 0 .../xacro/p3dx/pioneer3dx_body.xacro | 0 25 files changed, 0 insertions(+), 0 deletions(-) rename gazebo_plugins/test/{diff_drive => multi_robot_scenario}/launch/diff_drive.launch (100%) rename gazebo_plugins/test/{diff_drive => multi_robot_scenario}/launch/pioneer3dx.gazebo.launch (100%) rename gazebo_plugins/test/{diff_drive => multi_robot_scenario}/launch/pioneer3dx.rviz (100%) rename gazebo_plugins/test/{diff_drive => multi_robot_scenario}/launch/pioneer3dx.urdf.launch (100%) rename gazebo_plugins/test/{diff_drive => multi_robot_scenario}/meshes/laser/hokuyo.dae (100%) rename gazebo_plugins/test/{diff_drive => multi_robot_scenario}/meshes/p3dx/Coordinates (100%) rename gazebo_plugins/test/{diff_drive => multi_robot_scenario}/meshes/p3dx/back_rim.stl (100%) rename gazebo_plugins/test/{diff_drive => multi_robot_scenario}/meshes/p3dx/back_sonar.stl (100%) rename gazebo_plugins/test/{diff_drive => multi_robot_scenario}/meshes/p3dx/center_hubcap.stl (100%) rename gazebo_plugins/test/{diff_drive => multi_robot_scenario}/meshes/p3dx/center_wheel.stl (100%) rename gazebo_plugins/test/{diff_drive => multi_robot_scenario}/meshes/p3dx/chassis.stl (100%) rename gazebo_plugins/test/{diff_drive => multi_robot_scenario}/meshes/p3dx/front_rim.stl (100%) rename gazebo_plugins/test/{diff_drive => multi_robot_scenario}/meshes/p3dx/front_sonar.stl (100%) rename gazebo_plugins/test/{diff_drive => multi_robot_scenario}/meshes/p3dx/left_hubcap.stl (100%) rename gazebo_plugins/test/{diff_drive => multi_robot_scenario}/meshes/p3dx/left_wheel.stl (100%) rename gazebo_plugins/test/{diff_drive => multi_robot_scenario}/meshes/p3dx/right_hubcap.stl (100%) rename gazebo_plugins/test/{diff_drive => multi_robot_scenario}/meshes/p3dx/right_wheel.stl (100%) rename gazebo_plugins/test/{diff_drive => multi_robot_scenario}/meshes/p3dx/swivel.stl (100%) rename gazebo_plugins/test/{diff_drive => multi_robot_scenario}/meshes/p3dx/top.stl (100%) rename gazebo_plugins/test/{diff_drive => multi_robot_scenario}/xacro/camera/.camera.xacro.kate-swp (100%) rename gazebo_plugins/test/{diff_drive => multi_robot_scenario}/xacro/camera/camera.xacro (100%) rename gazebo_plugins/test/{diff_drive => multi_robot_scenario}/xacro/laser/hokuyo.xacro (100%) rename gazebo_plugins/test/{diff_drive => multi_robot_scenario}/xacro/materials.xacro (100%) rename gazebo_plugins/test/{diff_drive => multi_robot_scenario}/xacro/p3dx/pioneer3dx.xacro (100%) rename gazebo_plugins/test/{diff_drive => multi_robot_scenario}/xacro/p3dx/pioneer3dx_body.xacro (100%) diff --git a/gazebo_plugins/test/diff_drive/launch/diff_drive.launch b/gazebo_plugins/test/multi_robot_scenario/launch/diff_drive.launch similarity index 100% rename from gazebo_plugins/test/diff_drive/launch/diff_drive.launch rename to gazebo_plugins/test/multi_robot_scenario/launch/diff_drive.launch diff --git a/gazebo_plugins/test/diff_drive/launch/pioneer3dx.gazebo.launch b/gazebo_plugins/test/multi_robot_scenario/launch/pioneer3dx.gazebo.launch similarity index 100% rename from gazebo_plugins/test/diff_drive/launch/pioneer3dx.gazebo.launch rename to gazebo_plugins/test/multi_robot_scenario/launch/pioneer3dx.gazebo.launch diff --git a/gazebo_plugins/test/diff_drive/launch/pioneer3dx.rviz b/gazebo_plugins/test/multi_robot_scenario/launch/pioneer3dx.rviz similarity index 100% rename from gazebo_plugins/test/diff_drive/launch/pioneer3dx.rviz rename to gazebo_plugins/test/multi_robot_scenario/launch/pioneer3dx.rviz diff --git a/gazebo_plugins/test/diff_drive/launch/pioneer3dx.urdf.launch b/gazebo_plugins/test/multi_robot_scenario/launch/pioneer3dx.urdf.launch similarity index 100% rename from gazebo_plugins/test/diff_drive/launch/pioneer3dx.urdf.launch rename to gazebo_plugins/test/multi_robot_scenario/launch/pioneer3dx.urdf.launch diff --git a/gazebo_plugins/test/diff_drive/meshes/laser/hokuyo.dae b/gazebo_plugins/test/multi_robot_scenario/meshes/laser/hokuyo.dae similarity index 100% rename from gazebo_plugins/test/diff_drive/meshes/laser/hokuyo.dae rename to gazebo_plugins/test/multi_robot_scenario/meshes/laser/hokuyo.dae diff --git a/gazebo_plugins/test/diff_drive/meshes/p3dx/Coordinates b/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/Coordinates similarity index 100% rename from gazebo_plugins/test/diff_drive/meshes/p3dx/Coordinates rename to gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/Coordinates diff --git a/gazebo_plugins/test/diff_drive/meshes/p3dx/back_rim.stl b/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/back_rim.stl similarity index 100% rename from gazebo_plugins/test/diff_drive/meshes/p3dx/back_rim.stl rename to gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/back_rim.stl diff --git a/gazebo_plugins/test/diff_drive/meshes/p3dx/back_sonar.stl b/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/back_sonar.stl similarity index 100% rename from gazebo_plugins/test/diff_drive/meshes/p3dx/back_sonar.stl rename to gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/back_sonar.stl diff --git a/gazebo_plugins/test/diff_drive/meshes/p3dx/center_hubcap.stl b/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/center_hubcap.stl similarity index 100% rename from gazebo_plugins/test/diff_drive/meshes/p3dx/center_hubcap.stl rename to gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/center_hubcap.stl diff --git a/gazebo_plugins/test/diff_drive/meshes/p3dx/center_wheel.stl b/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/center_wheel.stl similarity index 100% rename from gazebo_plugins/test/diff_drive/meshes/p3dx/center_wheel.stl rename to gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/center_wheel.stl diff --git a/gazebo_plugins/test/diff_drive/meshes/p3dx/chassis.stl b/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/chassis.stl similarity index 100% rename from gazebo_plugins/test/diff_drive/meshes/p3dx/chassis.stl rename to gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/chassis.stl diff --git a/gazebo_plugins/test/diff_drive/meshes/p3dx/front_rim.stl b/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/front_rim.stl similarity index 100% rename from gazebo_plugins/test/diff_drive/meshes/p3dx/front_rim.stl rename to gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/front_rim.stl diff --git a/gazebo_plugins/test/diff_drive/meshes/p3dx/front_sonar.stl b/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/front_sonar.stl similarity index 100% rename from gazebo_plugins/test/diff_drive/meshes/p3dx/front_sonar.stl rename to gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/front_sonar.stl diff --git a/gazebo_plugins/test/diff_drive/meshes/p3dx/left_hubcap.stl b/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/left_hubcap.stl similarity index 100% rename from gazebo_plugins/test/diff_drive/meshes/p3dx/left_hubcap.stl rename to gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/left_hubcap.stl diff --git a/gazebo_plugins/test/diff_drive/meshes/p3dx/left_wheel.stl b/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/left_wheel.stl similarity index 100% rename from gazebo_plugins/test/diff_drive/meshes/p3dx/left_wheel.stl rename to gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/left_wheel.stl diff --git a/gazebo_plugins/test/diff_drive/meshes/p3dx/right_hubcap.stl b/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/right_hubcap.stl similarity index 100% rename from gazebo_plugins/test/diff_drive/meshes/p3dx/right_hubcap.stl rename to gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/right_hubcap.stl diff --git a/gazebo_plugins/test/diff_drive/meshes/p3dx/right_wheel.stl b/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/right_wheel.stl similarity index 100% rename from gazebo_plugins/test/diff_drive/meshes/p3dx/right_wheel.stl rename to gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/right_wheel.stl diff --git a/gazebo_plugins/test/diff_drive/meshes/p3dx/swivel.stl b/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/swivel.stl similarity index 100% rename from gazebo_plugins/test/diff_drive/meshes/p3dx/swivel.stl rename to gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/swivel.stl diff --git a/gazebo_plugins/test/diff_drive/meshes/p3dx/top.stl b/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/top.stl similarity index 100% rename from gazebo_plugins/test/diff_drive/meshes/p3dx/top.stl rename to gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/top.stl diff --git a/gazebo_plugins/test/diff_drive/xacro/camera/.camera.xacro.kate-swp b/gazebo_plugins/test/multi_robot_scenario/xacro/camera/.camera.xacro.kate-swp similarity index 100% rename from gazebo_plugins/test/diff_drive/xacro/camera/.camera.xacro.kate-swp rename to gazebo_plugins/test/multi_robot_scenario/xacro/camera/.camera.xacro.kate-swp diff --git a/gazebo_plugins/test/diff_drive/xacro/camera/camera.xacro b/gazebo_plugins/test/multi_robot_scenario/xacro/camera/camera.xacro similarity index 100% rename from gazebo_plugins/test/diff_drive/xacro/camera/camera.xacro rename to gazebo_plugins/test/multi_robot_scenario/xacro/camera/camera.xacro diff --git a/gazebo_plugins/test/diff_drive/xacro/laser/hokuyo.xacro b/gazebo_plugins/test/multi_robot_scenario/xacro/laser/hokuyo.xacro similarity index 100% rename from gazebo_plugins/test/diff_drive/xacro/laser/hokuyo.xacro rename to gazebo_plugins/test/multi_robot_scenario/xacro/laser/hokuyo.xacro diff --git a/gazebo_plugins/test/diff_drive/xacro/materials.xacro b/gazebo_plugins/test/multi_robot_scenario/xacro/materials.xacro similarity index 100% rename from gazebo_plugins/test/diff_drive/xacro/materials.xacro rename to gazebo_plugins/test/multi_robot_scenario/xacro/materials.xacro diff --git a/gazebo_plugins/test/diff_drive/xacro/p3dx/pioneer3dx.xacro b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx.xacro similarity index 100% rename from gazebo_plugins/test/diff_drive/xacro/p3dx/pioneer3dx.xacro rename to gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx.xacro diff --git a/gazebo_plugins/test/diff_drive/xacro/p3dx/pioneer3dx_body.xacro b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_body.xacro similarity index 100% rename from gazebo_plugins/test/diff_drive/xacro/p3dx/pioneer3dx_body.xacro rename to gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_body.xacro From 050b9e8044ed076d9f9072c69905a880a4be4ede Mon Sep 17 00:00:00 2001 From: Markus Bader Date: Tue, 25 Mar 2014 10:48:42 +0100 Subject: [PATCH 048/153] fixed links related to changed name --- ...ive.launch => multi_robot_scenario.launch} | 6 ++-- .../launch/pioneer3dx.gazebo.launch | 2 +- .../xacro/camera/.camera.xacro.kate-swp | Bin 143 -> 0 bytes .../xacro/camera/camera.xacro | 5 ++-- .../xacro/laser/hokuyo.xacro | 2 +- .../xacro/p3dx/pioneer3dx.xacro | 8 ++--- .../xacro/p3dx/pioneer3dx_body.xacro | 28 +++++++++--------- 7 files changed, 25 insertions(+), 26 deletions(-) rename gazebo_plugins/test/multi_robot_scenario/launch/{diff_drive.launch => multi_robot_scenario.launch} (74%) delete mode 100644 gazebo_plugins/test/multi_robot_scenario/xacro/camera/.camera.xacro.kate-swp diff --git a/gazebo_plugins/test/multi_robot_scenario/launch/diff_drive.launch b/gazebo_plugins/test/multi_robot_scenario/launch/multi_robot_scenario.launch similarity index 74% rename from gazebo_plugins/test/multi_robot_scenario/launch/diff_drive.launch rename to gazebo_plugins/test/multi_robot_scenario/launch/multi_robot_scenario.launch index 9c11740bb..5c3299de9 100644 --- a/gazebo_plugins/test/multi_robot_scenario/launch/diff_drive.launch +++ b/gazebo_plugins/test/multi_robot_scenario/launch/multi_robot_scenario.launch @@ -5,12 +5,12 @@ - + - + @@ -23,6 +23,6 @@ args="0.0 0.0 0.0 0.0 0.0 0.0 base_link r2/odom 200" /> - + diff --git a/gazebo_plugins/test/multi_robot_scenario/launch/pioneer3dx.gazebo.launch b/gazebo_plugins/test/multi_robot_scenario/launch/pioneer3dx.gazebo.launch index 9424fe5d4..35dd47a15 100644 --- a/gazebo_plugins/test/multi_robot_scenario/launch/pioneer3dx.gazebo.launch +++ b/gazebo_plugins/test/multi_robot_scenario/launch/pioneer3dx.gazebo.launch @@ -7,7 +7,7 @@ - + diff --git a/gazebo_plugins/test/multi_robot_scenario/xacro/camera/.camera.xacro.kate-swp b/gazebo_plugins/test/multi_robot_scenario/xacro/camera/.camera.xacro.kate-swp deleted file mode 100644 index dc9f4f3fb4717321e13adc312b7090f453b90820..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 143 zcmZQzU=Z?7EJ;-eE>A2_aLdd|RWQ;sU|?VnF!^(Dxx>~>_KaIU+lrc+wS$9zqEbLC w2E;O+NF0{*qSQ)Pu)GRL4v3XN@-PmPJTFj~5r~-}%0XNZU~~-*1ya1O0E*Ta761SM diff --git a/gazebo_plugins/test/multi_robot_scenario/xacro/camera/camera.xacro b/gazebo_plugins/test/multi_robot_scenario/xacro/camera/camera.xacro index 73c2e6b52..531e7d930 100644 --- a/gazebo_plugins/test/multi_robot_scenario/xacro/camera/camera.xacro +++ b/gazebo_plugins/test/multi_robot_scenario/xacro/camera/camera.xacro @@ -11,7 +11,6 @@ - @@ -24,7 +23,7 @@ - + @@ -34,7 +33,7 @@ - + diff --git a/gazebo_plugins/test/multi_robot_scenario/xacro/laser/hokuyo.xacro b/gazebo_plugins/test/multi_robot_scenario/xacro/laser/hokuyo.xacro index d1d10648f..e25639895 100644 --- a/gazebo_plugins/test/multi_robot_scenario/xacro/laser/hokuyo.xacro +++ b/gazebo_plugins/test/multi_robot_scenario/xacro/laser/hokuyo.xacro @@ -21,7 +21,7 @@ - + diff --git a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx.xacro b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx.xacro index 1b7fb81f5..7d5c0ddc1 100644 --- a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx.xacro +++ b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx.xacro @@ -3,15 +3,15 @@ - + - + - + - + diff --git a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_body.xacro b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_body.xacro index 780e901bb..160e2dc11 100644 --- a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_body.xacro +++ b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_body.xacro @@ -21,7 +21,7 @@ - + @@ -30,7 +30,7 @@ - + @@ -59,7 +59,7 @@ - + @@ -90,7 +90,7 @@ - + @@ -126,7 +126,7 @@ - + @@ -135,7 +135,7 @@ - + @@ -159,7 +159,7 @@ - + @@ -192,7 +192,7 @@ - + @@ -225,7 +225,7 @@ - + @@ -234,7 +234,7 @@ - + @@ -259,7 +259,7 @@ - + @@ -292,7 +292,7 @@ - + @@ -301,7 +301,7 @@ - + @@ -325,7 +325,7 @@ - + From 53a14e90e19616bcf2c5dec60f90c5197b1085fc Mon Sep 17 00:00:00 2001 From: Markus Bader Date: Tue, 25 Mar 2014 13:00:42 +0100 Subject: [PATCH 049/153] fixed camera to work with workspaces --- .../gazebo_plugins/gazebo_ros_camera_utils.h | 3 + .../gazebo_plugins/gazebo_ros_sensor_util.h | 73 +++++++++ .../src/gazebo_ros_camera_utils.cpp | 29 ++-- gazebo_plugins/src/gazebo_ros_laser.cpp | 45 ++---- .../launch/multi_robot_scenario.launch | 2 +- .../launch/pioneer3dx.rviz | 142 +++++------------- .../xacro/camera/camera.xacro | 3 +- 7 files changed, 146 insertions(+), 151 deletions(-) create mode 100644 gazebo_plugins/include/gazebo_plugins/gazebo_ros_sensor_util.h diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera_utils.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera_utils.h index d74cb992e..e3c73965d 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera_utils.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera_utils.h @@ -113,6 +113,9 @@ namespace gazebo /// \brief ROS camera name private: std::string camera_name_; + /// \brief tf prefix + private: std::string tf_prefix_; + /// \brief ROS image topic name protected: std::string image_topic_name_; diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_sensor_util.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_sensor_util.h new file mode 100644 index 000000000..9f4f7262c --- /dev/null +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_sensor_util.h @@ -0,0 +1,73 @@ +/*************************************************************************** + * Copyright (C) 2014 by Markus Bader * + * markus.bader@tuwien.ac.at * + * * + * This program is free software; you can redistribute it and/or modify * + * it under the terms of the GNU General Public License as published by * + * the Free Software Foundation; either version 2 of the License, or * + * (at your option) any later version. * + * * + * This program is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU General Public License for more details. * + * * + * You should have received a copy of the GNU General Public License * + * along with this program; if not, write to the * + * Free Software Foundation, Inc., * + * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * + ***************************************************************************/ + + + +#ifndef GAZEBO_ROS_SENSOR_UTIL +#define GAZEBO_ROS_SENSOR_UTIL + + +#include +#include + +namespace gazebo +{ + +/** + * @brief accessing model name like suggested by nkoenig at http://answers.gazebosim.org/question/4878/multiple-robots-with-ros-plugins-sensor-plugin-vs/ + * @Author: Markus Bader + * @return accessing model name + **/ +inline std::string GetModelName(const sensors::SensorPtr &parent) { + std::string modelName; + std::vector values; + std::string scopedName = parent->GetScopedName(); + boost::replace_all(scopedName, "::", ","); + boost::split(values, scopedName, boost::is_any_of(",")); + if(values.size() < 2) { + modelName = ""; + } else { + modelName = values[1]; + } + return modelName; +} + +inline std::string GetRobotNamespace(const sensors::SensorPtr &parent, const sdf::ElementPtr &sdf, const char *pInfo = NULL) { + std::string name_space; + std::stringstream ss; + if (sdf->HasElement("robotNamespace")) { + name_space = sdf->Get("robotNamespace"); + if(name_space.empty()) { + ss << "the 'robotNamespace' param was empty"; + name_space = GetModelName(parent); + } else { + ss << "Using the 'robotNamespace' param: '" << name_space << "'"; + } + } else { + ss << "the 'robotNamespace' param did not exit"; + } + if(pInfo != NULL) { + ROS_INFO ( "%s Plugin (robotNamespace = %s), Info: %s" , pInfo, name_space.c_str(), ss.str().c_str() ); + } + return name_space; +} +} + +#endif //GAZEBO_ROS_SENSOR_UTIL diff --git a/gazebo_plugins/src/gazebo_ros_camera_utils.cpp b/gazebo_plugins/src/gazebo_ros_camera_utils.cpp index 70edb4362..3e30842d3 100644 --- a/gazebo_plugins/src/gazebo_ros_camera_utils.cpp +++ b/gazebo_plugins/src/gazebo_ros_camera_utils.cpp @@ -22,6 +22,7 @@ #include #include +#include #include #include #include @@ -38,6 +39,7 @@ #include #include +#include "gazebo_plugins/gazebo_ros_sensor_util.h" #include "gazebo_plugins/gazebo_ros_camera_utils.h" namespace gazebo @@ -115,10 +117,9 @@ void GazeboRosCameraUtils::Load(sensors::SensorPtr _parent, // pr2_gazebo_plugins this->world = this->world_; - this->robot_namespace_ = ""; - if (this->sdf->HasElement("robotNamespace")) - this->robot_namespace_ = this->sdf->Get("robotNamespace") + "/"; - + std::stringstream ss; + this->robot_namespace_ = GetRobotNamespace(_parent, _sdf, "Camera"); + this->image_topic_name_ = "image_raw"; if (this->sdf->HasElement("imageTopicName")) this->image_topic_name_ = this->sdf->Get("imageTopicName"); @@ -266,12 +267,8 @@ void GazeboRosCameraUtils::LoadThread() // associated ROS topics. this->parentSensor_->SetActive(false); - this->rosnode_ = new ros::NodeHandle(this->robot_namespace_ + - "/" + this->camera_name_); - - this->itnode_ = new image_transport::ImageTransport(*this->rosnode_); + this->rosnode_ = new ros::NodeHandle(this->robot_namespace_ + "/" + this->camera_name_); - // resolve tf prefix std::string key; if(this->rosnode_->searchParam("tf_prefix", key)){ std::string prefix; @@ -279,6 +276,20 @@ void GazeboRosCameraUtils::LoadThread() this->frame_name_ = tf::resolve(prefix, this->frame_name_); } + this->itnode_ = new image_transport::ImageTransport(*this->rosnode_); + + // resolve tf prefix + this->tf_prefix_ = tf::getPrefixParam(*this->rosnode_); + if(this->tf_prefix_.empty()) { + this->tf_prefix_ = this->robot_namespace_; + boost::trim_right_if(this->tf_prefix_,boost::is_any_of("/")); + } + this->frame_name_ = tf::resolve(this->tf_prefix_, this->frame_name_); + + ROS_INFO("Camera Plugin (ns = %s) , set to \"%s\"", + this->robot_namespace_.c_str(), this->tf_prefix_.c_str()); + + if (!this->camera_name_.empty()) { dyn_srv_ = diff --git a/gazebo_plugins/src/gazebo_ros_laser.cpp b/gazebo_plugins/src/gazebo_ros_laser.cpp index bd0888e95..bdbc35ddf 100644 --- a/gazebo_plugins/src/gazebo_ros_laser.cpp +++ b/gazebo_plugins/src/gazebo_ros_laser.cpp @@ -38,6 +38,7 @@ #include #include +#include namespace gazebo { @@ -71,36 +72,13 @@ void GazeboRosLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) // save pointers this->sdf = _sdf; - // accessing model name like suggested by nkoenig at http://answers.gazebosim.org/question/4878/multiple-robots-with-ros-plugins-sensor-plugin-vs/ - std::vector values; - std::string scopedName = _parent->GetScopedName(); - boost::replace_all(scopedName, "::", ","); - boost::split(values, scopedName, boost::is_any_of(",")); - std::string modelName; - if(values.size() < 2){ - modelName = ""; - } else { - modelName = values[1]; - } - - this->parent_ray_sensor_ = boost::dynamic_pointer_cast(_parent); if (!this->parent_ray_sensor_) gzthrow("GazeboRosLaser controller requires a Ray Sensor as its parent"); - this->robot_namespace_ = modelName; - if (this->sdf->HasElement("robotNamespace")){ - std::string tmp = this->sdf->Get("robotNamespace"); - if(!tmp.empty()){ - this->robot_namespace_ = tmp; - } - } - this->rosnode_ = new ros::NodeHandle(this->robot_namespace_); - - - ROS_INFO ( "Laser Plugin (robotNamespace = %s)" , this->robot_namespace_.c_str() ); + this->robot_namespace_ = GetRobotNamespace(_parent, _sdf, "Laser"); if (!this->sdf->HasElement("frameName")) { @@ -109,16 +87,7 @@ void GazeboRosLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) } else this->frame_name_ = this->sdf->Get("frameName"); - - this->tf_prefix_ = tf::getPrefixParam(*this->rosnode_); - if(this->tf_prefix_.empty()) { - this->tf_prefix_ = this->robot_namespace_; - boost::trim_right_if(this->tf_prefix_,boost::is_any_of("/")); - } - ROS_INFO("Laser Plugin (ns = %s) , set to \"%s\"", - this->robot_namespace_.c_str(), this->tf_prefix_.c_str()); - if (!this->sdf->HasElement("topicName")) { @@ -154,6 +123,16 @@ void GazeboRosLaser::LoadThread() this->pmq.startServiceThread(); + this->rosnode_ = new ros::NodeHandle(this->robot_namespace_); + + this->tf_prefix_ = tf::getPrefixParam(*this->rosnode_); + if(this->tf_prefix_.empty()) { + this->tf_prefix_ = this->robot_namespace_; + boost::trim_right_if(this->tf_prefix_,boost::is_any_of("/")); + } + ROS_INFO("Laser Plugin (ns = %s) , set to \"%s\"", + this->robot_namespace_.c_str(), this->tf_prefix_.c_str()); + // resolve tf prefix this->frame_name_ = tf::resolve(this->tf_prefix_, this->frame_name_); diff --git a/gazebo_plugins/test/multi_robot_scenario/launch/multi_robot_scenario.launch b/gazebo_plugins/test/multi_robot_scenario/launch/multi_robot_scenario.launch index 5c3299de9..ddc9f79d4 100644 --- a/gazebo_plugins/test/multi_robot_scenario/launch/multi_robot_scenario.launch +++ b/gazebo_plugins/test/multi_robot_scenario/launch/multi_robot_scenario.launch @@ -13,7 +13,7 @@ - + - + @@ -60,6 +60,7 @@ + rr true 0.0 camera From 2b065c33450852f69f53b1ee963d3b06730cbdf4 Mon Sep 17 00:00:00 2001 From: Markus Bader Date: Tue, 25 Mar 2014 13:07:38 +0100 Subject: [PATCH 050/153] fixed camera to work with workspaces --- .../test/multi_robot_scenario/xacro/camera/camera.xacro | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gazebo_plugins/test/multi_robot_scenario/xacro/camera/camera.xacro b/gazebo_plugins/test/multi_robot_scenario/xacro/camera/camera.xacro index 79869589e..f71c4a960 100644 --- a/gazebo_plugins/test/multi_robot_scenario/xacro/camera/camera.xacro +++ b/gazebo_plugins/test/multi_robot_scenario/xacro/camera/camera.xacro @@ -60,7 +60,7 @@ - rr + true 0.0 camera From 8440a0c5c9ac8078fb4b0bdfc1eebff495340c1e Mon Sep 17 00:00:00 2001 From: Dejan Pangercic Date: Tue, 25 Mar 2014 22:24:26 -0700 Subject: [PATCH 051/153] this corrects the right orientation of the laser scan and improves on comparison between 2 double numbers --- gazebo_plugins/src/gazebo_ros_block_laser.cpp | 26 +++++++++---------- 1 file changed, 12 insertions(+), 14 deletions(-) diff --git a/gazebo_plugins/src/gazebo_ros_block_laser.cpp b/gazebo_plugins/src/gazebo_ros_block_laser.cpp index b57c13aae..d8f40bc6a 100644 --- a/gazebo_plugins/src/gazebo_ros_block_laser.cpp +++ b/gazebo_plugins/src/gazebo_ros_block_laser.cpp @@ -40,6 +40,8 @@ #include +#define EPSILON_DIFF 0.000001 + namespace gazebo { // Register this plugin with the simulator @@ -321,31 +323,27 @@ void GazeboRosBlockLaser::PutLaserData(common::Time &_updateTime) /* point scan from laser */ /* */ /***************************************************************/ - if (r == maxRange - minRange) + //compare 2 doubles + double diffRange = maxRange - minRange; + double diff = diffRange - r; + if (fabs(diff) < EPSILON_DIFF) { // no noise if at max range geometry_msgs::Point32 point; - point.x = (r+minRange) * cos(pAngle)*cos(yAngle); - point.y = -(r+minRange) * sin(yAngle); - point.z = (r+minRange) * sin(pAngle)*cos(yAngle); - //pAngle is rotated by yAngle: - point.x = (r+minRange) * cos(pAngle) * cos(yAngle); - point.y = -(r+minRange) * cos(pAngle) * sin(yAngle); - point.z = (r+minRange) * sin(pAngle); + point.x = r * cos(pAngle) * cos(yAngle); + point.y = r * cos(pAngle) * sin(yAngle); + point.z = -r * sin(pAngle); this->cloud_msg_.points.push_back(point); } else { geometry_msgs::Point32 point; - point.x = (r+minRange) * cos(pAngle)*cos(yAngle) + this->GaussianKernel(0,this->gaussian_noise_) ; - point.y = -(r+minRange) * sin(yAngle) + this->GaussianKernel(0,this->gaussian_noise_) ; - point.z = (r+minRange) * sin(pAngle)*cos(yAngle) + this->GaussianKernel(0,this->gaussian_noise_) ; //pAngle is rotated by yAngle: - point.x = (r+minRange) * cos(pAngle) * cos(yAngle) + this->GaussianKernel(0,this->gaussian_noise_); - point.y = -(r+minRange) * cos(pAngle) * sin(yAngle) + this->GaussianKernel(0,this->gaussian_noise_); - point.z = (r+minRange) * sin(pAngle) + this->GaussianKernel(0,this->gaussian_noise_); + point.x = r * cos(pAngle) * cos(yAngle) + this->GaussianKernel(0,this->gaussian_noise_); + point.y = r * cos(pAngle) * sin(yAngle) + this->GaussianKernel(0,this->gaussian_noise_); + point.z = -r * sin(pAngle) + this->GaussianKernel(0,this->gaussian_noise_); this->cloud_msg_.points.push_back(point); } // only 1 channel From c2c6d79848c0e682c535f2a22c9c2f66e380a1b3 Mon Sep 17 00:00:00 2001 From: John Hsu Date: Wed, 26 Mar 2014 00:22:09 -0700 Subject: [PATCH 052/153] update test world for block laser --- .../test_worlds/gazebo_ros_block_laser.world | 30 +++++++++++-------- 1 file changed, 18 insertions(+), 12 deletions(-) diff --git a/gazebo_plugins/test/test_worlds/gazebo_ros_block_laser.world b/gazebo_plugins/test/test_worlds/gazebo_ros_block_laser.world index 68b765d12..905b3fc5a 100644 --- a/gazebo_plugins/test/test_worlds/gazebo_ros_block_laser.world +++ b/gazebo_plugins/test/test_worlds/gazebo_ros_block_laser.world @@ -36,7 +36,6 @@ 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 - 1.0 0.0 @@ -54,10 +53,12 @@ 0.05 0.05 0.05 + true 100.0 @@ -143,7 +144,6 @@ 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 - 1.0 0.0 @@ -161,10 +161,12 @@ 0.05 0.05 0.05 + true 100.0 @@ -245,7 +247,6 @@ 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 - 1.0 0.0 @@ -260,14 +261,16 @@ 0.0 0.1 0.0 0.0 0.0 0.0 - pr2/gripper_v0/l_finger_tip.stl + pr2/gripper_v0/l_finger_tip.stl 3 3 3 + true 100.0 @@ -276,7 +279,7 @@ 250 - pr2/gripper_v0/l_finger_tip.stl + pr2/gripper_v0/l_finger_tip.stl 3 3 3 @@ -311,7 +314,7 @@ 0.0 -0.1 0.0 0.0 0.0 0.0 - pr2/gripper_v0/l_finger_tip.dae + pr2/gripper_v0/l_finger_tip.dae 3 3 3 @@ -323,7 +326,7 @@ 250 - pr2/gripper_v0/l_finger_tip.dae + pr2/gripper_v0/l_finger_tip.dae 3 3 3 @@ -366,7 +369,6 @@ -0.1 -0.5 -0.5 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 - 1.0 0.0 @@ -384,10 +386,12 @@ 0.1 0.25 0.5 + true 100.0 @@ -434,7 +438,6 @@ 0.0 0.5 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 - 1.0 0.0 @@ -452,10 +455,12 @@ 0.05 + true 100.0 @@ -502,7 +507,6 @@ 0.5 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 - 1.0 0.0 @@ -521,10 +525,12 @@ 0.5 + true 100.0 @@ -588,7 +594,7 @@ - chair/models/Chair.stl + chair/models/Chair.stl 0.0254 0.0254 0.0254 @@ -621,7 +627,7 @@ - chair/models/Chair.dae + chair/models/Chair.dae 0.0254 0.0254 0.0254 From 1d244451622d25ce348b3b50cf4e5968e715430d Mon Sep 17 00:00:00 2001 From: John Hsu Date: Wed, 26 Mar 2014 18:29:22 -0700 Subject: [PATCH 053/153] catkin_generate_changelog and fix rst format for forthcoming logs --- gazebo_msgs/CHANGELOG.rst | 3 +++ gazebo_plugins/CHANGELOG.rst | 14 ++++++++++++++ gazebo_ros/CHANGELOG.rst | 9 +++++++++ gazebo_ros_control/CHANGELOG.rst | 16 ++++++++++++++++ gazebo_ros_pkgs/CHANGELOG.rst | 3 +++ 5 files changed, 45 insertions(+) diff --git a/gazebo_msgs/CHANGELOG.rst b/gazebo_msgs/CHANGELOG.rst index 611bfbe89..0f3306e73 100644 --- a/gazebo_msgs/CHANGELOG.rst +++ b/gazebo_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gazebo_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.3.4 (2013-11-13) ------------------ diff --git a/gazebo_plugins/CHANGELOG.rst b/gazebo_plugins/CHANGELOG.rst index caa2cc3bd..0ce810455 100644 --- a/gazebo_plugins/CHANGELOG.rst +++ b/gazebo_plugins/CHANGELOG.rst @@ -2,6 +2,20 @@ Changelog for package gazebo_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* update test world for block laser +* this corrects the right orientation of the laser scan and improves on comparison between 2 double numbers +* Initialize ``depth_image_connect_count_`` in openni_kinect plugin +* multicamera bad namespace. Fixes `#161 `_ + There was a race condition between GazeboRosCameraUtils::LoadThread + creating the ros::NodeHandle and GazeboRosCameraUtils::Load + suffixing the camera name in the namespace +* Use function for accessing scene node in gazebo_ros_video +* readded the trailing whitespace for cleaner diff +* the parent sensor in gazebo seems not to be active +* Contributors: Dejan Pangercic, Ian Chen, John Hsu, Jordi Pages, Toni Oliver, Ugo Cupcic + 2.3.4 (2013-11-13) ------------------ * Simplify ``gazebo_plugins/CMakeLists.txt`` diff --git a/gazebo_ros/CHANGELOG.rst b/gazebo_ros/CHANGELOG.rst index 5e02f1399..b84856d7d 100644 --- a/gazebo_ros/CHANGELOG.rst +++ b/gazebo_ros/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package gazebo_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* gazebo_ros: [less-than-minor] fix newlines +* gazebo_ros: remove assignment to self + If this is needed for any twisted reason, it should be made clear + anyway. Assuming this line is harmless and removing it because it + generates cppcheck warnings. +* Contributors: Paul Mathieu + 2.3.4 (2013-11-13) ------------------ * remove debug statement diff --git a/gazebo_ros_control/CHANGELOG.rst b/gazebo_ros_control/CHANGELOG.rst index 95cd2d361..b08bd64b4 100644 --- a/gazebo_ros_control/CHANGELOG.rst +++ b/gazebo_ros_control/CHANGELOG.rst @@ -2,6 +2,22 @@ Changelog for package gazebo_ros_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Removed some debugging code. +* joint->SetAngle() and joint->SetVelocity() are now used to control + position-controlled joints and velocity-controlled joints that do not + have PID gain values stored on the Parameter Server. +* Position-controlled and velocity-controlled joints now use PID controllers + instead of calling SetAngle() or SetVelocity(). readSim() now longer calls + angles::shortest_angular_distance() when a joint is prismatic. + PLUGINLIB_EXPORT_CLASS is now used to register the plugin. +* gazebo_ros_control now depends on control_toolbox. +* Added support for the position hardware interface. Completed support for the + velocity hardware interface. +* Removed the "support more hardware interfaces" line. +* Contributors: Jim Rothrock + 2.3.4 (2013-11-13) ------------------ * Merge pull request `#144 `_ from meyerj/fix-125 diff --git a/gazebo_ros_pkgs/CHANGELOG.rst b/gazebo_ros_pkgs/CHANGELOG.rst index 8acd1d72d..09ed3c807 100644 --- a/gazebo_ros_pkgs/CHANGELOG.rst +++ b/gazebo_ros_pkgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gazebo_ros_pkgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.3.4 (2013-11-13) ------------------ From d249d1d1b7a8f05e1a182572090180b410a25b15 Mon Sep 17 00:00:00 2001 From: John Hsu Date: Wed, 26 Mar 2014 18:29:42 -0700 Subject: [PATCH 054/153] catkin_tag_changelog --- gazebo_msgs/CHANGELOG.rst | 4 ++-- gazebo_plugins/CHANGELOG.rst | 4 ++-- gazebo_ros/CHANGELOG.rst | 4 ++-- gazebo_ros_control/CHANGELOG.rst | 4 ++-- gazebo_ros_pkgs/CHANGELOG.rst | 4 ++-- 5 files changed, 10 insertions(+), 10 deletions(-) diff --git a/gazebo_msgs/CHANGELOG.rst b/gazebo_msgs/CHANGELOG.rst index 0f3306e73..687092315 100644 --- a/gazebo_msgs/CHANGELOG.rst +++ b/gazebo_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.3.5 (2014-03-26) +------------------ 2.3.4 (2013-11-13) ------------------ diff --git a/gazebo_plugins/CHANGELOG.rst b/gazebo_plugins/CHANGELOG.rst index 0ce810455..f8d68efcc 100644 --- a/gazebo_plugins/CHANGELOG.rst +++ b/gazebo_plugins/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.3.5 (2014-03-26) +------------------ * update test world for block laser * this corrects the right orientation of the laser scan and improves on comparison between 2 double numbers * Initialize ``depth_image_connect_count_`` in openni_kinect plugin diff --git a/gazebo_ros/CHANGELOG.rst b/gazebo_ros/CHANGELOG.rst index b84856d7d..55f1ebe1e 100644 --- a/gazebo_ros/CHANGELOG.rst +++ b/gazebo_ros/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.3.5 (2014-03-26) +------------------ * gazebo_ros: [less-than-minor] fix newlines * gazebo_ros: remove assignment to self If this is needed for any twisted reason, it should be made clear diff --git a/gazebo_ros_control/CHANGELOG.rst b/gazebo_ros_control/CHANGELOG.rst index b08bd64b4..931195e86 100644 --- a/gazebo_ros_control/CHANGELOG.rst +++ b/gazebo_ros_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_ros_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.3.5 (2014-03-26) +------------------ * Removed some debugging code. * joint->SetAngle() and joint->SetVelocity() are now used to control position-controlled joints and velocity-controlled joints that do not diff --git a/gazebo_ros_pkgs/CHANGELOG.rst b/gazebo_ros_pkgs/CHANGELOG.rst index 09ed3c807..d5f26b773 100644 --- a/gazebo_ros_pkgs/CHANGELOG.rst +++ b/gazebo_ros_pkgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_ros_pkgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.3.5 (2014-03-26) +------------------ 2.3.4 (2013-11-13) ------------------ From 511677f3bdf8040805588ea3c298878e99493fa5 Mon Sep 17 00:00:00 2001 From: John Hsu Date: Wed, 26 Mar 2014 18:31:27 -0700 Subject: [PATCH 055/153] 2.3.5 --- gazebo_msgs/package.xml | 2 +- gazebo_plugins/package.xml | 2 +- gazebo_ros/package.xml | 2 +- gazebo_ros_control/package.xml | 2 +- gazebo_ros_pkgs/package.xml | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/gazebo_msgs/package.xml b/gazebo_msgs/package.xml index 9bd9e8554..aef8a61e4 100644 --- a/gazebo_msgs/package.xml +++ b/gazebo_msgs/package.xml @@ -1,7 +1,7 @@ gazebo_msgs - 2.3.4 + 2.3.5 Message and service data structures for interacting with Gazebo from ROS. diff --git a/gazebo_plugins/package.xml b/gazebo_plugins/package.xml index 069db344a..47ff4b837 100644 --- a/gazebo_plugins/package.xml +++ b/gazebo_plugins/package.xml @@ -1,6 +1,6 @@ gazebo_plugins - 2.3.4 + 2.3.5 Robot-independent Gazebo plugins for sensors, motors and dynamic reconfigurable components. diff --git a/gazebo_ros/package.xml b/gazebo_ros/package.xml index f30c93af1..bedc9315e 100644 --- a/gazebo_ros/package.xml +++ b/gazebo_ros/package.xml @@ -1,7 +1,7 @@ gazebo_ros - 2.3.4 + 2.3.5 Provides ROS plugins that offer message and service publishers for interfacing with Gazebo through ROS. Formally simulator_gazebo/gazebo diff --git a/gazebo_ros_control/package.xml b/gazebo_ros_control/package.xml index a6676bdaf..fbd582224 100644 --- a/gazebo_ros_control/package.xml +++ b/gazebo_ros_control/package.xml @@ -1,6 +1,6 @@ gazebo_ros_control - 2.3.4 + 2.3.5 gazebo_ros_control Jonathan Bohren diff --git a/gazebo_ros_pkgs/package.xml b/gazebo_ros_pkgs/package.xml index c25a70d60..699e0263c 100644 --- a/gazebo_ros_pkgs/package.xml +++ b/gazebo_ros_pkgs/package.xml @@ -1,7 +1,7 @@ gazebo_ros_pkgs - 2.3.4 + 2.3.5 Interface for using ROS with the Gazebo simulator. John Hsu From eb785eeafff035b117f7d1199bbe40cb1b1bfce2 Mon Sep 17 00:00:00 2001 From: John Hsu Date: Thu, 27 Mar 2014 16:19:34 -0700 Subject: [PATCH 056/153] catkin_generate_changelog --- gazebo_msgs/CHANGELOG.rst | 9 +++++++++ gazebo_plugins/CHANGELOG.rst | 9 +++++++++ gazebo_ros/CHANGELOG.rst | 9 +++++++++ gazebo_ros_control/CHANGELOG.rst | 10 ++++++++++ gazebo_ros_pkgs/CHANGELOG.rst | 9 +++++++++ 5 files changed, 46 insertions(+) diff --git a/gazebo_msgs/CHANGELOG.rst b/gazebo_msgs/CHANGELOG.rst index 533ac9da9..69e79b088 100644 --- a/gazebo_msgs/CHANGELOG.rst +++ b/gazebo_msgs/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package gazebo_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* merging from hydro-devel +* bump patch version for indigo-devel to 2.4.1 +* merging from indigo-devel after 2.3.4 release +* "2.4.0" +* catkin_generate_changelog +* Contributors: John Hsu + 2.4.1 (2013-11-13) ------------------ * rerelease because sdformat became libsdformat, but we also based change on 2.3.4 in hydro-devel. diff --git a/gazebo_plugins/CHANGELOG.rst b/gazebo_plugins/CHANGELOG.rst index 5eb755ef5..4ee7970f1 100644 --- a/gazebo_plugins/CHANGELOG.rst +++ b/gazebo_plugins/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package gazebo_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* merging from hydro-devel +* bump patch version for indigo-devel to 2.4.1 +* merging from indigo-devel after 2.3.4 release +* "2.4.0" +* catkin_generate_changelog +* Contributors: John Hsu + 2.4.1 (2013-11-13) ------------------ diff --git a/gazebo_ros/CHANGELOG.rst b/gazebo_ros/CHANGELOG.rst index 3fc9a60e3..1f02e82c5 100644 --- a/gazebo_ros/CHANGELOG.rst +++ b/gazebo_ros/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package gazebo_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* merging from hydro-devel +* bump patch version for indigo-devel to 2.4.1 +* merging from indigo-devel after 2.3.4 release +* "2.4.0" +* catkin_generate_changelog +* Contributors: John Hsu + 2.4.1 (2013-11-13) ------------------ diff --git a/gazebo_ros_control/CHANGELOG.rst b/gazebo_ros_control/CHANGELOG.rst index fa4dae4e6..219ed4562 100644 --- a/gazebo_ros_control/CHANGELOG.rst +++ b/gazebo_ros_control/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package gazebo_ros_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* merging from hydro-devel +* bump patch version for indigo-devel to 2.4.1 +* merging from indigo-devel after 2.3.4 release +* Merge branch 'hydro-devel' of github.com:ros-simulation/gazebo_ros_pkgs into indigo-devel +* "2.4.0" +* catkin_generate_changelog +* Contributors: John Hsu + 2.4.1 (2013-11-13) ------------------ diff --git a/gazebo_ros_pkgs/CHANGELOG.rst b/gazebo_ros_pkgs/CHANGELOG.rst index b1f5a2990..98a532b9d 100644 --- a/gazebo_ros_pkgs/CHANGELOG.rst +++ b/gazebo_ros_pkgs/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package gazebo_ros_pkgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* merging from hydro-devel +* bump patch version for indigo-devel to 2.4.1 +* merging from indigo-devel after 2.3.4 release +* "2.4.0" +* catkin_generate_changelog +* Contributors: John Hsu + 2.4.1 (2013-11-13) ------------------ * rerelease because sdformat became libsdformat, but we also based change on 2.3.4 in hydro-devel. From bad43ffb91b49453c10116292b2f33254b18ce4b Mon Sep 17 00:00:00 2001 From: John Hsu Date: Thu, 27 Mar 2014 16:19:52 -0700 Subject: [PATCH 057/153] catkin_tag_changelog --- gazebo_msgs/CHANGELOG.rst | 4 ++-- gazebo_plugins/CHANGELOG.rst | 4 ++-- gazebo_ros/CHANGELOG.rst | 4 ++-- gazebo_ros_control/CHANGELOG.rst | 4 ++-- gazebo_ros_pkgs/CHANGELOG.rst | 4 ++-- 5 files changed, 10 insertions(+), 10 deletions(-) diff --git a/gazebo_msgs/CHANGELOG.rst b/gazebo_msgs/CHANGELOG.rst index 69e79b088..02ddad5ab 100644 --- a/gazebo_msgs/CHANGELOG.rst +++ b/gazebo_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.4.2 (2014-03-27) +------------------ * merging from hydro-devel * bump patch version for indigo-devel to 2.4.1 * merging from indigo-devel after 2.3.4 release diff --git a/gazebo_plugins/CHANGELOG.rst b/gazebo_plugins/CHANGELOG.rst index 4ee7970f1..b5b7fa94a 100644 --- a/gazebo_plugins/CHANGELOG.rst +++ b/gazebo_plugins/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.4.2 (2014-03-27) +------------------ * merging from hydro-devel * bump patch version for indigo-devel to 2.4.1 * merging from indigo-devel after 2.3.4 release diff --git a/gazebo_ros/CHANGELOG.rst b/gazebo_ros/CHANGELOG.rst index 1f02e82c5..d9368f379 100644 --- a/gazebo_ros/CHANGELOG.rst +++ b/gazebo_ros/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.4.2 (2014-03-27) +------------------ * merging from hydro-devel * bump patch version for indigo-devel to 2.4.1 * merging from indigo-devel after 2.3.4 release diff --git a/gazebo_ros_control/CHANGELOG.rst b/gazebo_ros_control/CHANGELOG.rst index 219ed4562..84baac106 100644 --- a/gazebo_ros_control/CHANGELOG.rst +++ b/gazebo_ros_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_ros_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.4.2 (2014-03-27) +------------------ * merging from hydro-devel * bump patch version for indigo-devel to 2.4.1 * merging from indigo-devel after 2.3.4 release diff --git a/gazebo_ros_pkgs/CHANGELOG.rst b/gazebo_ros_pkgs/CHANGELOG.rst index 98a532b9d..c39367440 100644 --- a/gazebo_ros_pkgs/CHANGELOG.rst +++ b/gazebo_ros_pkgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_ros_pkgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.4.2 (2014-03-27) +------------------ * merging from hydro-devel * bump patch version for indigo-devel to 2.4.1 * merging from indigo-devel after 2.3.4 release From 66f2bbb2c981849cd535c8b11f01e47de694c427 Mon Sep 17 00:00:00 2001 From: John Hsu Date: Thu, 27 Mar 2014 16:20:37 -0700 Subject: [PATCH 058/153] 2.4.2 --- gazebo_msgs/package.xml | 2 +- gazebo_plugins/package.xml | 2 +- gazebo_ros/package.xml | 2 +- gazebo_ros_control/package.xml | 2 +- gazebo_ros_pkgs/package.xml | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/gazebo_msgs/package.xml b/gazebo_msgs/package.xml index c8b42c940..0b808703f 100644 --- a/gazebo_msgs/package.xml +++ b/gazebo_msgs/package.xml @@ -1,7 +1,7 @@ gazebo_msgs - 2.4.1 + 2.4.2 Message and service data structures for interacting with Gazebo from ROS. diff --git a/gazebo_plugins/package.xml b/gazebo_plugins/package.xml index cdc8010df..532a30be6 100644 --- a/gazebo_plugins/package.xml +++ b/gazebo_plugins/package.xml @@ -1,6 +1,6 @@ gazebo_plugins - 2.4.1 + 2.4.2 Robot-independent Gazebo plugins for sensors, motors and dynamic reconfigurable components. diff --git a/gazebo_ros/package.xml b/gazebo_ros/package.xml index 6215f9cf2..2c796570b 100644 --- a/gazebo_ros/package.xml +++ b/gazebo_ros/package.xml @@ -1,7 +1,7 @@ gazebo_ros - 2.4.1 + 2.4.2 Provides ROS plugins that offer message and service publishers for interfacing with Gazebo through ROS. Formally simulator_gazebo/gazebo diff --git a/gazebo_ros_control/package.xml b/gazebo_ros_control/package.xml index c96ae2455..12d1c0c45 100644 --- a/gazebo_ros_control/package.xml +++ b/gazebo_ros_control/package.xml @@ -1,6 +1,6 @@ gazebo_ros_control - 2.4.1 + 2.4.2 gazebo_ros_control Jonathan Bohren diff --git a/gazebo_ros_pkgs/package.xml b/gazebo_ros_pkgs/package.xml index 33aac4248..291ba568f 100644 --- a/gazebo_ros_pkgs/package.xml +++ b/gazebo_ros_pkgs/package.xml @@ -1,7 +1,7 @@ gazebo_ros_pkgs - 2.4.1 + 2.4.2 Interface for using ROS with the Gazebo simulator. John Hsu From 7727057c934cf3211d96fff9b529a6ca86798f38 Mon Sep 17 00:00:00 2001 From: Markus Bader Date: Mon, 7 Apr 2014 14:36:59 +0200 Subject: [PATCH 059/153] pioneer model update for the multi_robot_scenario --- .../xacro/p3dx/battery_block.xacro | 37 ++ .../xacro/p3dx/inertia_tensors.xacro | 73 ++++ .../xacro/p3dx/pioneer3dx_body.xacro | 315 +++--------------- .../xacro/p3dx/pioneer3dx_swivel.xacro | 115 +++++++ .../xacro/p3dx/pioneer3dx_wheel.xacro | 78 +++++ 5 files changed, 346 insertions(+), 272 deletions(-) create mode 100644 gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/battery_block.xacro create mode 100644 gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/inertia_tensors.xacro create mode 100644 gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_swivel.xacro create mode 100644 gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_wheel.xacro diff --git a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/battery_block.xacro b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/battery_block.xacro new file mode 100644 index 000000000..b881a2f9d --- /dev/null +++ b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/battery_block.xacro @@ -0,0 +1,37 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/inertia_tensors.xacro b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/inertia_tensors.xacro new file mode 100644 index 000000000..2d53402b7 --- /dev/null +++ b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/inertia_tensors.xacro @@ -0,0 +1,73 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_body.xacro b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_body.xacro index 160e2dc11..fc73c4af9 100644 --- a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_body.xacro +++ b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_body.xacro @@ -2,15 +2,21 @@ + + + + + + - + @@ -21,7 +27,7 @@ - + @@ -30,20 +36,19 @@ - + - - - - - + + + + @@ -52,263 +57,31 @@ - - - - - - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + - + + - - + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + @@ -325,7 +98,7 @@ - + @@ -339,7 +112,7 @@ - + @@ -351,25 +124,23 @@ - - - - false - true - true - left_hub_joint - right_hub_joint - 0.5380 - 0.18 - 20 - cmd_vel - odom - odom - base_link - 100.0 - - - - + + + + false + true + true + left_hub_joint + right_hub_joint + 0.5380 + 0.18 + 20 + cmd_vel + odom + odom + base_link + 100.0 + + diff --git a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_swivel.xacro b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_swivel.xacro new file mode 100644 index 000000000..a67f52aec --- /dev/null +++ b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_swivel.xacro @@ -0,0 +1,115 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_wheel.xacro b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_wheel.xacro new file mode 100644 index 000000000..65b5daa92 --- /dev/null +++ b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_wheel.xacro @@ -0,0 +1,78 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file From f7759d63b7c627cf3e0d6594a64406ae45a77108 Mon Sep 17 00:00:00 2001 From: Steven Peters Date: Mon, 7 Apr 2014 13:54:37 -0700 Subject: [PATCH 060/153] Fix #175: dynamic reconfigure dependency error --- gazebo_plugins/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index 7729ba51e..0b00afa62 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -120,6 +120,7 @@ catkin_package( gazebo SDF ) +add_dependencies(${PROJECT_NAME}_gencfg ${catkin_EXPORTED_TARGETS}) ## Executables add_executable(hokuyo_node src/hokuyo_node.cpp) From 9f551d15ec1fd1280510dfc7caa1ab5306ecdc43 Mon Sep 17 00:00:00 2001 From: Steven Peters Date: Mon, 7 Apr 2014 17:55:53 -0700 Subject: [PATCH 061/153] Remove unneeded dependency on pcl_ros pcl_ros hasn't been released yet into indigo. I asked @wjwwood about its status, and he pointed out that our dependency on pcl_ros probably isn't necessary. Lo and behold, we removed it from the header files, package.xml and CMakeLists.txt and gazebo_plugins still compiles. --- gazebo_plugins/CMakeLists.txt | 2 -- gazebo_plugins/include/gazebo_plugins/gazebo_ros_depth_camera.h | 1 - .../include/gazebo_plugins/gazebo_ros_openni_kinect.h | 1 - gazebo_plugins/package.xml | 2 -- 4 files changed, 6 deletions(-) diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index 7729ba51e..c79953053 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -18,7 +18,6 @@ find_package(catkin REQUIRED COMPONENTS driver_base rosgraph_msgs trajectory_msgs - pcl_ros pcl_conversions image_transport rosconsole @@ -112,7 +111,6 @@ catkin_package( driver_base rosgraph_msgs trajectory_msgs - pcl_ros pcl_conversions image_transport rosconsole diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_depth_camera.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_depth_camera.h index 5a6ebae63..ae07bc860 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_depth_camera.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_depth_camera.h @@ -32,7 +32,6 @@ #include #include -#include #include // ros messages stuff diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_openni_kinect.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_openni_kinect.h index 4b7992ee4..c1f59d6eb 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_openni_kinect.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_openni_kinect.h @@ -32,7 +32,6 @@ #include #include -#include #include // ros messages stuff diff --git a/gazebo_plugins/package.xml b/gazebo_plugins/package.xml index 532a30be6..de8b151a2 100644 --- a/gazebo_plugins/package.xml +++ b/gazebo_plugins/package.xml @@ -34,7 +34,6 @@ dynamic_reconfigure driver_base rosgraph_msgs - pcl_ros pcl_conversions image_transport rosconsole @@ -59,7 +58,6 @@ dynamic_reconfigure driver_base rosgraph_msgs - pcl_ros pcl_conversions image_transport rosconsole From 55192c21e6bbeba666a77c1b012413d9a3186ed5 Mon Sep 17 00:00:00 2001 From: Markus Bader Date: Tue, 8 Apr 2014 11:59:01 +0200 Subject: [PATCH 062/153] gazebo test model pionneer 3dx updated with xacro path variables --- .../xacro/laser/hokuyo.xacro | 2 +- .../xacro/p3dx/battery_block.xacro | 3 +- .../xacro/p3dx/inertia_tensors.xacro | 3 +- .../xacro/p3dx/pioneer3dx.xacro | 10 +- .../xacro/p3dx/pioneer3dx_body.xacro | 170 ++++-------------- .../xacro/p3dx/pioneer3dx_chassis.xacro | 65 +++++++ .../xacro/p3dx/pioneer3dx_plugins.xacro | 36 ++++ .../xacro/p3dx/pioneer3dx_sonar.xacro | 41 +++++ .../xacro/p3dx/pioneer3dx_swivel.xacro | 13 +- .../xacro/p3dx/pioneer3dx_wheel.xacro | 8 +- 10 files changed, 191 insertions(+), 160 deletions(-) create mode 100644 gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_chassis.xacro create mode 100644 gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_plugins.xacro create mode 100644 gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_sonar.xacro diff --git a/gazebo_plugins/test/multi_robot_scenario/xacro/laser/hokuyo.xacro b/gazebo_plugins/test/multi_robot_scenario/xacro/laser/hokuyo.xacro index e25639895..0e58c3675 100644 --- a/gazebo_plugins/test/multi_robot_scenario/xacro/laser/hokuyo.xacro +++ b/gazebo_plugins/test/multi_robot_scenario/xacro/laser/hokuyo.xacro @@ -2,7 +2,7 @@ - + diff --git a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/battery_block.xacro b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/battery_block.xacro index b881a2f9d..7881c8e86 100644 --- a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/battery_block.xacro +++ b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/battery_block.xacro @@ -1,6 +1,5 @@ - + diff --git a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/inertia_tensors.xacro b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/inertia_tensors.xacro index 2d53402b7..c7649ea01 100644 --- a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/inertia_tensors.xacro +++ b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/inertia_tensors.xacro @@ -2,8 +2,7 @@ - + diff --git a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx.xacro b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx.xacro index 7d5c0ddc1..eae7e4b94 100644 --- a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx.xacro +++ b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx.xacro @@ -1,15 +1,15 @@ - - - + + + - + - + diff --git a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_body.xacro b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_body.xacro index fc73c4af9..3e07c7131 100644 --- a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_body.xacro +++ b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_body.xacro @@ -1,146 +1,38 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + - - + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - chassis_swivel_joint, swivel_wheel_joint, left_hub_joint, right_hub_joint - 10.0 - true - - - - - - - false - true - true - left_hub_joint - right_hub_joint - 0.5380 - 0.18 - 20 - cmd_vel - odom - odom - base_link - 100.0 - - diff --git a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_chassis.xacro b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_chassis.xacro new file mode 100644 index 000000000..ec3c233a9 --- /dev/null +++ b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_chassis.xacro @@ -0,0 +1,65 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_plugins.xacro b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_plugins.xacro new file mode 100644 index 000000000..e8e1a9d74 --- /dev/null +++ b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_plugins.xacro @@ -0,0 +1,36 @@ + + + + + + + + chassis_swivel_joint, swivel_wheel_joint, left_hub_joint, right_hub_joint + 10.0 + true + + + + + + + + + false + true + true + left_hub_joint + right_hub_joint + 0.5380 + 0.18 + 20 + cmd_vel + odom + odom + base_link + 100.0 + + + + + diff --git a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_sonar.xacro b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_sonar.xacro new file mode 100644 index 000000000..5d0dc3efa --- /dev/null +++ b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_sonar.xacro @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_swivel.xacro b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_swivel.xacro index a67f52aec..f4530e276 100644 --- a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_swivel.xacro +++ b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_swivel.xacro @@ -1,9 +1,8 @@ - + - + @@ -24,7 +23,7 @@ - + @@ -60,7 +59,7 @@ - + @@ -69,7 +68,7 @@ - + @@ -93,7 +92,7 @@ - + diff --git a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_wheel.xacro b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_wheel.xacro index 65b5daa92..b323befb8 100644 --- a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_wheel.xacro +++ b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_wheel.xacro @@ -3,7 +3,7 @@ xmlns:xacro="http://ros.org/wiki/xacro"> - + @@ -21,7 +21,7 @@ - + @@ -54,7 +54,7 @@ - + @@ -63,7 +63,7 @@ - + From bc079f30fc255aff053a91b2081f5daafc4dcdce Mon Sep 17 00:00:00 2001 From: Markus Bader Date: Tue, 8 Apr 2014 13:55:40 +0200 Subject: [PATCH 063/153] minor fixes on relative paths in xacro for pioneer robot --- .../launch/pioneer3dx.rviz | 173 +++++++++++++++--- .../xacro/camera/camera.xacro | 22 +-- .../xacro/laser/hokuyo.xacro | 18 +- .../xacro/p3dx/pioneer3dx.xacro | 6 +- .../xacro/p3dx/pioneer3dx_body.xacro | 16 +- 5 files changed, 181 insertions(+), 54 deletions(-) diff --git a/gazebo_plugins/test/multi_robot_scenario/launch/pioneer3dx.rviz b/gazebo_plugins/test/multi_robot_scenario/launch/pioneer3dx.rviz index 392c60b23..4300d22df 100644 --- a/gazebo_plugins/test/multi_robot_scenario/launch/pioneer3dx.rviz +++ b/gazebo_plugins/test/multi_robot_scenario/launch/pioneer3dx.rviz @@ -67,6 +67,11 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false + battery0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true center_hubcap: Alpha: 1 Show Axes: false @@ -92,11 +97,6 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - front_sonar: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true left_hub: Alpha: 1 Show Axes: false @@ -117,12 +117,17 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + sonar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true swivel: Alpha: 1 Show Axes: false Show Trail: false Value: true - top_plate: + top: Alpha: 1 Show Axes: false Show Trail: false @@ -142,6 +147,8 @@ Visualization Manager: Value: true r1/base_link: Value: true + r1/battery0: + Value: true r1/center_hubcap: Value: true r1/center_wheel: @@ -152,8 +159,6 @@ Visualization Manager: Value: true r1/front_laser: Value: true - r1/front_sonar: - Value: true r1/left_hub: Value: true r1/left_wheel: @@ -164,12 +169,42 @@ Visualization Manager: Value: true r1/right_wheel: Value: true + r1/sonar: + Value: true r1/swivel: Value: true - r1/top_plate: + r1/top: + Value: true + r2/base_link: + Value: true + r2/battery0: + Value: true + r2/center_hubcap: + Value: true + r2/center_wheel: + Value: true + r2/chassis: + Value: true + r2/front_camera: + Value: true + r2/front_laser: + Value: true + r2/left_hub: + Value: true + r2/left_wheel: Value: true r2/odom: Value: true + r2/right_hub: + Value: true + r2/right_wheel: + Value: true + r2/sonar: + Value: true + r2/swivel: + Value: true + r2/top: + Value: true Marker Scale: 1 Name: TF Show Arrows: true @@ -180,26 +215,49 @@ Visualization Manager: r1/odom: r1/base_link: r1/chassis: + r1/battery0: + {} + r1/front_camera: + {} + r1/front_laser: + {} + r1/left_hub: + r1/left_wheel: + {} + r1/right_hub: + r1/right_wheel: + {} + r1/sonar: + {} r1/swivel: r1/center_wheel: r1/center_hubcap: {} - r1/top_plate: + r1/top: {} - r1/front_camera: - {} - r1/front_laser: - {} - r1/front_sonar: - {} - r1/left_hub: - r1/left_wheel: + r2/odom: + r2/base_link: + r2/chassis: + r2/battery0: {} - r1/right_hub: - r1/right_wheel: + r2/front_camera: + {} + r2/front_laser: + {} + r2/left_hub: + r2/left_wheel: + {} + r2/right_hub: + r2/right_wheel: + {} + r2/sonar: + {} + r2/swivel: + r2/center_wheel: + r2/center_hubcap: + {} + r2/top: {} - r2/odom: - {} Update Interval: 0 Value: true - Alpha: 1 @@ -212,6 +270,75 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + battery0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + center_hubcap: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + center_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + chassis: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_laser: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sonar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + swivel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true Name: RobotModel Robot Description: r2/robot_description TF Prefix: r2 @@ -279,7 +406,7 @@ Visualization Manager: - Class: rviz/Camera Enabled: true Image Rendering: background and overlay - Image Topic: /r1/camera/image_raw + Image Topic: /r1/front_camera/image_raw Name: Camera Overlay Alpha: 0.5 Queue Size: 2 diff --git a/gazebo_plugins/test/multi_robot_scenario/xacro/camera/camera.xacro b/gazebo_plugins/test/multi_robot_scenario/xacro/camera/camera.xacro index f71c4a960..8755051be 100644 --- a/gazebo_plugins/test/multi_robot_scenario/xacro/camera/camera.xacro +++ b/gazebo_plugins/test/multi_robot_scenario/xacro/camera/camera.xacro @@ -2,17 +2,17 @@ - + - - - - - + + + + + - + @@ -32,11 +32,11 @@ - + - + 30.0 @@ -63,10 +63,10 @@ true 0.0 - camera + ${name} image_raw camera_info - front_camera + ${name} 0.07 0.0 0.0 diff --git a/gazebo_plugins/test/multi_robot_scenario/xacro/laser/hokuyo.xacro b/gazebo_plugins/test/multi_robot_scenario/xacro/laser/hokuyo.xacro index 0e58c3675..1e20f0905 100644 --- a/gazebo_plugins/test/multi_robot_scenario/xacro/laser/hokuyo.xacro +++ b/gazebo_plugins/test/multi_robot_scenario/xacro/laser/hokuyo.xacro @@ -2,16 +2,16 @@ - + - + - - + + - + @@ -21,7 +21,7 @@ - + @@ -31,7 +31,7 @@ - + 0 0 0 0 0 0 @@ -53,8 +53,8 @@ - front_laser/scan - front_laser + ${name}/scan + ${name} - + - + - + diff --git a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_body.xacro b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_body.xacro index 3e07c7131..4b2163480 100644 --- a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_body.xacro +++ b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_body.xacro @@ -9,24 +9,24 @@ - + - + - + - + - + - - - + + + From 2f9e72b2b1f5f148ae9b8b91a1c2ecbca705d4ad Mon Sep 17 00:00:00 2001 From: Vincent Rabaud Date: Wed, 9 Apr 2014 01:46:30 +0200 Subject: [PATCH 064/153] remove PCL dependency --- gazebo_plugins/CMakeLists.txt | 3 - .../gazebo_plugins/gazebo_ros_depth_camera.h | 4 +- .../gazebo_plugins/gazebo_ros_openni_kinect.h | 4 +- gazebo_plugins/package.xml | 2 - .../src/gazebo_ros_depth_camera.cpp | 81 ++++++++++--------- .../src/gazebo_ros_openni_kinect.cpp | 50 ++++++------ 6 files changed, 67 insertions(+), 77 deletions(-) diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index 565ec0c1d..e894a7b7b 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -18,7 +18,6 @@ find_package(catkin REQUIRED COMPONENTS driver_base rosgraph_msgs trajectory_msgs - pcl_conversions image_transport rosconsole cv_bridge @@ -37,7 +36,6 @@ endif() # Depend on system install of Gazebo and SDFormat find_package(gazebo REQUIRED) -find_package(PCL REQUIRED) find_package(Boost REQUIRED COMPONENTS thread) execute_process(COMMAND @@ -111,7 +109,6 @@ catkin_package( driver_base rosgraph_msgs trajectory_msgs - pcl_conversions image_transport rosconsole DEPENDS diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_depth_camera.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_depth_camera.h index ae07bc860..ebf48b357 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_depth_camera.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_depth_camera.h @@ -32,8 +32,6 @@ #include #include -#include - // ros messages stuff #include #include @@ -123,7 +121,7 @@ namespace gazebo private: ros::Publisher point_cloud_pub_; private: ros::Publisher depth_image_pub_; - /// \brief PCL point cloud message + /// \brief PointCloud2 point cloud message private: sensor_msgs::PointCloud2 point_cloud_msg_; private: sensor_msgs::Image depth_image_msg_; diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_openni_kinect.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_openni_kinect.h index c1f59d6eb..5b01fb66a 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_openni_kinect.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_openni_kinect.h @@ -32,8 +32,6 @@ #include #include -#include - // ros messages stuff #include #include @@ -117,7 +115,7 @@ namespace gazebo private: ros::Publisher point_cloud_pub_; private: ros::Publisher depth_image_pub_; - /// \brief PCL point cloud message + /// \brief PointCloud2 point cloud message private: sensor_msgs::PointCloud2 point_cloud_msg_; private: sensor_msgs::Image depth_image_msg_; diff --git a/gazebo_plugins/package.xml b/gazebo_plugins/package.xml index de8b151a2..dc84def15 100644 --- a/gazebo_plugins/package.xml +++ b/gazebo_plugins/package.xml @@ -34,7 +34,6 @@ dynamic_reconfigure driver_base rosgraph_msgs - pcl_conversions image_transport rosconsole message_generation @@ -58,7 +57,6 @@ dynamic_reconfigure driver_base rosgraph_msgs - pcl_conversions image_transport rosconsole message_generation diff --git a/gazebo_plugins/src/gazebo_ros_depth_camera.cpp b/gazebo_plugins/src/gazebo_ros_depth_camera.cpp index 30fa05fc9..0bdad5ef0 100644 --- a/gazebo_plugins/src/gazebo_ros_depth_camera.cpp +++ b/gazebo_plugins/src/gazebo_ros_depth_camera.cpp @@ -31,7 +31,7 @@ #include #include -#include +#include #include @@ -246,24 +246,29 @@ void GazeboRosDepthCamera::OnNewRGBPointCloud(const float *_pcd, this->point_cloud_msg_.height = this->height; this->point_cloud_msg_.row_step = this->point_cloud_msg_.point_step * this->width; - pcl::PointCloud point_cloud; - point_cloud.points.resize(0); - point_cloud.is_dense = true; + sensor_msgs::PointCloud2Modifier pcd_modifier(point_cloud_msg_); + pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); + pcd_modifier.resize(_width*_height); + + point_cloud_msg_.is_dense = true; + + sensor_msgs::PointCloud2Iterator iter_x(point_cloud_msg_, "x"); + sensor_msgs::PointCloud2Iterator iter_y(point_cloud_msg_, "y"); + sensor_msgs::PointCloud2Iterator iter_z(point_cloud_msg_, "z"); + sensor_msgs::PointCloud2Iterator iter_rgb(point_cloud_msg_, "rgb"); for (unsigned int i = 0; i < _width; i++) { - for (unsigned int j = 0; j < _height; j++) + for (unsigned int j = 0; j < _height; j++, ++iter_x, ++iter_y, ++iter_z, ++iter_rgb) { unsigned int index = (j * _width) + i; - pcl::PointXYZRGB point; - point.x = _pcd[4 * index]; - point.y = _pcd[4 * index + 1]; - point.z = _pcd[4 * index + 2]; - point.rgb = _pcd[4 * index + 3]; - point_cloud.points.push_back(point); + *iter_x = _pcd[4 * index]; + *iter_y = _pcd[4 * index + 1]; + *iter_z = _pcd[4 * index + 2]; + *iter_rgb = _pcd[4 * index + 3]; if (i == _width /2 && j == _height / 2) { - uint32_t rgb = *reinterpret_cast(&point.rgb); + uint32_t rgb = *reinterpret_cast(&(*iter_rgb)); uint8_t r = (rgb >> 16) & 0x0000ff; uint8_t g = (rgb >> 8) & 0x0000ff; uint8_t b = (rgb) & 0x0000ff; @@ -272,10 +277,6 @@ void GazeboRosDepthCamera::OnNewRGBPointCloud(const float *_pcd, } } - point_cloud.header = pcl_conversions::toPCL(point_cloud_msg_.header); - - pcl::toROSMsg(point_cloud, this->point_cloud_msg_); - this->point_cloud_pub_.publish(this->point_cloud_msg_); this->lock_.unlock(); } @@ -361,10 +362,16 @@ bool GazeboRosDepthCamera::FillPointCloudHelper( uint32_t rows_arg, uint32_t cols_arg, uint32_t step_arg, void* data_arg) { - pcl::PointCloud point_cloud; + sensor_msgs::PointCloud2Modifier pcd_modifier(point_cloud_msg); + pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); + pcd_modifier.resize(rows_arg*cols_arg); - point_cloud.points.resize(0); - point_cloud.is_dense = true; + sensor_msgs::PointCloud2Iterator iter_x(point_cloud_msg_, "x"); + sensor_msgs::PointCloud2Iterator iter_y(point_cloud_msg_, "y"); + sensor_msgs::PointCloud2Iterator iter_z(point_cloud_msg_, "z"); + sensor_msgs::PointCloud2Iterator iter_rgb(point_cloud_msg_, "rgb"); + + point_cloud_msg.is_dense = true; float* toCopyFrom = (float*)data_arg; int index = 0; @@ -379,7 +386,7 @@ bool GazeboRosDepthCamera::FillPointCloudHelper( if (rows_arg>1) pAngle = atan2( (double)j - 0.5*(double)(rows_arg-1), fl); else pAngle = 0.0; - for (uint32_t i=0; i1) yAngle = atan2( (double)i - 0.5*(double)(cols_arg-1), fl); @@ -391,17 +398,16 @@ bool GazeboRosDepthCamera::FillPointCloudHelper( // hardcoded rotation rpy(-M_PI/2, 0, -M_PI/2) is built-in // to urdf, where the *_optical_frame should have above relative // rotation from the physical camera *_frame - pcl::PointXYZRGB point; - point.x = depth * tan(yAngle); - point.y = depth * tan(pAngle); + *iter_x = depth * tan(yAngle); + *iter_y = depth * tan(pAngle); if(depth > this->point_cloud_cutoff_) { - point.z = depth; + *iter_z = depth; } else //point in the unseeable range { - point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); - point_cloud.is_dense = false; + *iter_x = *iter_y = *iter_z = std::numeric_limits::quiet_NaN (); + point_cloud_msg.is_dense = false; } // put image color data for each point @@ -409,32 +415,27 @@ bool GazeboRosDepthCamera::FillPointCloudHelper( if (this->image_msg_.data.size() == rows_arg*cols_arg*3) { // color - point.r = image_src[i*3+j*cols_arg*3+0]; - point.g = image_src[i*3+j*cols_arg*3+1]; - point.b = image_src[i*3+j*cols_arg*3+2]; + iter_rgb[0] = image_src[i*3+j*cols_arg*3+0]; + iter_rgb[1] = image_src[i*3+j*cols_arg*3+1]; + iter_rgb[2] = image_src[i*3+j*cols_arg*3+2]; } else if (this->image_msg_.data.size() == rows_arg*cols_arg) { // mono (or bayer? @todo; fix for bayer) - point.r = image_src[i+j*cols_arg]; - point.g = image_src[i+j*cols_arg]; - point.b = image_src[i+j*cols_arg]; + iter_rgb[0] = image_src[i+j*cols_arg]; + iter_rgb[1] = image_src[i+j*cols_arg]; + iter_rgb[2] = image_src[i+j*cols_arg]; } else { // no image - point.r = 0; - point.g = 0; - point.b = 0; + iter_rgb[0] = 0; + iter_rgb[1] = 0; + iter_rgb[2] = 0; } - - point_cloud.points.push_back(point); } } - point_cloud.header = pcl_conversions::toPCL(point_cloud_msg.header); - - pcl::toROSMsg(point_cloud, point_cloud_msg); return true; } diff --git a/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp b/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp index 42faf3c31..bf8a79697 100644 --- a/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp +++ b/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp @@ -32,8 +32,7 @@ #include #include -// for creating PointCloud2 from pcl point cloud -#include +#include #include @@ -301,10 +300,15 @@ bool GazeboRosOpenniKinect::FillPointCloudHelper( uint32_t rows_arg, uint32_t cols_arg, uint32_t step_arg, void* data_arg) { - pcl::PointCloud point_cloud; + sensor_msgs::PointCloud2Modifier pcd_modifier(point_cloud_msg); + pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); + pcd_modifier.resize(rows_arg*cols_arg); + point_cloud_msg.is_dense = true; - point_cloud.points.resize(0); - point_cloud.is_dense = true; + sensor_msgs::PointCloud2Iterator iter_x(point_cloud_msg_, "x"); + sensor_msgs::PointCloud2Iterator iter_y(point_cloud_msg_, "y"); + sensor_msgs::PointCloud2Iterator iter_z(point_cloud_msg_, "z"); + sensor_msgs::PointCloud2Iterator iter_rgb(point_cloud_msg_, "rgb"); float* toCopyFrom = (float*)data_arg; int index = 0; @@ -319,7 +323,7 @@ bool GazeboRosOpenniKinect::FillPointCloudHelper( if (rows_arg>1) pAngle = atan2( (double)j - 0.5*(double)(rows_arg-1), fl); else pAngle = 0.0; - for (uint32_t i=0; i1) yAngle = atan2( (double)i - 0.5*(double)(cols_arg-1), fl); @@ -331,17 +335,16 @@ bool GazeboRosOpenniKinect::FillPointCloudHelper( // hardcoded rotation rpy(-M_PI/2, 0, -M_PI/2) is built-in // to urdf, where the *_optical_frame should have above relative // rotation from the physical camera *_frame - pcl::PointXYZRGB point; - point.x = depth * tan(yAngle); - point.y = depth * tan(pAngle); + *iter_x = depth * tan(yAngle); + *iter_y = depth * tan(pAngle); if(depth > this->point_cloud_cutoff_) { - point.z = depth; + *iter_z = depth; } else //point in the unseeable range { - point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); - point_cloud.is_dense = false; + *iter_x = *iter_y = *iter_z = std::numeric_limits::quiet_NaN (); + point_cloud_msg.is_dense = false; } // put image color data for each point @@ -349,32 +352,27 @@ bool GazeboRosOpenniKinect::FillPointCloudHelper( if (this->image_msg_.data.size() == rows_arg*cols_arg*3) { // color - point.r = image_src[i*3+j*cols_arg*3+0]; - point.g = image_src[i*3+j*cols_arg*3+1]; - point.b = image_src[i*3+j*cols_arg*3+2]; + iter_rgb[0] = image_src[i*3+j*cols_arg*3+0]; + iter_rgb[1] = image_src[i*3+j*cols_arg*3+1]; + iter_rgb[2] = image_src[i*3+j*cols_arg*3+2]; } else if (this->image_msg_.data.size() == rows_arg*cols_arg) { // mono (or bayer? @todo; fix for bayer) - point.r = image_src[i+j*cols_arg]; - point.g = image_src[i+j*cols_arg]; - point.b = image_src[i+j*cols_arg]; + iter_rgb[0] = image_src[i+j*cols_arg]; + iter_rgb[1] = image_src[i+j*cols_arg]; + iter_rgb[2] = image_src[i+j*cols_arg]; } else { // no image - point.r = 0; - point.g = 0; - point.b = 0; + iter_rgb[0] = 0; + iter_rgb[1] = 0; + iter_rgb[2] = 0; } - - point_cloud.points.push_back(point); } } - point_cloud.header = pcl_conversions::toPCL(point_cloud_msg.header); - - pcl::toROSMsg(point_cloud, point_cloud_msg); return true; } From bf00862f8b820705123317eadb2aa028f0af663e Mon Sep 17 00:00:00 2001 From: Steven Peters Date: Tue, 15 Apr 2014 11:07:31 -0700 Subject: [PATCH 065/153] Remove gazebo_ros dependency on gazebo_plugins --- gazebo_ros/CMakeLists.txt | 1 - gazebo_ros/package.xml | 2 -- 2 files changed, 3 deletions(-) diff --git a/gazebo_ros/CMakeLists.txt b/gazebo_ros/CMakeLists.txt index 6595c14b2..1c2ab2a51 100644 --- a/gazebo_ros/CMakeLists.txt +++ b/gazebo_ros/CMakeLists.txt @@ -49,7 +49,6 @@ catkin_package( message_generation std_msgs gazebo_msgs - gazebo_plugins DEPENDS TinyXML diff --git a/gazebo_ros/package.xml b/gazebo_ros/package.xml index 2c796570b..47be2f05f 100644 --- a/gazebo_ros/package.xml +++ b/gazebo_ros/package.xml @@ -25,7 +25,6 @@ cmake_modules gazebo gazebo_msgs - gazebo_plugins roslib roscpp tf @@ -40,7 +39,6 @@ gazebo gazebo_msgs - gazebo_plugins roslib roscpp tf From 57e9c8cfc057dbced3692d0aee4815a88594fbf2 Mon Sep 17 00:00:00 2001 From: Steven Peters Date: Mon, 7 Apr 2014 17:55:53 -0700 Subject: [PATCH 066/153] Remove unneeded dependency on pcl_ros pcl_ros hasn't been released yet into indigo. I asked @wjwwood about its status, and he pointed out that our dependency on pcl_ros probably isn't necessary. Lo and behold, we removed it from the header files, package.xml and CMakeLists.txt and gazebo_plugins still compiles. --- gazebo_plugins/CMakeLists.txt | 2 -- gazebo_plugins/include/gazebo_plugins/gazebo_ros_depth_camera.h | 1 - .../include/gazebo_plugins/gazebo_ros_openni_kinect.h | 1 - gazebo_plugins/package.xml | 2 -- 4 files changed, 6 deletions(-) diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index 7729ba51e..c79953053 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -18,7 +18,6 @@ find_package(catkin REQUIRED COMPONENTS driver_base rosgraph_msgs trajectory_msgs - pcl_ros pcl_conversions image_transport rosconsole @@ -112,7 +111,6 @@ catkin_package( driver_base rosgraph_msgs trajectory_msgs - pcl_ros pcl_conversions image_transport rosconsole diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_depth_camera.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_depth_camera.h index 5a6ebae63..ae07bc860 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_depth_camera.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_depth_camera.h @@ -32,7 +32,6 @@ #include #include -#include #include // ros messages stuff diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_openni_kinect.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_openni_kinect.h index 4b7992ee4..c1f59d6eb 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_openni_kinect.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_openni_kinect.h @@ -32,7 +32,6 @@ #include #include -#include #include // ros messages stuff diff --git a/gazebo_plugins/package.xml b/gazebo_plugins/package.xml index 47ff4b837..350e36a96 100644 --- a/gazebo_plugins/package.xml +++ b/gazebo_plugins/package.xml @@ -34,7 +34,6 @@ dynamic_reconfigure driver_base rosgraph_msgs - pcl_ros pcl_conversions image_transport rosconsole @@ -59,7 +58,6 @@ dynamic_reconfigure driver_base rosgraph_msgs - pcl_ros pcl_conversions image_transport rosconsole From 33a7d9a9f9f4c95d1d94d5a60b3aaf2972a02c00 Mon Sep 17 00:00:00 2001 From: Steven Peters Date: Tue, 15 Apr 2014 12:08:01 -0700 Subject: [PATCH 067/153] Fix STL iterator errors, misc. cppcheck (#182) There were some errors in STL iterators. Initialized values of member variables in constructor. Removed an unused variable (model_name). --- gazebo_ros/src/gazebo_ros_api_plugin.cpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/gazebo_ros/src/gazebo_ros_api_plugin.cpp b/gazebo_ros/src/gazebo_ros_api_plugin.cpp index 4fc33226d..ae753ca02 100644 --- a/gazebo_ros/src/gazebo_ros_api_plugin.cpp +++ b/gazebo_ros/src/gazebo_ros_api_plugin.cpp @@ -30,7 +30,9 @@ GazeboRosApiPlugin::GazeboRosApiPlugin() : physics_reconfigure_initialized_(false), world_created_(false), stop_(false), - plugin_loaded_(false) + plugin_loaded_(false), + pub_link_states_connection_count_(0), + pub_model_states_connection_count_(0) { robot_namespace_.clear(); } @@ -81,11 +83,17 @@ GazeboRosApiPlugin::~GazeboRosApiPlugin() // Delete Force and Wrench Jobs lock_.lock(); for (std::vector::iterator iter=force_joint_jobs_.begin();iter!=force_joint_jobs_.end();) + { delete (*iter); + iter = force_joint_jobs_.erase(iter); + } force_joint_jobs_.clear(); ROS_DEBUG_STREAM_NAMED("api_plugin","ForceJointJobs deleted"); for (std::vector::iterator iter=wrench_body_jobs_.begin();iter!=wrench_body_jobs_.end();) + { delete (*iter); + iter = wrench_body_jobs_.erase(iter); + } wrench_body_jobs_.clear(); lock_.unlock(); ROS_DEBUG_STREAM_NAMED("api_plugin","WrenchBodyJobs deleted"); @@ -518,9 +526,6 @@ bool GazeboRosApiPlugin::spawnURDFModel(gazebo_msgs::SpawnModel::Request &req, // get name space for the corresponding model plugins robot_namespace_ = req.robot_namespace; - // incoming robot name - std::string model_name = req.model_name; - // incoming robot model string std::string model_xml = req.model_xml; @@ -1657,7 +1662,7 @@ void GazeboRosApiPlugin::wrenchBodySchedulerSlot() iter = wrench_body_jobs_.erase(iter); } else - iter++; + ++iter; } lock_.unlock(); } @@ -1687,7 +1692,7 @@ void GazeboRosApiPlugin::forceJointSchedulerSlot() iter = force_joint_jobs_.erase(iter); } else - iter++; + ++iter; } lock_.unlock(); } From 280aec4167d93ae4ccfb2a2efc1f277c14efa196 Mon Sep 17 00:00:00 2001 From: Adolfo Rodriguez Tsouroukdissian Date: Wed, 16 Apr 2014 18:36:55 +0200 Subject: [PATCH 068/153] Fix broken build due to wrong rosconsole macro use --- gazebo_ros_control/src/gazebo_ros_control_plugin.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp b/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp index 88723320f..120432e38 100644 --- a/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp +++ b/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp @@ -148,7 +148,7 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element const std::string urdf_string = getURDF(robot_description_); if (!parseTransmissionsFromURDF(urdf_string)) { - ROS_ERROR("gazebo_ros_control", "Error parsing URDF in gazebo_ros_control plugin, plugin not active.\n"); + ROS_ERROR_NAMED("gazebo_ros_control", "Error parsing URDF in gazebo_ros_control plugin, plugin not active.\n"); return; } From 0cfbfde076e953d38db7fa56e2337602cc3aa29c Mon Sep 17 00:00:00 2001 From: Steven Peters Date: Wed, 16 Apr 2014 12:02:35 -0700 Subject: [PATCH 069/153] gazebo_plugins: add run-time dependency on gazebo_ros --- gazebo_plugins/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/gazebo_plugins/package.xml b/gazebo_plugins/package.xml index de8b151a2..8e0428dfb 100644 --- a/gazebo_plugins/package.xml +++ b/gazebo_plugins/package.xml @@ -44,6 +44,7 @@ gazebo gazebo_msgs + gazebo_ros geometry_msgs sensor_msgs trajectory_msgs From a889ef8b768861231a67b78781514d834f631b8e Mon Sep 17 00:00:00 2001 From: Adolfo Rodriguez Tsouroukdissian Date: Wed, 16 Apr 2014 18:37:54 +0200 Subject: [PATCH 070/153] Remove build-time dependency on gazebo_ros. --- gazebo_ros_control/CMakeLists.txt | 2 -- gazebo_ros_control/package.xml | 1 - 2 files changed, 3 deletions(-) diff --git a/gazebo_ros_control/CMakeLists.txt b/gazebo_ros_control/CMakeLists.txt index 049f927b1..01608eb5d 100644 --- a/gazebo_ros_control/CMakeLists.txt +++ b/gazebo_ros_control/CMakeLists.txt @@ -4,7 +4,6 @@ project(gazebo_ros_control) # Load catkin and all dependencies required for this package find_package(catkin REQUIRED COMPONENTS roscpp - gazebo_ros control_toolbox controller_manager hardware_interface @@ -20,7 +19,6 @@ find_package(gazebo REQUIRED) catkin_package( CATKIN_DEPENDS roscpp - gazebo_ros controller_manager pluginlib transmission_interface diff --git a/gazebo_ros_control/package.xml b/gazebo_ros_control/package.xml index 12d1c0c45..f1d32791c 100644 --- a/gazebo_ros_control/package.xml +++ b/gazebo_ros_control/package.xml @@ -18,7 +18,6 @@ catkin roscpp - gazebo_ros control_toolbox controller_manager pluginlib From 31308fac855e8010a94d67da7de5d2250bf1e134 Mon Sep 17 00:00:00 2001 From: Adolfo Rodriguez Tsouroukdissian Date: Wed, 16 Apr 2014 18:41:17 +0200 Subject: [PATCH 071/153] Compatibility with Indigo's ros_control. Also fixes #184. --- .../src/default_robot_hw_sim.cpp | 30 +++++++++++++------ 1 file changed, 21 insertions(+), 9 deletions(-) diff --git a/gazebo_ros_control/src/default_robot_hw_sim.cpp b/gazebo_ros_control/src/default_robot_hw_sim.cpp index 11882922a..548066b94 100644 --- a/gazebo_ros_control/src/default_robot_hw_sim.cpp +++ b/gazebo_ros_control/src/default_robot_hw_sim.cpp @@ -128,18 +128,30 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim continue; } - // Check that this transmission has one actuator - if(transmissions[j].actuators_.size() == 0) + std::vector joint_interfaces = transmissions[j].joints_[0].hardware_interfaces_; + if (joint_interfaces.empty() && + !(transmissions[j].actuators_.empty()) && + !(transmissions[j].actuators_[0].hardware_interfaces_.empty())) { - ROS_WARN_STREAM_NAMED("default_robot_hw_sim","Transmission " << transmissions[j].name_ - << " has no associated actuators."); + // TODO: Deprecate HW interface specification in actuators in ROS J + joint_interfaces = transmissions[j].actuators_[0].hardware_interfaces_; + ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "The element of tranmission " << + transmissions[j].name_ << " should be nested inside the element, not . " << + "The transmission will be properly loaded, but please update " << + "your robot model to remain compatible with future versions of the plugin."); + } + if (joint_interfaces.empty()) + { + ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "Joint " << transmissions[j].joints_[0].name_ << + " of transmission " << transmissions[j].name_ << " does not specify any hardware interface. " << + "Not adding it to the robot hardware simulation."); continue; } - else if(transmissions[j].actuators_.size() > 1) + else if (joint_interfaces.size() > 1) { - ROS_WARN_STREAM_NAMED("default_robot_hw_sim","Transmission " << transmissions[j].name_ - << " has more than one actuator. Currently the default robot hardware simulation " - << " interface only supports one."); + ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "Joint " << transmissions[j].joints_[0].name_ << + " of transmission " << transmissions[j].name_ << " specifies multiple hardware interfaces. " << + "Currently the default robot hardware simulation interface only supports one."); continue; } @@ -152,7 +164,7 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim joint_position_command_[j] = 0.0; joint_velocity_command_[j] = 0.0; - const std::string& hardware_interface = transmissions[j].actuators_[0].hardware_interface_; + const std::string& hardware_interface = joint_interfaces.front(); // Debug ROS_DEBUG_STREAM_NAMED("default_robot_hw_sim","Loading joint '" << joint_names_[j] From 4341b9eecbdc5acbe69f62321a71ed532efd8189 Mon Sep 17 00:00:00 2001 From: Markus Achtelik Date: Thu, 8 May 2014 18:20:20 +0200 Subject: [PATCH 072/153] added osx support for gazebo start scripts --- gazebo_ros/scripts/debug | 13 +++++++++---- gazebo_ros/scripts/gazebo | 15 ++++++++++----- gazebo_ros/scripts/gzclient | 9 +++++++-- gazebo_ros/scripts/gzserver | 13 +++++++++---- gazebo_ros/scripts/perf | 13 +++++++++---- 5 files changed, 44 insertions(+), 19 deletions(-) diff --git a/gazebo_ros/scripts/debug b/gazebo_ros/scripts/debug index f9a58f8d7..219a7cafb 100755 --- a/gazebo_ros/scripts/debug +++ b/gazebo_ros/scripts/debug @@ -1,16 +1,21 @@ #!/bin/sh final="$@" +EXT=so +if [ $(uname) == "Darwin" ]; then + EXT=dylib +fi + # add ros path plugin if it does not already exist in the passed in arguments -if [ `expr "$final" : '.*libgazebo_ros_paths_plugin\.so.*'` -eq 0 ] +if [ `expr "$final" : '.*libgazebo_ros_paths_plugin\.$EXT.*'` -eq 0 ] then - final="$final -s `catkin_find --first-only libgazebo_ros_paths_plugin.so`" + final="$final -s `catkin_find --first-only libgazebo_ros_paths_plugin.$EXT`" fi # add ros api plugin if it does not already exist in the passed in arguments -if [ `expr "$final" : '.*libgazebo\_ros\_api\_plugin\.so.*'` -eq 0 ] +if [ `expr "$final" : '.*libgazebo\_ros\_api\_plugin\.$EXT.*'` -eq 0 ] then - final="$final -s `catkin_find --first-only libgazebo_ros_api_plugin.so`" + final="$final -s `catkin_find --first-only libgazebo_ros_api_plugin.$EXT`" fi setup_path=$(pkg-config --variable=prefix gazebo)/share/gazebo/ diff --git a/gazebo_ros/scripts/gazebo b/gazebo_ros/scripts/gazebo index 1ea3788b4..ffb4a5ec6 100755 --- a/gazebo_ros/scripts/gazebo +++ b/gazebo_ros/scripts/gazebo @@ -1,19 +1,24 @@ #!/bin/sh final="$@" +EXT=so +if [ $(uname) == "Darwin" ]; then + EXT=dylib +fi + # add ros path plugin if it does not already exist in the passed in arguments -if [ `expr "$final" : '.*libgazebo_ros_paths_plugin\.so.*'` -eq 0 ] +if [ `expr "$final" : '.*libgazebo_ros_paths_plugin\.$EXT.*'` -eq 0 ] then - final="$final -s `catkin_find --first-only libgazebo_ros_paths_plugin.so`" + final="$final -s `catkin_find --first-only libgazebo_ros_paths_plugin.$EXT`" fi # add ros api plugin if it does not already exist in the passed in arguments -if [ `expr "$final" : '.*libgazebo\_ros\_api\_plugin\.so.*'` -eq 0 ] +if [ `expr "$final" : '.*libgazebo\_ros\_api\_plugin\.$EXT.*'` -eq 0 ] then - final="$final -s `catkin_find --first-only libgazebo_ros_api_plugin.so`" + final="$final -s `catkin_find --first-only libgazebo_ros_api_plugin.$EXT`" fi -client_final="-g `catkin_find --first-only libgazebo_ros_paths_plugin.so`" +client_final="-g `catkin_find --first-only libgazebo_ros_paths_plugin.$EXT`" # Combine the commands setup_path=$(pkg-config --variable=prefix gazebo)/share/gazebo/ diff --git a/gazebo_ros/scripts/gzclient b/gazebo_ros/scripts/gzclient index fe5ee86aa..5f7d2887e 100755 --- a/gazebo_ros/scripts/gzclient +++ b/gazebo_ros/scripts/gzclient @@ -1,10 +1,15 @@ #!/bin/sh final="$@" +EXT=so +if [ $(uname) == "Darwin" ]; then + EXT=dylib +fi + # add ros plugin if does not exist -if [ `expr "$final" : '.*libgazebo_ros_paths_plugin\.so.*'` -eq 0 ] +if [ `expr "$final" : '.*libgazebo_ros_paths_plugin\.$EXT.*'` -eq 0 ] then - final="$final -g `catkin_find --first-only libgazebo_ros_paths_plugin.so`" + final="$final -g `catkin_find --first-only libgazebo_ros_paths_plugin.$EXT`" fi # Combine the commands diff --git a/gazebo_ros/scripts/gzserver b/gazebo_ros/scripts/gzserver index 5f60a4692..e53d939ab 100755 --- a/gazebo_ros/scripts/gzserver +++ b/gazebo_ros/scripts/gzserver @@ -1,16 +1,21 @@ #!/bin/sh final="$@" +EXT=so +if [ $(uname) == "Darwin" ]; then + EXT=dylib +fi + # add ros path plugin if it does not already exist in the passed in arguments -if [ `expr "$final" : '.*libgazebo_ros_paths_plugin\.so.*'` -eq 0 ] +if [ `expr "$final" : '.*libgazebo_ros_paths_plugin\.$EXT.*'` -eq 0 ] then - final="$final -s `catkin_find --first-only libgazebo_ros_paths_plugin.so`" + final="$final -s `catkin_find --first-only libgazebo_ros_paths_plugin.$EXT`" fi # add ros api plugin if it does not already exist in the passed in arguments -if [ `expr "$final" : '.*libgazebo\_ros\_api\_plugin\.so.*'` -eq 0 ] +if [ `expr "$final" : '.*libgazebo\_ros\_api\_plugin\.$EXT.*'` -eq 0 ] then - final="$final -s `catkin_find --first-only libgazebo_ros_api_plugin.so`" + final="$final -s `catkin_find --first-only libgazebo_ros_api_plugin.$EXT`" fi setup_path=$(pkg-config --variable=prefix gazebo)/share/gazebo/ diff --git a/gazebo_ros/scripts/perf b/gazebo_ros/scripts/perf index 71ec5b01f..31f06d607 100755 --- a/gazebo_ros/scripts/perf +++ b/gazebo_ros/scripts/perf @@ -1,15 +1,20 @@ #!/bin/sh final="$@" +EXT=so +if [ $(uname) == "Darwin" ]; then + EXT=dylib +fi + # add ros plugin if does not exist -if [ `expr "$final" : '.*libgazebo_ros_paths_plugin\.so.*'` -eq 0 ] +if [ `expr "$final" : '.*libgazebo_ros_paths_plugin\.$EXT.*'` -eq 0 ] then - final="$final -s `catkin_find --first-only libgazebo_ros_paths_plugin.so`" + final="$final -s `catkin_find --first-only libgazebo_ros_paths_plugin.$EXT`" fi -if [ `expr "$final" : '.*libgazebo\_ros\_api\_plugin\.so.*'` -eq 0 ] +if [ `expr "$final" : '.*libgazebo\_ros\_api\_plugin\.$EXT.*'` -eq 0 ] then - final="$final -s `catkin_find --first-only libgazebo_ros_api_plugin.so`" + final="$final -s `catkin_find --first-only libgazebo_ros_api_plugin.$EXT`" fi setup_path=$(pkg-config --variable=prefix gazebo)/share/gazebo/ From 59bd4bb3c10ab3773bcdae06932860c54ae08a25 Mon Sep 17 00:00:00 2001 From: Steven Peters Date: Mon, 12 May 2014 10:25:20 -0700 Subject: [PATCH 073/153] update changelog --- gazebo_msgs/CHANGELOG.rst | 3 +++ gazebo_plugins/CHANGELOG.rst | 9 +++++++++ gazebo_ros/CHANGELOG.rst | 5 +++++ gazebo_ros_control/CHANGELOG.rst | 8 ++++++++ gazebo_ros_pkgs/CHANGELOG.rst | 3 +++ 5 files changed, 28 insertions(+) diff --git a/gazebo_msgs/CHANGELOG.rst b/gazebo_msgs/CHANGELOG.rst index 02ddad5ab..58e20efba 100644 --- a/gazebo_msgs/CHANGELOG.rst +++ b/gazebo_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gazebo_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.4.2 (2014-03-27) ------------------ * merging from hydro-devel diff --git a/gazebo_plugins/CHANGELOG.rst b/gazebo_plugins/CHANGELOG.rst index b5b7fa94a..18b2741ee 100644 --- a/gazebo_plugins/CHANGELOG.rst +++ b/gazebo_plugins/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package gazebo_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* gazebo_plugins: add run-time dependency on gazebo_ros +* Merge pull request `#176 `_ from ros-simulation/issue_175 + Fix `#175 `_: dynamic reconfigure dependency error +* Remove unneeded dependency on pcl_ros +* Fix `#175 `_: dynamic reconfigure dependency error +* Contributors: Steven Peters + 2.4.2 (2014-03-27) ------------------ * merging from hydro-devel diff --git a/gazebo_ros/CHANGELOG.rst b/gazebo_ros/CHANGELOG.rst index d9368f379..1b0a5785e 100644 --- a/gazebo_ros/CHANGELOG.rst +++ b/gazebo_ros/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gazebo_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Remove gazebo_ros dependency on gazebo_plugins +* Contributors: Steven Peters + 2.4.2 (2014-03-27) ------------------ * merging from hydro-devel diff --git a/gazebo_ros_control/CHANGELOG.rst b/gazebo_ros_control/CHANGELOG.rst index 84baac106..6472eb74e 100644 --- a/gazebo_ros_control/CHANGELOG.rst +++ b/gazebo_ros_control/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package gazebo_ros_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Compatibility with Indigo's ros_control. + Also fixes `#184 `_. +* Remove build-time dependency on gazebo_ros. +* Fix broken build due to wrong rosconsole macro use +* Contributors: Adolfo Rodriguez Tsouroukdissian + 2.4.2 (2014-03-27) ------------------ * merging from hydro-devel diff --git a/gazebo_ros_pkgs/CHANGELOG.rst b/gazebo_ros_pkgs/CHANGELOG.rst index c39367440..adb437761 100644 --- a/gazebo_ros_pkgs/CHANGELOG.rst +++ b/gazebo_ros_pkgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gazebo_ros_pkgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.4.2 (2014-03-27) ------------------ * merging from hydro-devel From d0eca3d907ce57cae53e076d9352fa16d4486495 Mon Sep 17 00:00:00 2001 From: Markus Achtelik Date: Thu, 8 May 2014 18:20:20 +0200 Subject: [PATCH 074/153] added osx support for gazebo start scripts --- gazebo_ros/scripts/debug | 13 +++++++++---- gazebo_ros/scripts/gazebo | 15 ++++++++++----- gazebo_ros/scripts/gzclient | 9 +++++++-- gazebo_ros/scripts/gzserver | 13 +++++++++---- gazebo_ros/scripts/perf | 13 +++++++++---- 5 files changed, 44 insertions(+), 19 deletions(-) diff --git a/gazebo_ros/scripts/debug b/gazebo_ros/scripts/debug index f9a58f8d7..219a7cafb 100755 --- a/gazebo_ros/scripts/debug +++ b/gazebo_ros/scripts/debug @@ -1,16 +1,21 @@ #!/bin/sh final="$@" +EXT=so +if [ $(uname) == "Darwin" ]; then + EXT=dylib +fi + # add ros path plugin if it does not already exist in the passed in arguments -if [ `expr "$final" : '.*libgazebo_ros_paths_plugin\.so.*'` -eq 0 ] +if [ `expr "$final" : '.*libgazebo_ros_paths_plugin\.$EXT.*'` -eq 0 ] then - final="$final -s `catkin_find --first-only libgazebo_ros_paths_plugin.so`" + final="$final -s `catkin_find --first-only libgazebo_ros_paths_plugin.$EXT`" fi # add ros api plugin if it does not already exist in the passed in arguments -if [ `expr "$final" : '.*libgazebo\_ros\_api\_plugin\.so.*'` -eq 0 ] +if [ `expr "$final" : '.*libgazebo\_ros\_api\_plugin\.$EXT.*'` -eq 0 ] then - final="$final -s `catkin_find --first-only libgazebo_ros_api_plugin.so`" + final="$final -s `catkin_find --first-only libgazebo_ros_api_plugin.$EXT`" fi setup_path=$(pkg-config --variable=prefix gazebo)/share/gazebo/ diff --git a/gazebo_ros/scripts/gazebo b/gazebo_ros/scripts/gazebo index 1ea3788b4..ffb4a5ec6 100755 --- a/gazebo_ros/scripts/gazebo +++ b/gazebo_ros/scripts/gazebo @@ -1,19 +1,24 @@ #!/bin/sh final="$@" +EXT=so +if [ $(uname) == "Darwin" ]; then + EXT=dylib +fi + # add ros path plugin if it does not already exist in the passed in arguments -if [ `expr "$final" : '.*libgazebo_ros_paths_plugin\.so.*'` -eq 0 ] +if [ `expr "$final" : '.*libgazebo_ros_paths_plugin\.$EXT.*'` -eq 0 ] then - final="$final -s `catkin_find --first-only libgazebo_ros_paths_plugin.so`" + final="$final -s `catkin_find --first-only libgazebo_ros_paths_plugin.$EXT`" fi # add ros api plugin if it does not already exist in the passed in arguments -if [ `expr "$final" : '.*libgazebo\_ros\_api\_plugin\.so.*'` -eq 0 ] +if [ `expr "$final" : '.*libgazebo\_ros\_api\_plugin\.$EXT.*'` -eq 0 ] then - final="$final -s `catkin_find --first-only libgazebo_ros_api_plugin.so`" + final="$final -s `catkin_find --first-only libgazebo_ros_api_plugin.$EXT`" fi -client_final="-g `catkin_find --first-only libgazebo_ros_paths_plugin.so`" +client_final="-g `catkin_find --first-only libgazebo_ros_paths_plugin.$EXT`" # Combine the commands setup_path=$(pkg-config --variable=prefix gazebo)/share/gazebo/ diff --git a/gazebo_ros/scripts/gzclient b/gazebo_ros/scripts/gzclient index fe5ee86aa..5f7d2887e 100755 --- a/gazebo_ros/scripts/gzclient +++ b/gazebo_ros/scripts/gzclient @@ -1,10 +1,15 @@ #!/bin/sh final="$@" +EXT=so +if [ $(uname) == "Darwin" ]; then + EXT=dylib +fi + # add ros plugin if does not exist -if [ `expr "$final" : '.*libgazebo_ros_paths_plugin\.so.*'` -eq 0 ] +if [ `expr "$final" : '.*libgazebo_ros_paths_plugin\.$EXT.*'` -eq 0 ] then - final="$final -g `catkin_find --first-only libgazebo_ros_paths_plugin.so`" + final="$final -g `catkin_find --first-only libgazebo_ros_paths_plugin.$EXT`" fi # Combine the commands diff --git a/gazebo_ros/scripts/gzserver b/gazebo_ros/scripts/gzserver index 5f60a4692..e53d939ab 100755 --- a/gazebo_ros/scripts/gzserver +++ b/gazebo_ros/scripts/gzserver @@ -1,16 +1,21 @@ #!/bin/sh final="$@" +EXT=so +if [ $(uname) == "Darwin" ]; then + EXT=dylib +fi + # add ros path plugin if it does not already exist in the passed in arguments -if [ `expr "$final" : '.*libgazebo_ros_paths_plugin\.so.*'` -eq 0 ] +if [ `expr "$final" : '.*libgazebo_ros_paths_plugin\.$EXT.*'` -eq 0 ] then - final="$final -s `catkin_find --first-only libgazebo_ros_paths_plugin.so`" + final="$final -s `catkin_find --first-only libgazebo_ros_paths_plugin.$EXT`" fi # add ros api plugin if it does not already exist in the passed in arguments -if [ `expr "$final" : '.*libgazebo\_ros\_api\_plugin\.so.*'` -eq 0 ] +if [ `expr "$final" : '.*libgazebo\_ros\_api\_plugin\.$EXT.*'` -eq 0 ] then - final="$final -s `catkin_find --first-only libgazebo_ros_api_plugin.so`" + final="$final -s `catkin_find --first-only libgazebo_ros_api_plugin.$EXT`" fi setup_path=$(pkg-config --variable=prefix gazebo)/share/gazebo/ diff --git a/gazebo_ros/scripts/perf b/gazebo_ros/scripts/perf index 71ec5b01f..31f06d607 100755 --- a/gazebo_ros/scripts/perf +++ b/gazebo_ros/scripts/perf @@ -1,15 +1,20 @@ #!/bin/sh final="$@" +EXT=so +if [ $(uname) == "Darwin" ]; then + EXT=dylib +fi + # add ros plugin if does not exist -if [ `expr "$final" : '.*libgazebo_ros_paths_plugin\.so.*'` -eq 0 ] +if [ `expr "$final" : '.*libgazebo_ros_paths_plugin\.$EXT.*'` -eq 0 ] then - final="$final -s `catkin_find --first-only libgazebo_ros_paths_plugin.so`" + final="$final -s `catkin_find --first-only libgazebo_ros_paths_plugin.$EXT`" fi -if [ `expr "$final" : '.*libgazebo\_ros\_api\_plugin\.so.*'` -eq 0 ] +if [ `expr "$final" : '.*libgazebo\_ros\_api\_plugin\.$EXT.*'` -eq 0 ] then - final="$final -s `catkin_find --first-only libgazebo_ros_api_plugin.so`" + final="$final -s `catkin_find --first-only libgazebo_ros_api_plugin.$EXT`" fi setup_path=$(pkg-config --variable=prefix gazebo)/share/gazebo/ From 344045227902f020e76f9826d7d8a6cc7593490d Mon Sep 17 00:00:00 2001 From: Steven Peters Date: Mon, 12 May 2014 10:32:12 -0700 Subject: [PATCH 075/153] update changelog --- gazebo_ros/CHANGELOG.rst | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gazebo_ros/CHANGELOG.rst b/gazebo_ros/CHANGELOG.rst index 1b0a5785e..46b6537c2 100644 --- a/gazebo_ros/CHANGELOG.rst +++ b/gazebo_ros/CHANGELOG.rst @@ -4,8 +4,9 @@ Changelog for package gazebo_ros Forthcoming ----------- +* added osx support for gazebo start scripts * Remove gazebo_ros dependency on gazebo_plugins -* Contributors: Steven Peters +* Contributors: Markus Achtelik, Steven Peters 2.4.2 (2014-03-27) ------------------ From 37466e4eeba82d4d1936783cad90635fddaa818a Mon Sep 17 00:00:00 2001 From: Steven Peters Date: Mon, 12 May 2014 10:35:33 -0700 Subject: [PATCH 076/153] 2.4.3 --- gazebo_msgs/CHANGELOG.rst | 4 ++-- gazebo_msgs/package.xml | 2 +- gazebo_plugins/CHANGELOG.rst | 4 ++-- gazebo_plugins/package.xml | 2 +- gazebo_ros/CHANGELOG.rst | 4 ++-- gazebo_ros/package.xml | 2 +- gazebo_ros_control/CHANGELOG.rst | 4 ++-- gazebo_ros_control/package.xml | 2 +- gazebo_ros_pkgs/CHANGELOG.rst | 4 ++-- gazebo_ros_pkgs/package.xml | 2 +- 10 files changed, 15 insertions(+), 15 deletions(-) diff --git a/gazebo_msgs/CHANGELOG.rst b/gazebo_msgs/CHANGELOG.rst index 58e20efba..b7ba8e413 100644 --- a/gazebo_msgs/CHANGELOG.rst +++ b/gazebo_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.4.3 (2014-05-12) +------------------ 2.4.2 (2014-03-27) ------------------ diff --git a/gazebo_msgs/package.xml b/gazebo_msgs/package.xml index 0b808703f..43724024d 100644 --- a/gazebo_msgs/package.xml +++ b/gazebo_msgs/package.xml @@ -1,7 +1,7 @@ gazebo_msgs - 2.4.2 + 2.4.3 Message and service data structures for interacting with Gazebo from ROS. diff --git a/gazebo_plugins/CHANGELOG.rst b/gazebo_plugins/CHANGELOG.rst index 18b2741ee..1cf0744e5 100644 --- a/gazebo_plugins/CHANGELOG.rst +++ b/gazebo_plugins/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.4.3 (2014-05-12) +------------------ * gazebo_plugins: add run-time dependency on gazebo_ros * Merge pull request `#176 `_ from ros-simulation/issue_175 Fix `#175 `_: dynamic reconfigure dependency error diff --git a/gazebo_plugins/package.xml b/gazebo_plugins/package.xml index 8e0428dfb..32b4fb697 100644 --- a/gazebo_plugins/package.xml +++ b/gazebo_plugins/package.xml @@ -1,6 +1,6 @@ gazebo_plugins - 2.4.2 + 2.4.3 Robot-independent Gazebo plugins for sensors, motors and dynamic reconfigurable components. diff --git a/gazebo_ros/CHANGELOG.rst b/gazebo_ros/CHANGELOG.rst index 46b6537c2..1abc236b8 100644 --- a/gazebo_ros/CHANGELOG.rst +++ b/gazebo_ros/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.4.3 (2014-05-12) +------------------ * added osx support for gazebo start scripts * Remove gazebo_ros dependency on gazebo_plugins * Contributors: Markus Achtelik, Steven Peters diff --git a/gazebo_ros/package.xml b/gazebo_ros/package.xml index 47be2f05f..b6b071d83 100644 --- a/gazebo_ros/package.xml +++ b/gazebo_ros/package.xml @@ -1,7 +1,7 @@ gazebo_ros - 2.4.2 + 2.4.3 Provides ROS plugins that offer message and service publishers for interfacing with Gazebo through ROS. Formally simulator_gazebo/gazebo diff --git a/gazebo_ros_control/CHANGELOG.rst b/gazebo_ros_control/CHANGELOG.rst index 6472eb74e..95d728109 100644 --- a/gazebo_ros_control/CHANGELOG.rst +++ b/gazebo_ros_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_ros_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.4.3 (2014-05-12) +------------------ * Compatibility with Indigo's ros_control. Also fixes `#184 `_. * Remove build-time dependency on gazebo_ros. diff --git a/gazebo_ros_control/package.xml b/gazebo_ros_control/package.xml index f1d32791c..85e9f8ca5 100644 --- a/gazebo_ros_control/package.xml +++ b/gazebo_ros_control/package.xml @@ -1,6 +1,6 @@ gazebo_ros_control - 2.4.2 + 2.4.3 gazebo_ros_control Jonathan Bohren diff --git a/gazebo_ros_pkgs/CHANGELOG.rst b/gazebo_ros_pkgs/CHANGELOG.rst index adb437761..132b4968a 100644 --- a/gazebo_ros_pkgs/CHANGELOG.rst +++ b/gazebo_ros_pkgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_ros_pkgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.4.3 (2014-05-12) +------------------ 2.4.2 (2014-03-27) ------------------ diff --git a/gazebo_ros_pkgs/package.xml b/gazebo_ros_pkgs/package.xml index 291ba568f..34201326b 100644 --- a/gazebo_ros_pkgs/package.xml +++ b/gazebo_ros_pkgs/package.xml @@ -1,7 +1,7 @@ gazebo_ros_pkgs - 2.4.2 + 2.4.3 Interface for using ROS with the Gazebo simulator. John Hsu From ef795adf2c9d5a6a53b6e696b3fc2a9c45c84ff6 Mon Sep 17 00:00:00 2001 From: Vincenzo Comito Date: Thu, 15 May 2014 12:04:52 +0200 Subject: [PATCH 077/153] Add verbose parameter Add verbose parameter for --verbose gazebo flag --- gazebo_ros/launch/empty_world.launch | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/gazebo_ros/launch/empty_world.launch b/gazebo_ros/launch/empty_world.launch index 2e02d7bcc..45b11270f 100644 --- a/gazebo_ros/launch/empty_world.launch +++ b/gazebo_ros/launch/empty_world.launch @@ -6,6 +6,7 @@ + @@ -18,12 +19,14 @@ + + + args="$(arg command_arg1) $(arg command_arg2) $(arg command_arg3) $(arg world_name)" /> From a512c14aa3759c1f51d5feeb23c38a83af37b2be Mon Sep 17 00:00:00 2001 From: Jonathan Bohren Date: Wed, 30 Oct 2013 04:10:57 -0400 Subject: [PATCH 078/153] hand_of_god: Adding hand-of-god plugin ros_force: Fixing error messages to refer to the right plugin --- gazebo_plugins/CMakeLists.txt | 7 + .../gazebo_plugins/gazebo_ros_hand_of_god.h | 75 +++++++ gazebo_plugins/package.xml | 2 + gazebo_plugins/src/gazebo_ros_force.cpp | 6 +- gazebo_plugins/src/gazebo_ros_hand_of_god.cpp | 190 ++++++++++++++++++ 5 files changed, 277 insertions(+), 3 deletions(-) create mode 100644 gazebo_plugins/include/gazebo_plugins/gazebo_ros_hand_of_god.h create mode 100644 gazebo_plugins/src/gazebo_ros_hand_of_god.cpp diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index 7729ba51e..d438e2a8b 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(catkin REQUIRED COMPONENTS nav_msgs urdf tf + tf2_ros dynamic_reconfigure driver_base rosgraph_msgs @@ -108,6 +109,7 @@ catkin_package( nav_msgs urdf tf + tf2_ros dynamic_reconfigure driver_base rosgraph_msgs @@ -218,6 +220,11 @@ target_link_libraries(gazebo_ros_video ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} $ add_library(gazebo_ros_planar_move src/gazebo_ros_planar_move.cpp) target_link_libraries(gazebo_ros_planar_move ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +add_library(gazebo_ros_hand_of_god src/gazebo_ros_hand_of_god.cpp) +set_target_properties(gazebo_ros_hand_of_god PROPERTIES LINK_FLAGS "${ld_flags}") +set_target_properties(gazebo_ros_hand_of_god PROPERTIES COMPILE_FLAGS "${cxx_flags}") +target_link_libraries(gazebo_ros_hand_of_god ${GAZEBO_LIBRARIES} ${SDFormat_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + ## ## Add your new plugin here ## diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_hand_of_god.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_hand_of_god.h new file mode 100644 index 000000000..55fae1daa --- /dev/null +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_hand_of_god.h @@ -0,0 +1,75 @@ +/* + * Gazebo - Outdoor Multi-Robot Simulator + * Copyright (C) 2003 + * Nate Koenig & Andrew Howard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ +/* + * Desc: 3D position interface. + * Author: Sachin Chitta and John Hsu + * Date: 10 June 2008 + * SVN: $Id$ + */ +#ifndef GAZEBO_ROS_TEMPLATE_HH +#define GAZEBO_ROS_TEMPLATE_HH + +#include + +#include +#include +#include +#include +#include + +#include +#include + +namespace gazebo +{ + + class GazeboRosHandOfGod : public ModelPlugin + { + /// \brief Constructor + public: GazeboRosHandOfGod(); + + /// \brief Destructor + public: virtual ~GazeboRosHandOfGod(); + + /// \brief Load the controller + public: void Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf ); + + /// \brief Update the controller + protected: virtual void GazeboUpdate(); + + /// Pointer to the update event connection + private: event::ConnectionPtr update_connection_; + boost::shared_ptr tf_buffer_; + boost::shared_ptr tf_listener_; + boost::shared_ptr tf_broadcaster_; + physics::ModelPtr model_; + physics::LinkPtr floating_link_; + std::string link_name_; + std::string robot_namespace_; + std::string frame_id_; + double kl_, ka_; + double cl_, ca_; + }; + +} + +#endif + diff --git a/gazebo_plugins/package.xml b/gazebo_plugins/package.xml index 069db344a..41d6e5e5d 100644 --- a/gazebo_plugins/package.xml +++ b/gazebo_plugins/package.xml @@ -31,6 +31,7 @@ nav_msgs urdf tf + tf2_ros dynamic_reconfigure driver_base rosgraph_msgs @@ -56,6 +57,7 @@ nav_msgs urdf tf + tf2_ros dynamic_reconfigure driver_base rosgraph_msgs diff --git a/gazebo_plugins/src/gazebo_ros_force.cpp b/gazebo_plugins/src/gazebo_ros_force.cpp index 7f10ce8e4..0f6cc0721 100644 --- a/gazebo_plugins/src/gazebo_ros_force.cpp +++ b/gazebo_plugins/src/gazebo_ros_force.cpp @@ -71,7 +71,7 @@ void GazeboRosForce::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) if (!_sdf->HasElement("bodyName")) { - ROS_FATAL("f3d plugin missing , cannot proceed"); + ROS_FATAL("force plugin missing , cannot proceed"); return; } else @@ -80,13 +80,13 @@ void GazeboRosForce::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) this->link_ = _model->GetLink(this->link_name_); if (!this->link_) { - ROS_FATAL("gazebo_ros_f3d plugin error: link named: %s does not exist\n",this->link_name_.c_str()); + ROS_FATAL("gazebo_ros_force plugin error: link named: %s does not exist\n",this->link_name_.c_str()); return; } if (!_sdf->HasElement("topicName")) { - ROS_FATAL("f3d plugin missing , cannot proceed"); + ROS_FATAL("force plugin missing , cannot proceed"); return; } else diff --git a/gazebo_plugins/src/gazebo_ros_hand_of_god.cpp b/gazebo_plugins/src/gazebo_ros_hand_of_god.cpp new file mode 100644 index 000000000..607476c9e --- /dev/null +++ b/gazebo_plugins/src/gazebo_ros_hand_of_god.cpp @@ -0,0 +1,190 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Open Source Robotics Foundation + * nor the names of its contributors may be + * used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/** + * \author Jonathan Bohren + * \desc A "hand-of-god" plugin which drives a floating object around based + * on the location of a TF frame. This plugin is useful for connecting human input + * devices to "god-like" objects in a Gazebo simulation. + */ + +#include +#include + +namespace gazebo +{ + // Register this plugin with the simulator + GZ_REGISTER_MODEL_PLUGIN(GazeboRosHandOfGod); + + //////////////////////////////////////////////////////////////////////////////// + // Constructor + GazeboRosHandOfGod::GazeboRosHandOfGod() : + ModelPlugin(), + robot_namespace_(""), + frame_id_("hog"), + kl_(200), + ka_(200) + { + } + + //////////////////////////////////////////////////////////////////////////////// + // Destructor + GazeboRosHandOfGod::~GazeboRosHandOfGod() + { + } + + //////////////////////////////////////////////////////////////////////////////// + // Load the controller + void GazeboRosHandOfGod::Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf ) + { + // Make sure the ROS node for Gazebo has already been initalized + if (!ros::isInitialized()) { + ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; + } + + // Get sdf parameters + if(_sdf->HasElement("robotNamespace")) { + this->robot_namespace_ = _sdf->Get("robotNamespace") + "/"; + } + + if(_sdf->HasElement("frameId")) { + this->frame_id_ = _sdf->Get("frameId"); + } + + if(_sdf->HasElement("kl")) { + this->kl_ = _sdf->Get("kl"); + } + if(_sdf->HasElement("ka")) { + this->ka_ = _sdf->Get("ka"); + } + + if(_sdf->HasElement("linkName")) { + this->link_name_ = _sdf->Get("linkName"); + } else { + ROS_FATAL_STREAM("The hand-of-god plugin requires a `linkName` parameter tag"); + return; + } + + // Store the model + model_ = _parent; + + // Disable gravity for the hog + model_->SetGravityMode(false); + + // Get the floating link + floating_link_ = model_->GetLink(link_name_); + if(!floating_link_) { + ROS_ERROR("Floating link not found!"); + const std::vector &links = model_->GetLinks(); + for(unsigned i=0; i < links.size(); i++) { + ROS_ERROR_STREAM(" -- Link "<GetName()); + } + return; + } + + cl_ = 2.0 * sqrt(kl_*floating_link_->GetInertial()->GetMass()); + ca_ = 2.0 * sqrt(ka_*floating_link_->GetInertial()->GetIXX()); + + // Create the TF listener for the desired position of the hog + tf_buffer_.reset(new tf2_ros::Buffer()); + tf_listener_.reset(new tf2_ros::TransformListener(*tf_buffer_)); + tf_broadcaster_.reset(new tf2_ros::TransformBroadcaster()); + + // Register update event handler + this->update_connection_ = event::Events::ConnectWorldUpdateBegin( + boost::bind(&GazeboRosHandOfGod::GazeboUpdate, this)); + } + + //////////////////////////////////////////////////////////////////////////////// + // Update the controller + void GazeboRosHandOfGod::GazeboUpdate() + { + // Get TF transform relative to the /world link + geometry_msgs::TransformStamped hog_desired_tform; + static bool errored = false; + try{ + hog_desired_tform = tf_buffer_->lookupTransform("world", frame_id_+"_desired", ros::Time(0)); + errored = false; + } catch (tf2::TransformException ex){ + if(!errored) { + ROS_ERROR("%s",ex.what()); + errored = true; + } + return; + } + + // Convert TF transform to Gazebo Pose + const geometry_msgs::Vector3 &p = hog_desired_tform.transform.translation; + const geometry_msgs::Quaternion &q = hog_desired_tform.transform.rotation; + gazebo::math::Pose hog_desired( + gazebo::math::Vector3(p.x, p.y, p.z), + gazebo::math::Quaternion(q.w, q.x, q.y, q.z)); + + // Relative transform from actual to desired pose + gazebo::math::Pose world_pose = floating_link_->GetDirtyPose(); + gazebo::math::Vector3 err_pos = hog_desired.pos - world_pose.pos; + // Get exponential coordinates for rotation + gazebo::math::Quaternion err_rot = (world_pose.rot.GetAsMatrix4().Inverse() * hog_desired.rot.GetAsMatrix4()).GetRotation(); + gazebo::math::Quaternion not_a_quaternion = err_rot.GetLog(); + + floating_link_->AddForce( + kl_ * err_pos - cl_ * floating_link_->GetWorldLinearVel()); + + floating_link_->AddRelativeTorque( + ka_ * gazebo::math::Vector3(not_a_quaternion.x, not_a_quaternion.y, not_a_quaternion.z) - ca_ * floating_link_->GetRelativeAngularVel()); + + // Convert actual pose to TransformStamped message + geometry_msgs::TransformStamped hog_actual_tform; + + hog_actual_tform.header.frame_id = "world"; + hog_actual_tform.header.stamp = ros::Time::now(); + + hog_actual_tform.child_frame_id = frame_id_ + "_actual"; + + hog_actual_tform.transform.translation.x = world_pose.pos.x; + hog_actual_tform.transform.translation.y = world_pose.pos.y; + hog_actual_tform.transform.translation.z = world_pose.pos.z; + + hog_actual_tform.transform.rotation.w = world_pose.rot.w; + hog_actual_tform.transform.rotation.x = world_pose.rot.x; + hog_actual_tform.transform.rotation.y = world_pose.rot.y; + hog_actual_tform.transform.rotation.z = world_pose.rot.z; + + tf_broadcaster_->sendTransform(hog_actual_tform); + } + +} From 23d9b92bfe7c7494edd7f92b4c2e208438cd7edc Mon Sep 17 00:00:00 2001 From: Jonathan Bohren Date: Thu, 15 May 2014 13:52:57 -0400 Subject: [PATCH 079/153] HoG: adding install target --- gazebo_plugins/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index d438e2a8b..5c7142d89 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -250,6 +250,7 @@ install(TARGETS gazebo_ros_imu gazebo_ros_f3d gazebo_ros_bumper + gazebo_ros_hand_of_god gazebo_ros_template gazebo_ros_projector gazebo_ros_prosilica From 6f14e7728a10610e9a0208c3914c45a4d85faa15 Mon Sep 17 00:00:00 2001 From: Jonathan Bohren Date: Wed, 30 Oct 2013 04:10:57 -0400 Subject: [PATCH 080/153] hand_of_god: Adding hand-of-god plugin ros_force: Fixing error messages to refer to the right plugin --- gazebo_plugins/CMakeLists.txt | 7 + .../gazebo_plugins/gazebo_ros_hand_of_god.h | 75 +++++++ gazebo_plugins/package.xml | 2 + gazebo_plugins/src/gazebo_ros_force.cpp | 6 +- gazebo_plugins/src/gazebo_ros_hand_of_god.cpp | 190 ++++++++++++++++++ 5 files changed, 277 insertions(+), 3 deletions(-) create mode 100644 gazebo_plugins/include/gazebo_plugins/gazebo_ros_hand_of_god.h create mode 100644 gazebo_plugins/src/gazebo_ros_hand_of_god.cpp diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index 565ec0c1d..a78154264 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(catkin REQUIRED COMPONENTS nav_msgs urdf tf + tf2_ros dynamic_reconfigure driver_base rosgraph_msgs @@ -107,6 +108,7 @@ catkin_package( nav_msgs urdf tf + tf2_ros dynamic_reconfigure driver_base rosgraph_msgs @@ -217,6 +219,11 @@ target_link_libraries(gazebo_ros_video ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} $ add_library(gazebo_ros_planar_move src/gazebo_ros_planar_move.cpp) target_link_libraries(gazebo_ros_planar_move ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +add_library(gazebo_ros_hand_of_god src/gazebo_ros_hand_of_god.cpp) +set_target_properties(gazebo_ros_hand_of_god PROPERTIES LINK_FLAGS "${ld_flags}") +set_target_properties(gazebo_ros_hand_of_god PROPERTIES COMPILE_FLAGS "${cxx_flags}") +target_link_libraries(gazebo_ros_hand_of_god ${GAZEBO_LIBRARIES} ${SDFormat_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + ## ## Add your new plugin here ## diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_hand_of_god.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_hand_of_god.h new file mode 100644 index 000000000..55fae1daa --- /dev/null +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_hand_of_god.h @@ -0,0 +1,75 @@ +/* + * Gazebo - Outdoor Multi-Robot Simulator + * Copyright (C) 2003 + * Nate Koenig & Andrew Howard + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ +/* + * Desc: 3D position interface. + * Author: Sachin Chitta and John Hsu + * Date: 10 June 2008 + * SVN: $Id$ + */ +#ifndef GAZEBO_ROS_TEMPLATE_HH +#define GAZEBO_ROS_TEMPLATE_HH + +#include + +#include +#include +#include +#include +#include + +#include +#include + +namespace gazebo +{ + + class GazeboRosHandOfGod : public ModelPlugin + { + /// \brief Constructor + public: GazeboRosHandOfGod(); + + /// \brief Destructor + public: virtual ~GazeboRosHandOfGod(); + + /// \brief Load the controller + public: void Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf ); + + /// \brief Update the controller + protected: virtual void GazeboUpdate(); + + /// Pointer to the update event connection + private: event::ConnectionPtr update_connection_; + boost::shared_ptr tf_buffer_; + boost::shared_ptr tf_listener_; + boost::shared_ptr tf_broadcaster_; + physics::ModelPtr model_; + physics::LinkPtr floating_link_; + std::string link_name_; + std::string robot_namespace_; + std::string frame_id_; + double kl_, ka_; + double cl_, ca_; + }; + +} + +#endif + diff --git a/gazebo_plugins/package.xml b/gazebo_plugins/package.xml index 32b4fb697..57ec545a9 100644 --- a/gazebo_plugins/package.xml +++ b/gazebo_plugins/package.xml @@ -31,6 +31,7 @@ nav_msgs urdf tf + tf2_ros dynamic_reconfigure driver_base rosgraph_msgs @@ -56,6 +57,7 @@ nav_msgs urdf tf + tf2_ros dynamic_reconfigure driver_base rosgraph_msgs diff --git a/gazebo_plugins/src/gazebo_ros_force.cpp b/gazebo_plugins/src/gazebo_ros_force.cpp index 7f10ce8e4..0f6cc0721 100644 --- a/gazebo_plugins/src/gazebo_ros_force.cpp +++ b/gazebo_plugins/src/gazebo_ros_force.cpp @@ -71,7 +71,7 @@ void GazeboRosForce::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) if (!_sdf->HasElement("bodyName")) { - ROS_FATAL("f3d plugin missing , cannot proceed"); + ROS_FATAL("force plugin missing , cannot proceed"); return; } else @@ -80,13 +80,13 @@ void GazeboRosForce::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) this->link_ = _model->GetLink(this->link_name_); if (!this->link_) { - ROS_FATAL("gazebo_ros_f3d plugin error: link named: %s does not exist\n",this->link_name_.c_str()); + ROS_FATAL("gazebo_ros_force plugin error: link named: %s does not exist\n",this->link_name_.c_str()); return; } if (!_sdf->HasElement("topicName")) { - ROS_FATAL("f3d plugin missing , cannot proceed"); + ROS_FATAL("force plugin missing , cannot proceed"); return; } else diff --git a/gazebo_plugins/src/gazebo_ros_hand_of_god.cpp b/gazebo_plugins/src/gazebo_ros_hand_of_god.cpp new file mode 100644 index 000000000..607476c9e --- /dev/null +++ b/gazebo_plugins/src/gazebo_ros_hand_of_god.cpp @@ -0,0 +1,190 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Open Source Robotics Foundation + * nor the names of its contributors may be + * used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/** + * \author Jonathan Bohren + * \desc A "hand-of-god" plugin which drives a floating object around based + * on the location of a TF frame. This plugin is useful for connecting human input + * devices to "god-like" objects in a Gazebo simulation. + */ + +#include +#include + +namespace gazebo +{ + // Register this plugin with the simulator + GZ_REGISTER_MODEL_PLUGIN(GazeboRosHandOfGod); + + //////////////////////////////////////////////////////////////////////////////// + // Constructor + GazeboRosHandOfGod::GazeboRosHandOfGod() : + ModelPlugin(), + robot_namespace_(""), + frame_id_("hog"), + kl_(200), + ka_(200) + { + } + + //////////////////////////////////////////////////////////////////////////////// + // Destructor + GazeboRosHandOfGod::~GazeboRosHandOfGod() + { + } + + //////////////////////////////////////////////////////////////////////////////// + // Load the controller + void GazeboRosHandOfGod::Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf ) + { + // Make sure the ROS node for Gazebo has already been initalized + if (!ros::isInitialized()) { + ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; + } + + // Get sdf parameters + if(_sdf->HasElement("robotNamespace")) { + this->robot_namespace_ = _sdf->Get("robotNamespace") + "/"; + } + + if(_sdf->HasElement("frameId")) { + this->frame_id_ = _sdf->Get("frameId"); + } + + if(_sdf->HasElement("kl")) { + this->kl_ = _sdf->Get("kl"); + } + if(_sdf->HasElement("ka")) { + this->ka_ = _sdf->Get("ka"); + } + + if(_sdf->HasElement("linkName")) { + this->link_name_ = _sdf->Get("linkName"); + } else { + ROS_FATAL_STREAM("The hand-of-god plugin requires a `linkName` parameter tag"); + return; + } + + // Store the model + model_ = _parent; + + // Disable gravity for the hog + model_->SetGravityMode(false); + + // Get the floating link + floating_link_ = model_->GetLink(link_name_); + if(!floating_link_) { + ROS_ERROR("Floating link not found!"); + const std::vector &links = model_->GetLinks(); + for(unsigned i=0; i < links.size(); i++) { + ROS_ERROR_STREAM(" -- Link "<GetName()); + } + return; + } + + cl_ = 2.0 * sqrt(kl_*floating_link_->GetInertial()->GetMass()); + ca_ = 2.0 * sqrt(ka_*floating_link_->GetInertial()->GetIXX()); + + // Create the TF listener for the desired position of the hog + tf_buffer_.reset(new tf2_ros::Buffer()); + tf_listener_.reset(new tf2_ros::TransformListener(*tf_buffer_)); + tf_broadcaster_.reset(new tf2_ros::TransformBroadcaster()); + + // Register update event handler + this->update_connection_ = event::Events::ConnectWorldUpdateBegin( + boost::bind(&GazeboRosHandOfGod::GazeboUpdate, this)); + } + + //////////////////////////////////////////////////////////////////////////////// + // Update the controller + void GazeboRosHandOfGod::GazeboUpdate() + { + // Get TF transform relative to the /world link + geometry_msgs::TransformStamped hog_desired_tform; + static bool errored = false; + try{ + hog_desired_tform = tf_buffer_->lookupTransform("world", frame_id_+"_desired", ros::Time(0)); + errored = false; + } catch (tf2::TransformException ex){ + if(!errored) { + ROS_ERROR("%s",ex.what()); + errored = true; + } + return; + } + + // Convert TF transform to Gazebo Pose + const geometry_msgs::Vector3 &p = hog_desired_tform.transform.translation; + const geometry_msgs::Quaternion &q = hog_desired_tform.transform.rotation; + gazebo::math::Pose hog_desired( + gazebo::math::Vector3(p.x, p.y, p.z), + gazebo::math::Quaternion(q.w, q.x, q.y, q.z)); + + // Relative transform from actual to desired pose + gazebo::math::Pose world_pose = floating_link_->GetDirtyPose(); + gazebo::math::Vector3 err_pos = hog_desired.pos - world_pose.pos; + // Get exponential coordinates for rotation + gazebo::math::Quaternion err_rot = (world_pose.rot.GetAsMatrix4().Inverse() * hog_desired.rot.GetAsMatrix4()).GetRotation(); + gazebo::math::Quaternion not_a_quaternion = err_rot.GetLog(); + + floating_link_->AddForce( + kl_ * err_pos - cl_ * floating_link_->GetWorldLinearVel()); + + floating_link_->AddRelativeTorque( + ka_ * gazebo::math::Vector3(not_a_quaternion.x, not_a_quaternion.y, not_a_quaternion.z) - ca_ * floating_link_->GetRelativeAngularVel()); + + // Convert actual pose to TransformStamped message + geometry_msgs::TransformStamped hog_actual_tform; + + hog_actual_tform.header.frame_id = "world"; + hog_actual_tform.header.stamp = ros::Time::now(); + + hog_actual_tform.child_frame_id = frame_id_ + "_actual"; + + hog_actual_tform.transform.translation.x = world_pose.pos.x; + hog_actual_tform.transform.translation.y = world_pose.pos.y; + hog_actual_tform.transform.translation.z = world_pose.pos.z; + + hog_actual_tform.transform.rotation.w = world_pose.rot.w; + hog_actual_tform.transform.rotation.x = world_pose.rot.x; + hog_actual_tform.transform.rotation.y = world_pose.rot.y; + hog_actual_tform.transform.rotation.z = world_pose.rot.z; + + tf_broadcaster_->sendTransform(hog_actual_tform); + } + +} From 41b0a2cd8d1a4ce705c4f9fa47a7cb97d7f6cfcf Mon Sep 17 00:00:00 2001 From: Jonathan Bohren Date: Thu, 15 May 2014 13:52:57 -0400 Subject: [PATCH 081/153] HoG: adding install target --- gazebo_plugins/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index a78154264..190dbb1ea 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -249,6 +249,7 @@ install(TARGETS gazebo_ros_imu gazebo_ros_f3d gazebo_ros_bumper + gazebo_ros_hand_of_god gazebo_ros_template gazebo_ros_projector gazebo_ros_prosilica From 9469e85ad817163f94be97d66cc45e6bec5dab28 Mon Sep 17 00:00:00 2001 From: Markus Bader Date: Sun, 18 May 2014 21:55:32 +0200 Subject: [PATCH 082/153] fixed gpu_laser to work with workspaces --- .../gazebo_plugins/gazebo_ros_gpu_laser.h | 3 +++ gazebo_plugins/src/gazebo_ros_gpu_laser.cpp | 23 ++++++++++++------- 2 files changed, 18 insertions(+), 8 deletions(-) diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_gpu_laser.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_gpu_laser.h index 741bc52f7..8e09e5e06 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_gpu_laser.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_gpu_laser.h @@ -75,6 +75,9 @@ namespace gazebo /// \brief frame transform name, should match link name private: std::string frame_name_; + + /// \brief tf prefix + private: std::string tf_prefix_; /// \brief for setting ROS name space private: std::string robot_namespace_; diff --git a/gazebo_plugins/src/gazebo_ros_gpu_laser.cpp b/gazebo_plugins/src/gazebo_ros_gpu_laser.cpp index 1302814f5..49ecf98c3 100644 --- a/gazebo_plugins/src/gazebo_ros_gpu_laser.cpp +++ b/gazebo_plugins/src/gazebo_ros_gpu_laser.cpp @@ -35,8 +35,10 @@ #include #include +#include #include "gazebo_plugins/gazebo_ros_gpu_laser.h" +#include namespace gazebo { @@ -78,9 +80,7 @@ void GazeboRosLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) if (!this->parent_ray_sensor_) gzthrow("GazeboRosLaser controller requires a Ray Sensor as its parent"); - this->robot_namespace_ = ""; - if (this->sdf->HasElement("robotNamespace")) - this->robot_namespace_ = this->sdf->Get("robotNamespace") + "/"; + this->robot_namespace_ = GetRobotNamespace(_parent, _sdf, "Laser"); if (!this->sdf->HasElement("frameName")) { @@ -120,16 +120,23 @@ void GazeboRosLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) // Load the controller void GazeboRosLaser::LoadThread() { - this->rosnode_ = new ros::NodeHandle(this->robot_namespace_); this->gazebo_node_ = gazebo::transport::NodePtr(new gazebo::transport::Node()); this->gazebo_node_->Init(this->world_name_); this->pmq.startServiceThread(); - + + this->rosnode_ = new ros::NodeHandle(this->robot_namespace_); + + this->tf_prefix_ = tf::getPrefixParam(*this->rosnode_); + if(this->tf_prefix_.empty()) { + this->tf_prefix_ = this->robot_namespace_; + boost::trim_right_if(this->tf_prefix_,boost::is_any_of("/")); + } + ROS_INFO("GPU Laser Plugin (ns = %s) , set to \"%s\"", + this->robot_namespace_.c_str(), this->tf_prefix_.c_str()); + // resolve tf prefix - std::string prefix; - this->rosnode_->getParam(std::string("tf_prefix"), prefix); - this->frame_name_ = tf::resolve(prefix, this->frame_name_); + this->frame_name_ = tf::resolve(this->tf_prefix_, this->frame_name_); if (this->topic_name_ != "") { From 638e63428e38034053f162be37e6a9ec9de5e7bd Mon Sep 17 00:00:00 2001 From: Markus Bader Date: Tue, 20 May 2014 09:29:35 +0200 Subject: [PATCH 083/153] GPU Laser test example added --- .../xacro/laser/hokuyo_gpu.xacro | 76 +++++++++++++++++++ .../xacro/p3dx/pioneer3dx.xacro | 6 ++ 2 files changed, 82 insertions(+) create mode 100644 gazebo_plugins/test/multi_robot_scenario/xacro/laser/hokuyo_gpu.xacro diff --git a/gazebo_plugins/test/multi_robot_scenario/xacro/laser/hokuyo_gpu.xacro b/gazebo_plugins/test/multi_robot_scenario/xacro/laser/hokuyo_gpu.xacro new file mode 100644 index 000000000..68c65960f --- /dev/null +++ b/gazebo_plugins/test/multi_robot_scenario/xacro/laser/hokuyo_gpu.xacro @@ -0,0 +1,76 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0 0 0 0 0 0 + 1 + 30 + true + + + + + 1 + 2.0944 + -2.0944 + + + 683 + + + + 0.10 + 140.0 + 0.01 + + + gaussian + + 0.0 + 0.01 + + + + + ${name}/scan + ${name} + + + + + + diff --git a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx.xacro b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx.xacro index 602f87e47..5b2a09fc5 100644 --- a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx.xacro +++ b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx.xacro @@ -8,8 +8,14 @@ + + + + + From f4974101f49c6dfd2d60c4049b9b4a7fa1911afd Mon Sep 17 00:00:00 2001 From: Markus Bader Date: Tue, 20 May 2014 09:30:21 +0200 Subject: [PATCH 084/153] Pioneer model: Diff_drive torque reduced --- .../multi_robot_scenario/xacro/p3dx/pioneer3dx_plugins.xacro | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_plugins.xacro b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_plugins.xacro index e8e1a9d74..b6ad098d4 100644 --- a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_plugins.xacro +++ b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_plugins.xacro @@ -23,7 +23,7 @@ right_hub_joint 0.5380 0.18 - 20 + 3 cmd_vel odom odom From b9d24d52d835781186ed010791179aa955014747 Mon Sep 17 00:00:00 2001 From: Scott K Logan Date: Tue, 20 May 2014 16:05:13 -0500 Subject: [PATCH 085/153] gazebo_ros_control: Add build-time dependency on gazebo This fixes a regression caused by a889ef8b768861231a67b78781514d834f631b8e --- gazebo_ros_control/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/gazebo_ros_control/package.xml b/gazebo_ros_control/package.xml index 85e9f8ca5..a696a26bd 100644 --- a/gazebo_ros_control/package.xml +++ b/gazebo_ros_control/package.xml @@ -18,6 +18,7 @@ catkin roscpp + gazebo control_toolbox controller_manager pluginlib From 68364b9e8743925847f8883d3b822bfaefe76880 Mon Sep 17 00:00:00 2001 From: Scott K Logan Date: Tue, 20 May 2014 16:06:25 -0500 Subject: [PATCH 086/153] gazebo_ros_control: Add dependency on angles --- gazebo_ros_control/CMakeLists.txt | 1 + gazebo_ros_control/package.xml | 2 ++ 2 files changed, 3 insertions(+) diff --git a/gazebo_ros_control/CMakeLists.txt b/gazebo_ros_control/CMakeLists.txt index 01608eb5d..87de359d9 100644 --- a/gazebo_ros_control/CMakeLists.txt +++ b/gazebo_ros_control/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(catkin REQUIRED COMPONENTS pluginlib joint_limits_interface urdf + angles ) # Depend on system install of Gazebo diff --git a/gazebo_ros_control/package.xml b/gazebo_ros_control/package.xml index a696a26bd..fac556b2c 100644 --- a/gazebo_ros_control/package.xml +++ b/gazebo_ros_control/package.xml @@ -25,6 +25,7 @@ transmission_interface joint_limits_interface urdf + angles roscpp gazebo_ros @@ -33,6 +34,7 @@ pluginlib transmission_interface urdf + angles From 69f25cabf40c38ad8513614052f3a701b2d12eb8 Mon Sep 17 00:00:00 2001 From: Markus Bader Date: Thu, 22 May 2014 13:42:14 +0200 Subject: [PATCH 087/153] ros diff drive supports now an acceleration limit --- .../gazebo_plugins/gazebo_ros_diff_drive.h | 2 + gazebo_plugins/src/gazebo_ros_diff_drive.cpp | 52 ++++++++++++++++++- .../xacro/p3dx/pioneer3dx_plugins.xacro | 7 +-- 3 files changed, 56 insertions(+), 5 deletions(-) diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_diff_drive.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_diff_drive.h index 3b1102a3b..40d3b9061 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_diff_drive.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_diff_drive.h @@ -98,6 +98,8 @@ namespace gazebo { double wheel_diameter_; double torque; double wheel_speed_[2]; + double max_accel; + double wheel_speed_instr_[2]; std::vector joints_; diff --git a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp index 25bd2bde2..a436379a5 100644 --- a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp +++ b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp @@ -38,6 +38,15 @@ * $ Id: 06/21/2013 11:23:40 AM piyushk $ */ + +/* + * + * The support of acceleration limit was added by + * \author George Todoran + * \author Markus Bader + * \date 22th of May 2014 + */ + #include #include @@ -205,6 +214,14 @@ void GazeboRosDiffDrive::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) { this->robot_namespace_.c_str(), value.c_str()); } } + this->max_accel=0; + if (!_sdf->HasElement("max_accel")) { + ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to %f", + this->robot_namespace_.c_str(), this->max_accel); + } else { + this->max_accel = _sdf->GetElement("max_accel")->Get(); + } + ROS_INFO("GazeboRosDiffDrive Plugin (ns = %s) , set to %s", this->robot_namespace_.c_str(), (this->publishWheelJointState_?"ture":"false")); @@ -333,8 +350,38 @@ void GazeboRosDiffDrive::UpdateChild() { // Update robot in case new velocities have been requested getWheelVelocities(); - joints_[LEFT]->SetVelocity(0, wheel_speed_[LEFT] / (wheel_diameter_ / 2.0)); - joints_[RIGHT]->SetVelocity(0, wheel_speed_[RIGHT] / (wheel_diameter_ / 2.0)); + + + double current_speed[2]; + + current_speed[LEFT] = joints_[LEFT]->GetVelocity(0) * (wheel_diameter_ / 2.0); + current_speed[RIGHT] = joints_[RIGHT]->GetVelocity(0) * (wheel_diameter_ / 2.0); + + if(max_accel==0 + || (fabs(wheel_speed_[LEFT] - current_speed[LEFT]) < 0.01) + || (fabs(wheel_speed_[RIGHT] - current_speed[RIGHT]) < 0.01)) { + //if max_accel==0, or target speed is reached + joints_[LEFT]->SetVelocity(0, wheel_speed_[LEFT]/ (wheel_diameter_ / 2.0)); + joints_[RIGHT]->SetVelocity(0, wheel_speed_[RIGHT]/ (wheel_diameter_ / 2.0)); + } + else { + + if(wheel_speed_[LEFT]>=current_speed[LEFT]) + wheel_speed_instr_[LEFT]+=fmin(wheel_speed_[LEFT]-current_speed[LEFT], max_accel * seconds_since_last_update); + else + wheel_speed_instr_[LEFT]+=fmax(wheel_speed_[LEFT]-current_speed[LEFT], -max_accel * seconds_since_last_update); + + if(wheel_speed_[RIGHT]>current_speed[RIGHT]) + wheel_speed_instr_[RIGHT]+=fmin(wheel_speed_[RIGHT]-current_speed[RIGHT], max_accel * seconds_since_last_update); + else + wheel_speed_instr_[RIGHT]+=fmax(wheel_speed_[RIGHT]-current_speed[RIGHT], -max_accel * seconds_since_last_update); + + // ROS_INFO("actual wheel speed = %lf, issued wheel speed= %lf", current_speed[LEFT], wheel_speed_[LEFT]); + // ROS_INFO("actual wheel speed = %lf, issued wheel speed= %lf", current_speed[RIGHT],wheel_speed_[RIGHT]); + + joints_[LEFT]->SetVelocity(0,wheel_speed_instr_[LEFT] / (wheel_diameter_ / 2.0)); + joints_[RIGHT]->SetVelocity(0,wheel_speed_instr_[RIGHT] / (wheel_diameter_ / 2.0)); + } last_update_time_+= common::Time(update_period_); @@ -428,3 +475,4 @@ void GazeboRosDiffDrive::publishOdometry(double step_time) { GZ_REGISTER_MODEL_PLUGIN(GazeboRosDiffDrive) } + diff --git a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_plugins.xacro b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_plugins.xacro index b6ad098d4..f1a61b916 100644 --- a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_plugins.xacro +++ b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_plugins.xacro @@ -21,14 +21,15 @@ true left_hub_joint right_hub_joint - 0.5380 + 0.3 0.18 - 3 + 20 cmd_vel odom odom base_link - 100.0 + 10.0 + 1.8 From 4776545954d55749f070eaf95b7d762d5eab0187 Mon Sep 17 00:00:00 2001 From: Jonathan Bohren Date: Mon, 26 May 2014 19:11:16 -0400 Subject: [PATCH 088/153] gazebo_ros_control: default_robot_hw_sim: Suppressing pid error message, depends on ros-controls/control_toolbox#21 --- gazebo_ros_control/src/default_robot_hw_sim.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gazebo_ros_control/src/default_robot_hw_sim.cpp b/gazebo_ros_control/src/default_robot_hw_sim.cpp index 11882922a..1f3e20a01 100644 --- a/gazebo_ros_control/src/default_robot_hw_sim.cpp +++ b/gazebo_ros_control/src/default_robot_hw_sim.cpp @@ -217,7 +217,7 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim // joint->SetVelocity() to control the joint. const ros::NodeHandle nh(model_nh, robot_namespace + "/gazebo_ros_control/pid_gains/" + joint_names_[j]); - if (pid_controllers_[j].init(nh)) + if (pid_controllers_[j].init(nh,true)) { switch (joint_control_methods_[j]) { From 12beb3e22abd51f163eeabac483c24a9d2f98f9a Mon Sep 17 00:00:00 2001 From: Arn-O Date: Fri, 30 May 2014 16:04:44 +0200 Subject: [PATCH 089/153] fix issue #198 Operator ``==`` is not recognized by sh scripts. --- gazebo_ros/scripts/gazebo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gazebo_ros/scripts/gazebo b/gazebo_ros/scripts/gazebo index ffb4a5ec6..668438583 100755 --- a/gazebo_ros/scripts/gazebo +++ b/gazebo_ros/scripts/gazebo @@ -2,7 +2,7 @@ final="$@" EXT=so -if [ $(uname) == "Darwin" ]; then +if [ $(uname) = "Darwin" ]; then EXT=dylib fi From 3cae54281803f6643141bf6a6518263d615d2010 Mon Sep 17 00:00:00 2001 From: Arn-O Date: Fri, 30 May 2014 16:05:06 +0200 Subject: [PATCH 090/153] fix issue #198 Operator ``==`` is not recognized by sh scripts. --- gazebo_ros/scripts/gdbrun | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gazebo_ros/scripts/gdbrun b/gazebo_ros/scripts/gdbrun index 04fc9d69c..9a503846c 100755 --- a/gazebo_ros/scripts/gdbrun +++ b/gazebo_ros/scripts/gdbrun @@ -1,6 +1,6 @@ #!/bin/bash extra_text="" -if [ "$1" == "--break-main" ]; then +if [ "$1" = "--break-main" ]; then extra_text="break main" shift fi From cefc9955a26b65334bcbeb6ee55bd5ebf609782a Mon Sep 17 00:00:00 2001 From: Arn-O Date: Fri, 30 May 2014 16:05:24 +0200 Subject: [PATCH 091/153] fix issue #198 Operator ``==`` is not recognized by sh scripts. --- gazebo_ros/scripts/gzclient | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gazebo_ros/scripts/gzclient b/gazebo_ros/scripts/gzclient index 5f7d2887e..4ad9c740b 100755 --- a/gazebo_ros/scripts/gzclient +++ b/gazebo_ros/scripts/gzclient @@ -2,7 +2,7 @@ final="$@" EXT=so -if [ $(uname) == "Darwin" ]; then +if [ $(uname) = "Darwin" ]; then EXT=dylib fi From 2ec199f2328b1abd48098ab3f56d6a497d63b869 Mon Sep 17 00:00:00 2001 From: Arn-O Date: Fri, 30 May 2014 16:05:44 +0200 Subject: [PATCH 092/153] fix issue #198 Operator ``==`` is not recognized by sh scripts. --- gazebo_ros/scripts/gzserver | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gazebo_ros/scripts/gzserver b/gazebo_ros/scripts/gzserver index e53d939ab..65f5010a5 100755 --- a/gazebo_ros/scripts/gzserver +++ b/gazebo_ros/scripts/gzserver @@ -2,7 +2,7 @@ final="$@" EXT=so -if [ $(uname) == "Darwin" ]; then +if [ $(uname) = "Darwin" ]; then EXT=dylib fi From 020ab9b84e4fc1beeaee790dd0ad88c8b5cc91dc Mon Sep 17 00:00:00 2001 From: Arn-O Date: Fri, 30 May 2014 16:06:01 +0200 Subject: [PATCH 093/153] fix issue #198 Operator ``==`` is not recognized by sh scripts. --- gazebo_ros/scripts/perf | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gazebo_ros/scripts/perf b/gazebo_ros/scripts/perf index 31f06d607..ec310abf1 100755 --- a/gazebo_ros/scripts/perf +++ b/gazebo_ros/scripts/perf @@ -2,7 +2,7 @@ final="$@" EXT=so -if [ $(uname) == "Darwin" ]; then +if [ $(uname) = "Darwin" ]; then EXT=dylib fi From 7db00d9086a27edaac723c669f5eb3f8fe081d9d Mon Sep 17 00:00:00 2001 From: Jon Binney Date: Sat, 31 May 2014 15:38:19 -0700 Subject: [PATCH 094/153] Fix repo names in package.xml's --- gazebo_msgs/package.xml | 4 ++-- gazebo_plugins/package.xml | 4 ++-- gazebo_ros/package.xml | 4 ++-- gazebo_ros_control/package.xml | 4 ++-- gazebo_ros_pkgs/package.xml | 4 ++-- 5 files changed, 10 insertions(+), 10 deletions(-) diff --git a/gazebo_msgs/package.xml b/gazebo_msgs/package.xml index 43724024d..a43bfe269 100644 --- a/gazebo_msgs/package.xml +++ b/gazebo_msgs/package.xml @@ -11,8 +11,8 @@ BSD http://gazebosim.org/wiki/Tutorials#ROS_Integration - https://github.com/osrf/gazebo_ros_pkgs/issues - https://github.com/osrf/gazebo_ros_pkgs + https://github.com/ros-simulation/gazebo_ros_pkgs/issues + https://github.com/ros-simulation/gazebo_ros_pkgs John Hsu diff --git a/gazebo_plugins/package.xml b/gazebo_plugins/package.xml index 57ec545a9..e326392b9 100644 --- a/gazebo_plugins/package.xml +++ b/gazebo_plugins/package.xml @@ -11,8 +11,8 @@ BSD http://gazebosim.org/wiki/Tutorials#ROS_Integration - https://github.com/osrf/gazebo_ros_pkgs/issues - https://github.com/osrf/gazebo_ros_pkgs + https://github.com/ros-simulation/gazebo_ros_pkgs/issues + https://github.com/ros-simulation/gazebo_ros_pkgs John Hsu diff --git a/gazebo_ros/package.xml b/gazebo_ros/package.xml index b6b071d83..edb2a19c8 100644 --- a/gazebo_ros/package.xml +++ b/gazebo_ros/package.xml @@ -13,8 +13,8 @@ Apache 2.0 http://gazebosim.org/wiki/Tutorials#ROS_Integration - https://github.com/osrf/gazebo_ros_pkgs/issues - https://github.com/osrf/gazebo_ros_pkgs + https://github.com/ros-simulation/gazebo_ros_pkgs/issues + https://github.com/ros-simulation/gazebo_ros_pkgs John Hsu Nate Koenig diff --git a/gazebo_ros_control/package.xml b/gazebo_ros_control/package.xml index fac556b2c..d1576dfae 100644 --- a/gazebo_ros_control/package.xml +++ b/gazebo_ros_control/package.xml @@ -9,8 +9,8 @@ BSD http://ros.org/wiki/gazebo_ros_control - https://github.com/osrf/gazebo_ros_pkgs/issues - https://github.com/osrf/gazebo_ros_pkgs + https://github.com/ros-simulation/gazebo_ros_pkgs/issues + https://github.com/ros-simulation/gazebo_ros_pkgs Jonathan Bohren Dave Coleman diff --git a/gazebo_ros_pkgs/package.xml b/gazebo_ros_pkgs/package.xml index 34201326b..82e4d1d76 100644 --- a/gazebo_ros_pkgs/package.xml +++ b/gazebo_ros_pkgs/package.xml @@ -10,8 +10,8 @@ BSD,LGPL,Apache 2.0 http://gazebosim.org/wiki/Tutorials#ROS_Integration - https://github.com/osrf/gazebo_ros_pkgs/issues - https://github.com/osrf/gazebo_ros_pkgs + https://github.com/ros-simulation/gazebo_ros_pkgs/issues + https://github.com/ros-simulation/gazebo_ros_pkgs John Hsu, Nate Koenig, Dave Coleman> From 197616a26c192dc39ad53d2061386873a4df92a5 Mon Sep 17 00:00:00 2001 From: Jon Binney Date: Sat, 31 May 2014 15:38:19 -0700 Subject: [PATCH 095/153] Fix repo names in package.xml's --- gazebo_msgs/package.xml | 4 ++-- gazebo_plugins/package.xml | 4 ++-- gazebo_ros/package.xml | 4 ++-- gazebo_ros_control/package.xml | 4 ++-- gazebo_ros_pkgs/package.xml | 4 ++-- 5 files changed, 10 insertions(+), 10 deletions(-) diff --git a/gazebo_msgs/package.xml b/gazebo_msgs/package.xml index aef8a61e4..21217194d 100644 --- a/gazebo_msgs/package.xml +++ b/gazebo_msgs/package.xml @@ -11,8 +11,8 @@ BSD http://gazebosim.org/wiki/Tutorials#ROS_Integration - https://github.com/osrf/gazebo_ros_pkgs/issues - https://github.com/osrf/gazebo_ros_pkgs + https://github.com/ros-simulation/gazebo_ros_pkgs/issues + https://github.com/ros-simulation/gazebo_ros_pkgs John Hsu diff --git a/gazebo_plugins/package.xml b/gazebo_plugins/package.xml index 2c45d3c0a..75ca23210 100644 --- a/gazebo_plugins/package.xml +++ b/gazebo_plugins/package.xml @@ -11,8 +11,8 @@ BSD http://gazebosim.org/wiki/Tutorials#ROS_Integration - https://github.com/osrf/gazebo_ros_pkgs/issues - https://github.com/osrf/gazebo_ros_pkgs + https://github.com/ros-simulation/gazebo_ros_pkgs/issues + https://github.com/ros-simulation/gazebo_ros_pkgs John Hsu diff --git a/gazebo_ros/package.xml b/gazebo_ros/package.xml index bedc9315e..587a87e38 100644 --- a/gazebo_ros/package.xml +++ b/gazebo_ros/package.xml @@ -13,8 +13,8 @@ Apache 2.0 http://gazebosim.org/wiki/Tutorials#ROS_Integration - https://github.com/osrf/gazebo_ros_pkgs/issues - https://github.com/osrf/gazebo_ros_pkgs + https://github.com/ros-simulation/gazebo_ros_pkgs/issues + https://github.com/ros-simulation/gazebo_ros_pkgs John Hsu Nate Koenig diff --git a/gazebo_ros_control/package.xml b/gazebo_ros_control/package.xml index fbd582224..955ae8a19 100644 --- a/gazebo_ros_control/package.xml +++ b/gazebo_ros_control/package.xml @@ -9,8 +9,8 @@ BSD http://ros.org/wiki/gazebo_ros_control - https://github.com/osrf/gazebo_ros_pkgs/issues - https://github.com/osrf/gazebo_ros_pkgs + https://github.com/ros-simulation/gazebo_ros_pkgs/issues + https://github.com/ros-simulation/gazebo_ros_pkgs Jonathan Bohren Dave Coleman diff --git a/gazebo_ros_pkgs/package.xml b/gazebo_ros_pkgs/package.xml index 699e0263c..c77e3025e 100644 --- a/gazebo_ros_pkgs/package.xml +++ b/gazebo_ros_pkgs/package.xml @@ -10,8 +10,8 @@ BSD,LGPL,Apache 2.0 http://gazebosim.org/wiki/Tutorials#ROS_Integration - https://github.com/osrf/gazebo_ros_pkgs/issues - https://github.com/osrf/gazebo_ros_pkgs + https://github.com/ros-simulation/gazebo_ros_pkgs/issues + https://github.com/ros-simulation/gazebo_ros_pkgs John Hsu, Nate Koenig, Dave Coleman> From 5505fe32c8950584417b8ff5f76c3def2c9dcce2 Mon Sep 17 00:00:00 2001 From: fsuarez6 Date: Thu, 5 Jun 2014 19:00:34 +0200 Subject: [PATCH 096/153] gazebo_plugins: Adding ForceTorqueSensor Plugin --- gazebo_plugins/CMakeLists.txt | 5 + .../gazebo_plugins/gazebo_ros_ft_sensor.h | 127 +++++++++++ gazebo_plugins/src/gazebo_ros_ft_sensor.cpp | 210 ++++++++++++++++++ 3 files changed, 342 insertions(+) create mode 100644 gazebo_plugins/include/gazebo_plugins/gazebo_ros_ft_sensor.h create mode 100644 gazebo_plugins/src/gazebo_ros_ft_sensor.cpp diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index 3e89a63c8..8e1b45f15 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -83,6 +83,7 @@ catkin_package( gazebo_ros_p3d gazebo_ros_imu gazebo_ros_f3d + gazebo_ros_ft_sensor gazebo_ros_bumper gazebo_ros_template gazebo_ros_projector @@ -223,6 +224,9 @@ set_target_properties(gazebo_ros_hand_of_god PROPERTIES LINK_FLAGS "${ld_flags}" set_target_properties(gazebo_ros_hand_of_god PROPERTIES COMPILE_FLAGS "${cxx_flags}") target_link_libraries(gazebo_ros_hand_of_god ${GAZEBO_LIBRARIES} ${SDFormat_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +add_library(gazebo_ros_ft_sensor src/gazebo_ros_ft_sensor.cpp) +target_link_libraries(gazebo_ros_ft_sensor ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + ## ## Add your new plugin here ## @@ -247,6 +251,7 @@ install(TARGETS gazebo_ros_p3d gazebo_ros_imu gazebo_ros_f3d + gazebo_ros_ft_sensor gazebo_ros_bumper gazebo_ros_hand_of_god gazebo_ros_template diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_ft_sensor.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_ft_sensor.h new file mode 100644 index 000000000..f86d05a74 --- /dev/null +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_ft_sensor.h @@ -0,0 +1,127 @@ +/* + * Copyright (C) 2014 + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ +/* + * Desc: Force Torque Sensor Plugin + * Author: Francisco Suarez-Ruiz + * Date: 5 June 2014 + */ + +#ifndef GAZEBO_ROS_FT_HH +#define GAZEBO_ROS_FT_HH + +// Custom Callback Queue +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace gazebo +{ + +/// \brief GazeboRosFT controller +/// This is a controller that simulates a 6 dof force sensor +class GazeboRosFT : public ModelPlugin +{ + /// \brief Constructor + /// \param parent The parent entity must be a Model + public: GazeboRosFT(); + + /// \brief Destructor + public: virtual ~GazeboRosFT(); + + /// \brief Load the controller + public: void Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf ); + + /// \brief Update the controller + protected: virtual void UpdateChild(); + + /// \brief A pointer to the Gazebo joint + private: physics::JointPtr joint_; + + /// \brief A pointer to the Gazebo parent link + private: physics::LinkPtr parent_link_; + + /// \brief A pointer to the Gazebo child link + private: physics::LinkPtr child_link_; + + /// \brief A pointer to the Gazebo model + private: physics::ModelPtr model_; + + /// \brief A pointer to the Gazebo world + private: physics::WorldPtr world_; + + /// \brief A pointer to the ROS node. A node will be instantiated if it does not exist. + private: ros::NodeHandle* rosnode_; + private: ros::Publisher pub_; + + /// \brief ROS WrenchStamped message + private: geometry_msgs::WrenchStamped wrench_msg_; + + /// \brief store bodyname + private: std::string joint_name_; + + /// \brief ROS WrenchStamped topic name + private: std::string topic_name_; + + /// \brief ROS frame transform name to use in the image message header. + /// FIXME: extract link name directly? + private: std::string frame_name_; + + /// \brief for setting ROS name space + private: std::string robot_namespace_; + + /// \brief A mutex to lock access to fields that are used in message callbacks + private: boost::mutex lock_; + + /// \brief save last_time + private: common::Time last_time_; + + // rate control + private: double update_rate_; + + /// \brief: keep track of number of connections + private: int ft_connect_count_; + private: void FTConnect(); + private: void FTDisconnect(); + + // Custom Callback Queue + private: ros::CallbackQueue queue_; + private: void QueueThread(); + private: boost::thread callback_queue_thread_; + + // Pointer to the update event connection + private: event::ConnectionPtr update_connection_; + +}; + +/** \} */ +/// @} + + +} + +#endif diff --git a/gazebo_plugins/src/gazebo_ros_ft_sensor.cpp b/gazebo_plugins/src/gazebo_ros_ft_sensor.cpp new file mode 100644 index 000000000..f7802c8f8 --- /dev/null +++ b/gazebo_plugins/src/gazebo_ros_ft_sensor.cpp @@ -0,0 +1,210 @@ +/* + * Copyright (C) 2014 + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ +/* + * Desc: Force Torque Sensor Plugin + * Author: Francisco Suarez-Ruiz + * Date: 5 June 2014 + */ + +#include +#include + +namespace gazebo +{ +GZ_REGISTER_MODEL_PLUGIN(GazeboRosFT); + +//////////////////////////////////////////////////////////////////////////////// +// Constructor +GazeboRosFT::GazeboRosFT() +{ + this->ft_connect_count_ = 0; +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor +GazeboRosFT::~GazeboRosFT() +{ + event::Events::DisconnectWorldUpdateBegin(this->update_connection_); + // Custom Callback Queue + this->queue_.clear(); + this->queue_.disable(); + this->rosnode_->shutdown(); + this->callback_queue_thread_.join(); + delete this->rosnode_; +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void GazeboRosFT::Load( physics::ModelPtr _model, sdf::ElementPtr _sdf ) +{ + // Save pointers + this->model_ = _model; + this->world_ = this->model_->GetWorld(); + + // load parameters + this->robot_namespace_ = ""; + if (_sdf->HasElement("robotNamespace")) + this->robot_namespace_ = _sdf->GetElement("robotNamespace")->Get() + "/"; + + if (!_sdf->HasElement("jointName")) + { + ROS_FATAL("ft_sensor plugin missing , cannot proceed"); + return; + } + else + this->joint_name_ = _sdf->GetElement("jointName")->Get(); + + this->joint_ = this->model_->GetJoint(this->joint_name_); + if (!this->joint_) + { + ROS_FATAL("gazebo_ros_ft_sensor plugin error: jointName: %s does not exist\n",this->joint_name_.c_str()); + return; + } + + this->parent_link_ = this->joint_->GetParent(); + this->child_link_ = this->joint_->GetChild(); + this->frame_name_ = this->child_link_->GetName(); + + ROS_INFO("ft_sensor plugin reporting wrench values to the frame [%s]", this->frame_name_.c_str()); + + if (!_sdf->HasElement("topicName")) + { + ROS_FATAL("ft_sensor plugin missing , cannot proceed"); + return; + } + else + this->topic_name_ = _sdf->GetElement("topicName")->Get(); + + if (!_sdf->HasElement("updateRate")) + { + ROS_DEBUG("ft_sensor plugin missing , defaults to 0.0" + " (as fast as possible)"); + this->update_rate_ = 0; + } + else + this->update_rate_ = _sdf->GetElement("updateRate")->Get(); + + // Make sure the ROS node for Gazebo has already been initialized + if (!ros::isInitialized()) + { + ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; + } + + this->rosnode_ = new ros::NodeHandle(this->robot_namespace_); + + // resolve tf prefix + std::string prefix; + this->rosnode_->getParam(std::string("tf_prefix"), prefix); + this->frame_name_ = tf::resolve(prefix, this->frame_name_); + + // Custom Callback Queue + ros::AdvertiseOptions ao = ros::AdvertiseOptions::create( + this->topic_name_,1, + boost::bind( &GazeboRosFT::FTConnect,this), + boost::bind( &GazeboRosFT::FTDisconnect,this), ros::VoidPtr(), &this->queue_); + this->pub_ = this->rosnode_->advertise(ao); + + // Custom Callback Queue + this->callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosFT::QueueThread,this ) ); + + // New Mechanism for Updating every World Cycle + // Listen to the update event. This event is broadcast every + // simulation iteration. + this->update_connection_ = event::Events::ConnectWorldUpdateBegin( + boost::bind(&GazeboRosFT::UpdateChild, this)); +} + +//////////////////////////////////////////////////////////////////////////////// +// Someone subscribes to me +void GazeboRosFT::FTConnect() +{ + this->ft_connect_count_++; +} + +//////////////////////////////////////////////////////////////////////////////// +// Someone subscribes to me +void GazeboRosFT::FTDisconnect() +{ + this->ft_connect_count_--; +} + +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void GazeboRosFT::UpdateChild() +{ + common::Time cur_time = this->world_->GetSimTime(); + + // rate control + if (this->update_rate_ > 0 && + (cur_time-this->last_time_).Double() < (1.0/this->update_rate_)) + return; + + if (this->ft_connect_count_ == 0) + return; + + physics::JointWrench wrench; + math::Vector3 torque; + math::Vector3 force; + + // FIXME: Should include options for diferent frames and measure directions + // E.g: https://bitbucket.org/osrf/gazebo/raw/default/gazebo/sensors/ForceTorqueSensor.hh + // Get force torque at the joint + // The wrench is reported in the CHILD + // The is child_to_parent + wrench = this->joint_->GetForceTorque(0); + force = wrench.body2Force; + torque = wrench.body2Torque; + + + this->lock_.lock(); + // copy data into wrench message + this->wrench_msg_.header.frame_id = this->frame_name_; + this->wrench_msg_.header.stamp.sec = (this->world_->GetSimTime()).sec; + this->wrench_msg_.header.stamp.nsec = (this->world_->GetSimTime()).nsec; + + this->wrench_msg_.wrench.force.x = force.x; + this->wrench_msg_.wrench.force.y = force.y; + this->wrench_msg_.wrench.force.z = force.z; + this->wrench_msg_.wrench.torque.x = torque.x; + this->wrench_msg_.wrench.torque.y = torque.y; + this->wrench_msg_.wrench.torque.z = torque.z; + + this->pub_.publish(this->wrench_msg_); + this->lock_.unlock(); + + // save last time stamp + this->last_time_ = cur_time; +} + +// Custom Callback Queue +//////////////////////////////////////////////////////////////////////////////// +// custom callback queue thread +void GazeboRosFT::QueueThread() +{ + static const double timeout = 0.01; + + while (this->rosnode_->ok()) + { + this->queue_.callAvailable(ros::WallDuration(timeout)); + } +} + +} From 18189587b39dc5a62a7bda4abd199b1a82e1334d Mon Sep 17 00:00:00 2001 From: fsuarez6 Date: Thu, 5 Jun 2014 19:31:21 +0200 Subject: [PATCH 097/153] Added description and example usage in the comments --- .../gazebo_plugins/gazebo_ros_ft_sensor.h | 35 ++++++++++++++++++- gazebo_plugins/src/gazebo_ros_ft_sensor.cpp | 4 ++- 2 files changed, 37 insertions(+), 2 deletions(-) diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_ft_sensor.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_ft_sensor.h index f86d05a74..2c4036612 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_ft_sensor.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_ft_sensor.h @@ -1,5 +1,7 @@ /* - * Copyright (C) 2014 + * Gazebo - Outdoor Multi-Robot Simulator + * Copyright (C) 2003 + * Nate Koenig & Andrew Howard * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -41,6 +43,37 @@ namespace gazebo { +/// @addtogroup gazebo_dynamic_plugins Gazebo ROS Dynamic Plugins +/// @{ +/** \defgroup GazeboRosFTSensor Plugin XML Reference and Example + + \brief Ros Gazebo Ros Force/Torque Sensor Plugin. + + This is a model plugin which broadcasts geometry_msgs/WrenchStamped messages + with measured force and torque on a specified joint. + + The wrench is reported in the joint CHILD link frame and the measure direction + is child-to-parent link. + + Example Usage: + + \verbatim + + + true + + + + + 100.0 + ft_sensor_topic + JOINT_NAME + + + \endverbatim +\{ +*/ + /// \brief GazeboRosFT controller /// This is a controller that simulates a 6 dof force sensor diff --git a/gazebo_plugins/src/gazebo_ros_ft_sensor.cpp b/gazebo_plugins/src/gazebo_ros_ft_sensor.cpp index f7802c8f8..99ab56135 100644 --- a/gazebo_plugins/src/gazebo_ros_ft_sensor.cpp +++ b/gazebo_plugins/src/gazebo_ros_ft_sensor.cpp @@ -1,5 +1,7 @@ /* - * Copyright (C) 2014 + * Gazebo - Outdoor Multi-Robot Simulator + * Copyright (C) 2003 + * Nate Koenig & Andrew Howard * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by From 6f302ede1648c89d378307a6753f9007d697bad9 Mon Sep 17 00:00:00 2001 From: fsuarez6 Date: Mon, 9 Jun 2014 12:58:52 +0200 Subject: [PATCH 098/153] gazebo_plugin: Added updateRate parameter to the gazebo_ros_imu plugin --- .../include/gazebo_plugins/gazebo_ros_imu.h | 3 +++ gazebo_plugins/src/gazebo_ros_imu.cpp | 18 ++++++++++++++++-- 2 files changed, 19 insertions(+), 2 deletions(-) diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_imu.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_imu.h index 4317fbaf0..6f3191713 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_imu.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_imu.h @@ -84,6 +84,9 @@ namespace gazebo private: math::Vector3 last_veul_; private: math::Vector3 apos_; private: math::Vector3 aeul_; + + // rate control + private: double update_rate_; /// \brief: keep initial pose to offset orientation in imu message private: math::Pose initial_pose_; diff --git a/gazebo_plugins/src/gazebo_ros_imu.cpp b/gazebo_plugins/src/gazebo_ros_imu.cpp index bc3e8ebf1..949707377 100644 --- a/gazebo_plugins/src/gazebo_ros_imu.cpp +++ b/gazebo_plugins/src/gazebo_ros_imu.cpp @@ -114,6 +114,15 @@ void GazeboRosIMU::LoadThread() } else this->offset_.rot = this->sdf->Get("rpyOffset"); + + if (!this->sdf->HasElement("updateRate")) + { + ROS_DEBUG("imu plugin missing , defaults to 0.0" + " (as fast as possible)"); + this->update_rate_ = 0; + } + else + this->update_rate_ = this->sdf->GetElement("updateRate")->Get(); // Make sure the ROS node for Gazebo has already been initialized @@ -187,6 +196,13 @@ bool GazeboRosIMU::ServiceCallback(std_srvs::Empty::Request &req, // Update the controller void GazeboRosIMU::UpdateChild() { + common::Time cur_time = this->world_->GetSimTime(); + + // rate control + if (this->update_rate_ > 0 && + (cur_time-this->last_time_).Double() < (1.0/this->update_rate_)) + return; + if ((this->pub_.getNumSubscribers() > 0 && this->topic_name_ != "")) { math::Pose pose; @@ -203,8 +219,6 @@ void GazeboRosIMU::UpdateChild() rot = this->offset_.rot*rot; rot.Normalize(); - common::Time cur_time = this->world_->GetSimTime(); - // get Rates math::Vector3 vpos = this->link->GetWorldLinearVel(); math::Vector3 veul = this->link->GetWorldAngularVel(); From b5c9f21044ce31e73dcd19e0b5f152001764dc16 Mon Sep 17 00:00:00 2001 From: Adolfo Rodriguez Tsouroukdissian Date: Tue, 10 Jun 2014 09:15:55 +0200 Subject: [PATCH 099/153] Revert 4776545, as it belongs in indigo-devel. --- gazebo_ros_control/src/default_robot_hw_sim.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gazebo_ros_control/src/default_robot_hw_sim.cpp b/gazebo_ros_control/src/default_robot_hw_sim.cpp index 1f3e20a01..11882922a 100644 --- a/gazebo_ros_control/src/default_robot_hw_sim.cpp +++ b/gazebo_ros_control/src/default_robot_hw_sim.cpp @@ -217,7 +217,7 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim // joint->SetVelocity() to control the joint. const ros::NodeHandle nh(model_nh, robot_namespace + "/gazebo_ros_control/pid_gains/" + joint_names_[j]); - if (pid_controllers_[j].init(nh,true)) + if (pid_controllers_[j].init(nh)) { switch (joint_control_methods_[j]) { From 36f5dfd07fba58c56eec50fc3a2ff533d629a65f Mon Sep 17 00:00:00 2001 From: Adolfo Rodriguez Tsouroukdissian Date: Tue, 10 Jun 2014 09:18:21 +0200 Subject: [PATCH 100/153] gazebo_ros_control: default_robot_hw_sim: Suppressing pid error message Depends on ros-controls/control_toolbox#21 --- gazebo_ros_control/src/default_robot_hw_sim.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gazebo_ros_control/src/default_robot_hw_sim.cpp b/gazebo_ros_control/src/default_robot_hw_sim.cpp index 548066b94..990cc30ad 100644 --- a/gazebo_ros_control/src/default_robot_hw_sim.cpp +++ b/gazebo_ros_control/src/default_robot_hw_sim.cpp @@ -229,7 +229,7 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim // joint->SetVelocity() to control the joint. const ros::NodeHandle nh(model_nh, robot_namespace + "/gazebo_ros_control/pid_gains/" + joint_names_[j]); - if (pid_controllers_[j].init(nh)) + if (pid_controllers_[j].init(nh, true)) { switch (joint_control_methods_[j]) { From 3feffdd28ce83e1e9a711218de3278c0753e40e7 Mon Sep 17 00:00:00 2001 From: fsuarez6 Date: Tue, 10 Jun 2014 12:15:25 +0200 Subject: [PATCH 101/153] Included changes suggested by @jonbinney --- gazebo_plugins/src/gazebo_ros_imu.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gazebo_plugins/src/gazebo_ros_imu.cpp b/gazebo_plugins/src/gazebo_ros_imu.cpp index 949707377..233a44fb9 100644 --- a/gazebo_plugins/src/gazebo_ros_imu.cpp +++ b/gazebo_plugins/src/gazebo_ros_imu.cpp @@ -86,7 +86,7 @@ void GazeboRosIMU::LoadThread() if (!this->sdf->HasElement("gaussianNoise")) { ROS_INFO("imu plugin missing , defaults to 0.0"); - this->gaussian_noise_ = 0; + this->gaussian_noise_ = 0.0; } else this->gaussian_noise_ = this->sdf->Get("gaussianNoise"); @@ -119,7 +119,7 @@ void GazeboRosIMU::LoadThread() { ROS_DEBUG("imu plugin missing , defaults to 0.0" " (as fast as possible)"); - this->update_rate_ = 0; + this->update_rate_ = 0.0; } else this->update_rate_ = this->sdf->GetElement("updateRate")->Get(); @@ -200,7 +200,7 @@ void GazeboRosIMU::UpdateChild() // rate control if (this->update_rate_ > 0 && - (cur_time-this->last_time_).Double() < (1.0/this->update_rate_)) + (cur_time - this->last_time_).Double() < (1.0 / this->update_rate_)) return; if ((this->pub_.getNumSubscribers() > 0 && this->topic_name_ != "")) From 57d96ca5ab8e64f042b894957d79e1ab2cb70502 Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Wed, 11 Jun 2014 00:08:14 +0900 Subject: [PATCH 102/153] Updated package.xml --- gazebo_plugins/package.xml | 1 - gazebo_ros_control/package.xml | 1 - gazebo_ros_pkgs/package.xml | 1 - 3 files changed, 3 deletions(-) diff --git a/gazebo_plugins/package.xml b/gazebo_plugins/package.xml index 75ca23210..8a757dac1 100644 --- a/gazebo_plugins/package.xml +++ b/gazebo_plugins/package.xml @@ -6,7 +6,6 @@ John Hsu - Dave Coleman BSD diff --git a/gazebo_ros_control/package.xml b/gazebo_ros_control/package.xml index 955ae8a19..fdbfde423 100644 --- a/gazebo_ros_control/package.xml +++ b/gazebo_ros_control/package.xml @@ -4,7 +4,6 @@ gazebo_ros_control Jonathan Bohren - Dave Coleman BSD diff --git a/gazebo_ros_pkgs/package.xml b/gazebo_ros_pkgs/package.xml index c77e3025e..3c353dc93 100644 --- a/gazebo_ros_pkgs/package.xml +++ b/gazebo_ros_pkgs/package.xml @@ -5,7 +5,6 @@ Interface for using ROS with the Gazebo simulator. John Hsu - Dave Coleman BSD,LGPL,Apache 2.0 From 1404d439a5b9c83cec41ee6b39eeccd8ca26da08 Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Wed, 11 Jun 2014 00:08:14 +0900 Subject: [PATCH 103/153] Updated package.xml --- gazebo_plugins/package.xml | 1 - gazebo_ros_control/package.xml | 1 - gazebo_ros_pkgs/package.xml | 1 - 3 files changed, 3 deletions(-) diff --git a/gazebo_plugins/package.xml b/gazebo_plugins/package.xml index e326392b9..db85d416a 100644 --- a/gazebo_plugins/package.xml +++ b/gazebo_plugins/package.xml @@ -6,7 +6,6 @@ John Hsu - Dave Coleman BSD diff --git a/gazebo_ros_control/package.xml b/gazebo_ros_control/package.xml index d1576dfae..109a45457 100644 --- a/gazebo_ros_control/package.xml +++ b/gazebo_ros_control/package.xml @@ -4,7 +4,6 @@ gazebo_ros_control Jonathan Bohren - Dave Coleman BSD diff --git a/gazebo_ros_pkgs/package.xml b/gazebo_ros_pkgs/package.xml index 82e4d1d76..d343c42ed 100644 --- a/gazebo_ros_pkgs/package.xml +++ b/gazebo_ros_pkgs/package.xml @@ -5,7 +5,6 @@ Interface for using ROS with the Gazebo simulator. John Hsu - Dave Coleman BSD,LGPL,Apache 2.0 From 8644da1b026b88f65924bd46d52974464c57546d Mon Sep 17 00:00:00 2001 From: Markus Bader Date: Wed, 11 Jun 2014 08:48:30 +0200 Subject: [PATCH 104/153] format patch of hsu applied --- gazebo_plugins/src/gazebo_ros_diff_drive.cpp | 789 ++++++++++--------- 1 file changed, 424 insertions(+), 365 deletions(-) diff --git a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp index a436379a5..cac5420c8 100644 --- a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp +++ b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp @@ -40,10 +40,10 @@ /* - * + * * The support of acceleration limit was added by * \author George Todoran - * \author Markus Bader + * \author Markus Bader * \date 22th of May 2014 */ @@ -64,413 +64,472 @@ #include #include -namespace gazebo { +namespace gazebo +{ -enum { - RIGHT, - LEFT, +enum +{ + RIGHT, + LEFT, }; GazeboRosDiffDrive::GazeboRosDiffDrive() {} // Destructor -GazeboRosDiffDrive::~GazeboRosDiffDrive() { -} +GazeboRosDiffDrive::~GazeboRosDiffDrive() {} // Load the controller -void GazeboRosDiffDrive::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) { - - this->parent = _parent; - this->world = _parent->GetWorld(); - - - this->robot_namespace_ = this->parent->GetName (); - if ( !_sdf->HasElement ( "robotNamespace" ) ) { - ROS_INFO ( "GazeboRosDiffDrive Plugin missing , defaults to \"%s\"", - this->robot_namespace_.c_str() ); - } else { - this->robot_namespace_ = _sdf->GetElement ( "robotNamespace" )->Get(); - if ( this->robot_namespace_.empty() ) this->robot_namespace_ = this->parent->GetName (); - } - if ( !robot_namespace_.empty() ) this->robot_namespace_ += "/"; - rosnode_ = boost::shared_ptr ( new ros::NodeHandle ( this->robot_namespace_ ) ); - - tf_prefix_ = tf::getPrefixParam(*rosnode_); - if(tf_prefix_.empty()) { - tf_prefix_ = robot_namespace_; - boost::trim_right_if(tf_prefix_,boost::is_any_of("/")); - } - ROS_INFO("GazeboRosDiffDrive Plugin (ns = %s) , set to \"%s\"", - this->robot_namespace_.c_str(), this->tf_prefix_.c_str()); - - - this->left_joint_name_ = "left_joint"; - if (!_sdf->HasElement("leftJoint")) { - ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to \"%s\"", - this->robot_namespace_.c_str(), this->left_joint_name_.c_str()); - } else { - this->left_joint_name_ = _sdf->GetElement("leftJoint")->Get(); - } - - this->right_joint_name_ = "right_joint"; - if (!_sdf->HasElement("rightJoint")) { - ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to \"%s\"", - this->robot_namespace_.c_str(), this->right_joint_name_.c_str()); - } else { - this->right_joint_name_ = _sdf->GetElement("rightJoint")->Get(); - } - - this->wheel_separation_ = 0.34; - if (!_sdf->HasElement("wheelSeparation")) { - ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to %f", - this->robot_namespace_.c_str(), this->wheel_separation_); - } else { - this->wheel_separation_ = - _sdf->GetElement("wheelSeparation")->Get(); - } - - this->wheel_diameter_ = 0.15; - if (!_sdf->HasElement("wheelDiameter")) { - ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to %f", - this->robot_namespace_.c_str(), this->wheel_diameter_); - } else { - this->wheel_diameter_ = _sdf->GetElement("wheelDiameter")->Get(); - } - - this->torque = 5.0; - if (!_sdf->HasElement("torque")) { - ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to %f", - this->robot_namespace_.c_str(), this->torque); - } else { - this->torque = _sdf->GetElement("torque")->Get(); - } - - this->command_topic_ = "cmd_vel"; - if (!_sdf->HasElement("commandTopic")) { - ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to \"%s\"", - this->robot_namespace_.c_str(), this->command_topic_.c_str()); - } else { - this->command_topic_ = _sdf->GetElement("commandTopic")->Get(); - } - - this->odometry_topic_ = "odom"; - if (!_sdf->HasElement("odometryTopic")) { - ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to \"%s\"", - this->robot_namespace_.c_str(), this->odometry_topic_.c_str()); - } else { - this->odometry_topic_ = _sdf->GetElement("odometryTopic")->Get(); - } - - this->odometry_frame_ = "odom"; - if (!_sdf->HasElement("odometryFrame")) { - ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to \"%s\"", - this->robot_namespace_.c_str(), this->odometry_frame_.c_str()); - } else { - this->odometry_frame_ = _sdf->GetElement("odometryFrame")->Get(); - } - - this->robot_base_frame_ = "base_footprint"; - if (!_sdf->HasElement("robotBaseFrame")) { - ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to \"%s\"", - this->robot_namespace_.c_str(), this->robot_base_frame_.c_str()); - } else { - this->robot_base_frame_ = _sdf->GetElement("robotBaseFrame")->Get(); - } - - this->update_rate_ = 100.0; - if (!_sdf->HasElement("updateRate")) { - ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to %f", - this->robot_namespace_.c_str(), this->update_rate_); - } else { - this->update_rate_ = _sdf->GetElement("updateRate")->Get(); - } - - - this->publishWheelTF_ = false; - if (_sdf->HasElement("publishWheelTF")) { - - std::string value = _sdf->GetElement("publishWheelTF")->Get(); - if(boost::iequals(value, std::string("true"))) { - this->publishWheelTF_ = true; - } else if(boost::iequals(value, std::string("false"))) { - this->publishWheelTF_ = false; - } else { - ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) was set to '%s', it must be set to 'true' or 'false'!", - this->robot_namespace_.c_str(), value.c_str()); - } - } - ROS_INFO("GazeboRosDiffDrive Plugin (ns = %s) , set to %s", - this->robot_namespace_.c_str(), (this->publishWheelTF_?"ture":"false")); - - this->publishWheelJointState_ = false; - if (_sdf->HasElement("publishWheelJointState")) { - std::string value = _sdf->GetElement("publishWheelJointState")->Get(); - if(boost::iequals(value, std::string("true"))) { - this->publishWheelJointState_ = true; - } else if(boost::iequals(value, std::string("false"))) { - this->publishWheelJointState_ = false; - } else { - ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) was set to '%s', it must be set to 'true' or 'false'!", - this->robot_namespace_.c_str(), value.c_str()); - } - } - this->max_accel=0; - if (!_sdf->HasElement("max_accel")) { - ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to %f", - this->robot_namespace_.c_str(), this->max_accel); - } else { - this->max_accel = _sdf->GetElement("max_accel")->Get(); +void GazeboRosDiffDrive::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) +{ + this->parent = _parent; + this->world = _parent->GetWorld(); + + this->robot_namespace_ = this->parent->GetName (); + if (!_sdf->HasElement("robotNamespace")) + { + ROS_INFO("GazeboRosDiffDrive Plugin missing , defaults to \"%s\"", + this->robot_namespace_.c_str()); + } + else + { + this->robot_namespace_ = _sdf->GetElement("robotNamespace")->Get(); + if (this->robot_namespace_.empty()) + this->robot_namespace_ = this->parent->GetName(); + } + if (!robot_namespace_.empty()) + this->robot_namespace_ += "/"; + rosnode_ = boost::shared_ptr(new ros::NodeHandle(this->robot_namespace_)); + + tf_prefix_ = tf::getPrefixParam(*rosnode_); + if(tf_prefix_.empty()) + { + tf_prefix_ = robot_namespace_; + boost::trim_right_if(tf_prefix_,boost::is_any_of("/")); + } + ROS_INFO("GazeboRosDiffDrive Plugin (ns = %s) , set to \"%s\"", + this->robot_namespace_.c_str(), this->tf_prefix_.c_str()); + + this->left_joint_name_ = "left_joint"; + if (!_sdf->HasElement("leftJoint")) + { + ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to \"%s\"", + this->robot_namespace_.c_str(), this->left_joint_name_.c_str()); + } + else + { + this->left_joint_name_ = _sdf->GetElement("leftJoint")->Get(); + } + + this->right_joint_name_ = "right_joint"; + if (!_sdf->HasElement("rightJoint")) + { + ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to \"%s\"", + this->robot_namespace_.c_str(), this->right_joint_name_.c_str()); + } + else + { + this->right_joint_name_ = _sdf->GetElement("rightJoint")->Get(); + } + + this->wheel_separation_ = 0.34; + if (!_sdf->HasElement("wheelSeparation")) + { + ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to %f", + this->robot_namespace_.c_str(), this->wheel_separation_); + } + else + { + this->wheel_separation_ = + _sdf->GetElement("wheelSeparation")->Get(); + } + + this->wheel_diameter_ = 0.15; + if (!_sdf->HasElement("wheelDiameter")) + { + ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to %f", + this->robot_namespace_.c_str(), this->wheel_diameter_); + } + else + { + this->wheel_diameter_ = _sdf->GetElement("wheelDiameter")->Get(); + } + + this->torque = 5.0; + if (!_sdf->HasElement("torque")) + { + ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to %f", + this->robot_namespace_.c_str(), this->torque); + } + else + { + this->torque = _sdf->GetElement("torque")->Get(); + } + + this->command_topic_ = "cmd_vel"; + if (!_sdf->HasElement("commandTopic")) + { + ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to \"%s\"", + this->robot_namespace_.c_str(), this->command_topic_.c_str()); + } + else + { + this->command_topic_ = _sdf->GetElement("commandTopic")->Get(); + } + + this->odometry_topic_ = "odom"; + if (!_sdf->HasElement("odometryTopic")) + { + ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to \"%s\"", + this->robot_namespace_.c_str(), this->odometry_topic_.c_str()); + } + else + { + this->odometry_topic_ = _sdf->GetElement("odometryTopic")->Get(); + } + + this->odometry_frame_ = "odom"; + if (!_sdf->HasElement("odometryFrame")) + { + ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to \"%s\"", + this->robot_namespace_.c_str(), this->odometry_frame_.c_str()); + } + else + { + this->odometry_frame_ = _sdf->GetElement("odometryFrame")->Get(); + } + + this->robot_base_frame_ = "base_footprint"; + if (!_sdf->HasElement("robotBaseFrame")) + { + ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to \"%s\"", + this->robot_namespace_.c_str(), this->robot_base_frame_.c_str()); + } + else + { + this->robot_base_frame_ = _sdf->GetElement("robotBaseFrame")->Get(); + } + + this->update_rate_ = 100.0; + if (!_sdf->HasElement("updateRate")) + { + ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to %f", + this->robot_namespace_.c_str(), this->update_rate_); + } + else + { + this->update_rate_ = _sdf->GetElement("updateRate")->Get(); + } + + + this->publishWheelTF_ = false; + if (_sdf->HasElement("publishWheelTF")) + { + std::string value = _sdf->GetElement("publishWheelTF")->Get(); + if(boost::iequals(value, std::string("true"))) + { + this->publishWheelTF_ = true; } - - ROS_INFO("GazeboRosDiffDrive Plugin (ns = %s) , set to %s", - this->robot_namespace_.c_str(), (this->publishWheelJointState_?"ture":"false")); - - // Initialize update rate stuff - if (this->update_rate_ > 0.0) { - this->update_period_ = 1.0 / this->update_rate_; - } else { - this->update_period_ = 0.0; + else if(boost::iequals(value, std::string("false"))) + { + this->publishWheelTF_ = false; } - last_update_time_ = this->world->GetSimTime(); - - // Initialize velocity stuff - wheel_speed_[RIGHT] = 0; - wheel_speed_[LEFT] = 0; - - x_ = 0; - rot_ = 0; - alive_ = true; - - joints_.resize(2); - joints_[LEFT] = this->parent->GetJoint(left_joint_name_); - joints_[RIGHT] = this->parent->GetJoint(right_joint_name_); - - if (!joints_[LEFT]) { - char error[200]; - snprintf(error, 200, - "GazeboRosDiffDrive Plugin (ns = %s) couldn't get left hinge joint named \"%s\"", - this->robot_namespace_.c_str(), this->left_joint_name_.c_str()); - gzthrow(error); + else + { + ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) was set to '%s', it must be set to 'true' or 'false'!", + this->robot_namespace_.c_str(), value.c_str()); } - if (!joints_[RIGHT]) { - char error[200]; - snprintf(error, 200, - "GazeboRosDiffDrive Plugin (ns = %s) couldn't get right hinge joint named \"%s\"", - this->robot_namespace_.c_str(), this->right_joint_name_.c_str()); - gzthrow(error); + } + ROS_INFO("GazeboRosDiffDrive Plugin (ns = %s) , set to %s", + this->robot_namespace_.c_str(), (this->publishWheelTF_?"ture":"false")); + + this->publishWheelJointState_ = false; + if (_sdf->HasElement("publishWheelJointState")) + { + std::string value = _sdf->GetElement("publishWheelJointState")->Get(); + if(boost::iequals(value, std::string("true"))) + { + this->publishWheelJointState_ = true; } - - joints_[LEFT]->SetMaxForce(0, torque); - joints_[RIGHT]->SetMaxForce(0, torque); - - // Make sure the ROS node for Gazebo has already been initialized - if (!ros::isInitialized()) + else if(boost::iequals(value, std::string("false"))) { - ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. " - << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); - return; + this->publishWheelJointState_ = false; } - - rosnode_ = boost::shared_ptr ( new ros::NodeHandle ( this->robot_namespace_ ) ); - ROS_INFO("Starting GazeboRosDiffDrive Plugin (ns = %s)!", this->robot_namespace_.c_str()); - - if(this->publishWheelJointState_) { - joint_state_publisher_ = rosnode_->advertise ( "joint_states",1000 ); + else + { + ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) was set to '%s', it must be set to 'true' or 'false'!", + this->robot_namespace_.c_str(), value.c_str()); } - - transform_broadcaster_ = boost::shared_ptr ( new tf::TransformBroadcaster() ); - - // ROS: Subscribe to the velocity command topic (usually "cmd_vel") - ros::SubscribeOptions so = - ros::SubscribeOptions::create(command_topic_, 1, - boost::bind(&GazeboRosDiffDrive::cmdVelCallback, this, _1), - ros::VoidPtr(), &queue_); - - cmd_vel_subscriber_ = rosnode_->subscribe(so); - - odometry_publisher_ = rosnode_->advertise(odometry_topic_, 1); - - // start custom queue for diff drive - this->callback_queue_thread_ = - boost::thread(boost::bind(&GazeboRosDiffDrive::QueueThread, this)); - - // listen to the update event (broadcast every simulation iteration) - this->update_connection_ = - event::Events::ConnectWorldUpdateBegin( - boost::bind(&GazeboRosDiffDrive::UpdateChild, this)); + } + this->max_accel = 0; + if (!_sdf->HasElement("max_accel")) + { + ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to %f", + this->robot_namespace_.c_str(), this->max_accel); + } + else + { + this->max_accel = _sdf->GetElement("max_accel")->Get(); + } + + ROS_INFO("GazeboRosDiffDrive Plugin (ns = %s) , set to %s", + this->robot_namespace_.c_str(), (this->publishWheelJointState_?"ture":"false")); + + // Initialize update rate stuff + if (this->update_rate_ > 0.0) + { + this->update_period_ = 1.0 / this->update_rate_; + } + else + { + this->update_period_ = 0.0; + } + last_update_time_ = this->world->GetSimTime(); + + // Initialize velocity stuff + wheel_speed_[RIGHT] = 0; + wheel_speed_[LEFT] = 0; + + x_ = 0; + rot_ = 0; + alive_ = true; + + joints_.resize(2); + joints_[LEFT] = this->parent->GetJoint(left_joint_name_); + joints_[RIGHT] = this->parent->GetJoint(right_joint_name_); + + if (!joints_[LEFT]) + { + char error[200]; + snprintf(error, 200, + "GazeboRosDiffDrive Plugin (ns = %s) couldn't get left hinge joint named \"%s\"", + this->robot_namespace_.c_str(), this->left_joint_name_.c_str()); + gzthrow(error); + } + if (!joints_[RIGHT]) + { + char error[200]; + snprintf(error, 200, + "GazeboRosDiffDrive Plugin (ns = %s) couldn't get right hinge joint named \"%s\"", + this->robot_namespace_.c_str(), this->right_joint_name_.c_str()); + gzthrow(error); + } + + joints_[LEFT]->SetMaxForce(0, torque); + joints_[RIGHT]->SetMaxForce(0, torque); + + // Make sure the ROS node for Gazebo has already been initialized + if (!ros::isInitialized()) + { + ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; + } + + rosnode_ = boost::shared_ptr(new ros::NodeHandle(this->robot_namespace_)); + ROS_INFO("Starting GazeboRosDiffDrive Plugin (ns = %s)!", this->robot_namespace_.c_str()); + + if(this->publishWheelJointState_) + { + joint_state_publisher_ = rosnode_->advertise("joint_states", 1000); + } + + transform_broadcaster_ = boost::shared_ptr(new tf::TransformBroadcaster()); + + // ROS: Subscribe to the velocity command topic (usually "cmd_vel") + ros::SubscribeOptions so = + ros::SubscribeOptions::create(command_topic_, 1, + boost::bind(&GazeboRosDiffDrive::cmdVelCallback, this, _1), + ros::VoidPtr(), &queue_); + + cmd_vel_subscriber_ = rosnode_->subscribe(so); + + odometry_publisher_ = rosnode_->advertise(odometry_topic_, 1); + + // start custom queue for diff drive + this->callback_queue_thread_ = + boost::thread(boost::bind(&GazeboRosDiffDrive::QueueThread, this)); + + // listen to the update event (broadcast every simulation iteration) + this->update_connection_ = + event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosDiffDrive::UpdateChild, this)); } -void GazeboRosDiffDrive::publishWheelJointState() { - ros::Time current_time = ros::Time::now(); - - joint_state_.header.stamp = current_time; - joint_state_.name.resize ( joints_.size() ); - joint_state_.position.resize ( joints_.size() ); - - for ( int i = 0; i < 2; i++ ) { - physics::JointPtr joint = joints_[i]; - math::Angle angle = joint->GetAngle ( 0 ); - joint_state_.name[i] = joint->GetName(); - joint_state_.position[i] = angle.Radian () ; - } - joint_state_publisher_.publish ( joint_state_ ); +void GazeboRosDiffDrive::publishWheelJointState() +{ + ros::Time current_time = ros::Time::now(); + + joint_state_.header.stamp = current_time; + joint_state_.name.resize ( joints_.size() ); + joint_state_.position.resize ( joints_.size() ); + + for ( int i = 0; i < 2; i++ ) + { + physics::JointPtr joint = joints_[i]; + math::Angle angle = joint->GetAngle ( 0 ); + joint_state_.name[i] = joint->GetName(); + joint_state_.position[i] = angle.Radian () ; + } + joint_state_publisher_.publish ( joint_state_ ); } -void GazeboRosDiffDrive::publishWheelTF() { - ros::Time current_time = ros::Time::now(); - for(int i = 0; i < 2; i++) { - std::string wheel_frame = - tf::resolve(tf_prefix_, joints_[i]->GetChild()->GetName ()); - std::string wheel_parent_frame = - tf::resolve(tf_prefix_, joints_[i]->GetParent()->GetName ()); +void GazeboRosDiffDrive::publishWheelTF() +{ + ros::Time current_time = ros::Time::now(); + for(int i = 0; i < 2; i++) + { + std::string wheel_frame = + tf::resolve(tf_prefix_, joints_[i]->GetChild()->GetName ()); + std::string wheel_parent_frame = + tf::resolve(tf_prefix_, joints_[i]->GetParent()->GetName ()); - math::Pose poseWheel = joints_[i]->GetChild()->GetRelativePose(); + math::Pose poseWheel = joints_[i]->GetChild()->GetRelativePose(); - tf::Quaternion qt(poseWheel.rot.x, poseWheel.rot.y, poseWheel.rot.z, poseWheel.rot.w); - tf::Vector3 vt(poseWheel.pos.x, poseWheel.pos.y, poseWheel.pos.z); + tf::Quaternion qt(poseWheel.rot.x, poseWheel.rot.y, poseWheel.rot.z, poseWheel.rot.w); + tf::Vector3 vt(poseWheel.pos.x, poseWheel.pos.y, poseWheel.pos.z); - tf::Transform tfWheel(qt, vt); - transform_broadcaster_->sendTransform( - tf::StampedTransform(tfWheel, current_time, - wheel_parent_frame, wheel_frame)); - } + tf::Transform tfWheel(qt, vt); + transform_broadcaster_->sendTransform( + tf::StampedTransform(tfWheel, current_time, wheel_parent_frame, wheel_frame)); + } } // Update the controller -void GazeboRosDiffDrive::UpdateChild() { - common::Time current_time = this->world->GetSimTime(); - double seconds_since_last_update = - (current_time - last_update_time_).Double(); - if (seconds_since_last_update > update_period_) { - - publishOdometry(seconds_since_last_update); - if(publishWheelTF_) publishWheelTF(); - if(publishWheelJointState_) publishWheelJointState(); - - // Update robot in case new velocities have been requested - getWheelVelocities(); - - - double current_speed[2]; - - current_speed[LEFT] = joints_[LEFT]->GetVelocity(0) * (wheel_diameter_ / 2.0); - current_speed[RIGHT] = joints_[RIGHT]->GetVelocity(0) * (wheel_diameter_ / 2.0); - - if(max_accel==0 - || (fabs(wheel_speed_[LEFT] - current_speed[LEFT]) < 0.01) - || (fabs(wheel_speed_[RIGHT] - current_speed[RIGHT]) < 0.01)) { - //if max_accel==0, or target speed is reached - joints_[LEFT]->SetVelocity(0, wheel_speed_[LEFT]/ (wheel_diameter_ / 2.0)); - joints_[RIGHT]->SetVelocity(0, wheel_speed_[RIGHT]/ (wheel_diameter_ / 2.0)); - } - else { - - if(wheel_speed_[LEFT]>=current_speed[LEFT]) - wheel_speed_instr_[LEFT]+=fmin(wheel_speed_[LEFT]-current_speed[LEFT], max_accel * seconds_since_last_update); - else - wheel_speed_instr_[LEFT]+=fmax(wheel_speed_[LEFT]-current_speed[LEFT], -max_accel * seconds_since_last_update); - - if(wheel_speed_[RIGHT]>current_speed[RIGHT]) - wheel_speed_instr_[RIGHT]+=fmin(wheel_speed_[RIGHT]-current_speed[RIGHT], max_accel * seconds_since_last_update); - else - wheel_speed_instr_[RIGHT]+=fmax(wheel_speed_[RIGHT]-current_speed[RIGHT], -max_accel * seconds_since_last_update); - - // ROS_INFO("actual wheel speed = %lf, issued wheel speed= %lf", current_speed[LEFT], wheel_speed_[LEFT]); - // ROS_INFO("actual wheel speed = %lf, issued wheel speed= %lf", current_speed[RIGHT],wheel_speed_[RIGHT]); +void GazeboRosDiffDrive::UpdateChild() +{ + common::Time current_time = this->world->GetSimTime(); + double seconds_since_last_update = (current_time - last_update_time_).Double(); + if (seconds_since_last_update > update_period_) + { + publishOdometry(seconds_since_last_update); + if(publishWheelTF_) publishWheelTF(); + if(publishWheelJointState_) publishWheelJointState(); + + // Update robot in case new velocities have been requested + getWheelVelocities(); + + double current_speed[2]; + + current_speed[LEFT] = joints_[LEFT]->GetVelocity(0) * (wheel_diameter_ / 2.0); + current_speed[RIGHT] = joints_[RIGHT]->GetVelocity(0) * (wheel_diameter_ / 2.0); + + if(max_accel == 0 || + (fabs(wheel_speed_[LEFT] - current_speed[LEFT]) < 0.01) || + (fabs(wheel_speed_[RIGHT] - current_speed[RIGHT]) < 0.01)) + { + //if max_accel == 0, or target speed is reached + joints_[LEFT]->SetVelocity(0, wheel_speed_[LEFT]/ (wheel_diameter_ / 2.0)); + joints_[RIGHT]->SetVelocity(0, wheel_speed_[RIGHT]/ (wheel_diameter_ / 2.0)); + } + else + { + if(wheel_speed_[LEFT]>=current_speed[LEFT]) + wheel_speed_instr_[LEFT]+=fmin(wheel_speed_[LEFT]-current_speed[LEFT], max_accel * seconds_since_last_update); + else + wheel_speed_instr_[LEFT]+=fmax(wheel_speed_[LEFT]-current_speed[LEFT], -max_accel * seconds_since_last_update); - joints_[LEFT]->SetVelocity(0,wheel_speed_instr_[LEFT] / (wheel_diameter_ / 2.0)); - joints_[RIGHT]->SetVelocity(0,wheel_speed_instr_[RIGHT] / (wheel_diameter_ / 2.0)); - } + if(wheel_speed_[RIGHT]>current_speed[RIGHT]) + wheel_speed_instr_[RIGHT]+=fmin(wheel_speed_[RIGHT]-current_speed[RIGHT], max_accel * seconds_since_last_update); + else + wheel_speed_instr_[RIGHT]+=fmax(wheel_speed_[RIGHT]-current_speed[RIGHT], -max_accel * seconds_since_last_update); - last_update_time_+= common::Time(update_period_); + // ROS_INFO("actual wheel speed = %lf, issued wheel speed= %lf", current_speed[LEFT], wheel_speed_[LEFT]); + // ROS_INFO("actual wheel speed = %lf, issued wheel speed= %lf", current_speed[RIGHT],wheel_speed_[RIGHT]); + joints_[LEFT]->SetVelocity(0,wheel_speed_instr_[LEFT] / (wheel_diameter_ / 2.0)); + joints_[RIGHT]->SetVelocity(0,wheel_speed_instr_[RIGHT] / (wheel_diameter_ / 2.0)); } + last_update_time_+= common::Time(update_period_); + } } // Finalize the controller -void GazeboRosDiffDrive::FiniChild() { - alive_ = false; - queue_.clear(); - queue_.disable(); - rosnode_->shutdown(); - callback_queue_thread_.join(); +void GazeboRosDiffDrive::FiniChild() +{ + alive_ = false; + queue_.clear(); + queue_.disable(); + rosnode_->shutdown(); + callback_queue_thread_.join(); } -void GazeboRosDiffDrive::getWheelVelocities() { - boost::mutex::scoped_lock scoped_lock(lock); +void GazeboRosDiffDrive::getWheelVelocities() +{ + boost::mutex::scoped_lock scoped_lock(lock); - double vr = x_; - double va = rot_; + double vr = x_; + double va = rot_; - wheel_speed_[LEFT] = vr + va * wheel_separation_ / 2.0; - wheel_speed_[RIGHT] = vr - va * wheel_separation_ / 2.0; + wheel_speed_[LEFT] = vr + va * wheel_separation_ / 2.0; + wheel_speed_[RIGHT] = vr - va * wheel_separation_ / 2.0; } -void GazeboRosDiffDrive::cmdVelCallback( - const geometry_msgs::Twist::ConstPtr& cmd_msg) { - - boost::mutex::scoped_lock scoped_lock(lock); - x_ = cmd_msg->linear.x; - rot_ = cmd_msg->angular.z; +void GazeboRosDiffDrive::cmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg) +{ + boost::mutex::scoped_lock scoped_lock(lock); + x_ = cmd_msg->linear.x; + rot_ = cmd_msg->angular.z; } -void GazeboRosDiffDrive::QueueThread() { - static const double timeout = 0.01; +void GazeboRosDiffDrive::QueueThread() +{ + static const double timeout = 0.01; - while (alive_ && rosnode_->ok()) { - queue_.callAvailable(ros::WallDuration(timeout)); - } + while (alive_ && rosnode_->ok()) + { + queue_.callAvailable(ros::WallDuration(timeout)); + } } -void GazeboRosDiffDrive::publishOdometry(double step_time) { - ros::Time current_time = ros::Time::now(); - std::string odom_frame = - tf::resolve(tf_prefix_, odometry_frame_); - std::string base_footprint_frame = - tf::resolve(tf_prefix_, robot_base_frame_); - - // getting data for base_footprint to odom transform - math::Pose pose = this->parent->GetWorldPose(); - - tf::Quaternion qt(pose.rot.x, pose.rot.y, pose.rot.z, pose.rot.w); - tf::Vector3 vt(pose.pos.x, pose.pos.y, pose.pos.z); - - tf::Transform base_footprint_to_odom(qt, vt); - transform_broadcaster_->sendTransform( - tf::StampedTransform(base_footprint_to_odom, current_time, - odom_frame, base_footprint_frame)); - - // publish odom topic - odom_.pose.pose.position.x = pose.pos.x; - odom_.pose.pose.position.y = pose.pos.y; - - odom_.pose.pose.orientation.x = pose.rot.x; - odom_.pose.pose.orientation.y = pose.rot.y; - odom_.pose.pose.orientation.z = pose.rot.z; - odom_.pose.pose.orientation.w = pose.rot.w; - odom_.pose.covariance[0] = 0.00001; - odom_.pose.covariance[7] = 0.00001; - odom_.pose.covariance[14] = 1000000000000.0; - odom_.pose.covariance[21] = 1000000000000.0; - odom_.pose.covariance[28] = 1000000000000.0; - odom_.pose.covariance[35] = 0.001; - - // get velocity in /odom frame - math::Vector3 linear; - linear = this->parent->GetWorldLinearVel(); - odom_.twist.twist.angular.z = this->parent->GetWorldAngularVel().z; - - // convert velocity to child_frame_id (aka base_footprint) - float yaw = pose.rot.GetYaw(); - odom_.twist.twist.linear.x = cosf(yaw) * linear.x + sinf(yaw) * linear.y; - odom_.twist.twist.linear.y = cosf(yaw) * linear.y - sinf(yaw) * linear.x; - - odom_.header.stamp = current_time; - odom_.header.frame_id = odom_frame; - odom_.child_frame_id = base_footprint_frame; - - odometry_publisher_.publish(odom_); +void GazeboRosDiffDrive::publishOdometry(double step_time) +{ + ros::Time current_time = ros::Time::now(); + std::string odom_frame = + tf::resolve(tf_prefix_, odometry_frame_); + std::string base_footprint_frame = + tf::resolve(tf_prefix_, robot_base_frame_); + + // getting data for base_footprint to odom transform + math::Pose pose = this->parent->GetWorldPose(); + + tf::Quaternion qt(pose.rot.x, pose.rot.y, pose.rot.z, pose.rot.w); + tf::Vector3 vt(pose.pos.x, pose.pos.y, pose.pos.z); + + tf::Transform base_footprint_to_odom(qt, vt); + transform_broadcaster_->sendTransform( + tf::StampedTransform(base_footprint_to_odom, current_time, + odom_frame, base_footprint_frame)); + + // publish odom topic + odom_.pose.pose.position.x = pose.pos.x; + odom_.pose.pose.position.y = pose.pos.y; + + odom_.pose.pose.orientation.x = pose.rot.x; + odom_.pose.pose.orientation.y = pose.rot.y; + odom_.pose.pose.orientation.z = pose.rot.z; + odom_.pose.pose.orientation.w = pose.rot.w; + odom_.pose.covariance[0] = 0.00001; + odom_.pose.covariance[7] = 0.00001; + odom_.pose.covariance[14] = 1000000000000.0; + odom_.pose.covariance[21] = 1000000000000.0; + odom_.pose.covariance[28] = 1000000000000.0; + odom_.pose.covariance[35] = 0.001; + + // get velocity in /odom frame + math::Vector3 linear; + linear = this->parent->GetWorldLinearVel(); + odom_.twist.twist.angular.z = this->parent->GetWorldAngularVel().z; + + // convert velocity to child_frame_id (aka base_footprint) + float yaw = pose.rot.GetYaw(); + odom_.twist.twist.linear.x = cosf(yaw) * linear.x + sinf(yaw) * linear.y; + odom_.twist.twist.linear.y = cosf(yaw) * linear.y - sinf(yaw) * linear.x; + + odom_.header.stamp = current_time; + odom_.header.frame_id = odom_frame; + odom_.child_frame_id = base_footprint_frame; + + odometry_publisher_.publish(odom_); } GZ_REGISTER_MODEL_PLUGIN(GazeboRosDiffDrive) From b3fc5f39505ad84aaba762b7bc6fac4701927f32 Mon Sep 17 00:00:00 2001 From: fsuarez6 Date: Wed, 11 Jun 2014 19:17:25 +0200 Subject: [PATCH 105/153] Changed measurement direction to "parent to child" --- gazebo_plugins/src/gazebo_ros_ft_sensor.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gazebo_plugins/src/gazebo_ros_ft_sensor.cpp b/gazebo_plugins/src/gazebo_ros_ft_sensor.cpp index 99ab56135..e5b0f4ae5 100644 --- a/gazebo_plugins/src/gazebo_ros_ft_sensor.cpp +++ b/gazebo_plugins/src/gazebo_ros_ft_sensor.cpp @@ -170,10 +170,10 @@ void GazeboRosFT::UpdateChild() // E.g: https://bitbucket.org/osrf/gazebo/raw/default/gazebo/sensors/ForceTorqueSensor.hh // Get force torque at the joint // The wrench is reported in the CHILD - // The is child_to_parent + // The is parent_to_child wrench = this->joint_->GetForceTorque(0); - force = wrench.body2Force; - torque = wrench.body2Torque; + force = -wrench.body2Force; + torque = -wrench.body2Torque; this->lock_.lock(); From fac19798b92d54925ad24c1e3bc56d4e360ea34e Mon Sep 17 00:00:00 2001 From: Ryohei Ueda Date: Fri, 13 Jun 2014 19:12:20 +0900 Subject: [PATCH 106/153] publish organized pointcloud from openni plugin --- gazebo_plugins/src/gazebo_ros_openni_kinect.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp b/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp index 42faf3c31..0c5b33fb8 100644 --- a/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp +++ b/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp @@ -313,6 +313,7 @@ bool GazeboRosOpenniKinect::FillPointCloudHelper( double fl = ((double)this->width) / (2.0 *tan(hfov/2.0)); // convert depth to point cloud + point_cloud.points.resize(cols_arg * rows_arg); for (uint32_t j=0; j Date: Mon, 23 Jun 2014 12:03:26 +0200 Subject: [PATCH 107/153] BDS licenses header fixed and tricycle drive plugin added --- gazebo_plugins/CMakeLists.txt | 12 +- .../gazebo_plugins/gazebo_ros_camera_utils.h | 1 + .../gazebo_plugins/gazebo_ros_diff_drive.h | 7 +- .../include/gazebo_plugins/gazebo_ros_laser.h | 2 + .../gazebo_plugins/gazebo_ros_sensor_util.h | 73 -- .../gazebo_ros_tricycle_drive.h | 166 +++++ .../include/gazebo_plugins/gazebo_ros_utils.h | 291 ++++++++ .../src/gazebo_ros_camera_utils.cpp | 1 - gazebo_plugins/src/gazebo_ros_diff_drive.cpp | 621 ++++++------------ gazebo_plugins/src/gazebo_ros_gpu_laser.cpp | 2 +- gazebo_plugins/src/gazebo_ros_laser.cpp | 1 - .../src/gazebo_ros_tricycle_drive.cpp | 362 ++++++++++ gazebo_plugins/src/gazebo_ros_utils.cpp | 155 +++++ .../xacro/p3dx/pioneer3dx.xacro | 8 +- .../xacro/p3dx/pioneer3dx_plugins.xacro | 1 + .../test/tricycle_drive/launch/.directory | 4 + .../launch/tricycle.gazebo.launch | 21 + .../test/tricycle_drive/launch/tricycle.rviz | 237 +++++++ .../launch/tricycle.urdf.launch | 17 + .../launch/tricycle_drive_scenario.launch | 16 + .../launch/tricycle_rviz.launch | 6 + .../test/tricycle_drive/xacro/.directory | 3 + .../test/tricycle_drive/xacro/materials.xacro | 36 + .../xacro/tricycle/inertia_tensors.xacro | 72 ++ .../xacro/tricycle/tricycle.xacro | 10 + .../xacro/tricycle/tricycle_body.xacro | 30 + .../xacro/tricycle/tricycle_chassis.xacro | 37 ++ .../xacro/tricycle/tricycle_plugins.xacro | 33 + .../tricycle_drive/xacro/tricycle/wheel.xacro | 41 ++ .../xacro/tricycle/wheel_actuated.xacro | 36 + 30 files changed, 1800 insertions(+), 502 deletions(-) delete mode 100644 gazebo_plugins/include/gazebo_plugins/gazebo_ros_sensor_util.h create mode 100644 gazebo_plugins/include/gazebo_plugins/gazebo_ros_tricycle_drive.h create mode 100644 gazebo_plugins/include/gazebo_plugins/gazebo_ros_utils.h create mode 100644 gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp create mode 100644 gazebo_plugins/src/gazebo_ros_utils.cpp create mode 100644 gazebo_plugins/test/tricycle_drive/launch/.directory create mode 100644 gazebo_plugins/test/tricycle_drive/launch/tricycle.gazebo.launch create mode 100644 gazebo_plugins/test/tricycle_drive/launch/tricycle.rviz create mode 100644 gazebo_plugins/test/tricycle_drive/launch/tricycle.urdf.launch create mode 100644 gazebo_plugins/test/tricycle_drive/launch/tricycle_drive_scenario.launch create mode 100644 gazebo_plugins/test/tricycle_drive/launch/tricycle_rviz.launch create mode 100644 gazebo_plugins/test/tricycle_drive/xacro/.directory create mode 100644 gazebo_plugins/test/tricycle_drive/xacro/materials.xacro create mode 100644 gazebo_plugins/test/tricycle_drive/xacro/tricycle/inertia_tensors.xacro create mode 100644 gazebo_plugins/test/tricycle_drive/xacro/tricycle/tricycle.xacro create mode 100644 gazebo_plugins/test/tricycle_drive/xacro/tricycle/tricycle_body.xacro create mode 100644 gazebo_plugins/test/tricycle_drive/xacro/tricycle/tricycle_chassis.xacro create mode 100644 gazebo_plugins/test/tricycle_drive/xacro/tricycle/tricycle_plugins.xacro create mode 100644 gazebo_plugins/test/tricycle_drive/xacro/tricycle/wheel.xacro create mode 100644 gazebo_plugins/test/tricycle_drive/xacro/tricycle/wheel_actuated.xacro diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index eeca7e42d..1e2a311c2 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -72,6 +72,7 @@ catkin_package( INCLUDE_DIRS include LIBRARIES vision_reconfigure + gazebo_ros_utils gazebo_ros_camera_utils gazebo_ros_camera gazebo_ros_multicamera @@ -92,6 +93,7 @@ catkin_package( gazebo_ros_joint_state_publisher gazebo_ros_joint_pose_trajectory gazebo_ros_diff_drive + gazebo_ros_tricycle_drive gazebo_ros_skid_steer_drive gazebo_ros_video gazebo_ros_planar_move @@ -129,6 +131,9 @@ target_link_libraries(hokuyo_node ${catkin_LIBRARIES} ) +add_library(gazebo_ros_utils src/gazebo_ros_utils.cpp) +target_link_libraries(gazebo_ros_utils ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + add_library(vision_reconfigure src/vision_reconfigure.cpp) add_dependencies(vision_reconfigure ${PROJECT_NAME}_gencfg) target_link_libraries(vision_reconfigure ${catkin_LIBRARIES}) @@ -215,7 +220,10 @@ add_dependencies(gazebo_ros_joint_pose_trajectory gazebo_msgs_gencpp) target_link_libraries(gazebo_ros_joint_pose_trajectory ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_library(gazebo_ros_diff_drive src/gazebo_ros_diff_drive.cpp) -target_link_libraries(gazebo_ros_diff_drive ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +target_link_libraries(gazebo_ros_diff_drive gazebo_ros_utils ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +add_library(gazebo_ros_tricycle_drive src/gazebo_ros_tricycle_drive.cpp) +target_link_libraries(gazebo_ros_tricycle_drive gazebo_ros_utils ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} ${catkin_LIBRARIES}) add_library(gazebo_ros_skid_steer_drive src/gazebo_ros_skid_steer_drive.cpp) target_link_libraries(gazebo_ros_skid_steer_drive ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) @@ -238,6 +246,7 @@ install(TARGETS hokuyo_node vision_reconfigure camera_synchronizer + gazebo_ros_utils gazebo_ros_camera_utils gazebo_ros_camera gazebo_ros_multicamera @@ -259,6 +268,7 @@ install(TARGETS gazebo_ros_joint_state_publisher gazebo_ros_joint_pose_trajectory gazebo_ros_diff_drive + gazebo_ros_tricycle_drive gazebo_ros_skid_steer_drive gazebo_ros_video gazebo_ros_planar_move diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera_utils.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera_utils.h index 46b423447..3d26d9a42 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera_utils.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera_utils.h @@ -46,6 +46,7 @@ #include #include #include +#include namespace gazebo { diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_diff_drive.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_diff_drive.h index 40d3b9061..747d68f09 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_diff_drive.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_diff_drive.h @@ -46,6 +46,7 @@ // Gazebo #include #include +#include // ROS #include @@ -87,13 +88,10 @@ namespace gazebo { void publishWheelJointState(); - physics::WorldPtr world; + GazeboRosPtr gazebo_ros_; physics::ModelPtr parent; event::ConnectionPtr update_connection_; - std::string left_joint_name_; - std::string right_joint_name_; - double wheel_separation_; double wheel_diameter_; double torque; @@ -104,7 +102,6 @@ namespace gazebo { std::vector joints_; // ROS STUFF - boost::shared_ptr rosnode_; ros::Publisher odometry_publisher_; ros::Subscriber cmd_vel_subscriber_; boost::shared_ptr transform_broadcaster_; diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_laser.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_laser.h index bbec66a3c..e89c38d42 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_laser.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_laser.h @@ -36,6 +36,7 @@ #include #include #include +#include #include @@ -59,6 +60,7 @@ namespace gazebo private: void LaserDisconnect(); // Pointer to the model + GazeboRosPtr gazebo_ros_; private: std::string world_name_; private: physics::WorldPtr world_; /// \brief The parent sensor diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_sensor_util.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_sensor_util.h deleted file mode 100644 index 9f4f7262c..000000000 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_sensor_util.h +++ /dev/null @@ -1,73 +0,0 @@ -/*************************************************************************** - * Copyright (C) 2014 by Markus Bader * - * markus.bader@tuwien.ac.at * - * * - * This program is free software; you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation; either version 2 of the License, or * - * (at your option) any later version. * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * You should have received a copy of the GNU General Public License * - * along with this program; if not, write to the * - * Free Software Foundation, Inc., * - * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * - ***************************************************************************/ - - - -#ifndef GAZEBO_ROS_SENSOR_UTIL -#define GAZEBO_ROS_SENSOR_UTIL - - -#include -#include - -namespace gazebo -{ - -/** - * @brief accessing model name like suggested by nkoenig at http://answers.gazebosim.org/question/4878/multiple-robots-with-ros-plugins-sensor-plugin-vs/ - * @Author: Markus Bader - * @return accessing model name - **/ -inline std::string GetModelName(const sensors::SensorPtr &parent) { - std::string modelName; - std::vector values; - std::string scopedName = parent->GetScopedName(); - boost::replace_all(scopedName, "::", ","); - boost::split(values, scopedName, boost::is_any_of(",")); - if(values.size() < 2) { - modelName = ""; - } else { - modelName = values[1]; - } - return modelName; -} - -inline std::string GetRobotNamespace(const sensors::SensorPtr &parent, const sdf::ElementPtr &sdf, const char *pInfo = NULL) { - std::string name_space; - std::stringstream ss; - if (sdf->HasElement("robotNamespace")) { - name_space = sdf->Get("robotNamespace"); - if(name_space.empty()) { - ss << "the 'robotNamespace' param was empty"; - name_space = GetModelName(parent); - } else { - ss << "Using the 'robotNamespace' param: '" << name_space << "'"; - } - } else { - ss << "the 'robotNamespace' param did not exit"; - } - if(pInfo != NULL) { - ROS_INFO ( "%s Plugin (robotNamespace = %s), Info: %s" , pInfo, name_space.c_str(), ss.str().c_str() ); - } - return name_space; -} -} - -#endif //GAZEBO_ROS_SENSOR_UTIL diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_tricycle_drive.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_tricycle_drive.h new file mode 100644 index 000000000..d7bd593f8 --- /dev/null +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_tricycle_drive.h @@ -0,0 +1,166 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Markus Bader + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/** + * \file gazebo_ros_tricycle_drive.cpp + * \brief A tricycle drive plugin for gazebo. + * \author Markus Bader + * \date 22th of June 2014 + */ + +#ifndef TRICYCLE_DRIVE_PLUGIN_HH +#define TRICYCLE_DRIVE_PLUGIN_HH + +#include + +// Gazebo +#include + +// ROS +#include +#include +#include +#include +#include +#include +#include +#include + +// Custom Callback Queue +#include +#include + +// Boost +#include +#include + +namespace gazebo { + +class Joint; +class Entity; + + +class GazeboRosTricycleDrive : public ModelPlugin { + + class TricycleDriveCmd { + public: + TricycleDriveCmd():speed(0), angle(0) {}; + double speed; + double angle; + }; + + enum OdomSource + { + ENCODER = 0, + GAZEBO = 1, + }; +public: + GazeboRosTricycleDrive(); + ~GazeboRosTricycleDrive(); + void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf); + +protected: + virtual void UpdateChild(); + virtual void FiniChild(); + +private: + GazeboRosPtr gazebo_ros_; + physics::ModelPtr parent; + void publishOdometry(double step_time); + void publishWheelTF(); /// publishes the wheel tf's + void publishWheelJointState(); + + event::ConnectionPtr update_connection_; + + physics::JointPtr joint_steering_; + physics::JointPtr joint_wheel_actuated_; + physics::JointPtr joint_wheel_encoder_left_; + physics::JointPtr joint_wheel_encoder_right_; + + double diameter_encoder_wheel_; + double diameter_actuated_wheel_; + double wheel_acceleration; + double steering_speed; + double separation_encoder_wheel_; + + OdomSource odom_source_; + double torque; + + std::string robot_namespace_; + std::string command_topic_; + std::string odometry_topic_; + std::string odometry_frame_; + std::string robot_base_frame_; + + // ROS STUFF + ros::Publisher odometry_publisher_; + ros::Subscriber cmd_vel_subscriber_; + boost::shared_ptr transform_broadcaster_; + sensor_msgs::JointState joint_state_; + ros::Publisher joint_state_publisher_; + nav_msgs::Odometry odom_; + + boost::mutex lock; + + + // Custom Callback Queue + ros::CallbackQueue queue_; + boost::thread callback_queue_thread_; + void QueueThread(); + + // DiffDrive stuff + void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg); + + /// updates the relative robot pose based on the wheel encoders + void UpdateOdometryEncoder(); + + TricycleDriveCmd cmd_; + bool alive_; + geometry_msgs::Pose2D pose_encoder_; + common::Time last_odom_update_; + + // Update Rate + double update_rate_; + double update_period_; + common::Time last_actuator_update_; + + // Flags + bool publishWheelTF_; + bool publishWheelJointState_; + +}; + +} + +#endif + diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_utils.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_utils.h new file mode 100644 index 000000000..3db4c45f5 --- /dev/null +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_utils.h @@ -0,0 +1,291 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Markus Bader + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#ifndef GAZEBO_ROS_UTILS_H +#define GAZEBO_ROS_UTILS_H +#include +#include + +#include +#include +#include +#include + +namespace gazebo +{ + +/** +* Accessing model name like suggested by nkoenig at http://answers.gazebosim.org/question/4878/multiple-robots-with-ros-plugins-sensor-plugin-vs/ +* @param parent +* @return accessing model name +**/ +inline std::string GetModelName ( const sensors::SensorPtr &parent ) +{ + std::string modelName; + std::vector values; + std::string scopedName = parent->GetScopedName(); + boost::replace_all ( scopedName, "::", "," ); + boost::split ( values, scopedName, boost::is_any_of ( "," ) ); + if ( values.size() < 2 ) { + modelName = ""; + } else { + modelName = values[1]; + } + return modelName; +} + +/** +* @brief Reads the name space tag of a sensor plugin +* @param parent +* @param sdf +* @param pInfo +* @return node namespace +**/ +inline std::string GetRobotNamespace ( const sensors::SensorPtr &parent, const sdf::ElementPtr &sdf, const char *pInfo = NULL ) +{ + std::string name_space; + std::stringstream ss; + if ( sdf->HasElement ( "robotNamespace" ) ) { + name_space = sdf->Get ( "robotNamespace" ); + if ( name_space.empty() ) { + ss << "the 'robotNamespace' param was empty"; + name_space = GetModelName ( parent ); + } else { + ss << "Using the 'robotNamespace' param: '" << name_space << "'"; + } + } else { + ss << "the 'robotNamespace' param did not exit"; + } + if ( pInfo != NULL ) { + ROS_INFO ( "%s Plugin (robotNamespace = %s), Info: %s" , pInfo, name_space.c_str(), ss.str().c_str() ); + } + return name_space; +} + +/** + * Gazebo ros helper class + * The class simplifies the parameter and rosnode handling + * @author Markus Bader + **/ +class GazeboRos +{ +private: + sdf::ElementPtr sdf_; /// sdf to read + std::string plugin_; /// name of the plugin class + std::string namespace_; /// name of the launched node + boost::shared_ptr rosnode_; /// rosnode + std::string tf_prefix_; /// prefix for the ros tf plublisher if not set it uses the namespace_ + std::string info_text; /// info text for log messages to identify the node + /** + * Reads the common plugin parameters used by the constructor + **/ + void readCommonParameter (); +public: + /** + * Constructor + * @param _parent models parent + * @param _sdf sdf to read + * @param _name of the plugin class + **/ + GazeboRos ( physics::ModelPtr &_parent, sdf::ElementPtr _sdf, const std::string &_plugin ) + : sdf_ ( _sdf ), plugin_ ( _plugin ) { + namespace_ = _parent->GetName (); + if ( !sdf_->HasElement ( "robotNamespace" ) ) { + ROS_INFO ( "%s missing , defaults is %s", plugin_.c_str(), namespace_.c_str() ); + } else { + namespace_ = sdf_->GetElement ( "robotNamespace" )->Get(); + if ( namespace_.empty() ) { + namespace_ = _parent->GetName(); + } + } + if ( !namespace_.empty() ) + this->namespace_ += "/"; + rosnode_ = boost::shared_ptr ( new ros::NodeHandle ( namespace_ ) ); + info_text = plugin_ + "(ns = " + namespace_ + ")"; + readCommonParameter (); + } + /** + * Constructor + * @param _parent sensor parent + * @param _sdf sdf to read + * @param _name of the plugin class + **/ + GazeboRos ( sensors::SensorPtr _parent, sdf::ElementPtr _sdf, const std::string &_plugin ) + : sdf_ ( _sdf ), plugin_ ( _plugin ) { + + std::stringstream ss; + if ( sdf_->HasElement ( "robotNamespace" ) ) { + namespace_ = sdf_->Get ( "robotNamespace" ); + if ( namespace_.empty() ) { + ss << "the 'robotNamespace' param was empty"; + namespace_ = GetModelName ( _parent ); + } else { + ss << "Using the 'robotNamespace' param: '" << namespace_ << "'"; + } + } else { + ss << "the 'robotNamespace' param did not exit"; + } + info_text = plugin_ + "(ns = " + namespace_ + ")"; + ROS_INFO ( "%s: %s" , info_text.c_str(), ss.str().c_str() ); + readCommonParameter (); + } + + /** + * Returns info text used for log messages + * @return class name and node name as string + **/ + const char* info() const; + /** + * returns the initialized created within the constuctor + * @return rosnode + **/ + boost::shared_ptr& node();; + /** + * returns the initialized within the constuctor + * @return rosnode + **/ + const boost::shared_ptr& node() const; + /** + * resolves a tf frame name by adding the tf_prefix initialized within the constuctor + * @param _name + * @retun resolved tf name + **/ + std::string resolveTF ( const std::string &_name ); + + /** + * reads the follwoing _tag_name paramer or sets a _default value + * @param _value + * @param _tag_name + * @param _default + * @retun sdf tag value + **/ + void getParameterBoolean ( bool &_value, const char *_tag_name, const bool &_default ); + /** + * reads the follwoing _tag_name paramer + * @param _value + * @param _tag_name + * @retun sdf tag value + **/ + void getParameterBoolean ( bool &_value, const char *_tag_name ); + /** + * retuns a JointPtr based on an sdf tag_name entry + * @param _value + * @param _tag_name + * @param _joint_default_name + * @retun JointPtr + **/ + physics::JointPtr getJoint ( physics::ModelPtr &_parent, const char *_tag_name, const std::string &_joint_default_name ); + /** + * checks if the ros not is initialized + * @retun JointPtr + **/ + void isInitialized(); + + + /** + * reads the follwoing _tag_name paramer or sets a _default value + * @param _value + * @param _tag_name + * @param _default + * @retun sdf tag value + **/ + template + void getParameter ( T &_value, const char *_tag_name, const T &_default ) { + _value = _default; + if ( !sdf_->HasElement ( _tag_name ) ) { + ROS_WARN ( "%s: missing <%s> default is %s", info(), _tag_name, boost::lexical_cast ( _default ).c_str() ); + } else { + this->getParameter ( _value, _tag_name ); + } + } + /** + * reads the follwoing _tag_name paramer + * @param _value + * @param _tag_name + * @param _default + * @retun sdf tag value + **/ + template + void getParameter ( T &_value, const char *_tag_name ) { + if ( sdf_->HasElement ( _tag_name ) ) { + _value = sdf_->GetElement ( _tag_name )->Get(); + } + ROS_DEBUG ( "%s: <%s> = %s", info(), _tag_name, boost::lexical_cast ( _value ).c_str() ); + + } + + /** + * reads the following _tag_name paramer and compares the value with an _options map keys and retuns the corresponding value. + * @param _value + * @param _tag_name + * @param _default + * @retun sdf tag value + **/ + template + void getParameter ( T &_value, const char *_tag_name, const std::map &_options, const T &_default ) { + _value = _default; + if ( !sdf_->HasElement ( _tag_name ) ) { + ROS_WARN ( "%s: missing <%s> default is %s", info(), _tag_name, boost::lexical_cast ( _default ).c_str() ); + } else { + this->getParameter ( _value, _tag_name, _options ); + } + } + /** + * reads the following _tag_name paramer and compares the value with an _options map keys and retuns the corresponding value. + * @param _value + * @param _tag_name + * @param _default + * @retun sdf tag value + **/ + template + void getParameter ( T &_value, const char *_tag_name, const std::map &_options ) { + typename std::map::const_iterator it; + if ( sdf_->HasElement ( _tag_name ) ) { + std::string value = sdf_->GetElement ( _tag_name )->Get(); + it = _options.find ( value ); + if ( it == _options.end() ) { + ROS_WARN ( "%s: <%s> no matching key to %s", info(), _tag_name, value.c_str() ); + } else { + _value = it->second; + } + } + ROS_DEBUG ( "%s: <%s> = %s := %s", info(), _tag_name, ( it == _options.end() ?"default":it->first.c_str() ), boost::lexical_cast ( _value ).c_str() ); + } +}; + +typedef boost::shared_ptr GazeboRosPtr; +} +#endif + + + diff --git a/gazebo_plugins/src/gazebo_ros_camera_utils.cpp b/gazebo_plugins/src/gazebo_ros_camera_utils.cpp index 3dbaaa93f..52fbabed9 100644 --- a/gazebo_plugins/src/gazebo_ros_camera_utils.cpp +++ b/gazebo_plugins/src/gazebo_ros_camera_utils.cpp @@ -39,7 +39,6 @@ #include #include -#include "gazebo_plugins/gazebo_ros_sensor_util.h" #include "gazebo_plugins/gazebo_ros_camera_utils.h" namespace gazebo diff --git a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp index cac5420c8..d56d49033 100644 --- a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp +++ b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp @@ -67,10 +67,9 @@ namespace gazebo { -enum -{ - RIGHT, - LEFT, +enum { + RIGHT, + LEFT, }; GazeboRosDiffDrive::GazeboRosDiffDrive() {} @@ -79,459 +78,249 @@ GazeboRosDiffDrive::GazeboRosDiffDrive() {} GazeboRosDiffDrive::~GazeboRosDiffDrive() {} // Load the controller -void GazeboRosDiffDrive::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) +void GazeboRosDiffDrive::Load ( physics::ModelPtr _parent, sdf::ElementPtr _sdf ) { - this->parent = _parent; - this->world = _parent->GetWorld(); - - this->robot_namespace_ = this->parent->GetName (); - if (!_sdf->HasElement("robotNamespace")) - { - ROS_INFO("GazeboRosDiffDrive Plugin missing , defaults to \"%s\"", - this->robot_namespace_.c_str()); - } - else - { - this->robot_namespace_ = _sdf->GetElement("robotNamespace")->Get(); - if (this->robot_namespace_.empty()) - this->robot_namespace_ = this->parent->GetName(); - } - if (!robot_namespace_.empty()) - this->robot_namespace_ += "/"; - rosnode_ = boost::shared_ptr(new ros::NodeHandle(this->robot_namespace_)); - - tf_prefix_ = tf::getPrefixParam(*rosnode_); - if(tf_prefix_.empty()) - { - tf_prefix_ = robot_namespace_; - boost::trim_right_if(tf_prefix_,boost::is_any_of("/")); - } - ROS_INFO("GazeboRosDiffDrive Plugin (ns = %s) , set to \"%s\"", - this->robot_namespace_.c_str(), this->tf_prefix_.c_str()); - - this->left_joint_name_ = "left_joint"; - if (!_sdf->HasElement("leftJoint")) - { - ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to \"%s\"", - this->robot_namespace_.c_str(), this->left_joint_name_.c_str()); - } - else - { - this->left_joint_name_ = _sdf->GetElement("leftJoint")->Get(); - } - - this->right_joint_name_ = "right_joint"; - if (!_sdf->HasElement("rightJoint")) - { - ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to \"%s\"", - this->robot_namespace_.c_str(), this->right_joint_name_.c_str()); - } - else - { - this->right_joint_name_ = _sdf->GetElement("rightJoint")->Get(); - } - - this->wheel_separation_ = 0.34; - if (!_sdf->HasElement("wheelSeparation")) - { - ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to %f", - this->robot_namespace_.c_str(), this->wheel_separation_); - } - else - { - this->wheel_separation_ = - _sdf->GetElement("wheelSeparation")->Get(); - } - - this->wheel_diameter_ = 0.15; - if (!_sdf->HasElement("wheelDiameter")) - { - ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to %f", - this->robot_namespace_.c_str(), this->wheel_diameter_); - } - else - { - this->wheel_diameter_ = _sdf->GetElement("wheelDiameter")->Get(); - } - - this->torque = 5.0; - if (!_sdf->HasElement("torque")) - { - ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to %f", - this->robot_namespace_.c_str(), this->torque); - } - else - { - this->torque = _sdf->GetElement("torque")->Get(); - } - - this->command_topic_ = "cmd_vel"; - if (!_sdf->HasElement("commandTopic")) - { - ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to \"%s\"", - this->robot_namespace_.c_str(), this->command_topic_.c_str()); - } - else - { - this->command_topic_ = _sdf->GetElement("commandTopic")->Get(); - } - - this->odometry_topic_ = "odom"; - if (!_sdf->HasElement("odometryTopic")) - { - ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to \"%s\"", - this->robot_namespace_.c_str(), this->odometry_topic_.c_str()); - } - else - { - this->odometry_topic_ = _sdf->GetElement("odometryTopic")->Get(); - } - - this->odometry_frame_ = "odom"; - if (!_sdf->HasElement("odometryFrame")) - { - ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to \"%s\"", - this->robot_namespace_.c_str(), this->odometry_frame_.c_str()); - } - else - { - this->odometry_frame_ = _sdf->GetElement("odometryFrame")->Get(); - } - - this->robot_base_frame_ = "base_footprint"; - if (!_sdf->HasElement("robotBaseFrame")) - { - ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to \"%s\"", - this->robot_namespace_.c_str(), this->robot_base_frame_.c_str()); - } - else - { - this->robot_base_frame_ = _sdf->GetElement("robotBaseFrame")->Get(); - } - - this->update_rate_ = 100.0; - if (!_sdf->HasElement("updateRate")) - { - ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to %f", - this->robot_namespace_.c_str(), this->update_rate_); - } - else - { - this->update_rate_ = _sdf->GetElement("updateRate")->Get(); - } - - - this->publishWheelTF_ = false; - if (_sdf->HasElement("publishWheelTF")) - { - std::string value = _sdf->GetElement("publishWheelTF")->Get(); - if(boost::iequals(value, std::string("true"))) - { - this->publishWheelTF_ = true; - } - else if(boost::iequals(value, std::string("false"))) - { - this->publishWheelTF_ = false; - } - else - { - ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) was set to '%s', it must be set to 'true' or 'false'!", - this->robot_namespace_.c_str(), value.c_str()); - } - } - ROS_INFO("GazeboRosDiffDrive Plugin (ns = %s) , set to %s", - this->robot_namespace_.c_str(), (this->publishWheelTF_?"ture":"false")); - - this->publishWheelJointState_ = false; - if (_sdf->HasElement("publishWheelJointState")) - { - std::string value = _sdf->GetElement("publishWheelJointState")->Get(); - if(boost::iequals(value, std::string("true"))) - { - this->publishWheelJointState_ = true; - } - else if(boost::iequals(value, std::string("false"))) - { - this->publishWheelJointState_ = false; - } - else + + this->parent = _parent; + gazebo_ros_ = GazeboRosPtr ( new GazeboRos ( _parent, _sdf, "DiffDrive" ) ); + // Make sure the ROS node for Gazebo has already been initialized + gazebo_ros_->isInitialized(); + + gazebo_ros_->getParameter ( command_topic_, "commandTopic", "cmd_vel" ); + gazebo_ros_->getParameter ( odometry_topic_, "odometryTopic", "odom" ); + gazebo_ros_->getParameter ( odometry_frame_, "odometryFrame", "odom" ); + gazebo_ros_->getParameter ( robot_base_frame_, "robotBaseFrame", "base_footprint" ); + gazebo_ros_->getParameterBoolean ( publishWheelTF_, "publishWheelTF", false ); + gazebo_ros_->getParameterBoolean ( publishWheelJointState_, "publishWheelJointState", false ); + + gazebo_ros_->getParameter ( wheel_separation_, "wheelSeparation", 0.34 ); + gazebo_ros_->getParameter ( wheel_diameter_, "wheelDiameter", 0.15 ); + gazebo_ros_->getParameter ( torque, "torque", 5.0 ); + gazebo_ros_->getParameter ( update_rate_, "updateRate", 100.0 ); + gazebo_ros_->getParameter ( max_accel, "max_accel", 0.0 ); + + + joints_.resize ( 2 ); + joints_[LEFT] = gazebo_ros_->getJoint ( parent, "leftJoint", "left_joint" ); + joints_[RIGHT] = gazebo_ros_->getJoint ( parent, "rightJoint", "right_joint" ); + joints_[LEFT]->SetMaxForce ( 0, torque ); + joints_[RIGHT]->SetMaxForce ( 0, torque ); + + + + // Initialize update rate stuff + if ( this->update_rate_ > 0.0 ) this->update_period_ = 1.0 / this->update_rate_; + else this->update_period_ = 0.0; + last_update_time_ = parent->GetWorld()->GetSimTime(); + + // Initialize velocity stuff + wheel_speed_[RIGHT] = 0; + wheel_speed_[LEFT] = 0; + + x_ = 0; + rot_ = 0; + alive_ = true; + + + if(this->publishWheelJointState_) { - ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) was set to '%s', it must be set to 'true' or 'false'!", - this->robot_namespace_.c_str(), value.c_str()); + joint_state_publisher_ = gazebo_ros_->node()->advertise("joint_states", 1000); + ROS_INFO("%s: Advertise joint_states!", gazebo_ros_->info()); } - } - this->max_accel = 0; - if (!_sdf->HasElement("max_accel")) - { - ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to %f", - this->robot_namespace_.c_str(), this->max_accel); - } - else - { - this->max_accel = _sdf->GetElement("max_accel")->Get(); - } - - ROS_INFO("GazeboRosDiffDrive Plugin (ns = %s) , set to %s", - this->robot_namespace_.c_str(), (this->publishWheelJointState_?"ture":"false")); - - // Initialize update rate stuff - if (this->update_rate_ > 0.0) - { - this->update_period_ = 1.0 / this->update_rate_; - } - else - { - this->update_period_ = 0.0; - } - last_update_time_ = this->world->GetSimTime(); - - // Initialize velocity stuff - wheel_speed_[RIGHT] = 0; - wheel_speed_[LEFT] = 0; - - x_ = 0; - rot_ = 0; - alive_ = true; - - joints_.resize(2); - joints_[LEFT] = this->parent->GetJoint(left_joint_name_); - joints_[RIGHT] = this->parent->GetJoint(right_joint_name_); - - if (!joints_[LEFT]) - { - char error[200]; - snprintf(error, 200, - "GazeboRosDiffDrive Plugin (ns = %s) couldn't get left hinge joint named \"%s\"", - this->robot_namespace_.c_str(), this->left_joint_name_.c_str()); - gzthrow(error); - } - if (!joints_[RIGHT]) - { - char error[200]; - snprintf(error, 200, - "GazeboRosDiffDrive Plugin (ns = %s) couldn't get right hinge joint named \"%s\"", - this->robot_namespace_.c_str(), this->right_joint_name_.c_str()); - gzthrow(error); - } - - joints_[LEFT]->SetMaxForce(0, torque); - joints_[RIGHT]->SetMaxForce(0, torque); - - // Make sure the ROS node for Gazebo has already been initialized - if (!ros::isInitialized()) - { - ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. " - << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); - return; - } - - rosnode_ = boost::shared_ptr(new ros::NodeHandle(this->robot_namespace_)); - ROS_INFO("Starting GazeboRosDiffDrive Plugin (ns = %s)!", this->robot_namespace_.c_str()); - - if(this->publishWheelJointState_) - { - joint_state_publisher_ = rosnode_->advertise("joint_states", 1000); - } - - transform_broadcaster_ = boost::shared_ptr(new tf::TransformBroadcaster()); - - // ROS: Subscribe to the velocity command topic (usually "cmd_vel") - ros::SubscribeOptions so = - ros::SubscribeOptions::create(command_topic_, 1, - boost::bind(&GazeboRosDiffDrive::cmdVelCallback, this, _1), - ros::VoidPtr(), &queue_); - - cmd_vel_subscriber_ = rosnode_->subscribe(so); - - odometry_publisher_ = rosnode_->advertise(odometry_topic_, 1); - - // start custom queue for diff drive - this->callback_queue_thread_ = - boost::thread(boost::bind(&GazeboRosDiffDrive::QueueThread, this)); - - // listen to the update event (broadcast every simulation iteration) - this->update_connection_ = - event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosDiffDrive::UpdateChild, this)); + + transform_broadcaster_ = boost::shared_ptr(new tf::TransformBroadcaster()); + + // ROS: Subscribe to the velocity command topic (usually "cmd_vel") + ROS_INFO("%s: Try to subscribe to %s!", gazebo_ros_->info(), command_topic_.c_str()); + + ros::SubscribeOptions so = + ros::SubscribeOptions::create(command_topic_, 1, + boost::bind(&GazeboRosDiffDrive::cmdVelCallback, this, _1), + ros::VoidPtr(), &queue_); + + cmd_vel_subscriber_ = gazebo_ros_->node()->subscribe(so); + ROS_INFO("%s: Subscribe to %s!", gazebo_ros_->info(), command_topic_.c_str()); + + odometry_publisher_ = gazebo_ros_->node()->advertise(odometry_topic_, 1); + ROS_INFO("%s: Advertise odom on %s !", gazebo_ros_->info(), odometry_topic_.c_str()); + + // start custom queue for diff drive + this->callback_queue_thread_ = + boost::thread ( boost::bind ( &GazeboRosDiffDrive::QueueThread, this ) ); + + // listen to the update event (broadcast every simulation iteration) + this->update_connection_ = + event::Events::ConnectWorldUpdateBegin ( boost::bind ( &GazeboRosDiffDrive::UpdateChild, this ) ); } void GazeboRosDiffDrive::publishWheelJointState() { - ros::Time current_time = ros::Time::now(); - - joint_state_.header.stamp = current_time; - joint_state_.name.resize ( joints_.size() ); - joint_state_.position.resize ( joints_.size() ); - - for ( int i = 0; i < 2; i++ ) - { - physics::JointPtr joint = joints_[i]; - math::Angle angle = joint->GetAngle ( 0 ); - joint_state_.name[i] = joint->GetName(); - joint_state_.position[i] = angle.Radian () ; - } - joint_state_publisher_.publish ( joint_state_ ); + ros::Time current_time = ros::Time::now(); + + joint_state_.header.stamp = current_time; + joint_state_.name.resize ( joints_.size() ); + joint_state_.position.resize ( joints_.size() ); + + for ( int i = 0; i < 2; i++ ) { + physics::JointPtr joint = joints_[i]; + math::Angle angle = joint->GetAngle ( 0 ); + joint_state_.name[i] = joint->GetName(); + joint_state_.position[i] = angle.Radian () ; + } + joint_state_publisher_.publish ( joint_state_ ); } void GazeboRosDiffDrive::publishWheelTF() { - ros::Time current_time = ros::Time::now(); - for(int i = 0; i < 2; i++) - { - std::string wheel_frame = - tf::resolve(tf_prefix_, joints_[i]->GetChild()->GetName ()); - std::string wheel_parent_frame = - tf::resolve(tf_prefix_, joints_[i]->GetParent()->GetName ()); - - math::Pose poseWheel = joints_[i]->GetChild()->GetRelativePose(); - - tf::Quaternion qt(poseWheel.rot.x, poseWheel.rot.y, poseWheel.rot.z, poseWheel.rot.w); - tf::Vector3 vt(poseWheel.pos.x, poseWheel.pos.y, poseWheel.pos.z); - - tf::Transform tfWheel(qt, vt); - transform_broadcaster_->sendTransform( - tf::StampedTransform(tfWheel, current_time, wheel_parent_frame, wheel_frame)); - } + ros::Time current_time = ros::Time::now(); + for ( int i = 0; i < 2; i++ ) { + + std::string wheel_frame = gazebo_ros_->resolveTF(joints_[i]->GetChild()->GetName ()); + std::string wheel_parent_frame = gazebo_ros_->resolveTF(joints_[i]->GetParent()->GetName ()); + + math::Pose poseWheel = joints_[i]->GetChild()->GetRelativePose(); + + tf::Quaternion qt ( poseWheel.rot.x, poseWheel.rot.y, poseWheel.rot.z, poseWheel.rot.w ); + tf::Vector3 vt ( poseWheel.pos.x, poseWheel.pos.y, poseWheel.pos.z ); + + tf::Transform tfWheel ( qt, vt ); + transform_broadcaster_->sendTransform ( + tf::StampedTransform ( tfWheel, current_time, wheel_parent_frame, wheel_frame ) ); + } } // Update the controller void GazeboRosDiffDrive::UpdateChild() { - common::Time current_time = this->world->GetSimTime(); - double seconds_since_last_update = (current_time - last_update_time_).Double(); - if (seconds_since_last_update > update_period_) - { - publishOdometry(seconds_since_last_update); - if(publishWheelTF_) publishWheelTF(); - if(publishWheelJointState_) publishWheelJointState(); - - // Update robot in case new velocities have been requested - getWheelVelocities(); - - double current_speed[2]; - - current_speed[LEFT] = joints_[LEFT]->GetVelocity(0) * (wheel_diameter_ / 2.0); - current_speed[RIGHT] = joints_[RIGHT]->GetVelocity(0) * (wheel_diameter_ / 2.0); - - if(max_accel == 0 || - (fabs(wheel_speed_[LEFT] - current_speed[LEFT]) < 0.01) || - (fabs(wheel_speed_[RIGHT] - current_speed[RIGHT]) < 0.01)) - { - //if max_accel == 0, or target speed is reached - joints_[LEFT]->SetVelocity(0, wheel_speed_[LEFT]/ (wheel_diameter_ / 2.0)); - joints_[RIGHT]->SetVelocity(0, wheel_speed_[RIGHT]/ (wheel_diameter_ / 2.0)); + common::Time current_time = parent->GetWorld()->GetSimTime(); + double seconds_since_last_update = ( current_time - last_update_time_ ).Double(); + if ( seconds_since_last_update > update_period_ ) { + publishOdometry ( seconds_since_last_update ); + if ( publishWheelTF_ ) publishWheelTF(); + if ( publishWheelJointState_ ) publishWheelJointState(); + + // Update robot in case new velocities have been requested + getWheelVelocities(); + + double current_speed[2]; + + current_speed[LEFT] = joints_[LEFT]->GetVelocity ( 0 ) * ( wheel_diameter_ / 2.0 ); + current_speed[RIGHT] = joints_[RIGHT]->GetVelocity ( 0 ) * ( wheel_diameter_ / 2.0 ); + + if ( max_accel == 0 || + ( fabs ( wheel_speed_[LEFT] - current_speed[LEFT] ) < 0.01 ) || + ( fabs ( wheel_speed_[RIGHT] - current_speed[RIGHT] ) < 0.01 ) ) { + //if max_accel == 0, or target speed is reached + joints_[LEFT]->SetVelocity ( 0, wheel_speed_[LEFT]/ ( wheel_diameter_ / 2.0 ) ); + joints_[RIGHT]->SetVelocity ( 0, wheel_speed_[RIGHT]/ ( wheel_diameter_ / 2.0 ) ); + } else { + if ( wheel_speed_[LEFT]>=current_speed[LEFT] ) + wheel_speed_instr_[LEFT]+=fmin ( wheel_speed_[LEFT]-current_speed[LEFT], max_accel * seconds_since_last_update ); + else + wheel_speed_instr_[LEFT]+=fmax ( wheel_speed_[LEFT]-current_speed[LEFT], -max_accel * seconds_since_last_update ); + + if ( wheel_speed_[RIGHT]>current_speed[RIGHT] ) + wheel_speed_instr_[RIGHT]+=fmin ( wheel_speed_[RIGHT]-current_speed[RIGHT], max_accel * seconds_since_last_update ); + else + wheel_speed_instr_[RIGHT]+=fmax ( wheel_speed_[RIGHT]-current_speed[RIGHT], -max_accel * seconds_since_last_update ); + + // ROS_INFO("actual wheel speed = %lf, issued wheel speed= %lf", current_speed[LEFT], wheel_speed_[LEFT]); + // ROS_INFO("actual wheel speed = %lf, issued wheel speed= %lf", current_speed[RIGHT],wheel_speed_[RIGHT]); + + joints_[LEFT]->SetVelocity ( 0,wheel_speed_instr_[LEFT] / ( wheel_diameter_ / 2.0 ) ); + joints_[RIGHT]->SetVelocity ( 0,wheel_speed_instr_[RIGHT] / ( wheel_diameter_ / 2.0 ) ); + } + last_update_time_+= common::Time ( update_period_ ); } - else - { - if(wheel_speed_[LEFT]>=current_speed[LEFT]) - wheel_speed_instr_[LEFT]+=fmin(wheel_speed_[LEFT]-current_speed[LEFT], max_accel * seconds_since_last_update); - else - wheel_speed_instr_[LEFT]+=fmax(wheel_speed_[LEFT]-current_speed[LEFT], -max_accel * seconds_since_last_update); - - if(wheel_speed_[RIGHT]>current_speed[RIGHT]) - wheel_speed_instr_[RIGHT]+=fmin(wheel_speed_[RIGHT]-current_speed[RIGHT], max_accel * seconds_since_last_update); - else - wheel_speed_instr_[RIGHT]+=fmax(wheel_speed_[RIGHT]-current_speed[RIGHT], -max_accel * seconds_since_last_update); - - // ROS_INFO("actual wheel speed = %lf, issued wheel speed= %lf", current_speed[LEFT], wheel_speed_[LEFT]); - // ROS_INFO("actual wheel speed = %lf, issued wheel speed= %lf", current_speed[RIGHT],wheel_speed_[RIGHT]); - - joints_[LEFT]->SetVelocity(0,wheel_speed_instr_[LEFT] / (wheel_diameter_ / 2.0)); - joints_[RIGHT]->SetVelocity(0,wheel_speed_instr_[RIGHT] / (wheel_diameter_ / 2.0)); - } - last_update_time_+= common::Time(update_period_); - } } // Finalize the controller void GazeboRosDiffDrive::FiniChild() { - alive_ = false; - queue_.clear(); - queue_.disable(); - rosnode_->shutdown(); - callback_queue_thread_.join(); + alive_ = false; + queue_.clear(); + queue_.disable(); + gazebo_ros_->node()->shutdown(); + callback_queue_thread_.join(); } void GazeboRosDiffDrive::getWheelVelocities() { - boost::mutex::scoped_lock scoped_lock(lock); + boost::mutex::scoped_lock scoped_lock ( lock ); - double vr = x_; - double va = rot_; + double vr = x_; + double va = rot_; - wheel_speed_[LEFT] = vr + va * wheel_separation_ / 2.0; - wheel_speed_[RIGHT] = vr - va * wheel_separation_ / 2.0; + wheel_speed_[LEFT] = vr + va * wheel_separation_ / 2.0; + wheel_speed_[RIGHT] = vr - va * wheel_separation_ / 2.0; } -void GazeboRosDiffDrive::cmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg) +void GazeboRosDiffDrive::cmdVelCallback ( const geometry_msgs::Twist::ConstPtr& cmd_msg ) { - boost::mutex::scoped_lock scoped_lock(lock); - x_ = cmd_msg->linear.x; - rot_ = cmd_msg->angular.z; + boost::mutex::scoped_lock scoped_lock ( lock ); + x_ = cmd_msg->linear.x; + rot_ = cmd_msg->angular.z; } void GazeboRosDiffDrive::QueueThread() { - static const double timeout = 0.01; + static const double timeout = 0.01; - while (alive_ && rosnode_->ok()) - { - queue_.callAvailable(ros::WallDuration(timeout)); - } + while ( alive_ && gazebo_ros_->node()->ok() ) { + queue_.callAvailable ( ros::WallDuration ( timeout ) ); + } } -void GazeboRosDiffDrive::publishOdometry(double step_time) +void GazeboRosDiffDrive::publishOdometry ( double step_time ) { - ros::Time current_time = ros::Time::now(); - std::string odom_frame = - tf::resolve(tf_prefix_, odometry_frame_); - std::string base_footprint_frame = - tf::resolve(tf_prefix_, robot_base_frame_); - - // getting data for base_footprint to odom transform - math::Pose pose = this->parent->GetWorldPose(); - - tf::Quaternion qt(pose.rot.x, pose.rot.y, pose.rot.z, pose.rot.w); - tf::Vector3 vt(pose.pos.x, pose.pos.y, pose.pos.z); - - tf::Transform base_footprint_to_odom(qt, vt); - transform_broadcaster_->sendTransform( - tf::StampedTransform(base_footprint_to_odom, current_time, - odom_frame, base_footprint_frame)); - - // publish odom topic - odom_.pose.pose.position.x = pose.pos.x; - odom_.pose.pose.position.y = pose.pos.y; - - odom_.pose.pose.orientation.x = pose.rot.x; - odom_.pose.pose.orientation.y = pose.rot.y; - odom_.pose.pose.orientation.z = pose.rot.z; - odom_.pose.pose.orientation.w = pose.rot.w; - odom_.pose.covariance[0] = 0.00001; - odom_.pose.covariance[7] = 0.00001; - odom_.pose.covariance[14] = 1000000000000.0; - odom_.pose.covariance[21] = 1000000000000.0; - odom_.pose.covariance[28] = 1000000000000.0; - odom_.pose.covariance[35] = 0.001; - - // get velocity in /odom frame - math::Vector3 linear; - linear = this->parent->GetWorldLinearVel(); - odom_.twist.twist.angular.z = this->parent->GetWorldAngularVel().z; - - // convert velocity to child_frame_id (aka base_footprint) - float yaw = pose.rot.GetYaw(); - odom_.twist.twist.linear.x = cosf(yaw) * linear.x + sinf(yaw) * linear.y; - odom_.twist.twist.linear.y = cosf(yaw) * linear.y - sinf(yaw) * linear.x; - - odom_.header.stamp = current_time; - odom_.header.frame_id = odom_frame; - odom_.child_frame_id = base_footprint_frame; - - odometry_publisher_.publish(odom_); + ros::Time current_time = ros::Time::now(); + std::string odom_frame = gazebo_ros_->resolveTF(odometry_frame_); + std::string base_footprint_frame = gazebo_ros_->resolveTF(robot_base_frame_); + + // getting data for base_footprint to odom transform + math::Pose pose = this->parent->GetWorldPose(); + + tf::Quaternion qt ( pose.rot.x, pose.rot.y, pose.rot.z, pose.rot.w ); + tf::Vector3 vt ( pose.pos.x, pose.pos.y, pose.pos.z ); + + tf::Transform base_footprint_to_odom ( qt, vt ); + transform_broadcaster_->sendTransform ( + tf::StampedTransform ( base_footprint_to_odom, current_time, + odom_frame, base_footprint_frame ) ); + + // publish odom topic + odom_.pose.pose.position.x = pose.pos.x; + odom_.pose.pose.position.y = pose.pos.y; + + odom_.pose.pose.orientation.x = pose.rot.x; + odom_.pose.pose.orientation.y = pose.rot.y; + odom_.pose.pose.orientation.z = pose.rot.z; + odom_.pose.pose.orientation.w = pose.rot.w; + odom_.pose.covariance[0] = 0.00001; + odom_.pose.covariance[7] = 0.00001; + odom_.pose.covariance[14] = 1000000000000.0; + odom_.pose.covariance[21] = 1000000000000.0; + odom_.pose.covariance[28] = 1000000000000.0; + odom_.pose.covariance[35] = 0.001; + + // get velocity in /odom frame + math::Vector3 linear; + linear = this->parent->GetWorldLinearVel(); + odom_.twist.twist.angular.z = this->parent->GetWorldAngularVel().z; + + // convert velocity to child_frame_id (aka base_footprint) + float yaw = pose.rot.GetYaw(); + odom_.twist.twist.linear.x = cosf ( yaw ) * linear.x + sinf ( yaw ) * linear.y; + odom_.twist.twist.linear.y = cosf ( yaw ) * linear.y - sinf ( yaw ) * linear.x; + + odom_.header.stamp = current_time; + odom_.header.frame_id = odom_frame; + odom_.child_frame_id = base_footprint_frame; + + odometry_publisher_.publish ( odom_ ); } -GZ_REGISTER_MODEL_PLUGIN(GazeboRosDiffDrive) +GZ_REGISTER_MODEL_PLUGIN ( GazeboRosDiffDrive ) } diff --git a/gazebo_plugins/src/gazebo_ros_gpu_laser.cpp b/gazebo_plugins/src/gazebo_ros_gpu_laser.cpp index 49ecf98c3..49c6c0d3f 100644 --- a/gazebo_plugins/src/gazebo_ros_gpu_laser.cpp +++ b/gazebo_plugins/src/gazebo_ros_gpu_laser.cpp @@ -38,7 +38,7 @@ #include #include "gazebo_plugins/gazebo_ros_gpu_laser.h" -#include +#include namespace gazebo { diff --git a/gazebo_plugins/src/gazebo_ros_laser.cpp b/gazebo_plugins/src/gazebo_ros_laser.cpp index bdbc35ddf..9f099258d 100644 --- a/gazebo_plugins/src/gazebo_ros_laser.cpp +++ b/gazebo_plugins/src/gazebo_ros_laser.cpp @@ -38,7 +38,6 @@ #include #include -#include namespace gazebo { diff --git a/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp b/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp new file mode 100644 index 000000000..2ca75f6a0 --- /dev/null +++ b/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp @@ -0,0 +1,362 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Markus Bader + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/** + * \file gazebo_ros_tricycle_drive.cpp + * \brief A tricycle drive plugin for gazebo. + * \author Markus Bader + * \date 22th of June 2014 + */ + + +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace gazebo +{ + +enum +{ + RIGHT, + LEFT, +}; + +GazeboRosTricycleDrive::GazeboRosTricycleDrive() {} + +// Destructor +GazeboRosTricycleDrive::~GazeboRosTricycleDrive() {} + +// Load the controller +void GazeboRosTricycleDrive::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) +{ + parent = _parent; + gazebo_ros_ = GazeboRosPtr(new GazeboRos(_parent, _sdf, "TricycleDrive")); + // Make sure the ROS node for Gazebo has already been initialized + gazebo_ros_->isInitialized(); + + gazebo_ros_->getParameter(diameter_actuated_wheel_, "actuatedWheelDiameter", 0.15); + gazebo_ros_->getParameter(diameter_encoder_wheel_, "encoderWheelDiameter", 0.15); + gazebo_ros_->getParameter(torque, "torque", 5.0); + gazebo_ros_->getParameter(command_topic_, "commandTopic", "cmd_vel"); + gazebo_ros_->getParameter(odometry_topic_, "odometryTopic", "odom"); + gazebo_ros_->getParameter(odometry_frame_, "odometryFrame", "odom"); + gazebo_ros_->getParameter(robot_base_frame_, "robotBaseFrame", "base_link"); + + gazebo_ros_->getParameter(update_rate_, "updateRate", 100.0); + gazebo_ros_->getParameter(wheel_acceleration, "wheel_acceleration", 0); + gazebo_ros_->getParameter(steering_speed, "steering_speed", 0); + gazebo_ros_->getParameter(separation_encoder_wheel_, "encoderWheelSeparation", 0.5); + + gazebo_ros_->getParameterBoolean(publishWheelTF_, "publishWheelTF", false); + gazebo_ros_->getParameterBoolean(publishWheelJointState_, "publishWheelJointState", false); + + gazebo_ros_->getParameterBoolean(publishWheelJointState_, "publishWheelJointState", false); + + joint_steering_ = gazebo_ros_->getJoint(parent, "steeringJoint", "front_steering_joint"); + joint_wheel_actuated_ = gazebo_ros_->getJoint(parent, "actuatedWheelJoint", "front_wheel_joint"); + joint_wheel_encoder_left_ = gazebo_ros_->getJoint(parent, "encoderWheelLeftJoint", "left_wheel_joint"); + joint_wheel_encoder_right_ = gazebo_ros_->getJoint(parent, "encoderWheelRightJoint", "right_wheel_joint"); + + std::map odomOptions; + odomOptions["encoder"] = ENCODER; + odomOptions["gazebo"] = GAZEBO; + gazebo_ros_->getParameter(odom_source_, "odometrySource", odomOptions, ENCODER); + + joint_wheel_actuated_->SetMaxForce(0, torque); + joint_steering_->SetMaxForce(0, torque); + + + // Initialize update rate stuff + if (this->update_rate_ > 0.0) this->update_period_ = 1.0 / this->update_rate_; + else this->update_period_ = 0.0; + last_actuator_update_ = parent->GetWorld()->GetSimTime(); + + // Initialize velocity stuff + alive_ = true; + + if(this->publishWheelJointState_) + { + joint_state_publisher_ = gazebo_ros_->node()->advertise("joint_states", 1000); + ROS_INFO("%s: Advertise joint_states!", gazebo_ros_->info()); + } + + transform_broadcaster_ = boost::shared_ptr(new tf::TransformBroadcaster()); + + // ROS: Subscribe to the velocity command topic (usually "cmd_vel") + ROS_INFO("%s: Try to subscribe to %s!", gazebo_ros_->info(), command_topic_.c_str()); + + ros::SubscribeOptions so = + ros::SubscribeOptions::create(command_topic_, 1, + boost::bind(&GazeboRosTricycleDrive::cmdVelCallback, this, _1), + ros::VoidPtr(), &queue_); + + cmd_vel_subscriber_ = gazebo_ros_->node()->subscribe(so); + ROS_INFO("%s: Subscribe to %s!", gazebo_ros_->info(), command_topic_.c_str()); + + odometry_publisher_ = gazebo_ros_->node()->advertise(odometry_topic_, 1); + ROS_INFO("%s: Advertise odom on %s !", gazebo_ros_->info(), odometry_topic_.c_str()); + + // start custom queue for diff drive + this->callback_queue_thread_ = boost::thread(boost::bind(&GazeboRosTricycleDrive::QueueThread, this)); + + // listen to the update event (broadcast every simulation iteration) + this->update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosTricycleDrive::UpdateChild, this)); + +} + +void GazeboRosTricycleDrive::publishWheelJointState() +{ + std::vector joints; + joints.push_back(joint_steering_); + joints.push_back(joint_wheel_actuated_); + joints.push_back(joint_wheel_encoder_left_); + joints.push_back(joint_wheel_encoder_right_); + + ros::Time current_time = ros::Time::now(); + joint_state_.header.stamp = current_time; + joint_state_.name.resize ( joints.size() ); + joint_state_.position.resize ( joints.size() ); + for(std::size_t i = 0; i < joints.size(); i++) { + joint_state_.name[i] = joints[i]->GetName(); + joint_state_.position[i] = joints[i]->GetAngle(i).Radian(); + } + joint_state_publisher_.publish ( joint_state_ ); +} + +void GazeboRosTricycleDrive::publishWheelTF() +{ + ros::Time current_time = ros::Time::now(); + std::vector joints; + joints.push_back(joint_steering_); + joints.push_back(joint_wheel_actuated_); + joints.push_back(joint_wheel_encoder_left_); + joints.push_back(joint_wheel_encoder_right_); + for(std::size_t i = 0; i < joints.size(); i++) + { + std::string frame = gazebo_ros_->resolveTF(joints[i]->GetName()); + std::string parent_frame = gazebo_ros_->resolveTF(joints[i]->GetParent()->GetName()); + + math::Pose pose = joints[i]->GetChild()->GetRelativePose(); + + tf::Quaternion qt(pose.rot.x, pose.rot.y, pose.rot.z, pose.rot.w); + tf::Vector3 vt(pose.pos.x, pose.pos.y, pose.pos.z); + + tf::Transform transform(qt, vt); + transform_broadcaster_->sendTransform(tf::StampedTransform(transform, current_time, parent_frame, frame)); + } + +} +// Update the controller +void GazeboRosTricycleDrive::UpdateChild() +{ + if(odom_source_ == ENCODER) UpdateOdometryEncoder(); + common::Time current_time = parent->GetWorld()->GetSimTime(); + double seconds_since_last_update = (current_time - last_actuator_update_).Double(); + if (seconds_since_last_update > update_period_) + { + publishOdometry(seconds_since_last_update); + if(publishWheelTF_) publishWheelTF(); + if(publishWheelJointState_) publishWheelJointState(); + + double current_wheel_speed = joint_wheel_actuated_->GetVelocity(0) * (diameter_actuated_wheel_ / 2.0); + double target_wheel_roation_speed = cmd_.speed / (diameter_actuated_wheel_ / 2.0); + + double target_steering_angle = cmd_.angle; + + joint_wheel_actuated_->SetVelocity(0, target_wheel_roation_speed); + joint_steering_->SetAngle(0, math::Angle(target_steering_angle)); + + + //ROS_INFO("v = %f, w = %f !", target_wheel_roation_speed, target_steering_angle); + + + last_actuator_update_ += common::Time(update_period_); + } +} + +// Finalize the controller +void GazeboRosTricycleDrive::FiniChild() +{ + alive_ = false; + queue_.clear(); + queue_.disable(); + gazebo_ros_->node()->shutdown(); + callback_queue_thread_.join(); +} + +void GazeboRosTricycleDrive::cmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg) +{ + boost::mutex::scoped_lock scoped_lock(lock); + cmd_.speed = cmd_msg->linear.x; + cmd_.angle = cmd_msg->angular.z; +} + +void GazeboRosTricycleDrive::QueueThread() +{ + static const double timeout = 0.01; + + while (alive_ && gazebo_ros_->node()->ok()) + { + queue_.callAvailable(ros::WallDuration(timeout)); + } +} + +void GazeboRosTricycleDrive::UpdateOdometryEncoder() { + double vl = joint_wheel_encoder_left_->GetVelocity(0); + double vr = joint_wheel_encoder_right_->GetVelocity(0); + common::Time current_time = parent->GetWorld()->GetSimTime(); + double seconds_since_last_update = (current_time - last_odom_update_).Double(); + last_odom_update_ = current_time; + + double b = separation_encoder_wheel_; + + // Book: Sigwart 2011 Autonompus Mobile Robots page:337 + double sl = vl * (diameter_encoder_wheel_ / 2.0) * seconds_since_last_update; + double sr = vr * (diameter_encoder_wheel_ / 2.0) * seconds_since_last_update; + double theta = (sl - sr)/b; + + + double dx = (sl + sr)/2.0 * cos(pose_encoder_.theta + (sl - sr)/(2.0*b)); + double dy = (sl + sr)/2.0 * sin(pose_encoder_.theta + (sl - sr)/(2.0*b)); + double dtheta = (sl - sr)/b; + + pose_encoder_.x += dx; + pose_encoder_.y += dy; + pose_encoder_.theta += dtheta; + + double w = dtheta/seconds_since_last_update; + double v = sqrt(dx*dx+dy*dy)/seconds_since_last_update; + + tf::Quaternion qt; + tf::Vector3 vt; + qt.setRPY(0,0,pose_encoder_.theta); + vt = tf::Vector3(pose_encoder_.x, pose_encoder_.y, 0); + + odom_.pose.pose.position.x = vt.x(); + odom_.pose.pose.position.y = vt.y(); + odom_.pose.pose.position.z = vt.z(); + + odom_.pose.pose.orientation.x = qt.x(); + odom_.pose.pose.orientation.y = qt.y(); + odom_.pose.pose.orientation.z = qt.z(); + odom_.pose.pose.orientation.w = qt.w(); + + odom_.twist.twist.angular.z = w; + odom_.twist.twist.linear.x = dx/seconds_since_last_update; + odom_.twist.twist.linear.y = dy/seconds_since_last_update; + + +} + +void GazeboRosTricycleDrive::publishOdometry(double step_time) +{ + + ros::Time current_time = ros::Time::now(); + std::string odom_frame = gazebo_ros_->resolveTF(odometry_frame_); + std::string base_footprint_frame = gazebo_ros_->resolveTF(robot_base_frame_); + + tf::Quaternion qt; + tf::Vector3 vt; + + if(odom_source_ == ENCODER) { + // getting data form encoder integration + qt = tf::Quaternion(odom_.pose.pose.orientation.x, odom_.pose.pose.orientation.y, odom_.pose.pose.orientation.z, odom_.pose.pose.orientation.w); + vt = tf::Vector3(odom_.pose.pose.position.x, odom_.pose.pose.position.y, odom_.pose.pose.position.z); + + } + if(odom_source_ == GAZEBO) { + // getting data form gazebo + math::Pose pose = parent->GetWorldPose(); + qt = tf::Quaternion(pose.rot.x, pose.rot.y, pose.rot.z, pose.rot.w); + vt = tf::Vector3(pose.pos.x, pose.pos.y, pose.pos.z); + + odom_.pose.pose.position.x = vt.x(); + odom_.pose.pose.position.y = vt.y(); + odom_.pose.pose.position.z = vt.z(); + + odom_.pose.pose.orientation.x = qt.x(); + odom_.pose.pose.orientation.y = qt.y(); + odom_.pose.pose.orientation.z = qt.z(); + odom_.pose.pose.orientation.w = qt.w(); + + // get velocity in /odom frame + math::Vector3 linear; + linear = parent->GetWorldLinearVel(); + odom_.twist.twist.angular.z = parent->GetWorldAngularVel().z; + + // convert velocity to child_frame_id (aka base_footprint) + float yaw = pose.rot.GetYaw(); + odom_.twist.twist.linear.x = cosf(yaw) * linear.x + sinf(yaw) * linear.y; + odom_.twist.twist.linear.y = cosf(yaw) * linear.y - sinf(yaw) * linear.x; + } + + tf::Transform base_footprint_to_odom(qt, vt); + transform_broadcaster_->sendTransform( + tf::StampedTransform(base_footprint_to_odom, current_time, + odom_frame, base_footprint_frame)); + + + // set covariance + odom_.pose.covariance[0] = 0.00001; + odom_.pose.covariance[7] = 0.00001; + odom_.pose.covariance[14] = 1000000000000.0; + odom_.pose.covariance[21] = 1000000000000.0; + odom_.pose.covariance[28] = 1000000000000.0; + odom_.pose.covariance[35] = 0.001; + + + // set header + odom_.header.stamp = current_time; + odom_.header.frame_id = odom_frame; + odom_.child_frame_id = base_footprint_frame; + + odometry_publisher_.publish(odom_); +} + +GZ_REGISTER_MODEL_PLUGIN(GazeboRosTricycleDrive) +} + diff --git a/gazebo_plugins/src/gazebo_ros_utils.cpp b/gazebo_plugins/src/gazebo_ros_utils.cpp new file mode 100644 index 000000000..e98ee192c --- /dev/null +++ b/gazebo_plugins/src/gazebo_ros_utils.cpp @@ -0,0 +1,155 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Markus Bader + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + + +#include + +#include +#include + +using namespace gazebo; + +const char* GazeboRos::info() const { + return info_text.c_str(); +} +boost::shared_ptr& GazeboRos ::node() { + return rosnode_; +} +const boost::shared_ptr& GazeboRos ::node() const { + return rosnode_; +} + +std::string GazeboRos ::resolveTF(const std::string &name) { + return tf::resolve(tf_prefix_, name); +} +void GazeboRos ::readCommonParameter() { + + ROS_INFO("Starting plugin %s!", info()); + + std::string debugLevel; + getParameter(debugLevel, "rosDebugLevel", "na"); + if(boost::iequals(debugLevel, std::string("Debug"))) { + if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug) ) { + ros::console::notifyLoggerLevelsChanged(); + } + } else if(boost::iequals(debugLevel, std::string("Info"))) { + if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info) ) { + ros::console::notifyLoggerLevelsChanged(); + } + } else if(boost::iequals(debugLevel, std::string("Warn"))) { + if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Warn) ) { + ros::console::notifyLoggerLevelsChanged(); + } + } else if(boost::iequals(debugLevel, std::string("Error"))) { + if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Error) ) { + ros::console::notifyLoggerLevelsChanged(); + } + } else if(boost::iequals(debugLevel, std::string("Fatal"))) { + if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Fatal) ) { + ros::console::notifyLoggerLevelsChanged(); + } + } + if (sdf_->HasElement("rosDebugLevel")) { + ROS_INFO("%s: = %s", info(), debugLevel.c_str()); + } + + + tf_prefix_ = tf::getPrefixParam(*rosnode_); + if(tf_prefix_.empty()) + { + tf_prefix_ = namespace_; + boost::trim_right_if(tf_prefix_,boost::is_any_of("/")); + } + ROS_INFO("%s: = %s", info(), tf_prefix_.c_str()); +} + + +void GazeboRos ::getParameterBoolean(bool &_value, const char *_tag_name, const bool &_default) { + _value = _default; + if (!sdf_->HasElement(_tag_name)) { + ROS_WARN("%s: missing <%s> default is %s", + info(), _tag_name, (_default?"ture":"false")); + } else { + getParameterBoolean(_value, _tag_name); + } + +} +void GazeboRos ::getParameterBoolean(bool &_value, const char *_tag_name) { + + if (sdf_->HasElement(_tag_name)) { + std::string value = sdf_->GetElement(_tag_name)->Get(); + if(boost::iequals(value, std::string("true"))) + { + _value = true; + } + else if(boost::iequals(value, std::string("false"))) + { + _value = false; + } + else + { + ROS_WARN("%s: <%s> must be either true or false", + info(), _tag_name); + } + } + ROS_DEBUG("%s: <%s> = %s", + info(), _tag_name, (_value?"ture":"false")); + +} + +physics::JointPtr GazeboRos::getJoint(physics::ModelPtr &_parent, const char *_tag_name, const std::string &_joint_default_name) { + std::string joint_name; + getParameter(joint_name, _tag_name, _joint_default_name); + physics::JointPtr joint = _parent->GetJoint(joint_name); + if (!joint) + { + char error[200]; + snprintf(error, 200, + "%s: couldn't get wheel hinge joint named %s", + info() , joint_name.c_str()); + gzthrow(error); + } + return joint; +} + +void GazeboRos::isInitialized() { + if (!ros::isInitialized()) + { + ROS_FATAL_STREAM(info() << "A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; + } +} + + + diff --git a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx.xacro b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx.xacro index 5b2a09fc5..9b1fd95bd 100644 --- a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx.xacro +++ b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx.xacro @@ -8,15 +8,15 @@ - - --> + - + diff --git a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_plugins.xacro b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_plugins.xacro index f1a61b916..7bfe2dedd 100644 --- a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_plugins.xacro +++ b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_plugins.xacro @@ -15,6 +15,7 @@ + Debug false true diff --git a/gazebo_plugins/test/tricycle_drive/launch/.directory b/gazebo_plugins/test/tricycle_drive/launch/.directory new file mode 100644 index 000000000..b0371cd66 --- /dev/null +++ b/gazebo_plugins/test/tricycle_drive/launch/.directory @@ -0,0 +1,4 @@ +[Dolphin] +Timestamp=2013,11,4,9,14,4 +Version=3 +ViewMode=2 diff --git a/gazebo_plugins/test/tricycle_drive/launch/tricycle.gazebo.launch b/gazebo_plugins/test/tricycle_drive/launch/tricycle.gazebo.launch new file mode 100644 index 000000000..1673b7882 --- /dev/null +++ b/gazebo_plugins/test/tricycle_drive/launch/tricycle.gazebo.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/gazebo_plugins/test/tricycle_drive/launch/tricycle.rviz b/gazebo_plugins/test/tricycle_drive/launch/tricycle.rviz new file mode 100644 index 000000000..49c0fc84f --- /dev/null +++ b/gazebo_plugins/test/tricycle_drive/launch/tricycle.rviz @@ -0,0 +1,237 @@ +Panels: + - Class: rviz/Displays + Help Height: 84 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Marker1 + - /Marker1/Namespaces1 + - /LaserScan1 + Splitter Ratio: 0.590385 + Tree Height: 690 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + base_link: + Value: true + r1/base_link: + Value: true + r1/chassis: + Value: true + r1/front_steering: + Value: true + r1/front_wheel: + Value: true + r1/left_wheel: + Value: true + r1/odom: + Value: true + r1/right_wheel: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + base_link: + r1/odom: + r1/base_link: + r1/chassis: + {} + r1/front_steering: + r1/front_wheel: + {} + r1/left_wheel: + {} + r1/right_wheel: + {} + Update Interval: 0 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: visualization_marker + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + chassis: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_steering: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: r1/robot_description + TF Prefix: r1 + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Flat Squares + Topic: /r1/front_laser/scan + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: r1/base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 7.43455 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.351517 + Y: 0.0334811 + Z: 0.722431 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.234797 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.99819 + Saved: ~ +Window Geometry: + Camera: + collapsed: false + Displays: + collapsed: false + Height: 1054 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000010f0000036bfc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000450000034c000000ee00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610100000397000000190000001900000019fb0000000c00430061006d00650072006103000003360000023100000250000001b3fb0000000c00430061006d00650072006101000002c5000000c80000000000000000000000010000010f0000036bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007301000000450000036b000000c100fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000068e00000046fc0100000002fb0000000800540069006d006501000000000000068e0000038b00fffffffb0000000800540069006d00650100000000000004500000000000000000000004640000036b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1678 + X: 1920 + Y: 0 diff --git a/gazebo_plugins/test/tricycle_drive/launch/tricycle.urdf.launch b/gazebo_plugins/test/tricycle_drive/launch/tricycle.urdf.launch new file mode 100644 index 000000000..c909ff523 --- /dev/null +++ b/gazebo_plugins/test/tricycle_drive/launch/tricycle.urdf.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/gazebo_plugins/test/tricycle_drive/launch/tricycle_drive_scenario.launch b/gazebo_plugins/test/tricycle_drive/launch/tricycle_drive_scenario.launch new file mode 100644 index 000000000..e2fd4057c --- /dev/null +++ b/gazebo_plugins/test/tricycle_drive/launch/tricycle_drive_scenario.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + diff --git a/gazebo_plugins/test/tricycle_drive/launch/tricycle_rviz.launch b/gazebo_plugins/test/tricycle_drive/launch/tricycle_rviz.launch new file mode 100644 index 000000000..d2d5b2d8e --- /dev/null +++ b/gazebo_plugins/test/tricycle_drive/launch/tricycle_rviz.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/gazebo_plugins/test/tricycle_drive/xacro/.directory b/gazebo_plugins/test/tricycle_drive/xacro/.directory new file mode 100644 index 000000000..b41f2a18a --- /dev/null +++ b/gazebo_plugins/test/tricycle_drive/xacro/.directory @@ -0,0 +1,3 @@ +[Dolphin] +Timestamp=2013,10,18,16,23,17 +ViewMode=2 diff --git a/gazebo_plugins/test/tricycle_drive/xacro/materials.xacro b/gazebo_plugins/test/tricycle_drive/xacro/materials.xacro new file mode 100644 index 000000000..f260fff42 --- /dev/null +++ b/gazebo_plugins/test/tricycle_drive/xacro/materials.xacro @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gazebo_plugins/test/tricycle_drive/xacro/tricycle/inertia_tensors.xacro b/gazebo_plugins/test/tricycle_drive/xacro/tricycle/inertia_tensors.xacro new file mode 100644 index 000000000..15874715a --- /dev/null +++ b/gazebo_plugins/test/tricycle_drive/xacro/tricycle/inertia_tensors.xacro @@ -0,0 +1,72 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/gazebo_plugins/test/tricycle_drive/xacro/tricycle/tricycle.xacro b/gazebo_plugins/test/tricycle_drive/xacro/tricycle/tricycle.xacro new file mode 100644 index 000000000..81119279e --- /dev/null +++ b/gazebo_plugins/test/tricycle_drive/xacro/tricycle/tricycle.xacro @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/gazebo_plugins/test/tricycle_drive/xacro/tricycle/tricycle_body.xacro b/gazebo_plugins/test/tricycle_drive/xacro/tricycle/tricycle_body.xacro new file mode 100644 index 000000000..6c9371ca7 --- /dev/null +++ b/gazebo_plugins/test/tricycle_drive/xacro/tricycle/tricycle_body.xacro @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gazebo_plugins/test/tricycle_drive/xacro/tricycle/tricycle_chassis.xacro b/gazebo_plugins/test/tricycle_drive/xacro/tricycle/tricycle_chassis.xacro new file mode 100644 index 000000000..1feeaf799 --- /dev/null +++ b/gazebo_plugins/test/tricycle_drive/xacro/tricycle/tricycle_chassis.xacro @@ -0,0 +1,37 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/gazebo_plugins/test/tricycle_drive/xacro/tricycle/tricycle_plugins.xacro b/gazebo_plugins/test/tricycle_drive/xacro/tricycle/tricycle_plugins.xacro new file mode 100644 index 000000000..1c7f26ce0 --- /dev/null +++ b/gazebo_plugins/test/tricycle_drive/xacro/tricycle/tricycle_plugins.xacro @@ -0,0 +1,33 @@ + + + + + + + + Debug + + false + true + true + front_steering_joint + front_wheel_joint + left_wheel_joint + right_wheel_joint + 0.135 + 0.135 + 0.548 + 20 + cmd_vel + odom + odom + encoder + base_link + 10.0 + 1.8 + 1.8 + + + + + diff --git a/gazebo_plugins/test/tricycle_drive/xacro/tricycle/wheel.xacro b/gazebo_plugins/test/tricycle_drive/xacro/tricycle/wheel.xacro new file mode 100644 index 000000000..691aa0c0c --- /dev/null +++ b/gazebo_plugins/test/tricycle_drive/xacro/tricycle/wheel.xacro @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gazebo_plugins/test/tricycle_drive/xacro/tricycle/wheel_actuated.xacro b/gazebo_plugins/test/tricycle_drive/xacro/tricycle/wheel_actuated.xacro new file mode 100644 index 000000000..0acacc810 --- /dev/null +++ b/gazebo_plugins/test/tricycle_drive/xacro/tricycle/wheel_actuated.xacro @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From c2f4dbea0e27a841e66da9056612c9ec3bad4dca Mon Sep 17 00:00:00 2001 From: Markus Bader Date: Mon, 23 Jun 2014 13:37:58 +0200 Subject: [PATCH 108/153] second robot for testing in tricycle_drive_scenario.launch added --- .../tricycle_drive/launch/tricycle_drive_scenario.launch | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/gazebo_plugins/test/tricycle_drive/launch/tricycle_drive_scenario.launch b/gazebo_plugins/test/tricycle_drive/launch/tricycle_drive_scenario.launch index e2fd4057c..2723ccd65 100644 --- a/gazebo_plugins/test/tricycle_drive/launch/tricycle_drive_scenario.launch +++ b/gazebo_plugins/test/tricycle_drive/launch/tricycle_drive_scenario.launch @@ -13,4 +13,13 @@ + + + + + + + + From 923f8848ecc7423d5f423fa896745210b6c5777e Mon Sep 17 00:00:00 2001 From: Markus Bader Date: Mon, 23 Jun 2014 15:27:09 +0200 Subject: [PATCH 109/153] simple linear controller for the tricycle_drive added --- .../gazebo_plugins/gazebo_ros_diff_drive.h | 4 +- .../gazebo_ros_tricycle_drive.h | 10 +- gazebo_plugins/src/gazebo_ros_diff_drive.cpp | 18 +- .../src/gazebo_ros_tricycle_drive.cpp | 259 ++++++++++-------- .../xacro/p3dx/pioneer3dx_plugins.xacro | 4 +- .../launch/tricycle_drive_scenario.launch | 36 ++- .../xacro/tricycle/tricycle_plugins.xacro | 9 +- 7 files changed, 197 insertions(+), 143 deletions(-) diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_diff_drive.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_diff_drive.h index 747d68f09..3ef3b2075 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_diff_drive.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_diff_drive.h @@ -94,9 +94,9 @@ namespace gazebo { double wheel_separation_; double wheel_diameter_; - double torque; + double wheel_torque; double wheel_speed_[2]; - double max_accel; + double wheel_accel; double wheel_speed_instr_[2]; std::vector joints_; diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_tricycle_drive.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_tricycle_drive.h index d7bd593f8..553120eb2 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_tricycle_drive.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_tricycle_drive.h @@ -99,6 +99,7 @@ class GazeboRosTricycleDrive : public ModelPlugin { void publishOdometry(double step_time); void publishWheelTF(); /// publishes the wheel tf's void publishWheelJointState(); + void motorController(double target_speed, double target_angle, double dt); event::ConnectionPtr update_connection_; @@ -109,12 +110,15 @@ class GazeboRosTricycleDrive : public ModelPlugin { double diameter_encoder_wheel_; double diameter_actuated_wheel_; - double wheel_acceleration; - double steering_speed; + double wheel_acceleration_; + double wheel_deceleration_; + double wheel_speed_tolerance_; + double steering_angle_tolerance_; + double steering_speed_; double separation_encoder_wheel_; OdomSource odom_source_; - double torque; + double wheel_torque_; std::string robot_namespace_; std::string command_topic_; diff --git a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp index d56d49033..dc9c6f6be 100644 --- a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp +++ b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp @@ -95,16 +95,16 @@ void GazeboRosDiffDrive::Load ( physics::ModelPtr _parent, sdf::ElementPtr _sdf gazebo_ros_->getParameter ( wheel_separation_, "wheelSeparation", 0.34 ); gazebo_ros_->getParameter ( wheel_diameter_, "wheelDiameter", 0.15 ); - gazebo_ros_->getParameter ( torque, "torque", 5.0 ); + gazebo_ros_->getParameter ( wheel_accel, "wheelAcceleration", 0.0 ); + gazebo_ros_->getParameter ( wheel_torque, "wheelTorque", 5.0 ); gazebo_ros_->getParameter ( update_rate_, "updateRate", 100.0 ); - gazebo_ros_->getParameter ( max_accel, "max_accel", 0.0 ); joints_.resize ( 2 ); joints_[LEFT] = gazebo_ros_->getJoint ( parent, "leftJoint", "left_joint" ); joints_[RIGHT] = gazebo_ros_->getJoint ( parent, "rightJoint", "right_joint" ); - joints_[LEFT]->SetMaxForce ( 0, torque ); - joints_[RIGHT]->SetMaxForce ( 0, torque ); + joints_[LEFT]->SetMaxForce ( 0, wheel_torque ); + joints_[RIGHT]->SetMaxForce ( 0, wheel_torque ); @@ -208,7 +208,7 @@ void GazeboRosDiffDrive::UpdateChild() current_speed[LEFT] = joints_[LEFT]->GetVelocity ( 0 ) * ( wheel_diameter_ / 2.0 ); current_speed[RIGHT] = joints_[RIGHT]->GetVelocity ( 0 ) * ( wheel_diameter_ / 2.0 ); - if ( max_accel == 0 || + if ( wheel_accel == 0 || ( fabs ( wheel_speed_[LEFT] - current_speed[LEFT] ) < 0.01 ) || ( fabs ( wheel_speed_[RIGHT] - current_speed[RIGHT] ) < 0.01 ) ) { //if max_accel == 0, or target speed is reached @@ -216,14 +216,14 @@ void GazeboRosDiffDrive::UpdateChild() joints_[RIGHT]->SetVelocity ( 0, wheel_speed_[RIGHT]/ ( wheel_diameter_ / 2.0 ) ); } else { if ( wheel_speed_[LEFT]>=current_speed[LEFT] ) - wheel_speed_instr_[LEFT]+=fmin ( wheel_speed_[LEFT]-current_speed[LEFT], max_accel * seconds_since_last_update ); + wheel_speed_instr_[LEFT]+=fmin ( wheel_speed_[LEFT]-current_speed[LEFT], wheel_accel * seconds_since_last_update ); else - wheel_speed_instr_[LEFT]+=fmax ( wheel_speed_[LEFT]-current_speed[LEFT], -max_accel * seconds_since_last_update ); + wheel_speed_instr_[LEFT]+=fmax ( wheel_speed_[LEFT]-current_speed[LEFT], -wheel_accel * seconds_since_last_update ); if ( wheel_speed_[RIGHT]>current_speed[RIGHT] ) - wheel_speed_instr_[RIGHT]+=fmin ( wheel_speed_[RIGHT]-current_speed[RIGHT], max_accel * seconds_since_last_update ); + wheel_speed_instr_[RIGHT]+=fmin ( wheel_speed_[RIGHT]-current_speed[RIGHT], wheel_accel * seconds_since_last_update ); else - wheel_speed_instr_[RIGHT]+=fmax ( wheel_speed_[RIGHT]-current_speed[RIGHT], -max_accel * seconds_since_last_update ); + wheel_speed_instr_[RIGHT]+=fmax ( wheel_speed_[RIGHT]-current_speed[RIGHT], -wheel_accel * seconds_since_last_update ); // ROS_INFO("actual wheel speed = %lf, issued wheel speed= %lf", current_speed[LEFT], wheel_speed_[LEFT]); // ROS_INFO("actual wheel speed = %lf, issued wheel speed= %lf", current_speed[RIGHT],wheel_speed_[RIGHT]); diff --git a/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp b/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp index 2ca75f6a0..a8c56ce60 100644 --- a/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp +++ b/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp @@ -14,8 +14,8 @@ * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT @@ -59,8 +59,7 @@ namespace gazebo { -enum -{ +enum { RIGHT, LEFT, }; @@ -71,98 +70,104 @@ GazeboRosTricycleDrive::GazeboRosTricycleDrive() {} GazeboRosTricycleDrive::~GazeboRosTricycleDrive() {} // Load the controller -void GazeboRosTricycleDrive::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) +void GazeboRosTricycleDrive::Load ( physics::ModelPtr _parent, sdf::ElementPtr _sdf ) { parent = _parent; - gazebo_ros_ = GazeboRosPtr(new GazeboRos(_parent, _sdf, "TricycleDrive")); + gazebo_ros_ = GazeboRosPtr ( new GazeboRos ( _parent, _sdf, "TricycleDrive" ) ); // Make sure the ROS node for Gazebo has already been initialized gazebo_ros_->isInitialized(); - gazebo_ros_->getParameter(diameter_actuated_wheel_, "actuatedWheelDiameter", 0.15); - gazebo_ros_->getParameter(diameter_encoder_wheel_, "encoderWheelDiameter", 0.15); - gazebo_ros_->getParameter(torque, "torque", 5.0); - gazebo_ros_->getParameter(command_topic_, "commandTopic", "cmd_vel"); - gazebo_ros_->getParameter(odometry_topic_, "odometryTopic", "odom"); - gazebo_ros_->getParameter(odometry_frame_, "odometryFrame", "odom"); - gazebo_ros_->getParameter(robot_base_frame_, "robotBaseFrame", "base_link"); + gazebo_ros_->getParameter ( diameter_actuated_wheel_, "actuatedWheelDiameter", 0.15 ); + gazebo_ros_->getParameter ( diameter_encoder_wheel_, "encoderWheelDiameter", 0.15 ); + gazebo_ros_->getParameter ( wheel_torque_, "wheelTorque", 5.0 ); + gazebo_ros_->getParameter ( command_topic_, "commandTopic", "cmd_vel" ); + gazebo_ros_->getParameter ( odometry_topic_, "odometryTopic", "odom" ); + gazebo_ros_->getParameter ( odometry_frame_, "odometryFrame", "odom" ); + gazebo_ros_->getParameter ( robot_base_frame_, "robotBaseFrame", "base_link" ); - gazebo_ros_->getParameter(update_rate_, "updateRate", 100.0); - gazebo_ros_->getParameter(wheel_acceleration, "wheel_acceleration", 0); - gazebo_ros_->getParameter(steering_speed, "steering_speed", 0); - gazebo_ros_->getParameter(separation_encoder_wheel_, "encoderWheelSeparation", 0.5); + gazebo_ros_->getParameter ( update_rate_, "updateRate", 100.0 ); + gazebo_ros_->getParameter ( wheel_acceleration_, "wheelAcceleration", 0 ); + gazebo_ros_->getParameter ( wheel_deceleration_, "wheelDeceleration", wheel_acceleration_ ); + gazebo_ros_->getParameter ( wheel_speed_tolerance_, "wheelSpeedTolerance", 0.01 ); + gazebo_ros_->getParameter ( steering_speed_, "steeringSpeed", 0 ); + gazebo_ros_->getParameter ( steering_angle_tolerance_, "steeringAngleTolerance", 0.01 ); + gazebo_ros_->getParameter ( separation_encoder_wheel_, "encoderWheelSeparation", 0.5 ); - gazebo_ros_->getParameterBoolean(publishWheelTF_, "publishWheelTF", false); - gazebo_ros_->getParameterBoolean(publishWheelJointState_, "publishWheelJointState", false); + gazebo_ros_->getParameterBoolean ( publishWheelTF_, "publishWheelTF", false ); + gazebo_ros_->getParameterBoolean ( publishWheelJointState_, "publishWheelJointState", false ); - gazebo_ros_->getParameterBoolean(publishWheelJointState_, "publishWheelJointState", false); + gazebo_ros_->getParameterBoolean ( publishWheelJointState_, "publishWheelJointState", false ); - joint_steering_ = gazebo_ros_->getJoint(parent, "steeringJoint", "front_steering_joint"); - joint_wheel_actuated_ = gazebo_ros_->getJoint(parent, "actuatedWheelJoint", "front_wheel_joint"); - joint_wheel_encoder_left_ = gazebo_ros_->getJoint(parent, "encoderWheelLeftJoint", "left_wheel_joint"); - joint_wheel_encoder_right_ = gazebo_ros_->getJoint(parent, "encoderWheelRightJoint", "right_wheel_joint"); + joint_steering_ = gazebo_ros_->getJoint ( parent, "steeringJoint", "front_steering_joint" ); + joint_wheel_actuated_ = gazebo_ros_->getJoint ( parent, "actuatedWheelJoint", "front_wheel_joint" ); + joint_wheel_encoder_left_ = gazebo_ros_->getJoint ( parent, "encoderWheelLeftJoint", "left_wheel_joint" ); + joint_wheel_encoder_right_ = gazebo_ros_->getJoint ( parent, "encoderWheelRightJoint", "right_wheel_joint" ); std::map odomOptions; odomOptions["encoder"] = ENCODER; odomOptions["gazebo"] = GAZEBO; - gazebo_ros_->getParameter(odom_source_, "odometrySource", odomOptions, ENCODER); + gazebo_ros_->getParameter ( odom_source_, "odometrySource", odomOptions, ENCODER ); - joint_wheel_actuated_->SetMaxForce(0, torque); - joint_steering_->SetMaxForce(0, torque); + joint_wheel_actuated_->SetMaxForce ( 0, wheel_torque_ ); + joint_steering_->SetMaxForce ( 0, wheel_torque_ ); // Initialize update rate stuff - if (this->update_rate_ > 0.0) this->update_period_ = 1.0 / this->update_rate_; + if ( this->update_rate_ > 0.0 ) this->update_period_ = 1.0 / this->update_rate_; else this->update_period_ = 0.0; last_actuator_update_ = parent->GetWorld()->GetSimTime(); // Initialize velocity stuff alive_ = true; - if(this->publishWheelJointState_) - { - joint_state_publisher_ = gazebo_ros_->node()->advertise("joint_states", 1000); - ROS_INFO("%s: Advertise joint_states!", gazebo_ros_->info()); + if ( this->publishWheelJointState_ ) { + joint_state_publisher_ = gazebo_ros_->node()->advertise ( "joint_states", 1000 ); + ROS_INFO ( "%s: Advertise joint_states!", gazebo_ros_->info() ); } - transform_broadcaster_ = boost::shared_ptr(new tf::TransformBroadcaster()); + transform_broadcaster_ = boost::shared_ptr ( new tf::TransformBroadcaster() ); // ROS: Subscribe to the velocity command topic (usually "cmd_vel") - ROS_INFO("%s: Try to subscribe to %s!", gazebo_ros_->info(), command_topic_.c_str()); + ROS_INFO ( "%s: Try to subscribe to %s!", gazebo_ros_->info(), command_topic_.c_str() ); ros::SubscribeOptions so = - ros::SubscribeOptions::create(command_topic_, 1, - boost::bind(&GazeboRosTricycleDrive::cmdVelCallback, this, _1), - ros::VoidPtr(), &queue_); + ros::SubscribeOptions::create ( command_topic_, 1, + boost::bind ( &GazeboRosTricycleDrive::cmdVelCallback, this, _1 ), + ros::VoidPtr(), &queue_ ); - cmd_vel_subscriber_ = gazebo_ros_->node()->subscribe(so); - ROS_INFO("%s: Subscribe to %s!", gazebo_ros_->info(), command_topic_.c_str()); + cmd_vel_subscriber_ = gazebo_ros_->node()->subscribe ( so ); + ROS_INFO ( "%s: Subscribe to %s!", gazebo_ros_->info(), command_topic_.c_str() ); - odometry_publisher_ = gazebo_ros_->node()->advertise(odometry_topic_, 1); - ROS_INFO("%s: Advertise odom on %s !", gazebo_ros_->info(), odometry_topic_.c_str()); + odometry_publisher_ = gazebo_ros_->node()->advertise ( odometry_topic_, 1 ); + ROS_INFO ( "%s: Advertise odom on %s !", gazebo_ros_->info(), odometry_topic_.c_str() ); // start custom queue for diff drive - this->callback_queue_thread_ = boost::thread(boost::bind(&GazeboRosTricycleDrive::QueueThread, this)); + this->callback_queue_thread_ = boost::thread ( boost::bind ( &GazeboRosTricycleDrive::QueueThread, this ) ); // listen to the update event (broadcast every simulation iteration) - this->update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosTricycleDrive::UpdateChild, this)); + this->update_connection_ = event::Events::ConnectWorldUpdateBegin ( boost::bind ( &GazeboRosTricycleDrive::UpdateChild, this ) ); } void GazeboRosTricycleDrive::publishWheelJointState() { std::vector joints; - joints.push_back(joint_steering_); - joints.push_back(joint_wheel_actuated_); - joints.push_back(joint_wheel_encoder_left_); - joints.push_back(joint_wheel_encoder_right_); + joints.push_back ( joint_steering_ ); + joints.push_back ( joint_wheel_actuated_ ); + joints.push_back ( joint_wheel_encoder_left_ ); + joints.push_back ( joint_wheel_encoder_right_ ); ros::Time current_time = ros::Time::now(); joint_state_.header.stamp = current_time; joint_state_.name.resize ( joints.size() ); joint_state_.position.resize ( joints.size() ); - for(std::size_t i = 0; i < joints.size(); i++) { + joint_state_.velocity.resize ( joints.size() ); + joint_state_.effort.resize ( joints.size() ); + for ( std::size_t i = 0; i < joints.size(); i++ ) { joint_state_.name[i] = joints[i]->GetName(); - joint_state_.position[i] = joints[i]->GetAngle(i).Radian(); + joint_state_.position[i] = joints[i]->GetAngle ( 0 ).Radian(); + joint_state_.velocity[i] = joints[i]->GetVelocity ( 0 ); + joint_state_.effort[i] = joints[i]->GetForce ( 0 ); } joint_state_publisher_.publish ( joint_state_ ); } @@ -171,51 +176,87 @@ void GazeboRosTricycleDrive::publishWheelTF() { ros::Time current_time = ros::Time::now(); std::vector joints; - joints.push_back(joint_steering_); - joints.push_back(joint_wheel_actuated_); - joints.push_back(joint_wheel_encoder_left_); - joints.push_back(joint_wheel_encoder_right_); - for(std::size_t i = 0; i < joints.size(); i++) - { - std::string frame = gazebo_ros_->resolveTF(joints[i]->GetName()); - std::string parent_frame = gazebo_ros_->resolveTF(joints[i]->GetParent()->GetName()); + joints.push_back ( joint_steering_ ); + joints.push_back ( joint_wheel_actuated_ ); + joints.push_back ( joint_wheel_encoder_left_ ); + joints.push_back ( joint_wheel_encoder_right_ ); + for ( std::size_t i = 0; i < joints.size(); i++ ) { + std::string frame = gazebo_ros_->resolveTF ( joints[i]->GetName() ); + std::string parent_frame = gazebo_ros_->resolveTF ( joints[i]->GetParent()->GetName() ); math::Pose pose = joints[i]->GetChild()->GetRelativePose(); - tf::Quaternion qt(pose.rot.x, pose.rot.y, pose.rot.z, pose.rot.w); - tf::Vector3 vt(pose.pos.x, pose.pos.y, pose.pos.z); + tf::Quaternion qt ( pose.rot.x, pose.rot.y, pose.rot.z, pose.rot.w ); + tf::Vector3 vt ( pose.pos.x, pose.pos.y, pose.pos.z ); - tf::Transform transform(qt, vt); - transform_broadcaster_->sendTransform(tf::StampedTransform(transform, current_time, parent_frame, frame)); + tf::Transform transform ( qt, vt ); + transform_broadcaster_->sendTransform ( tf::StampedTransform ( transform, current_time, parent_frame, frame ) ); } } // Update the controller void GazeboRosTricycleDrive::UpdateChild() { - if(odom_source_ == ENCODER) UpdateOdometryEncoder(); + if ( odom_source_ == ENCODER ) UpdateOdometryEncoder(); common::Time current_time = parent->GetWorld()->GetSimTime(); - double seconds_since_last_update = (current_time - last_actuator_update_).Double(); - if (seconds_since_last_update > update_period_) - { - publishOdometry(seconds_since_last_update); - if(publishWheelTF_) publishWheelTF(); - if(publishWheelJointState_) publishWheelJointState(); + double seconds_since_last_update = ( current_time - last_actuator_update_ ).Double(); + if ( seconds_since_last_update > update_period_ ) { - double current_wheel_speed = joint_wheel_actuated_->GetVelocity(0) * (diameter_actuated_wheel_ / 2.0); - double target_wheel_roation_speed = cmd_.speed / (diameter_actuated_wheel_ / 2.0); + publishOdometry ( seconds_since_last_update ); + if ( publishWheelTF_ ) publishWheelTF(); + if ( publishWheelJointState_ ) publishWheelJointState(); + double target_wheel_roation_speed = cmd_.speed / ( diameter_actuated_wheel_ / 2.0 ); double target_steering_angle = cmd_.angle; - joint_wheel_actuated_->SetVelocity(0, target_wheel_roation_speed); - joint_steering_->SetAngle(0, math::Angle(target_steering_angle)); - + motorController ( target_wheel_roation_speed, target_steering_angle, seconds_since_last_update ); //ROS_INFO("v = %f, w = %f !", target_wheel_roation_speed, target_steering_angle); + last_actuator_update_ += common::Time ( update_period_ ); + } +} + - last_actuator_update_ += common::Time(update_period_); +void GazeboRosTricycleDrive::motorController ( double target_speed, double target_angle, double dt ) +{ + double applied_speed = target_speed; + double applied_angle = target_angle; + double applied_steering_speed = 0; + + double current_speed = joint_wheel_actuated_->GetVelocity ( 0 ); + if ( wheel_acceleration_ > 0 ) { + double diff_speed = current_speed - target_speed; + if ( fabs ( diff_speed ) < wheel_speed_tolerance_ ) { + applied_speed = target_speed; + } else if ( diff_speed < target_speed ) { + applied_speed = current_speed + wheel_acceleration_ * dt; + } else { + applied_speed = current_speed - wheel_deceleration_ * dt; + } + } + joint_wheel_actuated_->SetVelocity ( 0, applied_speed ); + + double current_angle = joint_steering_->GetAngle ( 0 ).Radian(); + if(target_angle > +M_PI / 2.0) target_angle = +M_PI / 2.0; + if(target_angle < -M_PI / 2.0) target_angle = -M_PI / 2.0; + if ( steering_speed_ > 0 ) { + double diff_angle = current_angle - target_angle; + if ( fabs ( diff_angle ) < steering_angle_tolerance_ ) { + applied_steering_speed = 0; + } else if ( diff_angle < target_speed ) { + applied_steering_speed = steering_speed_; + } else { + applied_steering_speed = -steering_speed_; + } + joint_steering_->SetVelocity ( 0, applied_steering_speed ); + }else { + joint_steering_->SetAngle ( 0, math::Angle ( applied_angle ) ); } + //ROS_INFO ( "target: [%3.2f, %3.2f], current: [%3.2f, %3.2f], applied: [%3.2f, %3.2f/%3.2f] !", + // target_speed, target_angle, current_speed, current_angle, applied_speed, applied_angle, applied_steering_speed ); + + } // Finalize the controller @@ -228,9 +269,9 @@ void GazeboRosTricycleDrive::FiniChild() callback_queue_thread_.join(); } -void GazeboRosTricycleDrive::cmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg) +void GazeboRosTricycleDrive::cmdVelCallback ( const geometry_msgs::Twist::ConstPtr& cmd_msg ) { - boost::mutex::scoped_lock scoped_lock(lock); + boost::mutex::scoped_lock scoped_lock ( lock ); cmd_.speed = cmd_msg->linear.x; cmd_.angle = cmd_msg->angular.z; } @@ -239,42 +280,42 @@ void GazeboRosTricycleDrive::QueueThread() { static const double timeout = 0.01; - while (alive_ && gazebo_ros_->node()->ok()) - { - queue_.callAvailable(ros::WallDuration(timeout)); + while ( alive_ && gazebo_ros_->node()->ok() ) { + queue_.callAvailable ( ros::WallDuration ( timeout ) ); } } -void GazeboRosTricycleDrive::UpdateOdometryEncoder() { - double vl = joint_wheel_encoder_left_->GetVelocity(0); - double vr = joint_wheel_encoder_right_->GetVelocity(0); +void GazeboRosTricycleDrive::UpdateOdometryEncoder() +{ + double vl = joint_wheel_encoder_left_->GetVelocity ( 0 ); + double vr = joint_wheel_encoder_right_->GetVelocity ( 0 ); common::Time current_time = parent->GetWorld()->GetSimTime(); - double seconds_since_last_update = (current_time - last_odom_update_).Double(); + double seconds_since_last_update = ( current_time - last_odom_update_ ).Double(); last_odom_update_ = current_time; double b = separation_encoder_wheel_; // Book: Sigwart 2011 Autonompus Mobile Robots page:337 - double sl = vl * (diameter_encoder_wheel_ / 2.0) * seconds_since_last_update; - double sr = vr * (diameter_encoder_wheel_ / 2.0) * seconds_since_last_update; - double theta = (sl - sr)/b; + double sl = vl * ( diameter_encoder_wheel_ / 2.0 ) * seconds_since_last_update; + double sr = vr * ( diameter_encoder_wheel_ / 2.0 ) * seconds_since_last_update; + double theta = ( sl - sr ) /b; - double dx = (sl + sr)/2.0 * cos(pose_encoder_.theta + (sl - sr)/(2.0*b)); - double dy = (sl + sr)/2.0 * sin(pose_encoder_.theta + (sl - sr)/(2.0*b)); - double dtheta = (sl - sr)/b; + double dx = ( sl + sr ) /2.0 * cos ( pose_encoder_.theta + ( sl - sr ) / ( 2.0*b ) ); + double dy = ( sl + sr ) /2.0 * sin ( pose_encoder_.theta + ( sl - sr ) / ( 2.0*b ) ); + double dtheta = ( sl - sr ) /b; pose_encoder_.x += dx; pose_encoder_.y += dy; pose_encoder_.theta += dtheta; double w = dtheta/seconds_since_last_update; - double v = sqrt(dx*dx+dy*dy)/seconds_since_last_update; + double v = sqrt ( dx*dx+dy*dy ) /seconds_since_last_update; tf::Quaternion qt; tf::Vector3 vt; - qt.setRPY(0,0,pose_encoder_.theta); - vt = tf::Vector3(pose_encoder_.x, pose_encoder_.y, 0); + qt.setRPY ( 0,0,pose_encoder_.theta ); + vt = tf::Vector3 ( pose_encoder_.x, pose_encoder_.y, 0 ); odom_.pose.pose.position.x = vt.x(); odom_.pose.pose.position.y = vt.y(); @@ -292,27 +333,27 @@ void GazeboRosTricycleDrive::UpdateOdometryEncoder() { } -void GazeboRosTricycleDrive::publishOdometry(double step_time) +void GazeboRosTricycleDrive::publishOdometry ( double step_time ) { ros::Time current_time = ros::Time::now(); - std::string odom_frame = gazebo_ros_->resolveTF(odometry_frame_); - std::string base_footprint_frame = gazebo_ros_->resolveTF(robot_base_frame_); + std::string odom_frame = gazebo_ros_->resolveTF ( odometry_frame_ ); + std::string base_footprint_frame = gazebo_ros_->resolveTF ( robot_base_frame_ ); tf::Quaternion qt; tf::Vector3 vt; - if(odom_source_ == ENCODER) { + if ( odom_source_ == ENCODER ) { // getting data form encoder integration - qt = tf::Quaternion(odom_.pose.pose.orientation.x, odom_.pose.pose.orientation.y, odom_.pose.pose.orientation.z, odom_.pose.pose.orientation.w); - vt = tf::Vector3(odom_.pose.pose.position.x, odom_.pose.pose.position.y, odom_.pose.pose.position.z); + qt = tf::Quaternion ( odom_.pose.pose.orientation.x, odom_.pose.pose.orientation.y, odom_.pose.pose.orientation.z, odom_.pose.pose.orientation.w ); + vt = tf::Vector3 ( odom_.pose.pose.position.x, odom_.pose.pose.position.y, odom_.pose.pose.position.z ); } - if(odom_source_ == GAZEBO) { + if ( odom_source_ == GAZEBO ) { // getting data form gazebo math::Pose pose = parent->GetWorldPose(); - qt = tf::Quaternion(pose.rot.x, pose.rot.y, pose.rot.z, pose.rot.w); - vt = tf::Vector3(pose.pos.x, pose.pos.y, pose.pos.z); + qt = tf::Quaternion ( pose.rot.x, pose.rot.y, pose.rot.z, pose.rot.w ); + vt = tf::Vector3 ( pose.pos.x, pose.pos.y, pose.pos.z ); odom_.pose.pose.position.x = vt.x(); odom_.pose.pose.position.y = vt.y(); @@ -330,14 +371,14 @@ void GazeboRosTricycleDrive::publishOdometry(double step_time) // convert velocity to child_frame_id (aka base_footprint) float yaw = pose.rot.GetYaw(); - odom_.twist.twist.linear.x = cosf(yaw) * linear.x + sinf(yaw) * linear.y; - odom_.twist.twist.linear.y = cosf(yaw) * linear.y - sinf(yaw) * linear.x; + odom_.twist.twist.linear.x = cosf ( yaw ) * linear.x + sinf ( yaw ) * linear.y; + odom_.twist.twist.linear.y = cosf ( yaw ) * linear.y - sinf ( yaw ) * linear.x; } - tf::Transform base_footprint_to_odom(qt, vt); - transform_broadcaster_->sendTransform( - tf::StampedTransform(base_footprint_to_odom, current_time, - odom_frame, base_footprint_frame)); + tf::Transform base_footprint_to_odom ( qt, vt ); + transform_broadcaster_->sendTransform ( + tf::StampedTransform ( base_footprint_to_odom, current_time, + odom_frame, base_footprint_frame ) ); // set covariance @@ -354,9 +395,9 @@ void GazeboRosTricycleDrive::publishOdometry(double step_time) odom_.header.frame_id = odom_frame; odom_.child_frame_id = base_footprint_frame; - odometry_publisher_.publish(odom_); + odometry_publisher_.publish ( odom_ ); } -GZ_REGISTER_MODEL_PLUGIN(GazeboRosTricycleDrive) +GZ_REGISTER_MODEL_PLUGIN ( GazeboRosTricycleDrive ) } diff --git a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_plugins.xacro b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_plugins.xacro index 7bfe2dedd..7b76fd237 100644 --- a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_plugins.xacro +++ b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_plugins.xacro @@ -24,13 +24,13 @@ right_hub_joint 0.3 0.18 - 20 + 20 + 1.8 cmd_vel odom odom base_link 10.0 - 1.8 diff --git a/gazebo_plugins/test/tricycle_drive/launch/tricycle_drive_scenario.launch b/gazebo_plugins/test/tricycle_drive/launch/tricycle_drive_scenario.launch index 2723ccd65..e3a48b5be 100644 --- a/gazebo_plugins/test/tricycle_drive/launch/tricycle_drive_scenario.launch +++ b/gazebo_plugins/test/tricycle_drive/launch/tricycle_drive_scenario.launch @@ -1,25 +1,31 @@ + + - + - - - - - + + + + + + - + + - - - - - + + + + + + - + + diff --git a/gazebo_plugins/test/tricycle_drive/xacro/tricycle/tricycle_plugins.xacro b/gazebo_plugins/test/tricycle_drive/xacro/tricycle/tricycle_plugins.xacro index 1c7f26ce0..ef5494e27 100644 --- a/gazebo_plugins/test/tricycle_drive/xacro/tricycle/tricycle_plugins.xacro +++ b/gazebo_plugins/test/tricycle_drive/xacro/tricycle/tricycle_plugins.xacro @@ -17,15 +17,18 @@ 0.135 0.135 0.548 - 20 cmd_vel odom odom encoder base_link 10.0 - 1.8 - 1.8 + 1.8 + 5.0 + 0.05 + 20 + 0.4 + 0.02 From 7b1fdb0cb0e7c2abdf3acb63b4d98cbf2ad7885e Mon Sep 17 00:00:00 2001 From: Markus Bader Date: Wed, 25 Jun 2014 12:13:19 +0200 Subject: [PATCH 110/153] minor fix --- gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp b/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp index a8c56ce60..64ffe5333 100644 --- a/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp +++ b/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp @@ -256,7 +256,6 @@ void GazeboRosTricycleDrive::motorController ( double target_speed, double targe //ROS_INFO ( "target: [%3.2f, %3.2f], current: [%3.2f, %3.2f], applied: [%3.2f, %3.2f/%3.2f] !", // target_speed, target_angle, current_speed, current_angle, applied_speed, applied_angle, applied_steering_speed ); - } // Finalize the controller From 46d773d2e6eb894c0b69375643f7cb0268a0460c Mon Sep 17 00:00:00 2001 From: Markus Bader Date: Wed, 2 Jul 2014 13:08:14 +0200 Subject: [PATCH 111/153] gazebo_ros_diff_drive is now able to use the wheels rotation of the optometry or the gazebo ground truth based on the 'odometrySource' parameter --- .../gazebo_plugins/gazebo_ros_diff_drive.h | 12 +- gazebo_plugins/src/gazebo_ros_diff_drive.cpp | 121 +++++++++++++----- .../src/gazebo_ros_tricycle_drive.cpp | 2 - .../xacro/p3dx/pioneer3dx_plugins.xacro | 1 + 4 files changed, 103 insertions(+), 33 deletions(-) diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_diff_drive.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_diff_drive.h index 3ef3b2075..0f694bb50 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_diff_drive.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_diff_drive.h @@ -53,8 +53,8 @@ #include #include #include +#include #include -#include #include // Custom Callback Queue @@ -72,6 +72,11 @@ namespace gazebo { class GazeboRosDiffDrive : public ModelPlugin { + enum OdomSource + { + ENCODER = 0, + GAZEBO = 1, + }; public: GazeboRosDiffDrive(); ~GazeboRosDiffDrive(); @@ -86,6 +91,7 @@ namespace gazebo { void getWheelVelocities(); void publishWheelTF(); /// publishes the wheel tf's void publishWheelJointState(); + void UpdateOdometryEncoder(); GazeboRosPtr gazebo_ros_; @@ -135,6 +141,10 @@ namespace gazebo { double update_period_; common::Time last_update_time_; + OdomSource odom_source_; + geometry_msgs::Pose2D pose_encoder_; + common::Time last_odom_update_; + // Flags bool publishWheelTF_; bool publishWheelJointState_; diff --git a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp index dc9c6f6be..92628a580 100644 --- a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp +++ b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp @@ -56,13 +56,6 @@ #include #include -#include -#include -#include -#include -#include -#include -#include namespace gazebo { @@ -98,6 +91,10 @@ void GazeboRosDiffDrive::Load ( physics::ModelPtr _parent, sdf::ElementPtr _sdf gazebo_ros_->getParameter ( wheel_accel, "wheelAcceleration", 0.0 ); gazebo_ros_->getParameter ( wheel_torque, "wheelTorque", 5.0 ); gazebo_ros_->getParameter ( update_rate_, "updateRate", 100.0 ); + std::map odomOptions; + odomOptions["encoder"] = ENCODER; + odomOptions["gazebo"] = GAZEBO; + gazebo_ros_->getParameter ( odom_source_, "odometrySource", odomOptions, ENCODER ); joints_.resize ( 2 ); @@ -193,6 +190,8 @@ void GazeboRosDiffDrive::publishWheelTF() // Update the controller void GazeboRosDiffDrive::UpdateChild() { + + if ( odom_source_ == ENCODER ) UpdateOdometryEncoder(); common::Time current_time = parent->GetWorld()->GetSimTime(); double seconds_since_last_update = ( current_time - last_update_time_ ).Double(); if ( seconds_since_last_update > update_period_ ) { @@ -272,31 +271,101 @@ void GazeboRosDiffDrive::QueueThread() } } +void GazeboRosDiffDrive::UpdateOdometryEncoder() +{ + double vl = joints_[LEFT]->GetVelocity ( 0 ); + double vr = joints_[RIGHT]->GetVelocity ( 0 ); + common::Time current_time = parent->GetWorld()->GetSimTime(); + double seconds_since_last_update = ( current_time - last_odom_update_ ).Double(); + last_odom_update_ = current_time; + + double b = wheel_separation_; + + // Book: Sigwart 2011 Autonompus Mobile Robots page:337 + double sl = vl * ( wheel_diameter_ / 2.0 ) * seconds_since_last_update; + double sr = vr * ( wheel_diameter_ / 2.0 ) * seconds_since_last_update; + double theta = ( sl - sr ) /b; + + + double dx = ( sl + sr ) /2.0 * cos ( pose_encoder_.theta + ( sl - sr ) / ( 2.0*b ) ); + double dy = ( sl + sr ) /2.0 * sin ( pose_encoder_.theta + ( sl - sr ) / ( 2.0*b ) ); + double dtheta = ( sl - sr ) /b; + + pose_encoder_.x += dx; + pose_encoder_.y += dy; + pose_encoder_.theta += dtheta; + + double w = dtheta/seconds_since_last_update; + double v = sqrt ( dx*dx+dy*dy ) /seconds_since_last_update; + + tf::Quaternion qt; + tf::Vector3 vt; + qt.setRPY ( 0,0,pose_encoder_.theta ); + vt = tf::Vector3 ( pose_encoder_.x, pose_encoder_.y, 0 ); + + odom_.pose.pose.position.x = vt.x(); + odom_.pose.pose.position.y = vt.y(); + odom_.pose.pose.position.z = vt.z(); + + odom_.pose.pose.orientation.x = qt.x(); + odom_.pose.pose.orientation.y = qt.y(); + odom_.pose.pose.orientation.z = qt.z(); + odom_.pose.pose.orientation.w = qt.w(); + + odom_.twist.twist.angular.z = w; + odom_.twist.twist.linear.x = dx/seconds_since_last_update; + odom_.twist.twist.linear.y = dy/seconds_since_last_update; +} + void GazeboRosDiffDrive::publishOdometry ( double step_time ) { + ros::Time current_time = ros::Time::now(); - std::string odom_frame = gazebo_ros_->resolveTF(odometry_frame_); - std::string base_footprint_frame = gazebo_ros_->resolveTF(robot_base_frame_); - - // getting data for base_footprint to odom transform - math::Pose pose = this->parent->GetWorldPose(); + std::string odom_frame = gazebo_ros_->resolveTF ( odometry_frame_ ); + std::string base_footprint_frame = gazebo_ros_->resolveTF ( robot_base_frame_ ); + + tf::Quaternion qt; + tf::Vector3 vt; - tf::Quaternion qt ( pose.rot.x, pose.rot.y, pose.rot.z, pose.rot.w ); - tf::Vector3 vt ( pose.pos.x, pose.pos.y, pose.pos.z ); + if ( odom_source_ == ENCODER ) { + // getting data form encoder integration + qt = tf::Quaternion ( odom_.pose.pose.orientation.x, odom_.pose.pose.orientation.y, odom_.pose.pose.orientation.z, odom_.pose.pose.orientation.w ); + vt = tf::Vector3 ( odom_.pose.pose.position.x, odom_.pose.pose.position.y, odom_.pose.pose.position.z ); + + } + if ( odom_source_ == GAZEBO ) { + // getting data form gazebo + math::Pose pose = parent->GetWorldPose(); + qt = tf::Quaternion ( pose.rot.x, pose.rot.y, pose.rot.z, pose.rot.w ); + vt = tf::Vector3 ( pose.pos.x, pose.pos.y, pose.pos.z ); + + odom_.pose.pose.position.x = vt.x(); + odom_.pose.pose.position.y = vt.y(); + odom_.pose.pose.position.z = vt.z(); + + odom_.pose.pose.orientation.x = qt.x(); + odom_.pose.pose.orientation.y = qt.y(); + odom_.pose.pose.orientation.z = qt.z(); + odom_.pose.pose.orientation.w = qt.w(); + + // get velocity in /odom frame + math::Vector3 linear; + linear = parent->GetWorldLinearVel(); + odom_.twist.twist.angular.z = parent->GetWorldAngularVel().z; + + // convert velocity to child_frame_id (aka base_footprint) + float yaw = pose.rot.GetYaw(); + odom_.twist.twist.linear.x = cosf ( yaw ) * linear.x + sinf ( yaw ) * linear.y; + odom_.twist.twist.linear.y = cosf ( yaw ) * linear.y - sinf ( yaw ) * linear.x; + } tf::Transform base_footprint_to_odom ( qt, vt ); transform_broadcaster_->sendTransform ( tf::StampedTransform ( base_footprint_to_odom, current_time, odom_frame, base_footprint_frame ) ); - // publish odom topic - odom_.pose.pose.position.x = pose.pos.x; - odom_.pose.pose.position.y = pose.pos.y; - odom_.pose.pose.orientation.x = pose.rot.x; - odom_.pose.pose.orientation.y = pose.rot.y; - odom_.pose.pose.orientation.z = pose.rot.z; - odom_.pose.pose.orientation.w = pose.rot.w; + // set covariance odom_.pose.covariance[0] = 0.00001; odom_.pose.covariance[7] = 0.00001; odom_.pose.covariance[14] = 1000000000000.0; @@ -304,16 +373,8 @@ void GazeboRosDiffDrive::publishOdometry ( double step_time ) odom_.pose.covariance[28] = 1000000000000.0; odom_.pose.covariance[35] = 0.001; - // get velocity in /odom frame - math::Vector3 linear; - linear = this->parent->GetWorldLinearVel(); - odom_.twist.twist.angular.z = this->parent->GetWorldAngularVel().z; - - // convert velocity to child_frame_id (aka base_footprint) - float yaw = pose.rot.GetYaw(); - odom_.twist.twist.linear.x = cosf ( yaw ) * linear.x + sinf ( yaw ) * linear.y; - odom_.twist.twist.linear.y = cosf ( yaw ) * linear.y - sinf ( yaw ) * linear.x; + // set header odom_.header.stamp = current_time; odom_.header.frame_id = odom_frame; odom_.child_frame_id = base_footprint_frame; diff --git a/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp b/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp index 64ffe5333..5350bed12 100644 --- a/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp +++ b/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp @@ -328,8 +328,6 @@ void GazeboRosTricycleDrive::UpdateOdometryEncoder() odom_.twist.twist.angular.z = w; odom_.twist.twist.linear.x = dx/seconds_since_last_update; odom_.twist.twist.linear.y = dy/seconds_since_last_update; - - } void GazeboRosTricycleDrive::publishOdometry ( double step_time ) diff --git a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_plugins.xacro b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_plugins.xacro index 7b76fd237..40ebeb0de 100644 --- a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_plugins.xacro +++ b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_plugins.xacro @@ -29,6 +29,7 @@ cmd_vel odom odom + gazebo base_link 10.0 From 0456be28dd96e66f487c89720d6e228f2d337c0f Mon Sep 17 00:00:00 2001 From: Markus Bader Date: Wed, 2 Jul 2014 13:26:32 +0200 Subject: [PATCH 112/153] gazebo_ros_diff_drive gazebo_ros_tricycle_drive encoderSource option names updated --- .../include/gazebo_plugins/gazebo_ros_diff_drive.h | 2 +- .../include/gazebo_plugins/gazebo_ros_tricycle_drive.h | 2 +- gazebo_plugins/src/gazebo_ros_diff_drive.cpp | 8 ++++---- gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp | 8 ++++---- .../xacro/p3dx/pioneer3dx_plugins.xacro | 2 +- 5 files changed, 11 insertions(+), 11 deletions(-) diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_diff_drive.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_diff_drive.h index 0f694bb50..4046b3acc 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_diff_drive.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_diff_drive.h @@ -75,7 +75,7 @@ namespace gazebo { enum OdomSource { ENCODER = 0, - GAZEBO = 1, + WORLD = 1, }; public: GazeboRosDiffDrive(); diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_tricycle_drive.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_tricycle_drive.h index 553120eb2..0dd3aa937 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_tricycle_drive.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_tricycle_drive.h @@ -82,7 +82,7 @@ class GazeboRosTricycleDrive : public ModelPlugin { enum OdomSource { ENCODER = 0, - GAZEBO = 1, + WORLD = 1, }; public: GazeboRosTricycleDrive(); diff --git a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp index 92628a580..beab7d79c 100644 --- a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp +++ b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp @@ -93,8 +93,8 @@ void GazeboRosDiffDrive::Load ( physics::ModelPtr _parent, sdf::ElementPtr _sdf gazebo_ros_->getParameter ( update_rate_, "updateRate", 100.0 ); std::map odomOptions; odomOptions["encoder"] = ENCODER; - odomOptions["gazebo"] = GAZEBO; - gazebo_ros_->getParameter ( odom_source_, "odometrySource", odomOptions, ENCODER ); + odomOptions["world"] = WORLD; + gazebo_ros_->getParameter ( odom_source_, "odometrySource", odomOptions, WORLD ); joints_.resize ( 2 ); @@ -333,8 +333,8 @@ void GazeboRosDiffDrive::publishOdometry ( double step_time ) vt = tf::Vector3 ( odom_.pose.pose.position.x, odom_.pose.pose.position.y, odom_.pose.pose.position.z ); } - if ( odom_source_ == GAZEBO ) { - // getting data form gazebo + if ( odom_source_ == WORLD ) { + // getting data form gazebo world math::Pose pose = parent->GetWorldPose(); qt = tf::Quaternion ( pose.rot.x, pose.rot.y, pose.rot.z, pose.rot.w ); vt = tf::Vector3 ( pose.pos.x, pose.pos.y, pose.pos.z ); diff --git a/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp b/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp index 5350bed12..1642e804a 100644 --- a/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp +++ b/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp @@ -105,8 +105,8 @@ void GazeboRosTricycleDrive::Load ( physics::ModelPtr _parent, sdf::ElementPtr _ std::map odomOptions; odomOptions["encoder"] = ENCODER; - odomOptions["gazebo"] = GAZEBO; - gazebo_ros_->getParameter ( odom_source_, "odometrySource", odomOptions, ENCODER ); + odomOptions["world"] = WORLD; + gazebo_ros_->getParameter ( odom_source_, "odometrySource", odomOptions, WORLD ); joint_wheel_actuated_->SetMaxForce ( 0, wheel_torque_ ); joint_steering_->SetMaxForce ( 0, wheel_torque_ ); @@ -346,8 +346,8 @@ void GazeboRosTricycleDrive::publishOdometry ( double step_time ) vt = tf::Vector3 ( odom_.pose.pose.position.x, odom_.pose.pose.position.y, odom_.pose.pose.position.z ); } - if ( odom_source_ == GAZEBO ) { - // getting data form gazebo + if ( odom_source_ == WORLD ) { + // getting data form gazebo world math::Pose pose = parent->GetWorldPose(); qt = tf::Quaternion ( pose.rot.x, pose.rot.y, pose.rot.z, pose.rot.w ); vt = tf::Vector3 ( pose.pos.x, pose.pos.y, pose.pos.z ); diff --git a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_plugins.xacro b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_plugins.xacro index 40ebeb0de..7b5f860c4 100644 --- a/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_plugins.xacro +++ b/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_plugins.xacro @@ -29,7 +29,7 @@ cmd_vel odom odom - gazebo + world base_link 10.0 From 5db9c9268870e8b394a43250596c3ef56eb2382d Mon Sep 17 00:00:00 2001 From: gborque Date: Mon, 14 Jul 2014 16:18:58 +0200 Subject: [PATCH 113/153] Added Gaussian Noise generator --- .../gazebo_plugins/gazebo_ros_ft_sensor.h | 101 +++++++++--------- gazebo_plugins/src/gazebo_ros_ft_sensor.cpp | 97 +++++++++++------ 2 files changed, 117 insertions(+), 81 deletions(-) diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_ft_sensor.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_ft_sensor.h index 2c4036612..f0fd9b871 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_ft_sensor.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_ft_sensor.h @@ -1,29 +1,28 @@ /* - * Gazebo - Outdoor Multi-Robot Simulator - * Copyright (C) 2003 - * Nate Koenig & Andrew Howard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - * - */ +* Gazebo - Outdoor Multi-Robot Simulator +* Copyright (C) 2003 +* Nate Koenig & Andrew Howard +* +* This program is free software; you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation; either version 2 of the License, or +* (at your option) any later version. +* +* This program is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with this program; if not, write to the Free Software +* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +* +*/ /* - * Desc: Force Torque Sensor Plugin - * Author: Francisco Suarez-Ruiz - * Date: 5 June 2014 - */ - +* Desc: Force Torque Sensor Plugin +* Author: Francisco Suarez-Ruiz +* Date: 5 June 2014 +*/ #ifndef GAZEBO_ROS_FT_HH #define GAZEBO_ROS_FT_HH @@ -47,30 +46,28 @@ namespace gazebo /// @{ /** \defgroup GazeboRosFTSensor Plugin XML Reference and Example - \brief Ros Gazebo Ros Force/Torque Sensor Plugin. - - This is a model plugin which broadcasts geometry_msgs/WrenchStamped messages - with measured force and torque on a specified joint. - - The wrench is reported in the joint CHILD link frame and the measure direction - is child-to-parent link. - - Example Usage: - - \verbatim - - - true - - - - - 100.0 - ft_sensor_topic - JOINT_NAME - - - \endverbatim +\brief Ros Gazebo Ros Force/Torque Sensor Plugin. +This is a model plugin which broadcasts geometry_msgs/WrenchStamped messages +with measured force and torque on a specified joint. +The wrench is reported in the joint CHILD link frame and the measure direction +is child-to-parent link. + +Example Usage: + +\verbatim + + +true + + + + +100.0 +ft_sensor_topic +JOINT_NAME + + +\endverbatim \{ */ @@ -91,6 +88,12 @@ class GazeboRosFT : public ModelPlugin /// \brief Update the controller protected: virtual void UpdateChild(); + + /// \brief Gaussian noise + private: double gaussian_noise_; + private: unsigned int seed; + /// \brief Gaussian noise generator + private: double GaussianKernel(double mu, double sigma); /// \brief A pointer to the Gazebo joint private: physics::JointPtr joint_; @@ -107,7 +110,7 @@ class GazeboRosFT : public ModelPlugin /// \brief A pointer to the Gazebo world private: physics::WorldPtr world_; - /// \brief A pointer to the ROS node. A node will be instantiated if it does not exist. + /// \brief A pointer to the ROS node. A node will be instantiated if it does not exist. private: ros::NodeHandle* rosnode_; private: ros::Publisher pub_; diff --git a/gazebo_plugins/src/gazebo_ros_ft_sensor.cpp b/gazebo_plugins/src/gazebo_ros_ft_sensor.cpp index e5b0f4ae5..0e10ed65c 100644 --- a/gazebo_plugins/src/gazebo_ros_ft_sensor.cpp +++ b/gazebo_plugins/src/gazebo_ros_ft_sensor.cpp @@ -1,28 +1,28 @@ /* - * Gazebo - Outdoor Multi-Robot Simulator - * Copyright (C) 2003 - * Nate Koenig & Andrew Howard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - * - */ +* Gazebo - Outdoor Multi-Robot Simulator +* Copyright (C) 2003 +* Nate Koenig & Andrew Howard +* +* This program is free software; you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation; either version 2 of the License, or +* (at your option) any later version. +* +* This program is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with this program; if not, write to the Free Software +* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +* +*/ /* - * Desc: Force Torque Sensor Plugin - * Author: Francisco Suarez-Ruiz - * Date: 5 June 2014 - */ +* Desc: Force Torque Sensor Plugin +* Author: Francisco Suarez-Ruiz +* Date: 5 June 2014 +*/ #include #include @@ -36,6 +36,7 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboRosFT); GazeboRosFT::GazeboRosFT() { this->ft_connect_count_ = 0; + this->seed = 0; } //////////////////////////////////////////////////////////////////////////////// @@ -93,6 +94,14 @@ void GazeboRosFT::Load( physics::ModelPtr _model, sdf::ElementPtr _sdf ) else this->topic_name_ = _sdf->GetElement("topicName")->Get(); + if (!_sdf->HasElement("gaussianNoise")) + { + ROS_INFO("imu plugin missing , defaults to 0.0"); + this->gaussian_noise_ = 0.0; + } + else + this->gaussian_noise_ = _sdf->Get("gaussianNoise"); + if (!_sdf->HasElement("updateRate")) { ROS_DEBUG("ft_sensor plugin missing , defaults to 0.0" @@ -170,10 +179,10 @@ void GazeboRosFT::UpdateChild() // E.g: https://bitbucket.org/osrf/gazebo/raw/default/gazebo/sensors/ForceTorqueSensor.hh // Get force torque at the joint // The wrench is reported in the CHILD - // The is parent_to_child + // The is child_to_parent wrench = this->joint_->GetForceTorque(0); - force = -wrench.body2Force; - torque = -wrench.body2Torque; + force = wrench.body2Force; + torque = wrench.body2Torque; this->lock_.lock(); @@ -182,12 +191,12 @@ void GazeboRosFT::UpdateChild() this->wrench_msg_.header.stamp.sec = (this->world_->GetSimTime()).sec; this->wrench_msg_.header.stamp.nsec = (this->world_->GetSimTime()).nsec; - this->wrench_msg_.wrench.force.x = force.x; - this->wrench_msg_.wrench.force.y = force.y; - this->wrench_msg_.wrench.force.z = force.z; - this->wrench_msg_.wrench.torque.x = torque.x; - this->wrench_msg_.wrench.torque.y = torque.y; - this->wrench_msg_.wrench.torque.z = torque.z; + this->wrench_msg_.wrench.force.x = force.x + this->GaussianKernel(0, this->gaussian_noise_); + this->wrench_msg_.wrench.force.y = force.y + this->GaussianKernel(0, this->gaussian_noise_); + this->wrench_msg_.wrench.force.z = force.z + this->GaussianKernel(0, this->gaussian_noise_); + this->wrench_msg_.wrench.torque.x = torque.x + this->GaussianKernel(0, this->gaussian_noise_); + this->wrench_msg_.wrench.torque.y = torque.y + this->GaussianKernel(0, this->gaussian_noise_); + this->wrench_msg_.wrench.torque.z = torque.z + this->GaussianKernel(0, this->gaussian_noise_); this->pub_.publish(this->wrench_msg_); this->lock_.unlock(); @@ -196,6 +205,30 @@ void GazeboRosFT::UpdateChild() this->last_time_ = cur_time; } +////////////////////////////////////////////////////////////////////////////// +// Utility for adding noise +double GazeboRosFT::GaussianKernel(double mu, double sigma) +{ + // using Box-Muller transform to generate two independent standard + // normally disbributed normal variables see wikipedia + + // normalized uniform random variable + double U = static_cast(rand_r(&this->seed)) / + static_cast(RAND_MAX); + + // normalized uniform random variable + double V = static_cast(rand_r(&this->seed)) / + static_cast(RAND_MAX); + + double X = sqrt(-2.0 * ::log(U)) * cos(2.0*M_PI * V); + // double Y = sqrt(-2.0 * ::log(U)) * sin(2.0*M_PI * V); + + // there are 2 indep. vars, we'll just use X + // scale to our mu and sigma + X = sigma * X + mu; + return X; +} + // Custom Callback Queue //////////////////////////////////////////////////////////////////////////////// // custom callback queue thread From 160fc6ecbc9426f7e68fa6a763d444e90f507c31 Mon Sep 17 00:00:00 2001 From: Alexander Bubeck Date: Tue, 15 Jul 2014 10:26:22 +0200 Subject: [PATCH 114/153] Should fix build error for binary releases. See: http://www.ros.org/debbuild/indigo.html?q=gazebo_ros_control --- gazebo_ros_control/package.xml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gazebo_ros_control/package.xml b/gazebo_ros_control/package.xml index 109a45457..d51880a2a 100644 --- a/gazebo_ros_control/package.xml +++ b/gazebo_ros_control/package.xml @@ -26,7 +26,8 @@ urdf angles - roscpp + roscpp + gazebo gazebo_ros control_toolbox controller_manager From 2c6bb2249ccf6b242feafef958b4c5eae402bf09 Mon Sep 17 00:00:00 2001 From: Adolfo Rodriguez Tsouroukdissian Date: Tue, 15 Jul 2014 11:25:11 +0200 Subject: [PATCH 115/153] Update package.xml Add new maintainer. --- gazebo_ros_control/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/gazebo_ros_control/package.xml b/gazebo_ros_control/package.xml index fdbfde423..1b02a020b 100644 --- a/gazebo_ros_control/package.xml +++ b/gazebo_ros_control/package.xml @@ -4,6 +4,7 @@ gazebo_ros_control Jonathan Bohren + Adolfo Rodriguez Tsouroukdissian BSD From 0fea2ca9b90f985ac6a84c5502083513589b1dd1 Mon Sep 17 00:00:00 2001 From: Adolfo Rodriguez Tsouroukdissian Date: Tue, 15 Jul 2014 11:25:11 +0200 Subject: [PATCH 116/153] Update package.xml Add new maintainer. --- gazebo_ros_control/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/gazebo_ros_control/package.xml b/gazebo_ros_control/package.xml index d51880a2a..6f8ccd731 100644 --- a/gazebo_ros_control/package.xml +++ b/gazebo_ros_control/package.xml @@ -4,6 +4,7 @@ gazebo_ros_control Jonathan Bohren + Adolfo Rodriguez Tsouroukdissian BSD From 0eaeca54e082091422630495a9dd2f9dcccfc7fa Mon Sep 17 00:00:00 2001 From: Steven Peters Date: Fri, 18 Jul 2014 16:46:25 -0700 Subject: [PATCH 117/153] Update Changelog --- gazebo_msgs/CHANGELOG.rst | 5 ++++ gazebo_plugins/CHANGELOG.rst | 39 ++++++++++++++++++++++++++++++++ gazebo_ros/CHANGELOG.rst | 10 ++++++++ gazebo_ros_control/CHANGELOG.rst | 17 ++++++++++++++ gazebo_ros_pkgs/CHANGELOG.rst | 6 +++++ 5 files changed, 77 insertions(+) diff --git a/gazebo_msgs/CHANGELOG.rst b/gazebo_msgs/CHANGELOG.rst index b7ba8e413..c58651cc3 100644 --- a/gazebo_msgs/CHANGELOG.rst +++ b/gazebo_msgs/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gazebo_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix repo names in package.xml's +* Contributors: Jon Binney + 2.4.3 (2014-05-12) ------------------ diff --git a/gazebo_plugins/CHANGELOG.rst b/gazebo_plugins/CHANGELOG.rst index 1cf0744e5..a33c12550 100644 --- a/gazebo_plugins/CHANGELOG.rst +++ b/gazebo_plugins/CHANGELOG.rst @@ -2,6 +2,45 @@ Changelog for package gazebo_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge branch 'hydro-devel' into indigo-devel +* gazebo_ros_diff_drive gazebo_ros_tricycle_drive encoderSource option names updated +* gazebo_ros_diff_drive is now able to use the wheels rotation of the optometry or the gazebo ground truth based on the 'odometrySource' parameter +* simple linear controller for the tricycle_drive added +* second robot for testing in tricycle_drive_scenario.launch added +* Merge remote-tracking branch 'upstream/hydro-devel' into hydro-devel +* BDS licenses header fixed and tricycle drive plugin added +* format patch of hsu applied +* Updated package.xml +* Fix repo names in package.xml's +* ros diff drive supports now an acceleration limit +* Pioneer model: Diff_drive torque reduced +* GPU Laser test example added +* fixed gpu_laser to work with workspaces +* hand_of_god: Adding hand-of-god plugin + ros_force: Fixing error messages to refer to the right plugin +* Remove unneeded dependency on pcl_ros +* minor fixes on relative paths in xacro for pioneer robot +* gazebo test model pionneer 3dx updated with xacro path variables +* pioneer model update for the multi_robot_scenario +* Merge remote-tracking branch 'upstream/hydro-devel' into hydro-devel +* fixed camera to work with workspaces +* fixed links related to changed name +* diff drive name changed to multi robot scenario +* working camera added +* Merge remote-tracking branch 'upstream/hydro-devel' into hydro-devel +* fix in pioneer xacro model for diff_drive +* Laser colour in rviz changed +* A test model for the ros_diff_drive ros_laser and joint_state_publisher added +* the ros_laser checkes now for the model name and adds it als prefix +* joint velocity fixed using radius instead of diameter +* ROS_INFO on laser plugin added to see if it starts +* fetched with upstream +* gazebo_ros_diff_drive was enhanced to publish the wheels tf or the wheels joint state depending on two additinal xml options +* Gazebo ROS joint state publisher added +* Contributors: Dave Coleman, John Hsu, Jon Binney, Jonathan Bohren, Markus Bader, Steven Peters + 2.4.3 (2014-05-12) ------------------ * gazebo_plugins: add run-time dependency on gazebo_ros diff --git a/gazebo_ros/CHANGELOG.rst b/gazebo_ros/CHANGELOG.rst index 1abc236b8..246602d60 100644 --- a/gazebo_ros/CHANGELOG.rst +++ b/gazebo_ros/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package gazebo_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix repo names in package.xml's +* fix issue `#198 `_ + Operator ``==`` is not recognized by sh scripts. +* Add verbose parameter + Add verbose parameter for --verbose gazebo flag +* added osx support for gazebo start scripts +* Contributors: Arn-O, Jon Binney, Markus Achtelik, Vincenzo Comito + 2.4.3 (2014-05-12) ------------------ * added osx support for gazebo start scripts diff --git a/gazebo_ros_control/CHANGELOG.rst b/gazebo_ros_control/CHANGELOG.rst index 95d728109..e84149eaf 100644 --- a/gazebo_ros_control/CHANGELOG.rst +++ b/gazebo_ros_control/CHANGELOG.rst @@ -2,6 +2,23 @@ Changelog for package gazebo_ros_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update package.xml + Add new maintainer. +* Should fix build error for binary releases. + See: http://www.ros.org/debbuild/indigo.html?q=gazebo_ros_control +* Updated package.xml +* gazebo_ros_control: default_robot_hw_sim: Suppressing pid error message + Depends on `ros-controls/control_toolbox#21 `_ +* Revert 4776545, as it belongs in indigo-devel. +* Fix repo names in package.xml's +* gazebo_ros_control: default_robot_hw_sim: Suppressing pid error message, depends on `ros-controls/control_toolbox#21 `_ +* gazebo_ros_control: Add dependency on angles +* gazebo_ros_control: Add build-time dependency on gazebo + This fixes a regression caused by a889ef8b768861231a67b78781514d834f631b8e +* Contributors: Adolfo Rodriguez Tsouroukdissian, Alexander Bubeck, Dave Coleman, Jon Binney, Jonathan Bohren, Scott K Logan + 2.4.3 (2014-05-12) ------------------ * Compatibility with Indigo's ros_control. diff --git a/gazebo_ros_pkgs/CHANGELOG.rst b/gazebo_ros_pkgs/CHANGELOG.rst index 132b4968a..fa8f2773b 100644 --- a/gazebo_ros_pkgs/CHANGELOG.rst +++ b/gazebo_ros_pkgs/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package gazebo_ros_pkgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Updated package.xml +* Fix repo names in package.xml's +* Contributors: Dave Coleman, Jon Binney + 2.4.3 (2014-05-12) ------------------ From 25534781a5655e6eb1138a7ccc37a38185e20042 Mon Sep 17 00:00:00 2001 From: Steven Peters Date: Fri, 18 Jul 2014 16:47:15 -0700 Subject: [PATCH 118/153] 2.4.4 --- gazebo_msgs/CHANGELOG.rst | 4 ++-- gazebo_msgs/package.xml | 2 +- gazebo_plugins/CHANGELOG.rst | 4 ++-- gazebo_plugins/package.xml | 2 +- gazebo_ros/CHANGELOG.rst | 4 ++-- gazebo_ros/package.xml | 2 +- gazebo_ros_control/CHANGELOG.rst | 4 ++-- gazebo_ros_control/package.xml | 2 +- gazebo_ros_pkgs/CHANGELOG.rst | 4 ++-- gazebo_ros_pkgs/package.xml | 2 +- 10 files changed, 15 insertions(+), 15 deletions(-) diff --git a/gazebo_msgs/CHANGELOG.rst b/gazebo_msgs/CHANGELOG.rst index c58651cc3..a412ecadd 100644 --- a/gazebo_msgs/CHANGELOG.rst +++ b/gazebo_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.4.4 (2014-07-18) +------------------ * Fix repo names in package.xml's * Contributors: Jon Binney diff --git a/gazebo_msgs/package.xml b/gazebo_msgs/package.xml index a43bfe269..43bc28464 100644 --- a/gazebo_msgs/package.xml +++ b/gazebo_msgs/package.xml @@ -1,7 +1,7 @@ gazebo_msgs - 2.4.3 + 2.4.4 Message and service data structures for interacting with Gazebo from ROS. diff --git a/gazebo_plugins/CHANGELOG.rst b/gazebo_plugins/CHANGELOG.rst index a33c12550..e1c57612a 100644 --- a/gazebo_plugins/CHANGELOG.rst +++ b/gazebo_plugins/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.4.4 (2014-07-18) +------------------ * Merge branch 'hydro-devel' into indigo-devel * gazebo_ros_diff_drive gazebo_ros_tricycle_drive encoderSource option names updated * gazebo_ros_diff_drive is now able to use the wheels rotation of the optometry or the gazebo ground truth based on the 'odometrySource' parameter diff --git a/gazebo_plugins/package.xml b/gazebo_plugins/package.xml index db85d416a..a21eea255 100644 --- a/gazebo_plugins/package.xml +++ b/gazebo_plugins/package.xml @@ -1,6 +1,6 @@ gazebo_plugins - 2.4.3 + 2.4.4 Robot-independent Gazebo plugins for sensors, motors and dynamic reconfigurable components. diff --git a/gazebo_ros/CHANGELOG.rst b/gazebo_ros/CHANGELOG.rst index 246602d60..beabba097 100644 --- a/gazebo_ros/CHANGELOG.rst +++ b/gazebo_ros/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.4.4 (2014-07-18) +------------------ * Fix repo names in package.xml's * fix issue `#198 `_ Operator ``==`` is not recognized by sh scripts. diff --git a/gazebo_ros/package.xml b/gazebo_ros/package.xml index edb2a19c8..49f0c1e46 100644 --- a/gazebo_ros/package.xml +++ b/gazebo_ros/package.xml @@ -1,7 +1,7 @@ gazebo_ros - 2.4.3 + 2.4.4 Provides ROS plugins that offer message and service publishers for interfacing with Gazebo through ROS. Formally simulator_gazebo/gazebo diff --git a/gazebo_ros_control/CHANGELOG.rst b/gazebo_ros_control/CHANGELOG.rst index e84149eaf..41286d44c 100644 --- a/gazebo_ros_control/CHANGELOG.rst +++ b/gazebo_ros_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_ros_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.4.4 (2014-07-18) +------------------ * Update package.xml Add new maintainer. * Should fix build error for binary releases. diff --git a/gazebo_ros_control/package.xml b/gazebo_ros_control/package.xml index 6f8ccd731..a16d5f06b 100644 --- a/gazebo_ros_control/package.xml +++ b/gazebo_ros_control/package.xml @@ -1,6 +1,6 @@ gazebo_ros_control - 2.4.3 + 2.4.4 gazebo_ros_control Jonathan Bohren diff --git a/gazebo_ros_pkgs/CHANGELOG.rst b/gazebo_ros_pkgs/CHANGELOG.rst index fa8f2773b..6f0154a56 100644 --- a/gazebo_ros_pkgs/CHANGELOG.rst +++ b/gazebo_ros_pkgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_ros_pkgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.4.4 (2014-07-18) +------------------ * Updated package.xml * Fix repo names in package.xml's * Contributors: Dave Coleman, Jon Binney diff --git a/gazebo_ros_pkgs/package.xml b/gazebo_ros_pkgs/package.xml index d343c42ed..5b74ccd6a 100644 --- a/gazebo_ros_pkgs/package.xml +++ b/gazebo_ros_pkgs/package.xml @@ -1,7 +1,7 @@ gazebo_ros_pkgs - 2.4.3 + 2.4.4 Interface for using ROS with the Gazebo simulator. John Hsu From 793a522e2875911a267cd2c25b348b2b6156554a Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Thu, 31 Jul 2014 13:06:38 -0700 Subject: [PATCH 119/153] Fix a link on the readme --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 6d4fde2c1..cf7bf0535 100644 --- a/README.md +++ b/README.md @@ -6,7 +6,7 @@ Wrappers, tools and additional API's for using ROS with the Gazebo simulator. Fo [Installing gazebo_ros_pkgs](http://gazebosim.org/wiki/Tutorials/1.9/Installing_gazebo_ros_Packages) ### Documentation and Tutorials -[On gazebosim.org](http://gazebosim.org/wiki/Tutorials#ROS_Integration) +[On gazebosim.org](http://gazebosim.org/tutorials?cat=connect_ros) ### Develop and Contribute From fbcc26eeb29c4fe661ac738583250ae51b296d47 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Thu, 31 Jul 2014 13:34:54 -0700 Subject: [PATCH 120/153] Fix build with gazebo4 and indigo --- gazebo_plugins/src/gazebo_ros_gpu_laser.cpp | 7 ++- gazebo_plugins/src/gazebo_ros_laser.cpp | 27 ++++---- gazebo_ros/src/gazebo_ros_api_plugin.cpp | 63 +++++++++++-------- .../src/default_robot_hw_sim.cpp | 5 +- .../src/gazebo_ros_control_plugin.cpp | 3 +- 5 files changed, 60 insertions(+), 45 deletions(-) diff --git a/gazebo_plugins/src/gazebo_ros_gpu_laser.cpp b/gazebo_plugins/src/gazebo_ros_gpu_laser.cpp index 49c6c0d3f..811fc81b2 100644 --- a/gazebo_plugins/src/gazebo_ros_gpu_laser.cpp +++ b/gazebo_plugins/src/gazebo_ros_gpu_laser.cpp @@ -33,6 +33,7 @@ #include #include #include +#include #include #include @@ -124,9 +125,9 @@ void GazeboRosLaser::LoadThread() this->gazebo_node_->Init(this->world_name_); this->pmq.startServiceThread(); - + this->rosnode_ = new ros::NodeHandle(this->robot_namespace_); - + this->tf_prefix_ = tf::getPrefixParam(*this->rosnode_); if(this->tf_prefix_.empty()) { this->tf_prefix_ = this->robot_namespace_; @@ -134,7 +135,7 @@ void GazeboRosLaser::LoadThread() } ROS_INFO("GPU Laser Plugin (ns = %s) , set to \"%s\"", this->robot_namespace_.c_str(), this->tf_prefix_.c_str()); - + // resolve tf prefix this->frame_name_ = tf::resolve(this->tf_prefix_, this->frame_name_); diff --git a/gazebo_plugins/src/gazebo_ros_laser.cpp b/gazebo_plugins/src/gazebo_ros_laser.cpp index 9f099258d..815c456e1 100644 --- a/gazebo_plugins/src/gazebo_ros_laser.cpp +++ b/gazebo_plugins/src/gazebo_ros_laser.cpp @@ -33,6 +33,7 @@ #include #include #include +#include #include #include @@ -78,7 +79,7 @@ void GazeboRosLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) gzthrow("GazeboRosLaser controller requires a Ray Sensor as its parent"); this->robot_namespace_ = GetRobotNamespace(_parent, _sdf, "Laser"); - + if (!this->sdf->HasElement("frameName")) { ROS_INFO("Laser plugin missing , defaults to /world"); @@ -86,8 +87,8 @@ void GazeboRosLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) } else this->frame_name_ = this->sdf->Get("frameName"); - - + + if (!this->sdf->HasElement("topicName")) { ROS_INFO("Laser plugin missing , defaults to /world"); @@ -104,8 +105,8 @@ void GazeboRosLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. " << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); return; - } - + } + ROS_INFO ( "Starting Laser Plugin (ns = %s)!", this->robot_namespace_.c_str() ); // ros callback queue for processing subscription this->deferred_load_thread_ = boost::thread( @@ -123,7 +124,7 @@ void GazeboRosLaser::LoadThread() this->pmq.startServiceThread(); this->rosnode_ = new ros::NodeHandle(this->robot_namespace_); - + this->tf_prefix_ = tf::getPrefixParam(*this->rosnode_); if(this->tf_prefix_.empty()) { this->tf_prefix_ = this->robot_namespace_; @@ -131,7 +132,7 @@ void GazeboRosLaser::LoadThread() } ROS_INFO("Laser Plugin (ns = %s) , set to \"%s\"", this->robot_namespace_.c_str(), this->tf_prefix_.c_str()); - + // resolve tf prefix this->frame_name_ = tf::resolve(this->tf_prefix_, this->frame_name_); @@ -159,8 +160,8 @@ void GazeboRosLaser::LaserConnect() { this->laser_connect_count_++; if (this->laser_connect_count_ == 1) - this->laser_scan_sub_ = - this->gazebo_node_->Subscribe(this->parent_ray_sensor_->GetTopic(), + this->laser_scan_sub_ = + this->gazebo_node_->Subscribe(this->parent_ray_sensor_->GetTopic(), &GazeboRosLaser::OnScan, this); } @@ -190,12 +191,12 @@ void GazeboRosLaser::OnScan(ConstLaserScanStampedPtr &_msg) laser_msg.range_min = _msg->scan().range_min(); laser_msg.range_max = _msg->scan().range_max(); laser_msg.ranges.resize(_msg->scan().ranges_size()); - std::copy(_msg->scan().ranges().begin(), - _msg->scan().ranges().end(), + std::copy(_msg->scan().ranges().begin(), + _msg->scan().ranges().end(), laser_msg.ranges.begin()); laser_msg.intensities.resize(_msg->scan().intensities_size()); - std::copy(_msg->scan().intensities().begin(), - _msg->scan().intensities().end(), + std::copy(_msg->scan().intensities().begin(), + _msg->scan().intensities().end(), laser_msg.intensities.begin()); this->pub_queue_->push(laser_msg, this->pub_); } diff --git a/gazebo_ros/src/gazebo_ros_api_plugin.cpp b/gazebo_ros/src/gazebo_ros_api_plugin.cpp index 4fc33226d..716945f42 100644 --- a/gazebo_ros/src/gazebo_ros_api_plugin.cpp +++ b/gazebo_ros/src/gazebo_ros_api_plugin.cpp @@ -1088,14 +1088,16 @@ bool GazeboRosApiPlugin::setPhysicsProperties(gazebo_msgs::SetPhysicsProperties: // stuff only works in ODE right now ode_pe->SetAutoDisableFlag(req.ode_config.auto_disable_bodies); - ode_pe->SetSORPGSPreconIters(req.ode_config.sor_pgs_precon_iters); - ode_pe->SetSORPGSIters(req.ode_config.sor_pgs_iters); - ode_pe->SetSORPGSW(req.ode_config.sor_pgs_w); - ode_pe->SetWorldCFM(req.ode_config.cfm); - ode_pe->SetWorldERP(req.ode_config.erp); - ode_pe->SetContactSurfaceLayer(req.ode_config.contact_surface_layer); - ode_pe->SetContactMaxCorrectingVel(req.ode_config.contact_max_correcting_vel); - ode_pe->SetMaxContacts(req.ode_config.max_contacts); + ode_pe->SetParam("precon_iters", req.ode_config.sor_pgs_precon_iters); + ode_pe->SetParam("iters", req.ode_config.sor_pgs_iters); + ode_pe->SetParam("sor", req.ode_config.sor_pgs_w); + ode_pe->SetParam("cfm", req.ode_config.cfm); + ode_pe->SetParam("erp", req.ode_config.erp); + ode_pe->SetParam("contact_surface_layer", + req.ode_config.contact_surface_layer); + ode_pe->SetParam("contact_max_correcting_vel", + req.ode_config.contact_max_correcting_vel); + ode_pe->SetParam("max_contacts", req.ode_config.max_contacts); world_->SetPaused(is_paused); @@ -1117,15 +1119,24 @@ bool GazeboRosApiPlugin::getPhysicsProperties(gazebo_msgs::GetPhysicsProperties: res.gravity.z = gravity.z; // stuff only works in ODE right now - res.ode_config.auto_disable_bodies = world_->GetPhysicsEngine()->GetAutoDisableFlag(); - res.ode_config.sor_pgs_precon_iters = world_->GetPhysicsEngine()->GetSORPGSPreconIters(); - res.ode_config.sor_pgs_iters = world_->GetPhysicsEngine()->GetSORPGSIters(); - res.ode_config.sor_pgs_w = world_->GetPhysicsEngine()->GetSORPGSW(); - res.ode_config.contact_surface_layer = world_->GetPhysicsEngine()->GetContactSurfaceLayer(); - res.ode_config.contact_max_correcting_vel = world_->GetPhysicsEngine()->GetContactMaxCorrectingVel(); - res.ode_config.cfm = world_->GetPhysicsEngine()->GetWorldCFM(); - res.ode_config.erp = world_->GetPhysicsEngine()->GetWorldERP(); - res.ode_config.max_contacts = world_->GetPhysicsEngine()->GetMaxContacts(); + res.ode_config.auto_disable_bodies = + world_->GetPhysicsEngine()->GetAutoDisableFlag(); + res.ode_config.sor_pgs_precon_iters = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("precon_iters")); + res.ode_config.sor_pgs_iters = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("iters")); + res.ode_config.sor_pgs_w = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("sor")); + res.ode_config.contact_surface_layer = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("contact_surface_layer")); + res.ode_config.contact_max_correcting_vel = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("contact_max_correcting_vel")); + res.ode_config.cfm = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("cfm")); + res.ode_config.erp = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("erp")); + res.ode_config.max_contacts = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("max_contacts")); res.success = true; res.status_message = "GetPhysicsProperties: got properties"; @@ -1154,23 +1165,23 @@ bool GazeboRosApiPlugin::setJointProperties(gazebo_msgs::SetJointProperties::Req for(unsigned int i=0;i< req.ode_joint_config.damping.size();i++) joint->SetDamping(i,req.ode_joint_config.damping[i]); for(unsigned int i=0;i< req.ode_joint_config.hiStop.size();i++) - joint->SetAttribute("hi_stop",i,req.ode_joint_config.hiStop[i]); + joint->SetParam("hi_stop",i,req.ode_joint_config.hiStop[i]); for(unsigned int i=0;i< req.ode_joint_config.loStop.size();i++) - joint->SetAttribute("lo_stop",i,req.ode_joint_config.loStop[i]); + joint->SetParam("lo_stop",i,req.ode_joint_config.loStop[i]); for(unsigned int i=0;i< req.ode_joint_config.erp.size();i++) - joint->SetAttribute("erp",i,req.ode_joint_config.erp[i]); + joint->SetParam("erp",i,req.ode_joint_config.erp[i]); for(unsigned int i=0;i< req.ode_joint_config.cfm.size();i++) - joint->SetAttribute("cfm",i,req.ode_joint_config.cfm[i]); + joint->SetParam("cfm",i,req.ode_joint_config.cfm[i]); for(unsigned int i=0;i< req.ode_joint_config.stop_erp.size();i++) - joint->SetAttribute("stop_erp",i,req.ode_joint_config.stop_erp[i]); + joint->SetParam("stop_erp",i,req.ode_joint_config.stop_erp[i]); for(unsigned int i=0;i< req.ode_joint_config.stop_cfm.size();i++) - joint->SetAttribute("stop_cfm",i,req.ode_joint_config.stop_cfm[i]); + joint->SetParam("stop_cfm",i,req.ode_joint_config.stop_cfm[i]); for(unsigned int i=0;i< req.ode_joint_config.fudge_factor.size();i++) - joint->SetAttribute("fudge_factor",i,req.ode_joint_config.fudge_factor[i]); + joint->SetParam("fudge_factor",i,req.ode_joint_config.fudge_factor[i]); for(unsigned int i=0;i< req.ode_joint_config.fmax.size();i++) - joint->SetAttribute("fmax",i,req.ode_joint_config.fmax[i]); + joint->SetParam("fmax",i,req.ode_joint_config.fmax[i]); for(unsigned int i=0;i< req.ode_joint_config.vel.size();i++) - joint->SetAttribute("vel",i,req.ode_joint_config.vel[i]); + joint->SetParam("vel",i,req.ode_joint_config.vel[i]); res.success = true; res.status_message = "SetJointProperties: properties set"; diff --git a/gazebo_ros_control/src/default_robot_hw_sim.cpp b/gazebo_ros_control/src/default_robot_hw_sim.cpp index 11882922a..864149c3b 100644 --- a/gazebo_ros_control/src/default_robot_hw_sim.cpp +++ b/gazebo_ros_control/src/default_robot_hw_sim.cpp @@ -152,7 +152,8 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim joint_position_command_[j] = 0.0; joint_velocity_command_[j] = 0.0; - const std::string& hardware_interface = transmissions[j].actuators_[0].hardware_interface_; + const std::string &hardware_interface = + transmissions[j].actuators_[0].hardware_interfaces_[0]; // Debug ROS_DEBUG_STREAM_NAMED("default_robot_hw_sim","Loading joint '" << joint_names_[j] @@ -288,7 +289,7 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim break; case POSITION: - sim_joints_[j]->SetAngle(0, joint_position_command_[j]); + sim_joints_[j]->SetPosition(0, joint_position_command_[j]); break; case POSITION_PID: diff --git a/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp b/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp index 88723320f..a70515ef0 100644 --- a/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp +++ b/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp @@ -148,7 +148,8 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element const std::string urdf_string = getURDF(robot_description_); if (!parseTransmissionsFromURDF(urdf_string)) { - ROS_ERROR("gazebo_ros_control", "Error parsing URDF in gazebo_ros_control plugin, plugin not active.\n"); + ROS_ERROR_NAMED("gazebo_ros_control", + "Error parsing URDF in gazebo_ros_control plugin(plugin not active).\n"); return; } From 885ee0b21bd1c2dae52135e3f2cc6566cd73a0e1 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Thu, 31 Jul 2014 13:57:16 -0700 Subject: [PATCH 121/153] Fix to work with gazebo3 --- gazebo_ros_control/src/default_robot_hw_sim.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gazebo_ros_control/src/default_robot_hw_sim.cpp b/gazebo_ros_control/src/default_robot_hw_sim.cpp index 864149c3b..87c1fc274 100644 --- a/gazebo_ros_control/src/default_robot_hw_sim.cpp +++ b/gazebo_ros_control/src/default_robot_hw_sim.cpp @@ -289,7 +289,11 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim break; case POSITION: +#if GAZEBO_VERSION_MAJOR >= 4 sim_joints_[j]->SetPosition(0, joint_position_command_[j]); +#else + sim_joints_[j]->SetAngle(0, joint_position_command_[j]); +#endif break; case POSITION_PID: From 78216fb4053b653cd144ac1953dde36c14c58fc7 Mon Sep 17 00:00:00 2001 From: osrf Date: Thu, 31 Jul 2014 14:26:19 -0700 Subject: [PATCH 122/153] Update for hydro + gazebo 1.9 --- gazebo_ros/src/gazebo_ros_api_plugin.cpp | 44 +++++++++++++++++++ .../src/default_robot_hw_sim.cpp | 5 +++ 2 files changed, 49 insertions(+) diff --git a/gazebo_ros/src/gazebo_ros_api_plugin.cpp b/gazebo_ros/src/gazebo_ros_api_plugin.cpp index 716945f42..ee22f4567 100644 --- a/gazebo_ros/src/gazebo_ros_api_plugin.cpp +++ b/gazebo_ros/src/gazebo_ros_api_plugin.cpp @@ -21,6 +21,7 @@ */ #include +#include #include "gazebo_ros_api_plugin.h" namespace gazebo @@ -1088,6 +1089,7 @@ bool GazeboRosApiPlugin::setPhysicsProperties(gazebo_msgs::SetPhysicsProperties: // stuff only works in ODE right now ode_pe->SetAutoDisableFlag(req.ode_config.auto_disable_bodies); +#if GAZEBO_MAJOR_VERSION >= 4 ode_pe->SetParam("precon_iters", req.ode_config.sor_pgs_precon_iters); ode_pe->SetParam("iters", req.ode_config.sor_pgs_iters); ode_pe->SetParam("sor", req.ode_config.sor_pgs_w); @@ -1098,6 +1100,16 @@ bool GazeboRosApiPlugin::setPhysicsProperties(gazebo_msgs::SetPhysicsProperties: ode_pe->SetParam("contact_max_correcting_vel", req.ode_config.contact_max_correcting_vel); ode_pe->SetParam("max_contacts", req.ode_config.max_contacts); +#else + ode_pe->SetSORPGSPreconIters(req.ode_config.sor_pgs_precon_iters); + ode_pe->SetSORPGSIters(req.ode_config.sor_pgs_iters); + ode_pe->SetSORPGSW(req.ode_config.sor_pgs_w); + ode_pe->SetWorldCFM(req.ode_config.cfm); + ode_pe->SetWorldERP(req.ode_config.erp); + ode_pe->SetContactSurfaceLayer(req.ode_config.contact_surface_layer); + ode_pe->SetContactMaxCorrectingVel(req.ode_config.contact_max_correcting_vel); + ode_pe->SetMaxContacts(req.ode_config.max_contacts); +#endif world_->SetPaused(is_paused); @@ -1121,6 +1133,7 @@ bool GazeboRosApiPlugin::getPhysicsProperties(gazebo_msgs::GetPhysicsProperties: // stuff only works in ODE right now res.ode_config.auto_disable_bodies = world_->GetPhysicsEngine()->GetAutoDisableFlag(); +#if GAZEBO_MAJOR_VERSION >= 4 res.ode_config.sor_pgs_precon_iters = boost::any_cast( world_->GetPhysicsEngine()->GetParam("precon_iters")); res.ode_config.sor_pgs_iters = boost::any_cast( @@ -1137,6 +1150,16 @@ bool GazeboRosApiPlugin::getPhysicsProperties(gazebo_msgs::GetPhysicsProperties: world_->GetPhysicsEngine()->GetParam("erp")); res.ode_config.max_contacts = boost::any_cast( world_->GetPhysicsEngine()->GetParam("max_contacts")); +#else + res.ode_config.sor_pgs_precon_iters = world_->GetPhysicsEngine()->GetSORPGSPreconIters(); + res.ode_config.sor_pgs_iters = world_->GetPhysicsEngine()->GetSORPGSIters(); + res.ode_config.sor_pgs_w = world_->GetPhysicsEngine()->GetSORPGSW(); + res.ode_config.contact_surface_layer = world_->GetPhysicsEngine()->GetContactSurfaceLayer(); + res.ode_config.contact_max_correcting_vel = world_->GetPhysicsEngine()->GetContactMaxCorrectingVel(); + res.ode_config.cfm = world_->GetPhysicsEngine()->GetWorldCFM(); + res.ode_config.erp = world_->GetPhysicsEngine()->GetWorldERP(); + res.ode_config.max_contacts = world_->GetPhysicsEngine()->GetMaxContacts(); +#endif res.success = true; res.status_message = "GetPhysicsProperties: got properties"; @@ -1164,6 +1187,7 @@ bool GazeboRosApiPlugin::setJointProperties(gazebo_msgs::SetJointProperties::Req { for(unsigned int i=0;i< req.ode_joint_config.damping.size();i++) joint->SetDamping(i,req.ode_joint_config.damping[i]); +#if GAZEBO_MAJOR_VERSION >= 4 for(unsigned int i=0;i< req.ode_joint_config.hiStop.size();i++) joint->SetParam("hi_stop",i,req.ode_joint_config.hiStop[i]); for(unsigned int i=0;i< req.ode_joint_config.loStop.size();i++) @@ -1182,6 +1206,26 @@ bool GazeboRosApiPlugin::setJointProperties(gazebo_msgs::SetJointProperties::Req joint->SetParam("fmax",i,req.ode_joint_config.fmax[i]); for(unsigned int i=0;i< req.ode_joint_config.vel.size();i++) joint->SetParam("vel",i,req.ode_joint_config.vel[i]); +#else + for(unsigned int i=0;i< req.ode_joint_config.hiStop.size();i++) + joint->SetAttribute("hi_stop",i,req.ode_joint_config.hiStop[i]); + for(unsigned int i=0;i< req.ode_joint_config.loStop.size();i++) + joint->SetAttribute("lo_stop",i,req.ode_joint_config.loStop[i]); + for(unsigned int i=0;i< req.ode_joint_config.erp.size();i++) + joint->SetAttribute("erp",i,req.ode_joint_config.erp[i]); + for(unsigned int i=0;i< req.ode_joint_config.cfm.size();i++) + joint->SetAttribute("cfm",i,req.ode_joint_config.cfm[i]); + for(unsigned int i=0;i< req.ode_joint_config.stop_erp.size();i++) + joint->SetAttribute("stop_erp",i,req.ode_joint_config.stop_erp[i]); + for(unsigned int i=0;i< req.ode_joint_config.stop_cfm.size();i++) + joint->SetAttribute("stop_cfm",i,req.ode_joint_config.stop_cfm[i]); + for(unsigned int i=0;i< req.ode_joint_config.fudge_factor.size();i++) + joint->SetAttribute("fudge_factor",i,req.ode_joint_config.fudge_factor[i]); + for(unsigned int i=0;i< req.ode_joint_config.fmax.size();i++) + joint->SetAttribute("fmax",i,req.ode_joint_config.fmax[i]); + for(unsigned int i=0;i< req.ode_joint_config.vel.size();i++) + joint->SetAttribute("vel",i,req.ode_joint_config.vel[i]); +#endif res.success = true; res.status_message = "SetJointProperties: properties set"; diff --git a/gazebo_ros_control/src/default_robot_hw_sim.cpp b/gazebo_ros_control/src/default_robot_hw_sim.cpp index 87c1fc274..dadf304de 100644 --- a/gazebo_ros_control/src/default_robot_hw_sim.cpp +++ b/gazebo_ros_control/src/default_robot_hw_sim.cpp @@ -152,8 +152,13 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim joint_position_command_[j] = 0.0; joint_velocity_command_[j] = 0.0; +#if ROS_VERSION_MINOR > 10 || ROS_VERSION_MAJOR > 1 const std::string &hardware_interface = transmissions[j].actuators_[0].hardware_interfaces_[0]; +#else + const std::string &hardware_interface = + transmissions[j].actuators_[0].hardware_interface_; +#endif // Debug ROS_DEBUG_STREAM_NAMED("default_robot_hw_sim","Loading joint '" << joint_names_[j] From ca25590474cf9cd9db97239ffd44efd93d95dbec Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Thu, 31 Jul 2014 14:32:45 -0700 Subject: [PATCH 123/153] Removed a few warnings --- gazebo_ros/src/gazebo_ros_api_plugin.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gazebo_ros/src/gazebo_ros_api_plugin.cpp b/gazebo_ros/src/gazebo_ros_api_plugin.cpp index ee22f4567..1114f8768 100644 --- a/gazebo_ros/src/gazebo_ros_api_plugin.cpp +++ b/gazebo_ros/src/gazebo_ros_api_plugin.cpp @@ -1089,7 +1089,7 @@ bool GazeboRosApiPlugin::setPhysicsProperties(gazebo_msgs::SetPhysicsProperties: // stuff only works in ODE right now ode_pe->SetAutoDisableFlag(req.ode_config.auto_disable_bodies); -#if GAZEBO_MAJOR_VERSION >= 4 +#if GAZEBO_MAJOR_VERSION >= 3 ode_pe->SetParam("precon_iters", req.ode_config.sor_pgs_precon_iters); ode_pe->SetParam("iters", req.ode_config.sor_pgs_iters); ode_pe->SetParam("sor", req.ode_config.sor_pgs_w); @@ -1133,7 +1133,7 @@ bool GazeboRosApiPlugin::getPhysicsProperties(gazebo_msgs::GetPhysicsProperties: // stuff only works in ODE right now res.ode_config.auto_disable_bodies = world_->GetPhysicsEngine()->GetAutoDisableFlag(); -#if GAZEBO_MAJOR_VERSION >= 4 +#if GAZEBO_MAJOR_VERSION >= 3 res.ode_config.sor_pgs_precon_iters = boost::any_cast( world_->GetPhysicsEngine()->GetParam("precon_iters")); res.ode_config.sor_pgs_iters = boost::any_cast( From cafd3e217512f16d9dbac6a65a9226ff5958e2d6 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Thu, 31 Jul 2014 14:34:53 -0700 Subject: [PATCH 124/153] Reduced changes --- gazebo_ros_control/src/gazebo_ros_control_plugin.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp b/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp index a70515ef0..120432e38 100644 --- a/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp +++ b/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp @@ -148,8 +148,7 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element const std::string urdf_string = getURDF(robot_description_); if (!parseTransmissionsFromURDF(urdf_string)) { - ROS_ERROR_NAMED("gazebo_ros_control", - "Error parsing URDF in gazebo_ros_control plugin(plugin not active).\n"); + ROS_ERROR_NAMED("gazebo_ros_control", "Error parsing URDF in gazebo_ros_control plugin, plugin not active.\n"); return; } From ac88561c8fd189304753b6a72bcf720159275a56 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Mon, 4 Aug 2014 07:59:24 -0700 Subject: [PATCH 125/153] Fixed boost any cast --- gazebo_ros/src/gazebo_ros_api_plugin.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gazebo_ros/src/gazebo_ros_api_plugin.cpp b/gazebo_ros/src/gazebo_ros_api_plugin.cpp index 1114f8768..21da6c34a 100644 --- a/gazebo_ros/src/gazebo_ros_api_plugin.cpp +++ b/gazebo_ros/src/gazebo_ros_api_plugin.cpp @@ -1134,9 +1134,9 @@ bool GazeboRosApiPlugin::getPhysicsProperties(gazebo_msgs::GetPhysicsProperties: res.ode_config.auto_disable_bodies = world_->GetPhysicsEngine()->GetAutoDisableFlag(); #if GAZEBO_MAJOR_VERSION >= 3 - res.ode_config.sor_pgs_precon_iters = boost::any_cast( + res.ode_config.sor_pgs_precon_iters = boost::any_cast( world_->GetPhysicsEngine()->GetParam("precon_iters")); - res.ode_config.sor_pgs_iters = boost::any_cast( + res.ode_config.sor_pgs_iters = boost::any_cast( world_->GetPhysicsEngine()->GetParam("iters")); res.ode_config.sor_pgs_w = boost::any_cast( world_->GetPhysicsEngine()->GetParam("sor")); @@ -1148,7 +1148,7 @@ bool GazeboRosApiPlugin::getPhysicsProperties(gazebo_msgs::GetPhysicsProperties: world_->GetPhysicsEngine()->GetParam("cfm")); res.ode_config.erp = boost::any_cast( world_->GetPhysicsEngine()->GetParam("erp")); - res.ode_config.max_contacts = boost::any_cast( + res.ode_config.max_contacts = boost::any_cast( world_->GetPhysicsEngine()->GetParam("max_contacts")); #else res.ode_config.sor_pgs_precon_iters = world_->GetPhysicsEngine()->GetSORPGSPreconIters(); From c2704f0f112457469ce38f93c19a9675ff2921d4 Mon Sep 17 00:00:00 2001 From: Jose Luis Rivero Date: Wed, 6 Aug 2014 01:03:13 +0200 Subject: [PATCH 126/153] Port fix_build branch for indigo-devel See pull request #221 --- gazebo_plugins/src/gazebo_ros_gpu_laser.cpp | 7 ++- gazebo_plugins/src/gazebo_ros_laser.cpp | 27 ++++----- gazebo_ros/src/gazebo_ros_api_plugin.cpp | 57 ++++++++++++++++++- .../src/default_robot_hw_sim.cpp | 4 ++ 4 files changed, 78 insertions(+), 17 deletions(-) diff --git a/gazebo_plugins/src/gazebo_ros_gpu_laser.cpp b/gazebo_plugins/src/gazebo_ros_gpu_laser.cpp index 49c6c0d3f..811fc81b2 100644 --- a/gazebo_plugins/src/gazebo_ros_gpu_laser.cpp +++ b/gazebo_plugins/src/gazebo_ros_gpu_laser.cpp @@ -33,6 +33,7 @@ #include #include #include +#include #include #include @@ -124,9 +125,9 @@ void GazeboRosLaser::LoadThread() this->gazebo_node_->Init(this->world_name_); this->pmq.startServiceThread(); - + this->rosnode_ = new ros::NodeHandle(this->robot_namespace_); - + this->tf_prefix_ = tf::getPrefixParam(*this->rosnode_); if(this->tf_prefix_.empty()) { this->tf_prefix_ = this->robot_namespace_; @@ -134,7 +135,7 @@ void GazeboRosLaser::LoadThread() } ROS_INFO("GPU Laser Plugin (ns = %s) , set to \"%s\"", this->robot_namespace_.c_str(), this->tf_prefix_.c_str()); - + // resolve tf prefix this->frame_name_ = tf::resolve(this->tf_prefix_, this->frame_name_); diff --git a/gazebo_plugins/src/gazebo_ros_laser.cpp b/gazebo_plugins/src/gazebo_ros_laser.cpp index 9f099258d..815c456e1 100644 --- a/gazebo_plugins/src/gazebo_ros_laser.cpp +++ b/gazebo_plugins/src/gazebo_ros_laser.cpp @@ -33,6 +33,7 @@ #include #include #include +#include #include #include @@ -78,7 +79,7 @@ void GazeboRosLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) gzthrow("GazeboRosLaser controller requires a Ray Sensor as its parent"); this->robot_namespace_ = GetRobotNamespace(_parent, _sdf, "Laser"); - + if (!this->sdf->HasElement("frameName")) { ROS_INFO("Laser plugin missing , defaults to /world"); @@ -86,8 +87,8 @@ void GazeboRosLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) } else this->frame_name_ = this->sdf->Get("frameName"); - - + + if (!this->sdf->HasElement("topicName")) { ROS_INFO("Laser plugin missing , defaults to /world"); @@ -104,8 +105,8 @@ void GazeboRosLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. " << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); return; - } - + } + ROS_INFO ( "Starting Laser Plugin (ns = %s)!", this->robot_namespace_.c_str() ); // ros callback queue for processing subscription this->deferred_load_thread_ = boost::thread( @@ -123,7 +124,7 @@ void GazeboRosLaser::LoadThread() this->pmq.startServiceThread(); this->rosnode_ = new ros::NodeHandle(this->robot_namespace_); - + this->tf_prefix_ = tf::getPrefixParam(*this->rosnode_); if(this->tf_prefix_.empty()) { this->tf_prefix_ = this->robot_namespace_; @@ -131,7 +132,7 @@ void GazeboRosLaser::LoadThread() } ROS_INFO("Laser Plugin (ns = %s) , set to \"%s\"", this->robot_namespace_.c_str(), this->tf_prefix_.c_str()); - + // resolve tf prefix this->frame_name_ = tf::resolve(this->tf_prefix_, this->frame_name_); @@ -159,8 +160,8 @@ void GazeboRosLaser::LaserConnect() { this->laser_connect_count_++; if (this->laser_connect_count_ == 1) - this->laser_scan_sub_ = - this->gazebo_node_->Subscribe(this->parent_ray_sensor_->GetTopic(), + this->laser_scan_sub_ = + this->gazebo_node_->Subscribe(this->parent_ray_sensor_->GetTopic(), &GazeboRosLaser::OnScan, this); } @@ -190,12 +191,12 @@ void GazeboRosLaser::OnScan(ConstLaserScanStampedPtr &_msg) laser_msg.range_min = _msg->scan().range_min(); laser_msg.range_max = _msg->scan().range_max(); laser_msg.ranges.resize(_msg->scan().ranges_size()); - std::copy(_msg->scan().ranges().begin(), - _msg->scan().ranges().end(), + std::copy(_msg->scan().ranges().begin(), + _msg->scan().ranges().end(), laser_msg.ranges.begin()); laser_msg.intensities.resize(_msg->scan().intensities_size()); - std::copy(_msg->scan().intensities().begin(), - _msg->scan().intensities().end(), + std::copy(_msg->scan().intensities().begin(), + _msg->scan().intensities().end(), laser_msg.intensities.begin()); this->pub_queue_->push(laser_msg, this->pub_); } diff --git a/gazebo_ros/src/gazebo_ros_api_plugin.cpp b/gazebo_ros/src/gazebo_ros_api_plugin.cpp index 4fc33226d..21da6c34a 100644 --- a/gazebo_ros/src/gazebo_ros_api_plugin.cpp +++ b/gazebo_ros/src/gazebo_ros_api_plugin.cpp @@ -21,6 +21,7 @@ */ #include +#include #include "gazebo_ros_api_plugin.h" namespace gazebo @@ -1088,6 +1089,18 @@ bool GazeboRosApiPlugin::setPhysicsProperties(gazebo_msgs::SetPhysicsProperties: // stuff only works in ODE right now ode_pe->SetAutoDisableFlag(req.ode_config.auto_disable_bodies); +#if GAZEBO_MAJOR_VERSION >= 3 + ode_pe->SetParam("precon_iters", req.ode_config.sor_pgs_precon_iters); + ode_pe->SetParam("iters", req.ode_config.sor_pgs_iters); + ode_pe->SetParam("sor", req.ode_config.sor_pgs_w); + ode_pe->SetParam("cfm", req.ode_config.cfm); + ode_pe->SetParam("erp", req.ode_config.erp); + ode_pe->SetParam("contact_surface_layer", + req.ode_config.contact_surface_layer); + ode_pe->SetParam("contact_max_correcting_vel", + req.ode_config.contact_max_correcting_vel); + ode_pe->SetParam("max_contacts", req.ode_config.max_contacts); +#else ode_pe->SetSORPGSPreconIters(req.ode_config.sor_pgs_precon_iters); ode_pe->SetSORPGSIters(req.ode_config.sor_pgs_iters); ode_pe->SetSORPGSW(req.ode_config.sor_pgs_w); @@ -1096,6 +1109,7 @@ bool GazeboRosApiPlugin::setPhysicsProperties(gazebo_msgs::SetPhysicsProperties: ode_pe->SetContactSurfaceLayer(req.ode_config.contact_surface_layer); ode_pe->SetContactMaxCorrectingVel(req.ode_config.contact_max_correcting_vel); ode_pe->SetMaxContacts(req.ode_config.max_contacts); +#endif world_->SetPaused(is_paused); @@ -1117,7 +1131,26 @@ bool GazeboRosApiPlugin::getPhysicsProperties(gazebo_msgs::GetPhysicsProperties: res.gravity.z = gravity.z; // stuff only works in ODE right now - res.ode_config.auto_disable_bodies = world_->GetPhysicsEngine()->GetAutoDisableFlag(); + res.ode_config.auto_disable_bodies = + world_->GetPhysicsEngine()->GetAutoDisableFlag(); +#if GAZEBO_MAJOR_VERSION >= 3 + res.ode_config.sor_pgs_precon_iters = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("precon_iters")); + res.ode_config.sor_pgs_iters = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("iters")); + res.ode_config.sor_pgs_w = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("sor")); + res.ode_config.contact_surface_layer = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("contact_surface_layer")); + res.ode_config.contact_max_correcting_vel = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("contact_max_correcting_vel")); + res.ode_config.cfm = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("cfm")); + res.ode_config.erp = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("erp")); + res.ode_config.max_contacts = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("max_contacts")); +#else res.ode_config.sor_pgs_precon_iters = world_->GetPhysicsEngine()->GetSORPGSPreconIters(); res.ode_config.sor_pgs_iters = world_->GetPhysicsEngine()->GetSORPGSIters(); res.ode_config.sor_pgs_w = world_->GetPhysicsEngine()->GetSORPGSW(); @@ -1126,6 +1159,7 @@ bool GazeboRosApiPlugin::getPhysicsProperties(gazebo_msgs::GetPhysicsProperties: res.ode_config.cfm = world_->GetPhysicsEngine()->GetWorldCFM(); res.ode_config.erp = world_->GetPhysicsEngine()->GetWorldERP(); res.ode_config.max_contacts = world_->GetPhysicsEngine()->GetMaxContacts(); +#endif res.success = true; res.status_message = "GetPhysicsProperties: got properties"; @@ -1153,6 +1187,26 @@ bool GazeboRosApiPlugin::setJointProperties(gazebo_msgs::SetJointProperties::Req { for(unsigned int i=0;i< req.ode_joint_config.damping.size();i++) joint->SetDamping(i,req.ode_joint_config.damping[i]); +#if GAZEBO_MAJOR_VERSION >= 4 + for(unsigned int i=0;i< req.ode_joint_config.hiStop.size();i++) + joint->SetParam("hi_stop",i,req.ode_joint_config.hiStop[i]); + for(unsigned int i=0;i< req.ode_joint_config.loStop.size();i++) + joint->SetParam("lo_stop",i,req.ode_joint_config.loStop[i]); + for(unsigned int i=0;i< req.ode_joint_config.erp.size();i++) + joint->SetParam("erp",i,req.ode_joint_config.erp[i]); + for(unsigned int i=0;i< req.ode_joint_config.cfm.size();i++) + joint->SetParam("cfm",i,req.ode_joint_config.cfm[i]); + for(unsigned int i=0;i< req.ode_joint_config.stop_erp.size();i++) + joint->SetParam("stop_erp",i,req.ode_joint_config.stop_erp[i]); + for(unsigned int i=0;i< req.ode_joint_config.stop_cfm.size();i++) + joint->SetParam("stop_cfm",i,req.ode_joint_config.stop_cfm[i]); + for(unsigned int i=0;i< req.ode_joint_config.fudge_factor.size();i++) + joint->SetParam("fudge_factor",i,req.ode_joint_config.fudge_factor[i]); + for(unsigned int i=0;i< req.ode_joint_config.fmax.size();i++) + joint->SetParam("fmax",i,req.ode_joint_config.fmax[i]); + for(unsigned int i=0;i< req.ode_joint_config.vel.size();i++) + joint->SetParam("vel",i,req.ode_joint_config.vel[i]); +#else for(unsigned int i=0;i< req.ode_joint_config.hiStop.size();i++) joint->SetAttribute("hi_stop",i,req.ode_joint_config.hiStop[i]); for(unsigned int i=0;i< req.ode_joint_config.loStop.size();i++) @@ -1171,6 +1225,7 @@ bool GazeboRosApiPlugin::setJointProperties(gazebo_msgs::SetJointProperties::Req joint->SetAttribute("fmax",i,req.ode_joint_config.fmax[i]); for(unsigned int i=0;i< req.ode_joint_config.vel.size();i++) joint->SetAttribute("vel",i,req.ode_joint_config.vel[i]); +#endif res.success = true; res.status_message = "SetJointProperties: properties set"; diff --git a/gazebo_ros_control/src/default_robot_hw_sim.cpp b/gazebo_ros_control/src/default_robot_hw_sim.cpp index 990cc30ad..bc904ed21 100644 --- a/gazebo_ros_control/src/default_robot_hw_sim.cpp +++ b/gazebo_ros_control/src/default_robot_hw_sim.cpp @@ -300,7 +300,11 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim break; case POSITION: +#if GAZEBO_VERSION_MAJOR >= 4 + sim_joints_[j]->SetPosition(0, joint_position_command_[j]); +#else sim_joints_[j]->SetAngle(0, joint_position_command_[j]); +#endif break; case POSITION_PID: From c95a9c18fe3e24354f5882a816ae215630dc2c7e Mon Sep 17 00:00:00 2001 From: Steven Peters Date: Wed, 13 Aug 2014 10:39:11 -0700 Subject: [PATCH 127/153] Replace SetAngle with SetPosition for gazebo 4 and up --- gazebo_plugins/src/gazebo_ros_joint_pose_trajectory.cpp | 6 ++++++ gazebo_plugins/src/gazebo_ros_joint_trajectory.cpp | 6 ++++++ gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp | 4 ++++ 3 files changed, 16 insertions(+) diff --git a/gazebo_plugins/src/gazebo_ros_joint_pose_trajectory.cpp b/gazebo_plugins/src/gazebo_ros_joint_pose_trajectory.cpp index 0acbb7713..be0f0dd25 100644 --- a/gazebo_plugins/src/gazebo_ros_joint_pose_trajectory.cpp +++ b/gazebo_plugins/src/gazebo_ros_joint_pose_trajectory.cpp @@ -340,8 +340,14 @@ void GazeboRosJointPoseTrajectory::UpdateStates() { // this is not the most efficient way to set things if (this->joints_[i]) + { +#if GAZEBO_MAJOR_VERSION >= 4 + this->joints_[i]->SetPosition(0, +#else this->joints_[i]->SetAngle(0, +#endif this->points_[this->trajectory_index].positions[i]); + } } // set model pose diff --git a/gazebo_plugins/src/gazebo_ros_joint_trajectory.cpp b/gazebo_plugins/src/gazebo_ros_joint_trajectory.cpp index eb7748b00..9da5052b9 100644 --- a/gazebo_plugins/src/gazebo_ros_joint_trajectory.cpp +++ b/gazebo_plugins/src/gazebo_ros_joint_trajectory.cpp @@ -342,8 +342,14 @@ void GazeboRosJointTrajectory::UpdateStates() { // this is not the most efficient way to set things if (this->joints_[i]) + { +#if GAZEBO_MAJOR_VERSION >= 4 + this->joints_[i]->SetPosition(0, +#else this->joints_[i]->SetAngle(0, +#endif this->points_[this->trajectory_index].positions[i]); + } } // set model pose diff --git a/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp b/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp index 1642e804a..97c5ebb78 100644 --- a/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp +++ b/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp @@ -251,7 +251,11 @@ void GazeboRosTricycleDrive::motorController ( double target_speed, double targe } joint_steering_->SetVelocity ( 0, applied_steering_speed ); }else { +#if GAZEBO_MAJOR_VERSION >= 4 + joint_steering_->SetPosition ( 0, applied_angle ); +#else joint_steering_->SetAngle ( 0, math::Angle ( applied_angle ) ); +#endif } //ROS_INFO ( "target: [%3.2f, %3.2f], current: [%3.2f, %3.2f], applied: [%3.2f, %3.2f/%3.2f] !", // target_speed, target_angle, current_speed, current_angle, applied_speed, applied_angle, applied_steering_speed ); From 466ccab275d5ddbb82cb55c73e88680907e0f041 Mon Sep 17 00:00:00 2001 From: Steven Peters Date: Wed, 13 Aug 2014 10:53:47 -0700 Subject: [PATCH 128/153] Fix typo: GAZEBO_VERSION_MAJOR -> GAZEBO_MAJOR_VERSION --- gazebo_ros_control/src/default_robot_hw_sim.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gazebo_ros_control/src/default_robot_hw_sim.cpp b/gazebo_ros_control/src/default_robot_hw_sim.cpp index bc904ed21..12d9ad0ac 100644 --- a/gazebo_ros_control/src/default_robot_hw_sim.cpp +++ b/gazebo_ros_control/src/default_robot_hw_sim.cpp @@ -300,7 +300,7 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim break; case POSITION: -#if GAZEBO_VERSION_MAJOR >= 4 +#if GAZEBO_MAJOR_VERSION >= 4 sim_joints_[j]->SetPosition(0, joint_position_command_[j]); #else sim_joints_[j]->SetAngle(0, joint_position_command_[j]); From ccb39e526d7359fc712ef67d32506e1c72a07f36 Mon Sep 17 00:00:00 2001 From: John Hsu Date: Wed, 13 Aug 2014 16:07:07 -0700 Subject: [PATCH 129/153] update headers to apache 2.0 license --- .../gazebo_plugins/gazebo_ros_block_laser.h | 27 +++++++--------- .../gazebo_plugins/gazebo_ros_depth_camera.h | 28 +++++++---------- .../include/gazebo_plugins/gazebo_ros_f3d.h | 28 +++++++---------- .../include/gazebo_plugins/gazebo_ros_force.h | 27 +++++++--------- .../gazebo_plugins/gazebo_ros_hand_of_god.h | 28 +++++++---------- .../gazebo_plugins/gazebo_ros_openni_kinect.h | 28 +++++++---------- .../gazebo_plugins/gazebo_ros_template.h | 28 +++++++---------- gazebo_ros/src/gazebo_ros_api_plugin.h | 31 ++++++++----------- 8 files changed, 96 insertions(+), 129 deletions(-) diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_block_laser.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_block_laser.h index f2adce0f2..cb68d8f32 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_block_laser.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_block_laser.h @@ -1,28 +1,23 @@ /* - * Gazebo - Outdoor Multi-Robot Simulator - * Copyright (C) 2003 - * Nate Koenig & Andrew Howard + * Copyright (C) 2012-2014 Open Source Robotics Foundation * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. + * http://www.apache.org/licenses/LICENSE-2.0 * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. * - */ +*/ /* * Desc: ros laser controller. * Author: Nathan Koenig * Date: 01 Feb 2007 - * SVN: $Id: gazebo_ros_block_laser.hh 6656 2008-06-20 22:52:19Z natepak $ */ #ifndef GAZEBO_ROS_BLOCK_LASER_HH diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_depth_camera.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_depth_camera.h index ae07bc860..5535e23d1 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_depth_camera.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_depth_camera.h @@ -1,29 +1,25 @@ /* - * Gazebo - Outdoor Multi-Robot Simulator - * Copyright (C) 2003 - * Nate Koenig & Andrew Howard + * Copyright (C) 2012-2014 Open Source Robotics Foundation * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. + * http://www.apache.org/licenses/LICENSE-2.0 * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. * - */ +*/ /* * Desc: A dynamic controller plugin that publishes ROS image_raw camera_info topic for generic camera sensor. * Author: John Hsu * Date: 24 Sept 2008 - * SVN: $Id$ */ + #ifndef GAZEBO_ROS_DEPTH_CAMERA_HH #define GAZEBO_ROS_DEPTH_CAMERA_HH diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_f3d.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_f3d.h index ec487d834..a04779d49 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_f3d.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_f3d.h @@ -1,29 +1,25 @@ /* - * Gazebo - Outdoor Multi-Robot Simulator - * Copyright (C) 2003 - * Nate Koenig & Andrew Howard + * Copyright (C) 2012-2014 Open Source Robotics Foundation * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. + * http://www.apache.org/licenses/LICENSE-2.0 * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. * - */ +*/ /* * Desc: 3D Applied Force Feedback Interface * Author: John Hsu * Date: 24 Sept 2008 - * SVN: $Id$ */ + #ifndef GAZEBO_ROS_F3D_HH #define GAZEBO_ROS_F3D_HH diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.h index 39110a131..d356a01b4 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.h @@ -1,28 +1,25 @@ /* - * Gazebo - Outdoor Multi-Robot Simulator - * Copyright (C) 2003 - * Nate Koenig & Andrew Howard + * Copyright (C) 2012-2014 Open Source Robotics Foundation * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. + * http://www.apache.org/licenses/LICENSE-2.0 * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. * - */ +*/ /* * Desc: A dynamic controller plugin that performs generic force interface. * Author: John Hsu * Date: 24 Sept 2008 */ + #ifndef GAZEBO_ROS_FORCE_HH #define GAZEBO_ROS_FORCE_HH diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_hand_of_god.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_hand_of_god.h index 55fae1daa..4773b8029 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_hand_of_god.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_hand_of_god.h @@ -1,29 +1,25 @@ /* - * Gazebo - Outdoor Multi-Robot Simulator - * Copyright (C) 2003 - * Nate Koenig & Andrew Howard + * Copyright (C) 2012-2014 Open Source Robotics Foundation * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. + * http://www.apache.org/licenses/LICENSE-2.0 * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. * - */ +*/ /* * Desc: 3D position interface. * Author: Sachin Chitta and John Hsu * Date: 10 June 2008 - * SVN: $Id$ */ + #ifndef GAZEBO_ROS_TEMPLATE_HH #define GAZEBO_ROS_TEMPLATE_HH diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_openni_kinect.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_openni_kinect.h index c1f59d6eb..fe3f6ab7a 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_openni_kinect.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_openni_kinect.h @@ -1,29 +1,25 @@ /* - * Gazebo - Outdoor Multi-Robot Simulator - * Copyright (C) 2003 - * Nate Koenig & Andrew Howard + * Copyright (C) 2012-2014 Open Source Robotics Foundation * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. + * http://www.apache.org/licenses/LICENSE-2.0 * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. * - */ +*/ /* * Desc: A dynamic controller plugin that publishes ROS image_raw camera_info topic for generic camera sensor. * Author: John Hsu * Date: 24 Sept 2008 - * SVN: $Id$ */ + #ifndef GAZEBO_ROS_OPENNI_KINECT_HH #define GAZEBO_ROS_OPENNI_KINECT_HH diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_template.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_template.h index 255367dbe..4e1f18597 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_template.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_template.h @@ -1,29 +1,25 @@ /* - * Gazebo - Outdoor Multi-Robot Simulator - * Copyright (C) 2003 - * Nate Koenig & Andrew Howard + * Copyright (C) 2012-2014 Open Source Robotics Foundation * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. + * http://www.apache.org/licenses/LICENSE-2.0 * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. * - */ +*/ /* * Desc: 3D position interface. * Author: Sachin Chitta and John Hsu * Date: 10 June 2008 - * SVN: $Id$ */ + #ifndef GAZEBO_ROS_TEMPLATE_HH #define GAZEBO_ROS_TEMPLATE_HH diff --git a/gazebo_ros/src/gazebo_ros_api_plugin.h b/gazebo_ros/src/gazebo_ros_api_plugin.h index ed8b75c1b..76adbf3df 100644 --- a/gazebo_ros/src/gazebo_ros_api_plugin.h +++ b/gazebo_ros/src/gazebo_ros_api_plugin.h @@ -1,28 +1,23 @@ /* - * Gazebo - Outdoor Multi-Robot Simulator - * Copyright (C) 2003 - * Nate Koenig & Andrew Howard + * Copyright (C) 2012-2014 Open Source Robotics Foundation * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. + * http://www.apache.org/licenses/LICENSE-2.0 * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. * - */ - -/* Desc: External interfaces for Gazebo +*/ +/* + * Desc: External interfaces for Gazebo * Author: Nate Koenig, John Hsu, Dave Coleman * Date: 25 Apr 2010 - * SVN: $Id: main.cc 8598 2010-03-22 21:59:24Z hsujohnhsu $ */ #ifndef __GAZEBO_ROS_API_PLUGIN_HH__ From 4225bf4dd6bbc599679c487abc51947b841c94d0 Mon Sep 17 00:00:00 2001 From: John Hsu Date: Wed, 13 Aug 2014 16:10:31 -0700 Subject: [PATCH 130/153] update headers to apache 2.0 license --- gazebo_plugins/package.xml | 2 +- gazebo_ros/src/gazebo_ros_paths_plugin.cpp | 43 ++++++---------------- 2 files changed, 13 insertions(+), 32 deletions(-) diff --git a/gazebo_plugins/package.xml b/gazebo_plugins/package.xml index a21eea255..e60c0e43a 100644 --- a/gazebo_plugins/package.xml +++ b/gazebo_plugins/package.xml @@ -7,7 +7,7 @@ John Hsu - BSD + BSD, Apache 2.0 http://gazebosim.org/wiki/Tutorials#ROS_Integration https://github.com/ros-simulation/gazebo_ros_pkgs/issues diff --git a/gazebo_ros/src/gazebo_ros_paths_plugin.cpp b/gazebo_ros/src/gazebo_ros_paths_plugin.cpp index 38362e55f..8bff3f4ed 100644 --- a/gazebo_ros/src/gazebo_ros_paths_plugin.cpp +++ b/gazebo_ros/src/gazebo_ros_paths_plugin.cpp @@ -1,38 +1,19 @@ -/********************************************************************* - * Software License Agreement (BSD License) +/* + * Copyright (C) 2012-2014 Open Source Robotics Foundation * - * Copyright (c) 2013, Open Source Robotics Foundation - * All rights reserved. + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: + * http://www.apache.org/licenses/LICENSE-2.0 * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Open Source Robotics Foundation - * nor the names of its contributors may be - * used to endorse or promote products derived - * from this software without specific prior written permission. + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - +*/ /* * Author: John Hsu, Nate Koenig, Dave Coleman * Desc: External interfaces for Gazebo From fcc6ea3782502a2d16e5db47189e0aa59b076fb0 Mon Sep 17 00:00:00 2001 From: hsu Date: Fri, 15 Aug 2014 12:17:20 -0700 Subject: [PATCH 131/153] Update default_robot_hw_sim.cpp --- gazebo_ros_control/src/default_robot_hw_sim.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gazebo_ros_control/src/default_robot_hw_sim.cpp b/gazebo_ros_control/src/default_robot_hw_sim.cpp index dadf304de..1a45eb9c7 100644 --- a/gazebo_ros_control/src/default_robot_hw_sim.cpp +++ b/gazebo_ros_control/src/default_robot_hw_sim.cpp @@ -294,7 +294,7 @@ class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim break; case POSITION: -#if GAZEBO_VERSION_MAJOR >= 4 +#if GAZEBO_MAJOR_VERSION >= 4 sim_joints_[j]->SetPosition(0, joint_position_command_[j]); #else sim_joints_[j]->SetAngle(0, joint_position_command_[j]); From b13d90a1a6bea027df8b686abac021e09dbd48ea Mon Sep 17 00:00:00 2001 From: Jose Luis Rivero Date: Mon, 18 Aug 2014 20:20:29 +0200 Subject: [PATCH 132/153] Update changelogs for the upcoming release --- gazebo_msgs/CHANGELOG.rst | 5 ++++ gazebo_plugins/CHANGELOG.rst | 41 ++++++++++++++++++++++++++++++++ gazebo_ros/CHANGELOG.rst | 20 ++++++++++++++++ gazebo_ros_control/CHANGELOG.rst | 11 +++++++++ gazebo_ros_pkgs/CHANGELOG.rst | 6 +++++ 5 files changed, 83 insertions(+) diff --git a/gazebo_msgs/CHANGELOG.rst b/gazebo_msgs/CHANGELOG.rst index 687092315..9fa5c9d01 100644 --- a/gazebo_msgs/CHANGELOG.rst +++ b/gazebo_msgs/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package gazebo_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix repo names in package.xml's +* Contributors: Jon Binney + 2.3.5 (2014-03-26) ------------------ diff --git a/gazebo_plugins/CHANGELOG.rst b/gazebo_plugins/CHANGELOG.rst index f8d68efcc..460c04ffc 100644 --- a/gazebo_plugins/CHANGELOG.rst +++ b/gazebo_plugins/CHANGELOG.rst @@ -2,6 +2,47 @@ Changelog for package gazebo_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix build with gazebo4 and indigo +* gazebo_ros_diff_drive gazebo_ros_tricycle_drive encoderSource option names updated +* gazebo_ros_diff_drive is now able to use the wheels rotation of the optometry or the gazebo ground truth based on the 'odometrySource' parameter +* simple linear controller for the tricycle_drive added +* second robot for testing in tricycle_drive_scenario.launch added +* BDS licenses header fixed and tricycle drive plugin added +* Fix repo names in package.xml's +* ros diff drive supports now an acceleration limit +* Pioneer model: Diff_drive torque reduced +* GPU Laser test example added +* fixed gpu_laser to work with workspaces +* Merge pull request `#139 `_ from jbohren-forks/hand-of-god + Adding hand-of-god plugin +* HoG: adding install target +* hand_of_god: Adding hand-of-god plugin + ros_force: Fixing error messages to refer to the right plugin +* Remove unneeded dependency on pcl_ros + pcl_ros hasn't been released yet into indigo. I asked @wjwwood about + its status, and he pointed out that our dependency on pcl_ros + probably isn't necessary. Lo and behold, we removed it from the + header files, package.xml and CMakeLists.txt and gazebo_plugins + still compiles. +* minor fixes on relative paths in xacro for pioneer robot +* gazebo test model pionneer 3dx updated with xacro path variables +* pioneer model update for the multi_robot_scenario +* fixed camera to work with workspaces +* fixed links related to changed name +* diff drive name changed to multi robot scenario +* working camera added +* fix in pioneer xacro model for diff_drive +* Laser colour in rviz changed +* A test model for the ros_diff_drive ros_laser and joint_state_publisher added +* the ros_laser checkes now for the model name and adds it als prefix +* joint velocity fixed using radius instead of diameter +* ROS_INFO on laser plugin added to see if it starts +* gazebo_ros_diff_drive was enhanced to publish the wheels tf or the wheels joint state depending on two additinal xml options +* Gazebo ROS joint state publisher added +* Contributors: Dave Coleman, Jon Binney, Jonathan Bohren, Markus Bader, Nate Koenig, Steven Peters + 2.3.5 (2014-03-26) ------------------ * update test world for block laser diff --git a/gazebo_ros/CHANGELOG.rst b/gazebo_ros/CHANGELOG.rst index 55f1ebe1e..a76986ebf 100644 --- a/gazebo_ros/CHANGELOG.rst +++ b/gazebo_ros/CHANGELOG.rst @@ -2,6 +2,26 @@ Changelog for package gazebo_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update for hydro + gazebo 1.9 +* Fix build with gazebo4 and indigo +* Fix repo names in package.xml's +* fix issue `#198 `_ + Operator ``==`` is not recognized by sh scripts. +* fix issue `#198 `_ + Operator ``==`` is not recognized by sh scripts. +* fix issue `#198 `_ + Operator ``==`` is not recognized by sh scripts. +* fix issue `#198 `_ + Operator ``==`` is not recognized by sh scripts. +* fix issue `#198 `_ + Operator ``==`` is not recognized by sh scripts. +* Add verbose parameter + Add verbose parameter for --verbose gazebo flag +* added osx support for gazebo start scripts +* Contributors: Arn-O, Jon Binney, Markus Achtelik, Nate Koenig, Vincenzo Comito + 2.3.5 (2014-03-26) ------------------ * gazebo_ros: [less-than-minor] fix newlines diff --git a/gazebo_ros_control/CHANGELOG.rst b/gazebo_ros_control/CHANGELOG.rst index 931195e86..7af5645c0 100644 --- a/gazebo_ros_control/CHANGELOG.rst +++ b/gazebo_ros_control/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package gazebo_ros_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update default_robot_hw_sim.cpp +* Update for hydro + gazebo 1.9 +* Fix to work with gazebo3 +* Fix build with gazebo4 and indigo +* Revert 4776545, as it belongs in indigo-devel. +* Fix repo names in package.xml's +* gazebo_ros_control: default_robot_hw_sim: Suppressing pid error message, depends on `ros-controls/control_toolbox#21 `_ +* Contributors: Adolfo Rodriguez Tsouroukdissian, Dave Coleman, Jon Binney, Jonathan Bohren, Nate Koenig, John Hsu + 2.3.5 (2014-03-26) ------------------ * Removed some debugging code. diff --git a/gazebo_ros_pkgs/CHANGELOG.rst b/gazebo_ros_pkgs/CHANGELOG.rst index d5f26b773..0676c05f3 100644 --- a/gazebo_ros_pkgs/CHANGELOG.rst +++ b/gazebo_ros_pkgs/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package gazebo_ros_pkgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Updated package.xml +* Fix repo names in package.xml's +* Contributors: Dave Coleman, Jon Binney + 2.3.5 (2014-03-26) ------------------ From 4f1a8a6fe9e28bbe02a8a4c70e48b64294e08ed0 Mon Sep 17 00:00:00 2001 From: Jose Luis Rivero Date: Mon, 18 Aug 2014 20:22:23 +0200 Subject: [PATCH 133/153] 2.3.6 --- gazebo_msgs/CHANGELOG.rst | 4 ++-- gazebo_msgs/package.xml | 2 +- gazebo_plugins/CHANGELOG.rst | 4 ++-- gazebo_plugins/package.xml | 2 +- gazebo_ros/CHANGELOG.rst | 4 ++-- gazebo_ros/package.xml | 2 +- gazebo_ros_control/CHANGELOG.rst | 4 ++-- gazebo_ros_control/package.xml | 2 +- gazebo_ros_pkgs/CHANGELOG.rst | 4 ++-- gazebo_ros_pkgs/package.xml | 2 +- 10 files changed, 15 insertions(+), 15 deletions(-) diff --git a/gazebo_msgs/CHANGELOG.rst b/gazebo_msgs/CHANGELOG.rst index 9fa5c9d01..1e3897725 100644 --- a/gazebo_msgs/CHANGELOG.rst +++ b/gazebo_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.3.6 (2014-08-18) +------------------ * Fix repo names in package.xml's * Contributors: Jon Binney diff --git a/gazebo_msgs/package.xml b/gazebo_msgs/package.xml index 21217194d..7ff94209b 100644 --- a/gazebo_msgs/package.xml +++ b/gazebo_msgs/package.xml @@ -1,7 +1,7 @@ gazebo_msgs - 2.3.5 + 2.3.6 Message and service data structures for interacting with Gazebo from ROS. diff --git a/gazebo_plugins/CHANGELOG.rst b/gazebo_plugins/CHANGELOG.rst index 460c04ffc..1526458ad 100644 --- a/gazebo_plugins/CHANGELOG.rst +++ b/gazebo_plugins/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.3.6 (2014-08-18) +------------------ * Fix build with gazebo4 and indigo * gazebo_ros_diff_drive gazebo_ros_tricycle_drive encoderSource option names updated * gazebo_ros_diff_drive is now able to use the wheels rotation of the optometry or the gazebo ground truth based on the 'odometrySource' parameter diff --git a/gazebo_plugins/package.xml b/gazebo_plugins/package.xml index 8a757dac1..77b3fc8ba 100644 --- a/gazebo_plugins/package.xml +++ b/gazebo_plugins/package.xml @@ -1,6 +1,6 @@ gazebo_plugins - 2.3.5 + 2.3.6 Robot-independent Gazebo plugins for sensors, motors and dynamic reconfigurable components. diff --git a/gazebo_ros/CHANGELOG.rst b/gazebo_ros/CHANGELOG.rst index a76986ebf..517a71e2a 100644 --- a/gazebo_ros/CHANGELOG.rst +++ b/gazebo_ros/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.3.6 (2014-08-18) +------------------ * Update for hydro + gazebo 1.9 * Fix build with gazebo4 and indigo * Fix repo names in package.xml's diff --git a/gazebo_ros/package.xml b/gazebo_ros/package.xml index 587a87e38..17c4d4466 100644 --- a/gazebo_ros/package.xml +++ b/gazebo_ros/package.xml @@ -1,7 +1,7 @@ gazebo_ros - 2.3.5 + 2.3.6 Provides ROS plugins that offer message and service publishers for interfacing with Gazebo through ROS. Formally simulator_gazebo/gazebo diff --git a/gazebo_ros_control/CHANGELOG.rst b/gazebo_ros_control/CHANGELOG.rst index 7af5645c0..1853c5d2d 100644 --- a/gazebo_ros_control/CHANGELOG.rst +++ b/gazebo_ros_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_ros_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.3.6 (2014-08-18) +------------------ * Update default_robot_hw_sim.cpp * Update for hydro + gazebo 1.9 * Fix to work with gazebo3 diff --git a/gazebo_ros_control/package.xml b/gazebo_ros_control/package.xml index 1b02a020b..4b636b2f2 100644 --- a/gazebo_ros_control/package.xml +++ b/gazebo_ros_control/package.xml @@ -1,6 +1,6 @@ gazebo_ros_control - 2.3.5 + 2.3.6 gazebo_ros_control Jonathan Bohren diff --git a/gazebo_ros_pkgs/CHANGELOG.rst b/gazebo_ros_pkgs/CHANGELOG.rst index 0676c05f3..327272db9 100644 --- a/gazebo_ros_pkgs/CHANGELOG.rst +++ b/gazebo_ros_pkgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_ros_pkgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.3.6 (2014-08-18) +------------------ * Updated package.xml * Fix repo names in package.xml's * Contributors: Dave Coleman, Jon Binney diff --git a/gazebo_ros_pkgs/package.xml b/gazebo_ros_pkgs/package.xml index 3c353dc93..1e736797f 100644 --- a/gazebo_ros_pkgs/package.xml +++ b/gazebo_ros_pkgs/package.xml @@ -1,7 +1,7 @@ gazebo_ros_pkgs - 2.3.5 + 2.3.6 Interface for using ROS with the Gazebo simulator. John Hsu From 898067af14f5a4b28998695098501aa012db5e2a Mon Sep 17 00:00:00 2001 From: Jose Luis Rivero Date: Mon, 18 Aug 2014 21:43:22 +0200 Subject: [PATCH 134/153] Changelogs for upcoming release --- gazebo_msgs/CHANGELOG.rst | 3 +++ gazebo_plugins/CHANGELOG.rst | 7 +++++++ gazebo_ros/CHANGELOG.rst | 6 ++++++ gazebo_ros_control/CHANGELOG.rst | 7 +++++++ gazebo_ros_pkgs/CHANGELOG.rst | 3 +++ 5 files changed, 26 insertions(+) diff --git a/gazebo_msgs/CHANGELOG.rst b/gazebo_msgs/CHANGELOG.rst index a412ecadd..1de7de8d7 100644 --- a/gazebo_msgs/CHANGELOG.rst +++ b/gazebo_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gazebo_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.4.4 (2014-07-18) ------------------ * Fix repo names in package.xml's diff --git a/gazebo_plugins/CHANGELOG.rst b/gazebo_plugins/CHANGELOG.rst index e1c57612a..40a9cebb0 100644 --- a/gazebo_plugins/CHANGELOG.rst +++ b/gazebo_plugins/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package gazebo_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Replace SetAngle with SetPosition for gazebo 4 and up +* Port fix_build branch for indigo-devel + See pull request `#221 `_ +* Contributors: Jose Luis Rivero, Steven Peters + 2.4.4 (2014-07-18) ------------------ * Merge branch 'hydro-devel' into indigo-devel diff --git a/gazebo_ros/CHANGELOG.rst b/gazebo_ros/CHANGELOG.rst index beabba097..42b07776e 100644 --- a/gazebo_ros/CHANGELOG.rst +++ b/gazebo_ros/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package gazebo_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Port fix_build branch for indigo-devel + See pull request `#221 `_ +* Contributors: Jose Luis Rivero + 2.4.4 (2014-07-18) ------------------ * Fix repo names in package.xml's diff --git a/gazebo_ros_control/CHANGELOG.rst b/gazebo_ros_control/CHANGELOG.rst index 41286d44c..0995871fd 100644 --- a/gazebo_ros_control/CHANGELOG.rst +++ b/gazebo_ros_control/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package gazebo_ros_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Fix typo: GAZEBO_VERSION_MAJOR -> GAZEBO_MAJOR_VERSION +* Port fix_build branch for indigo-devel + See pull request `#221 `_ +* Contributors: Jose Luis Rivero, Steven Peters + 2.4.4 (2014-07-18) ------------------ * Update package.xml diff --git a/gazebo_ros_pkgs/CHANGELOG.rst b/gazebo_ros_pkgs/CHANGELOG.rst index 6f0154a56..6adf4cbee 100644 --- a/gazebo_ros_pkgs/CHANGELOG.rst +++ b/gazebo_ros_pkgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gazebo_ros_pkgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.4.4 (2014-07-18) ------------------ * Updated package.xml From 5448f9a9085ddb133f5cfad262e7ae0eff9b6e6b Mon Sep 17 00:00:00 2001 From: Jose Luis Rivero Date: Mon, 18 Aug 2014 21:44:04 +0200 Subject: [PATCH 135/153] 2.4.5 --- gazebo_msgs/CHANGELOG.rst | 4 ++-- gazebo_msgs/package.xml | 2 +- gazebo_plugins/CHANGELOG.rst | 4 ++-- gazebo_plugins/package.xml | 2 +- gazebo_ros/CHANGELOG.rst | 4 ++-- gazebo_ros/package.xml | 2 +- gazebo_ros_control/CHANGELOG.rst | 4 ++-- gazebo_ros_control/package.xml | 2 +- gazebo_ros_pkgs/CHANGELOG.rst | 4 ++-- gazebo_ros_pkgs/package.xml | 2 +- 10 files changed, 15 insertions(+), 15 deletions(-) diff --git a/gazebo_msgs/CHANGELOG.rst b/gazebo_msgs/CHANGELOG.rst index 1de7de8d7..d3601ea23 100644 --- a/gazebo_msgs/CHANGELOG.rst +++ b/gazebo_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.4.5 (2014-08-18) +------------------ 2.4.4 (2014-07-18) ------------------ diff --git a/gazebo_msgs/package.xml b/gazebo_msgs/package.xml index 43bc28464..57c7d0d98 100644 --- a/gazebo_msgs/package.xml +++ b/gazebo_msgs/package.xml @@ -1,7 +1,7 @@ gazebo_msgs - 2.4.4 + 2.4.5 Message and service data structures for interacting with Gazebo from ROS. diff --git a/gazebo_plugins/CHANGELOG.rst b/gazebo_plugins/CHANGELOG.rst index 40a9cebb0..fc60c11ae 100644 --- a/gazebo_plugins/CHANGELOG.rst +++ b/gazebo_plugins/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.4.5 (2014-08-18) +------------------ * Replace SetAngle with SetPosition for gazebo 4 and up * Port fix_build branch for indigo-devel See pull request `#221 `_ diff --git a/gazebo_plugins/package.xml b/gazebo_plugins/package.xml index a21eea255..688232b11 100644 --- a/gazebo_plugins/package.xml +++ b/gazebo_plugins/package.xml @@ -1,6 +1,6 @@ gazebo_plugins - 2.4.4 + 2.4.5 Robot-independent Gazebo plugins for sensors, motors and dynamic reconfigurable components. diff --git a/gazebo_ros/CHANGELOG.rst b/gazebo_ros/CHANGELOG.rst index 42b07776e..f9eab4183 100644 --- a/gazebo_ros/CHANGELOG.rst +++ b/gazebo_ros/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.4.5 (2014-08-18) +------------------ * Port fix_build branch for indigo-devel See pull request `#221 `_ * Contributors: Jose Luis Rivero diff --git a/gazebo_ros/package.xml b/gazebo_ros/package.xml index 49f0c1e46..e260e90fa 100644 --- a/gazebo_ros/package.xml +++ b/gazebo_ros/package.xml @@ -1,7 +1,7 @@ gazebo_ros - 2.4.4 + 2.4.5 Provides ROS plugins that offer message and service publishers for interfacing with Gazebo through ROS. Formally simulator_gazebo/gazebo diff --git a/gazebo_ros_control/CHANGELOG.rst b/gazebo_ros_control/CHANGELOG.rst index 0995871fd..732fdc695 100644 --- a/gazebo_ros_control/CHANGELOG.rst +++ b/gazebo_ros_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_ros_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.4.5 (2014-08-18) +------------------ * Fix typo: GAZEBO_VERSION_MAJOR -> GAZEBO_MAJOR_VERSION * Port fix_build branch for indigo-devel See pull request `#221 `_ diff --git a/gazebo_ros_control/package.xml b/gazebo_ros_control/package.xml index a16d5f06b..914c2cf1c 100644 --- a/gazebo_ros_control/package.xml +++ b/gazebo_ros_control/package.xml @@ -1,6 +1,6 @@ gazebo_ros_control - 2.4.4 + 2.4.5 gazebo_ros_control Jonathan Bohren diff --git a/gazebo_ros_pkgs/CHANGELOG.rst b/gazebo_ros_pkgs/CHANGELOG.rst index 6adf4cbee..d48699e51 100644 --- a/gazebo_ros_pkgs/CHANGELOG.rst +++ b/gazebo_ros_pkgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_ros_pkgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.4.5 (2014-08-18) +------------------ 2.4.4 (2014-07-18) ------------------ diff --git a/gazebo_ros_pkgs/package.xml b/gazebo_ros_pkgs/package.xml index 5b74ccd6a..83bd78c45 100644 --- a/gazebo_ros_pkgs/package.xml +++ b/gazebo_ros_pkgs/package.xml @@ -1,7 +1,7 @@ gazebo_ros_pkgs - 2.4.4 + 2.4.5 Interface for using ROS with the Gazebo simulator. John Hsu From c1a73b0d4f22d1295df66086acaebec78e54eddc Mon Sep 17 00:00:00 2001 From: John Hsu Date: Wed, 27 Aug 2014 17:38:10 -0700 Subject: [PATCH 136/153] compatibility with gazebo 4.x --- gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp b/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp index 1642e804a..97c5ebb78 100644 --- a/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp +++ b/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp @@ -251,7 +251,11 @@ void GazeboRosTricycleDrive::motorController ( double target_speed, double targe } joint_steering_->SetVelocity ( 0, applied_steering_speed ); }else { +#if GAZEBO_MAJOR_VERSION >= 4 + joint_steering_->SetPosition ( 0, applied_angle ); +#else joint_steering_->SetAngle ( 0, math::Angle ( applied_angle ) ); +#endif } //ROS_INFO ( "target: [%3.2f, %3.2f], current: [%3.2f, %3.2f], applied: [%3.2f, %3.2f/%3.2f] !", // target_speed, target_angle, current_speed, current_angle, applied_speed, applied_angle, applied_steering_speed ); From 72fd65374f68d2e7f9ee9cd4138584e128510008 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Wed, 27 Aug 2014 19:06:26 -0700 Subject: [PATCH 137/153] Fixes for calling GetParam() with different physic engines. --- gazebo_ros/src/gazebo_ros_api_plugin.cpp | 96 ++++++++++++++++++++---- 1 file changed, 80 insertions(+), 16 deletions(-) diff --git a/gazebo_ros/src/gazebo_ros_api_plugin.cpp b/gazebo_ros/src/gazebo_ros_api_plugin.cpp index 21da6c34a..528f45fde 100644 --- a/gazebo_ros/src/gazebo_ros_api_plugin.cpp +++ b/gazebo_ros/src/gazebo_ros_api_plugin.cpp @@ -1134,22 +1134,86 @@ bool GazeboRosApiPlugin::getPhysicsProperties(gazebo_msgs::GetPhysicsProperties: res.ode_config.auto_disable_bodies = world_->GetPhysicsEngine()->GetAutoDisableFlag(); #if GAZEBO_MAJOR_VERSION >= 3 - res.ode_config.sor_pgs_precon_iters = boost::any_cast( - world_->GetPhysicsEngine()->GetParam("precon_iters")); - res.ode_config.sor_pgs_iters = boost::any_cast( - world_->GetPhysicsEngine()->GetParam("iters")); - res.ode_config.sor_pgs_w = boost::any_cast( - world_->GetPhysicsEngine()->GetParam("sor")); - res.ode_config.contact_surface_layer = boost::any_cast( - world_->GetPhysicsEngine()->GetParam("contact_surface_layer")); - res.ode_config.contact_max_correcting_vel = boost::any_cast( - world_->GetPhysicsEngine()->GetParam("contact_max_correcting_vel")); - res.ode_config.cfm = boost::any_cast( - world_->GetPhysicsEngine()->GetParam("cfm")); - res.ode_config.erp = boost::any_cast( - world_->GetPhysicsEngine()->GetParam("erp")); - res.ode_config.max_contacts = boost::any_cast( - world_->GetPhysicsEngine()->GetParam("max_contacts")); + try + { + res.ode_config.sor_pgs_precon_iters = + boost::any_cast(world_->GetPhysicsEngine()->GetParam("precon_iters")); + } + catch(boost::bad_lexical_cast &e) + { + ROS_ERROR("precon_iters not supported by current physics engine[%s].\n", e.what()); + res.ode_config.sor_pgs_precon_iters = 0; + } + try + { + res.ode_config.sor_pgs_iters = + boost::any_cast(world_->GetPhysicsEngine()->GetParam("iters")); + } + catch(boost::bad_lexical_cast &e) + { + ROS_ERROR("iters not supported by current physics engine[%s].\n", e.what()); + res.ode_config.sor_pgs_iters = 0; + } + try + { + res.ode_config.sor_pgs_w = + boost::any_cast(world_->GetPhysicsEngine()->GetParam("sor")); + } + catch(boost::bad_lexical_cast &e) + { + ROS_ERROR("sor not supported by current physics engine[%s].\n", e.what()); + res.ode_config.sor_pgs_w = 0.0; + } + try + { + res.ode_config.contact_surface_layer = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("contact_surface_layer")); + } + catch(boost::bad_lexical_cast &e) + { + ROS_ERROR("contact_surface_layer not supported by current physics engine[%s].\n", e.what()); + res.ode_config.contact_surface_layer = 0.0; + } + try + { + res.ode_config.contact_max_correcting_vel = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("contact_max_correcting_vel")); + } + catch(boost::bad_lexical_cast &e) + { + ROS_ERROR("contact_max_correcting_vel not supported by current physics engine[%s].\n", e.what()); + res.ode_config.contact_max_correcting_vel = 0.0; + } + try + { + res.ode_config.cfm = + boost::any_cast(world_->GetPhysicsEngine()->GetParam("cfm")); + } + catch(boost::bad_lexical_cast &e) + { + ROS_ERROR("cfm not supported by current physics engine[%s].\n", e.what()); + res.ode_config.cfm = 0.0; + } + try + { + res.ode_config.erp = + boost::any_cast(world_->GetPhysicsEngine()->GetParam("erp")); + } + catch(boost::bad_lexical_cast &e) + { + ROS_ERROR("erp not supported by current physics engine[%s].\n", e.what()); + res.ode_config.erp = 0.0; + } + try + { + res.ode_config.max_contacts = + boost::any_cast(world_->GetPhysicsEngine()->GetParam("max_contacts")); + } + catch(boost::bad_lexical_cast &e) + { + ROS_ERROR("max_contacts not supported by current physics engine[%s].\n", e.what()); + res.ode_config.max_contacts = 0; + } #else res.ode_config.sor_pgs_precon_iters = world_->GetPhysicsEngine()->GetSORPGSPreconIters(); res.ode_config.sor_pgs_iters = world_->GetPhysicsEngine()->GetSORPGSIters(); From 748612b0e298f70f72993e37c1e796c92b429ccc Mon Sep 17 00:00:00 2001 From: John Hsu Date: Thu, 28 Aug 2014 16:15:12 -0700 Subject: [PATCH 138/153] check physics engine type before calling set_physics_properties and get_physics_properteis --- gazebo_ros/src/gazebo_ros_api_plugin.cpp | 134 +++++++++++++---------- 1 file changed, 77 insertions(+), 57 deletions(-) diff --git a/gazebo_ros/src/gazebo_ros_api_plugin.cpp b/gazebo_ros/src/gazebo_ros_api_plugin.cpp index 21da6c34a..a22272ecd 100644 --- a/gazebo_ros/src/gazebo_ros_api_plugin.cpp +++ b/gazebo_ros/src/gazebo_ros_api_plugin.cpp @@ -1082,40 +1082,50 @@ bool GazeboRosApiPlugin::setPhysicsProperties(gazebo_msgs::SetPhysicsProperties: world_->SetPaused(true); // supported updates - gazebo::physics::PhysicsEnginePtr ode_pe = (world_->GetPhysicsEngine()); - ode_pe->SetMaxStepSize(req.time_step); - ode_pe->SetRealTimeUpdateRate(req.max_update_rate); - ode_pe->SetGravity(gazebo::math::Vector3(req.gravity.x,req.gravity.y,req.gravity.z)); + gazebo::physics::PhysicsEnginePtr pe = (world_->GetPhysicsEngine()); + pe->SetMaxStepSize(req.time_step); + pe->SetRealTimeUpdateRate(req.max_update_rate); + pe->SetGravity(gazebo::math::Vector3(req.gravity.x,req.gravity.y,req.gravity.z)); - // stuff only works in ODE right now - ode_pe->SetAutoDisableFlag(req.ode_config.auto_disable_bodies); + if (world_->GetPhysicsEngine()->GetType() == "ode") + { + // stuff only works in ODE right now + pe->SetAutoDisableFlag(req.ode_config.auto_disable_bodies); #if GAZEBO_MAJOR_VERSION >= 3 - ode_pe->SetParam("precon_iters", req.ode_config.sor_pgs_precon_iters); - ode_pe->SetParam("iters", req.ode_config.sor_pgs_iters); - ode_pe->SetParam("sor", req.ode_config.sor_pgs_w); - ode_pe->SetParam("cfm", req.ode_config.cfm); - ode_pe->SetParam("erp", req.ode_config.erp); - ode_pe->SetParam("contact_surface_layer", - req.ode_config.contact_surface_layer); - ode_pe->SetParam("contact_max_correcting_vel", - req.ode_config.contact_max_correcting_vel); - ode_pe->SetParam("max_contacts", req.ode_config.max_contacts); + pe->SetParam("precon_iters", req.ode_config.sor_pgs_precon_iters); + pe->SetParam("iters", req.ode_config.sor_pgs_iters); + pe->SetParam("sor", req.ode_config.sor_pgs_w); + pe->SetParam("cfm", req.ode_config.cfm); + pe->SetParam("erp", req.ode_config.erp); + pe->SetParam("contact_surface_layer", + req.ode_config.contact_surface_layer); + pe->SetParam("contact_max_correcting_vel", + req.ode_config.contact_max_correcting_vel); + pe->SetParam("max_contacts", req.ode_config.max_contacts); #else - ode_pe->SetSORPGSPreconIters(req.ode_config.sor_pgs_precon_iters); - ode_pe->SetSORPGSIters(req.ode_config.sor_pgs_iters); - ode_pe->SetSORPGSW(req.ode_config.sor_pgs_w); - ode_pe->SetWorldCFM(req.ode_config.cfm); - ode_pe->SetWorldERP(req.ode_config.erp); - ode_pe->SetContactSurfaceLayer(req.ode_config.contact_surface_layer); - ode_pe->SetContactMaxCorrectingVel(req.ode_config.contact_max_correcting_vel); - ode_pe->SetMaxContacts(req.ode_config.max_contacts); + pe->SetSORPGSPreconIters(req.ode_config.sor_pgs_precon_iters); + pe->SetSORPGSIters(req.ode_config.sor_pgs_iters); + pe->SetSORPGSW(req.ode_config.sor_pgs_w); + pe->SetWorldCFM(req.ode_config.cfm); + pe->SetWorldERP(req.ode_config.erp); + pe->SetContactSurfaceLayer(req.ode_config.contact_surface_layer); + pe->SetContactMaxCorrectingVel(req.ode_config.contact_max_correcting_vel); + pe->SetMaxContacts(req.ode_config.max_contacts); #endif - world_->SetPaused(is_paused); + world_->SetPaused(is_paused); - res.success = true; - res.status_message = "physics engine updated"; - return true; + res.success = true; + res.status_message = "physics engine updated"; + } + else + { + /// \TODO: add support for simbody, dart and bullet physics properties. + ROS_ERROR("ROS set_physics_properties service call does not yet support physics engine [%s].", world_->GetPhysicsEngine()->GetType().c_str()); + res.success = false; + res.status_message = "Physics engine [" + world_->GetPhysicsEngine()->GetType() + "]: set_physics_properties not supported."; + } + return res.success; } bool GazeboRosApiPlugin::getPhysicsProperties(gazebo_msgs::GetPhysicsProperties::Request &req, @@ -1131,39 +1141,49 @@ bool GazeboRosApiPlugin::getPhysicsProperties(gazebo_msgs::GetPhysicsProperties: res.gravity.z = gravity.z; // stuff only works in ODE right now - res.ode_config.auto_disable_bodies = - world_->GetPhysicsEngine()->GetAutoDisableFlag(); + if (world_->GetPhysicsEngine()->GetType() == "ode") + { + res.ode_config.auto_disable_bodies = + world_->GetPhysicsEngine()->GetAutoDisableFlag(); #if GAZEBO_MAJOR_VERSION >= 3 - res.ode_config.sor_pgs_precon_iters = boost::any_cast( - world_->GetPhysicsEngine()->GetParam("precon_iters")); - res.ode_config.sor_pgs_iters = boost::any_cast( - world_->GetPhysicsEngine()->GetParam("iters")); - res.ode_config.sor_pgs_w = boost::any_cast( - world_->GetPhysicsEngine()->GetParam("sor")); - res.ode_config.contact_surface_layer = boost::any_cast( - world_->GetPhysicsEngine()->GetParam("contact_surface_layer")); - res.ode_config.contact_max_correcting_vel = boost::any_cast( - world_->GetPhysicsEngine()->GetParam("contact_max_correcting_vel")); - res.ode_config.cfm = boost::any_cast( - world_->GetPhysicsEngine()->GetParam("cfm")); - res.ode_config.erp = boost::any_cast( - world_->GetPhysicsEngine()->GetParam("erp")); - res.ode_config.max_contacts = boost::any_cast( - world_->GetPhysicsEngine()->GetParam("max_contacts")); + res.ode_config.sor_pgs_precon_iters = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("precon_iters")); + res.ode_config.sor_pgs_iters = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("iters")); + res.ode_config.sor_pgs_w = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("sor")); + res.ode_config.contact_surface_layer = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("contact_surface_layer")); + res.ode_config.contact_max_correcting_vel = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("contact_max_correcting_vel")); + res.ode_config.cfm = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("cfm")); + res.ode_config.erp = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("erp")); + res.ode_config.max_contacts = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("max_contacts")); #else - res.ode_config.sor_pgs_precon_iters = world_->GetPhysicsEngine()->GetSORPGSPreconIters(); - res.ode_config.sor_pgs_iters = world_->GetPhysicsEngine()->GetSORPGSIters(); - res.ode_config.sor_pgs_w = world_->GetPhysicsEngine()->GetSORPGSW(); - res.ode_config.contact_surface_layer = world_->GetPhysicsEngine()->GetContactSurfaceLayer(); - res.ode_config.contact_max_correcting_vel = world_->GetPhysicsEngine()->GetContactMaxCorrectingVel(); - res.ode_config.cfm = world_->GetPhysicsEngine()->GetWorldCFM(); - res.ode_config.erp = world_->GetPhysicsEngine()->GetWorldERP(); - res.ode_config.max_contacts = world_->GetPhysicsEngine()->GetMaxContacts(); + res.ode_config.sor_pgs_precon_iters = world_->GetPhysicsEngine()->GetSORPGSPreconIters(); + res.ode_config.sor_pgs_iters = world_->GetPhysicsEngine()->GetSORPGSIters(); + res.ode_config.sor_pgs_w = world_->GetPhysicsEngine()->GetSORPGSW(); + res.ode_config.contact_surface_layer = world_->GetPhysicsEngine()->GetContactSurfaceLayer(); + res.ode_config.contact_max_correcting_vel = world_->GetPhysicsEngine()->GetContactMaxCorrectingVel(); + res.ode_config.cfm = world_->GetPhysicsEngine()->GetWorldCFM(); + res.ode_config.erp = world_->GetPhysicsEngine()->GetWorldERP(); + res.ode_config.max_contacts = world_->GetPhysicsEngine()->GetMaxContacts(); #endif - res.success = true; - res.status_message = "GetPhysicsProperties: got properties"; - return true; + res.success = true; + res.status_message = "GetPhysicsProperties: got properties"; + } + else + { + /// \TODO: add support for simbody, dart and bullet physics properties. + ROS_ERROR("ROS get_physics_properties service call does not yet support physics engine [%s].", world_->GetPhysicsEngine()->GetType().c_str()); + res.success = false; + res.status_message = "Physics engine [" + world_->GetPhysicsEngine()->GetType() + "]: get_physics_properties not supported."; + } + return res.success; } bool GazeboRosApiPlugin::setJointProperties(gazebo_msgs::SetJointProperties::Request &req, From 606b68d9f4593c4292cab2f4e25cfd4b6d0d4d0b Mon Sep 17 00:00:00 2001 From: John Hsu Date: Thu, 28 Aug 2014 16:18:04 -0700 Subject: [PATCH 139/153] check physics engine type before calling set_physics_properties and get_physics_properteis --- gazebo_ros/src/gazebo_ros_api_plugin.cpp | 190 +++++++++-------------- 1 file changed, 73 insertions(+), 117 deletions(-) diff --git a/gazebo_ros/src/gazebo_ros_api_plugin.cpp b/gazebo_ros/src/gazebo_ros_api_plugin.cpp index 528f45fde..a22272ecd 100644 --- a/gazebo_ros/src/gazebo_ros_api_plugin.cpp +++ b/gazebo_ros/src/gazebo_ros_api_plugin.cpp @@ -1082,40 +1082,50 @@ bool GazeboRosApiPlugin::setPhysicsProperties(gazebo_msgs::SetPhysicsProperties: world_->SetPaused(true); // supported updates - gazebo::physics::PhysicsEnginePtr ode_pe = (world_->GetPhysicsEngine()); - ode_pe->SetMaxStepSize(req.time_step); - ode_pe->SetRealTimeUpdateRate(req.max_update_rate); - ode_pe->SetGravity(gazebo::math::Vector3(req.gravity.x,req.gravity.y,req.gravity.z)); + gazebo::physics::PhysicsEnginePtr pe = (world_->GetPhysicsEngine()); + pe->SetMaxStepSize(req.time_step); + pe->SetRealTimeUpdateRate(req.max_update_rate); + pe->SetGravity(gazebo::math::Vector3(req.gravity.x,req.gravity.y,req.gravity.z)); - // stuff only works in ODE right now - ode_pe->SetAutoDisableFlag(req.ode_config.auto_disable_bodies); + if (world_->GetPhysicsEngine()->GetType() == "ode") + { + // stuff only works in ODE right now + pe->SetAutoDisableFlag(req.ode_config.auto_disable_bodies); #if GAZEBO_MAJOR_VERSION >= 3 - ode_pe->SetParam("precon_iters", req.ode_config.sor_pgs_precon_iters); - ode_pe->SetParam("iters", req.ode_config.sor_pgs_iters); - ode_pe->SetParam("sor", req.ode_config.sor_pgs_w); - ode_pe->SetParam("cfm", req.ode_config.cfm); - ode_pe->SetParam("erp", req.ode_config.erp); - ode_pe->SetParam("contact_surface_layer", - req.ode_config.contact_surface_layer); - ode_pe->SetParam("contact_max_correcting_vel", - req.ode_config.contact_max_correcting_vel); - ode_pe->SetParam("max_contacts", req.ode_config.max_contacts); + pe->SetParam("precon_iters", req.ode_config.sor_pgs_precon_iters); + pe->SetParam("iters", req.ode_config.sor_pgs_iters); + pe->SetParam("sor", req.ode_config.sor_pgs_w); + pe->SetParam("cfm", req.ode_config.cfm); + pe->SetParam("erp", req.ode_config.erp); + pe->SetParam("contact_surface_layer", + req.ode_config.contact_surface_layer); + pe->SetParam("contact_max_correcting_vel", + req.ode_config.contact_max_correcting_vel); + pe->SetParam("max_contacts", req.ode_config.max_contacts); #else - ode_pe->SetSORPGSPreconIters(req.ode_config.sor_pgs_precon_iters); - ode_pe->SetSORPGSIters(req.ode_config.sor_pgs_iters); - ode_pe->SetSORPGSW(req.ode_config.sor_pgs_w); - ode_pe->SetWorldCFM(req.ode_config.cfm); - ode_pe->SetWorldERP(req.ode_config.erp); - ode_pe->SetContactSurfaceLayer(req.ode_config.contact_surface_layer); - ode_pe->SetContactMaxCorrectingVel(req.ode_config.contact_max_correcting_vel); - ode_pe->SetMaxContacts(req.ode_config.max_contacts); + pe->SetSORPGSPreconIters(req.ode_config.sor_pgs_precon_iters); + pe->SetSORPGSIters(req.ode_config.sor_pgs_iters); + pe->SetSORPGSW(req.ode_config.sor_pgs_w); + pe->SetWorldCFM(req.ode_config.cfm); + pe->SetWorldERP(req.ode_config.erp); + pe->SetContactSurfaceLayer(req.ode_config.contact_surface_layer); + pe->SetContactMaxCorrectingVel(req.ode_config.contact_max_correcting_vel); + pe->SetMaxContacts(req.ode_config.max_contacts); #endif - world_->SetPaused(is_paused); + world_->SetPaused(is_paused); - res.success = true; - res.status_message = "physics engine updated"; - return true; + res.success = true; + res.status_message = "physics engine updated"; + } + else + { + /// \TODO: add support for simbody, dart and bullet physics properties. + ROS_ERROR("ROS set_physics_properties service call does not yet support physics engine [%s].", world_->GetPhysicsEngine()->GetType().c_str()); + res.success = false; + res.status_message = "Physics engine [" + world_->GetPhysicsEngine()->GetType() + "]: set_physics_properties not supported."; + } + return res.success; } bool GazeboRosApiPlugin::getPhysicsProperties(gazebo_msgs::GetPhysicsProperties::Request &req, @@ -1131,103 +1141,49 @@ bool GazeboRosApiPlugin::getPhysicsProperties(gazebo_msgs::GetPhysicsProperties: res.gravity.z = gravity.z; // stuff only works in ODE right now - res.ode_config.auto_disable_bodies = - world_->GetPhysicsEngine()->GetAutoDisableFlag(); -#if GAZEBO_MAJOR_VERSION >= 3 - try - { - res.ode_config.sor_pgs_precon_iters = - boost::any_cast(world_->GetPhysicsEngine()->GetParam("precon_iters")); - } - catch(boost::bad_lexical_cast &e) - { - ROS_ERROR("precon_iters not supported by current physics engine[%s].\n", e.what()); - res.ode_config.sor_pgs_precon_iters = 0; - } - try - { - res.ode_config.sor_pgs_iters = - boost::any_cast(world_->GetPhysicsEngine()->GetParam("iters")); - } - catch(boost::bad_lexical_cast &e) - { - ROS_ERROR("iters not supported by current physics engine[%s].\n", e.what()); - res.ode_config.sor_pgs_iters = 0; - } - try - { - res.ode_config.sor_pgs_w = - boost::any_cast(world_->GetPhysicsEngine()->GetParam("sor")); - } - catch(boost::bad_lexical_cast &e) - { - ROS_ERROR("sor not supported by current physics engine[%s].\n", e.what()); - res.ode_config.sor_pgs_w = 0.0; - } - try + if (world_->GetPhysicsEngine()->GetType() == "ode") { + res.ode_config.auto_disable_bodies = + world_->GetPhysicsEngine()->GetAutoDisableFlag(); +#if GAZEBO_MAJOR_VERSION >= 3 + res.ode_config.sor_pgs_precon_iters = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("precon_iters")); + res.ode_config.sor_pgs_iters = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("iters")); + res.ode_config.sor_pgs_w = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("sor")); res.ode_config.contact_surface_layer = boost::any_cast( world_->GetPhysicsEngine()->GetParam("contact_surface_layer")); - } - catch(boost::bad_lexical_cast &e) - { - ROS_ERROR("contact_surface_layer not supported by current physics engine[%s].\n", e.what()); - res.ode_config.contact_surface_layer = 0.0; - } - try - { res.ode_config.contact_max_correcting_vel = boost::any_cast( world_->GetPhysicsEngine()->GetParam("contact_max_correcting_vel")); - } - catch(boost::bad_lexical_cast &e) - { - ROS_ERROR("contact_max_correcting_vel not supported by current physics engine[%s].\n", e.what()); - res.ode_config.contact_max_correcting_vel = 0.0; - } - try - { - res.ode_config.cfm = - boost::any_cast(world_->GetPhysicsEngine()->GetParam("cfm")); - } - catch(boost::bad_lexical_cast &e) - { - ROS_ERROR("cfm not supported by current physics engine[%s].\n", e.what()); - res.ode_config.cfm = 0.0; - } - try - { - res.ode_config.erp = - boost::any_cast(world_->GetPhysicsEngine()->GetParam("erp")); - } - catch(boost::bad_lexical_cast &e) - { - ROS_ERROR("erp not supported by current physics engine[%s].\n", e.what()); - res.ode_config.erp = 0.0; - } - try - { - res.ode_config.max_contacts = - boost::any_cast(world_->GetPhysicsEngine()->GetParam("max_contacts")); - } - catch(boost::bad_lexical_cast &e) - { - ROS_ERROR("max_contacts not supported by current physics engine[%s].\n", e.what()); - res.ode_config.max_contacts = 0; - } + res.ode_config.cfm = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("cfm")); + res.ode_config.erp = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("erp")); + res.ode_config.max_contacts = boost::any_cast( + world_->GetPhysicsEngine()->GetParam("max_contacts")); #else - res.ode_config.sor_pgs_precon_iters = world_->GetPhysicsEngine()->GetSORPGSPreconIters(); - res.ode_config.sor_pgs_iters = world_->GetPhysicsEngine()->GetSORPGSIters(); - res.ode_config.sor_pgs_w = world_->GetPhysicsEngine()->GetSORPGSW(); - res.ode_config.contact_surface_layer = world_->GetPhysicsEngine()->GetContactSurfaceLayer(); - res.ode_config.contact_max_correcting_vel = world_->GetPhysicsEngine()->GetContactMaxCorrectingVel(); - res.ode_config.cfm = world_->GetPhysicsEngine()->GetWorldCFM(); - res.ode_config.erp = world_->GetPhysicsEngine()->GetWorldERP(); - res.ode_config.max_contacts = world_->GetPhysicsEngine()->GetMaxContacts(); + res.ode_config.sor_pgs_precon_iters = world_->GetPhysicsEngine()->GetSORPGSPreconIters(); + res.ode_config.sor_pgs_iters = world_->GetPhysicsEngine()->GetSORPGSIters(); + res.ode_config.sor_pgs_w = world_->GetPhysicsEngine()->GetSORPGSW(); + res.ode_config.contact_surface_layer = world_->GetPhysicsEngine()->GetContactSurfaceLayer(); + res.ode_config.contact_max_correcting_vel = world_->GetPhysicsEngine()->GetContactMaxCorrectingVel(); + res.ode_config.cfm = world_->GetPhysicsEngine()->GetWorldCFM(); + res.ode_config.erp = world_->GetPhysicsEngine()->GetWorldERP(); + res.ode_config.max_contacts = world_->GetPhysicsEngine()->GetMaxContacts(); #endif - res.success = true; - res.status_message = "GetPhysicsProperties: got properties"; - return true; + res.success = true; + res.status_message = "GetPhysicsProperties: got properties"; + } + else + { + /// \TODO: add support for simbody, dart and bullet physics properties. + ROS_ERROR("ROS get_physics_properties service call does not yet support physics engine [%s].", world_->GetPhysicsEngine()->GetType().c_str()); + res.success = false; + res.status_message = "Physics engine [" + world_->GetPhysicsEngine()->GetType() + "]: get_physics_properties not supported."; + } + return res.success; } bool GazeboRosApiPlugin::setJointProperties(gazebo_msgs::SetJointProperties::Request &req, From 3f708b1063a35180916934624a1f89b38d2d62dd Mon Sep 17 00:00:00 2001 From: John Hsu Date: Thu, 28 Aug 2014 16:46:50 -0700 Subject: [PATCH 140/153] check deprecation of gazebo::Joint::SetAngle by SetPosition --- gazebo_plugins/src/gazebo_ros_joint_pose_trajectory.cpp | 4 ++++ gazebo_plugins/src/gazebo_ros_joint_trajectory.cpp | 4 ++++ 2 files changed, 8 insertions(+) diff --git a/gazebo_plugins/src/gazebo_ros_joint_pose_trajectory.cpp b/gazebo_plugins/src/gazebo_ros_joint_pose_trajectory.cpp index 0acbb7713..b68a575ac 100644 --- a/gazebo_plugins/src/gazebo_ros_joint_pose_trajectory.cpp +++ b/gazebo_plugins/src/gazebo_ros_joint_pose_trajectory.cpp @@ -340,7 +340,11 @@ void GazeboRosJointPoseTrajectory::UpdateStates() { // this is not the most efficient way to set things if (this->joints_[i]) +#if GAZEBO_MAJOR_VERSION >= 4 + this->joints_[i]->SetPosition(0, +#else this->joints_[i]->SetAngle(0, +#endif this->points_[this->trajectory_index].positions[i]); } diff --git a/gazebo_plugins/src/gazebo_ros_joint_trajectory.cpp b/gazebo_plugins/src/gazebo_ros_joint_trajectory.cpp index eb7748b00..173f5be41 100644 --- a/gazebo_plugins/src/gazebo_ros_joint_trajectory.cpp +++ b/gazebo_plugins/src/gazebo_ros_joint_trajectory.cpp @@ -342,7 +342,11 @@ void GazeboRosJointTrajectory::UpdateStates() { // this is not the most efficient way to set things if (this->joints_[i]) +#if GAZEBO_MAJOR_VERSION >= 4 + this->joints_[i]->SetPosition(0, +#else this->joints_[i]->SetAngle(0, +#endif this->points_[this->trajectory_index].positions[i]); } From c785227c5f9ca603c6a52c9e23571460cae570b3 Mon Sep 17 00:00:00 2001 From: John Hsu Date: Thu, 28 Aug 2014 17:34:30 -0700 Subject: [PATCH 141/153] merging --- .../gazebo_plugins/gazebo_ros_diff_drive.h | 2 +- gazebo_plugins/src/gazebo_ros_diff_drive.cpp | 22 +++++++++++++++---- 2 files changed, 19 insertions(+), 5 deletions(-) diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_diff_drive.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_diff_drive.h index 4046b3acc..74335f612 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_diff_drive.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_diff_drive.h @@ -123,7 +123,7 @@ namespace gazebo { std::string odometry_topic_; std::string odometry_frame_; std::string robot_base_frame_; - + bool publish_tf_; // Custom Callback Queue ros::CallbackQueue queue_; boost::thread callback_queue_thread_; diff --git a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp index beab7d79c..aeed36c19 100644 --- a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp +++ b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp @@ -105,6 +105,14 @@ void GazeboRosDiffDrive::Load ( physics::ModelPtr _parent, sdf::ElementPtr _sdf + this->publish_tf_ = true; + if (!_sdf->HasElement("publishTf")) { + ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to %f", + this->robot_namespace_.c_str(), this->publish_tf_); + } else { + this->publish_tf_ = _sdf->GetElement("publishTf")->Get(); + } + // Initialize update rate stuff if ( this->update_rate_ > 0.0 ) this->update_period_ = 1.0 / this->update_rate_; else this->update_period_ = 0.0; @@ -119,7 +127,7 @@ void GazeboRosDiffDrive::Load ( physics::ModelPtr _parent, sdf::ElementPtr _sdf alive_ = true; - if(this->publishWheelJointState_) + if (this->publishWheelJointState_) { joint_state_publisher_ = gazebo_ros_->node()->advertise("joint_states", 1000); ROS_INFO("%s: Advertise joint_states!", gazebo_ros_->info()); @@ -138,8 +146,11 @@ void GazeboRosDiffDrive::Load ( physics::ModelPtr _parent, sdf::ElementPtr _sdf cmd_vel_subscriber_ = gazebo_ros_->node()->subscribe(so); ROS_INFO("%s: Subscribe to %s!", gazebo_ros_->info(), command_topic_.c_str()); - odometry_publisher_ = gazebo_ros_->node()->advertise(odometry_topic_, 1); - ROS_INFO("%s: Advertise odom on %s !", gazebo_ros_->info(), odometry_topic_.c_str()); + if (this->publish_tf_) + { + odometry_publisher_ = gazebo_ros_->node()->advertise(odometry_topic_, 1); + ROS_INFO("%s: Advertise odom on %s !", gazebo_ros_->info(), odometry_topic_.c_str()); + } // start custom queue for diff drive this->callback_queue_thread_ = @@ -159,6 +170,9 @@ void GazeboRosDiffDrive::publishWheelJointState() joint_state_.name.resize ( joints_.size() ); joint_state_.position.resize ( joints_.size() ); + if(this->publish_tf_) + publishOdometry(seconds_since_last_update); + for ( int i = 0; i < 2; i++ ) { physics::JointPtr joint = joints_[i]; math::Angle angle = joint->GetAngle ( 0 ); @@ -195,7 +209,7 @@ void GazeboRosDiffDrive::UpdateChild() common::Time current_time = parent->GetWorld()->GetSimTime(); double seconds_since_last_update = ( current_time - last_update_time_ ).Double(); if ( seconds_since_last_update > update_period_ ) { - publishOdometry ( seconds_since_last_update ); + if(this->publish_tf_) publishOdometry ( seconds_since_last_update ); if ( publishWheelTF_ ) publishWheelTF(); if ( publishWheelJointState_ ) publishWheelJointState(); From 143d7ab1eceea5983dab1c9adf35bbfc40c59695 Mon Sep 17 00:00:00 2001 From: John Hsu Date: Thu, 28 Aug 2014 17:36:45 -0700 Subject: [PATCH 142/153] fix style --- gazebo_plugins/src/gazebo_ros_diff_drive.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp index aeed36c19..e11749a7f 100644 --- a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp +++ b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp @@ -170,7 +170,7 @@ void GazeboRosDiffDrive::publishWheelJointState() joint_state_.name.resize ( joints_.size() ); joint_state_.position.resize ( joints_.size() ); - if(this->publish_tf_) + if (this->publish_tf_) publishOdometry(seconds_since_last_update); for ( int i = 0; i < 2; i++ ) { @@ -209,7 +209,7 @@ void GazeboRosDiffDrive::UpdateChild() common::Time current_time = parent->GetWorld()->GetSimTime(); double seconds_since_last_update = ( current_time - last_update_time_ ).Double(); if ( seconds_since_last_update > update_period_ ) { - if(this->publish_tf_) publishOdometry ( seconds_since_last_update ); + if (this->publish_tf_) publishOdometry ( seconds_since_last_update ); if ( publishWheelTF_ ) publishWheelTF(); if ( publishWheelJointState_ ) publishWheelJointState(); From a613fe412b76c4eff1ca044418e03f79eed6c00d Mon Sep 17 00:00:00 2001 From: John Hsu Date: Thu, 28 Aug 2014 18:40:42 -0700 Subject: [PATCH 143/153] fix bad merge --- gazebo_plugins/src/gazebo_ros_diff_drive.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp index e11749a7f..32aa1f31d 100644 --- a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp +++ b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp @@ -170,9 +170,6 @@ void GazeboRosDiffDrive::publishWheelJointState() joint_state_.name.resize ( joints_.size() ); joint_state_.position.resize ( joints_.size() ); - if (this->publish_tf_) - publishOdometry(seconds_since_last_update); - for ( int i = 0; i < 2; i++ ) { physics::JointPtr joint = joints_[i]; math::Angle angle = joint->GetAngle ( 0 ); From 9f9023bc2655290b8dea89cbac767b30836a5ffe Mon Sep 17 00:00:00 2001 From: fsuarez6 Date: Fri, 29 Aug 2014 11:03:27 -0400 Subject: [PATCH 144/153] Updated to Apache 2.0 license --- .../gazebo_plugins/gazebo_ros_ft_sensor.h | 41 +++++++++---------- gazebo_plugins/src/gazebo_ros_ft_sensor.cpp | 40 ++++++++---------- 2 files changed, 37 insertions(+), 44 deletions(-) diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_ft_sensor.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_ft_sensor.h index f0fd9b871..087525de5 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_ft_sensor.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_ft_sensor.h @@ -1,28 +1,25 @@ /* -* Gazebo - Outdoor Multi-Robot Simulator -* Copyright (C) 2003 -* Nate Koenig & Andrew Howard -* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 2 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, -* but WITHOUT ANY WARRANTY; without even the implied warranty of -* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -* GNU General Public License for more details. -* -* You should have received a copy of the GNU General Public License -* along with this program; if not, write to the Free Software -* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -* + * Copyright (C) 2012-2014 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * */ /* -* Desc: Force Torque Sensor Plugin -* Author: Francisco Suarez-Ruiz -* Date: 5 June 2014 -*/ + * Desc: Force Torque Sensor Plugin + * Author: Francisco Suarez-Ruiz + * Date: 5 June 2014 + */ + #ifndef GAZEBO_ROS_FT_HH #define GAZEBO_ROS_FT_HH diff --git a/gazebo_plugins/src/gazebo_ros_ft_sensor.cpp b/gazebo_plugins/src/gazebo_ros_ft_sensor.cpp index 0e10ed65c..db694e7f0 100644 --- a/gazebo_plugins/src/gazebo_ros_ft_sensor.cpp +++ b/gazebo_plugins/src/gazebo_ros_ft_sensor.cpp @@ -1,28 +1,24 @@ /* -* Gazebo - Outdoor Multi-Robot Simulator -* Copyright (C) 2003 -* Nate Koenig & Andrew Howard -* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 2 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, -* but WITHOUT ANY WARRANTY; without even the implied warranty of -* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -* GNU General Public License for more details. -* -* You should have received a copy of the GNU General Public License -* along with this program; if not, write to the Free Software -* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -* + * Copyright (C) 2012-2014 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * */ /* -* Desc: Force Torque Sensor Plugin -* Author: Francisco Suarez-Ruiz -* Date: 5 June 2014 -*/ + * Desc: Force Torque Sensor Plugin + * Author: Francisco Suarez-Ruiz + * Date: 5 June 2014 + */ #include #include From aee31f696995497833598e3946f93cd2d6e7832a Mon Sep 17 00:00:00 2001 From: John Hsu Date: Sat, 30 Aug 2014 19:41:06 -0700 Subject: [PATCH 145/153] fix merge --- gazebo_plugins/src/gazebo_ros_openni_kinect.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp b/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp index fdb4fc804..ecfb2c363 100644 --- a/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp +++ b/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp @@ -317,7 +317,6 @@ bool GazeboRosOpenniKinect::FillPointCloudHelper( double fl = ((double)this->width) / (2.0 *tan(hfov/2.0)); // convert depth to point cloud - point_cloud.points.resize(cols_arg * rows_arg); for (uint32_t j=0; j Date: Sat, 30 Aug 2014 19:42:12 -0700 Subject: [PATCH 146/153] Update gazebo_ros_openni_kinect.cpp --- gazebo_plugins/src/gazebo_ros_openni_kinect.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp b/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp index ecfb2c363..bf8a79697 100644 --- a/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp +++ b/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp @@ -370,7 +370,6 @@ bool GazeboRosOpenniKinect::FillPointCloudHelper( iter_rgb[1] = 0; iter_rgb[2] = 0; } - } } From d26b92def7715c0656498e923b0f7ea59b434d4e Mon Sep 17 00:00:00 2001 From: Jose Luis Rivero Date: Mon, 1 Sep 2014 19:27:55 +0200 Subject: [PATCH 147/153] Changelogs for version 2.4.6 --- gazebo_msgs/CHANGELOG.rst | 3 +++ gazebo_plugins/CHANGELOG.rst | 24 ++++++++++++++++++++++++ gazebo_ros/CHANGELOG.rst | 21 +++++++++++++++++++++ gazebo_ros_control/CHANGELOG.rst | 10 ++++++++++ gazebo_ros_pkgs/CHANGELOG.rst | 3 +++ 5 files changed, 61 insertions(+) diff --git a/gazebo_msgs/CHANGELOG.rst b/gazebo_msgs/CHANGELOG.rst index d3601ea23..691edfab7 100644 --- a/gazebo_msgs/CHANGELOG.rst +++ b/gazebo_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gazebo_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.4.5 (2014-08-18) ------------------ diff --git a/gazebo_plugins/CHANGELOG.rst b/gazebo_plugins/CHANGELOG.rst index fc60c11ae..c4253047b 100644 --- a/gazebo_plugins/CHANGELOG.rst +++ b/gazebo_plugins/CHANGELOG.rst @@ -2,6 +2,30 @@ Changelog for package gazebo_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update gazebo_ros_openni_kinect.cpp +* merging from hydro-devel into indigo-devel +* Merge pull request `#204 `_ from fsuarez6/hydro-devel + gazebo_plugins: Adding ForceTorqueSensor Plugin +* Updated to Apache 2.0 license +* Merge pull request `#180 `_ from vrabaud/indigo-devel + remove PCL dependency +* merging +* check deprecation of gazebo::Joint::SetAngle by SetPosition +* compatibility with gazebo 4.x +* Update changelogs for the upcoming release +* Fix build with gazebo4 and indigo +* Added Gaussian Noise generator +* publish organized pointcloud from openni plugin +* Changed measurement direction to "parent to child" +* gazebo_plugin: Added updateRate parameter to the gazebo_ros_imu plugin +* gazebo_plugins: Adding ForceTorqueSensor Plugin +* remove PCL dependency +* ros_camera_utils: Adding CameraInfoManager to satisfy full ROS camera API (relies on https://github.com/ros-perception/image_common/pull/20 ) + ros_camera_utils: Adding CameraInfoManager to satisfy full ROS camera API (relies on https://github.com/ros-perception/image_common/pull/20 ) +* Contributors: John Hsu, Jonathan Bohren, Jose Luis Rivero, Nate Koenig, Ryohei Ueda, Vincent Rabaud, fsuarez6, gborque, John Binney + 2.4.5 (2014-08-18) ------------------ * Replace SetAngle with SetPosition for gazebo 4 and up diff --git a/gazebo_ros/CHANGELOG.rst b/gazebo_ros/CHANGELOG.rst index f9eab4183..4915456ab 100644 --- a/gazebo_ros/CHANGELOG.rst +++ b/gazebo_ros/CHANGELOG.rst @@ -2,6 +2,27 @@ Changelog for package gazebo_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Merge pull request `#232 `_ from ros-simulation/fix_get_physics_properties_non_ode + Fix get physics properties non ode +* Merge pull request `#183 `_ from ros-simulation/issue_182 + Fix STL iterator errors, misc. cppcheck (`#182 `_) +* check physics engine type before calling set_physics_properties and get_physics_properteis +* check physics engine type before calling set_physics_properties and get_physics_properteis +* Fixes for calling GetParam() with different physic engines. +* 2.3.6 +* Update changelogs for the upcoming release +* Fixed boost any cast +* Removed a few warnings +* Update for hydro + gazebo 1.9 +* Fix build with gazebo4 and indigo +* Fix STL iterator errors, misc. cppcheck (`#182 `_) + There were some errors in STL iterators. + Initialized values of member variables in constructor. + Removed an unused variable (model_name). +* Contributors: Carlos Agüero, John Hsu, Jose Luis Rivero, Nate Koenig, Steven Peters, hsu, osrf + 2.4.5 (2014-08-18) ------------------ * Port fix_build branch for indigo-devel diff --git a/gazebo_ros_control/CHANGELOG.rst b/gazebo_ros_control/CHANGELOG.rst index 732fdc695..a9539b08c 100644 --- a/gazebo_ros_control/CHANGELOG.rst +++ b/gazebo_ros_control/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package gazebo_ros_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Update default_robot_hw_sim.cpp +* Reduced changes +* Fix to work with gazebo3 +* Fix build with gazebo4 and indigo +* Update package.xml + Add new maintainer. +* Contributors: Adolfo Rodriguez Tsouroukdissian, Jose Luis Rivero, Nate Koenig, hsu + 2.4.5 (2014-08-18) ------------------ * Fix typo: GAZEBO_VERSION_MAJOR -> GAZEBO_MAJOR_VERSION diff --git a/gazebo_ros_pkgs/CHANGELOG.rst b/gazebo_ros_pkgs/CHANGELOG.rst index d48699e51..9d530ccdd 100644 --- a/gazebo_ros_pkgs/CHANGELOG.rst +++ b/gazebo_ros_pkgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package gazebo_ros_pkgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 2.4.5 (2014-08-18) ------------------ From 589a137b42125785abb2fe551b9b2282b04eecb2 Mon Sep 17 00:00:00 2001 From: Jose Luis Rivero Date: Mon, 1 Sep 2014 19:28:22 +0200 Subject: [PATCH 148/153] 2.4.6 --- gazebo_msgs/CHANGELOG.rst | 4 ++-- gazebo_msgs/package.xml | 2 +- gazebo_plugins/CHANGELOG.rst | 4 ++-- gazebo_plugins/package.xml | 2 +- gazebo_ros/CHANGELOG.rst | 4 ++-- gazebo_ros/package.xml | 2 +- gazebo_ros_control/CHANGELOG.rst | 4 ++-- gazebo_ros_control/package.xml | 2 +- gazebo_ros_pkgs/CHANGELOG.rst | 4 ++-- gazebo_ros_pkgs/package.xml | 2 +- 10 files changed, 15 insertions(+), 15 deletions(-) diff --git a/gazebo_msgs/CHANGELOG.rst b/gazebo_msgs/CHANGELOG.rst index 691edfab7..e16394ecf 100644 --- a/gazebo_msgs/CHANGELOG.rst +++ b/gazebo_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.4.6 (2014-09-01) +------------------ 2.4.5 (2014-08-18) ------------------ diff --git a/gazebo_msgs/package.xml b/gazebo_msgs/package.xml index 57c7d0d98..a7e73c4d0 100644 --- a/gazebo_msgs/package.xml +++ b/gazebo_msgs/package.xml @@ -1,7 +1,7 @@ gazebo_msgs - 2.4.5 + 2.4.6 Message and service data structures for interacting with Gazebo from ROS. diff --git a/gazebo_plugins/CHANGELOG.rst b/gazebo_plugins/CHANGELOG.rst index c4253047b..9911983f1 100644 --- a/gazebo_plugins/CHANGELOG.rst +++ b/gazebo_plugins/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.4.6 (2014-09-01) +------------------ * Update gazebo_ros_openni_kinect.cpp * merging from hydro-devel into indigo-devel * Merge pull request `#204 `_ from fsuarez6/hydro-devel diff --git a/gazebo_plugins/package.xml b/gazebo_plugins/package.xml index 053775b4e..f068838e6 100644 --- a/gazebo_plugins/package.xml +++ b/gazebo_plugins/package.xml @@ -1,6 +1,6 @@ gazebo_plugins - 2.4.5 + 2.4.6 Robot-independent Gazebo plugins for sensors, motors and dynamic reconfigurable components. diff --git a/gazebo_ros/CHANGELOG.rst b/gazebo_ros/CHANGELOG.rst index 4915456ab..5ed8b310e 100644 --- a/gazebo_ros/CHANGELOG.rst +++ b/gazebo_ros/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.4.6 (2014-09-01) +------------------ * Merge pull request `#232 `_ from ros-simulation/fix_get_physics_properties_non_ode Fix get physics properties non ode * Merge pull request `#183 `_ from ros-simulation/issue_182 diff --git a/gazebo_ros/package.xml b/gazebo_ros/package.xml index e260e90fa..96d5de47c 100644 --- a/gazebo_ros/package.xml +++ b/gazebo_ros/package.xml @@ -1,7 +1,7 @@ gazebo_ros - 2.4.5 + 2.4.6 Provides ROS plugins that offer message and service publishers for interfacing with Gazebo through ROS. Formally simulator_gazebo/gazebo diff --git a/gazebo_ros_control/CHANGELOG.rst b/gazebo_ros_control/CHANGELOG.rst index a9539b08c..ee034b9ca 100644 --- a/gazebo_ros_control/CHANGELOG.rst +++ b/gazebo_ros_control/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_ros_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.4.6 (2014-09-01) +------------------ * Update default_robot_hw_sim.cpp * Reduced changes * Fix to work with gazebo3 diff --git a/gazebo_ros_control/package.xml b/gazebo_ros_control/package.xml index 914c2cf1c..44d12ade3 100644 --- a/gazebo_ros_control/package.xml +++ b/gazebo_ros_control/package.xml @@ -1,6 +1,6 @@ gazebo_ros_control - 2.4.5 + 2.4.6 gazebo_ros_control Jonathan Bohren diff --git a/gazebo_ros_pkgs/CHANGELOG.rst b/gazebo_ros_pkgs/CHANGELOG.rst index 9d530ccdd..5c1762418 100644 --- a/gazebo_ros_pkgs/CHANGELOG.rst +++ b/gazebo_ros_pkgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package gazebo_ros_pkgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.4.6 (2014-09-01) +------------------ 2.4.5 (2014-08-18) ------------------ diff --git a/gazebo_ros_pkgs/package.xml b/gazebo_ros_pkgs/package.xml index 83bd78c45..19e4c2d0d 100644 --- a/gazebo_ros_pkgs/package.xml +++ b/gazebo_ros_pkgs/package.xml @@ -1,7 +1,7 @@ gazebo_ros_pkgs - 2.4.5 + 2.4.6 Interface for using ROS with the Gazebo simulator. John Hsu From 13ecc89074a4c96aa15e7b18a13f6fa40634bf8a Mon Sep 17 00:00:00 2001 From: John Hsu Date: Mon, 8 Sep 2014 00:47:11 -0700 Subject: [PATCH 149/153] fix compiler warning --- gazebo_plugins/src/gazebo_ros_diff_drive.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp index 32aa1f31d..99dbd549e 100644 --- a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp +++ b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp @@ -107,7 +107,7 @@ void GazeboRosDiffDrive::Load ( physics::ModelPtr _parent, sdf::ElementPtr _sdf this->publish_tf_ = true; if (!_sdf->HasElement("publishTf")) { - ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to %f", + ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to %d", this->robot_namespace_.c_str(), this->publish_tf_); } else { this->publish_tf_ = _sdf->GetElement("publishTf")->Get(); From 3d4513db441137612bf91c8c1ddbb897e457949b Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Mon, 15 Sep 2014 07:23:45 +0200 Subject: [PATCH 150/153] [gazebo_ros] Fix for #246 Fixing issue #246 in gzserver. --- gazebo_ros/scripts/gzserver | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gazebo_ros/scripts/gzserver b/gazebo_ros/scripts/gzserver index 65f5010a5..c45a5658a 100755 --- a/gazebo_ros/scripts/gzserver +++ b/gazebo_ros/scripts/gzserver @@ -7,13 +7,13 @@ if [ $(uname) = "Darwin" ]; then fi # add ros path plugin if it does not already exist in the passed in arguments -if [ `expr "$final" : '.*libgazebo_ros_paths_plugin\.$EXT.*'` -eq 0 ] +if [ `expr "$final" : ".*libgazebo_ros_paths_plugin\.$EXT.*"` -eq 0 ] then final="$final -s `catkin_find --first-only libgazebo_ros_paths_plugin.$EXT`" fi # add ros api plugin if it does not already exist in the passed in arguments -if [ `expr "$final" : '.*libgazebo\_ros\_api\_plugin\.$EXT.*'` -eq 0 ] +if [ `expr "$final" : ".*libgazebo_ros_api_plugin\.$EXT.*"` -eq 0 ] then final="$final -s `catkin_find --first-only libgazebo_ros_api_plugin.$EXT`" fi From 4b3ad73086ee3df3a0891b80bd4f770a1a0ee639 Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Mon, 13 Oct 2014 18:03:59 +0200 Subject: [PATCH 151/153] Extended the fix for #246 also to debug, gazebo, gzclient and perf scripts. --- gazebo_ros/scripts/debug | 4 ++-- gazebo_ros/scripts/gazebo | 4 ++-- gazebo_ros/scripts/gzclient | 2 +- gazebo_ros/scripts/perf | 4 ++-- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/gazebo_ros/scripts/debug b/gazebo_ros/scripts/debug index 219a7cafb..37cd902de 100755 --- a/gazebo_ros/scripts/debug +++ b/gazebo_ros/scripts/debug @@ -7,13 +7,13 @@ if [ $(uname) == "Darwin" ]; then fi # add ros path plugin if it does not already exist in the passed in arguments -if [ `expr "$final" : '.*libgazebo_ros_paths_plugin\.$EXT.*'` -eq 0 ] +if [ `expr "$final" : ".*libgazebo_ros_paths_plugin\.$EXT.*"` -eq 0 ] then final="$final -s `catkin_find --first-only libgazebo_ros_paths_plugin.$EXT`" fi # add ros api plugin if it does not already exist in the passed in arguments -if [ `expr "$final" : '.*libgazebo\_ros\_api\_plugin\.$EXT.*'` -eq 0 ] +if [ `expr "$final" : ".*libgazebo_ros_api_plugin\.$EXT.*"` -eq 0 ] then final="$final -s `catkin_find --first-only libgazebo_ros_api_plugin.$EXT`" fi diff --git a/gazebo_ros/scripts/gazebo b/gazebo_ros/scripts/gazebo index 668438583..d994be4c9 100755 --- a/gazebo_ros/scripts/gazebo +++ b/gazebo_ros/scripts/gazebo @@ -7,13 +7,13 @@ if [ $(uname) = "Darwin" ]; then fi # add ros path plugin if it does not already exist in the passed in arguments -if [ `expr "$final" : '.*libgazebo_ros_paths_plugin\.$EXT.*'` -eq 0 ] +if [ `expr "$final" : ".*libgazebo_ros_paths_plugin\.$EXT.*"` -eq 0 ] then final="$final -s `catkin_find --first-only libgazebo_ros_paths_plugin.$EXT`" fi # add ros api plugin if it does not already exist in the passed in arguments -if [ `expr "$final" : '.*libgazebo\_ros\_api\_plugin\.$EXT.*'` -eq 0 ] +if [ `expr "$final" : ".*libgazebo_ros_api_plugin\.$EXT.*"` -eq 0 ] then final="$final -s `catkin_find --first-only libgazebo_ros_api_plugin.$EXT`" fi diff --git a/gazebo_ros/scripts/gzclient b/gazebo_ros/scripts/gzclient index 4ad9c740b..b56c7a9b6 100755 --- a/gazebo_ros/scripts/gzclient +++ b/gazebo_ros/scripts/gzclient @@ -7,7 +7,7 @@ if [ $(uname) = "Darwin" ]; then fi # add ros plugin if does not exist -if [ `expr "$final" : '.*libgazebo_ros_paths_plugin\.$EXT.*'` -eq 0 ] +if [ `expr "$final" : ".*libgazebo_ros_paths_plugin\.$EXT.*"` -eq 0 ] then final="$final -g `catkin_find --first-only libgazebo_ros_paths_plugin.$EXT`" fi diff --git a/gazebo_ros/scripts/perf b/gazebo_ros/scripts/perf index ec310abf1..85b63b30f 100755 --- a/gazebo_ros/scripts/perf +++ b/gazebo_ros/scripts/perf @@ -7,12 +7,12 @@ if [ $(uname) = "Darwin" ]; then fi # add ros plugin if does not exist -if [ `expr "$final" : '.*libgazebo_ros_paths_plugin\.$EXT.*'` -eq 0 ] +if [ `expr "$final" : ".*libgazebo_ros_paths_plugin\.$EXT.*"` -eq 0 ] then final="$final -s `catkin_find --first-only libgazebo_ros_paths_plugin.$EXT`" fi -if [ `expr "$final" : '.*libgazebo\_ros\_api\_plugin\.$EXT.*'` -eq 0 ] +if [ `expr "$final" : ".*libgazebo_ros_api_plugin\.$EXT.*"` -eq 0 ] then final="$final -s `catkin_find --first-only libgazebo_ros_api_plugin.$EXT`" fi From 2851ad0cd0f2c02924e6dfc6bd5230c34355071f Mon Sep 17 00:00:00 2001 From: ipa-fxm Date: Thu, 9 Oct 2014 10:20:19 +0200 Subject: [PATCH 152/153] move declaration for DefaultRobotHWSim to header file --- gazebo_ros_control/CMakeLists.txt | 1 + .../gazebo_ros_control/default_robot_hw_sim.h | 139 ++++ .../src/default_robot_hw_sim.cpp | 734 ++++++++---------- 3 files changed, 475 insertions(+), 399 deletions(-) create mode 100644 gazebo_ros_control/include/gazebo_ros_control/default_robot_hw_sim.h diff --git a/gazebo_ros_control/CMakeLists.txt b/gazebo_ros_control/CMakeLists.txt index 87de359d9..3bd395a3d 100644 --- a/gazebo_ros_control/CMakeLists.txt +++ b/gazebo_ros_control/CMakeLists.txt @@ -24,6 +24,7 @@ catkin_package( pluginlib transmission_interface INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} default_robot_hw_sim DEPENDS gazebo ) diff --git a/gazebo_ros_control/include/gazebo_ros_control/default_robot_hw_sim.h b/gazebo_ros_control/include/gazebo_ros_control/default_robot_hw_sim.h new file mode 100644 index 000000000..ea65641d3 --- /dev/null +++ b/gazebo_ros_control/include/gazebo_ros_control/default_robot_hw_sim.h @@ -0,0 +1,139 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Open Source Robotics Foundation + * Copyright (c) 2013, The Johns Hopkins University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Open Source Robotics Foundation + * nor the names of its contributors may be + * used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Dave Coleman, Johnathan Bohren + Desc: Hardware Interface for any simulated robot in Gazebo +*/ + +#ifndef _GAZEBO_ROS_CONTROL___DEFAULT_ROBOT_HW_SIM_H_ +#define _GAZEBO_ROS_CONTROL___DEFAULT_ROBOT_HW_SIM_H_ + +// ros_control +#include +#include +#include +#include +#include +#include +#include + +// Gazebo +#include +#include +#include + +// ROS +#include +#include +#include + +// gazebo_ros_control +#include + +// URDF +#include + + + +namespace gazebo_ros_control +{ + +class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim +{ +public: + + virtual bool initSim( + const std::string& robot_namespace, + ros::NodeHandle model_nh, + gazebo::physics::ModelPtr parent_model, + const urdf::Model *const urdf_model, + std::vector transmissions); + + virtual void readSim(ros::Time time, ros::Duration period); + + virtual void writeSim(ros::Time time, ros::Duration period); + +protected: + // Methods used to control a joint. + enum ControlMethod {EFFORT, POSITION, POSITION_PID, VELOCITY, VELOCITY_PID}; + + // Register the limits of the joint specified by joint_name and joint_handle. The limits are + // retrieved from joint_limit_nh. If urdf_model is not NULL, limits are retrieved from it also. + // Return the joint's type, lower position limit, upper position limit, and effort limit. + void registerJointLimits(const std::string& joint_name, + const hardware_interface::JointHandle& joint_handle, + const ControlMethod ctrl_method, + const ros::NodeHandle& joint_limit_nh, + const urdf::Model *const urdf_model, + int *const joint_type, double *const lower_limit, + double *const upper_limit, double *const effort_limit); + + unsigned int n_dof_; + + hardware_interface::JointStateInterface js_interface_; + hardware_interface::EffortJointInterface ej_interface_; + hardware_interface::PositionJointInterface pj_interface_; + hardware_interface::VelocityJointInterface vj_interface_; + + joint_limits_interface::EffortJointSaturationInterface ej_sat_interface_; + joint_limits_interface::EffortJointSoftLimitsInterface ej_limits_interface_; + joint_limits_interface::PositionJointSaturationInterface pj_sat_interface_; + joint_limits_interface::PositionJointSoftLimitsInterface pj_limits_interface_; + joint_limits_interface::VelocityJointSaturationInterface vj_sat_interface_; + joint_limits_interface::VelocityJointSoftLimitsInterface vj_limits_interface_; + + std::vector joint_names_; + std::vector joint_types_; + std::vector joint_lower_limits_; + std::vector joint_upper_limits_; + std::vector joint_effort_limits_; + std::vector joint_control_methods_; + std::vector pid_controllers_; + std::vector joint_position_; + std::vector joint_velocity_; + std::vector joint_effort_; + std::vector joint_effort_command_; + std::vector joint_position_command_; + std::vector joint_velocity_command_; + + std::vector sim_joints_; +}; + +typedef boost::shared_ptr DefaultRobotHWSimPtr; + +} + +#endif // #ifndef __GAZEBO_ROS_CONTROL_PLUGIN_DEFAULT_ROBOT_HW_SIM_H_ diff --git a/gazebo_ros_control/src/default_robot_hw_sim.cpp b/gazebo_ros_control/src/default_robot_hw_sim.cpp index 12d9ad0ac..e7eb7b762 100644 --- a/gazebo_ros_control/src/default_robot_hw_sim.cpp +++ b/gazebo_ros_control/src/default_robot_hw_sim.cpp @@ -38,33 +38,9 @@ Desc: Hardware Interface for any simulated robot in Gazebo */ -#ifndef _GAZEBO_ROS_CONTROL___DEFAULT_ROBOT_HW_SIM_H_ -#define _GAZEBO_ROS_CONTROL___DEFAULT_ROBOT_HW_SIM_H_ - -// ros_control -#include -#include -#include -#include -#include -#include -#include - -// Gazebo -#include -#include -#include - -// ROS -#include -#include -#include - -// gazebo_ros_control -#include - -// URDF -#include + +#include + namespace { @@ -79,437 +55,397 @@ double clamp(const double val, const double min_val, const double max_val) namespace gazebo_ros_control { -class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim + +bool DefaultRobotHWSim::initSim( + const std::string& robot_namespace, + ros::NodeHandle model_nh, + gazebo::physics::ModelPtr parent_model, + const urdf::Model *const urdf_model, + std::vector transmissions) { -public: - - bool initSim( - const std::string& robot_namespace, - ros::NodeHandle model_nh, - gazebo::physics::ModelPtr parent_model, - const urdf::Model *const urdf_model, - std::vector transmissions) + // getJointLimits() searches joint_limit_nh for joint limit parameters. The format of each + // parameter's name is "joint_limits/". An example is "joint_limits/axle_joint". + const ros::NodeHandle joint_limit_nh(model_nh, robot_namespace); + + // Resize vectors to our DOF + n_dof_ = transmissions.size(); + joint_names_.resize(n_dof_); + joint_types_.resize(n_dof_); + joint_lower_limits_.resize(n_dof_); + joint_upper_limits_.resize(n_dof_); + joint_effort_limits_.resize(n_dof_); + joint_control_methods_.resize(n_dof_); + pid_controllers_.resize(n_dof_); + joint_position_.resize(n_dof_); + joint_velocity_.resize(n_dof_); + joint_effort_.resize(n_dof_); + joint_effort_command_.resize(n_dof_); + joint_position_command_.resize(n_dof_); + joint_velocity_command_.resize(n_dof_); + + // Initialize values + for(unsigned int j=0; j < n_dof_; j++) { - // getJointLimits() searches joint_limit_nh for joint limit parameters. The format of each - // parameter's name is "joint_limits/". An example is "joint_limits/axle_joint". - const ros::NodeHandle joint_limit_nh(model_nh, robot_namespace); - - // Resize vectors to our DOF - n_dof_ = transmissions.size(); - joint_names_.resize(n_dof_); - joint_types_.resize(n_dof_); - joint_lower_limits_.resize(n_dof_); - joint_upper_limits_.resize(n_dof_); - joint_effort_limits_.resize(n_dof_); - joint_control_methods_.resize(n_dof_); - pid_controllers_.resize(n_dof_); - joint_position_.resize(n_dof_); - joint_velocity_.resize(n_dof_); - joint_effort_.resize(n_dof_); - joint_effort_command_.resize(n_dof_); - joint_position_command_.resize(n_dof_); - joint_velocity_command_.resize(n_dof_); - - // Initialize values - for(unsigned int j=0; j < n_dof_; j++) + // Check that this transmission has one joint + if(transmissions[j].joints_.size() == 0) { - // Check that this transmission has one joint - if(transmissions[j].joints_.size() == 0) - { - ROS_WARN_STREAM_NAMED("default_robot_hw_sim","Transmission " << transmissions[j].name_ - << " has no associated joints."); - continue; - } - else if(transmissions[j].joints_.size() > 1) - { - ROS_WARN_STREAM_NAMED("default_robot_hw_sim","Transmission " << transmissions[j].name_ - << " has more than one joint. Currently the default robot hardware simulation " - << " interface only supports one."); - continue; - } + ROS_WARN_STREAM_NAMED("default_robot_hw_sim","Transmission " << transmissions[j].name_ + << " has no associated joints."); + continue; + } + else if(transmissions[j].joints_.size() > 1) + { + ROS_WARN_STREAM_NAMED("default_robot_hw_sim","Transmission " << transmissions[j].name_ + << " has more than one joint. Currently the default robot hardware simulation " + << " interface only supports one."); + continue; + } - std::vector joint_interfaces = transmissions[j].joints_[0].hardware_interfaces_; - if (joint_interfaces.empty() && - !(transmissions[j].actuators_.empty()) && - !(transmissions[j].actuators_[0].hardware_interfaces_.empty())) - { - // TODO: Deprecate HW interface specification in actuators in ROS J - joint_interfaces = transmissions[j].actuators_[0].hardware_interfaces_; - ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "The element of tranmission " << - transmissions[j].name_ << " should be nested inside the element, not . " << - "The transmission will be properly loaded, but please update " << - "your robot model to remain compatible with future versions of the plugin."); - } - if (joint_interfaces.empty()) - { - ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "Joint " << transmissions[j].joints_[0].name_ << - " of transmission " << transmissions[j].name_ << " does not specify any hardware interface. " << - "Not adding it to the robot hardware simulation."); - continue; - } - else if (joint_interfaces.size() > 1) - { - ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "Joint " << transmissions[j].joints_[0].name_ << - " of transmission " << transmissions[j].name_ << " specifies multiple hardware interfaces. " << - "Currently the default robot hardware simulation interface only supports one."); - continue; - } + std::vector joint_interfaces = transmissions[j].joints_[0].hardware_interfaces_; + if (joint_interfaces.empty() && + !(transmissions[j].actuators_.empty()) && + !(transmissions[j].actuators_[0].hardware_interfaces_.empty())) + { + // TODO: Deprecate HW interface specification in actuators in ROS J + joint_interfaces = transmissions[j].actuators_[0].hardware_interfaces_; + ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "The element of tranmission " << + transmissions[j].name_ << " should be nested inside the element, not . " << + "The transmission will be properly loaded, but please update " << + "your robot model to remain compatible with future versions of the plugin."); + } + if (joint_interfaces.empty()) + { + ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "Joint " << transmissions[j].joints_[0].name_ << + " of transmission " << transmissions[j].name_ << " does not specify any hardware interface. " << + "Not adding it to the robot hardware simulation."); + continue; + } + else if (joint_interfaces.size() > 1) + { + ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "Joint " << transmissions[j].joints_[0].name_ << + " of transmission " << transmissions[j].name_ << " specifies multiple hardware interfaces. " << + "Currently the default robot hardware simulation interface only supports one. Using the first entry!"); + //continue; + } - // Add data from transmission - joint_names_[j] = transmissions[j].joints_[0].name_; - joint_position_[j] = 1.0; - joint_velocity_[j] = 0.0; - joint_effort_[j] = 1.0; // N/m for continuous joints - joint_effort_command_[j] = 0.0; - joint_position_command_[j] = 0.0; - joint_velocity_command_[j] = 0.0; + // Add data from transmission + joint_names_[j] = transmissions[j].joints_[0].name_; + joint_position_[j] = 1.0; + joint_velocity_[j] = 0.0; + joint_effort_[j] = 1.0; // N/m for continuous joints + joint_effort_command_[j] = 0.0; + joint_position_command_[j] = 0.0; + joint_velocity_command_[j] = 0.0; - const std::string& hardware_interface = joint_interfaces.front(); + const std::string& hardware_interface = joint_interfaces.front(); - // Debug - ROS_DEBUG_STREAM_NAMED("default_robot_hw_sim","Loading joint '" << joint_names_[j] - << "' of type '" << hardware_interface << "'"); + // Debug + ROS_DEBUG_STREAM_NAMED("default_robot_hw_sim","Loading joint '" << joint_names_[j] + << "' of type '" << hardware_interface << "'"); - // Create joint state interface for all joints - js_interface_.registerHandle(hardware_interface::JointStateHandle( - joint_names_[j], &joint_position_[j], &joint_velocity_[j], &joint_effort_[j])); + // Create joint state interface for all joints + js_interface_.registerHandle(hardware_interface::JointStateHandle( + joint_names_[j], &joint_position_[j], &joint_velocity_[j], &joint_effort_[j])); - // Decide what kind of command interface this actuator/joint has - hardware_interface::JointHandle joint_handle; - if(hardware_interface == "EffortJointInterface") - { - // Create effort joint interface - joint_control_methods_[j] = EFFORT; - joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]), - &joint_effort_command_[j]); - ej_interface_.registerHandle(joint_handle); - } - else if(hardware_interface == "PositionJointInterface") - { - // Create position joint interface - joint_control_methods_[j] = POSITION; - joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]), - &joint_position_command_[j]); - pj_interface_.registerHandle(joint_handle); - } - else if(hardware_interface == "VelocityJointInterface") - { - // Create velocity joint interface - joint_control_methods_[j] = VELOCITY; - joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]), - &joint_velocity_command_[j]); - vj_interface_.registerHandle(joint_handle); - } - else - { - ROS_FATAL_STREAM_NAMED("default_robot_hw_sim","No matching hardware interface found for '" - << hardware_interface ); - return false; - } + // Decide what kind of command interface this actuator/joint has + hardware_interface::JointHandle joint_handle; + if(hardware_interface == "EffortJointInterface") + { + // Create effort joint interface + joint_control_methods_[j] = EFFORT; + joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]), + &joint_effort_command_[j]); + ej_interface_.registerHandle(joint_handle); + } + else if(hardware_interface == "PositionJointInterface") + { + // Create position joint interface + joint_control_methods_[j] = POSITION; + joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]), + &joint_position_command_[j]); + pj_interface_.registerHandle(joint_handle); + } + else if(hardware_interface == "VelocityJointInterface") + { + // Create velocity joint interface + joint_control_methods_[j] = VELOCITY; + joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]), + &joint_velocity_command_[j]); + vj_interface_.registerHandle(joint_handle); + } + else + { + ROS_FATAL_STREAM_NAMED("default_robot_hw_sim","No matching hardware interface found for '" + << hardware_interface ); + return false; + } - // Get the gazebo joint that corresponds to the robot joint. - //ROS_DEBUG_STREAM_NAMED("default_robot_hw_sim", "Getting pointer to gazebo joint: " - // << joint_names_[j]); - gazebo::physics::JointPtr joint = parent_model->GetJoint(joint_names_[j]); - if (!joint) - { - ROS_ERROR_STREAM("This robot has a joint named \"" << joint_names_[j] - << "\" which is not in the gazebo model."); - return false; - } - sim_joints_.push_back(joint); + // Get the gazebo joint that corresponds to the robot joint. + //ROS_DEBUG_STREAM_NAMED("default_robot_hw_sim", "Getting pointer to gazebo joint: " + // << joint_names_[j]); + gazebo::physics::JointPtr joint = parent_model->GetJoint(joint_names_[j]); + if (!joint) + { + ROS_ERROR_STREAM("This robot has a joint named \"" << joint_names_[j] + << "\" which is not in the gazebo model."); + return false; + } + sim_joints_.push_back(joint); - registerJointLimits(joint_names_[j], joint_handle, joint_control_methods_[j], - joint_limit_nh, urdf_model, - &joint_types_[j], &joint_lower_limits_[j], &joint_upper_limits_[j], - &joint_effort_limits_[j]); - if (joint_control_methods_[j] != EFFORT) + registerJointLimits(joint_names_[j], joint_handle, joint_control_methods_[j], + joint_limit_nh, urdf_model, + &joint_types_[j], &joint_lower_limits_[j], &joint_upper_limits_[j], + &joint_effort_limits_[j]); + if (joint_control_methods_[j] != EFFORT) + { + // Initialize the PID controller. If no PID gain values are found, use joint->SetAngle() or + // joint->SetVelocity() to control the joint. + const ros::NodeHandle nh(model_nh, robot_namespace + "/gazebo_ros_control/pid_gains/" + + joint_names_[j]); + if (pid_controllers_[j].init(nh, true)) { - // Initialize the PID controller. If no PID gain values are found, use joint->SetAngle() or - // joint->SetVelocity() to control the joint. - const ros::NodeHandle nh(model_nh, robot_namespace + "/gazebo_ros_control/pid_gains/" + - joint_names_[j]); - if (pid_controllers_[j].init(nh, true)) + switch (joint_control_methods_[j]) { - switch (joint_control_methods_[j]) - { - case POSITION: - joint_control_methods_[j] = POSITION_PID; - break; - case VELOCITY: - joint_control_methods_[j] = VELOCITY_PID; - break; - } - } - else - { - // joint->SetMaxForce() must be called if joint->SetAngle() or joint->SetVelocity() are - // going to be called. joint->SetMaxForce() must *not* be called if joint->SetForce() is - // going to be called. - joint->SetMaxForce(0, joint_effort_limits_[j]); + case POSITION: + joint_control_methods_[j] = POSITION_PID; + break; + case VELOCITY: + joint_control_methods_[j] = VELOCITY_PID; + break; } } + else + { + // joint->SetMaxForce() must be called if joint->SetAngle() or joint->SetVelocity() are + // going to be called. joint->SetMaxForce() must *not* be called if joint->SetForce() is + // going to be called. + joint->SetMaxForce(0, joint_effort_limits_[j]); + } } + } - // Register interfaces - registerInterface(&js_interface_); - registerInterface(&ej_interface_); - registerInterface(&pj_interface_); - registerInterface(&vj_interface_); + // Register interfaces + registerInterface(&js_interface_); + registerInterface(&ej_interface_); + registerInterface(&pj_interface_); + registerInterface(&vj_interface_); - return true; - } + return true; +} - void readSim(ros::Time time, ros::Duration period) +void DefaultRobotHWSim::readSim(ros::Time time, ros::Duration period) +{ + for(unsigned int j=0; j < n_dof_; j++) { - for(unsigned int j=0; j < n_dof_; j++) + // Gazebo has an interesting API... + if (joint_types_[j] == urdf::Joint::PRISMATIC) { - // Gazebo has an interesting API... - if (joint_types_[j] == urdf::Joint::PRISMATIC) - { - joint_position_[j] = sim_joints_[j]->GetAngle(0).Radian(); - } - else - { - joint_position_[j] += angles::shortest_angular_distance(joint_position_[j], - sim_joints_[j]->GetAngle(0).Radian()); - } - joint_velocity_[j] = sim_joints_[j]->GetVelocity(0); - joint_effort_[j] = sim_joints_[j]->GetForce((unsigned int)(0)); + joint_position_[j] = sim_joints_[j]->GetAngle(0).Radian(); + } + else + { + joint_position_[j] += angles::shortest_angular_distance(joint_position_[j], + sim_joints_[j]->GetAngle(0).Radian()); } + joint_velocity_[j] = sim_joints_[j]->GetVelocity(0); + joint_effort_[j] = sim_joints_[j]->GetForce((unsigned int)(0)); } +} - void writeSim(ros::Time time, ros::Duration period) +void DefaultRobotHWSim::writeSim(ros::Time time, ros::Duration period) +{ + ej_sat_interface_.enforceLimits(period); + ej_limits_interface_.enforceLimits(period); + pj_sat_interface_.enforceLimits(period); + pj_limits_interface_.enforceLimits(period); + vj_sat_interface_.enforceLimits(period); + vj_limits_interface_.enforceLimits(period); + + for(unsigned int j=0; j < n_dof_; j++) { - ej_sat_interface_.enforceLimits(period); - ej_limits_interface_.enforceLimits(period); - pj_sat_interface_.enforceLimits(period); - pj_limits_interface_.enforceLimits(period); - vj_sat_interface_.enforceLimits(period); - vj_limits_interface_.enforceLimits(period); - - for(unsigned int j=0; j < n_dof_; j++) + switch (joint_control_methods_[j]) { - switch (joint_control_methods_[j]) - { - case EFFORT: - { - const double effort = joint_effort_command_[j]; - sim_joints_[j]->SetForce(0, effort); - } - break; + case EFFORT: + { + const double effort = joint_effort_command_[j]; + sim_joints_[j]->SetForce(0, effort); + } + break; - case POSITION: + case POSITION: #if GAZEBO_MAJOR_VERSION >= 4 - sim_joints_[j]->SetPosition(0, joint_position_command_[j]); + sim_joints_[j]->SetPosition(0, joint_position_command_[j]); #else - sim_joints_[j]->SetAngle(0, joint_position_command_[j]); + sim_joints_[j]->SetAngle(0, joint_position_command_[j]); #endif - break; + break; - case POSITION_PID: + case POSITION_PID: + { + double error; + switch (joint_types_[j]) { - double error; - switch (joint_types_[j]) - { - case urdf::Joint::REVOLUTE: - angles::shortest_angular_distance_with_limits(joint_position_[j], - joint_position_command_[j], - joint_lower_limits_[j], - joint_upper_limits_[j], - error); - break; - case urdf::Joint::CONTINUOUS: - error = angles::shortest_angular_distance(joint_position_[j], - joint_position_command_[j]); - break; - default: - error = joint_position_command_[j] - joint_position_[j]; - } - - const double effort_limit = joint_effort_limits_[j]; - const double effort = clamp(pid_controllers_[j].computeCommand(error, period), - -effort_limit, effort_limit); - sim_joints_[j]->SetForce(0, effort); + case urdf::Joint::REVOLUTE: + angles::shortest_angular_distance_with_limits(joint_position_[j], + joint_position_command_[j], + joint_lower_limits_[j], + joint_upper_limits_[j], + error); + break; + case urdf::Joint::CONTINUOUS: + error = angles::shortest_angular_distance(joint_position_[j], + joint_position_command_[j]); + break; + default: + error = joint_position_command_[j] - joint_position_[j]; } - break; - case VELOCITY: - sim_joints_[j]->SetVelocity(0, joint_velocity_command_[j]); - break; - - case VELOCITY_PID: - const double error = joint_velocity_command_[j] - joint_velocity_[j]; const double effort_limit = joint_effort_limits_[j]; const double effort = clamp(pid_controllers_[j].computeCommand(error, period), -effort_limit, effort_limit); sim_joints_[j]->SetForce(0, effort); - break; - } + } + break; + + case VELOCITY: + sim_joints_[j]->SetVelocity(0, joint_velocity_command_[j]); + break; + + case VELOCITY_PID: + const double error = joint_velocity_command_[j] - joint_velocity_[j]; + const double effort_limit = joint_effort_limits_[j]; + const double effort = clamp(pid_controllers_[j].computeCommand(error, period), + -effort_limit, effort_limit); + sim_joints_[j]->SetForce(0, effort); + break; } } +} -private: - // Methods used to control a joint. - enum ControlMethod {EFFORT, POSITION, POSITION_PID, VELOCITY, VELOCITY_PID}; - - // Register the limits of the joint specified by joint_name and joint_handle. The limits are - // retrieved from joint_limit_nh. If urdf_model is not NULL, limits are retrieved from it also. - // Return the joint's type, lower position limit, upper position limit, and effort limit. - void registerJointLimits(const std::string& joint_name, - const hardware_interface::JointHandle& joint_handle, - const ControlMethod ctrl_method, - const ros::NodeHandle& joint_limit_nh, - const urdf::Model *const urdf_model, - int *const joint_type, double *const lower_limit, - double *const upper_limit, double *const effort_limit) - { - *joint_type = urdf::Joint::UNKNOWN; - *lower_limit = -std::numeric_limits::max(); - *upper_limit = std::numeric_limits::max(); - *effort_limit = std::numeric_limits::max(); - joint_limits_interface::JointLimits limits; - bool has_limits = false; - joint_limits_interface::SoftJointLimits soft_limits; - bool has_soft_limits = false; +// Register the limits of the joint specified by joint_name and joint_handle. The limits are +// retrieved from joint_limit_nh. If urdf_model is not NULL, limits are retrieved from it also. +// Return the joint's type, lower position limit, upper position limit, and effort limit. +void DefaultRobotHWSim::registerJointLimits(const std::string& joint_name, + const hardware_interface::JointHandle& joint_handle, + const ControlMethod ctrl_method, + const ros::NodeHandle& joint_limit_nh, + const urdf::Model *const urdf_model, + int *const joint_type, double *const lower_limit, + double *const upper_limit, double *const effort_limit) +{ + *joint_type = urdf::Joint::UNKNOWN; + *lower_limit = -std::numeric_limits::max(); + *upper_limit = std::numeric_limits::max(); + *effort_limit = std::numeric_limits::max(); + + joint_limits_interface::JointLimits limits; + bool has_limits = false; + joint_limits_interface::SoftJointLimits soft_limits; + bool has_soft_limits = false; - if (urdf_model != NULL) + if (urdf_model != NULL) + { + const boost::shared_ptr urdf_joint = urdf_model->getJoint(joint_name); + if (urdf_joint != NULL) { - const boost::shared_ptr urdf_joint = urdf_model->getJoint(joint_name); - if (urdf_joint != NULL) - { - *joint_type = urdf_joint->type; - // Get limits from the URDF file. - if (joint_limits_interface::getJointLimits(urdf_joint, limits)) - has_limits = true; - if (joint_limits_interface::getSoftJointLimits(urdf_joint, soft_limits)) - has_soft_limits = true; - } + *joint_type = urdf_joint->type; + // Get limits from the URDF file. + if (joint_limits_interface::getJointLimits(urdf_joint, limits)) + has_limits = true; + if (joint_limits_interface::getSoftJointLimits(urdf_joint, soft_limits)) + has_soft_limits = true; } - // Get limits from the parameter server. - if (joint_limits_interface::getJointLimits(joint_name, joint_limit_nh, limits)) - has_limits = true; - - if (!has_limits) - return; + } + // Get limits from the parameter server. + if (joint_limits_interface::getJointLimits(joint_name, joint_limit_nh, limits)) + has_limits = true; - if (*joint_type == urdf::Joint::UNKNOWN) - { - // Infer the joint type. + if (!has_limits) + return; - if (limits.has_position_limits) - { - *joint_type = urdf::Joint::REVOLUTE; - } - else - { - if (limits.angle_wraparound) - *joint_type = urdf::Joint::CONTINUOUS; - else - *joint_type = urdf::Joint::PRISMATIC; - } - } + if (*joint_type == urdf::Joint::UNKNOWN) + { + // Infer the joint type. if (limits.has_position_limits) { - *lower_limit = limits.min_position; - *upper_limit = limits.max_position; + *joint_type = urdf::Joint::REVOLUTE; } - if (limits.has_effort_limits) - *effort_limit = limits.max_effort; + else + { + if (limits.angle_wraparound) + *joint_type = urdf::Joint::CONTINUOUS; + else + *joint_type = urdf::Joint::PRISMATIC; + } + } + + if (limits.has_position_limits) + { + *lower_limit = limits.min_position; + *upper_limit = limits.max_position; + } + if (limits.has_effort_limits) + *effort_limit = limits.max_effort; - if (has_soft_limits) + if (has_soft_limits) + { + switch (ctrl_method) { - switch (ctrl_method) - { - case EFFORT: - { - const joint_limits_interface::EffortJointSoftLimitsHandle - limits_handle(joint_handle, limits, soft_limits); - ej_limits_interface_.registerHandle(limits_handle); - } - break; - case POSITION: - { - const joint_limits_interface::PositionJointSoftLimitsHandle - limits_handle(joint_handle, limits, soft_limits); - pj_limits_interface_.registerHandle(limits_handle); - } - break; - case VELOCITY: - { - const joint_limits_interface::VelocityJointSoftLimitsHandle - limits_handle(joint_handle, limits, soft_limits); - vj_limits_interface_.registerHandle(limits_handle); - } - break; - } + case EFFORT: + { + const joint_limits_interface::EffortJointSoftLimitsHandle + limits_handle(joint_handle, limits, soft_limits); + ej_limits_interface_.registerHandle(limits_handle); + } + break; + case POSITION: + { + const joint_limits_interface::PositionJointSoftLimitsHandle + limits_handle(joint_handle, limits, soft_limits); + pj_limits_interface_.registerHandle(limits_handle); + } + break; + case VELOCITY: + { + const joint_limits_interface::VelocityJointSoftLimitsHandle + limits_handle(joint_handle, limits, soft_limits); + vj_limits_interface_.registerHandle(limits_handle); + } + break; } - else + } + else + { + switch (ctrl_method) { - switch (ctrl_method) - { - case EFFORT: - { - const joint_limits_interface::EffortJointSaturationHandle - sat_handle(joint_handle, limits); - ej_sat_interface_.registerHandle(sat_handle); - } - break; - case POSITION: - { - const joint_limits_interface::PositionJointSaturationHandle - sat_handle(joint_handle, limits); - pj_sat_interface_.registerHandle(sat_handle); - } - break; - case VELOCITY: - { - const joint_limits_interface::VelocityJointSaturationHandle - sat_handle(joint_handle, limits); - vj_sat_interface_.registerHandle(sat_handle); - } - break; - } + case EFFORT: + { + const joint_limits_interface::EffortJointSaturationHandle + sat_handle(joint_handle, limits); + ej_sat_interface_.registerHandle(sat_handle); + } + break; + case POSITION: + { + const joint_limits_interface::PositionJointSaturationHandle + sat_handle(joint_handle, limits); + pj_sat_interface_.registerHandle(sat_handle); + } + break; + case VELOCITY: + { + const joint_limits_interface::VelocityJointSaturationHandle + sat_handle(joint_handle, limits); + vj_sat_interface_.registerHandle(sat_handle); + } + break; } } +} - unsigned int n_dof_; - - hardware_interface::JointStateInterface js_interface_; - hardware_interface::EffortJointInterface ej_interface_; - hardware_interface::PositionJointInterface pj_interface_; - hardware_interface::VelocityJointInterface vj_interface_; - - joint_limits_interface::EffortJointSaturationInterface ej_sat_interface_; - joint_limits_interface::EffortJointSoftLimitsInterface ej_limits_interface_; - joint_limits_interface::PositionJointSaturationInterface pj_sat_interface_; - joint_limits_interface::PositionJointSoftLimitsInterface pj_limits_interface_; - joint_limits_interface::VelocityJointSaturationInterface vj_sat_interface_; - joint_limits_interface::VelocityJointSoftLimitsInterface vj_limits_interface_; - - std::vector joint_names_; - std::vector joint_types_; - std::vector joint_lower_limits_; - std::vector joint_upper_limits_; - std::vector joint_effort_limits_; - std::vector joint_control_methods_; - std::vector pid_controllers_; - std::vector joint_position_; - std::vector joint_velocity_; - std::vector joint_effort_; - std::vector joint_effort_command_; - std::vector joint_position_command_; - std::vector joint_velocity_command_; - - std::vector sim_joints_; -}; - -typedef boost::shared_ptr DefaultRobotHWSimPtr; } PLUGINLIB_EXPORT_CLASS(gazebo_ros_control::DefaultRobotHWSim, gazebo_ros_control::RobotHWSim) - -#endif // #ifndef __GAZEBO_ROS_CONTROL_PLUGIN_DEFAULT_ROBOT_HW_SIM_H_ From cdb2d2630ddcc4a9d7d8b8508b60056dab553281 Mon Sep 17 00:00:00 2001 From: Robert Codd-Downey Date: Fri, 7 Nov 2014 10:08:45 -0500 Subject: [PATCH 153/153] Update CMakeLists.txt --- gazebo_plugins/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/gazebo_plugins/CMakeLists.txt b/gazebo_plugins/CMakeLists.txt index 2d4283ee3..2ef56f6e7 100644 --- a/gazebo_plugins/CMakeLists.txt +++ b/gazebo_plugins/CMakeLists.txt @@ -65,6 +65,7 @@ include_directories(include link_directories( ${GAZEBO_LIBRARY_DIRS} + ${OGRE_LIBRARY_DIRS} ) catkin_package(