Skip to content

Commit 2c94eca

Browse files
jprobergejproberge
authored
Merge pull request ros-industrial-attic#133 from christian-rauch/melodic
Melodic port
2 parents 58217ae + 61b8a9e commit 2c94eca

File tree

4 files changed

+84
-12
lines changed

4 files changed

+84
-12
lines changed

‎robotiq_3f_gripper_articulated_gazebo_plugins/src/RobotiqHandPlugin.cpp‎

Lines changed: 74 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -34,7 +34,6 @@ I'm not sure exactly where the dependency chain includes PID.hh for the first ti
3434
#define private public
3535
#include <gazebo/common/Plugin.hh>
3636
#include <gazebo/common/Time.hh>
37-
#include <gazebo/math/Angle.hh>
3837
#include <gazebo/physics/physics.hh>
3938
#include <robotiq_3f_gripper_articulated_gazebo_plugins/RobotiqHandPlugin.h>
4039
#undef private
@@ -69,7 +68,9 @@ RobotiqHandPlugin::RobotiqHandPlugin()
6968
////////////////////////////////////////////////////////////////////////////////
7069
RobotiqHandPlugin::~RobotiqHandPlugin()
7170
{
71+
#if GAZEBO_MAJOR_VERSION < 9
7272
gazebo::event::Events::DisconnectWorldUpdateBegin(this->updateConnection);
73+
#endif
7374
this->rosNode->shutdown();
7475
this->rosQueue.clear();
7576
this->rosQueue.disable();
@@ -204,7 +205,11 @@ void RobotiqHandPlugin::Load(gazebo::physics::ModelPtr _parent,
204205
this->subHandleCommand = this->rosNode->subscribe(handleCommandSo);
205206

206207
// Controller time control.
208+
#if GAZEBO_MAJOR_VERSION >= 9
209+
this->lastControllerUpdateTime = this->world->SimTime();
210+
#else
207211
this->lastControllerUpdateTime = this->world->GetSimTime();
212+
#endif
208213

209214
// Start callback queue.
210215
this->callbackQueueThread =
@@ -322,14 +327,22 @@ bool RobotiqHandPlugin::IsHandFullyOpen()
322327

323328
// The hand will be fully open when all the fingers are within 'tolerance'
324329
// from their lower limits.
330+
#if GAZEBO_MAJOR_VERSION >= 9
331+
ignition::math::Angle tolerance;
332+
tolerance.Degree(1.0);
333+
#else
325334
gazebo::math::Angle tolerance;
326335
tolerance.SetFromDegree(1.0);
336+
#endif
327337

328338
for (int i = 2; i < this->NumJoints; ++i)
329339
{
330340
fingersOpen = fingersOpen &&
331-
(this->joints[i]->GetAngle(0) <
332-
(this->joints[i]->GetLowerLimit(0) + tolerance));
341+
#if GAZEBO_MAJOR_VERSION >= 9
342+
(this->joints[i]->Position(0) < (this->joints[i]->LowerLimit(0) + tolerance()));
343+
#else
344+
(this->joints[i]->GetAngle(0) < (this->joints[i]->GetLowerLimit(0) + tolerance));
345+
#endif
333346
}
334347

335348
return fingersOpen;
@@ -339,8 +352,11 @@ bool RobotiqHandPlugin::IsHandFullyOpen()
339352
void RobotiqHandPlugin::UpdateStates()
340353
{
341354
boost::mutex::scoped_lock lock(this->controlMutex);
342-
355+
#if GAZEBO_MAJOR_VERSION >= 9
356+
gazebo::common::Time curTime = this->world->SimTime();
357+
#else
343358
gazebo::common::Time curTime = this->world->GetSimTime();
359+
#endif
344360

345361
// Step 1: State transitions.
346362
if (curTime > this->lastControllerUpdateTime)
@@ -507,18 +523,28 @@ uint8_t RobotiqHandPlugin::GetCurrentPosition(
507523
const gazebo::physics::JointPtr &_joint)
508524
{
509525
// Full range of motion.
510-
gazebo::math::Angle range =
511-
_joint->GetUpperLimit(0) - _joint->GetLowerLimit(0);
526+
#if GAZEBO_MAJOR_VERSION >= 9
527+
ignition::math::Angle range = _joint->UpperLimit(0) - _joint->LowerLimit(0);
528+
#else
529+
gazebo::math::Angle range = _joint->GetUpperLimit(0) - _joint->GetLowerLimit(0);
530+
#endif
512531

513532
// The maximum value in pinch mode is 177.
514533
if (this->graspingMode == Pinch)
515534
range *= 177.0 / 255.0;
516535

517536
// Angle relative to the lower limit.
537+
#if GAZEBO_MAJOR_VERSION >= 9
538+
ignition::math::Angle relAngle = _joint->Position(0) - _joint->LowerLimit(0);
539+
#else
518540
gazebo::math::Angle relAngle = _joint->GetAngle(0) - _joint->GetLowerLimit(0);
541+
#endif
519542

520-
return
521-
static_cast<uint8_t>(round(255.0 * relAngle.Radian() / range.Radian()));
543+
#if GAZEBO_MAJOR_VERSION >= 9
544+
return static_cast<uint8_t>(round(255.0 * relAngle() / range()));
545+
#else
546+
static_cast<uint8_t>(round(255.0 * relAngle.Radian() / range.Radian()));
547+
#endif
522548
}
523549

524550
////////////////////////////////////////////////////////////////////////////////
@@ -645,7 +671,11 @@ void RobotiqHandPlugin::GetAndPublishJointState(
645671
this->jointStates.header.stamp = ros::Time(_curTime.sec, _curTime.nsec);
646672
for (size_t i = 0; i < this->joints.size(); ++i)
647673
{
674+
#if GAZEBO_MAJOR_VERSION >= 9
675+
this->jointStates.position[i] = this->joints[i]->Position(0);
676+
#else
648677
this->jointStates.position[i] = this->joints[i]->GetAngle(0).Radian();
678+
#endif
649679
this->jointStates.velocity[i] = this->joints[i]->GetVelocity(0);
650680
// better to use GetForceTorque dot joint axis
651681
this->jointStates.effort[i] = this->joints[i]->GetForce(0u);
@@ -674,7 +704,11 @@ void RobotiqHandPlugin::UpdatePIDControl(double _dt)
674704
switch (this->graspingMode)
675705
{
676706
case Wide:
707+
#if GAZEBO_MAJOR_VERSION >= 9
708+
targetPose = this->joints[i]->UpperLimit(0);
709+
#else
677710
targetPose = this->joints[i]->GetUpperLimit(0).Radian();
711+
#endif
678712
break;
679713

680714
case Pinch:
@@ -684,9 +718,15 @@ void RobotiqHandPlugin::UpdatePIDControl(double _dt)
684718

685719
case Scissor:
686720
// Max position is reached at value 215.
721+
#if GAZEBO_MAJOR_VERSION >= 9
722+
targetPose = this->joints[i]->UpperLimit(0) -
723+
(this->joints[i]->UpperLimit(0) -
724+
this->joints[i]->LowerLimit(0)) * (215.0 / 255.0)
725+
#else
687726
targetPose = this->joints[i]->GetUpperLimit(0).Radian() -
688727
(this->joints[i]->GetUpperLimit(0).Radian() -
689728
this->joints[i]->GetLowerLimit(0).Radian()) * (215.0 / 255.0)
729+
#endif
690730
* this->handleCommand.rPRA / 255.0;
691731
break;
692732
}
@@ -696,7 +736,11 @@ void RobotiqHandPlugin::UpdatePIDControl(double _dt)
696736
switch (this->graspingMode)
697737
{
698738
case Wide:
739+
#if GAZEBO_MAJOR_VERSION >= 9
740+
targetPose = this->joints[i]->LowerLimit(0);
741+
#else
699742
targetPose = this->joints[i]->GetLowerLimit(0).Radian();
743+
#endif
700744
break;
701745

702746
case Pinch:
@@ -706,9 +750,15 @@ void RobotiqHandPlugin::UpdatePIDControl(double _dt)
706750

707751
case Scissor:
708752
// Max position is reached at value 215.
753+
#if GAZEBO_MAJOR_VERSION >= 9
754+
targetPose = this->joints[i]->LowerLimit(0) +
755+
(this->joints[i]->UpperLimit(0) -
756+
this->joints[i]->LowerLimit(0)) * (215.0 / 255.0)
757+
#else
709758
targetPose = this->joints[i]->GetLowerLimit(0).Radian() +
710759
(this->joints[i]->GetUpperLimit(0).Radian() -
711760
this->joints[i]->GetLowerLimit(0).Radian()) * (215.0 / 255.0)
761+
#endif
712762
* this->handleCommand.rPRA / 255.0;
713763
break;
714764
}
@@ -718,9 +768,15 @@ void RobotiqHandPlugin::UpdatePIDControl(double _dt)
718768
if (this->graspingMode == Pinch)
719769
{
720770
// Max position is reached at value 177.
771+
#if GAZEBO_MAJOR_VERSION >= 9
772+
targetPose = this->joints[i]->LowerLimit(0) +
773+
(this->joints[i]->UpperLimit(0) -
774+
this->joints[i]->LowerLimit(0)) * (177.0 / 255.0)
775+
#else
721776
targetPose = this->joints[i]->GetLowerLimit(0).Radian() +
722777
(this->joints[i]->GetUpperLimit(0).Radian() -
723778
this->joints[i]->GetLowerLimit(0).Radian()) * (177.0 / 255.0)
779+
#endif
724780
* this->handleCommand.rPRA / 255.0;
725781
}
726782
else if (this->graspingMode == Scissor)
@@ -731,15 +787,25 @@ void RobotiqHandPlugin::UpdatePIDControl(double _dt)
731787
}
732788
else
733789
{
790+
#if GAZEBO_MAJOR_VERSION >= 9
791+
targetPose = this->joints[i]->LowerLimit(0) +
792+
(this->joints[i]->UpperLimit(0) -
793+
this->joints[i]->LowerLimit(0))
794+
#else
734795
targetPose = this->joints[i]->GetLowerLimit(0).Radian() +
735796
(this->joints[i]->GetUpperLimit(0).Radian() -
736797
this->joints[i]->GetLowerLimit(0).Radian())
798+
#endif
737799
* this->handleCommand.rPRA / 255.0;
738800
}
739801
}
740802

741803
// Get the current pose.
804+
#if GAZEBO_MAJOR_VERSION >= 9
805+
double currentPose = this->joints[i]->Position(0);
806+
#else
742807
double currentPose = this->joints[i]->GetAngle(0).Radian();
808+
#endif
743809

744810
// Position error.
745811
double poseError = currentPose - targetPose;

‎robotiq_3f_gripper_control/include/robotiq_3f_gripper_control/robotiq_3f_gripper_can_client.h‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -76,8 +76,8 @@ class Robotiq3FGripperCanClient : public Robotiq3FGripperClientBase
7676
GripperInput input_;
7777
GripperOutput output_;
7878

79-
can::CommInterface::FrameListener::Ptr frame_listener_;
80-
can::StateInterface::StateListener::Ptr state_listener_;
79+
can::CommInterface::FrameListenerConstSharedPtr frame_listener_;
80+
can::StateInterface::StateListenerConstSharedPtr state_listener_;
8181

8282
void frameCallback(const can::Frame &f);
8383
void stateCallback(const can::State &s);

‎robotiq_3f_gripper_control/src/robotiq_3f_gripper_control/robotiq_3f_gripper_can_client.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -65,7 +65,7 @@ void Robotiq3FGripperCanClient::writeOutputs(const GripperOutput &output)
6565
serial_request.is_error = 0;
6666
serial_request.is_rtr = 0;
6767
serial_request.is_extended = 0;
68-
serial_request.data.assign((unsigned char)0);
68+
serial_request.data.fill((unsigned char)0);
6969

7070
serial_request.data[0] = 0x2F;
7171
serial_request.data[1] = 0x00;
@@ -218,7 +218,7 @@ Robotiq3FGripperCanClient::GripperInput Robotiq3FGripperCanClient::readInputs()
218218
serial_request.is_error = 0;
219219
serial_request.is_rtr = 0;
220220
serial_request.is_extended = 0;
221-
serial_request.data.assign(static_cast<unsigned char>(0));
221+
serial_request.data.fill(static_cast<unsigned char>(0));
222222

223223
serial_request.data[0] = 0x4F;
224224
serial_request.data[1] = 0x00;

‎robotiq_3f_gripper_joint_state_publisher/CMakeLists.txt‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -41,3 +41,9 @@ target_link_libraries(robotiq_3f_gripper_joint_states
4141
${catkin_LIBRARIES}
4242
)
4343

44+
install(TARGETS robotiq_3f_gripper_joint_states
45+
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
46+
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
47+
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
48+
)
49+

0 commit comments

Comments
 (0)