From 559a440c5fba99ba9fffeff76a19f4c291f706f0 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 28 Jul 2022 00:58:46 +0800 Subject: [PATCH 1/5] Added debug print Signed-off-by: Yadunund --- src/egm_manager.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/egm_manager.cpp b/src/egm_manager.cpp index 91847b2..18ab145 100644 --- a/src/egm_manager.cpp +++ b/src/egm_manager.cpp @@ -39,6 +39,8 @@ #include "abb_egm_rws_managers/egm_manager.h" +#include + namespace { /** @@ -82,13 +84,17 @@ missed_messages_{MISSED_MESSAGES_THRESHOLD} // Check for special cases (six axes robot is used by default). if(configuration.mech_unit_group.has_robot()) { + std::cout << "[egm_manager] Configuration mech unit has robot!" << std::endl; if(configuration.mech_unit_group.robot().axes_total() == 7) { + std::cout << "[egm_manager] The mech unit has 7 Axes!" << std::endl; interface_cfg.axes = egm::RobotAxes::Seven; } + std::cout << "[egm_manager] The mech unit has 6 Axes!" << std::endl; } else { + std::cout << "[egm_manager] Configuration mech unit does not have a robot! Assuming None!" << std::endl; interface_cfg.axes = egm::RobotAxes::None; } From c3c88f2be138112d2644aea7fa614ff7d32f3042 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 28 Jul 2022 01:31:31 +0800 Subject: [PATCH 2/5] More debug Signed-off-by: Yadunund --- src/utilities.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/utilities.cpp b/src/utilities.cpp index 569ad70..79cb5c9 100644 --- a/src/utilities.cpp +++ b/src/utilities.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include "abb_egm_rws_managers/utilities.h" @@ -86,13 +87,16 @@ void initializeMotionData(MotionData& motion_data, const RobotControllerDescript auto createMotionUnit{[&](const MechanicalUnit& unit) { MotionData::MechanicalUnit motion_unit{}; - + std::cout << "[initializeMotionData] ### Creating Motion Unit ###" << std::endl; motion_unit.name = unit.name(); + std::cout << "name: " << motion_unit.name << std::endl; motion_unit.type = unit.type(); + std::cout << "type enum: " << motion_unit.type << std::endl; motion_unit.active = unit.mode() == MechanicalUnit_Mode_ACTIVATED; motion_unit.supported_by_egm = false; // Set indicator for if the unit is supported by EGM or not. + // TODO(YV): Check support for 4 joints if(unit.type() == MechanicalUnit_Type_TCP_ROBOT) { // TCP robots: @@ -153,6 +157,8 @@ void initializeMotionData(MotionData& motion_data, const RobotControllerDescript if(group.has_robot() && !group.robot().standardized_joints().empty()) { + std::cout << "[initializeMotionData]: Robot " << motion_group.name << " has " + << group.robot().axes_total() << " axes" << std::endl; if(group.robot().axes_total() != group.robot().standardized_joints_size()) { throw std::runtime_error{"Number of robot axes are not equal to expected number of joints"}; From 57e7f8ff701c562d406ba96338b9cd39c5197558 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 28 Jul 2022 16:31:18 +0800 Subject: [PATCH 3/5] Successfully able to connect to EGM client on robot Signed-off-by: Yadunund --- src/egm_manager.cpp | 8 +++++++- src/utilities.cpp | 17 ++++++++++++++++- 2 files changed, 23 insertions(+), 2 deletions(-) diff --git a/src/egm_manager.cpp b/src/egm_manager.cpp index 18ab145..c3600fe 100644 --- a/src/egm_manager.cpp +++ b/src/egm_manager.cpp @@ -90,7 +90,13 @@ missed_messages_{MISSED_MESSAGES_THRESHOLD} std::cout << "[egm_manager] The mech unit has 7 Axes!" << std::endl; interface_cfg.axes = egm::RobotAxes::Seven; } - std::cout << "[egm_manager] The mech unit has 6 Axes!" << std::endl; + else if(configuration.mech_unit_group.robot().axes_total() == 4) + { + std::cout << "[egm_manager] The mech unit has 4 Axes!" << std::endl; + interface_cfg.axes = egm::RobotAxes::Four; + } + else + std::cout << "[egm_manager] The mech unit will default to 6 Axes!" << std::endl; } else { diff --git a/src/utilities.cpp b/src/utilities.cpp index 79cb5c9..d817f40 100644 --- a/src/utilities.cpp +++ b/src/utilities.cpp @@ -93,7 +93,6 @@ void initializeMotionData(MotionData& motion_data, const RobotControllerDescript motion_unit.type = unit.type(); std::cout << "type enum: " << motion_unit.type << std::endl; motion_unit.active = unit.mode() == MechanicalUnit_Mode_ACTIVATED; - motion_unit.supported_by_egm = false; // Set indicator for if the unit is supported by EGM or not. // TODO(YV): Check support for 4 joints @@ -114,6 +113,16 @@ void initializeMotionData(MotionData& motion_data, const RobotControllerDescript motion_unit.supported_by_egm = true; } } + else if (unit.axes_total() == 4) + { + std::cout << "setting supported_by_egm = true for robot with " + << unit.axes_total() << "axes" << std::endl; + motion_unit.supported_by_egm = true; + } + else + { + motion_unit.supported_by_egm = false; + } } else if(unit.type() == MechanicalUnit_Type_ROBOT || unit.type() == MechanicalUnit_Type_SINGLE) { @@ -139,6 +148,12 @@ void initializeMotionData(MotionData& motion_data, const RobotControllerDescript motion_joint.command.position = 0.0; motion_joint.command.velocity = 0.0; motion_unit.joints.push_back(motion_joint); + std::cout <<"Configured joint: " << motion_joint.name + << " with rotation capability: " << motion_joint.rotational + << " and limits [" << motion_joint.lower_limit << "," + << motion_joint.upper_limit << "]" + << std::endl; + } return motion_unit; From ff1927fd269ef05fe31e6237eee1c2fca9e1029b Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 28 Jul 2022 18:41:58 +0800 Subject: [PATCH 4/5] Cleanup printouts Signed-off-by: Yadunund --- src/egm_manager.cpp | 10 +++------- src/utilities.cpp | 14 -------------- 2 files changed, 3 insertions(+), 21 deletions(-) diff --git a/src/egm_manager.cpp b/src/egm_manager.cpp index c3600fe..1a2df35 100644 --- a/src/egm_manager.cpp +++ b/src/egm_manager.cpp @@ -39,8 +39,6 @@ #include "abb_egm_rws_managers/egm_manager.h" -#include - namespace { /** @@ -84,23 +82,21 @@ missed_messages_{MISSED_MESSAGES_THRESHOLD} // Check for special cases (six axes robot is used by default). if(configuration.mech_unit_group.has_robot()) { - std::cout << "[egm_manager] Configuration mech unit has robot!" << std::endl; if(configuration.mech_unit_group.robot().axes_total() == 7) { - std::cout << "[egm_manager] The mech unit has 7 Axes!" << std::endl; interface_cfg.axes = egm::RobotAxes::Seven; } else if(configuration.mech_unit_group.robot().axes_total() == 4) { - std::cout << "[egm_manager] The mech unit has 4 Axes!" << std::endl; interface_cfg.axes = egm::RobotAxes::Four; } else - std::cout << "[egm_manager] The mech unit will default to 6 Axes!" << std::endl; + { + // Default to six axes as defined in the BaseConfiguration constructor + } } else { - std::cout << "[egm_manager] Configuration mech unit does not have a robot! Assuming None!" << std::endl; interface_cfg.axes = egm::RobotAxes::None; } diff --git a/src/utilities.cpp b/src/utilities.cpp index d817f40..7fb2c31 100644 --- a/src/utilities.cpp +++ b/src/utilities.cpp @@ -37,7 +37,6 @@ #include #include #include -#include #include "abb_egm_rws_managers/utilities.h" @@ -87,11 +86,8 @@ void initializeMotionData(MotionData& motion_data, const RobotControllerDescript auto createMotionUnit{[&](const MechanicalUnit& unit) { MotionData::MechanicalUnit motion_unit{}; - std::cout << "[initializeMotionData] ### Creating Motion Unit ###" << std::endl; motion_unit.name = unit.name(); - std::cout << "name: " << motion_unit.name << std::endl; motion_unit.type = unit.type(); - std::cout << "type enum: " << motion_unit.type << std::endl; motion_unit.active = unit.mode() == MechanicalUnit_Mode_ACTIVATED; // Set indicator for if the unit is supported by EGM or not. @@ -115,8 +111,6 @@ void initializeMotionData(MotionData& motion_data, const RobotControllerDescript } else if (unit.axes_total() == 4) { - std::cout << "setting supported_by_egm = true for robot with " - << unit.axes_total() << "axes" << std::endl; motion_unit.supported_by_egm = true; } else @@ -148,12 +142,6 @@ void initializeMotionData(MotionData& motion_data, const RobotControllerDescript motion_joint.command.position = 0.0; motion_joint.command.velocity = 0.0; motion_unit.joints.push_back(motion_joint); - std::cout <<"Configured joint: " << motion_joint.name - << " with rotation capability: " << motion_joint.rotational - << " and limits [" << motion_joint.lower_limit << "," - << motion_joint.upper_limit << "]" - << std::endl; - } return motion_unit; @@ -172,8 +160,6 @@ void initializeMotionData(MotionData& motion_data, const RobotControllerDescript if(group.has_robot() && !group.robot().standardized_joints().empty()) { - std::cout << "[initializeMotionData]: Robot " << motion_group.name << " has " - << group.robot().axes_total() << " axes" << std::endl; if(group.robot().axes_total() != group.robot().standardized_joints_size()) { throw std::runtime_error{"Number of robot axes are not equal to expected number of joints"}; From 2e42b1df49708f3768db08f8d4e70ef57b9a3303 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Thu, 28 Jul 2022 18:52:01 +0800 Subject: [PATCH 5/5] Undo comment and line break Signed-off-by: Yadunund --- src/utilities.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/utilities.cpp b/src/utilities.cpp index 7fb2c31..1f53a62 100644 --- a/src/utilities.cpp +++ b/src/utilities.cpp @@ -86,12 +86,12 @@ void initializeMotionData(MotionData& motion_data, const RobotControllerDescript auto createMotionUnit{[&](const MechanicalUnit& unit) { MotionData::MechanicalUnit motion_unit{}; + motion_unit.name = unit.name(); motion_unit.type = unit.type(); motion_unit.active = unit.mode() == MechanicalUnit_Mode_ACTIVATED; // Set indicator for if the unit is supported by EGM or not. - // TODO(YV): Check support for 4 joints if(unit.type() == MechanicalUnit_Type_TCP_ROBOT) { // TCP robots: