Go to the documentation of this file.00001 #include "robodyn_controllers/RosMsgToolFrameManager.h"
00002
00003
00004 RosMsgToolFrameManager::RosMsgToolFrameManager()
00005 {
00006 }
00007
00008 void RosMsgToolFrameManager::updateToolFrames(const r2_msgs::ToolFrame& toolFrameMsg, KdlTreeUtilities& kdlTree)
00009 {
00010 switch (toolFrameMsg.actionType)
00011 {
00012 case r2_msgs::ToolFrame::RESET: resetToolFrames(toolFrameMsg, kdlTree);
00013 break;
00014
00015 case r2_msgs::ToolFrame::SET: setToolFrames(toolFrameMsg, kdlTree);
00016 break;
00017
00018 case r2_msgs::ToolFrame::ADD: addToolFrames(toolFrameMsg, kdlTree);
00019 break;
00020
00021 case r2_msgs::ToolFrame::REMOVE: removeToolFrames(toolFrameMsg, kdlTree);
00022 break;
00023
00024 }
00025
00026 }
00027
00028 void RosMsgToolFrameManager::resetToolFrames(const r2_msgs::ToolFrame& toolFrameMsg, KdlTreeUtilities& kdlTree)
00029 {
00030 KdlTreeUtilities tempTree = kdlTree;
00031 ToolFrames tempToolFrameList = toolFrameList;
00032
00033 try
00034 {
00036 for (ToolFrameItr itr = toolFrameList.begin(); itr != toolFrameList.end(); ++itr)
00037 {
00038 kdlTree.removeSegment(itr->name);
00039 }
00040
00041 toolFrameList.clear();
00042
00043 }
00044 catch (std::exception& e)
00045 {
00046 NasaCommonLogging::Logger::getCategory("gov.nasa.controllers.RosMsgToolFrameManager") << log4cpp::Priority::ERROR << "Unable to RESET tool frames: Returning to the previous tree.";
00047 toolFrameList = tempToolFrameList;
00048 kdlTree = tempTree;
00049 }
00050
00051 }
00052
00053
00054 void RosMsgToolFrameManager::setToolFrames(const r2_msgs::ToolFrame& toolFrameMsg, KdlTreeUtilities& kdlTree)
00055 {
00056 KdlTreeUtilities tempTree = kdlTree;
00057 ToolFrames tempToolFrameList = toolFrameList;
00058
00059 try
00060 {
00062 for (ToolFrameItr itr = toolFrameList.begin(); itr != toolFrameList.end(); ++itr)
00063 {
00064 kdlTree.removeSegment(itr->name);
00065 }
00066
00067 toolFrameList.clear();
00068
00069 RosMsgConverter::ToolFrameToSegment(toolFrameMsg, toolFrameList);
00070
00072 for (ToolFrameItr itr = toolFrameList.begin(); itr != toolFrameList.end(); ++itr)
00073 {
00074 kdlTree.addSegment(itr->segment, itr->parent);
00075 }
00076 }
00077 catch (std::exception& e)
00078 {
00079 NasaCommonLogging::Logger::getCategory("gov.nasa.controllers.RosMsgToolFrameManager") << log4cpp::Priority::ERROR << "Unable to set requested tool frame: " << e.what() << " Returning to the previous tree.";
00080 toolFrameList = tempToolFrameList;
00081 kdlTree = tempTree;
00082 }
00083
00084 }
00085
00086 void RosMsgToolFrameManager::addToolFrames(const r2_msgs::ToolFrame& toolFrameMsg, KdlTreeUtilities& kdlTree)
00087 {
00088 KdlTreeUtilities tempTree = kdlTree;
00089 ToolFrames tempToolFrameList = toolFrameList;
00090
00091 try
00092 {
00093 toolFrameListToAdd.clear();
00094
00095 RosMsgConverter::ToolFrameToSegmentAdd(toolFrameMsg, toolFrameList, toolFrameListToAdd);
00096
00098 for (ToolFrameItr itr = toolFrameListToAdd.begin(); itr != toolFrameListToAdd.end(); ++itr)
00099 {
00100 kdlTree.addSegment(itr->segment, itr->parent);
00101 }
00102 }
00103 catch (std::exception& e)
00104 {
00105 NasaCommonLogging::Logger::getCategory("gov.nasa.controllers.RosMsgToolFrameManager") << log4cpp::Priority::ERROR << "Unable to add the requested tool frame: " << e.what() << " Returning to the previous tree.";
00106 toolFrameList = tempToolFrameList;
00107 kdlTree = tempTree;
00108 }
00109
00110 }
00111
00112 void RosMsgToolFrameManager::removeToolFrames(const r2_msgs::ToolFrame& toolFrameMsg, KdlTreeUtilities& kdlTree)
00113 {
00114 KdlTreeUtilities tempTree = kdlTree;
00115 ToolFrames tempToolFrameList = toolFrameList;
00116
00117 try
00118 {
00119 toolFrameListToRemove.clear();
00120
00121 RosMsgConverter::ToolFrameToSegmentRemove(toolFrameMsg, toolFrameListToRemove);
00122
00124 for (ToolFrameItr itr = toolFrameListToRemove.begin(); itr != toolFrameListToRemove.end(); ++itr)
00125 {
00126 kdlTree.removeSegment(itr->name);
00127 }
00128
00130 for (ToolFrameItr itr = toolFrameList.begin(); itr != toolFrameList.end();)
00131 {
00132 if (!kdlTree.hasSegment(itr->name))
00133 {
00134 toolFrameList.erase(itr);
00135 }
00136 else
00137 {
00138 ++itr;
00139 }
00140 }
00141 }
00142 catch (std::exception& e)
00143 {
00144 NasaCommonLogging::Logger::getCategory("gov.nasa.controllers.RosMsgToolFrameManager") << log4cpp::Priority::ERROR << "Unable to remove the requested tool frame: " << e.what() << " Returning to the previous tree.";
00145 toolFrameList = tempToolFrameList;
00146 kdlTree = tempTree;
00147 }
00148
00149 }