Skip to content

Commit be9ed40

Browse files
committed
Adapt code to new Gazebo 9 API
1 parent 5aa2391 commit be9ed40

File tree

1 file changed

+10
-8
lines changed

1 file changed

+10
-8
lines changed

src/gazebo_ros_link_attacher.cpp

Lines changed: 10 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@
44
#include "gazebo_ros_link_attacher/Attach.h"
55
#include "gazebo_ros_link_attacher/AttachRequest.h"
66
#include "gazebo_ros_link_attacher/AttachResponse.h"
7+
#include <ignition/math/Pose3.hh>
78

89
namespace gazebo
910
{
@@ -33,7 +34,7 @@ namespace gazebo
3334
}
3435

3536
this->world = _world;
36-
this->physics = this->world->GetPhysicsEngine();
37+
this->physics = this->world->Physics();
3738
this->attach_service_ = this->nh_.advertiseService("attach", &GazeboRosLinkAttacher::attach_callback, this);
3839
ROS_INFO_STREAM("Attach service at: " << this->nh_.resolveName("attach"));
3940
this->detach_service_ = this->nh_.advertiseService("detach", &GazeboRosLinkAttacher::detach_callback, this);
@@ -63,13 +64,14 @@ namespace gazebo
6364
j.model2 = model2;
6465
j.link2 = link2;
6566
ROS_DEBUG_STREAM("Getting BasePtr of " << model1);
66-
physics::BasePtr b1 = this->world->GetByName(model1);
67+
physics::BasePtr b1 = this->world->ModelByName(model1);
68+
6769
if (b1 == NULL){
6870
ROS_ERROR_STREAM(model1 << " model was not found");
6971
return false;
7072
}
7173
ROS_DEBUG_STREAM("Getting BasePtr of " << model2);
72-
physics::BasePtr b2 = this->world->GetByName(model2);
74+
physics::BasePtr b2 = this->world->ModelByName(model2);
7375
if (b2 == NULL){
7476
ROS_ERROR_STREAM(model2 << " model was not found");
7577
return false;
@@ -91,7 +93,7 @@ namespace gazebo
9193
ROS_ERROR_STREAM("link1 inertia is NULL!");
9294
}
9395
else
94-
ROS_DEBUG_STREAM("link1 inertia is not NULL, for example, mass is: " << l1->GetInertial()->GetMass());
96+
ROS_DEBUG_STREAM("link1 inertia is not NULL, for example, mass is: " << l1->GetInertial()->Mass());
9597
j.l1 = l1;
9698
ROS_DEBUG_STREAM("Getting link: '" << link2 << "' from model: '" << model2 << "'");
9799
physics::LinkPtr l2 = m2->GetLink(link2);
@@ -103,7 +105,7 @@ namespace gazebo
103105
ROS_ERROR_STREAM("link2 inertia is NULL!");
104106
}
105107
else
106-
ROS_DEBUG_STREAM("link2 inertia is not NULL, for example, mass is: " << l2->GetInertial()->GetMass());
108+
ROS_DEBUG_STREAM("link2 inertia is not NULL, for example, mass is: " << l2->GetInertial()->Mass());
107109
j.l2 = l2;
108110

109111
ROS_DEBUG_STREAM("Links are: " << l1->GetName() << " and " << l2->GetName());
@@ -115,7 +117,7 @@ namespace gazebo
115117
ROS_DEBUG_STREAM("Attach");
116118
j.joint->Attach(l1, l2);
117119
ROS_DEBUG_STREAM("Loading links");
118-
j.joint->Load(l1, l2, math::Pose());
120+
j.joint->Load(l1, l2, ignition::math::Pose3d());
119121
ROS_DEBUG_STREAM("SetModel");
120122
j.joint->SetModel(m2);
121123
/*
@@ -133,9 +135,9 @@ namespace gazebo
133135
*/
134136

135137
ROS_DEBUG_STREAM("SetHightstop");
136-
j.joint->SetHighStop(0, 0);
138+
j.joint->SetUpperLimit(0, 0);
137139
ROS_DEBUG_STREAM("SetLowStop");
138-
j.joint->SetLowStop(0, 0);
140+
j.joint->SetLowerLimit(0, 0);
139141
ROS_DEBUG_STREAM("Init");
140142
j.joint->Init();
141143
ROS_INFO_STREAM("Attach finished.");

0 commit comments

Comments
 (0)