Skip to content

Feature: Loads controller for Gazebo and allows to use prefixes. #180

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 1 commit into
base: kinetic-devel
Choose a base branch
from
Open

Feature: Loads controller for Gazebo and allows to use prefixes. #180

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
26 changes: 10 additions & 16 deletions ..._gazebo_plugins/include/robotiq_3f_gripper_articulated_gazebo_plugins/RobotiqHandPlugin.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,8 +37,8 @@

/// \brief A plugin that implements the Robotiq 3-Finger Adaptative Gripper.
/// The plugin exposes the next parameters via SDF tags:
/// * <side> Determines if we are controlling the left or right hand. This is
/// a required parameter and the allowed values are 'left' or 'right'
/// * <prefix> Defines a prefix that is used for the joints and the topics.
/// This parameter is optional.
/// * <kp_position> P gain for the PID that controls the position
/// of the joints. This parameter is optional.
/// * <ki_position> I gain for the PID that controls the position
Expand Down Expand Up @@ -186,18 +186,6 @@ class RobotiqHandPlugin : public gazebo::ModelPlugin
/// \brief Max. joint speed (rad/s). Finger is 125mm and tip speed is 110mm/s.
private: static constexpr double MaxVelocity = 0.88;

/// \brief Default topic name for sending control updates to the left hand.
private: static const std::string DefaultLeftTopicCommand;

/// \brief Default topic name for receiving state updates from the left hand.
private: static const std::string DefaultLeftTopicState;

/// \brief Default topic name for sending control updates to the right hand.
private: static const std::string DefaultRightTopicCommand;

/// \brief Default topic name for receiving state updates from the right hand.
private: static const std::string DefaultRightTopicState;

/// \brief ROS NodeHandle.
private: boost::scoped_ptr<ros::NodeHandle> rosNode;

Expand Down Expand Up @@ -271,8 +259,14 @@ class RobotiqHandPlugin : public gazebo::ModelPlugin
/// \brief Pointer to the SDF of this plugin.
private: sdf::ElementPtr sdf;

/// \brief Used to select between 'left' or 'right' hand.
private: std::string side;
/// \brief Used to select a prefix for the hand.
private: std::string prefix;

/// \brief Topic name for sending control updates to the hand.
private: std::string topicCommand;

/// \brief Topic name for receiving state updates from the hand.
private: std::string topicState;

/// \brief Vector containing all the joint names.
private: std::vector<std::string> jointNames;
Expand Down
67 changes: 17 additions & 50 deletions robotiq_3f_gripper_articulated_gazebo_plugins/src/RobotiqHandPlugin.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,15 +38,6 @@ I'm not sure exactly where the dependency chain includes PID.hh for the first ti
#include <robotiq_3f_gripper_articulated_gazebo_plugins/RobotiqHandPlugin.h>
#undef private

// Default topic names initialization.
const std::string RobotiqHandPlugin::DefaultLeftTopicCommand =
"/left_hand/command";
const std::string RobotiqHandPlugin::DefaultLeftTopicState =
"/left_hand/state";
const std::string RobotiqHandPlugin::DefaultRightTopicCommand =
"/right_hand/command";
const std::string RobotiqHandPlugin::DefaultRightTopicState =
"/right_hand/state";

