diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_imu.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_imu.h index f47dac497..6fa506515 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_imu.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_imu.h @@ -75,7 +75,7 @@ namespace gazebo private: std::string topic_name_; /// \brief allow specifying constant xyz and rpy offsets - private: math::Pose offset_; + private: ignition::math::Pose3d offset_; /// \brief A mutex to lock access to fields /// that are used in message callbacks @@ -83,16 +83,16 @@ namespace gazebo /// \brief save last_time private: common::Time last_time_; - private: math::Vector3 last_vpos_; - private: math::Vector3 last_veul_; - private: math::Vector3 apos_; - private: math::Vector3 aeul_; + private: ignition::math::Vector3d last_vpos_; + private: ignition::math::Vector3d last_veul_; + private: ignition::math::Vector3d apos_; + private: ignition::math::Vector3d aeul_; // rate control private: double update_rate_; /// \brief: keep initial pose to offset orientation in imu message - private: math::Pose initial_pose_; + private: ignition::math::Pose3d initial_pose_; /// \brief Gaussian noise private: double gaussian_noise_; diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_imu_sensor.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_imu_sensor.h index 6eea02195..15fa56343 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_imu_sensor.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_imu_sensor.h @@ -19,8 +19,8 @@ #include #include -#include -#include +#include +#include #include #include #include @@ -79,11 +79,11 @@ namespace gazebo /// \brief Pointer to the sdf config file. sdf::ElementPtr sdf; /// \brief Orientation data from the sensor. - gazebo::math::Quaternion orientation; + ignition::math::Quaterniond orientation; /// \brief Linear acceleration data from the sensor. - math::Vector3 accelerometer_data; + ignition::math::Vector3d accelerometer_data; /// \brief Angular velocity data from the sensor. - math::Vector3 gyroscope_data; + ignition::math::Vector3d gyroscope_data; /// \brief Seed for the Gaussian noise generator. unsigned int seed; @@ -100,8 +100,8 @@ namespace gazebo /// \brief Gaussian noise. double gaussian_noise; /// \brief Offset parameter, position part is unused. - math::Pose offset; + ignition::math::Pose3d offset; }; } -#endif //GAZEBO_ROS_IMU_SENSOR_H \ No newline at end of file +#endif //GAZEBO_ROS_IMU_SENSOR_H diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_p3d.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_p3d.h index f113637fa..c8216a424 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_p3d.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_p3d.h @@ -89,21 +89,21 @@ namespace gazebo private: std::string tf_frame_name_; /// \brief allow specifying constant xyz and rpy offsets - private: math::Pose offset_; + private: ignition::math::Pose3d offset_; /// \brief mutex to lock access to fields used in message callbacks private: boost::mutex lock; /// \brief save last_time private: common::Time last_time_; - private: math::Vector3 last_vpos_; - private: math::Vector3 last_veul_; - private: math::Vector3 apos_; - private: math::Vector3 aeul_; - private: math::Vector3 last_frame_vpos_; - private: math::Vector3 last_frame_veul_; - private: math::Vector3 frame_apos_; - private: math::Vector3 frame_aeul_; + private: ignition::math::Vector3d last_vpos_; + private: ignition::math::Vector3d last_veul_; + private: ignition::math::Vector3d apos_; + private: ignition::math::Vector3d aeul_; + private: ignition::math::Vector3d last_frame_vpos_; + private: ignition::math::Vector3d last_frame_veul_; + private: ignition::math::Vector3d frame_apos_; + private: ignition::math::Vector3d frame_aeul_; // rate control private: double update_rate_; diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_planar_move.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_planar_move.h index 33303656b..d399443d1 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_planar_move.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_planar_move.h @@ -90,7 +90,7 @@ namespace gazebo { double rot_; bool alive_; common::Time last_odom_publish_time_; - math::Pose last_odom_pose_; + ignition::math::Pose3d last_odom_pose_; }; diff --git a/gazebo_plugins/src/gazebo_ros_block_laser.cpp b/gazebo_plugins/src/gazebo_ros_block_laser.cpp index 6c536ab8c..900520967 100644 --- a/gazebo_plugins/src/gazebo_ros_block_laser.cpp +++ b/gazebo_plugins/src/gazebo_ros_block_laser.cpp @@ -82,7 +82,7 @@ void GazeboRosBlockLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) std::string worldName = _parent->WorldName(); this->world_ = physics::get_world(worldName); - last_update_time_ = this->world_->GetSimTime(); + last_update_time_ = this->world_->SimTime(); this->node_ = transport::NodePtr(new transport::Node()); this->node_->Init(worldName); @@ -405,9 +405,9 @@ void GazeboRosBlockLaser::OnStats( const boost::shared_ptrsim_time_ = msgs::Convert( _msg->sim_time() ); - math::Pose pose; - pose.pos.x = 0.5*sin(0.01*this->sim_time_.Double()); - gzdbg << "plugin simTime [" << this->sim_time_.Double() << "] update pose [" << pose.pos.x << "]\n"; + ignition::math::Pose3d pose; + pose.Pos().X() = 0.5*sin(0.01*this->sim_time_.Double()); + gzdbg << "plugin simTime [" << this->sim_time_.Double() << "] update pose [" << pose.Pos().X() << "]\n"; } diff --git a/gazebo_plugins/src/gazebo_ros_bumper.cpp b/gazebo_plugins/src/gazebo_ros_bumper.cpp index 8c211cdec..e594743c1 100644 --- a/gazebo_plugins/src/gazebo_ros_bumper.cpp +++ b/gazebo_plugins/src/gazebo_ros_bumper.cpp @@ -32,9 +32,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include #include @@ -157,7 +157,7 @@ void GazeboRosBumper::OnContact() *Simulator::Instance()->GetMRMutex()); // look through all models in the world, search for body // name that matches frameName - phyaics::Model_V all_models = World::Instance()->GetModels(); + physics::Model_V all_models = World::Instance()->GetModels(); for (physics::Model_V::iterator iter = all_models.begin(); iter != all_models.end(); iter++) { @@ -177,13 +177,13 @@ void GazeboRosBumper::OnContact() */ // get reference frame (body(link)) pose and subtract from it to get // relative force, torque, position and normal vectors - math::Pose pose, frame_pose; - math::Quaternion rot, frame_rot; - math::Vector3 pos, frame_pos; + ignition::math::Pose3d pose, frame_pose; + ignition::math::Quaterniond rot, frame_rot; + ignition::math::Vector3d pos, frame_pos; /* if (myFrame) { - frame_pose = myFrame->GetWorldPose(); //-this->myBody->GetCoMPose(); + frame_pose = myFrame->WorldPose(); //-this->myBody->GetCoMPose(); frame_pos = frame_pose.pos; frame_rot = frame_pose.rot; } @@ -192,9 +192,9 @@ void GazeboRosBumper::OnContact() { // no specific frames specified, use identity pose, keeping // relative frame at inertial origin - frame_pos = math::Vector3(0, 0, 0); - frame_rot = math::Quaternion(1, 0, 0, 0); // gazebo u,x,y,z == identity - frame_pose = math::Pose(frame_pos, frame_rot); + frame_pos = ignition::math::Vector3d(0, 0, 0); + frame_rot = ignition::math::Quaterniond(1, 0, 0, 0); // gazebo u,x,y,z == identity + frame_pose = ignition::math::Pose3d(frame_pos, frame_rot); } @@ -242,34 +242,34 @@ void GazeboRosBumper::OnContact() { // loop through individual contacts between collision1 and collision2 // gzerr << j << " Position:" - // << contact.position(j).x() << " " - // << contact.position(j).y() << " " - // << contact.position(j).z() << "\n"; + // << contact.position(j).X()() << " " + // << contact.position(j).Y()() << " " + // << contact.position(j).Z()() << "\n"; // gzerr << " Normal:" - // << contact.normal(j).x() << " " - // << contact.normal(j).y() << " " - // << contact.normal(j).z() << "\n"; + // << contact.normal(j).X()() << " " + // << contact.normal(j).Y()() << " " + // << contact.normal(j).Z()() << "\n"; // gzerr << " Depth:" << contact.depth(j) << "\n"; // Get force, torque and rotate into user specified frame. // frame_rot is identity if world is used (default for now) - math::Vector3 force = frame_rot.RotateVectorReverse(math::Vector3( + ignition::math::Vector3d force = frame_rot.RotateVectorReverse(ignition::math::Vector3d( contact.wrench(j).body_1_wrench().force().x(), contact.wrench(j).body_1_wrench().force().y(), contact.wrench(j).body_1_wrench().force().z())); - math::Vector3 torque = frame_rot.RotateVectorReverse(math::Vector3( + ignition::math::Vector3d torque = frame_rot.RotateVectorReverse(ignition::math::Vector3d( contact.wrench(j).body_1_wrench().torque().x(), contact.wrench(j).body_1_wrench().torque().y(), contact.wrench(j).body_1_wrench().torque().z())); // set wrenches geometry_msgs::Wrench wrench; - wrench.force.x = force.x; - wrench.force.y = force.y; - wrench.force.z = force.z; - wrench.torque.x = torque.x; - wrench.torque.y = torque.y; - wrench.torque.z = torque.z; + wrench.force.x = force.X(); + wrench.force.y = force.Y(); + wrench.force.z = force.Z(); + wrench.torque.x = torque.X(); + wrench.torque.y = torque.Y(); + wrench.torque.z = torque.Z(); state.wrenches.push_back(wrench); total_wrench.force.x += wrench.force.x; @@ -281,27 +281,27 @@ void GazeboRosBumper::OnContact() // transform contact positions into relative frame // set contact positions - gazebo::math::Vector3 position = frame_rot.RotateVectorReverse( - math::Vector3(contact.position(j).x(), - contact.position(j).y(), - contact.position(j).z()) - frame_pos); + ignition::math::Vector3d position = frame_rot.RotateVectorReverse( + ignition::math::Vector3d(contact.position(j).x(), + contact.position(j).y(), + contact.position(j).z()) - frame_pos); geometry_msgs::Vector3 contact_position; - contact_position.x = position.x; - contact_position.y = position.y; - contact_position.z = position.z; + contact_position.x = position.X(); + contact_position.y = position.Y(); + contact_position.z = position.Z(); state.contact_positions.push_back(contact_position); // rotate normal into user specified frame. // frame_rot is identity if world is used. - math::Vector3 normal = frame_rot.RotateVectorReverse( - math::Vector3(contact.normal(j).x(), - contact.normal(j).y(), - contact.normal(j).z())); + ignition::math::Vector3d normal = frame_rot.RotateVectorReverse( + ignition::math::Vector3d(contact.normal(j).x(), + contact.normal(j).y(), + contact.normal(j).z())); // set contact normals geometry_msgs::Vector3 contact_normal; - contact_normal.x = normal.x; - contact_normal.y = normal.y; - contact_normal.z = normal.z; + contact_normal.x = normal.X(); + contact_normal.y = normal.Y(); + contact_normal.z = normal.Z(); state.contact_normals.push_back(contact_normal); // set contact depth, interpenetration diff --git a/gazebo_plugins/src/gazebo_ros_camera_utils.cpp b/gazebo_plugins/src/gazebo_ros_camera_utils.cpp index a174fcfc8..e104b152b 100644 --- a/gazebo_plugins/src/gazebo_ros_camera_utils.cpp +++ b/gazebo_plugins/src/gazebo_ros_camera_utils.cpp @@ -476,7 +476,7 @@ void GazeboRosCameraUtils::Init() else { // check against float precision - if (!gazebo::math::equal(this->focal_length_, computed_focal_length)) + if (!ignition::math::equal(this->focal_length_, computed_focal_length)) { ROS_WARN_NAMED("camera_utils", "The [%f] you have provided for camera_ [%s]" " is inconsistent with specified image_width [%d] and" diff --git a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp index 90631cbf2..37f808e1f 100644 --- a/gazebo_plugins/src/gazebo_ros_diff_drive.cpp +++ b/gazebo_plugins/src/gazebo_ros_diff_drive.cpp @@ -52,7 +52,6 @@ #include -#include #include #include @@ -133,7 +132,7 @@ void GazeboRosDiffDrive::Load ( physics::ModelPtr _parent, sdf::ElementPtr _sdf // 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(); + last_update_time_ = parent->GetWorld()->SimTime(); // Initialize velocity stuff wheel_speed_[RIGHT] = 0; @@ -185,7 +184,7 @@ void GazeboRosDiffDrive::Load ( physics::ModelPtr _parent, sdf::ElementPtr _sdf void GazeboRosDiffDrive::Reset() { - last_update_time_ = parent->GetWorld()->GetSimTime(); + last_update_time_ = parent->GetWorld()->SimTime(); pose_encoder_.x = 0; pose_encoder_.y = 0; pose_encoder_.theta = 0; @@ -210,11 +209,11 @@ void GazeboRosDiffDrive::publishWheelJointState() for ( int i = 0; i < 2; i++ ) { physics::JointPtr joint = joints_[i]; - math::Angle angle = joint->GetAngle ( 0 ); + ignition::math::Angle angle = joint->Position(0); joint_state_.name[i] = joint->GetName(); - joint_state_.position[i] = angle.Radian () ; + joint_state_.position[i] = angle.Radian(); } - joint_state_publisher_.publish ( joint_state_ ); + joint_state_publisher_.publish(joint_state_); } void GazeboRosDiffDrive::publishWheelTF() @@ -225,10 +224,10 @@ void GazeboRosDiffDrive::publishWheelTF() 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(); + ignition::math::Pose3d poseWheel = joints_[i]->GetChild()->RelativePose(); - 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 ( @@ -259,7 +258,7 @@ void GazeboRosDiffDrive::UpdateChild() if ( odom_source_ == ENCODER ) UpdateOdometryEncoder(); - common::Time current_time = parent->GetWorld()->GetSimTime(); + common::Time current_time = parent->GetWorld()->SimTime(); double seconds_since_last_update = ( current_time - last_update_time_ ).Double(); if ( seconds_since_last_update > update_period_ ) { @@ -361,7 +360,7 @@ void GazeboRosDiffDrive::UpdateOdometryEncoder() { double vl = joints_[LEFT]->GetVelocity ( 0 ); double vr = joints_[RIGHT]->GetVelocity ( 0 ); - common::Time current_time = parent->GetWorld()->GetSimTime(); + common::Time current_time = parent->GetWorld()->SimTime(); double seconds_since_last_update = ( current_time - last_odom_update_ ).Double(); last_odom_update_ = current_time; @@ -431,9 +430,9 @@ void GazeboRosDiffDrive::publishOdometry ( double step_time ) } 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 ); + ignition::math::Pose3d pose = parent->WorldPose(); + 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(); @@ -445,14 +444,14 @@ void GazeboRosDiffDrive::publishOdometry ( double step_time ) 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; + ignition::math::Vector3d linear; + linear = parent->WorldLinearVel(); + odom_.twist.twist.angular.z = parent->WorldAngularVel().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; + float yaw = pose.Rot().Yaw(); + 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 ); diff --git a/gazebo_plugins/src/gazebo_ros_f3d.cpp b/gazebo_plugins/src/gazebo_ros_f3d.cpp index 0d9710b9e..0c0a21536 100644 --- a/gazebo_plugins/src/gazebo_ros_f3d.cpp +++ b/gazebo_plugins/src/gazebo_ros_f3d.cpp @@ -39,7 +39,7 @@ GazeboRosF3D::GazeboRosF3D() // Destructor GazeboRosF3D::~GazeboRosF3D() { - event::Events::DisconnectWorldUpdateBegin(this->update_connection_); + this->update_connection_.reset(); // Custom Callback Queue this->queue_.clear(); this->queue_.disable(); @@ -149,27 +149,27 @@ void GazeboRosF3D::UpdateChild() if (this->f3d_connect_count_ == 0) return; - math::Vector3 torque; - math::Vector3 force; + ignition::math::Vector3d torque; + ignition::math::Vector3d force; // get force on body - force = this->link_->GetWorldForce(); + force = this->link_->WorldForce(); // get torque on body - torque = this->link_->GetWorldTorque(); + torque = this->link_->WorldTorque(); 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->wrench_msg_.header.stamp.sec = (this->world_->SimTime()).sec; + this->wrench_msg_.header.stamp.nsec = (this->world_->SimTime()).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(); diff --git a/gazebo_plugins/src/gazebo_ros_force.cpp b/gazebo_plugins/src/gazebo_ros_force.cpp index 4acb9b972..769f14e42 100644 --- a/gazebo_plugins/src/gazebo_ros_force.cpp +++ b/gazebo_plugins/src/gazebo_ros_force.cpp @@ -46,7 +46,7 @@ GazeboRosForce::GazeboRosForce() // Destructor GazeboRosForce::~GazeboRosForce() { - event::Events::DisconnectWorldUpdateBegin(this->update_connection_); + this->update_connection_.reset(); // Custom Callback Queue this->queue_.clear(); @@ -137,8 +137,8 @@ void GazeboRosForce::UpdateObjectForce(const geometry_msgs::Wrench::ConstPtr& _m void GazeboRosForce::UpdateChild() { this->lock_.lock(); - math::Vector3 force(this->wrench_msg_.force.x,this->wrench_msg_.force.y,this->wrench_msg_.force.z); - math::Vector3 torque(this->wrench_msg_.torque.x,this->wrench_msg_.torque.y,this->wrench_msg_.torque.z); + ignition::math::Vector3d force(this->wrench_msg_.force.x,this->wrench_msg_.force.y,this->wrench_msg_.force.z); + ignition::math::Vector3d torque(this->wrench_msg_.torque.x,this->wrench_msg_.torque.y,this->wrench_msg_.torque.z); this->link_->AddForce(force); this->link_->AddTorque(torque); this->lock_.unlock(); diff --git a/gazebo_plugins/src/gazebo_ros_ft_sensor.cpp b/gazebo_plugins/src/gazebo_ros_ft_sensor.cpp index 8e46930f8..f4d1dcfb4 100644 --- a/gazebo_plugins/src/gazebo_ros_ft_sensor.cpp +++ b/gazebo_plugins/src/gazebo_ros_ft_sensor.cpp @@ -39,7 +39,8 @@ GazeboRosFT::GazeboRosFT() // Destructor GazeboRosFT::~GazeboRosFT() { - event::Events::DisconnectWorldUpdateBegin(this->update_connection_); + // Conversion from DisconnectWorldUpdateBegin in migration from gazebo7 + this->update_connection_.reset(); // Custom Callback Queue this->queue_.clear(); this->queue_.disable(); @@ -157,7 +158,7 @@ void GazeboRosFT::FTDisconnect() // Update the controller void GazeboRosFT::UpdateChild() { - common::Time cur_time = this->world_->GetSimTime(); + common::Time cur_time = this->world_->SimTime(); // rate control if (this->update_rate_ > 0 && @@ -168,8 +169,8 @@ void GazeboRosFT::UpdateChild() return; physics::JointWrench wrench; - math::Vector3 torque; - math::Vector3 force; + ignition::math::Vector3d torque; + ignition::math::Vector3d force; // FIXME: Should include options for diferent frames and measure directions // E.g: https://bitbucket.org/osrf/gazebo/raw/default/gazebo/sensors/ForceTorqueSensor.hh @@ -184,15 +185,15 @@ void GazeboRosFT::UpdateChild() 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->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->wrench_msg_.header.stamp.sec = (this->world_->SimTime()).sec; + this->wrench_msg_.header.stamp.nsec = (this->world_->SimTime()).nsec; + + 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(); diff --git a/gazebo_plugins/src/gazebo_ros_hand_of_god.cpp b/gazebo_plugins/src/gazebo_ros_hand_of_god.cpp index e5008edae..b6822ae62 100644 --- a/gazebo_plugins/src/gazebo_ros_hand_of_god.cpp +++ b/gazebo_plugins/src/gazebo_ros_hand_of_god.cpp @@ -115,8 +115,8 @@ namespace gazebo return; } - cl_ = 2.0 * sqrt(kl_*floating_link_->GetInertial()->GetMass()); - ca_ = 2.0 * sqrt(ka_*floating_link_->GetInertial()->GetIXX()); + cl_ = 2.0 * sqrt(kl_*floating_link_->GetInertial()->Mass()); + ca_ = 2.0 * sqrt(ka_*floating_link_->GetInertial()->IXX()); // Create the TF listener for the desired position of the hog tf_buffer_.reset(new tf2_ros::Buffer()); @@ -149,22 +149,24 @@ namespace gazebo // 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)); + ignition::math::Pose3d hog_desired( + ignition::math::Vector3d(p.x, p.y, p.z), + ignition::math::Quaterniond(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; + ignition::math::Pose3d world_pose = floating_link_->DirtyPose(); + ignition::math::Vector3d 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(); + ignition::math::Quaterniond err_rot = (ignition::math::Matrix4d(world_pose).Inverse() * + ignition::math::Matrix4d(hog_desired) + ).Rotation(); + ignition::math::Quaterniond not_a_quaternion = err_rot.Log(); floating_link_->AddForce( - kl_ * err_pos - cl_ * floating_link_->GetWorldLinearVel()); + kl_ * err_pos - cl_ * floating_link_->WorldLinearVel()); floating_link_->AddRelativeTorque( - ka_ * gazebo::math::Vector3(not_a_quaternion.x, not_a_quaternion.y, not_a_quaternion.z) - ca_ * floating_link_->GetRelativeAngularVel()); + ka_ * ignition::math::Vector3d(not_a_quaternion.X(), not_a_quaternion.Y(), not_a_quaternion.Z()) - ca_ * floating_link_->RelativeAngularVel()); // Convert actual pose to TransformStamped message geometry_msgs::TransformStamped hog_actual_tform; @@ -174,14 +176,14 @@ namespace gazebo 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.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; + 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); } diff --git a/gazebo_plugins/src/gazebo_ros_imu.cpp b/gazebo_plugins/src/gazebo_ros_imu.cpp index 1fddd515e..35eb60dd5 100644 --- a/gazebo_plugins/src/gazebo_ros_imu.cpp +++ b/gazebo_plugins/src/gazebo_ros_imu.cpp @@ -38,7 +38,7 @@ GazeboRosIMU::GazeboRosIMU() // Destructor GazeboRosIMU::~GazeboRosIMU() { - event::Events::DisconnectWorldUpdateBegin(this->update_connection_); + this->update_connection_.reset(); // Finalize the controller this->rosnode_->shutdown(); this->callback_queue_thread_.join(); @@ -102,18 +102,18 @@ void GazeboRosIMU::LoadThread() if (!this->sdf->HasElement("xyzOffset")) { ROS_INFO_NAMED("imu", "imu plugin missing , defaults to 0s"); - this->offset_.pos = math::Vector3(0, 0, 0); + this->offset_.Pos() = ignition::math::Vector3d(0, 0, 0); } else - this->offset_.pos = this->sdf->Get("xyzOffset"); + this->offset_.Pos() = this->sdf->Get("xyzOffset"); if (!this->sdf->HasElement("rpyOffset")) { ROS_INFO_NAMED("imu", "imu plugin missing , defaults to 0s"); - this->offset_.rot = math::Vector3(0, 0, 0); + this->offset_.Rot() = ignition::math::Quaterniond(0, 0, 0); } else - this->offset_.rot = this->sdf->Get("rpyOffset"); + this->offset_.Rot() = this->sdf->Get("rpyOffset"); if (!this->sdf->HasElement("updateRate")) { @@ -147,7 +147,7 @@ void GazeboRosIMU::LoadThread() // assert that the body by link_name_ exists this->link = boost::dynamic_pointer_cast( - this->world_->GetEntity(this->link_name_)); + this->world_->EntityByName(this->link_name_)); if (!this->link) { ROS_FATAL_NAMED("imu", "gazebo_ros_imu plugin error: bodyName: %s does not exist\n", @@ -171,11 +171,11 @@ void GazeboRosIMU::LoadThread() } // Initialize the controller - this->last_time_ = this->world_->GetSimTime(); + this->last_time_ = this->world_->SimTime(); // this->initial_pose_ = this->link->GetPose(); - this->last_vpos_ = this->link->GetWorldLinearVel(); - this->last_veul_ = this->link->GetWorldAngularVel(); + this->last_vpos_ = this->link->WorldLinearVel(); + this->last_veul_ = this->link->WorldAngularVel(); this->apos_ = 0; this->aeul_ = 0; @@ -203,7 +203,7 @@ bool GazeboRosIMU::ServiceCallback(std_srvs::Empty::Request &req, // Update the controller void GazeboRosIMU::UpdateChild() { - common::Time cur_time = this->world_->GetSimTime(); + common::Time cur_time = this->world_->SimTime(); // rate control if (this->update_rate_ > 0 && @@ -212,23 +212,23 @@ void GazeboRosIMU::UpdateChild() if ((this->pub_.getNumSubscribers() > 0 && this->topic_name_ != "")) { - math::Pose pose; - math::Quaternion rot; - math::Vector3 pos; + ignition::math::Pose3d pose; + ignition::math::Quaterniond rot; + ignition::math::Vector3d pos; // Get Pose/Orientation ///@todo: verify correctness - pose = this->link->GetWorldPose(); + pose = this->link->WorldPose(); // apply xyz offsets and get position and rotation components - pos = pose.pos + this->offset_.pos; - rot = pose.rot; + pos = pose.Pos() + this->offset_.Pos(); + rot = pose.Rot(); // apply rpy offsets - rot = this->offset_.rot*rot; + rot = this->offset_.Rot() * rot; rot.Normalize(); // get Rates - math::Vector3 vpos = this->link->GetWorldLinearVel(); - math::Vector3 veul = this->link->GetWorldAngularVel(); + ignition::math::Vector3d vpos = this->link->WorldLinearVel(); + ignition::math::Vector3d veul = this->link->WorldAngularVel(); // differentiate to get accelerations double tmp_dt = this->last_time_.Double() - cur_time.Double(); @@ -253,34 +253,34 @@ void GazeboRosIMU::UpdateChild() // rot = this->initial_pose_.rot*rot; // rot.Normalize(); - this->imu_msg_.orientation.x = rot.x; - this->imu_msg_.orientation.y = rot.y; - this->imu_msg_.orientation.z = rot.z; - this->imu_msg_.orientation.w = rot.w; + this->imu_msg_.orientation.x = rot.X(); + this->imu_msg_.orientation.y = rot.Y(); + this->imu_msg_.orientation.z = rot.Z(); + this->imu_msg_.orientation.w = rot.W(); // pass euler angular rates - math::Vector3 linear_velocity( - veul.x + this->GaussianKernel(0, this->gaussian_noise_), - veul.y + this->GaussianKernel(0, this->gaussian_noise_), - veul.z + this->GaussianKernel(0, this->gaussian_noise_)); + ignition::math::Vector3d linear_velocity( + veul.X() + this->GaussianKernel(0, this->gaussian_noise_), + veul.Y() + this->GaussianKernel(0, this->gaussian_noise_), + veul.Z() + this->GaussianKernel(0, this->gaussian_noise_)); // rotate into local frame // @todo: deal with offsets! linear_velocity = rot.RotateVector(linear_velocity); - this->imu_msg_.angular_velocity.x = linear_velocity.x; - this->imu_msg_.angular_velocity.y = linear_velocity.y; - this->imu_msg_.angular_velocity.z = linear_velocity.z; + this->imu_msg_.angular_velocity.x = linear_velocity.X(); + this->imu_msg_.angular_velocity.y = linear_velocity.Y(); + this->imu_msg_.angular_velocity.z = linear_velocity.Z(); // pass accelerations - math::Vector3 linear_acceleration( - apos_.x + this->GaussianKernel(0, this->gaussian_noise_), - apos_.y + this->GaussianKernel(0, this->gaussian_noise_), - apos_.z + this->GaussianKernel(0, this->gaussian_noise_)); + ignition::math::Vector3d linear_acceleration( + apos_.X() + this->GaussianKernel(0, this->gaussian_noise_), + apos_.Y() + this->GaussianKernel(0, this->gaussian_noise_), + apos_.Z() + this->GaussianKernel(0, this->gaussian_noise_)); // rotate into local frame // @todo: deal with offsets! linear_acceleration = rot.RotateVector(linear_acceleration); - this->imu_msg_.linear_acceleration.x = linear_acceleration.x; - this->imu_msg_.linear_acceleration.y = linear_acceleration.y; - this->imu_msg_.linear_acceleration.z = linear_acceleration.z; + this->imu_msg_.linear_acceleration.x = linear_acceleration.X(); + this->imu_msg_.linear_acceleration.y = linear_acceleration.Y(); + this->imu_msg_.linear_acceleration.z = linear_acceleration.Z(); // fill in covariance matrix /// @todo: let user set separate linear and angular covariance values. diff --git a/gazebo_plugins/src/gazebo_ros_imu_sensor.cpp b/gazebo_plugins/src/gazebo_ros_imu_sensor.cpp index 33dc74a2b..0be116e0a 100644 --- a/gazebo_plugins/src/gazebo_ros_imu_sensor.cpp +++ b/gazebo_plugins/src/gazebo_ros_imu_sensor.cpp @@ -23,11 +23,11 @@ GZ_REGISTER_SENSOR_PLUGIN(gazebo::GazeboRosImuSensor) gazebo::GazeboRosImuSensor::GazeboRosImuSensor(): SensorPlugin() { - accelerometer_data = math::Vector3(0, 0, 0); - gyroscope_data = math::Vector3(0, 0, 0); - orientation = math::Quaternion(1,0,0,0); - seed=0; - sensor=NULL; + accelerometer_data = ignition::math::Vector3d(0, 0, 0); + gyroscope_data = ignition::math::Vector3d(0, 0, 0); + orientation = ignition::math::Quaterniond(1,0,0,0); + seed = 0; + sensor = NULL; } void gazebo::GazeboRosImuSensor::Load(gazebo::sensors::SensorPtr sensor_, sdf::ElementPtr sdf_) @@ -82,28 +82,28 @@ void gazebo::GazeboRosImuSensor::UpdateChild(const gazebo::common::UpdateInfo &/ if(imu_data_publisher.getNumSubscribers() > 0) { #if GAZEBO_MAJOR_VERSION >= 6 - orientation = offset.rot*sensor->Orientation(); //applying offsets to the orientation measurement + orientation = offset.Rot() * sensor->Orientation(); //applying offsets to the orientation measurement accelerometer_data = sensor->LinearAcceleration(); gyroscope_data = sensor->AngularVelocity(); #else - orientation = offset.rot*sensor->GetOrientation(); //applying offsets to the orientation measurement + orientation = offset.rot * sensor->GetOrientation(); //applying offsets to the orientation measurement accelerometer_data = sensor->GetLinearAcceleration(); gyroscope_data = sensor->GetAngularVelocity(); #endif //Guassian noise is applied to all measurements - imu_msg.orientation.x = orientation.x + GuassianKernel(0,gaussian_noise); - imu_msg.orientation.y = orientation.y + GuassianKernel(0,gaussian_noise); - imu_msg.orientation.z = orientation.z + GuassianKernel(0,gaussian_noise); - imu_msg.orientation.w = orientation.w + GuassianKernel(0,gaussian_noise); + imu_msg.orientation.x = orientation.X() + GuassianKernel(0,gaussian_noise); + imu_msg.orientation.y = orientation.Y() + GuassianKernel(0,gaussian_noise); + imu_msg.orientation.z = orientation.Z() + GuassianKernel(0,gaussian_noise); + imu_msg.orientation.w = orientation.W() + GuassianKernel(0,gaussian_noise); - imu_msg.linear_acceleration.x = accelerometer_data.x + GuassianKernel(0,gaussian_noise); - imu_msg.linear_acceleration.y = accelerometer_data.y + GuassianKernel(0,gaussian_noise); - imu_msg.linear_acceleration.z = accelerometer_data.z + GuassianKernel(0,gaussian_noise); + imu_msg.linear_acceleration.x = accelerometer_data.X() + GuassianKernel(0,gaussian_noise); + imu_msg.linear_acceleration.y = accelerometer_data.Y() + GuassianKernel(0,gaussian_noise); + imu_msg.linear_acceleration.z = accelerometer_data.Z() + GuassianKernel(0,gaussian_noise); - imu_msg.angular_velocity.x = gyroscope_data.x + GuassianKernel(0,gaussian_noise); - imu_msg.angular_velocity.y = gyroscope_data.y + GuassianKernel(0,gaussian_noise); - imu_msg.angular_velocity.z = gyroscope_data.z + GuassianKernel(0,gaussian_noise); + imu_msg.angular_velocity.x = gyroscope_data.X() + GuassianKernel(0,gaussian_noise); + imu_msg.angular_velocity.y = gyroscope_data.Y() + GuassianKernel(0,gaussian_noise); + imu_msg.angular_velocity.z = gyroscope_data.Z() + GuassianKernel(0,gaussian_noise); //covariance is related to the Gaussian noise double gn2 = gaussian_noise*gaussian_noise; @@ -219,25 +219,25 @@ bool gazebo::GazeboRosImuSensor::LoadParameters() //POSITION OFFSET, UNUSED if (sdf->HasElement("xyzOffset")) { - offset.pos = sdf->Get("xyzOffset"); - ROS_INFO_STREAM(" set to: " << offset.pos[0] << ' ' << offset.pos[1] << ' ' << offset.pos[2]); + offset.Pos() = sdf->Get("xyzOffset"); + ROS_INFO_STREAM(" set to: " << offset.Pos()[0] << ' ' << offset.Pos()[1] << ' ' << offset.Pos()[2]); } else { - offset.pos = math::Vector3(0, 0, 0); - ROS_WARN_STREAM("missing , set to default: " << offset.pos[0] << ' ' << offset.pos[1] << ' ' << offset.pos[2]); + offset.Pos() =ignition::math::Vector3d(0, 0, 0); + ROS_WARN_STREAM("missing , set to default: " << offset.Pos()[0] << ' ' << offset.Pos()[1] << ' ' << offset.Pos()[2]); } //ORIENTATION OFFSET if (sdf->HasElement("rpyOffset")) { - offset.rot = sdf->Get("rpyOffset"); - ROS_INFO_STREAM(" set to: " << offset.rot.GetRoll() << ' ' << offset.rot.GetPitch() << ' ' << offset.rot.GetYaw()); + offset.Rot() = sdf->Get("rpyOffset"); + ROS_INFO_STREAM(" set to: " << offset.Rot().Roll() << ' ' << offset.Rot().Pitch() << ' ' << offset.Rot().Yaw()); } else { - offset.rot = math::Vector3(0, 0, 0); - ROS_WARN_STREAM("missing , set to default: " << offset.rot.GetRoll() << ' ' << offset.rot.GetPitch() << ' ' << offset.rot.GetYaw()); + offset.Rot() = ignition::math::Quaterniond(0, 0, 0); + ROS_WARN_STREAM("missing , set to default: " << offset.Rot().Roll() << ' ' << offset.Rot().Pitch() << ' ' << offset.Rot().Yaw()); } return true; @@ -247,9 +247,9 @@ gazebo::GazeboRosImuSensor::~GazeboRosImuSensor() { if (connection.get()) { - gazebo::event::Events::DisconnectWorldUpdateBegin(connection); + connection.reset(); connection = gazebo::event::ConnectionPtr(); } node->shutdown(); -} \ No newline at end of file +} diff --git a/gazebo_plugins/src/gazebo_ros_joint_pose_trajectory.cpp b/gazebo_plugins/src/gazebo_ros_joint_pose_trajectory.cpp index 6eadd47b8..e4cd281f5 100644 --- a/gazebo_plugins/src/gazebo_ros_joint_pose_trajectory.cpp +++ b/gazebo_plugins/src/gazebo_ros_joint_pose_trajectory.cpp @@ -47,7 +47,7 @@ ROS_DEPRECATED GazeboRosJointPoseTrajectory::GazeboRosJointPoseTrajectory() // // Destructor GazeboRosJointPoseTrajectory::~GazeboRosJointPoseTrajectory() { - event::Events::DisconnectWorldUpdateBegin(this->update_connection_); + this->update_connection_.reset(); // Finalize the controller this->rosnode_->shutdown(); this->queue_.clear(); @@ -66,7 +66,7 @@ void GazeboRosJointPoseTrajectory::Load(physics::ModelPtr _model, this->sdf = _sdf; this->world_ = this->model_->GetWorld(); - // this->world_->GetPhysicsEngine()->SetGravity(math::Vector3(0, 0, 0)); + // this->world_->Engine()->SetGravity(ignition::math::Vector3d(0, 0, 0)); // load parameters this->robot_namespace_ = ""; @@ -143,7 +143,7 @@ void GazeboRosJointPoseTrajectory::LoadThread() } #endif - this->last_time_ = this->world_->GetSimTime(); + this->last_time_ = this->world_->SimTime(); // start custom queue for joint trajectory plugin ros topics this->callback_queue_thread_ = @@ -171,7 +171,7 @@ void GazeboRosJointPoseTrajectory::SetTrajectory( this->reference_link_name_ != "map") { physics::EntityPtr ent = - this->world_->GetEntity(this->reference_link_name_); + this->world_->EntityByName(this->reference_link_name_); if (ent) this->reference_link_ = boost::dynamic_pointer_cast(ent); if (!this->reference_link_) @@ -211,7 +211,7 @@ void GazeboRosJointPoseTrajectory::SetTrajectory( // trajectory start time this->trajectory_start = gazebo::common::Time(trajectory->header.stamp.sec, trajectory->header.stamp.nsec); - common::Time cur_time = this->world_->GetSimTime(); + common::Time cur_time = this->world_->SimTime(); if (this->trajectory_start < cur_time) this->trajectory_start = cur_time; @@ -222,8 +222,8 @@ void GazeboRosJointPoseTrajectory::SetTrajectory( if (this->disable_physics_updates_) { - this->physics_engine_enabled_ = this->world_->GetEnablePhysicsEngine(); - this->world_->EnablePhysicsEngine(false); + this->world_->SetPhysicsEnabled(false); + this->physics_engine_enabled_ = false; } } @@ -245,7 +245,7 @@ bool GazeboRosJointPoseTrajectory::SetTrajectory( this->reference_link_name_ != "map") { physics::EntityPtr ent = - this->world_->GetEntity(this->reference_link_name_); + this->world_->EntityByName(this->reference_link_name_); if (ent) this->reference_link_ = boost::dynamic_pointer_cast(ent); if (!this->reference_link_) @@ -262,7 +262,7 @@ bool GazeboRosJointPoseTrajectory::SetTrajectory( " inertially", this->reference_link_->GetName().c_str()); } - this->model_ = this->world_->GetModel(req.model_name); + this->model_ = this->world_->ModelByName(req.model_name); if (!this->model_) // look for it by frame_id name { this->model_ = this->reference_link_->GetParentModel(); @@ -295,8 +295,8 @@ bool GazeboRosJointPoseTrajectory::SetTrajectory( this->disable_physics_updates_ = req.disable_physics_updates; if (this->disable_physics_updates_) { - this->physics_engine_enabled_ = this->world_->GetEnablePhysicsEngine(); - this->world_->EnablePhysicsEngine(false); + this->world_->SetPhysicsEnabled(false); + this->physics_engine_enabled_ = false; } return true; @@ -310,7 +310,7 @@ void GazeboRosJointPoseTrajectory::UpdateStates() boost::mutex::scoped_lock lock(this->update_mutex); if (this->has_trajectory_) { - common::Time cur_time = this->world_->GetSimTime(); + common::Time cur_time = this->world_->SimTime(); // roll out trajectory via set model configuration // gzerr << "i[" << trajectory_index << "] time " // << trajectory_start << " now: " << cur_time << " : "<< "\n"; @@ -325,10 +325,10 @@ void GazeboRosJointPoseTrajectory::UpdateStates() cur_time.Double(), this->trajectory_index, this->points_.size()); // get reference link pose before updates - math::Pose reference_pose = this->model_->GetWorldPose(); + ignition::math::Pose3d reference_pose = this->model_->WorldPose(); if (this->reference_link_) { - reference_pose = this->reference_link_->GetWorldPose(); + reference_pose = this->reference_link_->WorldPose(); } // trajectory roll-out based on time: @@ -384,7 +384,7 @@ void GazeboRosJointPoseTrajectory::UpdateStates() this->reference_link_.reset(); this->has_trajectory_ = false; if (this->disable_physics_updates_) - this->world_->EnablePhysicsEngine(this->physics_engine_enabled_); + this->world_->SetPhysicsEnabled(this->physics_engine_enabled_); } } } diff --git a/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp b/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp index 9b735eac3..3cf9b9606 100644 --- a/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp +++ b/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp @@ -78,7 +78,7 @@ void GazeboRosJointStatePublisher::Load ( physics::ModelPtr _parent, sdf::Elemen } else { this->update_period_ = 0.0; } - last_update_time_ = this->world_->GetSimTime(); + last_update_time_ = this->world_->SimTime(); for ( unsigned int i = 0; i< joint_names_.size(); i++ ) { joints_.push_back ( this->parent_->GetJoint ( joint_names_[i] ) ); @@ -90,7 +90,7 @@ void GazeboRosJointStatePublisher::Load ( physics::ModelPtr _parent, sdf::Elemen tf_prefix_ = tf::getPrefixParam ( *rosnode_ ); joint_state_publisher_ = rosnode_->advertise ( "joint_states",1000 ); - last_update_time_ = this->world_->GetSimTime(); + last_update_time_ = this->world_->SimTime(); // Listen to the update event. This event is broadcast every // simulation iteration. this->updateConnection = event::Events::ConnectWorldUpdateBegin ( @@ -99,7 +99,7 @@ void GazeboRosJointStatePublisher::Load ( physics::ModelPtr _parent, sdf::Elemen void GazeboRosJointStatePublisher::OnUpdate ( const common::UpdateInfo & _info ) { // Apply a small linear velocity to the model. - common::Time current_time = this->world_->GetSimTime(); + common::Time current_time = this->world_->SimTime(); double seconds_since_last_update = ( current_time - last_update_time_ ).Double(); if ( seconds_since_last_update > update_period_ ) { @@ -120,7 +120,7 @@ void GazeboRosJointStatePublisher::publishJointStates() { for ( int i = 0; i < joints_.size(); i++ ) { physics::JointPtr joint = joints_[i]; - math::Angle angle = joint->GetAngle ( 0 ); + ignition::math::Angle angle = joint->Position ( 0 ); joint_state_.name[i] = joint->GetName(); joint_state_.position[i] = angle.Radian () ; } diff --git a/gazebo_plugins/src/gazebo_ros_joint_trajectory.cpp b/gazebo_plugins/src/gazebo_ros_joint_trajectory.cpp index 646b95e20..768989b20 100644 --- a/gazebo_plugins/src/gazebo_ros_joint_trajectory.cpp +++ b/gazebo_plugins/src/gazebo_ros_joint_trajectory.cpp @@ -48,7 +48,7 @@ ROS_DEPRECATED GazeboRosJointTrajectory::GazeboRosJointTrajectory() // replaced // Destructor GazeboRosJointTrajectory::~GazeboRosJointTrajectory() { - event::Events::DisconnectWorldUpdateBegin(this->update_connection_); + this->update_connection_.reset(); // Finalize the controller this->rosnode_->shutdown(); this->queue_.clear(); @@ -67,7 +67,7 @@ void GazeboRosJointTrajectory::Load(physics::ModelPtr _model, this->sdf = _sdf; this->world_ = this->model_->GetWorld(); - // this->world_->GetPhysicsEngine()->SetGravity(math::Vector3(0, 0, 0)); + // this->world_->Engine()->SetGravity(ignition::math::Vector3d(0, 0, 0)); // load parameters this->robot_namespace_ = ""; @@ -145,7 +145,7 @@ void GazeboRosJointTrajectory::LoadThread() } #endif - this->last_time_ = this->world_->GetSimTime(); + this->last_time_ = this->world_->SimTime(); // start custom queue for joint trajectory plugin ros topics this->callback_queue_thread_ = @@ -173,7 +173,7 @@ void GazeboRosJointTrajectory::SetTrajectory( this->reference_link_name_ != "map") { physics::EntityPtr ent = - this->world_->GetEntity(this->reference_link_name_); + this->world_->EntityByName(this->reference_link_name_); if (ent) this->reference_link_ = boost::dynamic_pointer_cast(ent); if (!this->reference_link_) @@ -213,7 +213,7 @@ void GazeboRosJointTrajectory::SetTrajectory( // trajectory start time this->trajectory_start = gazebo::common::Time(trajectory->header.stamp.sec, trajectory->header.stamp.nsec); - common::Time cur_time = this->world_->GetSimTime(); + common::Time cur_time = this->world_->SimTime(); if (this->trajectory_start < cur_time) this->trajectory_start = cur_time; @@ -224,8 +224,8 @@ void GazeboRosJointTrajectory::SetTrajectory( if (this->disable_physics_updates_) { - this->physics_engine_enabled_ = this->world_->GetEnablePhysicsEngine(); - this->world_->EnablePhysicsEngine(false); + this->world_->SetPhysicsEnabled(false); + this->physics_engine_enabled_ = false; } } @@ -247,7 +247,7 @@ bool GazeboRosJointTrajectory::SetTrajectory( this->reference_link_name_ != "map") { physics::EntityPtr ent = - this->world_->GetEntity(this->reference_link_name_); + this->world_->EntityByName(this->reference_link_name_); if (ent) this->reference_link_ = boost::shared_dynamic_cast(ent); if (!this->reference_link_) @@ -264,7 +264,7 @@ bool GazeboRosJointTrajectory::SetTrajectory( " inertially", this->reference_link_->GetName().c_str()); } - this->model_ = this->world_->GetModel(req.model_name); + this->model_ = this->world_->ModelByName(req.model_name); if (!this->model_) // look for it by frame_id name { this->model_ = this->reference_link_->GetParentModel(); @@ -297,8 +297,8 @@ bool GazeboRosJointTrajectory::SetTrajectory( this->disable_physics_updates_ = req.disable_physics_updates; if (this->disable_physics_updates_) { - this->physics_engine_enabled_ = this->world_->GetEnablePhysicsEngine(); - this->world_->EnablePhysicsEngine(false); + this->world_->SetPhysicsEnabled(false); + this->physics_engine_enabled_ = false; } return true; @@ -312,7 +312,7 @@ void GazeboRosJointTrajectory::UpdateStates() boost::mutex::scoped_lock lock(this->update_mutex); if (this->has_trajectory_) { - common::Time cur_time = this->world_->GetSimTime(); + common::Time cur_time = this->world_->SimTime(); // roll out trajectory via set model configuration // gzerr << "i[" << trajectory_index << "] time " // << trajectory_start << " now: " << cur_time << " : "<< "\n"; @@ -327,10 +327,10 @@ void GazeboRosJointTrajectory::UpdateStates() cur_time.Double(), this->trajectory_index, this->points_.size()); // get reference link pose before updates - math::Pose reference_pose = this->model_->GetWorldPose(); + ignition::math::Pose3d reference_pose = this->model_->WorldPose(); if (this->reference_link_) { - reference_pose = this->reference_link_->GetWorldPose(); + reference_pose = this->reference_link_->WorldPose(); } // trajectory roll-out based on time: @@ -386,7 +386,7 @@ void GazeboRosJointTrajectory::UpdateStates() this->reference_link_.reset(); this->has_trajectory_ = false; if (this->disable_physics_updates_) - this->world_->EnablePhysicsEngine(this->physics_engine_enabled_); + this->world_->SetPhysicsEnabled(this->physics_engine_enabled_); } } } diff --git a/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp b/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp index 139d54228..f9f1a214f 100644 --- a/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp +++ b/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp @@ -435,7 +435,7 @@ void GazeboRosOpenniKinect::PublishCameraInfo() if (this->depth_info_connect_count_ > 0) { this->sensor_update_time_ = this->parentSensor_->LastMeasurementTime(); - common::Time cur_time = this->world_->GetSimTime(); + common::Time cur_time = this->world_->SimTime(); if (this->sensor_update_time_ - this->last_depth_image_camera_info_update_time_ >= this->update_period_) { this->PublishCameraInfo(this->depth_image_camera_info_pub_); diff --git a/gazebo_plugins/src/gazebo_ros_p3d.cpp b/gazebo_plugins/src/gazebo_ros_p3d.cpp index 1fad5732c..72f4cbd2e 100644 --- a/gazebo_plugins/src/gazebo_ros_p3d.cpp +++ b/gazebo_plugins/src/gazebo_ros_p3d.cpp @@ -36,7 +36,7 @@ GazeboRosP3D::GazeboRosP3D() // Destructor GazeboRosP3D::~GazeboRosP3D() { - event::Events::DisconnectWorldUpdateBegin(this->update_connection_); + this->update_connection_.reset(); // Finalize the controller this->rosnode_->shutdown(); this->p3d_queue_.clear(); @@ -94,18 +94,18 @@ void GazeboRosP3D::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) if (!_sdf->HasElement("xyzOffset")) { ROS_DEBUG_NAMED("p3d", "p3d plugin missing , defaults to 0s"); - this->offset_.pos = math::Vector3(0, 0, 0); + this->offset_.Pos() = ignition::math::Vector3d(0, 0, 0); } else - this->offset_.pos = _sdf->GetElement("xyzOffset")->Get(); + this->offset_.Pos() = _sdf->GetElement("xyzOffset")->Get(); if (!_sdf->HasElement("rpyOffset")) { ROS_DEBUG_NAMED("p3d", "p3d plugin missing , defaults to 0s"); - this->offset_.rot = math::Vector3(0, 0, 0); + this->offset_.Rot() = ignition::math::Quaterniond(0, 0, 0); } else - this->offset_.rot = _sdf->GetElement("rpyOffset")->Get(); + this->offset_.Rot() = _sdf->GetElement("rpyOffset")->Get(); if (!_sdf->HasElement("gaussianNoise")) { @@ -149,10 +149,10 @@ void GazeboRosP3D::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) this->rosnode_->advertise(this->topic_name_, 1); } - this->last_time_ = this->world_->GetSimTime(); + this->last_time_ = this->world_->SimTime(); // initialize body - this->last_vpos_ = this->link_->GetWorldLinearVel(); - this->last_veul_ = this->link_->GetWorldAngularVel(); + this->last_vpos_ = this->link_->WorldLinearVel(); + this->last_veul_ = this->link_->WorldAngularVel(); this->apos_ = 0; this->aeul_ = 0; @@ -178,8 +178,8 @@ void GazeboRosP3D::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) ROS_DEBUG_NAMED("p3d", "got body %s", this->reference_link_->GetName().c_str()); this->frame_apos_ = 0; this->frame_aeul_ = 0; - this->last_frame_vpos_ = this->reference_link_->GetWorldLinearVel(); - this->last_frame_veul_ = this->reference_link_->GetWorldAngularVel(); + this->last_frame_vpos_ = this->reference_link_->WorldLinearVel(); + this->last_frame_veul_ = this->reference_link_->WorldAngularVel(); } @@ -201,7 +201,7 @@ void GazeboRosP3D::UpdateChild() if (!this->link_) return; - common::Time cur_time = this->world_->GetSimTime(); + common::Time cur_time = this->world_->SimTime(); // rate control if (this->update_rate_ > 0 && @@ -225,38 +225,38 @@ void GazeboRosP3D::UpdateChild() this->pose_msg_.child_frame_id = this->link_name_; - math::Pose pose, frame_pose; - math::Vector3 frame_vpos; - math::Vector3 frame_veul; + ignition::math::Pose3d pose, frame_pose; + ignition::math::Vector3d frame_vpos; + ignition::math::Vector3d frame_veul; // get inertial Rates - math::Vector3 vpos = this->link_->GetWorldLinearVel(); - math::Vector3 veul = this->link_->GetWorldAngularVel(); + ignition::math::Vector3d vpos = this->link_->WorldLinearVel(); + ignition::math::Vector3d veul = this->link_->WorldAngularVel(); // Get Pose/Orientation - pose = this->link_->GetWorldPose(); + pose = this->link_->WorldPose(); // Apply Reference Frame if (this->reference_link_) { // convert to relative pose - frame_pose = this->reference_link_->GetWorldPose(); - pose.pos = pose.pos - frame_pose.pos; - pose.pos = frame_pose.rot.RotateVectorReverse(pose.pos); - pose.rot *= frame_pose.rot.GetInverse(); + frame_pose = this->reference_link_->WorldPose(); + pose.Pos() = pose.Pos() - frame_pose.Pos(); + pose.Pos() = frame_pose.Rot().RotateVectorReverse(pose.Pos()); + pose.Rot() *= frame_pose.Rot().Inverse(); // convert to relative rates - frame_vpos = this->reference_link_->GetWorldLinearVel(); - frame_veul = this->reference_link_->GetWorldAngularVel(); - vpos = frame_pose.rot.RotateVector(vpos - frame_vpos); - veul = frame_pose.rot.RotateVector(veul - frame_veul); + frame_vpos = this->reference_link_->WorldLinearVel(); + frame_veul = this->reference_link_->WorldAngularVel(); + vpos = frame_pose.Rot().RotateVector(vpos - frame_vpos); + veul = frame_pose.Rot().RotateVector(veul - frame_veul); } // Apply Constant Offsets // apply xyz offsets and get position and rotation components - pose.pos = pose.pos + this->offset_.pos; + pose.Pos() = pose.Pos() + this->offset_.Pos(); // apply rpy offsets - pose.rot = this->offset_.rot*pose.rot; - pose.rot.Normalize(); + pose.Rot() = this->offset_.Rot() * pose.Rot(); + pose.Rot().Normalize(); // compute accelerations (not used) this->apos_ = (this->last_vpos_ - vpos) / tmp_dt; @@ -270,27 +270,27 @@ void GazeboRosP3D::UpdateChild() this->last_frame_veul_ = frame_veul; // Fill out messages - this->pose_msg_.pose.pose.position.x = pose.pos.x; - this->pose_msg_.pose.pose.position.y = pose.pos.y; - this->pose_msg_.pose.pose.position.z = pose.pos.z; + this->pose_msg_.pose.pose.position.x = pose.Pos().X(); + this->pose_msg_.pose.pose.position.y = pose.Pos().Y(); + this->pose_msg_.pose.pose.position.z = pose.Pos().Z(); - this->pose_msg_.pose.pose.orientation.x = pose.rot.x; - this->pose_msg_.pose.pose.orientation.y = pose.rot.y; - this->pose_msg_.pose.pose.orientation.z = pose.rot.z; - this->pose_msg_.pose.pose.orientation.w = pose.rot.w; + this->pose_msg_.pose.pose.orientation.x = pose.Rot().X(); + this->pose_msg_.pose.pose.orientation.y = pose.Rot().Y(); + this->pose_msg_.pose.pose.orientation.z = pose.Rot().Z(); + this->pose_msg_.pose.pose.orientation.w = pose.Rot().W(); - this->pose_msg_.twist.twist.linear.x = vpos.x + + this->pose_msg_.twist.twist.linear.x = vpos.X() + this->GaussianKernel(0, this->gaussian_noise_); - this->pose_msg_.twist.twist.linear.y = vpos.y + + this->pose_msg_.twist.twist.linear.y = vpos.Y() + this->GaussianKernel(0, this->gaussian_noise_); - this->pose_msg_.twist.twist.linear.z = vpos.z + + this->pose_msg_.twist.twist.linear.z = vpos.Z() + this->GaussianKernel(0, this->gaussian_noise_); // pass euler angular rates - this->pose_msg_.twist.twist.angular.x = veul.x + + this->pose_msg_.twist.twist.angular.x = veul.X() + this->GaussianKernel(0, this->gaussian_noise_); - this->pose_msg_.twist.twist.angular.y = veul.y + + this->pose_msg_.twist.twist.angular.y = veul.Y() + this->GaussianKernel(0, this->gaussian_noise_); - this->pose_msg_.twist.twist.angular.z = veul.z + + this->pose_msg_.twist.twist.angular.z = veul.Z() + this->GaussianKernel(0, this->gaussian_noise_); // fill in covariance matrix diff --git a/gazebo_plugins/src/gazebo_ros_planar_move.cpp b/gazebo_plugins/src/gazebo_ros_planar_move.cpp index dc770809a..ea1c0f336 100644 --- a/gazebo_plugins/src/gazebo_ros_planar_move.cpp +++ b/gazebo_plugins/src/gazebo_ros_planar_move.cpp @@ -112,8 +112,8 @@ namespace gazebo odometry_rate_ = sdf->GetElement("odometryRate")->Get(); } - last_odom_publish_time_ = parent_->GetWorld()->GetSimTime(); - last_odom_pose_ = parent_->GetWorldPose(); + last_odom_publish_time_ = parent_->GetWorld()->SimTime(); + last_odom_pose_ = parent_->WorldPose(); x_ = 0; y_ = 0; rot_ = 0; @@ -160,15 +160,15 @@ namespace gazebo void GazeboRosPlanarMove::UpdateChild() { boost::mutex::scoped_lock scoped_lock(lock); - math::Pose pose = parent_->GetWorldPose(); - float yaw = pose.rot.GetYaw(); - parent_->SetLinearVel(math::Vector3( + ignition::math::Pose3d pose = parent_->WorldPose(); + float yaw = pose.Rot().Yaw(); + parent_->SetLinearVel(ignition::math::Vector3d( x_ * cosf(yaw) - y_ * sinf(yaw), y_ * cosf(yaw) + x_ * sinf(yaw), 0)); - parent_->SetAngularVel(math::Vector3(0, 0, rot_)); + parent_->SetAngularVel(ignition::math::Vector3d(0, 0, rot_)); if (odometry_rate_ > 0.0) { - common::Time current_time = parent_->GetWorld()->GetSimTime(); + common::Time current_time = parent_->GetWorld()->SimTime(); double seconds_since_last_update = (current_time - last_odom_publish_time_).Double(); if (seconds_since_last_update > (1.0 / odometry_rate_)) { @@ -214,10 +214,10 @@ namespace gazebo tf::resolve(tf_prefix_, robot_base_frame_); // getting data for base_footprint to odom transform - math::Pose pose = this->parent_->GetWorldPose(); + ignition::math::Pose3d pose = this->parent_->WorldPose(); - 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 base_footprint_to_odom(qt, vt); transform_broadcaster_->sendTransform( @@ -225,13 +225,13 @@ namespace gazebo 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.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.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; @@ -240,9 +240,9 @@ namespace gazebo odom_.pose.covariance[35] = 0.001; // get velocity in /odom frame - math::Vector3 linear; - linear.x = (pose.pos.x - last_odom_pose_.pos.x) / step_time; - linear.y = (pose.pos.y - last_odom_pose_.pos.y) / step_time; + ignition::math::Vector3d linear; + linear.X() = (pose.Pos().X() - last_odom_pose_.Pos().X()) / step_time; + linear.Y() = (pose.Pos().Y() - last_odom_pose_.Pos().Y()) / step_time; if (rot_ > M_PI / step_time) { // we cannot calculate the angular velocity correctly @@ -250,8 +250,8 @@ namespace gazebo } else { - float last_yaw = last_odom_pose_.rot.GetYaw(); - float current_yaw = pose.rot.GetYaw(); + float last_yaw = last_odom_pose_.Rot().Yaw(); + float current_yaw = pose.Rot().Yaw(); while (current_yaw < last_yaw - M_PI) current_yaw += 2 * M_PI; while (current_yaw > last_yaw + M_PI) current_yaw -= 2 * M_PI; float angular_diff = current_yaw - last_yaw; @@ -260,9 +260,9 @@ namespace gazebo last_odom_pose_ = pose; // 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; + float yaw = pose.Rot().Yaw(); + 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; diff --git a/gazebo_plugins/src/gazebo_ros_projector.cpp b/gazebo_plugins/src/gazebo_ros_projector.cpp index 7a11472d6..635deffa4 100644 --- a/gazebo_plugins/src/gazebo_ros_projector.cpp +++ b/gazebo_plugins/src/gazebo_ros_projector.cpp @@ -82,7 +82,7 @@ void GazeboRosProjector::Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf ) // Create a new transport node for talking to the projector this->node_.reset(new transport::Node()); // Initialize the node with the world name - this->node_->Init(this->world_->GetName()); + this->node_->Init(this->world_->Name()); // Setting projector topic std::string name = std::string("~/") + _parent->GetName() + "/" + _sdf->Get("projector"); diff --git a/gazebo_plugins/src/gazebo_ros_prosilica.cpp b/gazebo_plugins/src/gazebo_ros_prosilica.cpp index d74bc5212..cb25c41d7 100644 --- a/gazebo_plugins/src/gazebo_ros_prosilica.cpp +++ b/gazebo_plugins/src/gazebo_ros_prosilica.cpp @@ -333,9 +333,9 @@ void GazeboRosProsilica::OnStats( const boost::shared_ptrsimTime = msgs::Convert( _msg->sim_time() ); - math::Pose pose; - pose.pos.x = 0.5*sin(0.01*this->simTime.Double()); - gzdbg << "plugin simTime [" << this->simTime.Double() << "] update pose [" << pose.pos.x << "]\n"; + ignition::math::Pose3d pose; + pose.Pos().X() = 0.5*sin(0.01*this->simTime.Double()); + gzdbg << "plugin simTime [" << this->simTime.Double() << "] update pose [" << pose.Pos().X() << "]\n"; } */ diff --git a/gazebo_plugins/src/gazebo_ros_range.cpp b/gazebo_plugins/src/gazebo_ros_range.cpp index 54940b245..9ecaf4062 100644 --- a/gazebo_plugins/src/gazebo_ros_range.cpp +++ b/gazebo_plugins/src/gazebo_ros_range.cpp @@ -243,7 +243,7 @@ void GazeboRosRange::OnNewLaserScans() { if (this->topic_name_ != "") { - common::Time cur_time = this->world_->GetSimTime(); + common::Time cur_time = this->world_->SimTime(); if (cur_time - this->last_update_time_ >= this->update_period_) { common::Time sensor_update_time = diff --git a/gazebo_plugins/src/gazebo_ros_skid_steer_drive.cpp b/gazebo_plugins/src/gazebo_ros_skid_steer_drive.cpp index 92e572134..0227c8c16 100644 --- a/gazebo_plugins/src/gazebo_ros_skid_steer_drive.cpp +++ b/gazebo_plugins/src/gazebo_ros_skid_steer_drive.cpp @@ -42,7 +42,6 @@ #include -#include #include #include @@ -231,7 +230,7 @@ namespace gazebo { } else { this->update_period_ = 0.0; } - last_update_time_ = this->world->GetSimTime(); + last_update_time_ = this->world->SimTime(); // Initialize velocity stuff wheel_speed_[RIGHT_FRONT] = 0; @@ -330,7 +329,7 @@ namespace gazebo { // Update the controller void GazeboRosSkidSteerDrive::UpdateChild() { - common::Time current_time = this->world->GetSimTime(); + common::Time current_time = this->world->SimTime(); double seconds_since_last_update = (current_time - last_update_time_).Double(); if (seconds_since_last_update > update_period_) { @@ -404,10 +403,10 @@ namespace gazebo { // TODO create some non-perfect odometry! // getting data for base_footprint to odom transform - math::Pose pose = this->parent->GetWorldPose(); + ignition::math::Pose3d pose = this->parent->WorldPose(); - 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 base_footprint_to_odom(qt, vt); if (this->broadcast_tf_) { @@ -419,13 +418,13 @@ namespace gazebo { } // publish odom topic - odom_.pose.pose.position.x = pose.pos.x; - odom_.pose.pose.position.y = pose.pos.y; + 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.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] = this->covariance_x_; odom_.pose.covariance[7] = this->covariance_y_; odom_.pose.covariance[14] = 1000000000000.0; @@ -434,14 +433,14 @@ namespace gazebo { odom_.pose.covariance[35] = this->covariance_yaw_; // get velocity in /odom frame - math::Vector3 linear; - linear = this->parent->GetWorldLinearVel(); - odom_.twist.twist.angular.z = this->parent->GetWorldAngularVel().z; + ignition::math::Vector3d linear; + linear = this->parent->WorldLinearVel(); + odom_.twist.twist.angular.z = this->parent->WorldAngularVel().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; + float yaw = pose.Rot().Yaw(); + 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.covariance[0] = this->covariance_x_; odom_.twist.covariance[7] = this->covariance_y_; odom_.twist.covariance[14] = 1000000000000.0; diff --git a/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp b/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp index 74f6c4f5f..dabc8baf9 100644 --- a/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp +++ b/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp @@ -44,7 +44,6 @@ #include -#include #include #include @@ -120,7 +119,7 @@ void GazeboRosTricycleDrive::Load ( physics::ModelPtr _parent, sdf::ElementPtr _ // 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(); + last_actuator_update_ = parent->GetWorld()->SimTime(); // Initialize velocity stuff alive_ = true; @@ -170,7 +169,7 @@ 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(); - joint_state_.position[i] = joints[i]->GetAngle ( 0 ).Radian(); + joint_state_.position[i] = joints[i]->Position(0); joint_state_.velocity[i] = joints[i]->GetVelocity ( 0 ); joint_state_.effort[i] = joints[i]->GetForce ( 0 ); } @@ -189,10 +188,10 @@ void GazeboRosTricycleDrive::publishWheelTF() 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(); + ignition::math::Pose3d pose = joints[i]->GetChild()->RelativePose(); - 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 ) ); @@ -203,7 +202,7 @@ void GazeboRosTricycleDrive::publishWheelTF() void GazeboRosTricycleDrive::UpdateChild() { if ( odom_source_ == ENCODER ) UpdateOdometryEncoder(); - common::Time current_time = parent->GetWorld()->GetSimTime(); + common::Time current_time = parent->GetWorld()->SimTime(); double seconds_since_last_update = ( current_time - last_actuator_update_ ).Double(); if ( seconds_since_last_update > update_period_ ) { @@ -254,7 +253,7 @@ void GazeboRosTricycleDrive::motorController ( double target_speed, double targe joint_wheel_actuated_->SetVelocity ( 0, applied_speed ); #endif - double current_angle = joint_steering_->GetAngle ( 0 ).Radian(); + double current_angle = joint_steering_->Position(0); // truncate target angle if (target_angle > +M_PI / 2.0) @@ -349,7 +348,7 @@ 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(); + common::Time current_time = parent->GetWorld()->SimTime(); double seconds_since_last_update = ( current_time - last_odom_update_ ).Double(); last_odom_update_ = current_time; @@ -409,9 +408,9 @@ void GazeboRosTricycleDrive::publishOdometry ( double step_time ) } 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 ); + ignition::math::Pose3d pose = parent->WorldPose(); + 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(); @@ -423,14 +422,14 @@ void GazeboRosTricycleDrive::publishOdometry ( double step_time ) 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; + ignition::math::Vector3d linear; + linear = parent->WorldLinearVel(); + odom_.twist.twist.angular.z = parent->WorldAngularVel().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; + float yaw = pose.Rot().Yaw(); + 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 ); diff --git a/gazebo_plugins/src/gazebo_ros_vacuum_gripper.cpp b/gazebo_plugins/src/gazebo_ros_vacuum_gripper.cpp index 57e4fa4a1..a5f0309e8 100644 --- a/gazebo_plugins/src/gazebo_ros_vacuum_gripper.cpp +++ b/gazebo_plugins/src/gazebo_ros_vacuum_gripper.cpp @@ -44,7 +44,8 @@ GazeboRosVacuumGripper::GazeboRosVacuumGripper() // Destructor GazeboRosVacuumGripper::~GazeboRosVacuumGripper() { - event::Events::DisconnectWorldUpdateBegin(update_connection_); + // Conversion from DisconnectWorldUpdateBegin in migration from gazebo7 + update_connection_.reset(); // Custom Callback Queue queue_.clear(); @@ -179,8 +180,8 @@ void GazeboRosVacuumGripper::UpdateChild() } // apply force lock_.lock(); - math::Pose parent_pose = link_->GetWorldPose(); - physics::Model_V models = world_->GetModels(); + ignition::math::Pose3d parent_pose = link_->WorldPose(); + physics::Model_V models = world_->Models(); for (size_t i = 0; i < models.size(); i++) { if (models[i]->GetName() == link_->GetName() || models[i]->GetName() == parent_->GetName()) @@ -189,25 +190,25 @@ void GazeboRosVacuumGripper::UpdateChild() } physics::Link_V links = models[i]->GetLinks(); for (size_t j = 0; j < links.size(); j++) { - math::Pose link_pose = links[j]->GetWorldPose(); - math::Pose diff = parent_pose - link_pose; - double norm = diff.pos.GetLength(); + ignition::math::Pose3d link_pose = links[j]->WorldPose(); + ignition::math::Pose3d diff = parent_pose - link_pose; + double norm = diff.Pos().Length(); if (norm < 0.05) { - links[j]->SetLinearAccel(link_->GetWorldLinearAccel()); - links[j]->SetAngularAccel(link_->GetWorldAngularAccel()); - links[j]->SetLinearVel(link_->GetWorldLinearVel()); - links[j]->SetAngularVel(link_->GetWorldAngularVel()); + links[j]->SetLinearAccel(link_->WorldLinearAccel()); + links[j]->SetAngularAccel(link_->WorldAngularAccel()); + links[j]->SetLinearVel(link_->WorldLinearVel()); + links[j]->SetAngularVel(link_->WorldAngularVel()); double norm_force = 1 / norm; if (norm < 0.01) { // apply friction like force // TODO(unknown): should apply friction actually - link_pose.Set(parent_pose.pos, link_pose.rot); + link_pose.Set(parent_pose.Pos(), link_pose.Rot()); links[j]->SetWorldPose(link_pose); } if (norm_force > 20) { norm_force = 20; // max_force } - math::Vector3 force = norm_force * diff.pos.Normalize(); + ignition::math::Vector3d force = norm_force * diff.Pos().Normalize(); links[j]->AddForce(force); grasping_msg.data = true; } diff --git a/gazebo_plugins/src/gazebo_ros_video.cpp b/gazebo_plugins/src/gazebo_ros_video.cpp index 649f36384..4e4971179 100644 --- a/gazebo_plugins/src/gazebo_ros_video.cpp +++ b/gazebo_plugins/src/gazebo_ros_video.cpp @@ -115,7 +115,7 @@ namespace gazebo // Destructor GazeboRosVideo::~GazeboRosVideo() { - event::Events::DisconnectWorldUpdateBegin(update_connection_); + update_connection_.reset(); // Custom Callback Queue queue_.clear(); diff --git a/gazebo_plugins/test/pub_joint_trajectory_test.cpp b/gazebo_plugins/test/pub_joint_trajectory_test.cpp index a1591711d..3c8b0e335 100644 --- a/gazebo_plugins/test/pub_joint_trajectory_test.cpp +++ b/gazebo_plugins/test/pub_joint_trajectory_test.cpp @@ -1,7 +1,7 @@ #include #include #include -#include +#include #include int main(int argc, char** argv) @@ -62,15 +62,15 @@ int main(int argc, char** argv) sjt.request.joint_trajectory = jt; sjt.request.disable_physics_updates = false; - gazebo::math::Quaternion r(0, 0, M_PI); + ignition::math::Quaterniond r(0, 0, M_PI); geometry_msgs::Pose p; p.position.x = 0; p.position.y = 0; p.position.z = 0; - p.orientation.x = r.x; - p.orientation.y = r.y; - p.orientation.z = r.z; - p.orientation.w = r.w; + p.orientation.x = r.X(); + p.orientation.y = r.Y(); + p.orientation.z = r.Z(); + p.orientation.w = r.W(); sjt.request.model_pose = p; sjt.request.set_model_pose = true;