From 3f050e823c745270d04c0fd25c6ea14526efe586 Mon Sep 17 00:00:00 2001 From: Frederik Hegger Date: Fri, 8 May 2015 09:02:51 +0200 Subject: [PATCH 1/5] add a dynamic check if a gripper is available --- .../YouBotOODLWrapper.h | 2 +- src/YouBotOODLWrapper.cpp | 97 ++++++++++--------- 2 files changed, 54 insertions(+), 45 deletions(-) diff --git a/include/youbot_driver_ros_interface/YouBotOODLWrapper.h b/include/youbot_driver_ros_interface/YouBotOODLWrapper.h index c234d69..cf147d6 100644 --- a/include/youbot_driver_ros_interface/YouBotOODLWrapper.h +++ b/include/youbot_driver_ros_interface/YouBotOODLWrapper.h @@ -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. diff --git a/src/YouBotOODLWrapper.cpp b/src/YouBotOODLWrapper.cpp index 3767324..0864239 100644 --- a/src/YouBotOODLWrapper.cpp +++ b/src/YouBotOODLWrapper.cpp @@ -115,7 +115,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; @@ -150,10 +150,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 (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); @@ -167,18 +180,18 @@ void YouBotOODLWrapper::initializeArm(std::string armName, bool enableStandardGr youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->calibrateGripper(); } + catch (std::exception& e) + { + ROS_ERROR_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 (youBotConfiguration.youBotArmConfigurations.size())); - return; + ROS_ERROR_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 @@ -201,15 +214,10 @@ void YouBotOODLWrapper::initializeArm(std::string armName, bool enableStandardGr topicName << youBotConfiguration.youBotArmConfigurations[armIndex].commandTopicName << "joint_states"; youBotConfiguration.youBotArmConfigurations[armIndex].armJointStatePublisher = node.advertise (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 (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 (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(""); @@ -856,32 +864,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); + if(youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->hasGripper()) { + 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--; + + 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; + } + catch (std::exception& e) { + std::string errorMessage = e.what(); + ROS_WARN("Cannot read gripper values: %s", errorMessage.c_str()); } - gripperCycleCounter--; - - 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; - } - 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 From ab9a44c7b700bf86526fc5412960e511ff0c47d1 Mon Sep 17 00:00:00 2001 From: Frederik Hegger Date: Fri, 8 May 2015 17:47:22 +0200 Subject: [PATCH 2/5] use a parameter to set the cycle rate for getting gripper readings --- .../YouBotOODLWrapper.h | 8 +++++--- launch/youbot_driver.launch | 1 + src/YouBotOODLWrapper.cpp | 20 ++++++++++--------- 3 files changed, 17 insertions(+), 12 deletions(-) diff --git a/include/youbot_driver_ros_interface/YouBotOODLWrapper.h b/include/youbot_driver_ros_interface/YouBotOODLWrapper.h index cf147d6..fd101ae 100644 --- a/include/youbot_driver_ros_interface/YouBotOODLWrapper.h +++ b/include/youbot_driver_ros_interface/YouBotOODLWrapper.h @@ -256,9 +256,8 @@ 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); @@ -266,6 +265,7 @@ class YouBotOODLWrapper //double trajectoryVelocityGain; //double trajectoryPositionGain; double youBotDriverCycleFrequencyInHz; + double youBotDriverGripperReadingsCycleFrequencyInHz; /// diagnostic msgs ros::Time lastDiagnosticPublishTime; @@ -281,6 +281,8 @@ class YouBotOODLWrapper bool areBaseMotorsSwitchedOn; bool areArmMotorsSwitchedOn; + + ros::Time last_gripper_readings_time_; }; } // namespace youBot diff --git a/launch/youbot_driver.launch b/launch/youbot_driver.launch index 7d43d86..d766742 100644 --- a/launch/youbot_driver.launch +++ b/launch/youbot_driver.launch @@ -16,6 +16,7 @@ or enable the USE_SETCAP flag in the cmake file and recompile again. + diff --git a/src/YouBotOODLWrapper.cpp b/src/YouBotOODLWrapper.cpp index 0864239..7396374 100644 --- a/src/YouBotOODLWrapper.cpp +++ b/src/YouBotOODLWrapper.cpp @@ -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("/dashboard/platform_state", 1); diagnosticArrayPublisher = n.advertise("/diagnostics", 1); + + last_gripper_readings_time_ = ros::Time::now(); } YouBotOODLWrapper::~YouBotOODLWrapper() @@ -864,17 +866,15 @@ 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. */ - if(youBotConfiguration.youBotArmConfigurations[armIndex].youBotArm->hasGripper()) { + + 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(); - 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--; + gripperBar1.getData(gripperBar1Position); + gripperBar2.getData(gripperBar2Position); armJointStateMessages[armIndex].name[youBotArmDoF + 0] = youBotConfiguration.youBotArmConfigurations[armIndex].gripperFingerNames[YouBotArmConfiguration::LEFT_FINGER_INDEX]; double leftGipperFingerPosition = gripperBar1Position.barPosition.value(); @@ -883,6 +883,8 @@ void YouBotOODLWrapper::computeOODLSensorReadings() 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(); @@ -890,7 +892,7 @@ void YouBotOODLWrapper::computeOODLSensorReadings() } } - /* + /* if (trajectoryActionServerEnable) { // updating joint states in trajectory action From 8474c5cabba89888baa77cf85bd9da377bc78c2e Mon Sep 17 00:00:00 2001 From: Frederik Hegger Date: Fri, 8 May 2015 17:51:08 +0200 Subject: [PATCH 3/5] add missing closing of param tag --- launch/youbot_driver.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/youbot_driver.launch b/launch/youbot_driver.launch index d766742..ec05438 100644 --- a/launch/youbot_driver.launch +++ b/launch/youbot_driver.launch @@ -16,7 +16,7 @@ or enable the USE_SETCAP flag in the cmake file and recompile again. - From c24ce569857eaa2a9122c9831c562366c7f90247 Mon Sep 17 00:00:00 2001 From: Frederik Hegger Date: Wed, 27 May 2015 12:19:26 +0000 Subject: [PATCH 4/5] do not publish empty joint state fields if no gripper is attached --- src/YouBotOODLWrapper.cpp | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/src/YouBotOODLWrapper.cpp b/src/YouBotOODLWrapper.cpp index 7396374..70c6414 100644 --- a/src/YouBotOODLWrapper.cpp +++ b/src/YouBotOODLWrapper.cpp @@ -821,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) { From a8b4c611b36ecba33d7a90b984a9377143364c32 Mon Sep 17 00:00:00 2001 From: Frederik Hegger Date: Fri, 29 May 2015 07:31:00 +0000 Subject: [PATCH 5/5] change debug output level from error to warn --- src/YouBotOODLWrapper.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/YouBotOODLWrapper.cpp b/src/YouBotOODLWrapper.cpp index 70c6414..3e3ed38 100644 --- a/src/YouBotOODLWrapper.cpp +++ b/src/YouBotOODLWrapper.cpp @@ -184,12 +184,12 @@ void YouBotOODLWrapper::initializeArm(std::string armName) } catch (std::exception& e) { - ROS_ERROR_STREAM("Gripper on arm \"" << armName << "\" could not be initialized: " << e.what()); + ROS_WARN_STREAM("Gripper on arm \"" << armName << "\" could not be initialized: " << e.what()); } } else { - ROS_ERROR_STREAM("Gripper on arm \"" << armName << "\" not found or disabled in the config file!"); + ROS_WARN_STREAM("Gripper on arm \"" << armName << "\" not found or disabled in the config file!"); }