Skip to content

Add gripper availabilty check #13

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
frederikhegger wants to merge 6 commits into
base: indigo-devel
Choose a base branch
from
Open

Add gripper availabilty check #13

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
3f050e8
add a dynamic check if a gripper is available
May 8, 2015
ab9a44c
use a parameter to set the cycle rate for getting gripper readings
May 8, 2015
8474c5c
add missing closing of param tag
May 8, 2015
c24ce56
do not publish empty joint state fields if no gripper is attached
May 27, 2015
a8b4c61
change debug output level from error to warn
May 29, 2015
d0d49a4
Merge branch 'indigo-devel' of https://github.com/youbot/youbot_drive…
Jun 10, 2016
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
10 changes: 6 additions & 4 deletions include/youbot_driver_ros_interface/YouBotOODLWrapper.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -108,7 +108,7 @@ class YouBotOODLWrapper
* @param armName Name of the base. Used to open the configuration file e.g. youbot-manipulator.cfg
* @param enableStandardGripper If set to true, then the default gripper of the youBot will be initialized.
*/
void initializeArm(std::string armName, bool enableStandardGripper = true);
void initializeArm(std::string armName);

/**
* @brief Stops all initialized elements.
Expand Down Expand Up @@ -256,16 +256,16 @@ class YouBotOODLWrapper
/// Tell if a goal is currently active.
bool armHasActiveJointTrajectoryGoal;

youbot::GripperSensedBarPosition gripperBar1Position;
youbot::GripperSensedBarPosition gripperBar2Position;
int gripperCycleCounter;
youbot::GripperSensedBarPosition gripperBar1Position;
youbot::GripperSensedBarPosition gripperBar2Position;

//void executeActionServer(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, int armIndex);

//bool trajectoryActionServerEnable;
//double trajectoryVelocityGain;
//double trajectoryPositionGain;
double youBotDriverCycleFrequencyInHz;
double youBotDriverGripperReadingsCycleFrequencyInHz;

/// diagnostic msgs
ros::Time lastDiagnosticPublishTime;
Expand All @@ -281,6 +281,8 @@ class YouBotOODLWrapper

bool areBaseMotorsSwitchedOn;
bool areArmMotorsSwitchedOn;

ros::Time last_gripper_readings_time_;
};

} // namespace youBot
Expand Down
1 change: 1 addition & 0 deletions launch/youbot_driver.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@ or enable the USE_SETCAP flag in the cmake file and recompile again.
<param name="youBotHasBase" type="bool" value="true"/>
<param name="youBotHasArms" type="bool" value="true"/>
<param name="youBotDriverCycleFrequencyInHz" type="double" value="50.0"/>
<param name="youBotDriverGripperReadingsCycleFrequencyInHz" type="double" value="5" />
<param name="youBotConfigurationFilePath" type="string" value="$(find youbot_driver)/config"/>

<param name="trajectoryActionServerEnable" type="bool" value="true"/>
Expand Down
1 change: 1 addition & 0 deletions launch/youbot_driver_dual_arm.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@ or enable the USE_SETCAP flag in the cmake file and recompile again.
<param name="youBotHasBase" type="bool" value="true"/>
<param name="youBotHasArms" type="bool" value="true"/>
<param name="youBotDriverCycleFrequencyInHz" type="double" value="50.0"/>
<param name="youBotDriverGripperReadingsCycleFrequencyInHz" type="double" value="5" />
<param name="youBotConfigurationFilePath" type="string" value="$(find youbot_driver)/config"/>

<param name="trajectoryActionServerEnable" type="bool" value="true"/>
Expand Down
110 changes: 64 additions & 46 deletions src/YouBotOODLWrapper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -63,14 +63,16 @@ node(n)
armJointStateMessages.clear();

n.param("youBotDriverCycleFrequencyInHz", youBotDriverCycleFrequencyInHz, 50.0);
n.param("youBotDriverGripperReadingsCycleFrequencyInHz", youBotDriverGripperReadingsCycleFrequencyInHz, 5.0);
//n.param("trajectoryActionServerEnable", trajectoryActionServerEnable, false);
//n.param("trajectoryVelocityGain", trajectoryVelocityGain, 0.0);
//n.param("trajectoryPositionGain", trajectoryPositionGain, 5.0);
gripperCycleCounter = 0;
diagnosticNameArm = "platform_Arm";
diagnosticNameBase = "platform_Base";
dashboardMessagePublisher = n.advertise<pr2_msgs::PowerBoardState>("/dashboard/platform_state", 1);
diagnosticArrayPublisher = n.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);

last_gripper_readings_time_ = ros::Time::now();
}

YouBotOODLWrapper::~YouBotOODLWrapper()
Expand Down Expand Up @@ -115,7 +117,7 @@ void YouBotOODLWrapper::initializeBase(std::string baseName)
areBaseMotorsSwitchedOn = true;
}

void YouBotOODLWrapper::initializeArm(std::string armName, bool enableStandardGripper)
void YouBotOODLWrapper::initializeArm(std::string armName)
{
int armIndex;
youbot::JointName jointNameParameter;
Expand Down Expand Up @@ -150,10 +152,23 @@ void YouBotOODLWrapper::initializeArm(std::string armName, bool enableStandardGr

youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->doJointCommutation();
youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->calibrateManipulator();
if (enableStandardGripper)
}
catch (std::exception& e)
{
youBotConfiguration.youBotArmConfigurations.pop_back();
std::string errorMessage = e.what();
ROS_FATAL("%s", errorMessage.c_str());
ROS_ERROR("Arm \"%s\" could not be initialized.", armName.c_str());
ROS_INFO("System has %i initialized arm(s).", static_cast<int> (youBotConfiguration.youBotArmConfigurations.size()));
return;
}

if(youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->hasGripper())
{
try
{
youbot::GripperBarName barName;
std::string gripperBarName;
youbot::GripperBarName barName;
std::string gripperBarName;

youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->getArmGripper().getGripperBar1().getConfigurationParameter(barName);
barName.getParameter(gripperBarName);
Expand All @@ -167,18 +182,18 @@ void YouBotOODLWrapper::initializeArm(std::string armName, bool enableStandardGr

youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->calibrateGripper();
}
catch (std::exception& e)
{
ROS_WARN_STREAM("Gripper on arm \"" << armName << "\" could not be initialized: " << e.what());
}
}
catch (std::exception& e)
else
{
youBotConfiguration.youBotArmConfigurations.pop_back();
std::string errorMessage = e.what();
ROS_FATAL("%s", errorMessage.c_str());
ROS_ERROR("Arm \"%s\" could not be initialized.", armName.c_str());
ROS_INFO("System has %i initialized arm(s).", static_cast<int> (youBotConfiguration.youBotArmConfigurations.size()));
return;
ROS_WARN_STREAM("Gripper on arm \"" << armName << "\" not found or disabled in the config file!");
}



/* setup input/output communication */
topicName.str("");
topicName << youBotConfiguration.youBotArmConfigurations[armIndex].commandTopicName << "arm_controller/position_command"; // e.g. arm_1/arm_controller/positionCommand
Expand All @@ -201,15 +216,10 @@ void YouBotOODLWrapper::initializeArm(std::string armName, bool enableStandardGr
topicName << youBotConfiguration.youBotArmConfigurations[armIndex].commandTopicName << "joint_states";
youBotConfiguration.youBotArmConfigurations[armIndex].armJointStatePublisher = node.advertise<sensor_msgs::JointState > (topicName.str(), 1); //TODO different names or one topic?

if (enableStandardGripper)
{
topicName.str("");
topicName << youBotConfiguration.youBotArmConfigurations[armIndex].commandTopicName << "gripper_controller/position_command";
youBotConfiguration.youBotArmConfigurations[armIndex].gripperPositionCommandSubscriber = node.subscribe<brics_actuator::JointPositions > (topicName.str(), 1000, boost::bind(&YouBotOODLWrapper::gripperPositionsCommandCallback, this, _1, armIndex));
youBotConfiguration.youBotArmConfigurations[armIndex].lastGripperCommand = 0.0; //This is true if the gripper is calibrated.
}


topicName.str("");
topicName << youBotConfiguration.youBotArmConfigurations[armIndex].commandTopicName << "gripper_controller/position_command";
youBotConfiguration.youBotArmConfigurations[armIndex].gripperPositionCommandSubscriber = node.subscribe<brics_actuator::JointPositions > (topicName.str(), 1000, boost::bind(&YouBotOODLWrapper::gripperPositionsCommandCallback, this, _1, armIndex));
youBotConfiguration.youBotArmConfigurations[armIndex].lastGripperCommand = 0.0; //This is true if the gripper is calibrated.

/* setup services*/
serviceName.str("");
Expand Down Expand Up @@ -811,9 +821,16 @@ void YouBotOODLWrapper::computeOODLSensorReadings()

/* fill joint state message */
armJointStateMessages[armIndex].header.stamp = currentTime;
armJointStateMessages[armIndex].name.resize(youBotArmDoF + youBotNumberOfFingers);
armJointStateMessages[armIndex].position.resize(youBotArmDoF + youBotNumberOfFingers);
armJointStateMessages[armIndex].velocity.resize(youBotArmDoF + youBotNumberOfFingers);

double numberOfArmjoints = 0;
if(youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->hasGripper())
numberOfArmjoints = youBotArmDoF + youBotNumberOfFingers;
else
numberOfArmjoints = youBotArmDoF;

armJointStateMessages[armIndex].name.resize(numberOfArmjoints);
armJointStateMessages[armIndex].position.resize(numberOfArmjoints);
armJointStateMessages[armIndex].velocity.resize(numberOfArmjoints);

if (youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm == 0)
{
Expand Down Expand Up @@ -856,32 +873,33 @@ void YouBotOODLWrapper::computeOODLSensorReadings()
* themselves. Of course if the finger are screwed to the most inner position (i.e. the can close completely),
* than it is correct.
*/
try
{
youbot::YouBotGripperBar& gripperBar1 = youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->getArmGripper().getGripperBar1();
youbot::YouBotGripperBar& gripperBar2 = youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->getArmGripper().getGripperBar2();

if (gripperCycleCounter == 0) { //workaround: avoid congestion of mailbox message by querying only every ith iteration
gripperCycleCounter = youBotDriverCycleFrequencyInHz/5; //approx. 5Hz here
gripperBar1.getData(gripperBar1Position);
gripperBar2.getData(gripperBar2Position);
}
gripperCycleCounter--;
if(youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->hasGripper() &&
(ros::Duration(ros::Time::now() - last_gripper_readings_time_).toSec() > (1 / youBotDriverGripperReadingsCycleFrequencyInHz))) {
try {
youbot::YouBotGripperBar& gripperBar1 = youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->getArmGripper().getGripperBar1();
youbot::YouBotGripperBar& gripperBar2 = youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->getArmGripper().getGripperBar2();

armJointStateMessages[armIndex].name[youBotArmDoF + 0] = youBotConfiguration.youBotArmConfigurations[armIndex].gripperFingerNames[YouBotArmConfiguration::LEFT_FINGER_INDEX];
double leftGipperFingerPosition = gripperBar1Position.barPosition.value();
armJointStateMessages[armIndex].position[youBotArmDoF + 0] = leftGipperFingerPosition;
gripperBar1.getData(gripperBar1Position);
gripperBar2.getData(gripperBar2Position);

double rightGipperFingerPosition = gripperBar2Position.barPosition.value();
armJointStateMessages[armIndex].name[youBotArmDoF + 1] = youBotConfiguration.youBotArmConfigurations[armIndex].gripperFingerNames[YouBotArmConfiguration::RIGHT_FINGER_INDEX];
armJointStateMessages[armIndex].position[youBotArmDoF + 1] = rightGipperFingerPosition;
}
catch (std::exception& e)
{
std::string errorMessage = e.what();
ROS_WARN("Cannot read gripper values: %s", errorMessage.c_str());
armJointStateMessages[armIndex].name[youBotArmDoF + 0] = youBotConfiguration.youBotArmConfigurations[armIndex].gripperFingerNames[YouBotArmConfiguration::LEFT_FINGER_INDEX];
double leftGipperFingerPosition = gripperBar1Position.barPosition.value();
armJointStateMessages[armIndex].position[youBotArmDoF + 0] = leftGipperFingerPosition;

double rightGipperFingerPosition = gripperBar2Position.barPosition.value();
armJointStateMessages[armIndex].name[youBotArmDoF + 1] = youBotConfiguration.youBotArmConfigurations[armIndex].gripperFingerNames[YouBotArmConfiguration::RIGHT_FINGER_INDEX];
armJointStateMessages[armIndex].position[youBotArmDoF + 1] = rightGipperFingerPosition;

last_gripper_readings_time_ = ros::Time::now();
}
catch (std::exception& e) {
std::string errorMessage = e.what();
ROS_WARN("Cannot read gripper values: %s", errorMessage.c_str());
}
}
/*

/*
if (trajectoryActionServerEnable)
{
// updating joint states in trajectory action
Expand Down