diff --git a/include/phoxi_camera/PhoXiConversions.h b/include/phoxi_camera/PhoXiConversions.h index dfc5f4f..f0f5f68 100644 --- a/include/phoxi_camera/PhoXiConversions.h +++ b/include/phoxi_camera/PhoXiConversions.h @@ -8,23 +8,30 @@ #include #include -void toPhoXiCameraDeviceInforamtion(const pho::api::PhoXiDeviceInformation& phoXiDeviceInformation,phoxi_camera::PhoXiDeviceInformation &phoXiCameraDeviceInformation){ +void toPhoXiCameraDeviceInforamtion(const pho::api::PhoXiDeviceInformation& phoXiDeviceInformation, + phoxi_camera::PhoXiDeviceInformation& phoXiCameraDeviceInformation) { phoXiCameraDeviceInformation.name = phoXiDeviceInformation.Name; - phoXiCameraDeviceInformation.type = phoxi_camera::PhoXiDeviceInformation::PhoXiDeviceType((int)phoXiDeviceInformation.Type); + phoXiCameraDeviceInformation.type = phoxi_camera::PhoXiDeviceInformation::PhoXiDeviceType( + (int)phoXiDeviceInformation.Type); phoXiCameraDeviceInformation.hwIdentification = phoXiDeviceInformation.HWIdentification; - if (phoXiDeviceInformation.Status.Ready == true ){ - phoXiCameraDeviceInformation.status = phoXiDeviceInformation.Status.Ready ? phoxi_camera::PhoXiDeviceInformation::PhoXiConnectionStatus::Ready : phoxi_camera::PhoXiDeviceInformation::PhoXiConnectionStatus::Starting; - } - else{ - phoXiCameraDeviceInformation.status = phoXiDeviceInformation.Status.Attached ? phoxi_camera::PhoXiDeviceInformation::PhoXiConnectionStatus::Ready : phoxi_camera::PhoXiDeviceInformation::PhoXiConnectionStatus::Occupied; + if (phoXiDeviceInformation.Status.Ready == true) { + phoXiCameraDeviceInformation.status = phoXiDeviceInformation.Status.Ready + ? phoxi_camera::PhoXiDeviceInformation::PhoXiConnectionStatus::Ready + : phoxi_camera::PhoXiDeviceInformation::PhoXiConnectionStatus::Starting; + } else { + phoXiCameraDeviceInformation.status = phoXiDeviceInformation.Status.Attached + ? phoxi_camera::PhoXiDeviceInformation::PhoXiConnectionStatus::Ready + : phoxi_camera::PhoXiDeviceInformation::PhoXiConnectionStatus::Occupied; } phoXiCameraDeviceInformation.firmwareVersion = phoXiDeviceInformation.FirmwareVersion; } -void toPhoXiCameraDeviceInforamtion(const std::vector& phoXiDeviceInformation,std::vector &phoXiCameraDeviceInformation){ + +void toPhoXiCameraDeviceInforamtion(const std::vector& phoXiDeviceInformation, + std::vector& phoXiCameraDeviceInformation) { phoXiCameraDeviceInformation.clear(); phoXiCameraDeviceInformation.resize(phoXiDeviceInformation.size()); for (int i = 0; i < phoXiDeviceInformation.size(); ++i) { - toPhoXiCameraDeviceInforamtion(phoXiDeviceInformation[i],phoXiCameraDeviceInformation[i]); + toPhoXiCameraDeviceInforamtion(phoXiDeviceInformation[i], phoXiCameraDeviceInformation[i]); } } diff --git a/include/phoxi_camera/PhoXiDeviceInformation.h b/include/phoxi_camera/PhoXiDeviceInformation.h index 5aa6c2d..7985d9f 100644 --- a/include/phoxi_camera/PhoXiDeviceInformation.h +++ b/include/phoxi_camera/PhoXiDeviceInformation.h @@ -7,32 +7,36 @@ #include -namespace phoxi_camera{ +namespace phoxi_camera { class PhoXiInterface; class PhoXiDeviceInformation { public: friend PhoXiInterface; - enum PhoXiConnectionStatus{ + enum PhoXiConnectionStatus { Unknown = 0, Ready = 1, Occupied = 2, Starting = 3 }; - enum PhoXiDeviceType{ + enum PhoXiDeviceType { PhoXiScanner, PhoXiCamera, //future camera support NoValue }; + operator std::string() const { return hwIdentification; } - bool operator==(const PhoXiDeviceInformation & other){ + + bool operator==(const PhoXiDeviceInformation& other) { return hwIdentification == other.hwIdentification; } - bool operator==(const std::string & hwIdentification){ + + bool operator==(const std::string& hwIdentification) { return this->hwIdentification == hwIdentification; } + std::string name; PhoXiDeviceType type; std::string hwIdentification; diff --git a/include/phoxi_camera/PhoXiException.h b/include/phoxi_camera/PhoXiException.h index 6942976..3a8bd38 100644 --- a/include/phoxi_camera/PhoXiException.h +++ b/include/phoxi_camera/PhoXiException.h @@ -6,51 +6,54 @@ #define PROJECT_PHOXIEXCEPTION_H #include -namespace phoxi_camera{ + +namespace phoxi_camera { class PhoXiInterfaceException : public std::exception { public: - PhoXiInterfaceException(std::string message) : message(message){ + PhoXiInterfaceException(std::string message) : message(message) { } - virtual const char* what() const throw() - { + + virtual const char* what() const throw() { return message.c_str(); } + private: std::string message; }; class PhoXiControlNotRunning : public PhoXiInterfaceException { public: - PhoXiControlNotRunning(std::string message) : PhoXiInterfaceException(message){ + PhoXiControlNotRunning(std::string message) : PhoXiInterfaceException(message) { } }; class PhoXiScannerNotFound : public PhoXiInterfaceException { public: - PhoXiScannerNotFound(std::string message) : PhoXiInterfaceException(message){ + PhoXiScannerNotFound(std::string message) : PhoXiInterfaceException(message) { } }; + class UnableToConnect : public PhoXiInterfaceException { public: - UnableToConnect(std::string message) : PhoXiInterfaceException(message){ + UnableToConnect(std::string message) : PhoXiInterfaceException(message) { } }; class UnableToStartAcquisition : public PhoXiInterfaceException { public: - UnableToStartAcquisition(std::string message) : PhoXiInterfaceException(message){ + UnableToStartAcquisition(std::string message) : PhoXiInterfaceException(message) { } }; class UnableToStopAcquisition : public PhoXiInterfaceException { public: - UnableToStopAcquisition(std::string message) : PhoXiInterfaceException(message){ + UnableToStopAcquisition(std::string message) : PhoXiInterfaceException(message) { } }; class CorruptedFrame : public PhoXiInterfaceException { public: - CorruptedFrame(std::string message) : PhoXiInterfaceException(message){ + CorruptedFrame(std::string message) : PhoXiInterfaceException(message) { } }; @@ -61,27 +64,27 @@ namespace phoxi_camera{ }; - class PhoXiScannerNotConnected : public PhoXiInterfaceException { + class PhoXiScannerNotConnected : public PhoXiInterfaceException { public: - PhoXiScannerNotConnected(std::string message) : PhoXiInterfaceException(message){ + PhoXiScannerNotConnected(std::string message) : PhoXiInterfaceException(message) { } }; - class CoordinateSpaceNotSupported : public PhoXiInterfaceException { - public: - CoordinateSpaceNotSupported(std::string message) : PhoXiInterfaceException(message){ + class CoordinateSpaceNotSupported : public PhoXiInterfaceException { + public: + CoordinateSpaceNotSupported(std::string message) : PhoXiInterfaceException(message) { } }; - class InvalidTransformationMatrix : public PhoXiInterfaceException { + class InvalidTransformationMatrix : public PhoXiInterfaceException { public: - InvalidTransformationMatrix(std::string message) : PhoXiInterfaceException(message){ + InvalidTransformationMatrix(std::string message) : PhoXiInterfaceException(message) { } }; - class InvalidTriggerMode : public PhoXiInterfaceException { + class InvalidTriggerMode : public PhoXiInterfaceException { public: - InvalidTriggerMode(std::string message) : PhoXiInterfaceException(message){ + InvalidTriggerMode(std::string message) : PhoXiInterfaceException(message) { } }; } diff --git a/include/phoxi_camera/PhoXiInterface.h b/include/phoxi_camera/PhoXiInterface.h index 964d4b4..7179e20 100644 --- a/include/phoxi_camera/PhoXiInterface.h +++ b/include/phoxi_camera/PhoXiInterface.h @@ -17,11 +17,11 @@ #define STR(x) STR_HELPER(x) #define PHOXI_API_VERSION STR(PHOXI_API_VERSION_MAJOR) "." STR(PHOXI_API_VERSION_MINOR) "." STR(PHOXI_API_VERSION_PATCH) #if PHOXI_API_VERSION_MAJOR == 1 - #if PHOXI_API_VERSION_MINOR == 1 - #define PHOXI_API_v1_1 - #elif PHOXI_API_VERSION_MINOR == 2 - #define PHOXI_API_v1_2 - #endif +#if PHOXI_API_VERSION_MINOR == 1 +#define PHOXI_API_v1_1 +#elif PHOXI_API_VERSION_MINOR == 2 +#define PHOXI_API_v1_2 +#endif #endif //* PhoXiInterface @@ -29,8 +29,8 @@ * Wrapper to PhoXi 3D Scanner api to make interface easier * */ -namespace phoxi_camera{ -class PhoXiInterface { +namespace phoxi_camera { + class PhoXiInterface { public: /** @@ -44,6 +44,7 @@ class PhoXiInterface { * \throw PhoXiControlNotRunning when PhoXi Control is not running */ std::vector deviceList(); + /** * Return all PhoXi 3D Scanners ids connected on network. * @@ -51,6 +52,7 @@ class PhoXiInterface { * \throw PhoXiControlNotRunning when PhoXi Control is not running */ std::vector cameraList(); + /** * Connect to camera. * @@ -61,11 +63,15 @@ class PhoXiInterface { * \throw PhoXiScannerNotFound when PhoXi 3D Scanner with HWIdentification is not available on network * \throw UnableToStartAcquisition when connection failed */ - void connectCamera(std::string HWIdentification, pho::api::PhoXiTriggerMode mode = pho::api::PhoXiTriggerMode::Software, bool startAcquisition = true); + void connectCamera(std::string HWIdentification, + pho::api::PhoXiTriggerMode mode = pho::api::PhoXiTriggerMode::Software, + bool startAcquisition = true); + /** * Disconnect from camera if connected to any. */ void disconnectCamera(); + /** * Get frame based on id. If id is negative new image is triggered and new PFrame returned. * @@ -74,6 +80,7 @@ class PhoXiInterface { * \throw PhoXiScannerNotConnected when no scanner is connected */ pho::api::PFrame getPFrame(int id = -1); + /** * Get point cloud * @@ -81,38 +88,46 @@ class PhoXiInterface { * \throw PhoXiScannerNotConnected when no scanner is connected */ std::shared_ptr> getPointCloud(bool organized = true); + /** * Convert PFrame to point cloud * * \param organized - if true return organized point cloud */ - static std::shared_ptr> getPointCloudFromFrame(pho::api::PFrame frame,bool organized = true); + static std::shared_ptr> + getPointCloudFromFrame(pho::api::PFrame frame, bool organized = true); + /** * Convert PFrame to organized point cloud * * \return organized point cloud */ static std::shared_ptr> getOrganizedCloudFromFrame(pho::api::PFrame frame); + /** * Convert PFrame to unorganized point cloud * * \return unorganized point cloud */ static std::shared_ptr> getUnorganizedCloudFromFrame(pho::api::PFrame frame); + /** * Test if connection to PhoXi 3D Scanner is working * * \throw PhoXiScannerNotConnected when no scanner is connected */ void isOk(); + /** * Test if connection to PhoXi 3D Scanner is working */ bool isConnected(); + /** * Test if PhoXi 3D Scanner is Acquiring */ bool isAcquiring(); + /** * Start acquisition * @@ -120,6 +135,7 @@ class PhoXiInterface { * \throw UnableToStartAcquisition if acquisition was not started */ void startAcquisition(); + /** * Stop acquisition * @@ -127,6 +143,7 @@ class PhoXiInterface { * \throw UnableToStartAcquisition if acquisition was not stopped */ void stopAcquisition(); + /** * Trigger new Image * @@ -183,6 +200,7 @@ class PhoXiInterface { pho::api::PhoXiTriggerMode getTriggerMode(); #ifndef PHOXI_API_v1_1 + /** * Set coordinate space * @@ -208,7 +226,8 @@ class PhoXiInterface { * \throw PhoXiScannerNotConnected when no scanner is connected * \throw CoordinateSpaceNotSupported when space is not supported */ - void setTransformation(pho::api::PhoXiCoordinateTransformation coordinateTransformation,pho::api::PhoXiCoordinateSpace space,bool setSpace = true, bool saveSettings = true); + void setTransformation(pho::api::PhoXiCoordinateTransformation coordinateTransformation, + pho::api::PhoXiCoordinateSpace space, bool setSpace = true, bool saveSettings = true); /** * Set transformation matrix space @@ -222,24 +241,26 @@ class PhoXiInterface { * \throw CoordinateSpaceNotSupported when space is not supported */ template - void setTransformation(Eigen::Matrix transformation,pho::api::PhoXiCoordinateSpace space,bool setSpace = true, bool saveSettings = true){ - setTransformation(getPhoXiCoordinateTransformation(transformation),space,setSpace,saveSettings); + void setTransformation(Eigen::Matrix transformation, pho::api::PhoXiCoordinateSpace space, + bool setSpace = true, bool saveSettings = true) { + setTransformation(getPhoXiCoordinateTransformation(transformation), space, setSpace, saveSettings); } + template /** * Convert eigen matrix to pho::api::PhoXiCoordinateTransformation */ - static pho::api::PhoXiCoordinateTransformation getPhoXiCoordinateTransformation(Eigen::Matrix mat){ + static pho::api::PhoXiCoordinateTransformation getPhoXiCoordinateTransformation(Eigen::Matrix mat) { pho::api::PhoXiCoordinateTransformation coordinateTransformation; for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { - coordinateTransformation.Rotation[i][j] = mat(i,j); + coordinateTransformation.Rotation[i][j] = mat(i, j); } } - coordinateTransformation.Translation.x = mat(0,3); - coordinateTransformation.Translation.y = mat(1,3); - coordinateTransformation.Translation.z = mat(2,3); + coordinateTransformation.Translation.x = mat(0, 3); + coordinateTransformation.Translation.y = mat(1, 3); + coordinateTransformation.Translation.z = mat(2, 3); return coordinateTransformation; } @@ -249,7 +270,7 @@ class PhoXiInterface { * \param path full path of file with extension * \return bool whether saving proceed successful */ - bool saveLastFrame(const std::string &path); + bool saveLastFrame(const std::string& path); /** * get PhoXi Api Version @@ -258,16 +279,19 @@ class PhoXiInterface { */ std::string getApiVersion(); + /** * get PhoXi Scanner firmware Version * * \return connected PhoXi Scanner firmware Version */ std::string getFirmwareVersion(); + #endif protected: pho::api::PPhoXi scanner; pho::api::PhoXiFactory phoXiFactory; + /** * Return all PhoXi 3D Scanners ids connected on network. * @@ -275,6 +299,7 @@ class PhoXiInterface { * \throw PhoXiControlNotRunning when PhoXi Control is not running */ std::vector deviceInforamtionList(); + private: int last_frame_id = -1; }; diff --git a/include/phoxi_camera/RosConversions.h b/include/phoxi_camera/RosConversions.h index 3b7ebaf..e4d5d66 100644 --- a/include/phoxi_camera/RosConversions.h +++ b/include/phoxi_camera/RosConversions.h @@ -8,7 +8,8 @@ #include #include -void phoXiDeviceInforamtionToRosMsg(const phoxi_camera::PhoXiDeviceInformation& phoXiDeviceInformation, phoxi_camera::DeviceInformation& deviceInformationMsg){ +void phoXiDeviceInforamtionToRosMsg(const phoxi_camera::PhoXiDeviceInformation& phoXiDeviceInformation, + phoxi_camera::DeviceInformation& deviceInformationMsg) { deviceInformationMsg.name = phoXiDeviceInformation.name; deviceInformationMsg.type.type = (int)phoXiDeviceInformation.type; deviceInformationMsg.hwIdentification = phoXiDeviceInformation.hwIdentification; @@ -16,14 +17,15 @@ void phoXiDeviceInforamtionToRosMsg(const phoxi_camera::PhoXiDeviceInformation& deviceInformationMsg.firmwareVersion = phoXiDeviceInformation.firmwareVersion; } -void phoXiDeviceInforamtionToRosMsg(const std::vector& phoXiDeviceInformation, std::vector& deviceInformationMsg){ +void phoXiDeviceInforamtionToRosMsg(const std::vector& phoXiDeviceInformation, + std::vector& deviceInformationMsg) { deviceInformationMsg.clear(); deviceInformationMsg.resize(phoXiDeviceInformation.size()); - for (const auto &device : phoXiDeviceInformation){ - phoXiDeviceInforamtionToRosMsg(device,deviceInformationMsg.back()); + for (const auto& device : phoXiDeviceInformation) { + phoXiDeviceInforamtionToRosMsg(device, deviceInformationMsg.back()); } - for (int i = 0; i < phoXiDeviceInformation.size() ; ++i) { - phoXiDeviceInforamtionToRosMsg(phoXiDeviceInformation[i],deviceInformationMsg[i]); + for (int i = 0; i < phoXiDeviceInformation.size(); ++i) { + phoXiDeviceInforamtionToRosMsg(phoXiDeviceInformation[i], deviceInformationMsg[i]); } } diff --git a/include/phoxi_camera/RosInterface.h b/include/phoxi_camera/RosInterface.h index 7f1e5a8..7ed0a21 100644 --- a/include/phoxi_camera/RosInterface.h +++ b/include/phoxi_camera/RosInterface.h @@ -37,92 +37,130 @@ #include #include -namespace phoxi_camera{ - class RosInterface : protected PhoXiInterface { - public: - RosInterface(); - protected: - void publishFrame(pho::api::PFrame frame); - pho::api::PFrame getPFrame(int id = -1); - int triggerImage(); - void connectCamera(std::string HWIdentification, pho::api::PhoXiTriggerMode mode = pho::api::PhoXiTriggerMode::Software, bool startAcquisition = true); - std::string getTriggerMode(pho::api::PhoXiTriggerMode mode); - void getDefaultDynamicReconfigureConfig(phoxi_camera::phoxi_cameraConfig& config); - - std::string frameId; - private: - bool getDeviceList(phoxi_camera::GetDeviceList::Request &req, phoxi_camera::GetDeviceList::Response &res); - bool connectCamera(phoxi_camera::ConnectCamera::Request &req, phoxi_camera::ConnectCamera::Response &res); - bool isConnected(phoxi_camera::IsConnected::Request &req, phoxi_camera::IsConnected::Response &res); - bool isAcquiring(phoxi_camera::IsAcquiring::Request &req, phoxi_camera::IsAcquiring::Response &res); - bool isConnected(phoxi_camera::GetBool::Request &req, phoxi_camera::GetBool::Response &res); - bool isAcquiring(phoxi_camera::GetBool::Request &req, phoxi_camera::GetBool::Response &res); - bool startAcquisition(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res); - bool stopAcquisition(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res); - bool startAcquisition(phoxi_camera::Empty::Request &req, phoxi_camera::Empty::Response &res); - bool stopAcquisition(phoxi_camera::Empty::Request &req, phoxi_camera::Empty::Response &res); - bool triggerImage(phoxi_camera::TriggerImage::Request &req, phoxi_camera::TriggerImage::Response &res); - bool getFrame(phoxi_camera::GetFrame::Request &req, phoxi_camera::GetFrame::Response &res); - bool saveFrame(phoxi_camera::SaveFrame::Request &req, phoxi_camera::SaveFrame::Response &res); - bool disconnectCamera(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res); - bool getHardwareIdentification(phoxi_camera::GetHardwareIdentification::Request &req, phoxi_camera::GetHardwareIdentification::Response &res); - bool getSupportedCapturingModes(phoxi_camera::GetSupportedCapturingModes::Request &req, phoxi_camera::GetSupportedCapturingModes::Response &res); - bool getApiVersion(phoxi_camera::GetString::Request &req, phoxi_camera::GetString::Response &res); - bool getFirmwareVersion(phoxi_camera::GetString::Request &req, phoxi_camera::GetString::Response &res); - void dynamicReconfigureCallback(phoxi_camera::phoxi_cameraConfig &config, uint32_t level); - void diagnosticCallback(diagnostic_updater::DiagnosticStatusWrapper& status); - void diagnosticTimerCallback(const ros::TimerEvent&); - void initFromPhoXi(); +namespace phoxi_camera { + class RosInterface : protected PhoXiInterface { + public: + RosInterface(); + + protected: + void publishFrame(pho::api::PFrame frame); + + pho::api::PFrame getPFrame(int id = -1); + + int triggerImage(); + + void connectCamera(std::string HWIdentification, + pho::api::PhoXiTriggerMode mode = pho::api::PhoXiTriggerMode::Software, + bool startAcquisition = true); + + std::string getTriggerMode(pho::api::PhoXiTriggerMode mode); + + void getDefaultDynamicReconfigureConfig(phoxi_camera::phoxi_cameraConfig& config); + + std::string frameId; + private: + bool getDeviceList(phoxi_camera::GetDeviceList::Request& req, phoxi_camera::GetDeviceList::Response& res); + + bool connectCamera(phoxi_camera::ConnectCamera::Request& req, phoxi_camera::ConnectCamera::Response& res); + + bool isConnected(phoxi_camera::IsConnected::Request& req, phoxi_camera::IsConnected::Response& res); + + bool isAcquiring(phoxi_camera::IsAcquiring::Request& req, phoxi_camera::IsAcquiring::Response& res); + + bool isConnected(phoxi_camera::GetBool::Request& req, phoxi_camera::GetBool::Response& res); + + bool isAcquiring(phoxi_camera::GetBool::Request& req, phoxi_camera::GetBool::Response& res); + + bool startAcquisition(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); + + bool stopAcquisition(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); + + bool startAcquisition(phoxi_camera::Empty::Request& req, phoxi_camera::Empty::Response& res); + + bool stopAcquisition(phoxi_camera::Empty::Request& req, phoxi_camera::Empty::Response& res); + + bool triggerImage(phoxi_camera::TriggerImage::Request& req, phoxi_camera::TriggerImage::Response& res); + + bool getFrame(phoxi_camera::GetFrame::Request& req, phoxi_camera::GetFrame::Response& res); + + bool saveFrame(phoxi_camera::SaveFrame::Request& req, phoxi_camera::SaveFrame::Response& res); + + bool disconnectCamera(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res); + + bool getHardwareIdentification(phoxi_camera::GetHardwareIdentification::Request& req, + phoxi_camera::GetHardwareIdentification::Response& res); + + bool getSupportedCapturingModes(phoxi_camera::GetSupportedCapturingModes::Request& req, + phoxi_camera::GetSupportedCapturingModes::Response& res); + + bool getApiVersion(phoxi_camera::GetString::Request& req, phoxi_camera::GetString::Response& res); + + bool getFirmwareVersion(phoxi_camera::GetString::Request& req, phoxi_camera::GetString::Response& res); + + void dynamicReconfigureCallback(phoxi_camera::phoxi_cameraConfig& config, uint32_t level); + + void diagnosticCallback(diagnostic_updater::DiagnosticStatusWrapper& status); + + void diagnosticTimerCallback(const ros::TimerEvent&); + + void initFromPhoXi(); + #ifndef PHOXI_API_v1_1 - bool setCoordianteSpace(phoxi_camera::SetCoordinatesSpace::Request &req, phoxi_camera::SetCoordinatesSpace::Response &res); - bool setTransformation(phoxi_camera::SetTransformationMatrix::Request &req, phoxi_camera::SetTransformationMatrix::Response &res); - bool saveLastFrame(phoxi_camera::SaveLastFrame::Request &req, phoxi_camera::SaveLastFrame::Response &res); + + bool setCoordianteSpace(phoxi_camera::SetCoordinatesSpace::Request& req, + phoxi_camera::SetCoordinatesSpace::Response& res); + + bool setTransformation(phoxi_camera::SetTransformationMatrix::Request& req, + phoxi_camera::SetTransformationMatrix::Response& res); + + bool saveLastFrame(phoxi_camera::SaveLastFrame::Request& req, phoxi_camera::SaveLastFrame::Response& res); + #endif - //node handle - ros::NodeHandle nh; - - //ros service servers - ros::ServiceServer getDeviceListService; - ros::ServiceServer connectCameraService; - ros::ServiceServer isConnectedService; - ros::ServiceServer isAcquiringService; - ros::ServiceServer isAcquiringServiceV2; - ros::ServiceServer isConnectedServiceV2; - ros::ServiceServer startAcquisitionService; - ros::ServiceServer stopAcquisitionService; - ros::ServiceServer startAcquisitionServiceV2; - ros::ServiceServer stopAcquisitionServiceV2; - ros::ServiceServer triggerImageService; - ros::ServiceServer getFrameService; - ros::ServiceServer saveFrameService; - ros::ServiceServer saveLastFrameService; - ros::ServiceServer disconnectCameraService; - ros::ServiceServer getHardwareIdentificationService; - ros::ServiceServer getSupportedCapturingModesService; - ros::ServiceServer setCoordianteSpaceService; - ros::ServiceServer setTransformationService; - ros::ServiceServer getApiVersionService; - ros::ServiceServer getFirmwareVersionService; - - //ros publishers - ros::Publisher cloudPub; - ros::Publisher normalMapPub; - ros::Publisher confidenceMapPub; - ros::Publisher rawTexturePub; - ros::Publisher rgbTexturePub; - ros::Publisher depthMapPub; - - //dynamic reconfigure - boost::recursive_mutex dynamicReconfigureMutex; - dynamic_reconfigure::Server dynamicReconfigureServer; - phoxi_camera::phoxi_cameraConfig dynamicReconfigureConfig; - - //diagnostic - diagnostic_updater::Updater diagnosticUpdater; - diagnostic_updater::FunctionDiagnosticTask PhoXi3DscannerDiagnosticTask; - ros::Timer diagnosticTimer; - - }; + //node handle + ros::NodeHandle nh; + + //ros service servers + ros::ServiceServer getDeviceListService; + ros::ServiceServer connectCameraService; + ros::ServiceServer isConnectedService; + ros::ServiceServer isAcquiringService; + ros::ServiceServer isAcquiringServiceV2; + ros::ServiceServer isConnectedServiceV2; + ros::ServiceServer startAcquisitionService; + ros::ServiceServer stopAcquisitionService; + ros::ServiceServer startAcquisitionServiceV2; + ros::ServiceServer stopAcquisitionServiceV2; + ros::ServiceServer triggerImageService; + ros::ServiceServer getFrameService; + ros::ServiceServer saveFrameService; + ros::ServiceServer saveLastFrameService; + ros::ServiceServer disconnectCameraService; + ros::ServiceServer getHardwareIdentificationService; + ros::ServiceServer getSupportedCapturingModesService; + ros::ServiceServer setCoordianteSpaceService; + ros::ServiceServer setTransformationService; + ros::ServiceServer getApiVersionService; + ros::ServiceServer getFirmwareVersionService; + + //ros publishers + ros::Publisher cloudPub; + ros::Publisher normalMapPub; + ros::Publisher confidenceMapPub; + ros::Publisher rawTexturePub; + ros::Publisher rgbTexturePub; + ros::Publisher depthMapPub; + + //dynamic reconfigure + boost::recursive_mutex dynamicReconfigureMutex; + dynamic_reconfigure::Server dynamicReconfigureServer; + phoxi_camera::phoxi_cameraConfig dynamicReconfigureConfig; + + //diagnostic + diagnostic_updater::Updater diagnosticUpdater; + diagnostic_updater::FunctionDiagnosticTask PhoXi3DscannerDiagnosticTask; + ros::Timer diagnosticTimer; + + }; } diff --git a/include/phoxi_camera/Types.h b/include/phoxi_camera/Types.h index 927467f..676cabf 100644 --- a/include/phoxi_camera/Types.h +++ b/include/phoxi_camera/Types.h @@ -6,7 +6,8 @@ #define PROJECT_COORDINATESSPACE_H #include -namespace phoxi_camera{ + +namespace phoxi_camera { typedef pho::api::PhoXiCoordinateSpace CoordinateSpace; typedef pho::api::PhoXiTriggerMode TriggerMode; } diff --git a/src/PhoXiInterface.cpp b/src/PhoXiInterface.cpp index 7119e27..8667627 100644 --- a/src/PhoXiInterface.cpp +++ b/src/PhoXiInterface.cpp @@ -11,24 +11,24 @@ namespace phoxi_camera { } - std::vector PhoXiInterface::deviceList(){ + std::vector PhoXiInterface::deviceList() { if (!phoXiFactory.isPhoXiControlRunning()) { scanner.Reset(); throw PhoXiControlNotRunning("PhoXi Control is not running"); } std::vector deviceInfo; auto dl = phoXiFactory.GetDeviceList(); - toPhoXiCameraDeviceInforamtion(dl,deviceInfo); + toPhoXiCameraDeviceInforamtion(dl, deviceInfo); return deviceInfo; } std::vector PhoXiInterface::cameraList() { - auto dl = deviceList(); - std::vector hwIdentificationList; - for (const auto& device : dl){ - hwIdentificationList.push_back(device); - } + auto dl = deviceList(); + std::vector hwIdentificationList; + for (const auto& device : dl) { + hwIdentificationList.push_back(device); + } return hwIdentificationList; // return std::vector(); } @@ -42,7 +42,7 @@ namespace phoxi_camera { } } std::vector cl = cameraList(); - auto it = std::find(cl.begin(),cl.end(),HWIdentification); + auto it = std::find(cl.begin(), cl.end(), HWIdentification); if (it == cl.end()) { throw PhoXiScannerNotFound("Scanner not found"); } @@ -65,7 +65,7 @@ namespace phoxi_camera { if (id < 0) { try { id = this->triggerImage(); - } catch (UnableToTriggerFrame &e) { + } catch (UnableToTriggerFrame& e) { throw; } @@ -79,16 +79,19 @@ namespace phoxi_camera { } std::shared_ptr> PhoXiInterface::getPointCloudFromFrame(pho::api::PFrame frame, bool organized) { - if(organized){ + if (organized) { return getOrganizedCloudFromFrame(frame); } return getUnorganizedCloudFromFrame(frame); } - std::shared_ptr> PhoXiInterface::getOrganizedCloudFromFrame(pho::api::PFrame frame){ + + std::shared_ptr> + PhoXiInterface::getOrganizedCloudFromFrame(pho::api::PFrame frame) { if (!frame || !frame->Successful) { throw CorruptedFrame("Corrupted frame!"); } - std::shared_ptr> cloud(new pcl::PointCloud(frame->GetResolution().Width, frame->GetResolution().Height)); + std::shared_ptr> cloud( + new pcl::PointCloud(frame->GetResolution().Width, frame->GetResolution().Height)); for (int r = 0; r < frame->GetResolution().Height; r++) { for (int c = 0; c < frame->GetResolution().Width; c++) { auto point = frame->PointCloud.At(r, c); @@ -108,7 +111,7 @@ namespace phoxi_camera { return cloud; } - std::shared_ptr> PhoXiInterface::getUnorganizedCloudFromFrame(pho::api::PFrame frame){ + std::shared_ptr> PhoXiInterface::getUnorganizedCloudFromFrame(pho::api::PFrame frame) { if (!frame || !frame->Successful) { throw CorruptedFrame("Corrupted frame!"); } @@ -116,7 +119,7 @@ namespace phoxi_camera { for (int r = 0; r < frame->GetResolution().Height; r++) { for (int c = 0; c < frame->GetResolution().Width; c++) { auto point = frame->PointCloud.At(r, c); - if(point == pho::api::Point3_32f(0,0,0)){ + if (point == pho::api::Point3_32f(0, 0, 0)) { continue; } pcl::PointNormal pclPoint; @@ -246,6 +249,7 @@ namespace phoxi_camera { } #ifndef PHOXI_API_v1_1 + void PhoXiInterface::setCoordinateSpace(pho::api::PhoXiCoordinateSpace space) { this->isOk(); scanner->CoordinatesSettings->CoordinateSpace = space; @@ -289,7 +293,7 @@ namespace phoxi_camera { } } - bool PhoXiInterface::saveLastFrame(const std::string &path) { + bool PhoXiInterface::saveLastFrame(const std::string& path) { if (last_frame_id == -1) { // this is needed due to PhoXi API bug // if you didn't trigger frame before, you can not save it @@ -299,13 +303,16 @@ namespace phoxi_camera { bool success = scanner->SaveLastOutput(path, this->last_frame_id); return success; } + #endif - std::string PhoXiInterface::getApiVersion(){ + + std::string PhoXiInterface::getApiVersion() { return PHOXI_API_VERSION; } - std::string PhoXiInterface::getFirmwareVersion(){ + + std::string PhoXiInterface::getFirmwareVersion() { auto list = cameraList(); - for(auto device : list){ + for (auto device : list) { } } diff --git a/src/RosInterface.cpp b/src/RosInterface.cpp index 5c23228..5d639c9 100644 --- a/src/RosInterface.cpp +++ b/src/RosInterface.cpp @@ -13,8 +13,11 @@ #include #include -namespace phoxi_camera{ - RosInterface::RosInterface() : nh("~"), dynamicReconfigureServer(dynamicReconfigureMutex,nh), PhoXi3DscannerDiagnosticTask("PhoXi3Dscanner",boost::bind(&RosInterface::diagnosticCallback, this, _1)) { +namespace phoxi_camera { + RosInterface::RosInterface() : nh("~"), dynamicReconfigureServer(dynamicReconfigureMutex, nh), + PhoXi3DscannerDiagnosticTask("PhoXi3Dscanner", + boost::bind(&RosInterface::diagnosticCallback, this, + _1)) { //create service servers getDeviceListService = nh.advertiseService("get_device_list", &RosInterface::getDeviceList, this); @@ -46,212 +49,230 @@ namespace phoxi_camera{ int topic_queue_size; nh.param("latch_topics", latch_topics, false); nh.param("topic_queue_size", topic_queue_size, 1); - cloudPub = nh.advertise < sensor_msgs::PointCloud2 >("pointcloud", 1,latch_topics); - normalMapPub = nh.advertise < sensor_msgs::Image > ("normal_map", topic_queue_size,latch_topics); - confidenceMapPub = nh.advertise < sensor_msgs::Image > ("confidence_map", topic_queue_size,latch_topics); - rawTexturePub = nh.advertise < sensor_msgs::Image > ("texture", topic_queue_size,latch_topics); - rgbTexturePub = nh.advertise < sensor_msgs::Image > ("rgb_texture", topic_queue_size,latch_topics); - depthMapPub = nh.advertise < sensor_msgs::Image > ("depth_map", topic_queue_size,latch_topics); + cloudPub = nh.advertise("pointcloud", 1, latch_topics); + normalMapPub = nh.advertise("normal_map", topic_queue_size, latch_topics); + confidenceMapPub = nh.advertise("confidence_map", topic_queue_size, latch_topics); + rawTexturePub = nh.advertise("texture", topic_queue_size, latch_topics); + rgbTexturePub = nh.advertise("rgb_texture", topic_queue_size, latch_topics); + depthMapPub = nh.advertise("depth_map", topic_queue_size, latch_topics); //set diagnostic Hw id diagnosticUpdater.setHardwareID("none"); diagnosticUpdater.add(PhoXi3DscannerDiagnosticTask); - diagnosticTimer = nh.createTimer(ros::Duration(5.0),&RosInterface::diagnosticTimerCallback, this); + diagnosticTimer = nh.createTimer(ros::Duration(5.0), &RosInterface::diagnosticTimerCallback, this); diagnosticTimer.start(); nh.param("frame_id", frameId, "PhoXi3Dscanner_sensor"); //connect to default scanner std::string scannerId; - if(nh.param("scanner_id", scannerId, "") && !scannerId.empty()){ + if (nh.param("scanner_id", scannerId, "") && !scannerId.empty()) { try { RosInterface::connectCamera(scannerId); - ROS_INFO("Connected to %s",scannerId.c_str()); - }catch(PhoXiInterfaceException& e) { - ROS_WARN("Connection to default scanner %s failed. %s ",scannerId.c_str(),e.what()); + ROS_INFO("Connected to %s", scannerId.c_str()); + } catch (PhoXiInterfaceException& e) { + ROS_WARN("Connection to default scanner %s failed. %s ", scannerId.c_str(), e.what()); } } - if(!PhoXiInterface::isConnected()){ + if (!PhoXiInterface::isConnected()) { getDefaultDynamicReconfigureConfig(dynamicReconfigureConfig); } //set dynamic reconfigure callback - dynamicReconfigureServer.setCallback(boost::bind(&RosInterface::dynamicReconfigureCallback,this, _1, _2)); + dynamicReconfigureServer.setCallback(boost::bind(&RosInterface::dynamicReconfigureCallback, this, _1, _2)); } - bool RosInterface::getDeviceList(phoxi_camera::GetDeviceList::Request &req, phoxi_camera::GetDeviceList::Response &res){ + bool + RosInterface::getDeviceList(phoxi_camera::GetDeviceList::Request& req, phoxi_camera::GetDeviceList::Response& res) { try { res.out = PhoXiInterface::cameraList(); - phoXiDeviceInforamtionToRosMsg(PhoXiInterface::deviceList(),res.device_information_list); + phoXiDeviceInforamtionToRosMsg(PhoXiInterface::deviceList(), res.device_information_list); res.len = res.out.size(); res.success = true; res.message = OKRESPONSE; - }catch (PhoXiInterfaceException &e){ + } catch (PhoXiInterfaceException& e) { res.success = false; res.message = e.what(); } return true; } - bool RosInterface::connectCamera(phoxi_camera::ConnectCamera::Request &req, phoxi_camera::ConnectCamera::Response &res){ + + bool + RosInterface::connectCamera(phoxi_camera::ConnectCamera::Request& req, phoxi_camera::ConnectCamera::Response& res) { try { RosInterface::connectCamera(req.name); res.success = true; res.message = OKRESPONSE; - }catch (PhoXiInterfaceException &e){ + } catch (PhoXiInterfaceException& e) { res.success = false; res.message = e.what(); } return true; } - bool RosInterface::isConnected(phoxi_camera::IsConnected::Request &req, phoxi_camera::IsConnected::Response &res){ + + bool RosInterface::isConnected(phoxi_camera::IsConnected::Request& req, phoxi_camera::IsConnected::Response& res) { res.connected = PhoXiInterface::isConnected(); return true; } - bool RosInterface::isAcquiring(phoxi_camera::IsAcquiring::Request &req, phoxi_camera::IsAcquiring::Response &res){ + + bool RosInterface::isAcquiring(phoxi_camera::IsAcquiring::Request& req, phoxi_camera::IsAcquiring::Response& res) { res.is_acquiring = PhoXiInterface::isAcquiring(); return true; } - bool RosInterface::isConnected(phoxi_camera::GetBool::Request &req, phoxi_camera::GetBool::Response &res){ + + bool RosInterface::isConnected(phoxi_camera::GetBool::Request& req, phoxi_camera::GetBool::Response& res) { res.value = PhoXiInterface::isConnected(); res.message = OKRESPONSE; //todo tot este premysliet res.success = true; return true; } - bool RosInterface::isAcquiring(phoxi_camera::GetBool::Request &req, phoxi_camera::GetBool::Response &res){ + + bool RosInterface::isAcquiring(phoxi_camera::GetBool::Request& req, phoxi_camera::GetBool::Response& res) { res.value = PhoXiInterface::isAcquiring(); res.message = OKRESPONSE; //todo tot este premysliet res.success = true; return true; } - bool RosInterface::startAcquisition(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res){ + + bool RosInterface::startAcquisition(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res) { try { PhoXiInterface::startAcquisition(); dynamicReconfigureConfig.start_acquisition = true; dynamicReconfigureServer.updateConfig(dynamicReconfigureConfig); diagnosticUpdater.force_update(); - }catch (PhoXiInterfaceException &e){ - ROS_ERROR("%s",e.what()); + } catch (PhoXiInterfaceException& e) { + ROS_ERROR("%s", e.what()); } return true; } - bool RosInterface::stopAcquisition(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res){ + + bool RosInterface::stopAcquisition(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res) { try { PhoXiInterface::stopAcquisition(); dynamicReconfigureConfig.start_acquisition = false; dynamicReconfigureServer.updateConfig(dynamicReconfigureConfig); diagnosticUpdater.force_update(); - }catch (PhoXiInterfaceException &e){ - ROS_ERROR("%s",e.what()); + } catch (PhoXiInterfaceException& e) { + ROS_ERROR("%s", e.what()); } return true; } - bool RosInterface::startAcquisition(phoxi_camera::Empty::Request &req, phoxi_camera::Empty::Response &res){ + + bool RosInterface::startAcquisition(phoxi_camera::Empty::Request& req, phoxi_camera::Empty::Response& res) { try { dynamicReconfigureConfig.start_acquisition = true; dynamicReconfigureServer.updateConfig(dynamicReconfigureConfig); PhoXiInterface::startAcquisition(); res.message = OKRESPONSE; res.success = true; - }catch (PhoXiInterfaceException &e){ + } catch (PhoXiInterfaceException& e) { res.message = e.what(); res.success = false; } return true; } - bool RosInterface::stopAcquisition(phoxi_camera::Empty::Request &req, phoxi_camera::Empty::Response &res){ + + bool RosInterface::stopAcquisition(phoxi_camera::Empty::Request& req, phoxi_camera::Empty::Response& res) { try { dynamicReconfigureConfig.start_acquisition = false; dynamicReconfigureServer.updateConfig(dynamicReconfigureConfig); PhoXiInterface::stopAcquisition(); res.message = OKRESPONSE; res.success = true; - }catch (PhoXiInterfaceException &e){ + } catch (PhoXiInterfaceException& e) { res.message = e.what(); res.success = false; } return true; } - bool RosInterface::triggerImage(phoxi_camera::TriggerImage::Request &req, phoxi_camera::TriggerImage::Response &res){ + + bool + RosInterface::triggerImage(phoxi_camera::TriggerImage::Request& req, phoxi_camera::TriggerImage::Response& res) { try { res.id = RosInterface::triggerImage(); res.success = true; res.message = OKRESPONSE; - }catch (PhoXiInterfaceException &e){ + } catch (PhoXiInterfaceException& e) { res.success = false; res.message = e.what(); } return true; } - bool RosInterface::getFrame(phoxi_camera::GetFrame::Request &req, phoxi_camera::GetFrame::Response &res){ + + bool RosInterface::getFrame(phoxi_camera::GetFrame::Request& req, phoxi_camera::GetFrame::Response& res) { try { pho::api::PFrame frame = getPFrame(req.in); publishFrame(frame); - if(!frame){ + if (!frame) { res.success = false; res.message = "Null frame!"; - } - else{ + } else { res.success = true; res.message = OKRESPONSE; } - }catch (PhoXiInterfaceException &e){ + } catch (PhoXiInterfaceException& e) { res.success = false; res.message = e.what(); } return true; } - bool RosInterface::saveFrame(phoxi_camera::SaveFrame::Request &req, phoxi_camera::SaveFrame::Response &res){ + bool RosInterface::saveFrame(phoxi_camera::SaveFrame::Request& req, phoxi_camera::SaveFrame::Response& res) { try { pho::api::PFrame frame = RosInterface::getPFrame(req.in); - if(!frame){ + if (!frame) { res.success = false; res.message = "Null frame!"; return true; } size_t pos = req.path.find("~"); - if(pos != std::string::npos){ + if (pos != std::string::npos) { char* home = std::getenv("HOME"); - if(!home){ + if (!home) { res.message = "'~' found in 'path' parameter but environment variable 'HOME' not found. Export' HOME' variable or pass absolute value to 'path' parameter."; res.success = false; return true; } - req.path.replace(pos,1,home); + req.path.replace(pos, 1, home); } - ROS_INFO("path: %s",req.path.c_str()); + ROS_INFO("path: %s", req.path.c_str()); frame->SaveAsPly(req.path); res.message = OKRESPONSE; res.success = true; - }catch (PhoXiInterfaceException &e){ + } catch (PhoXiInterfaceException& e) { res.success = false; res.message = e.what(); } return true; } - bool RosInterface::disconnectCamera(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res){ + + bool RosInterface::disconnectCamera(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res) { try { PhoXiInterface::disconnectCamera(); diagnosticUpdater.force_update(); - }catch (PhoXiInterfaceException &e){ + } catch (PhoXiInterfaceException& e) { //scanner is already disconnected on exception } return true; } - bool RosInterface::getHardwareIdentification(phoxi_camera::GetHardwareIdentification::Request &req, phoxi_camera::GetHardwareIdentification::Response &res){ + + bool RosInterface::getHardwareIdentification(phoxi_camera::GetHardwareIdentification::Request& req, + phoxi_camera::GetHardwareIdentification::Response& res) { try { res.hardware_identification = PhoXiInterface::getHardwareIdentification(); res.success = true; res.message = OKRESPONSE; - }catch (PhoXiInterfaceException &e){ + } catch (PhoXiInterfaceException& e) { res.success = false; res.message = e.what(); } return true; } - bool RosInterface::getSupportedCapturingModes(phoxi_camera::GetSupportedCapturingModes::Request &req, phoxi_camera::GetSupportedCapturingModes::Response &res){ + + bool RosInterface::getSupportedCapturingModes(phoxi_camera::GetSupportedCapturingModes::Request& req, + phoxi_camera::GetSupportedCapturingModes::Response& res) { try { std::vector modes = PhoXiInterface::getSupportedCapturingModes(); - for(int i =0; i < modes.size(); i++){ + for (int i = 0; i < modes.size(); i++) { phoxi_camera::PhoXiSize size; size.Height = modes[i].Resolution.Height; size.Width = modes[i].Resolution.Width; @@ -259,26 +280,29 @@ namespace phoxi_camera{ } res.success = true; res.message = OKRESPONSE; - }catch (PhoXiInterfaceException &e){ + } catch (PhoXiInterfaceException& e) { res.success = false; res.message = e.what(); } return true; } - bool RosInterface::getApiVersion(phoxi_camera::GetString::Request &req, phoxi_camera::GetString::Response &res){ + + bool RosInterface::getApiVersion(phoxi_camera::GetString::Request& req, phoxi_camera::GetString::Response& res) { res.value = PhoXiInterface::getApiVersion(); res.success = true; return true; } - bool RosInterface::getFirmwareVersion(phoxi_camera::GetString::Request &req, phoxi_camera::GetString::Response &res){ - try{ + + bool + RosInterface::getFirmwareVersion(phoxi_camera::GetString::Request& req, phoxi_camera::GetString::Response& res) { + try { auto dl = PhoXiInterface::deviceList(); - auto it = std::find(dl.begin(),dl.end(),PhoXiInterface::getHardwareIdentification()); - if(it != dl.end()){ + auto it = std::find(dl.begin(), dl.end(), PhoXiInterface::getHardwareIdentification()); + if (it != dl.end()) { res.value = it->firmwareVersion; } } - catch (phoxi_camera::PhoXiInterfaceException &e){ + catch (phoxi_camera::PhoXiInterfaceException& e) { res.message = e.what(); res.success = false; } @@ -299,17 +323,17 @@ namespace phoxi_camera{ header.frame_id = frameId; header.seq = frame->Info.FrameIndex; - if (frame->PointCloud.Empty()){ + if (frame->PointCloud.Empty()) { ROS_WARN("Empty point cloud!"); } else { - auto cloud = PhoXiInterface::getPointCloudFromFrame(frame,dynamicReconfigureConfig.organized_cloud); + auto cloud = PhoXiInterface::getPointCloudFromFrame(frame, dynamicReconfigureConfig.organized_cloud); sensor_msgs::PointCloud2 output_cloud; - pcl::toROSMsg(*cloud,output_cloud); + pcl::toROSMsg(*cloud, output_cloud); output_cloud.header = header; cloudPub.publish(output_cloud); } - if(frame->DepthMap.Empty()){ + if (frame->DepthMap.Empty()) { ROS_WARN("Empty depth map!"); } else { sensor_msgs::Image depth_map; @@ -336,7 +360,8 @@ namespace phoxi_camera{ frame->Texture.Size.Width * sizeof(float), // stepSize frame->Texture.operator[](0)); rawTexturePub.publish(texture); - cv::Mat cvGreyTexture(frame->Texture.Size.Height, frame->Texture.Size.Width, CV_32FC1, frame->Texture.operator[](0)); + cv::Mat cvGreyTexture(frame->Texture.Size.Height, frame->Texture.Size.Width, CV_32FC1, + frame->Texture.operator[](0)); cv::normalize(cvGreyTexture, cvGreyTexture, 0, 255, CV_MINMAX); cvGreyTexture.convertTo(cvGreyTexture, CV_8U); cv::equalizeHist(cvGreyTexture, cvGreyTexture); @@ -345,7 +370,7 @@ namespace phoxi_camera{ cv_bridge::CvImage rgbTexture(header, sensor_msgs::image_encodings::BGR8, cvRgbTexture); rgbTexturePub.publish(rgbTexture.toImageMsg()); } - if (frame->ConfidenceMap.Empty()){ + if (frame->ConfidenceMap.Empty()) { ROS_WARN("Empty confidence map!"); } else { sensor_msgs::Image confidence_map; @@ -360,7 +385,7 @@ namespace phoxi_camera{ confidenceMapPub.publish(confidence_map); } - if (frame->NormalMap.Empty()){ + if (frame->NormalMap.Empty()) { ROS_WARN("Empty normal map!"); } else { sensor_msgs::Image normal_map; @@ -377,14 +402,14 @@ namespace phoxi_camera{ } - void RosInterface::dynamicReconfigureCallback(phoxi_camera::phoxi_cameraConfig &config, uint32_t level) { - if(!PhoXiInterface::isConnected()){ + void RosInterface::dynamicReconfigureCallback(phoxi_camera::phoxi_cameraConfig& config, uint32_t level) { + if (!PhoXiInterface::isConnected()) { config = this->dynamicReconfigureConfig; return; } - try{ + try { if (level & (1 << 1)) { - switch (config.resolution){ + switch (config.resolution) { case 0: PhoXiInterface::setLowResolution(); break; @@ -411,7 +436,7 @@ namespace phoxi_camera{ } if (level & (1 << 4)) { - PhoXiInterface::setTriggerMode(config.trigger_mode,config.start_acquisition); + PhoXiInterface::setTriggerMode(config.trigger_mode, config.start_acquisition); this->dynamicReconfigureConfig.trigger_mode = config.trigger_mode; this->dynamicReconfigureConfig.start_acquisition = config.start_acquisition; } @@ -489,12 +514,12 @@ namespace phoxi_camera{ } #endif - }catch (PhoXiInterfaceException &e){ - ROS_WARN("%s",e.what()); + } catch (PhoXiInterfaceException& e) { + ROS_WARN("%s", e.what()); } } - pho::api::PFrame RosInterface::getPFrame(int id){ + pho::api::PFrame RosInterface::getPFrame(int id) { pho::api::PFrame frame = PhoXiInterface::getPFrame(id); //update dynamic reconfigure dynamicReconfigureConfig.trigger_mode = PhoXiInterface::scanner->TriggerMode.GetValue(); @@ -503,7 +528,7 @@ namespace phoxi_camera{ return frame; } - int RosInterface::triggerImage(){ + int RosInterface::triggerImage() { int id = PhoXiInterface::triggerImage(); //update dynamic reconfigure dynamicReconfigureConfig.coordinate_space = pho::api::PhoXiTriggerMode::Software; @@ -512,44 +537,42 @@ namespace phoxi_camera{ return id; } - void RosInterface::connectCamera(std::string HWIdentification, pho::api::PhoXiTriggerMode mode, bool startAcquisition){ - PhoXiInterface::connectCamera(HWIdentification,mode,startAcquisition); + void + RosInterface::connectCamera(std::string HWIdentification, pho::api::PhoXiTriggerMode mode, bool startAcquisition) { + PhoXiInterface::connectCamera(HWIdentification, mode, startAcquisition); bool initFromConfig = false; - nh.getParam("init_from_config",initFromConfig); - if(initFromConfig){ + nh.getParam("init_from_config", initFromConfig); + if (initFromConfig) { getDefaultDynamicReconfigureConfig(dynamicReconfigureConfig); - this->dynamicReconfigureCallback(dynamicReconfigureConfig,std::numeric_limits::max()); - } - else{ + this->dynamicReconfigureCallback(dynamicReconfigureConfig, std::numeric_limits::max()); + } else { initFromPhoXi(); } dynamicReconfigureServer.updateConfig(dynamicReconfigureConfig); diagnosticUpdater.force_update(); } - void RosInterface::diagnosticCallback(diagnostic_updater::DiagnosticStatusWrapper& status){ - if(PhoXiInterface::isConnected()){ - if(PhoXiInterface::isAcquiring()){ - status.summary(diagnostic_msgs::DiagnosticStatus::OK,"Ready"); - } - else{ - status.summary(diagnostic_msgs::DiagnosticStatus::WARN,"Acquisition not started"); + void RosInterface::diagnosticCallback(diagnostic_updater::DiagnosticStatusWrapper& status) { + if (PhoXiInterface::isConnected()) { + if (PhoXiInterface::isAcquiring()) { + status.summary(diagnostic_msgs::DiagnosticStatus::OK, "Ready"); + } else { + status.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Acquisition not started"); } - status.add("HardwareIdentification",std::string(scanner->HardwareIdentification)); - status.add("Trigger mode",getTriggerMode(scanner->TriggerMode)); + status.add("HardwareIdentification", std::string(scanner->HardwareIdentification)); + status.add("Trigger mode", getTriggerMode(scanner->TriggerMode)); - } - else{ - status.summary(diagnostic_msgs::DiagnosticStatus::ERROR,"Not connected"); + } else { + status.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Not connected"); } } - void RosInterface::diagnosticTimerCallback(const ros::TimerEvent&){ + void RosInterface::diagnosticTimerCallback(const ros::TimerEvent&) { diagnosticUpdater.force_update(); } - std::string RosInterface::getTriggerMode(pho::api::PhoXiTriggerMode mode){ - switch (mode){ + std::string RosInterface::getTriggerMode(pho::api::PhoXiTriggerMode mode) { + switch (mode) { case pho::api::PhoXiTriggerMode::Freerun: return "Freerun"; case pho::api::PhoXiTriggerMode::Software: @@ -563,18 +586,17 @@ namespace phoxi_camera{ } } - void RosInterface::initFromPhoXi(){ + void RosInterface::initFromPhoXi() { getDefaultDynamicReconfigureConfig(dynamicReconfigureConfig); - if(!scanner->isConnected()){ + if (!scanner->isConnected()) { ROS_WARN("Scanner not connected."); return; } ///resolution pho::api::PhoXiCapturingMode mode = scanner->CapturingMode; - if((mode.Resolution.Width == 2064) && (mode.Resolution.Height = 1544)){ + if ((mode.Resolution.Width == 2064) && (mode.Resolution.Height = 1544)) { this->dynamicReconfigureConfig.resolution = 1; - } - else{ + } else { this->dynamicReconfigureConfig.resolution = 0; } @@ -594,7 +616,7 @@ namespace phoxi_camera{ #ifndef PHOXI_API_v1_1 this->dynamicReconfigureConfig.coordinate_space = scanner->CoordinatesSettings->CoordinateSpace; this->dynamicReconfigureConfig.ambient_light_suppression = capturingSettings.AmbientLightSuppression; - this->dynamicReconfigureConfig.single_pattern_exposure = capturingSettings.SinglePatternExposure; + this->dynamicReconfigureConfig.single_pattern_exposure = capturingSettings.SinglePatternExposure; this->dynamicReconfigureConfig.camera_only_mode = capturingSettings.CameraOnlyMode; #endif this->dynamicReconfigureConfig.trigger_mode = scanner->TriggerMode.GetValue(); @@ -603,7 +625,9 @@ namespace phoxi_camera{ } #ifndef PHOXI_API_v1_1 - bool RosInterface::setCoordianteSpace(phoxi_camera::SetCoordinatesSpace::Request &req, phoxi_camera::SetCoordinatesSpace::Response &res){ + + bool RosInterface::setCoordianteSpace(phoxi_camera::SetCoordinatesSpace::Request& req, + phoxi_camera::SetCoordinatesSpace::Response& res) { try { PhoXiInterface::setCoordinateSpace(req.coordinates_space); //update dynamic reconfigure @@ -611,41 +635,44 @@ namespace phoxi_camera{ dynamicReconfigureServer.updateConfig(dynamicReconfigureConfig); res.success = true; res.message = OKRESPONSE; - }catch (PhoXiInterfaceException &e){ + } catch (PhoXiInterfaceException& e) { res.success = false; res.message = e.what(); } return true; } - bool RosInterface::setTransformation(phoxi_camera::SetTransformationMatrix::Request &req, phoxi_camera::SetTransformationMatrix::Response &res){ + bool RosInterface::setTransformation(phoxi_camera::SetTransformationMatrix::Request& req, + phoxi_camera::SetTransformationMatrix::Response& res) { try { Eigen::Affine3d transform; - tf::transformMsgToEigen(req.transform,transform); - PhoXiInterface::setTransformation(transform.matrix(),req.coordinates_space,req.set_space,req.save_settings); + tf::transformMsgToEigen(req.transform, transform); + PhoXiInterface::setTransformation(transform.matrix(), req.coordinates_space, req.set_space, + req.save_settings); //update dynamic reconfigure - if(req.set_space) { + if (req.set_space) { dynamicReconfigureConfig.coordinate_space = req.coordinates_space; } dynamicReconfigureServer.updateConfig(dynamicReconfigureConfig); res.success = true; res.message = OKRESPONSE; - }catch (PhoXiInterfaceException &e){ + } catch (PhoXiInterfaceException& e) { res.success = false; res.message = e.what(); } return true; } - bool RosInterface::saveLastFrame(phoxi_camera::SaveLastFrame::Request &req, phoxi_camera::SaveLastFrame::Response &res) { + bool + RosInterface::saveLastFrame(phoxi_camera::SaveLastFrame::Request& req, phoxi_camera::SaveLastFrame::Response& res) { std::string file_path = req.file_path; try { // ~ error handling size_t pos = file_path.find('~'); - if(pos != std::string::npos){ + if (pos != std::string::npos) { char* home = std::getenv("HOME"); - if(!home){ + if (!home) { res.message = "'~' found in 'file_path' parameter but environment variable 'HOME' not found. Export' HOME' variable or pass absolute value to 'path' parameter."; res.success = false; return true; @@ -659,7 +686,7 @@ namespace phoxi_camera{ pos = file_path.rfind('.') + 1; std::string extension = file_path.substr(pos, file_path.length()); - for (const auto &ext: extensions) { + for (const auto& ext: extensions) { if (ext == extension) { correct_ext = true; } @@ -671,9 +698,9 @@ namespace phoxi_camera{ } // save last frame - ROS_INFO("File path: %s",file_path.c_str()); + ROS_INFO("File path: %s", file_path.c_str()); bool result = PhoXiInterface::saveLastFrame(file_path); - if (result){ + if (result) { res.message = OKRESPONSE; res.success = true; } else { @@ -681,15 +708,17 @@ namespace phoxi_camera{ res.success = false; } - } catch (PhoXiInterfaceException &e){ + } catch (PhoXiInterfaceException& e) { res.success = false; res.message = e.what(); } return true; } + #endif - void RosInterface::getDefaultDynamicReconfigureConfig(phoxi_camera::phoxi_cameraConfig& config){ + + void RosInterface::getDefaultDynamicReconfigureConfig(phoxi_camera::phoxi_cameraConfig& config) { dynamicReconfigureServer.getConfigDefault(config); config.__fromServer__(nh); } diff --git a/src/phoxi_camera_node.cpp b/src/phoxi_camera_node.cpp index 285802b..53029c3 100644 --- a/src/phoxi_camera_node.cpp +++ b/src/phoxi_camera_node.cpp @@ -30,7 +30,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -int main(int argc, char **argv) { +int main(int argc, char** argv) { ros::init(argc, argv, "phoxi_camera"); phoxi_camera::RosInterface interface;