From d567d7b1d679708edd4aae0d08151ffd6077562f Mon Sep 17 00:00:00 2001 From: Konstantinos Chatzilygeroudis Date: Sat, 17 May 2014 19:18:57 +0300 Subject: [PATCH] Separated nao_dcm_camera class from nao_dcm_camera node.. --- nao_dcm_apps/nao_dcm_camera/CMakeLists.txt | 2 +- .../nao_dcm_camera/src/nao_camera.cpp | 85 -------------- .../nao_dcm_camera/src/nao_camera_node.cpp | 109 ++++++++++++++++++ 3 files changed, 110 insertions(+), 86 deletions(-) create mode 100644 nao_dcm_apps/nao_dcm_camera/src/nao_camera_node.cpp diff --git a/nao_dcm_apps/nao_dcm_camera/CMakeLists.txt b/nao_dcm_apps/nao_dcm_camera/CMakeLists.txt index ef4a48f..8103c0c 100644 --- a/nao_dcm_apps/nao_dcm_camera/CMakeLists.txt +++ b/nao_dcm_apps/nao_dcm_camera/CMakeLists.txt @@ -36,7 +36,7 @@ if( NAOqi_FOUND AND Boost_FOUND) ${catkin_INCLUDE_DIRS} ${NAOqi_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) - add_executable(nao_dcm_camera src/nao_camera.cpp) + add_executable(nao_dcm_camera src/nao_camera_node.cpp src/nao_camera.cpp) target_link_libraries(nao_dcm_camera ${catkin_LIBRARIES} ${NAOqi_LIBRARIES} diff --git a/nao_dcm_apps/nao_dcm_camera/src/nao_camera.cpp b/nao_dcm_apps/nao_dcm_camera/src/nao_camera.cpp index 4df350b..f51dccf 100644 --- a/nao_dcm_apps/nao_dcm_camera/src/nao_camera.cpp +++ b/nao_dcm_apps/nao_dcm_camera/src/nao_camera.cpp @@ -22,8 +22,6 @@ OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISE #include #include "nao_dcm_camera/nao_camera.h" -#include -#include using std::vector; @@ -382,86 +380,3 @@ void NaoCamera::dynamicReconfigureCb(nao_dcm_camera::NaoDCMCameraConfig &config, reconfiguring_ = false; } - -int main( int argc, char** argv ) -{ - int pport = 9559; - string pip = "127.0.0.1"; - ros::init(argc, argv, "nao_dcm_camera"); - ros::NodeHandle n; - ros::NodeHandle n_p("~"); - ros::NodeHandle top("CameraTopNode"), bottom("CameraBottomNode"); - if(!ros::master::check()) - { - cerr<<"Could not contact master!\nQuitting... "< broker; - try - { - broker = AL::ALBroker::createBroker(broker_name,broker_ip,broker_port,pip,pport,0); - } - catch(...) - { - ROS_ERROR("Failed to connect to Broker at %s:%d!",pip.c_str(),pport); - AL::ALBrokerManager::getInstance()->removeBroker(broker); - return -1; - } - - // Deal with ALBrokerManager singleton (add your broker into NAOqi) - AL::ALBrokerManager::setInstance(broker->fBrokerManager.lock()); - AL::ALBrokerManager::getInstance()->addBroker(broker); - - // Now it's time to load your module - boost::shared_ptr nao_cam = AL::ALModule::createModule(broker, "NaoCamera"); - nao_cam->connect(n,top,bottom); - if(!nao_cam->connected()) - { - ROS_ERROR("Could not connect to Nao robot!"); - AL::ALBrokerManager::getInstance()->killAllBroker(); - AL::ALBrokerManager::kill(); - return -1; - } - ros::Rate rate(communication_rate); - // Run the spinner in a separate thread to prevent lockups - ros::AsyncSpinner spinner(1); - spinner.start(); - if(broker->isModulePresent("NaoCamera")) - ROS_INFO("NaoCamera Module loaded succesfully!"); - else - { - ROS_ERROR("NaoCamera Module is not loaded!"); - return -1; - } - while(ros::ok()) - { - nao_cam->loop(ros::Time::now(), rate.expectedCycleTime()); - if(!nao_cam->connected()) - { - ROS_WARN("NaoCamera unexpectedly disconnected!"); - break; - } - rate.sleep(); - } - AL::ALBrokerManager::getInstance()->killAllBroker(); - AL::ALBrokerManager::kill(); - spinner.stop(); - ROS_INFO( "Quitting... " ); - return 0; -} diff --git a/nao_dcm_apps/nao_dcm_camera/src/nao_camera_node.cpp b/nao_dcm_apps/nao_dcm_camera/src/nao_camera_node.cpp new file mode 100644 index 0000000..b82634f --- /dev/null +++ b/nao_dcm_apps/nao_dcm_camera/src/nao_camera_node.cpp @@ -0,0 +1,109 @@ +/** +Copyright (c) 2014, Konstantinos Chatzilygeroudis +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer + in the documentation and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived + from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, +BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT +SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE +OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +**/ + +#include +#include "nao_dcm_camera/nao_camera.h" +#include +#include + +int main( int argc, char** argv ) +{ + int pport = 9559; + string pip = "127.0.0.1"; + ros::init(argc, argv, "nao_dcm_camera"); + ros::NodeHandle n; + ros::NodeHandle n_p("~"); + ros::NodeHandle top("CameraTopNode"), bottom("CameraBottomNode"); + if(!ros::master::check()) + { + cerr<<"Could not contact master!\nQuitting... "< broker; + try + { + broker = AL::ALBroker::createBroker(broker_name,broker_ip,broker_port,pip,pport,0); + } + catch(...) + { + ROS_ERROR("Failed to connect to Broker at %s:%d!",pip.c_str(),pport); + AL::ALBrokerManager::getInstance()->removeBroker(broker); + return -1; + } + + // Deal with ALBrokerManager singleton (add your broker into NAOqi) + AL::ALBrokerManager::setInstance(broker->fBrokerManager.lock()); + AL::ALBrokerManager::getInstance()->addBroker(broker); + + // Now it's time to load your module + boost::shared_ptr nao_cam = AL::ALModule::createModule(broker, "NaoCamera"); + nao_cam->connect(n,top,bottom); + if(!nao_cam->connected()) + { + ROS_ERROR("Could not connect to Nao robot!"); + AL::ALBrokerManager::getInstance()->killAllBroker(); + AL::ALBrokerManager::kill(); + return -1; + } + ros::Rate rate(communication_rate); + // Run the spinner in a separate thread to prevent lockups + ros::AsyncSpinner spinner(1); + spinner.start(); + if(broker->isModulePresent("NaoCamera")) + ROS_INFO("NaoCamera Module loaded succesfully!"); + else + { + ROS_ERROR("NaoCamera Module is not loaded!"); + return -1; + } + while(ros::ok()) + { + nao_cam->loop(ros::Time::now(), rate.expectedCycleTime()); + if(!nao_cam->connected()) + { + ROS_WARN("NaoCamera unexpectedly disconnected!"); + break; + } + rate.sleep(); + } + AL::ALBrokerManager::getInstance()->killAllBroker(); + AL::ALBrokerManager::kill(); + spinner.stop(); + ROS_INFO( "Quitting... " ); + return 0; +}