Skip to content

wrap calls to getInstance() into try/catch #14

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 1 commit into
base: indigo-devel
Choose a base branch
from
Open

wrap calls to getInstance() into try/catch #14

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
170 changes: 100 additions & 70 deletions src/YouBotOODLWrapper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -402,30 +402,38 @@ void YouBotOODLWrapper::armPositionsCommandCallback(const brics_actuator::JointP

/* loop over all youBot arm joints and check if something is in the received message that requires action */
ROS_ASSERT(youBotConfiguration.youBotArmConfigurations[armIndex].jointNames.size() == static_cast<unsigned int> (youBotArmDoF));
youbot::EthercatMaster::getInstance().AutomaticSendOn(false); // ensure that all joint values will be send at the same time
for (int i = 0; i < youBotArmDoF; ++i)

try
{
youbot::EthercatMaster::getInstance().AutomaticSendOn(false); // ensure that all joint values will be send at the same time

/* check what is in map */
map<string, double>::const_iterator jointIterator = jointNameToValueMapping.find(youBotConfiguration.youBotArmConfigurations[armIndex].jointNames[i]);
if (jointIterator != jointNameToValueMapping.end())
for (int i = 0; i < youBotArmDoF; ++i)
{

/* set the desired joint value */
ROS_DEBUG("Trying to set joint %s to new position value %f", (youBotConfiguration.youBotArmConfigurations[armIndex].jointNames[i]).c_str(), jointIterator->second);
desiredAngle.angle = jointIterator->second * radian;
try
/* check what is in map */
map<string, double>::const_iterator jointIterator = jointNameToValueMapping.find(youBotConfiguration.youBotArmConfigurations[armIndex].jointNames[i]);
if (jointIterator != jointNameToValueMapping.end())
{
youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->getArmJoint(i + 1).setData(desiredAngle); //youBot joints start with 1 not with 0 -> i + 1
}
catch (std::exception& e)
{
std::string errorMessage = e.what();
ROS_WARN("Cannot set arm joint %i: %s", i + 1, errorMessage.c_str());

/* set the desired joint value */
ROS_DEBUG("Trying to set joint %s to new position value %f", (youBotConfiguration.youBotArmConfigurations[armIndex].jointNames[i]).c_str(), jointIterator->second);
desiredAngle.angle = jointIterator->second * radian;
try
{
youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->getArmJoint(i + 1).setData(desiredAngle); //youBot joints start with 1 not with 0 -> i + 1
}
catch (std::exception& e)
{
std::string errorMessage = e.what();
ROS_WARN("Cannot set arm joint %i: %s", i + 1, errorMessage.c_str());
}
}
}
youbot::EthercatMaster::getInstance().AutomaticSendOn(true); // ensure that all joint values will be send at the same time
} catch(std::exception &e)
{
return;
}
youbot::EthercatMaster::getInstance().AutomaticSendOn(true); // ensure that all joint values will be send at the same time
}
else
{
Expand Down Expand Up @@ -469,31 +477,38 @@ void YouBotOODLWrapper::armVelocitiesCommandCallback(const brics_actuator::Joint

/* loop over all youBot arm joints and check if something is in the received message that requires action */
ROS_ASSERT(youBotConfiguration.youBotArmConfigurations[armIndex].jointNames.size() == static_cast<unsigned int> (youBotArmDoF));
youbot::EthercatMaster::getInstance().AutomaticSendOn(false); // ensure that all joint values will be send at the same time
for (int i = 0; i < youBotArmDoF; ++i)
{

/* check what is in map */
map<string, double>::const_iterator jointIterator = jointNameToValueMapping.find(youBotConfiguration.youBotArmConfigurations[armIndex].jointNames[i]);
if (jointIterator != jointNameToValueMapping.end())
try
{
youbot::EthercatMaster::getInstance().AutomaticSendOn(false); // ensure that all joint values will be send at the same time
for (int i = 0; i < youBotArmDoF; ++i)
{

/* set the desired joint value */
ROS_DEBUG("Trying to set joint %s to new velocity value %f", (youBotConfiguration.youBotArmConfigurations[armIndex].jointNames[i]).c_str(), jointIterator->second);
desiredAngularVelocity.angularVelocity = jointIterator->second * radian_per_second;
try
/* check what is in map */
map<string, double>::const_iterator jointIterator = jointNameToValueMapping.find(youBotConfiguration.youBotArmConfigurations[armIndex].jointNames[i]);
if (jointIterator != jointNameToValueMapping.end())
{
youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->getArmJoint(i + 1).setData(desiredAngularVelocity); //youBot joints start with 1 not with 0 -> i + 1

}
catch (std::exception& e)
{
std::string errorMessage = e.what();
ROS_WARN("Cannot set arm joint %i: %s", i + 1, errorMessage.c_str());
/* set the desired joint value */
ROS_DEBUG("Trying to set joint %s to new velocity value %f", (youBotConfiguration.youBotArmConfigurations[armIndex].jointNames[i]).c_str(), jointIterator->second);
desiredAngularVelocity.angularVelocity = jointIterator->second * radian_per_second;
try
{
youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->getArmJoint(i + 1).setData(desiredAngularVelocity); //youBot joints start with 1 not with 0 -> i + 1

}
catch (std::exception& e)
{
std::string errorMessage = e.what();
ROS_WARN("Cannot set arm joint %i: %s", i + 1, errorMessage.c_str());
}
}
}
youbot::EthercatMaster::getInstance().AutomaticSendOn(true); // ensure that all joint values will be send at the same time
} catch(std::exception &e)
{
return;
}
youbot::EthercatMaster::getInstance().AutomaticSendOn(true); // ensure that all joint values will be send at the same time
}
else
{
Expand Down Expand Up @@ -656,37 +671,43 @@ void YouBotOODLWrapper::gripperPositionsCommandCallback(const brics_actuator::Jo
}
}

youbot::EthercatMaster::getInstance().AutomaticSendOn(false); // ensure that all joint values will be send at the same time

/* check if something is in the received message that requires action for the left finger gripper */
gripperIterator = jointNameToValueMapping.find(youBotConfiguration.youBotArmConfigurations[armIndex].gripperFingerNames[YouBotArmConfiguration::LEFT_FINGER_INDEX]);
if (gripperIterator != jointNameToValueMapping.end()) {
ROS_DEBUG("Trying to set the left gripper finger to new value %f", gripperIterator->second);

leftGripperFingerPosition.barPosition = gripperIterator->second * meter;
try {
youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->getArmGripper().getGripperBar1().setData(leftGripperFingerPosition);
} catch (std::exception& e) {
std::string errorMessage = e.what();
ROS_WARN("Cannot set the left gripper finger: %s", errorMessage.c_str());
try
{
youbot::EthercatMaster::getInstance().AutomaticSendOn(false); // ensure that all joint values will be send at the same time

/* check if something is in the received message that requires action for the left finger gripper */
gripperIterator = jointNameToValueMapping.find(youBotConfiguration.youBotArmConfigurations[armIndex].gripperFingerNames[YouBotArmConfiguration::LEFT_FINGER_INDEX]);
if (gripperIterator != jointNameToValueMapping.end()) {
ROS_DEBUG("Trying to set the left gripper finger to new value %f", gripperIterator->second);

leftGripperFingerPosition.barPosition = gripperIterator->second * meter;
try {
youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->getArmGripper().getGripperBar1().setData(leftGripperFingerPosition);
} catch (std::exception& e) {
std::string errorMessage = e.what();
ROS_WARN("Cannot set the left gripper finger: %s", errorMessage.c_str());
}
}
}

/* check if something is in the received message that requires action for the right finger gripper */
gripperIterator = jointNameToValueMapping.find(youBotConfiguration.youBotArmConfigurations[armIndex].gripperFingerNames[YouBotArmConfiguration::RIGHT_FINGER_INDEX]);
if (gripperIterator != jointNameToValueMapping.end()) {
ROS_DEBUG("Trying to set the right gripper to new value %f", gripperIterator->second);

rightGripperFingerPosition.barPosition = gripperIterator->second * meter;
try {
youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->getArmGripper().getGripperBar2().setData(rightGripperFingerPosition);
} catch (std::exception& e) {
std::string errorMessage = e.what();
ROS_WARN("Cannot set the right gripper finger: %s", errorMessage.c_str());
/* check if something is in the received message that requires action for the right finger gripper */
gripperIterator = jointNameToValueMapping.find(youBotConfiguration.youBotArmConfigurations[armIndex].gripperFingerNames[YouBotArmConfiguration::RIGHT_FINGER_INDEX]);
if (gripperIterator != jointNameToValueMapping.end()) {
ROS_DEBUG("Trying to set the right gripper to new value %f", gripperIterator->second);

rightGripperFingerPosition.barPosition = gripperIterator->second * meter;
try {
youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->getArmGripper().getGripperBar2().setData(rightGripperFingerPosition);
} catch (std::exception& e) {
std::string errorMessage = e.what();
ROS_WARN("Cannot set the right gripper finger: %s", errorMessage.c_str());
}
}
}

youbot::EthercatMaster::getInstance().AutomaticSendOn(true); // ensure that all joint values will be send at the same time
youbot::EthercatMaster::getInstance().AutomaticSendOn(true); // ensure that all joint values will be send at the same time
} catch(std::exception &e)
{
return;
}
} else {
ROS_ERROR("Arm%i is not correctly initialized!", armIndex + 1);
}
Expand Down Expand Up @@ -1153,16 +1174,25 @@ void YouBotOODLWrapper::publishArmAndBaseDiagnostics(double publish_rate_in_secs

// EtherCAT status
diagnosticStatusMessage.name = "platform_EtherCAT";
if (youbot::EthercatMaster::getInstance().isEtherCATConnectionEstablished()) {
diagnosticStatusMessage.message = "EtherCAT connnection is established";
diagnosticStatusMessage.level = diagnostic_msgs::DiagnosticStatus::OK;
platformStateMessage.run_stop = false;
}
else {
diagnosticStatusMessage.message = "EtherCAT connnection lost";
diagnosticStatusMessage.level = diagnostic_msgs::DiagnosticStatus::ERROR;
platformStateMessage.run_stop = true;
}

try
{
if (youbot::EthercatMaster::getInstance().isEtherCATConnectionEstablished()) {
diagnosticStatusMessage.message = "EtherCAT connnection is established";
diagnosticStatusMessage.level = diagnostic_msgs::DiagnosticStatus::OK;
platformStateMessage.run_stop = false;
}
else {
diagnosticStatusMessage.message = "EtherCAT connnection lost";
diagnosticStatusMessage.level = diagnostic_msgs::DiagnosticStatus::ERROR;
platformStateMessage.run_stop = true;
}
} catch(std::exception &e)
{
diagnosticStatusMessage.message = "EtherCAT connnection lost";
diagnosticStatusMessage.level = diagnostic_msgs::DiagnosticStatus::ERROR;
platformStateMessage.run_stop = true;
}
diagnosticArrayMessage.status.push_back(diagnosticStatusMessage);


Expand Down