Go to the documentation of this file.00001 #include "hand_marker_publisher_alg_node.h"
00002
00003 HandMarkerPublisherAlgNode::HandMarkerPublisherAlgNode(void) :
00004 algorithm_base::IriBaseAlgorithm<HandMarkerPublisherAlgorithm>()
00005 {
00006
00007 this->public_node_handle_.getParam("arm_id", arm_id);
00008 first_time=true;
00009 pose.resize(3);
00010 angles.resize(4);
00011 arm = new CSegwayArmKinematics(arm_id);
00012
00013
00014 if(arm_id==0){
00015 strcpy(hand_tf,"/skeleton/right_hand_1");
00016 }else if(arm_id==1){
00017 strcpy(hand_tf,"/skeleton/left_hand_1");
00018 }
00019
00020
00021
00022
00023 this->hand_marker_publisher_ = this->public_node_handle_.advertise<visualization_msgs::Marker>("hand_marker", 100);
00024 this->hand_marker_shoulder_publisher_ = this->public_node_handle_.advertise<visualization_msgs::Marker>("hand_marker_shoulder", 100);
00025 this->hand_marker_IK_FK_publisher_ = this->public_node_handle_.advertise<visualization_msgs::Marker>("hand_marker_IK_FK", 100);
00026 this->hand_marker_workspace_publisher_ = this->public_node_handle_.advertise<visualization_msgs::Marker>("hand_marker_workspace", 100);
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040 geometry_msgs::Point p;
00041
00042 for(int i=1;i<=21;i++)
00043 {
00044 for(int j=1;j<=21;j++)
00045 {
00046 for(int k=1;k<=21;k++)
00047 {
00048
00049
00050
00051 p.x = (i*61.0-600.0)/1000.0;
00052 p.y = (j*61.0-600.0)/1000.0;
00053 p.z = (k*61.0-600.0)/1000.0;
00054
00055
00056 if(arm_id==0){
00057 pose[0]=p.x*1000.0;
00058 pose[1]=p.y*1000.0;
00059 pose[2]=p.z*1000.0;
00060 }else if(arm_id==1){
00061 pose[0]=-p.y*1000.0;
00062 pose[1]=p.z*1000.0;
00063 pose[2]=-p.x*1000.0;
00064 }
00065
00066 if((*arm).check_workspace(pose)) this->Marker_workspace_msg_.points.push_back(p);
00067 }
00068 }
00069 }
00070
00071 }
00072
00073 HandMarkerPublisherAlgNode::~HandMarkerPublisherAlgNode(void)
00074 {
00075
00076 }
00077
00078 void HandMarkerPublisherAlgNode::mainNodeThread(void)
00079 {
00080 static int calibration_countdown=10, arm_length=0, automatic_position_x=1,automatic_position_y=1,automatic_position_z=1;
00081
00082
00083
00084
00085 if(listener.frameExists("/skeleton/neck_1") && listener.frameExists(hand_tf)){
00086
00087 if(first_time){
00088 sleep(1.0);
00089 first_time=false;
00090 }
00091
00092 listener.lookupTransform("/skeleton/neck_1", hand_tf, ros::Time(0), echo_transform);
00093 v = echo_transform.getOrigin();
00094
00095
00096
00097 x=-v.getZ();
00098 y=-v.getX();
00099 z=v.getY();
00100
00101
00102
00103
00104
00105
00106
00107 this->Marker_msg_.pose.position.x = x;
00108 this->Marker_msg_.pose.position.y = y;
00109 this->Marker_msg_.pose.position.z = z;
00110
00111
00112
00113
00114 pose[0]=x*1000;
00115 pose[1]=y*1000;
00116 pose[2]=z*1000;
00117
00118 pose=(*arm).neck_to_shoulder_transform(pose);
00119
00120
00121 if(arm_id==0){
00122
00123 }else if(arm_id==1){
00124 float aux=pose[0];
00125 pose[0]=pose[1];
00126 pose[1]=-aux;
00127 pose[2]=pose[2];
00128 }
00129
00130
00131 if(sqrt(pose[0]*pose[0]+pose[1]*pose[1]+pose[2]*pose[2])>arm_length && calibration_countdown >= 0)
00132 {
00133 if(sqrt(pose[0]*pose[0]+pose[1]*pose[1]+pose[2]*pose[2])>arm_length)
00134 {
00135 arm_length=sqrt(pose[0]*pose[0]+pose[1]*pose[1]+pose[2]*pose[2]);
00136 }
00137 calibration_countdown--;
00138 ROS_WARN_STREAM("CALIBRATING: PLEASE EXTEND ARM " << calibration_countdown);
00139 sleep(1);
00140 }
00141
00142 }else first_time=true;
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201 this->Marker_shoulder_msg_.pose.position.x = pose[0]/1000;
00202 this->Marker_shoulder_msg_.pose.position.y = pose[1]/1000;
00203 this->Marker_shoulder_msg_.pose.position.z = pose[2]/1000;
00204
00205
00206
00207
00208 if(arm_id==0){
00209
00210 }else if(arm_id==1){
00211 float aux=pose[0];
00212 pose[0]=-pose[1];
00213 pose[1]=pose[2];
00214 pose[2]=-aux;
00215 }
00216
00217
00218
00219 pose[0]=pose[0]/arm_length*((*arm).D+(*arm).A);
00220 pose[1]=pose[1]/arm_length*((*arm).D+(*arm).A);
00221 pose[2]=pose[2]/arm_length*((*arm).D+(*arm).A);
00222
00223 if(!(*arm).check_workspace(pose)){
00224 this->Marker_IK_FK_msg_.pose.position.x = 0.0;
00225 this->Marker_IK_FK_msg_.pose.position.y = 0.0;
00226 this->Marker_IK_FK_msg_.pose.position.z = 0.0;
00227 }else{
00228 if(!(*arm).best_ikine(pose,0.0,angles)){
00229 this->Marker_IK_FK_msg_.pose.position.x = 1.0;
00230 this->Marker_IK_FK_msg_.pose.position.y = 1.0;
00231 this->Marker_IK_FK_msg_.pose.position.z = 1.0;
00232 }else{
00233 pose=(*arm).fkine(angles);
00234
00235 if(arm_id==0){
00236
00237 }else if(arm_id==1){
00238 float aux=pose[2];
00239 pose[2]=pose[1];
00240 pose[1]=-pose[0];
00241 pose[0]=-aux;
00242 }
00243
00244 this->Marker_IK_FK_msg_.pose.position.x = pose[0]/1000;
00245 this->Marker_IK_FK_msg_.pose.position.y = pose[1]/1000;
00246 this->Marker_IK_FK_msg_.pose.position.z = pose[2]/1000;
00247 }
00248 }
00249
00250
00251
00252
00253
00254
00255
00256 this->Marker_msg_.header.frame_id="/dabo/neck1";
00257 this->Marker_msg_.header.stamp = ros::Time::now();
00258
00259 this->Marker_msg_.ns = "basic_shapes";
00260 this->Marker_msg_.id = 0;
00261
00262 this->Marker_msg_.type = visualization_msgs::Marker::SPHERE;
00263 this->Marker_msg_.action = visualization_msgs::Marker::ADD;
00264
00265 this->Marker_msg_.pose.orientation.x = 0.0;
00266 this->Marker_msg_.pose.orientation.y = 0.0;
00267 this->Marker_msg_.pose.orientation.z = 0.0;
00268 this->Marker_msg_.pose.orientation.w = 1.0;
00269
00270 this->Marker_msg_.scale.x = 0.1;
00271 this->Marker_msg_.scale.y = 0.1;
00272 this->Marker_msg_.scale.z = 0.1;
00273
00274 this->Marker_msg_.color.r = 0.0f;
00275 this->Marker_msg_.color.g = 1.0f;
00276 this->Marker_msg_.color.b = 0.0f;
00277 this->Marker_msg_.color.a = 1.0;
00278
00279 this->Marker_msg_.lifetime = ros::Duration();
00280
00281 this->hand_marker_publisher_.publish(this->Marker_msg_);
00282
00283
00284
00285
00286 if(arm_id==0){
00287 this->Marker_shoulder_msg_.header.frame_id="/dabo/shoulder_left";
00288 }else if(arm_id==1){
00289 this->Marker_shoulder_msg_.header.frame_id="/dabo/shoulder_right";
00290 }
00291 this->Marker_shoulder_msg_.header.stamp = ros::Time::now();
00292
00293 this->Marker_shoulder_msg_.ns = "basic_shapes";
00294 this->Marker_shoulder_msg_.id = 1;
00295
00296 this->Marker_shoulder_msg_.type = visualization_msgs::Marker::SPHERE;
00297 this->Marker_shoulder_msg_.action = visualization_msgs::Marker::ADD;
00298
00299 this->Marker_shoulder_msg_.pose.orientation.x = 0.0;
00300 this->Marker_shoulder_msg_.pose.orientation.y = 0.0;
00301 this->Marker_shoulder_msg_.pose.orientation.z = 0.0;
00302 this->Marker_shoulder_msg_.pose.orientation.w = 1.0;
00303
00304 this->Marker_shoulder_msg_.scale.x = 0.1;
00305 this->Marker_shoulder_msg_.scale.y = 0.1;
00306 this->Marker_shoulder_msg_.scale.z = 0.1;
00307
00308 this->Marker_shoulder_msg_.color.r = 1.0f;
00309 this->Marker_shoulder_msg_.color.g = 0.0f;
00310 this->Marker_shoulder_msg_.color.b = 0.0f;
00311 this->Marker_shoulder_msg_.color.a = 1.0;
00312
00313 this->Marker_shoulder_msg_.lifetime = ros::Duration();
00314
00315 this->hand_marker_shoulder_publisher_.publish(this->Marker_shoulder_msg_);
00316
00317
00318
00319 if(arm_id==0){
00320 this->Marker_IK_FK_msg_.header.frame_id="/dabo/shoulder_left";
00321 }else if(arm_id==1){
00322 this->Marker_IK_FK_msg_.header.frame_id="/dabo/shoulder_right";
00323 }
00324
00325 this->Marker_IK_FK_msg_.header.stamp = ros::Time::now();
00326
00327 this->Marker_IK_FK_msg_.ns = "basic_shapes";
00328 this->Marker_IK_FK_msg_.id = 1;
00329
00330 this->Marker_IK_FK_msg_.type = visualization_msgs::Marker::SPHERE;
00331 this->Marker_IK_FK_msg_.action = visualization_msgs::Marker::ADD;
00332
00333 this->Marker_IK_FK_msg_.pose.orientation.x = 0.0;
00334 this->Marker_IK_FK_msg_.pose.orientation.y = 0.0;
00335 this->Marker_IK_FK_msg_.pose.orientation.z = 0.0;
00336 this->Marker_IK_FK_msg_.pose.orientation.w = 1.0;
00337
00338 this->Marker_IK_FK_msg_.scale.x = 0.1;
00339 this->Marker_IK_FK_msg_.scale.y = 0.1;
00340 this->Marker_IK_FK_msg_.scale.z = 0.1;
00341
00342 this->Marker_IK_FK_msg_.color.r = 0.0f;
00343 this->Marker_IK_FK_msg_.color.g = 0.5f;
00344 this->Marker_IK_FK_msg_.color.b = 0.8f;
00345 this->Marker_IK_FK_msg_.color.a = 1.0;
00346
00347 this->Marker_IK_FK_msg_.lifetime = ros::Duration();
00348
00349 this->hand_marker_IK_FK_publisher_.publish(this->Marker_IK_FK_msg_);
00350
00351
00352
00353 if(arm_id==0){
00354 this->Marker_workspace_msg_.header.frame_id = "/dabo/shoulder_left";
00355 }else if(arm_id==1){
00356 this->Marker_workspace_msg_.header.frame_id = "/dabo/shoulder_right";
00357 }
00358
00359 this->Marker_workspace_msg_.header.stamp = ros::Time::now();
00360 this->Marker_workspace_msg_.ns = "points_and_lines";
00361 this->Marker_workspace_msg_.action = visualization_msgs::Marker::ADD;
00362 this->Marker_workspace_msg_.pose.orientation.w = 1.0;
00363
00364 this->Marker_workspace_msg_.id = 0;
00365
00366 this->Marker_workspace_msg_.type = visualization_msgs::Marker::POINTS;
00367
00368
00369 this->Marker_workspace_msg_.scale.x = 0.005;
00370 this->Marker_workspace_msg_.scale.y = 0.005;
00371
00372
00373 this->Marker_workspace_msg_.color.b = 1.0f;
00374 this->Marker_workspace_msg_.color.a = 1.0;
00375
00376 this->hand_marker_workspace_publisher_.publish(this->Marker_workspace_msg_);
00377
00378
00379
00380
00381
00382
00383
00384
00385
00386
00387
00388
00389 }
00390
00391
00392
00393
00394
00395
00396
00397
00398
00399 void HandMarkerPublisherAlgNode::node_config_update(Config &config, uint32_t level)
00400 {
00401 this->alg_.lock();
00402
00403 this->alg_.unlock();
00404 }
00405
00406 void HandMarkerPublisherAlgNode::addNodeDiagnostics(void)
00407 {
00408 }
00409
00410
00411 int main(int argc,char *argv[])
00412 {
00413 return algorithm_base::main<HandMarkerPublisherAlgNode>(argc, argv, "hand_marker_publisher_alg_node");
00414 }