Fix #612 for Gazebo9 (#688)

* Fix #612 for Gazebo9

This commit fixes #612, but only for Gazebo9. Fixing it for Gazebo7
(the version used in ROS Kinetic) requires the following PR to be
backported to Gazebo 7 and 8:

https://bitbucket.org/osrf/gazebo/pull-requests/2814/fix-issue-2111-by-providing-options-to/diff

Once that PR has been backported, we can remove the GAZEBO_MAJOR_VERSION
guards from this PR so that the fix is active for the older Gazebo
versions as well.

Tested on Gazebo7 (where it compiles, but doesn't change anything) and
Gazebo9 (where it compiles and fixes the bug). I've tested it using the
instructions I've put into this repo:

https://github.com/mintar/mimic_joint_gazebo_tutorial

* remove old ifdef

gazebo_plugins/src/gazebo_ros_joint_pose_trajectory.cpp

@@ -384,8 +384,13 @@ void GazeboRosJointPoseTrajectory::UpdateStates()
             // this is not the most efficient way to set things
             if (this->joints_[i])
             {
+#if GAZEBO_MAJOR_VERSION >= 9
+              this->joints_[i]->SetPosition(0,
+                this->points_[this->trajectory_index].positions[i], true);
+#else
               this->joints_[i]->SetPosition(0,
                 this->points_[this->trajectory_index].positions[i]);
+#endif
             }
           }
 

gazebo_plugins/src/gazebo_ros_joint_trajectory.cpp

@@ -386,8 +386,13 @@ void GazeboRosJointTrajectory::UpdateStates()
             // this is not the most efficient way to set things
             if (this->joints_[i])
             {
+#if GAZEBO_MAJOR_VERSION >= 9
+              this->joints_[i]->SetPosition(0,
+                this->points_[this->trajectory_index].positions[i], true);
+#else
               this->joints_[i]->SetPosition(0,
                 this->points_[this->trajectory_index].positions[i]);
+#endif
             }
           }
 

gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp

@@ -317,7 +317,11 @@ void GazeboRosTricycleDrive::motorController ( double target_speed, double targe
       {
         applied_angle = target_angle;
       }
+#if GAZEBO_MAJOR_VERSION >= 9
+      joint_steering_->SetPosition(0, applied_angle, true);
+#else
       joint_steering_->SetPosition(0, applied_angle);
+#endif
     }
     //ROS_INFO_NAMED("tricycle_drive", "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 );

gazebo_ros_control/src/default_robot_hw_sim.cpp

@@ -306,10 +306,10 @@ void DefaultRobotHWSim::writeSim(ros::Time time, ros::Duration period)
         break;
 
       case POSITION:
-#if GAZEBO_MAJOR_VERSION >= 4
-        sim_joints_[j]->SetPosition(0, joint_position_command_[j]);
+#if GAZEBO_MAJOR_VERSION >= 9
+        sim_joints_[j]->SetPosition(0, joint_position_command_[j], true);
 #else
-        sim_joints_[j]->SetAngle(0, joint_position_command_[j]);
+        sim_joints_[j]->SetPosition(0, joint_position_command_[j]);
 #endif
         break;