Go to the documentation of this file.00001
00025 #include "denso_robot_core/denso_robot_core.h"
00026 #include "denso_robot_core/denso_controller_rc8.h"
00027 #include <boost/thread.hpp>
00028
00029 int main(int argc, char** argv)
00030 {
00031 ros::init(argc, argv, "denso_robot_core");
00032
00033 HRESULT hr;
00034
00035 denso_robot_core::DensoRobotCore engine;
00036
00037 hr = engine.Initialize();
00038 if(FAILED(hr)){
00039 ROS_ERROR("Failed to initialize. (%X)", hr);
00040 return 1;
00041 } else {
00042 boost::thread t(boost::bind(&denso_robot_core::DensoRobotCore::Start, &engine));
00043
00044 ros::spin();
00045
00046 engine.Stop();
00047 t.join();
00048
00049 return 0;
00050 }
00051 }
00052
00053 namespace denso_robot_core {
00054
00055 DensoRobotCore::DensoRobotCore()
00056 : m_ctrlType(0), m_mode(0), m_quit(false)
00057 {
00058 m_ctrl.reset();
00059 }
00060
00061 DensoRobotCore::~DensoRobotCore()
00062 {
00063
00064 }
00065
00066 HRESULT DensoRobotCore::Initialize()
00067 {
00068 ros::NodeHandle node;
00069 std::string name, filename;
00070
00071 if(!node.getParam("controller_name", name)){
00072 name = "";
00073 }
00074
00075 if(!node.getParam("controller_type", m_ctrlType)){
00076 return E_FAIL;
00077 }
00078
00079 if(!node.getParam("config_file", filename)){
00080 return E_FAIL;
00081 }
00082
00083 switch(m_ctrlType){
00084 case 8:
00085 m_ctrl = boost::make_shared<DensoControllerRC8>(name, &m_mode);
00086 break;
00087 default:
00088 return E_FAIL;
00089 }
00090
00091 return m_ctrl->InitializeBCAP(filename);
00092 }
00093
00094 void DensoRobotCore::Start()
00095 {
00096 ros::NodeHandle nd;
00097
00098 m_quit = false;
00099 m_ctrl->StartService(nd);
00100
00101 while(!m_quit && ros::ok()){
00102 ros::spinOnce();
00103 m_ctrl->Update();
00104 ros::Rate(1000).sleep();
00105 }
00106 }
00107
00108 void DensoRobotCore::Stop()
00109 {
00110 m_quit = true;
00111 m_ctrl->StopService();
00112 }
00113
00114 HRESULT DensoRobotCore::ChangeMode(int mode, bool service)
00115 {
00116 m_ctrl->StopService();
00117
00118 DensoRobot_Ptr pRob;
00119 HRESULT hr = m_ctrl->get_Robot(0, &pRob);
00120 if(SUCCEEDED(hr)) {
00121 switch(m_ctrlType) {
00122 case 8:
00123 hr = boost::dynamic_pointer_cast<DensoRobotRC8>(pRob)->ChangeMode(mode);
00124 break;
00125 default:
00126 hr = E_FAIL;
00127 break;
00128 }
00129 }
00130
00131 m_mode = SUCCEEDED(hr) ? mode : 0;
00132
00133 if((m_mode == 0) && service) {
00134 ros::NodeHandle nd;
00135 m_ctrl->StartService(nd);
00136 }
00137
00138 return hr;
00139 }
00140
00141 }