00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00046
00047
00049 #include "ros/ros.h"
00050 #include <mapping_srvs/TriggerSweep.h>
00051 using namespace mapping_srvs;
00052
00054
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
00069
00070 #define RAD2DEG(r) (float)((r) * 180 / M_PI)
00071
00072 #define DEG2RAD(d) (float)((d) * M_PI / 180)
00073 #define HOSTNAME "localhost"
00074
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
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
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
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
00111 virtual ~Amtec_Sweep_service () { }
00112
00114
00116
00117 bool
00118 amtec_sweep_main (TriggerSweep::Request &req, TriggerSweep::Response &resp)
00119 {
00120
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
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
00159
00160
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
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
00179 while (fabs(next_pos - RAD2DEG (device_actarray->actuators_data[rot_joint_].position)) > EPS)
00180 {
00181 playerc_client_read (client);
00182
00183 }
00184
00185
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
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
00213 void
00214 sweep (playerc_client_t *client, playerc_actarray_t *device,
00215 int actuator, float next_pos, float speed)
00216 {
00217
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
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
00235 float current_pos = RAD2DEG (device->actuators_data[actuator].position);
00236
00237
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
00254 do {
00255 playerc_client_read (client);
00256
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
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
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
00303 };
00304
00305
00306
00307 int main(int argc, char **argv)
00308 {
00309
00310 ros::init(argc, argv, "amtec_sweep_service_server");
00311 Amtec_Sweep_service p;
00312 ros::spin();
00313 return 0;
00314 }
00315