diff --git a/gazebo_plugins/src/gazebo_ros_joint_pose_trajectory.cpp b/gazebo_plugins/src/gazebo_ros_joint_pose_trajectory.cpp index aa0b8143d..e94b1e1d2 100644 --- a/gazebo_plugins/src/gazebo_ros_joint_pose_trajectory.cpp +++ b/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 } } diff --git a/gazebo_plugins/src/gazebo_ros_joint_trajectory.cpp b/gazebo_plugins/src/gazebo_ros_joint_trajectory.cpp index e468d5236..6d98803df 100644 --- a/gazebo_plugins/src/gazebo_ros_joint_trajectory.cpp +++ b/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 } } diff --git a/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp b/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp index 1cfd5f5b6..a2d37c365 100644 --- a/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp +++ b/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 ); diff --git a/gazebo_ros_control/src/default_robot_hw_sim.cpp b/gazebo_ros_control/src/default_robot_hw_sim.cpp index a295a3e12..46ff36e0d 100644 --- a/gazebo_ros_control/src/default_robot_hw_sim.cpp +++ b/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;