OccupancyGridMap3D.h
Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #ifndef NULL_COMPONENT_H
00011 #define NULL_COMPONENT_H
00012 
00013 #include <rtm/idl/BasicDataType.hh>
00014 #include <rtm/idl/InterfaceDataTypes.hh>
00015 #include "hrpsys/idl/pointcloud.hh"
00016 #include <rtm/Manager.h>
00017 #include <rtm/DataFlowComponentBase.h>
00018 #include <rtm/CorbaPort.h>
00019 #include <rtm/DataInPort.h>
00020 #include <rtm/DataOutPort.h>
00021 #include <rtm/idl/BasicDataTypeSkel.h>
00022 
00023 namespace octomap{
00024     class OcTree;
00025 };
00026 
00027 
00028 // Service implementation headers
00029 // <rtc-template block="service_impl_h">
00030 
00031 // </rtc-template>
00032 
00033 // Service Consumer stub headers
00034 // <rtc-template block="consumer_stub_h">
00035 
00036 // </rtc-template>
00037 
00038 #include "OGMap3DService_impl.h"
00039 
00040 using namespace RTC;
00041 
00045 class OccupancyGridMap3D
00046   : public RTC::DataFlowComponentBase
00047 {
00048  public:
00053   OccupancyGridMap3D(RTC::Manager* manager);
00057   virtual ~OccupancyGridMap3D();
00058 
00059   // The initialize action (on CREATED->ALIVE transition)
00060   // formaer rtc_init_entry()
00061   virtual RTC::ReturnCode_t onInitialize();
00062 
00063   // The finalize action (on ALIVE->END transition)
00064   // formaer rtc_exiting_entry()
00065   // virtual RTC::ReturnCode_t onFinalize();
00066 
00067   // The startup action when ExecutionContext startup
00068   // former rtc_starting_entry()
00069   // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id);
00070 
00071   // The shutdown action when ExecutionContext stop
00072   // former rtc_stopping_entry()
00073   // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id);
00074 
00075   // The activated action (Active state entry action)
00076   // former rtc_active_entry()
00077   virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id);
00078 
00079   // The deactivated action (Active state exit action)
00080   // former rtc_active_exit()
00081   virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);
00082 
00083   // The execution action that is invoked periodically
00084   // former rtc_active_do()
00085   virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);
00086 
00087   // The aborting action when main logic error occurred.
00088   // former rtc_aborting_entry()
00089   // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id);
00090 
00091   // The error action in ERROR state
00092   // former rtc_error_do()
00093   // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id);
00094 
00095   // The reset action that is invoked resetting
00096   // This is same but different the former rtc_init_entry()
00097   // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id);
00098 
00099   // The state update action that is invoked after onExecute() action
00100   // no corresponding operation exists in OpenRTm-aist-0.2.0
00101   // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id);
00102 
00103   // The action that is invoked when execution context's rate is changed
00104   // no corresponding operation exists in OpenRTm-aist-0.2.0
00105   // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id);
00106 
00107   OpenHRP::OGMap3D* getOGMap3D(const OpenHRP::AABB& region);
00108   void save(const char *filename);
00109   void clear();
00110 
00111  protected:
00112   // Configuration variable declaration
00113   // <rtc-template block="config_declare">
00114   
00115   // </rtc-template>
00116 
00117   PointCloudTypes::PointCloud m_cloud;
00118   TimedPose3D m_pose;
00119   TimedPoint3D m_sensorPos;
00120   TimedLong m_update;
00121   RangeData m_range;  
00122 
00123   // DataInPort declaration
00124   // <rtc-template block="inport_declare">
00125   InPort<RangeData> m_rangeIn;
00126   InPort<PointCloudTypes::PointCloud> m_cloudIn;
00127   InPort<TimedPose3D> m_poseIn;
00128   InPort<TimedPoint3D> m_sensorPosIn;
00129   InPort<TimedLong> m_updateIn;
00130   
00131   // </rtc-template>
00132 
00133   TimedLong m_updateSignal;
00134 
00135   // DataOutPort declaration
00136   // <rtc-template block="outport_declare">
00137   OutPort<TimedLong> m_updateOut;
00138   
00139   // </rtc-template>
00140 
00141   // CORBA Port declaration
00142   // <rtc-template block="corbaport_declare">
00143   RTC::CorbaPort m_OGMap3DServicePort;
00144   
00145   // </rtc-template>
00146 
00147   // Service declaration
00148   // <rtc-template block="service_declare">
00149   OGMap3DService_impl m_service0;
00150   
00151   // </rtc-template>
00152 
00153   // Consumer declaration
00154   // <rtc-template block="consumer_declare">
00155   
00156   // </rtc-template>
00157 
00158  private:
00159   octomap::OcTree *m_map, *m_knownMap;
00160   double m_occupiedThd, m_resolution;
00161   std::string m_initialMap;
00162   std::string m_knownMapPath;
00163   std::string m_cwd;
00164   coil::Mutex m_mutex;
00165   int m_debugLevel;
00166   int dummy;
00167 };
00168 
00169 
00170 extern "C"
00171 {
00172   void OccupancyGridMap3DInit(RTC::Manager* manager);
00173 };
00174 
00175 #endif // NULL_COMPONENT_H


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:55