AccelerationsTestPlugin.cc
Go to the documentation of this file.
1 // Copyright (c) 2016 The UUV Simulator Authors.
2 // All rights reserved.
3 //
4 // Licensed under the Apache License, Version 2.0 (the "License");
5 // you may not use this file except in compliance with the License.
6 // You may obtain a copy of the License at
7 //
8 // http://www.apache.org/licenses/LICENSE-2.0
9 //
10 // Unless required by applicable law or agreed to in writing, software
11 // distributed under the License is distributed on an "AS IS" BASIS,
12 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 // See the License for the specific language governing permissions and
14 // limitations under the License.
15 
16 #include <gazebo/gazebo.hh>
17 #include <gazebo/physics/Collision.hh>
18 #include <gazebo/physics/Link.hh>
19 #include <gazebo/physics/Model.hh>
20 #include <gazebo/physics/PhysicsEngine.hh>
21 #include <gazebo/physics/Shape.hh>
22 #include <gazebo/physics/World.hh>
23 #include <gazebo/transport/TransportTypes.hh>
24 #include <gazebo/transport/transport.hh>
25 
26 #include <ros/ros.h>
27 #include <geometry_msgs/Accel.h>
28 
31 
32 namespace gazebo {
33 
34 GZ_REGISTER_MODEL_PLUGIN(AccelerationsTestPlugin)
35 
36 AccelerationsTestPlugin::AccelerationsTestPlugin()
38 {
39 }
40 
43 {
44 #if GAZEBO_MAJOR_VERSION >= 8
45  this->updateConnection.reset();
46 #else
47  event::Events::DisconnectWorldUpdateBegin(this->updateConnection);
48 #endif
49 }
50 
52 void AccelerationsTestPlugin::Load(physics::ModelPtr _model,
53  sdf::ElementPtr _sdf)
54 {
55  GZ_ASSERT(_model != NULL, "Invalid model pointer");
56  GZ_ASSERT(_sdf != NULL, "Invalid SDF element pointer");
57 
58  this->model = _model;
59  this->world = _model->GetWorld();
60 
61  // Initialize the transport node
62  this->node = transport::NodePtr(new transport::Node());
63 #if GAZEBO_MAJOR_VERSION >= 8
64  this->node->Init(this->world->Name());
65 #else
66  this->node->Init(this->world->GetName());
67 #endif
68 
69  std::string link_name;
70  if (_sdf->HasElement("link_name"))
71  link_name = _sdf->GetElement("link_name")->Get<std::string>();
72  else
73  gzerr << "[TestPlugin] Please specify a link_name .\n";
74  this->link = this->model->GetLink(link_name);
75  if (this->link == NULL)
76  gzthrow("[TestPlugin] Could not find specified link \""
77  << link_name << "\".");
78 
79  // Connect the update event callback
80  this->Connect();
81 
82  // ROS:
83  if (!ros::isInitialized())
84  {
85  gzerr << "Not loading plugin since ROS has not been "
86  << "properly initialized. Try starting gazebo with ros plugin:\n"
87  << " gazebo -s libgazebo_ros_api_plugin.so\n";
88  return;
89  }
90 
91  this->rosNode.reset(new ros::NodeHandle(""));
92 
93  this->pub_accel_w_gazebo =
94  this->rosNode->advertise<geometry_msgs::Accel>("accel_w_gazebo", 10);
95  this->pub_accel_w_numeric =
96  this->rosNode->advertise<geometry_msgs::Accel>("accel_w_numeric", 10);
97 
98  this->pub_accel_b_gazebo =
99  this->rosNode->advertise<geometry_msgs::Accel>("accel_b_gazebo", 10);
100  this->pub_accel_b_numeric =
101  this->rosNode->advertise<geometry_msgs::Accel>("accel_b_numeric", 10);
102 }
103 
106 {
107  // Doing nothing for now
108 }
109 
110 geometry_msgs::Accel accelFromEigen(const Eigen::Vector6d& acc)
111 {
112  geometry_msgs::Accel amsg;
113  amsg.linear.x = acc[0];
114  amsg.linear.y = acc[1];
115  amsg.linear.z = acc[2];
116  amsg.angular.x = acc[3];
117  amsg.angular.y = acc[4];
118  amsg.angular.z = acc[5];
119  return amsg;
120 }
121 
122 Eigen::Matrix3d Matrix3ToEigen(const ignition::math::Matrix3d& m)
123 {
124  Eigen::Matrix3d r;
125  r << m(0, 0), m(0, 1), m(0, 2),
126  m(1, 0), m(1, 1), m(1, 2),
127  m(2, 0), m(2, 1), m(2, 1);
128  return r;
129 }
130 
132 void AccelerationsTestPlugin::Update(const common::UpdateInfo &_info)
133 {
134  double dt = (_info.simTime - lastTime).Double();
135 
136  // Link's pose
137  ignition::math::Pose3d pose_w_b;
138 #if GAZEBO_MAJOR_VERSION >= 8
139  pose_w_b = this->link->WorldPose();
140 #else
141  pose_w_b = this->link->GetWorldPose().Ign();
142 #endif
143 
144 
145 #if GAZEBO_MAJOR_VERSION >= 8
146 // Velocities of this link in link frame.
147 Eigen::Vector6d gazebo_b_v_w_b = EigenStack(
148  this->link->RelativeLinearVel(),
149  this->link->RelativeAngularVel());
150 
151 // Velocities of this link in world frame
152 Eigen::Vector6d gazebo_w_v_w_b = EigenStack(
153  this->link->WorldLinearVel(),
154  this->link->WorldAngularVel());
155 
156 
157 // Accelerations of this link in world frame
158 Eigen::Vector6d gazebo_w_a_w_b = EigenStack(
159  this->link->WorldLinearAccel(),
160  this->link->WorldAngularAccel());
161 
162 // Accelerations of this link in link frame
163 Eigen::Vector6d gazebo_b_a_w_b = EigenStack(
164  this->link->RelativeLinearAccel(),
165  this->link->RelativeAngularAccel());
166 #else
167  // Velocities of this link in link frame.
168  Eigen::Vector6d gazebo_b_v_w_b = EigenStack(
169  this->link->GetRelativeLinearVel().Ign(),
170  this->link->GetRelativeAngularVel().Ign());
171 
172  // Velocities of this link in world frame
173  Eigen::Vector6d gazebo_w_v_w_b = EigenStack(
174  this->link->GetWorldLinearVel().Ign(),
175  this->link->GetWorldAngularVel().Ign());
176 
177 
178  // Accelerations of this link in world frame
179  Eigen::Vector6d gazebo_w_a_w_b = EigenStack(
180  this->link->GetWorldLinearAccel().Ign(),
181  this->link->GetWorldAngularAccel().Ign());
182 
183  // Accelerations of this link in link frame
184  Eigen::Vector6d gazebo_b_a_w_b = EigenStack(
185  this->link->GetRelativeLinearAccel().Ign(),
186  this->link->GetRelativeAngularAccel().Ign());
187 #endif
188 
189  // Numerically computed accelerations
190  ignition::math::Matrix3d R_b_w = ignition::math::Matrix3d(pose_w_b.Rot().Inverse());
191 
192  Eigen::Matrix3d R_b_w_eigen = Matrix3ToEigen(R_b_w);
193  Eigen::Matrix6d R6_b_w_eigen;
194  R6_b_w_eigen << R_b_w_eigen, Eigen::Matrix3d::Zero(),
195  Eigen::Matrix3d::Zero(), R_b_w_eigen;
196 
197  // Actual numeric differentiation
198  Eigen::Vector6d num_w_a_w_b = (gazebo_w_v_w_b - last_w_v_w_b) / dt;
199  Eigen::Vector6d num_b_a_w_b = R6_b_w_eigen * num_w_a_w_b;
200 
201  // Publish all four variants via ROS for easy comparison
202  this->pub_accel_w_gazebo.publish(accelFromEigen(gazebo_w_a_w_b));
203  this->pub_accel_b_gazebo.publish(accelFromEigen(gazebo_b_a_w_b));
204 
205  this->pub_accel_w_numeric.publish(accelFromEigen(num_w_a_w_b));
206  this->pub_accel_b_numeric.publish(accelFromEigen(num_b_a_w_b));
207 
208  last_w_v_w_b = gazebo_w_v_w_b;
209  lastTime = _info.simTime;
210 }
211 
214 {
215  // Connect the update event
216  this->updateConnection = event::Events::ConnectWorldUpdateBegin(
217  boost::bind(&AccelerationsTestPlugin::Update,
218  this, _1));
219 }
220 }
void publish(const boost::shared_ptr< M > &message) const
gazebo::event::ConnectionPtr updateConnection
Update event.
gazebo::physics::WorldPtr world
Pointer to the world plugin.
ROSCPP_DECL bool isInitialized()
GZ_REGISTER_MODEL_PLUGIN(UmbilicalPlugin)
common::Time lastTime
Time stamp of previous time step.
Eigen::Matrix3d Matrix3ToEigen(const ignition::math::Matrix3d &m)
gazebo::physics::ModelPtr model
Pointer to the model structure.
Eigen::Matrix< double, 6, 1 > Vector6d
virtual void Connect()
Connects the update event callback.
virtual ~AccelerationsTestPlugin()
Destructor.
geometry_msgs::Accel accelFromEigen(const Eigen::Vector6d &acc)
void Update(const gazebo::common::UpdateInfo &_info)
Update the simulation state.
boost::scoped_ptr< ros::NodeHandle > rosNode
Eigen::Vector6d EigenStack(const ignition::math::Vector3d &_x, const ignition::math::Vector3d &_y)
Eigen::Matrix< double, 6, 6 > Matrix6d
physics::LinkPtr link
Link of test object.
Eigen::Vector6d last_w_v_w_b
Velocity of link with respect to world frame in previous time step.
virtual void Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr _sdf)
gazebo::transport::NodePtr node
Gazebo node.


uuv_gazebo_ros_plugins
Author(s): Musa Morena Marcusso Manhaes , Sebastian Scherer , Luiz Ricardo Douat
autogenerated on Thu Jun 18 2020 03:28:28