/src/server/server.cpp - Diff - humotion - Research for Cognitive Interaction

Revision 0c8d22a5 src/server/server.cpp

     * Excellence Initiative.
     */
     #include "server/server.h"
     #include "server/middleware_ros.h"
     #include <boost/algorithm/string.hpp>
     #include <string>
     using namespace std;
     using namespace boost;
     using namespace humotion;
     using namespace humotion::server;
     #include "humotion/server/middleware_ros.h"
     #include "humotion/server/server.h"
     using humotion::server::Server;
     //! set up constant for updaterate (50Hz)
     const float Server::MOTION_UPDATERATE = 50.0;
     //! constructor
     //! open a new server instance.
     Server::Server(string scope, string mw, JointInterface *_joint_interface){
         //convert mw to uppercase:
         to_upper(mw);
     Server::Server(std::string scope, std::string mw, JointInterface *_joint_interface) {
         // convert mw to uppercase
         boost::to_upper(mw);
         printf("> initializing humotion server (on %s, middleware=%s)\n",scope.c_str(),mw.c_str());
         printf("> initializing humotion server (on %s, middleware=%s)\n", scope.c_str(), mw.c_str());
         //store pointer to joint interface
         // store pointer to joint interface
         joint_interface = _joint_interface;
         //tell joint interface our framerate:
         // tell joint interface our framerate
         joint_interface->set_framerate(MOTION_UPDATERATE);
         //create controller
         // create controller
         controller = new Controller(joint_interface);
         controller->init_motion_generators();
         //start middleware:
         if (mw == "ROS"){
         // start middleware
         if (mw == "ROS") {
             middleware = new MiddlewareROS(scope, controller);
         }else{
             printf("> ERROR: invalid mw '%s' given. RSB support was dropped. Please use ROS\n\n",mw.c_str());
         } else {
             printf("> ERROR: invalid mw '%s' given. RSB support was dropped. Please use ROS\n\n",
                    mw.c_str());
             exit(EXIT_FAILURE);
+        }
         //start motion generation thread:
         // start motion generation thread
         start_motion_generation_thread();
+    }
     //! destructor
     Server::~Server(){
     Server::~Server() {
+    }
     //! middleware still running?
     bool Server::ok(){
     bool Server::ok() {
         return middleware->ok();
+    }
     //! start main thread
     void Server::start_motion_generation_thread(){
         motion_generation_thread_ptr = new boost::thread(boost::bind( &Server::motion_generation_thread, this));
     void Server::start_motion_generation_thread() {
         motion_generation_thread_ptr = new boost::thread(boost::bind(
                                                              &Server::motion_generation_thread, this));
+    }
     //! main thread that handles all data in/out
     //! this thread will take care of gathering all input data and
     //! then generate the output targets at a fixed rate of \sa MOTION_UPDATERATE
     void Server::motion_generation_thread(){
     void Server::motion_generation_thread() {
         unsigned int incoming_data_count_invalid = 0;
         printf("> started motion generation thread\n");
         //calculate loop delay:
         // calculate loop delay
         float loop_delay = 1000.0 / MOTION_UPDATERATE;
         boost::system_time timeout = get_system_time() + posix_time::milliseconds(loop_delay);
         printf("> one loop = %3.1fms\n",loop_delay);
         boost::system_time timeout = boost::get_system_time() +
                                      boost::posix_time::milliseconds(loop_delay);
         printf("> one loop = %3.1fms\n", loop_delay);
         //wait for incoming joint data:
         while (middleware->ok()){
             //mw tick
         // wait for incoming joint data
         while (middleware->ok()) {
             // mw tick
             middleware->tick();
             unsigned int incoming_data_count = joint_interface->get_and_clear_incoming_position_count();
             if (incoming_data_count == 0){
             if (incoming_data_count == 0) {
                 incoming_data_count_invalid++;
                 if (incoming_data_count_invalid >= MOTION_UPDATERATE){
                 if (incoming_data_count_invalid >= MOTION_UPDATERATE) {
                     printf("> waiting for valid incoming joint data (position+velocity)\n");
                     incoming_data_count_invalid = 0;
+                }
             }else{
                 //fine, joint data is arriving, exit waiting loop
             } else {
                 // fine, joint data is arriving, exit waiting loop
                 break;
+            }
             thread::sleep(timeout);
             timeout = get_system_time() + posix_time::milliseconds(loop_delay);
             boost::thread::sleep(timeout);
             timeout = boost::get_system_time() + boost::posix_time::milliseconds(loop_delay);
+        }
         printf("> joint data arrived, control loop active.\n");
         controller->set_activated();
         //fine, data is arriving, activate and run control loop:
         while (middleware->ok()){
             //mw tick
         // fine, data is arriving, activate and run control loop
         while (middleware->ok()) {
             // mw tick
             middleware->tick();
             //do some logging:
             //controller->dump_angles();
             // do some logging
             // controller->dump_angles();
             //calculate all targets
             // calculate all targets
             controller->calculate_targets();
             //publish data to joints
             // publish data to joints
             controller->publish_targets();
             //finally execute the movement:
             // finally execute the movement
             joint_interface->execute_motion();
             //check if we received joint information in the last step:
             // check if we received joint information in the last step
             unsigned int incoming_data_count = joint_interface->get_and_clear_incoming_position_count();
             if (incoming_data_count == 0){
                 //printf("> WARNING: no incoming joint data during the last iteration...\n");
             if (incoming_data_count == 0) {
                 // printf("> WARNING: no incoming joint data during the last iteration...\n");
                 incoming_data_count_invalid++;
                 if (incoming_data_count_invalid >= MOTION_UPDATERATE){
                 if (incoming_data_count_invalid >= MOTION_UPDATERATE) {
                     printf("> ERROR: no incoming joint data for >1 second -> will exit now\n");
                     exit(EXIT_FAILURE);
+                }
             }else{
             } else {
                 incoming_data_count_invalid = 0;
+            }
             thread::sleep(timeout);
             timeout = get_system_time() + posix_time::milliseconds(loop_delay);
             boost::thread::sleep(timeout);
             timeout = boost::get_system_time() + boost::posix_time::milliseconds(loop_delay);
+        }
         printf("> motion generation thread exited.\n");

Also available in: Unified diff