diff --git a/gazebo_plugins/src/gazebo_ros_bumper.cpp b/gazebo_plugins/src/gazebo_ros_bumper.cpp index 6ad43e1ff..2c5ba04ba 100644 --- a/gazebo_plugins/src/gazebo_ros_bumper.cpp +++ b/gazebo_plugins/src/gazebo_ros_bumper.cpp @@ -183,9 +183,9 @@ void GazeboRosBumper::OnContact() /* if (myFrame) { - frame_pose = myFrame->GetWorldPose(); //-this->myBody->GetCoMPose(); - frame_pos = frame_pose.pos; - frame_rot = frame_pose.rot; + frame_pose = myFrame->WorldPose(); //-this->myBody->GetCoMPose(); + frame_pos = frame_pose.Pos(); + frame_rot = frame_pose.Rot(); } else */ diff --git a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp index 6d286a09d..df18bba08 100644 --- a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp +++ b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp @@ -203,9 +203,13 @@ void GazeboRosDiffDrive::publishWheelJointState() for ( int i = 0; i < 2; i++ ) { physics::JointPtr joint = joints_[i]; - ignition::math::Angle angle = joint->GetAngle ( 0 ).Ign(); +#if GAZEBO_MAJOR_VERSION >= 8 + double position = joint->Position ( 0 ); +#else + double position = joint->GetAngle ( 0 ).Radian(); +#endif joint_state_.name[i] = joint->GetName(); - joint_state_.position[i] = angle.Radian () ; + joint_state_.position[i] = position; } joint_state_publisher_.publish ( joint_state_ ); } diff --git a/gazebo_plugins/src/gazebo_ros_hand_of_god.cpp b/gazebo_plugins/src/gazebo_ros_hand_of_god.cpp index 1dcbac6d3..2725febde 100644 --- a/gazebo_plugins/src/gazebo_ros_hand_of_god.cpp +++ b/gazebo_plugins/src/gazebo_ros_hand_of_god.cpp @@ -161,9 +161,11 @@ namespace gazebo // Relative transform from actual to desired pose #if GAZEBO_MAJOR_VERSION >= 8 ignition::math::Pose3d world_pose = floating_link_->DirtyPose(); + ignition::math::Vector3d worldLinearVel = floating_link_->WorldLinearVel(); ignition::math::Vector3d relativeAngularVel = floating_link_->RelativeAngularVel(); #else ignition::math::Pose3d world_pose = floating_link_->GetDirtyPose().Ign(); + ignition::math::Vector3d worldLinearVel = floating_link_->GetWorldLinearVel().Ign(); ignition::math::Vector3d relativeAngularVel = floating_link_->GetRelativeAngularVel().Ign(); #endif ignition::math::Vector3d err_pos = hog_desired.Pos() - world_pose.Pos(); @@ -173,7 +175,7 @@ namespace gazebo ignition::math::Quaterniond not_a_quaternion = err_rot.Log(); floating_link_->AddForce( - kl_ * err_pos - cl_ * floating_link_->GetWorldLinearVel().Ign()); + kl_ * err_pos - cl_ * worldLinearVel); floating_link_->AddRelativeTorque( ka_ * ignition::math::Vector3d(not_a_quaternion.X(), not_a_quaternion.Y(), not_a_quaternion.Z()) diff --git a/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp b/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp index 7eae0a450..c105a41bd 100644 --- a/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp +++ b/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp @@ -124,9 +124,13 @@ void GazeboRosJointStatePublisher::publishJointStates() { for ( int i = 0; i < joints_.size(); i++ ) { physics::JointPtr joint = joints_[i]; - ignition::math::Angle angle = joint->GetAngle ( 0 ).Ign(); +#if GAZEBO_MAJOR_VERSION >= 8 + double position = joint->Position ( 0 ); +#else + double position = joint->GetAngle ( 0 ).Radian(); +#endif joint_state_.name[i] = joint->GetName(); - joint_state_.position[i] = angle.Radian () ; + joint_state_.position[i] = position; } joint_state_publisher_.publish ( joint_state_ ); } diff --git a/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp b/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp index 50dbd2d92..42c93dc85 100644 --- a/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp +++ b/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp @@ -166,7 +166,11 @@ void GazeboRosTricycleDrive::publishWheelJointState() joint_state_.effort.resize ( joints.size() ); for ( std::size_t i = 0; i < joints.size(); i++ ) { joint_state_.name[i] = joints[i]->GetName(); +#if GAZEBO_MAJOR_VERSION >= 8 + joint_state_.position[i] = joints[i]->Position ( 0 ); +#else joint_state_.position[i] = joints[i]->GetAngle ( 0 ).Radian(); +#endif joint_state_.velocity[i] = joints[i]->GetVelocity ( 0 ); joint_state_.effort[i] = joints[i]->GetForce ( 0 ); } @@ -250,7 +254,11 @@ void GazeboRosTricycleDrive::motorController ( double target_speed, double targe } joint_wheel_actuated_->SetParam ( "vel", 0, applied_speed ); +#if GAZEBO_MAJOR_VERSION >= 8 + double current_angle = joint_steering_->Position ( 0 ); +#else double current_angle = joint_steering_->GetAngle ( 0 ).Radian(); +#endif // truncate target angle if (target_angle > +M_PI / 2.0) diff --git a/gazebo_ros/src/gazebo_ros_api_plugin.cpp b/gazebo_ros/src/gazebo_ros_api_plugin.cpp index b72f2c852..b0c8bd4cb 100644 --- a/gazebo_ros/src/gazebo_ros_api_plugin.cpp +++ b/gazebo_ros/src/gazebo_ros_api_plugin.cpp @@ -1063,7 +1063,7 @@ bool GazeboRosApiPlugin::getJointProperties(gazebo_msgs::GetJointProperties::Req res.damping.clear(); // to be added to gazebo //res.damping.push_back(joint->GetDamping(0)); - res.position.clear(); // use GetAngle(i) + res.position.clear(); #if GAZEBO_MAJOR_VERSION >= 8 res.position.push_back(joint->Position(0)); #else @@ -1866,28 +1866,25 @@ bool GazeboRosApiPlugin::applyBodyWrench(gazebo_msgs::ApplyBodyWrench::Request & // first, translate by reference point to the body frame #if GAZEBO_MAJOR_VERSION >= 8 ignition::math::Pose3d framePose = frame->WorldPose(); -#else - ignition::math::Pose3d framePose = frame->GetWorldPose().Ign(); -#endif -#if GAZEBO_MAJOR_VERSION >= 8 ignition::math::Pose3d bodyPose = body->WorldPose(); #else + ignition::math::Pose3d framePose = frame->GetWorldPose().Ign(); ignition::math::Pose3d bodyPose = body->GetWorldPose().Ign(); #endif ignition::math::Pose3d target_to_reference = framePose - bodyPose; ROS_DEBUG_NAMED("api_plugin", "reference frame for applied wrench: [%f %f %f, %f %f %f]-[%f %f %f, %f %f %f]=[%f %f %f, %f %f %f]", - body->GetWorldPose().Ign().Pos().X(), - body->GetWorldPose().Ign().Pos().Y(), - body->GetWorldPose().Ign().Pos().Z(), - body->GetWorldPose().Ign().Rot().Euler().X(), - body->GetWorldPose().Ign().Rot().Euler().Y(), - body->GetWorldPose().Ign().Rot().Euler().Z(), - frame->GetWorldPose().Ign().Pos().X(), - frame->GetWorldPose().Ign().Pos().Y(), - frame->GetWorldPose().Ign().Pos().Z(), - frame->GetWorldPose().Ign().Rot().Euler().X(), - frame->GetWorldPose().Ign().Rot().Euler().Y(), - frame->GetWorldPose().Ign().Rot().Euler().Z(), + bodyPose.Pos().X(), + bodyPose.Pos().Y(), + bodyPose.Pos().Z(), + bodyPose.Rot().Euler().X(), + bodyPose.Rot().Euler().Y(), + bodyPose.Rot().Euler().Z(), + framePose.Pos().X(), + framePose.Pos().Y(), + framePose.Pos().Z(), + framePose.Rot().Euler().X(), + framePose.Rot().Euler().Y(), + framePose.Rot().Euler().Z(), target_to_reference.Pos().X(), target_to_reference.Pos().Y(), target_to_reference.Pos().Z(), diff --git a/gazebo_ros_control/src/default_robot_hw_sim.cpp b/gazebo_ros_control/src/default_robot_hw_sim.cpp index 169854b1a..a295a3e12 100644 --- a/gazebo_ros_control/src/default_robot_hw_sim.cpp +++ b/gazebo_ros_control/src/default_robot_hw_sim.cpp @@ -251,14 +251,19 @@ void DefaultRobotHWSim::readSim(ros::Time time, ros::Duration period) for(unsigned int j=0; j < n_dof_; j++) { // Gazebo has an interesting API... +#if GAZEBO_MAJOR_VERSION >= 8 + double position = sim_joints_[j]->Position(0); +#else + double position = sim_joints_[j]->GetAngle(0).Radian(); +#endif if (joint_types_[j] == urdf::Joint::PRISMATIC) { - joint_position_[j] = sim_joints_[j]->GetAngle(0).Radian(); + joint_position_[j] = position; } else { joint_position_[j] += angles::shortest_angular_distance(joint_position_[j], - sim_joints_[j]->GetAngle(0).Radian()); + position); } joint_velocity_[j] = sim_joints_[j]->GetVelocity(0); joint_effort_[j] = sim_joints_[j]->GetForce((unsigned int)(0));