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
00034 #include "ros/ros.h"
00035 #include "std_msgs/String.h"
00036 #include <sstream>
00037 #include <string>
00038 #include <iostream>
00039 #include <math.h>
00040 #include <cmath>
00041 #include <vector>
00042
00043 #include <visualization_msgs/Marker.h>
00044 #include <tf/transform_broadcaster.h>
00045 #include <tf/transform_listener.h>
00046 #include <tf/transform_datatypes.h>
00047 #include <boost/format.hpp>
00048
00049 #include <imu_network/filtered_imu.h>
00050 #include <imu_network/filtered_imu_network.h>
00051
00052 #include <kfilter/ekfilter.hpp>
00053 #include <kfilter/kvector.hpp>
00054
00055
00056 #include <stdlib.h>
00057 #include <cstdlib>
00058 #include <cfloat>
00059 #include <float.h>
00060 #include <assert.h>
00061
00062
00063 #define MAX_MEAN_IT 64 //number of iterations to measure the mean values from gyroscopes
00064
00065 #define GYRO_CONVERT M_PI/180*0.069565 // conversion of gyro's values from ADC to rad/sec
00066 #define DEGtoRAD M_PI/180
00067
00068 using namespace std;
00069
00070 ros::NodeHandle* n;
00071
00072 tf::TransformBroadcaster* br;
00073 tf::TransformListener* listener;
00074
00075 ros::Publisher chatter_pub;
00076 ros::Publisher vis_pub;
00077
00078
00079 void publish_accel(int n, float ax , float ay , float az);
00080 void publish_gyro(int n, float gx , float gy , float gz);
00081 void publish_magn(int n, float mx , float my , float mz);
00082 void publish_sensor_type(void);
00083 void publish_sensor_number(int n);
00084
00085 float get_theta(float iteration1, float iteration2,float time_interval);
00086 float get_magnitude(float x, float y, float z);
00087 float get_angle(float num, float denom);
00088
00089
00090 int sensors_number;
00091 int first_run=0;
00092 int marker_id;
00093 int mean_it = 0;
00094 tf::Transform transform1;
00095 tf::Transform transform_tmp;
00096
00097 vector<ros::Time>time_stamp;
00098 vector<float>gyro_valx;
00099 vector<float>gyro_valy;
00100 vector<float>gyro_valz;
00101
00102 vector<float>gyro_mean_vect[MAX_MEAN_IT][3];
00103 vector<float>gyro_real_mean[3];
00104
00105 int sensors_pos_y[9] = {0 , 5 , 30 , 35 , 8.75 , 8.75 , 26.25 , 26.25 , 17.5};
00106 int sensors_pos_z[9] = {0 , 5 , 5 , 0 , -20 , -35 , -20 , -35 , 15 };
00107 tf::Matrix3x3 imu_transform[9];
00108 tf::Matrix3x3 imu_transform_result;
00109
00110 void chatterCallback(const imu_network::filtered_imu_network::ConstPtr& msg)
00111 {
00112 double RPY_orient[3];
00113 double RPY_init[3];
00114 if(first_run ==0)
00115 {
00116 imu_transform[0].setValue(1,0,0,0,0,1,0,-1,0);
00117 imu_transform[1].setValue(1,0,0,0,1,0,0,0,1);
00118 imu_transform[2].setValue(1,0,0,0,1,0,0,0,1);
00119 imu_transform[3].setValue(1,0,0,0,0,1,0,-1,0);
00120 imu_transform[4].setValue(0,1,0,0,0,1,1,0,0);
00121 imu_transform[5].setValue(0,1,0,0,0,1,1,0,0);
00122 imu_transform[6].setValue(0,-1,0,0,0,-1,1,0,0);
00123 imu_transform[7].setValue(0,-1,0,0,0,-1,1,0,0);
00124 imu_transform[8].setValue(-1,0,0,0,0,-1,0,-1,0);
00125 sensors_number=(int)msg->filtered_imu_network[0].total_number;
00126
00127 for (int i=0;i<sensors_number;i++)
00128 {
00129
00130
00131 string sensor_base = str( boost::format("/sensor_base_%d") % i );
00132 string sensor = str( boost::format("/sensor_%d") % i );
00133
00134 transform1.setOrigin( tf::Vector3(0.0, sensors_pos_y[i], sensors_pos_z[i]) );
00135 transform1.setRotation( tf::createQuaternionFromRPY(0,0,0) );
00136 br->sendTransform(tf::StampedTransform(transform1, ros::Time::now(), "/world", sensor_base));
00137
00138 imu_transform[i].getRPY(RPY_init[0],RPY_init[1],RPY_init[2]);
00139
00140 transform_tmp.setOrigin( tf::Vector3(0.0,0.0,0.0) );
00141 transform_tmp.setRotation(tf::createQuaternionFromRPY(RPY_orient[0],RPY_orient[1],RPY_orient[2]) );
00142 br->sendTransform(tf::StampedTransform(transform_tmp, ros::Time::now()-ros::Duration(5),sensor_base, sensor));
00143
00144 gyro_valx.push_back(msg->filtered_imu_network[i].angular_velocity_x);
00145 gyro_valy.push_back(msg->filtered_imu_network[i].angular_velocity_y);
00146 gyro_valz.push_back(msg->filtered_imu_network[i].angular_velocity_z);
00147
00148 cout<<gyro_valx[i]<<" "<<gyro_valy[i]<<" "<<gyro_valz[i]<<endl;
00149
00150 }
00151 first_run = 1 ;
00152 return;
00153 }
00154
00155 float sensor_var[sensors_number][13];
00156
00157 marker_id = 0;
00158
00159 for(int i = 0;i<sensors_number;i++)
00160 {
00161
00162 sensor_var[i][0]=msg->filtered_imu_network[i].linear_acceleration_x;
00163 sensor_var[i][1]=msg->filtered_imu_network[i].linear_acceleration_y;
00164 sensor_var[i][2]=msg->filtered_imu_network[i].linear_acceleration_z;
00165 sensor_var[i][3]=msg->filtered_imu_network[i].angular_velocity_x;
00166 sensor_var[i][4]=msg->filtered_imu_network[i].angular_velocity_y;
00167 sensor_var[i][5]=msg->filtered_imu_network[i].angular_velocity_z;
00168 sensor_var[i][6]=msg->filtered_imu_network[i].angular_displacement_x;
00169 sensor_var[i][7]=msg->filtered_imu_network[i].angular_displacement_y;
00170 sensor_var[i][8]=msg->filtered_imu_network[i].angular_displacement_z;
00171 sensor_var[i][9]=msg->filtered_imu_network[i].magnetometer_x;
00172 sensor_var[i][10]=msg->filtered_imu_network[i].magnetometer_y;
00173 sensor_var[i][11]=msg->filtered_imu_network[i].magnetometer_z;
00174 sensor_var[i][12]=msg->filtered_imu_network[i].number;
00175
00176
00177
00178 string sensor_base = str( boost::format("/sensor_base_%d") % i );
00179 string sensor = str( boost::format("/sensor_%d") % i );
00180
00181
00182 transform1.setOrigin( tf::Vector3(0.0, sensors_pos_y[i], sensors_pos_z[i]) );
00183 transform1.setRotation( tf::createQuaternionFromRPY(0,0,0) );
00184 br->sendTransform(tf::StampedTransform(transform1, ros::Time::now(), "/world", sensor_base));
00185
00186
00187 tf::StampedTransform transform_new;
00188 tf::StampedTransform transform_sensors;
00189 try
00190 {
00191 float delta_thetax = 0;
00192 float delta_thetay = 0;
00193 float delta_thetaz = 0;
00194
00195 listener->lookupTransform(sensor_base, sensor, ros::Time(0), transform_new);
00196 tf::Quaternion q;
00197 q=transform_new.getRotation();
00198 tf::Matrix3x3(q).getRPY(RPY_orient[0],RPY_orient[1],RPY_orient[2]);
00199
00200
00201 delta_thetax = get_theta(gyro_valx[i],sensor_var[i][3],0.025);
00202 delta_thetay = get_theta(gyro_valy[i],sensor_var[i][4],0.025);
00203 delta_thetaz = get_theta(gyro_valz[i],sensor_var[i][5],0.025);
00204
00205 if (msg->filtered_imu_network[i].algorithm == 0)
00206 {
00207 ROS_INFO("KALMAN_FILTER");
00208 transform_tmp.setRotation( tf::createQuaternionFromRPY(RPY_orient[0]+delta_thetax,RPY_orient[1]+delta_thetay,RPY_orient[2]+delta_thetaz));
00209 transform_tmp.setOrigin(tf::Vector3(0,0,0));
00210 br->sendTransform(tf::StampedTransform(transform_tmp, ros::Time::now(), sensor_base, sensor));
00211
00212 gyro_valx[i] = sensor_var[i][3];
00213 gyro_valy[i] = sensor_var[i][4];
00214 gyro_valz[i] = sensor_var[i][5];
00215 }
00216 if (msg->filtered_imu_network[i].algorithm == 1)
00217 {
00218 ROS_INFO("COMPLEMENTARY_FILTER");
00219
00220 tf::Quaternion quat = tf::createQuaternionFromRPY(sensor_var[i][6],sensor_var[i][7],sensor_var[i][8]);
00221 imu_transform_result = tf::Matrix3x3(quat) * imu_transform[i].inverse();
00222 imu_transform_result.getRPY(RPY_init[0],RPY_init[1],RPY_init[2]);
00223 transform_tmp.setRotation(tf::createQuaternionFromRPY(RPY_init[0],RPY_init[1],RPY_init[2]));
00224
00225 transform_tmp.setRotation(tf::createQuaternionFromRPY(sensor_var[i][6],sensor_var[i][7],sensor_var[i][8]) * tf::createQuaternionFromRPY(RPY_init[0],RPY_init[1],RPY_init[2]).inverse());
00226
00227 transform_tmp.setOrigin(tf::Vector3(0,0,0));
00228
00229 br->sendTransform(tf::StampedTransform(transform_tmp, ros::Time::now(), sensor_base, sensor));
00230
00231 }
00232
00233
00234 }
00235 catch (tf::TransformException ex)
00236 {
00237 ROS_ERROR("%s",ex.what());
00238 }
00239
00240 publish_accel(i,sensor_var[i][0],sensor_var[i][1],sensor_var[i][2]);
00241 publish_gyro(i,sensor_var[i][3],sensor_var[i][4],sensor_var[i][5]);
00242 publish_magn(i,sensor_var[i][9],sensor_var[i][10],sensor_var[i][11]);
00243
00244 publish_sensor_type();
00245 publish_sensor_number(i);
00246 }
00247
00248 }
00249 float get_angle(float num, float denom)
00250 {
00251 if (atan2(num,denom)!=NAN)
00252 return atan2(num,denom);
00253 else
00254 return 0;
00255 }
00256
00257 float get_magnitude(float x, float y, float z)
00258 {
00259 return sqrt(x*x+y*y+z*z);
00260 }
00261 float get_theta(float iteration1, float iteration2,float time_interval)
00262 {
00263 float ret;
00264 float alpha = 0.5;
00265
00266 ret = (iteration2-iteration1)*time_interval;
00267
00268 return ret;
00269 }
00270
00271 void publish_accel(int n, float ax , float ay , float az)
00272 {
00273 int x_pos = -10;
00274 int y_pos = 10*n;
00275
00276 marker_id++;
00277 visualization_msgs::Marker marker;
00278 marker.header.frame_id = "/world";
00279 marker.header.stamp = ros::Time::now();
00280 marker.ns = "my_namespace";
00281 marker.id =marker_id;
00282 marker.type = visualization_msgs::Marker::CUBE;
00283 marker.action = visualization_msgs::Marker::ADD;
00284 marker.pose.position.x = x_pos;
00285 marker.pose.position.y = sensors_pos_y[n];
00286 marker.pose.position.z = sensors_pos_z[n];
00287 marker.pose.orientation.x = 1.0;
00288 marker.pose.orientation.y = 0.0;
00289 marker.pose.orientation.z = 0.0;
00290 marker.pose.orientation.w = 0.0;
00291 marker.scale.x = 2.0;
00292 marker.scale.y = 2.0;
00293 marker.scale.z = 2.0;
00294 marker.color.a = 0.7;
00295 marker.color.r = 0.2;
00296 marker.color.g = 0.2;
00297 marker.color.b = 0.2;
00298
00299 marker_id++;
00300 visualization_msgs::Marker marker2;
00301 marker2.header.frame_id = "/world";
00302 marker2.header.stamp = ros::Time::now();
00303 marker2.ns = "my_namespace";
00304 marker2.id =marker_id;
00305 marker2.type = visualization_msgs::Marker::ARROW;
00306 marker2.action = visualization_msgs::Marker::ADD;
00307 marker2.pose.position.x = x_pos;
00308 marker2.pose.position.y = sensors_pos_y[n];
00309 marker2.pose.position.z = sensors_pos_z[n];
00310 marker2.pose.orientation.x = 1.0;
00311 marker2.pose.orientation.y = 0.0;
00312 marker2.pose.orientation.z = 0.0;
00313 marker2.pose.orientation.w = 0.0;
00314 marker2.scale.x = 3.0;
00315 marker2.scale.y = 5.0;
00316 marker2.scale.z = 18.0*ax/2048+1;
00317 marker2.color.a = 1.0;
00318 marker2.color.r = 1.0;
00319 marker2.color.g = 0.0;
00320 marker2.color.b = 0.0;
00321
00322 marker_id++;
00323 visualization_msgs::Marker marker3;
00324 marker3.header.frame_id = "/world";
00325 marker3.header.stamp = ros::Time::now();
00326 marker3.ns = "my_namespace";
00327 marker3.id =marker_id;
00328 marker3.type = visualization_msgs::Marker::ARROW;
00329 marker3.action = visualization_msgs::Marker::ADD;
00330 marker3.pose.position.x = x_pos;
00331 marker3.pose.position.y = sensors_pos_y[n];
00332 marker3.pose.position.z = sensors_pos_z[n];
00333 marker3.pose.orientation.x = 1.0;
00334 marker3.pose.orientation.y = 1.0;
00335 marker3.pose.orientation.z = 0.0;
00336 marker3.pose.orientation.w = 0.0;
00337 marker3.scale.x = 3.0;
00338 marker3.scale.y = 3.0;
00339 marker3.scale.z = 18.0*ay/2048+1;
00340 marker3.color.a = 1.0;
00341 marker3.color.r = 0.0;
00342 marker3.color.g = 1.0;
00343 marker3.color.b = 0.0;
00344
00345 marker_id++;
00346 visualization_msgs::Marker marker4;
00347 marker4.header.frame_id = "/world";
00348 marker4.header.stamp = ros::Time::now();
00349 marker4.ns = "my_namespace";
00350 marker4.id =marker_id;
00351 marker4.type = visualization_msgs::Marker::ARROW;
00352 marker4.action = visualization_msgs::Marker::ADD;
00353 marker4.pose.position.x = x_pos;
00354 marker4.pose.position.y = sensors_pos_y[n];
00355 marker4.pose.position.z = sensors_pos_z[n];
00356 marker4.pose.orientation.x = 1.0;
00357 marker4.pose.orientation.y = 0.0;
00358 marker4.pose.orientation.z = 1.0;
00359 marker4.pose.orientation.w = 0.0;
00360 marker4.scale.x = 3.0;
00361 marker4.scale.y = 3.0;
00362 marker4.scale.z = 18.0*az/2048+1;
00363 marker4.color.a = 1.0;
00364 marker4.color.r = 0.0;
00365 marker4.color.g = 0.0;
00366 marker4.color.b = 1.0;
00367
00368 vis_pub.publish(marker);
00369 vis_pub.publish( marker2 );
00370 vis_pub.publish( marker3 );
00371 vis_pub.publish( marker4 );
00372 }
00373
00374 void publish_gyro(int n, float gx , float gy , float gz)
00375 {
00376 int x_pos = 0;
00377
00378 string sensor = str( boost::format("/sensor_%d") % n );
00379
00380 marker_id++;
00381 visualization_msgs::Marker marker;
00382 marker.header.frame_id = sensor;
00383 marker.header.stamp = ros::Time::now();
00384 marker.ns = "my_namespace";
00385 marker.id =marker_id;
00386 marker.type = visualization_msgs::Marker::CUBE;
00387 marker.action = visualization_msgs::Marker::ADD;
00388 marker.pose.position.x = x_pos;
00389 marker.pose.position.y = 0;
00390 marker.pose.position.z = 0;
00391 marker.pose.orientation.x = 1.0;
00392 marker.pose.orientation.y = 0.0;
00393 marker.pose.orientation.z = 0.0;
00394 marker.pose.orientation.w = 0.0;
00395 marker.scale.x = 3.0;
00396 marker.scale.y = 3.0;
00397 marker.scale.z = 0.6;
00398 marker.color.a = 0.7;
00399 marker.color.r = 0.2;
00400 marker.color.g = 0.2;
00401 marker.color.b = 0.2;
00402
00403 marker_id++;
00404 visualization_msgs::Marker marker2;
00405 marker2.header.frame_id = sensor;
00406 marker2.header.stamp = ros::Time::now();
00407 marker2.ns = "my_namespace";
00408 marker2.id =marker_id;
00409 marker2.type = visualization_msgs::Marker::ARROW;
00410 marker2.action = visualization_msgs::Marker::ADD;
00411 marker2.pose.position.x = x_pos;
00412 marker2.pose.position.y = 0;
00413 marker2.pose.position.z = 0;
00414 marker2.pose.orientation.x = 1.0;
00415 marker2.pose.orientation.y = 0.0;
00416 marker2.pose.orientation.z = 0.0;
00417 marker2.pose.orientation.w = 0.0;
00418 marker2.scale.x = 3.0;
00419 marker2.scale.y = 5.0;
00420 marker2.scale.z = 20.0*gx/10000+1;
00421 marker2.color.a = 1.0;
00422 marker2.color.r = 1.0;
00423 marker2.color.g = 0.0;
00424 marker2.color.b = 0.0;
00425
00426 marker_id++;
00427 visualization_msgs::Marker marker3;
00428 marker3.header.frame_id = sensor;
00429 marker3.header.stamp = ros::Time::now();
00430 marker3.ns = "my_namespace";
00431 marker3.id =marker_id;
00432 marker3.type = visualization_msgs::Marker::ARROW;
00433 marker3.action = visualization_msgs::Marker::ADD;
00434 marker3.pose.position.x = x_pos;
00435 marker3.pose.position.y = 0;
00436 marker3.pose.position.z = 0;
00437 marker3.pose.orientation.x = 1.0;
00438 marker3.pose.orientation.y = 1.0;
00439 marker3.pose.orientation.z = 0.0;
00440 marker3.pose.orientation.w = 0.0;
00441 marker3.scale.x = 3.0;
00442 marker3.scale.y = 5.0;
00443 marker3.scale.z = 20.0*gy/10000+1;
00444 marker3.color.a = 1.0;
00445 marker3.color.r = 0.0;
00446 marker3.color.g = 1.0;
00447 marker3.color.b = 0.0;
00448
00449 marker_id++;
00450 visualization_msgs::Marker marker4;
00451 marker4.header.frame_id = sensor;
00452 marker4.header.stamp = ros::Time::now();
00453 marker4.ns = "my_namespace";
00454 marker4.id =marker_id;
00455 marker4.type = visualization_msgs::Marker::ARROW;
00456 marker4.action = visualization_msgs::Marker::ADD;
00457 marker4.pose.position.x = x_pos;
00458 marker4.pose.position.y = 0;
00459 marker4.pose.position.z = 0;
00460 marker4.pose.orientation.x = 1.0;
00461 marker4.pose.orientation.y = 0.0;
00462 marker4.pose.orientation.z = 1.0;
00463 marker4.pose.orientation.w = 0.0;
00464 marker4.scale.x = 3.0;
00465 marker4.scale.y = 5.0;
00466 marker4.scale.z = 20.0*gz/10000+1;
00467 marker4.color.a = 1.0;
00468 marker4.color.r = 0.0;
00469 marker4.color.g = 0.0;
00470 marker4.color.b = 1.0;
00471
00472 vis_pub.publish(marker);
00473 vis_pub.publish( marker2 );
00474 vis_pub.publish( marker3 );
00475 vis_pub.publish( marker4 );
00476 }
00477
00478 void publish_magn(int n, float mx , float my , float mz)
00479 {
00480 int x_pos = 10;
00481 int y_pos = 10*n;
00482
00483 marker_id++;
00484 visualization_msgs::Marker marker;
00485 marker.header.frame_id = "/world";
00486 marker.header.stamp = ros::Time::now();
00487 marker.ns = "my_namespace";
00488 marker.id =marker_id;
00489 marker.type = visualization_msgs::Marker::CUBE;
00490 marker.action = visualization_msgs::Marker::ADD;
00491 marker.pose.position.x = x_pos;
00492 marker.pose.position.y = sensors_pos_y[n];
00493 marker.pose.position.z = sensors_pos_z[n];
00494 marker.pose.orientation.x = 1.0;
00495 marker.pose.orientation.y = 0.0;
00496 marker.pose.orientation.z = 0.0;
00497 marker.pose.orientation.w = 0.0;
00498 marker.scale.x = 2.0;
00499 marker.scale.y = 2.0;
00500 marker.scale.z = 2.0;
00501 marker.color.a = 0.7;
00502 marker.color.r = 0.2;
00503 marker.color.g = 0.2;
00504 marker.color.b = 0.2;
00505
00506 marker_id++;
00507 visualization_msgs::Marker marker2;
00508 marker2.header.frame_id = "/world";
00509 marker2.header.stamp = ros::Time::now();
00510 marker2.ns = "my_namespace";
00511 marker2.id =marker_id;
00512 marker2.type = visualization_msgs::Marker::ARROW;
00513 marker2.action = visualization_msgs::Marker::ADD;
00514 marker2.pose.position.x = x_pos;
00515 marker2.pose.position.y = sensors_pos_y[n];
00516 marker2.pose.position.z = sensors_pos_z[n];
00517 marker2.pose.orientation.x = 1.0;
00518 marker2.pose.orientation.y = 0.0;
00519 marker2.pose.orientation.z = 0.0;
00520 marker2.pose.orientation.w = 0.0;
00521 marker2.scale.x = 3.0;
00522 marker2.scale.y = 5.0;
00523 marker2.scale.z = 10.0*mx/2048+1;
00524 marker2.color.a = 1.0;
00525 marker2.color.r = 1.0;
00526 marker2.color.g = 0.0;
00527 marker2.color.b = 0.0;
00528
00529
00530 marker_id++;
00531 visualization_msgs::Marker marker3;
00532 marker3.header.frame_id = "/world";
00533 marker3.header.stamp = ros::Time::now();
00534 marker3.ns = "my_namespace";
00535 marker3.id =marker_id;
00536 marker3.type = visualization_msgs::Marker::ARROW;
00537 marker3.action = visualization_msgs::Marker::ADD;
00538 marker3.pose.position.x = x_pos;
00539 marker3.pose.position.y = sensors_pos_y[n];
00540 marker3.pose.position.z = sensors_pos_z[n];
00541 marker3.pose.orientation.x = 1.0;
00542 marker3.pose.orientation.y = 1.0;
00543 marker3.pose.orientation.z = 0.0;
00544 marker3.pose.orientation.w = 0.0;
00545 marker3.scale.x = 3.0;
00546 marker3.scale.y = 5.0;
00547 marker3.scale.z = 10.0*my/2048+1;
00548 marker3.color.a = 1.0;
00549 marker3.color.r = 0.0;
00550 marker3.color.g = 1.0;
00551 marker3.color.b = 0.0;
00552
00553 marker_id++;
00554 visualization_msgs::Marker marker4;
00555 marker4.header.frame_id = "/world";
00556 marker4.header.stamp = ros::Time::now();
00557 marker4.ns = "my_namespace";
00558 marker4.id =marker_id;
00559 marker4.type = visualization_msgs::Marker::ARROW;
00560 marker4.action = visualization_msgs::Marker::ADD;
00561 marker4.pose.position.x = x_pos;
00562 marker4.pose.position.y = sensors_pos_y[n];
00563 marker4.pose.position.z = sensors_pos_z[n];
00564 marker4.pose.orientation.x = 1.0;
00565 marker4.pose.orientation.y = 0.0;
00566 marker4.pose.orientation.z = 1.0;
00567 marker4.pose.orientation.w = 0.0;
00568 marker4.scale.x = 3.0;
00569 marker4.scale.y = 5.0;
00570 marker4.scale.z = 10.0*mz/2048+1;
00571 marker4.color.a = 1.0;
00572 marker4.color.r = 0.0;
00573 marker4.color.g = 0.0;
00574 marker4.color.b = 1.0;
00575
00576 vis_pub.publish(marker);
00577 vis_pub.publish( marker2 );
00578 vis_pub.publish( marker3 );
00579 vis_pub.publish( marker4 );
00580 }
00581
00582 void publish_sensor_type(void)
00583 {
00584 string accel = "Acelerometro";
00585 string gyro = "Giroscopio";
00586 string magn = "Magnetometro";
00587
00588 marker_id++;
00589 visualization_msgs::Marker marker;
00590 marker.header.frame_id = "/world";
00591 marker.header.stamp = ros::Time();
00592 marker.ns = "my_namespace";
00593 marker.id =marker_id;
00594 marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00595 marker.action = visualization_msgs::Marker::ADD;
00596 marker.pose.position.x = -10;
00597 marker.pose.position.y = 0;
00598 marker.pose.position.z = -5;
00599 marker.pose.orientation.x = 1.0;
00600 marker.pose.orientation.y = 1.0;
00601 marker.pose.orientation.z = 0.0;
00602 marker.pose.orientation.w = 0.0;
00603 marker.scale.x = 0.5;
00604 marker.scale.y = 0.5;
00605 marker.scale.z = 1.5;
00606 marker.color.a = 1.0;
00607 marker.color.r = 0.0;
00608 marker.color.g = 0.0;
00609 marker.color.b = 0.0;
00610 marker.text =accel;
00611
00612 marker_id++;
00613 visualization_msgs::Marker marker2;
00614 marker2.header.frame_id = "/world";
00615 marker2.header.stamp = ros::Time();
00616 marker2.ns = "my_namespace";
00617 marker2.id =marker_id;
00618 marker2.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00619 marker2.action = visualization_msgs::Marker::ADD;
00620 marker2.pose.position.x = 0;
00621 marker2.pose.position.y = 0;
00622 marker2.pose.position.z = -5;
00623 marker2.pose.orientation.x = 1.0;
00624 marker2.pose.orientation.y = 1.0;
00625 marker2.pose.orientation.z = 0.0;
00626 marker2.pose.orientation.w = 0.0;
00627 marker2.scale.x = 0.5;
00628 marker2.scale.y = 0.5;
00629 marker2.scale.z = 1.5;
00630 marker2.color.a = 1.0;
00631 marker2.color.r = 0.0;
00632 marker2.color.g = 0.0;
00633 marker2.color.b = 0.0;
00634 marker2.text =gyro;
00635
00636 marker_id++;
00637 visualization_msgs::Marker marker3;
00638 marker3.header.frame_id = "/world";
00639 marker3.header.stamp = ros::Time();
00640 marker3.ns = "my_namespace";
00641 marker3.id =marker_id;
00642 marker3.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00643 marker3.action = visualization_msgs::Marker::ADD;
00644 marker3.pose.position.x = 10;
00645 marker3.pose.position.y = 0;
00646 marker3.pose.position.z = -5;
00647 marker3.pose.orientation.x = 1.0;
00648 marker3.pose.orientation.y = 1.0;
00649 marker3.pose.orientation.z = 0.0;
00650 marker3.pose.orientation.w = 0.0;
00651 marker3.scale.x = 5.5;
00652 marker3.scale.y = 2.5;
00653 marker3.scale.z = 1.5;
00654 marker3.color.a = 1.0;
00655 marker3.color.r = 0.0;
00656 marker3.color.g = 0.0;
00657 marker3.color.b = 0.0;
00658 marker3.text =magn;
00659
00660 vis_pub.publish(marker);
00661 vis_pub.publish(marker2);
00662 vis_pub.publish(marker3);
00663 }
00664
00665 void publish_sensor_number(int n)
00666 {
00667 string sens = "Sensor ";
00668 string nsensor;
00669
00670 int sensors_pos_y[4] = {0 , 5 , 30 , 35};
00671
00672 ostringstream convert;
00673 convert << n;
00674 nsensor = sens + convert.str();
00675
00676
00677
00678 int z_pos = -10;
00679 marker_id++;
00680 visualization_msgs::Marker marker;
00681 marker.header.frame_id = "/world";
00682 marker.header.stamp = ros::Time();
00683 marker.ns = "my_namespace";
00684 marker.id =marker_id;
00685 marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00686 marker.action = visualization_msgs::Marker::ADD;
00687 marker.pose.position.x = -15;
00688 marker.pose.position.y = sensors_pos_y[n];
00689 marker.pose.position.z = z_pos;
00690 marker.pose.orientation.x = 1.0;
00691 marker.pose.orientation.y = 1.0;
00692 marker.pose.orientation.z = 0.0;
00693 marker.pose.orientation.w = 0.0;
00694 marker.scale.x = 0.5;
00695 marker.scale.y = 0.5;
00696 marker.scale.z = 1.5;
00697 marker.color.a = 1.0;
00698 marker.color.r = 0.0;
00699 marker.color.g = 0.0;
00700 marker.color.b = 0.0;
00701 marker.text =nsensor;
00702
00703 vis_pub.publish(marker);
00704 }
00709 int main(int argc, char **argv)
00710 {
00711 ros::init(argc, argv, "imu_rviz");
00712 n = new(ros::NodeHandle);
00713 br = new(tf::TransformBroadcaster);
00714 listener = new(tf::TransformListener);
00715
00716 chatter_pub = n->advertise<imu_network::filtered_imu_network>("topic_filtered_imu", 1000);
00717 ros::Subscriber sub = n->subscribe("topic_filtered_imu", 1000, chatterCallback);
00718
00719
00720 vis_pub = n->advertise<visualization_msgs::Marker>( "marker_sensor", 0 );
00721
00722 ros::spin();
00723
00724 return 0;
00725 }