PathFollower.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #include "PathFollower.h"
00011 #include "PathType.h"
00012 // Module specification
00013 // <rtc-template block="module_spec">
00014 static const char* pathfollower_spec[] =
00015   {
00016     "implementation_id", "PathFollower",
00017     "type_name",         "PathFollower",
00018     "description",       "compornent",
00019     "version",           "1.0",
00020     "vendor",            "MyName",
00021     "category",          "example",
00022     "activity_type",     "DataFlowComponent",
00023     "max_instance",      "10",
00024     "language",          "C++",
00025     "lang_type",         "compile",
00026     // Configuration variables
00027     "conf.default.max_v", "1.0",
00028     "conf.default.max_w", "0.3",
00029     "conf.default.max_acc_v", "0.1",
00030     "conf.default.max_acc_w", "0.1",
00031     "conf.default.line_C1", "0.01",
00032     "conf.default.line_K1", "800",
00033     "conf.default.line_K2", "300",
00034     "conf.default.line_K3", "400",
00035     "conf.default.line_Dist", "0.05",
00036     "conf.default.control_cycle", "0.02",
00037 
00038     ""
00039   };
00040 // </rtc-template>
00041 
00042 PathFollower::PathFollower(RTC::Manager* manager)
00043   : RTC::DataFlowComponentBase(manager),
00044     // <rtc-template block="initializer">
00045     m_positionIn("position", m_position),
00046     m_target_pathIn("target_path", m_target_path),
00047     m_velocityOut("velocity", m_velocity),
00048     
00049     // </rtc-template>
00050         dummy(0)
00051 {
00052   registerInPort("position", m_positionIn);
00053   registerInPort("target_path", m_target_pathIn);
00054   
00055   // Set OutPort buffer
00056   registerOutPort("velocity", m_velocityOut);
00057 
00058   initPathFollower();
00059 }
00060 
00061 PathFollower::~PathFollower()
00062 {
00063 }
00064 
00065 
00066 RTC::ReturnCode_t PathFollower::onInitialize()
00067 {
00068   // <rtc-template block="bind_config">
00069   // Bind variables and configuration variable
00070   bindParameter("max_v", m_max_v, "1.0");
00071   bindParameter("max_w", m_max_w, "0.3");
00072   bindParameter("max_acc_v", m_max_acc_v, "0.1");
00073   bindParameter("max_acc_w", m_max_acc_w, "0.1");
00074   bindParameter("line_C1", m_line_C1, "0.01");
00075   bindParameter("line_K1", m_line_K1, "800");
00076   bindParameter("line_K2", m_line_K2, "300");
00077   bindParameter("line_K3", m_line_K3, "400");
00078   bindParameter("line_Dist", m_line_Dist, "0.05");
00079   bindParameter("control_cycle", m_control_cycle, "0.02");
00080   
00081   // </rtc-template>
00082   return RTC::RTC_OK;
00083 }
00084 
00085 
00086 
00087 RTC::ReturnCode_t PathFollower::onActivated(RTC::UniqueId ec_id)
00088 {
00089   run_mode = RUN_STOP;
00090   return RTC::RTC_OK;
00091 }
00092 
00093 RTC::ReturnCode_t PathFollower::onExecute(RTC::UniqueId ec_id)
00094 {
00095   /*next_path*/
00096   if(!m_target_pathIn.isEmpty()){
00097     m_target_pathIn.read();
00098     path_x = m_target_path.x;
00099     path_y = m_target_path.y;
00100     path_theta = m_target_path.theta;
00101     path_v = m_target_path.v;
00102     path_w = m_target_path.w;
00103     run_mode = m_target_path.type;
00104     printf("%d %f %f %f %f %f\n",run_mode,path_x,path_y,path_theta,path_v,path_w);
00105   }
00106 
00107   /*input odometry */
00108   while(!m_positionIn.isEmpty())m_positionIn.read();
00109 
00110   now_x = m_position.data.position.x;
00111   now_y = m_position.data.position.y;
00112   now_theta = m_position.data.heading;
00113 
00114   /*path following*/
00115   path_following(now_x,now_y,now_theta);
00116 
00117   /*output velocity*/
00118   m_velocity.data.vx = target_v*1.5;
00119   m_velocity.data.vy = 0;
00120   m_velocity.data.va = target_w*1.5;
00121   m_velocityOut.write();
00122 
00123   return RTC::RTC_OK;
00124 }
00125 
00126 
00127 extern "C"
00128 {
00129 
00130   void PathFollowerInit(RTC::Manager* manager)
00131   {
00132     RTC::Properties profile(pathfollower_spec);
00133     manager->registerFactory(profile,
00134                              RTC::Create<PathFollower>,
00135                              RTC::Delete<PathFollower>);
00136   }
00137 
00138 };
00139 
00140 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


RS003
Author(s):
autogenerated on Thu Jun 27 2013 14:58:49