////////////////////////////////////////////////////////////////////////////////
RobotiqHandPlugin::RobotiqHandPlugin()
Expand Down Expand Up @@ -85,22 +76,10 @@ void RobotiqHandPlugin::Load(gazebo::physics::ModelPtr _parent,
this->world = this->model->GetWorld();
this->sdf = _sdf;

if (!this->sdf->HasElement("side") ||
!this->sdf->GetElement("side")->GetValue()->Get(this->side) ||
((this->side != "left") && (this->side != "right")))
{
gzerr << "Failed to determine which hand we're controlling; "
"aborting plugin load. <Side> should be either 'left' or 'right'."
<< std::endl;
return;
if (this->sdf->HasElement("prefix")) {
this->sdf->GetElement("prefix")->GetValue()->Get(this->prefix);
}

// Load the vector of all joints.
std::string prefix;
if (this->side == "left")
prefix = "l_";
else
prefix = "r_";
gzlog << "Launching with prefix '" << this->prefix << "'" << std::endl;

// Load the vector of all joints.
if (!this->FindJoints())
Expand All @@ -122,15 +101,10 @@ void RobotiqHandPlugin::Load(gazebo::physics::ModelPtr _parent,
}
gzlog << "Initialized the joint state vector" << std::endl;

// Default ROS topic names.
std::string controlTopicName = this->DefaultLeftTopicCommand;
std::string stateTopicName = this->DefaultLeftTopicState;
if (this->side == "right")
{
controlTopicName = this->DefaultRightTopicCommand;
stateTopicName = this->DefaultRightTopicState;
}
gzlog << "Using control topic " << controlTopicName << std::endl;
// ROS topic names.
this->topicCommand = prefix + std::string("hand/command");
this->topicState = prefix + std::string("hand/state");
gzlog << "Using control topic " << this->topicCommand << std::endl;

for (int i = 0; i < this->NumJoints; ++i)
{
Expand Down Expand Up @@ -161,10 +135,10 @@ void RobotiqHandPlugin::Load(gazebo::physics::ModelPtr _parent,

// Overload the ROS topics for the hand if they are available.
if (this->sdf->HasElement("topic_command"))
controlTopicName = this->sdf->Get<std::string>("topic_command");
this->topicCommand = this->sdf->Get<std::string>("topic_command");

if (this->sdf->HasElement("topic_state"))
stateTopicName = this->sdf->Get<std::string>("topic_state");
this->topicState = this->sdf->Get<std::string>("topic_state");

// Initialize ROS.
if (!ros::isInitialized())
Expand All @@ -184,18 +158,17 @@ void RobotiqHandPlugin::Load(gazebo::physics::ModelPtr _parent,
// Broadcasts state.
this->pubHandleStateQueue = this->pmq.addPub<robotiq_3f_gripper_articulated_msgs::Robotiq3FGripperRobotInput>();
this->pubHandleState = this->rosNode->advertise<robotiq_3f_gripper_articulated_msgs::Robotiq3FGripperRobotInput>(
stateTopicName, 100, true);
this->topicState, 100, true);

// Broadcast joint state.
std::string topicBase = std::string("robotiq_hands/") + this->side;
this->pubJointStatesQueue = this->pmq.addPub<sensor_msgs::JointState>();
this->pubJointStates = this->rosNode->advertise<sensor_msgs::JointState>(
topicBase + std::string("_hand/joint_states"), 10);
this->prefix + std::string("hand/joint_states"), 10);

// Subscribe to user published handle control commands.
ros::SubscribeOptions handleCommandSo =
ros::SubscribeOptions::create<robotiq_3f_gripper_articulated_msgs::Robotiq3FGripperRobotOutput>(
controlTopicName, 100,
this->topicCommand, 100,
boost::bind(&RobotiqHandPlugin::SetHandleCommand, this, _1),
ros::VoidPtr(), &this->rosQueue);

Expand All @@ -221,8 +194,7 @@ void RobotiqHandPlugin::Load(gazebo::physics::ModelPtr _parent,
boost::bind(&RobotiqHandPlugin::UpdateStates, this));

// Log information.
gzlog << "RobotiqHandPlugin loaded for " << this->side << " hand."
<< std::endl;
gzlog << "RobotiqHandPlugin loaded with prefix " << this->prefix << std::endl;
for (int i = 0; i < this->NumJoints; ++i)
{
gzlog << "Position PID parameters for joint ["
Expand All @@ -236,8 +208,8 @@ void RobotiqHandPlugin::Load(gazebo::physics::ModelPtr _parent,
<< "\tCmdMax: " << this->posePID[i].cmdMax << std::endl
<< std::endl;
}
gzlog << "Topic for sending hand commands: [" << controlTopicName
<< "]\nTopic for receiving hand state: [" << stateTopicName
gzlog << "Topic for sending hand commands: [" << this->topicCommand
<< "]\nTopic for receiving hand state: [" << this->topicState
<< "]" << std::endl;
}

Expand Down Expand Up @@ -840,12 +812,8 @@ bool RobotiqHandPlugin::FindJoints()
{
// Load up the joints we expect to use, finger by finger.
gazebo::physics::JointPtr joint;
std::string prefix;
std::string prefix = this->prefix;
std::string suffix;
if (this->side == "left")
prefix = "l_";
else
prefix = "r_";

// palm_finger_1_joint (actuated).
suffix = "palm_finger_1_joint";
Expand Down Expand Up @@ -935,8 +903,7 @@ bool RobotiqHandPlugin::FindJoints()
return false;
this->jointNames.push_back(prefix + suffix);

gzlog << "RobotiqHandPlugin found all joints for " << this->side
<< " hand." << std::endl;
gzlog << "RobotiqHandPlugin found all joints for hand with prefix " << this->prefix << std::endl;
return true;
}

Expand Down
29 changes: 29 additions & 0 deletions robotiq_3f_gripper_visualization/cfg/robotiq-3f-gripper_actuated_gazebo.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,29 @@
<?xml version="1.0" ?>

<!--
robotiq-3f-gripper_actuated_gazebo - actuated version of the robotiq robotiq-3f-gripper, 3 fingered gripper.
-->
<robot name="robotiq-3f-gripper_actuated_gazebo" xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:include filename="$(find robotiq_3f_gripper_visualization)/cfg/robotiq_hand_macro.urdf.xacro" />

<link name="world"/>

<robotiq_hand prefix="" parent="world" reflect="">
<origin xyz="0 0 0.5" rpy="0 0 0"/>
</robotiq_hand>

<gazebo>
<!-- plugin for the RobotiQ hand -->
<plugin name="robotiq_hand_plugin" filename="libRobotiqHandPlugin.so">
<kp_position>10.0</kp_position>
<kd_position>0.5</kd_position>
<!-- Some optional parameters. Check the plugin header file for all of them. -->
<!--<prefix>pre_</prefix>-->
<!-- This is chosen, so it's compatible with the "Robotiq3FGripperSimpleController.py"-->
<topic_command>/Robotiq3FGripperRobotOutput</topic_command>
<!-- <topic_state>/hand/state</topic_state> -->

</plugin>
</gazebo>
</robot>

2 changes: 1 addition & 1 deletion robotiq_3f_gripper_visualization/launch/robotiq_gripper_upload.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
<?xml version="1.0"?>
<launch>
<param name="robot_description" command="$(find xacro)/xacro.py '$(find robotiq_3f_gripper_visualization)/cfg/robotiq-3f-gripper_articulated.xacro'" />
<param name="robot_description" command="$(find xacro)/xacro.py '$(find robotiq_3f_gripper_visualization)/cfg/robotiq-3f-gripper_actuated_gazebo.xacro'" />
</launch>