From b5b8ddc7e3ff4492aa28e41e8093764aaa509ab7 Mon Sep 17 00:00:00 2001 From: Naoki Mizuno Date: Tue, 20 Mar 2018 14:37:59 +0900 Subject: [PATCH] Add support for Gazebo9 This fixes the problem related to Gazebo's position control, which can be seen when dropping a position-controlled robot. The issue is fixed by using the _preserveWorldVelocity argument introduced in Gazebo 9.0.0. https://bitbucket.org/osrf/gazebo/pull-requests/2814/fix-issue-2111-by-providing-options-to/diff Related issue/commit in gazebo_ros_pkgs, which introduces a similar fix: https://github.com/ros-simulation/gazebo_ros_pkgs/commit/3164e4c6299407209a15fd87300f4364dc2e8063 https://github.com/ros-simulation/gazebo_ros_pkgs/issues/479 --- .../src/steer_bot_hardware_gazebo.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/steer_bot_hardware_gazebo/src/steer_bot_hardware_gazebo.cpp b/steer_bot_hardware_gazebo/src/steer_bot_hardware_gazebo.cpp index 4fe19a7..554bc74 100644 --- a/steer_bot_hardware_gazebo/src/steer_bot_hardware_gazebo.cpp +++ b/steer_bot_hardware_gazebo/src/steer_bot_hardware_gazebo.cpp @@ -212,7 +212,11 @@ namespace steer_bot_hardware_gazebo { pos_cmd = front_steer_jnt_pos_cmd_; } +#if GAZEBO_MAJOR_VERSION >= 9 + sim_joints_[i]->SetPosition(0, pos_cmd, true); +#else sim_joints_[i]->SetPosition(0, pos_cmd); +#endif } else if(gazebo_jnt_name == virtual_front_steer_jnt_names_[INDEX_LEFT]) { @@ -228,7 +232,11 @@ namespace steer_bot_hardware_gazebo { pos_cmd = 2*front_steer_jnt_pos_cmd_; } +#if GAZEBO_MAJOR_VERSION >= 9 + sim_joints_[i]->SetPosition(0, pos_cmd, true); +#else sim_joints_[i]->SetPosition(0, pos_cmd); +#endif } else { @@ -461,8 +469,13 @@ namespace steer_bot_hardware_gazebo void SteerBotHardwareGazebo::GetCurrentState(std::vector& _jnt_pos, std::vector& _jnt_vel, std::vector& _jnt_eff, const int _if_index, const int _sim_jnt_index) { +#if GAZEBO_MAJOR_VERSION >= 9 + _jnt_pos[_if_index] += + angles::shortest_angular_distance(_jnt_pos[_if_index], sim_joints_[_sim_jnt_index]->Position(0u)); +#else _jnt_pos[_if_index] += angles::shortest_angular_distance(_jnt_pos[_if_index], sim_joints_[_sim_jnt_index]->GetAngle(0u).Radian()); +#endif _jnt_vel[_if_index] = sim_joints_[_sim_jnt_index]->GetVelocity(0u); _jnt_eff[_if_index] = sim_joints_[_sim_jnt_index]->GetForce(0u); }