sweep_laser_scan_cli_client.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009 Dejan Pangercic <pangercic -=- cs.tum.edu>
00003  *
00004  * All rights reserved.
00005  *
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are met:
00008  *
00009  *     * Redistributions of source code must retain the above copyright
00010  *       notice, this list of conditions and the following disclaimer.
00011  *     * Redistributions in binary form must reproduce the above copyright
00012  *       notice, this list of conditions and the following disclaimer in the
00013  *       documentation and/or other materials provided with the distribution.
00014  *
00015  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00016  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00017  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00018  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00019  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00020  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00021  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00022  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00023  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00024  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00025  * POSSIBILITY OF SUCH DAMAGE.
00026  *
00027  * $Id: sweep_laser_scan_cli_client.cpp 121 2009-08-28 12:46:27Z dejanpan $
00028  *
00029  */
00030 
00046 
00047 //ros includes
00049 #include "ros/ros.h"
00050 #include <mapping_srvs/TriggerSweep.h>
00051 using namespace mapping_srvs;
00052 
00054 //player includes
00056 #include <ncurses.h>
00057 #include <playerc.h>
00058 #include <string>
00059 #include <math.h>
00060 #include <stdio.h>
00061 #include <cstdlib>
00062 #include <getopt.h>
00063 #include <iostream>
00064 using namespace std;
00065 
00067 //player defines
00069 // Convert radians to degrees
00070 #define RAD2DEG(r) (float)((r) * 180 / M_PI)
00071 // Convert degrees to radians
00072 #define DEG2RAD(d) (float)((d) * M_PI / 180)
00073 #define HOSTNAME "localhost"
00074 // what diference between angles is considered 0?
00075 #define EPS 0.01
00076 
00077 
00078 class Amtec_Sweep_service{
00079 
00080 private:
00081   ros::NodeHandle nh_;
00082 public:
00083   ros::ServiceServer service_;
00084   //set options to some impossible values
00085   string object_;
00086   double angle_filename_, start_angle_, end_angle_;
00087   int interface_, rot_joint_, is_lms400_;
00088   double rot_speed_;
00089   bool debug_output_;
00091   //Contructor
00093   Amtec_Sweep_service()
00094   {
00095     nh_.param("~start_angle", start_angle_, 0.0);
00096     nh_.param("~end_angle", end_angle_, 10.0);
00097     nh_.param("~interface", interface_, 1);
00098     //are we scanning using lms400 and player logging
00099     nh_.param("~is_lms400_", is_lms400_, 0);
00100     nh_.param("~rot_joint", rot_joint_, 4);
00101     nh_.param("~rot_speed", rot_speed_, 0.1);
00102     service_ = nh_.advertiseService("amtec_sweep", &Amtec_Sweep_service::amtec_sweep_main, this);  
00103     debug_output_ = false;
00104     if(debug_output_)
00105       ROS_INFO("Amtec_Sweep_service has been initialized");  
00106   }
00107 
00109   //Destructor
00111   virtual ~Amtec_Sweep_service () { }
00112 
00114   //service function, used to be main as player client
00116 
00117   bool
00118   amtec_sweep_main (TriggerSweep::Request &req, TriggerSweep::Response &resp)
00119   {
00120     //update parameters on the fly
00121     update_parameters_from_server();
00122 
00123     if(debug_output_)
00124       ROS_INFO("In amtec_sweep_main");
00125     angle_filename_ = req.angle_filename;
00126     object_ = req.object;
00127         
00128     playerc_client_t *client;
00129     void *rdevice;
00130     int port = 6665;
00131     playerc_actarray_t *device_actarray;
00132     playerc_laser_t *device_laser;
00133     playerc_log_t *device_log;
00134     client = playerc_client_create (NULL, HOSTNAME, port);
00135     playerc_client_connect (client);
00136     
00137     //return from go_to_pose
00138     float next_pos;
00139     
00140     device_actarray  = playerc_actarray_create (client, interface_);
00141     device_laser  = playerc_laser_create (client, 0);
00142     if(is_lms400_)
00143       device_log = playerc_log_create      (client, 0);
00144     
00145     playerc_actarray_subscribe (device_actarray, PLAYER_OPEN_MODE);
00146     if(is_lms400_)
00147       playerc_log_subscribe      (device_log, PLAYER_OPEN_MODE);
00148     playerc_laser_subscribe      (device_laser, PLAYER_OPEN_MODE);
00149     
00151     
00152     for (int t = 0; t < 20; t++)
00153       rdevice = playerc_client_read (client);
00154     
00155     int current_actuator = device_actarray->actuators_count - 1;
00156     
00157     rdevice = playerc_client_read (client);
00158     //print_data (device_actarray);
00159     
00160     //enable logging
00161     if(is_lms400_)
00162       {
00163         char angle_tmp[100];
00164         int angle_int = round(req.angle_filename);
00165         sprintf (angle_tmp, "%d",  angle_int);
00166         string logFileName = req.object + "_" +  string(angle_tmp) + "_" + ".log";
00167         playerc_log_set_filename (device_log, logFileName.c_str());
00168         playerc_log_set_write_state (device_log, 1);
00169         if(debug_output_)
00170           ROS_INFO("Logging : enabled"); 
00171       }
00172     //sweeping
00173     next_pos = go_to_pose(client, device_actarray, rot_joint_, start_angle_, end_angle_);
00174     if(debug_output_)
00175       ROS_INFO("Sweeping to next_pos: %f", next_pos);
00176     sweep (client, device_actarray, rot_joint_, next_pos, rot_speed_);
00177 
00178     //wait until end angle reached
00179     while (fabs(next_pos  -   RAD2DEG (device_actarray->actuators_data[rot_joint_].position)) > EPS)
00180       {
00181         playerc_client_read (client);
00182         //printf ("Angle  %f\n", fabs(next_pos  -   RAD2DEG (device_actarray->actuators_data[rot_joint_].position)));
00183       }
00184     
00185     //logging disabled
00186     if(is_lms400_)
00187       {
00188         playerc_log_set_write_state (device_log, 0);
00189         if(debug_output_)
00190           ROS_INFO ("Logging : disabled"); 
00191       }
00192     //clean up player
00193     if(debug_output_)
00194       ROS_INFO("unsubscribing\n");
00195     playerc_actarray_unsubscribe (device_actarray);
00196     playerc_actarray_destroy (device_actarray);
00197     
00198     if(is_lms400_)
00199       {
00200         playerc_log_unsubscribe (device_log);
00201         playerc_log_destroy (device_log);
00202       }
00203 
00204     playerc_laser_unsubscribe (device_laser);
00205     playerc_laser_destroy (device_laser);
00206     
00207     return true;
00208   }
00209 
00211   // sweep one cube
00213   void
00214   sweep (playerc_client_t *client, playerc_actarray_t *device, 
00215          int actuator, float next_pos, float speed)
00216   {
00217     // Set the speed to move with
00218     playerc_actarray_speed_config (device, actuator, speed);
00219     if(debug_output_)
00220       ROS_INFO ("Sweeping. Next position is %f \n", next_pos);
00221     playerc_actarray_position_cmd (device, actuator, DEG2RAD (next_pos));
00222     
00223   }
00224 
00226   // find the start_angle
00228   float
00229   go_to_pose (playerc_client_t *client, playerc_actarray_t *device, 
00230               int actuator, float min_angle, float max_angle)
00231   {
00232     float next_pos;
00233     
00234     // Find the current position
00235     float current_pos = RAD2DEG (device->actuators_data[actuator].position);
00236     
00237     // Go to either min_angle or max_angle depending which one is closer
00238     if (current_pos < min_angle)
00239       next_pos = min_angle;
00240     
00241     if (current_pos > max_angle)
00242       next_pos = max_angle;
00243     
00244     if (fabs (current_pos - min_angle) > fabs (current_pos - max_angle))
00245       next_pos = max_angle;
00246     else
00247       next_pos = min_angle;
00248     if(debug_output_)
00249       ROS_INFO ("Init sweeping to: %f (dist to min: %f, dist to max:  %f) \n", 
00250                 next_pos, fabs (current_pos - min_angle), fabs (current_pos - max_angle));
00251     
00252     playerc_actarray_position_cmd (device, actuator, DEG2RAD (next_pos));
00253     // Loop until the actuator is idle-ing or break-ed.
00254     do {
00255       playerc_client_read (client);
00256       //print_data (device);
00257       current_pos = RAD2DEG (device->actuators_data[actuator].position);
00258     }  
00259     while ( (fabs (current_pos - next_pos) > EPS) || (
00260                                                       (device->actuators_data[actuator].state != PLAYER_ACTARRAY_ACTSTATE_IDLE) &&
00261                                                       (device->actuators_data[actuator].state != PLAYER_ACTARRAY_ACTSTATE_BRAKED))
00262             );
00263     
00264     if (next_pos == max_angle)
00265       next_pos = min_angle;
00266     else
00267       next_pos = max_angle;
00268     
00269     return next_pos;
00270   }
00271 
00273   // //print data to stdio
00275   void
00276   print_data (playerc_actarray_t *device)
00277   {
00278     int i;
00279     for (i = 0; i < device->actuators_count; i++)
00280       {
00281         printf ("X%d> pos, speed, accel, cur, state  : [%f, %f, %f, %f, %d]\n", (i+1), 
00282                 RAD2DEG (device->actuators_data[i].position), 
00283                 device->actuators_data[i].speed, 
00284                 device->actuators_data[i].acceleration, 
00285                 device->actuators_data[i].current, 
00286                 device->actuators_data[i].state);
00287       }
00288   }
00289 
00291   // //update parameters on the fly
00293   void
00294   update_parameters_from_server()
00295   {
00296     if (nh_.hasParam("~rot_speed"))
00297       nh_.getParam("~rot_speed", rot_speed_);
00298     if (nh_.hasParam("~rot_joint_"))
00299       nh_.getParam("~rot_joint_", rot_joint_);
00300   }
00301     
00302   //class Amtec_Sweep_service ends here  
00303 };
00304 
00305   
00306 
00307 int main(int argc, char **argv)
00308 {
00309   //ROS
00310   ros::init(argc, argv, "amtec_sweep_service_server");
00311   Amtec_Sweep_service p;    
00312   ros::spin();
00313   return 0;
00314 }
00315 


player_log_actarray
Author(s): Radu Bogdan Rusu
autogenerated on Mon Oct 6 2014 09:38:35