diff --git a/src/appmod.cpp b/src/appmod.cpp
index f234a7269c8a9a1f51e4bdd7b049d5c4544a4beb..29d8a6524999c94ca8f56ce3ea0c8b89b3c6fb18 100644
--- a/src/appmod.cpp
+++ b/src/appmod.cpp
@@ -33,7 +33,7 @@
int main(int argc, char** argv) {
- ros::init(argc, argv, "Application Model");
+ ros::init(argc, argv, "Application_Model");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
@@ -62,252 +62,103 @@ int main(int argc, char** argv) {
move_group.move();
if(another_pose.position.z==0.9)
- ROS_INFO(" THE ROBOT HAS MOVED TO POSITION AS INTENDED ");
+ ROS_INFO(" THE ROBOT HAS MOVED TO POSITION AS INTENDED ");
else
ROS_INFO("THE ROBOT HAS NOT MOVED TO THE POSITION AS INTENDED");
-
ros::shutdown();
-
abstractobject o1;
- graspobject o2;
- hand o3;
- humanspace o4;
-/*
- joint o5a;
- joint o5b;
- joint o5c;
- joint o5d;
- joint o5e;
- joint o5f;
- joint o5g;
-*/
- obstacle o6;
- robot o7;
- robotfpe o8;
- world o9;
- worldobject o10;
-
- int yx=0;
- o7.jo[++yx]->name= "panda_link1";
- o7.jo[++yx]->name= "panda_link2";
- o7.jo[++yx]->name= "panda_link3";
- o7.jo[++yx]->name= "panda_link4";
- o7.jo[++yx]->name= "panda_link5";
- o7.jo[++yx]->name= "panda_link6";
- o7.jo[++yx]->name= "panda_link7";
-
- /* o7.jo1->or2= {1.2,5.3,6.5,7.4};
- o7.jo2->or2= {2.5,3.6,5.8,4.9};
- o7.jo3->or2= {5.1,1.2,9.9,6.8};
- o7.jo4->or2= {6.3,3.4,8.5,9.6};
- o7.jo5->or2= {8.2,2.6,7.9,5.5};
- o7.jo6->or2= {7.3,4.4,6.7,4.0};
- o7.jo7->or2= {7.3,4.4,6.7,4.0};
-
- o7.jo1->p1= {1.3, 10.1, 9.4};
- o7.jo2->p1= {14.7, 11.3, 6.5};
- o7.jo3->p1= {4.9, 13.8, 7.7};
- o7.jo4->p1= {2.4, 12.5, 1.6};
- o7.jo5->p1= {3.1, 16.2, 5.3};
- o7.jo6->p1= {11.5, 15.4, 3.4};
- o7.jo7->p1= {5.1, 18.3, 4.2};
-*/
- o9.c->robname="Robot FPE";
- o9.c->of=true;
- o9.c->moving=false;
- o9.c->grasppos=true;
- o9.c->planninggrp= "Panda Teaching";
-
- o8.h->grippos= {12.1, 14.3, 18.2};
- o3.grippos={1.1,2.3,2.1};
-
- o9.a->humannum = 1;
- o9.b1->name="Cube";
- o9.b1->size={5.2, 7.5, 9.1};
- o9.b1->p1= {13.2, 14.1, 17.5};
- o9.b1->x= 2.3;
- o9.b1->y = 1.2;
- o9.b1->z = 0.2;
- o9.b1->w = 0.1;
- o9.b2->name="Ball";
- o9.b2->size={1.3, 2.1, 3.2};
- o9.b2->p1= {12.5, 16.4, 17.3};
- o9.b2->x= 1.1;
- o9.b2->y = 1.4;
- o9.b2->z = 1.6;
- o9.b2->w = 1.9;
-
- tf2_ros::Buffer tfBuffer;
- tf2_ros::TransformListener tfListener(tfBuffer);
-
- ros::Rate rate(10);
- int i=1;
- int h=1;
- while (node_handle.ok()) {
- //for (const auto &topic : ROSNODE1::topics)
- {
- geometry_msgs::TransformStamped transformStamped[7];
-
- try { transformStamped[i++] = tfBuffer.lookupTransform("world", o7.jo[h++]->name, ros::Time(0));
- } catch (tf2::TransformException &ex) {
- ROS_WARN("%s", ex.what());
- ros::Duration(0.1).sleep();
- continue;
- }
- if(h==7)
- ROS_INFO(" TEST CASE SUCCESSFUL. ROBOT FPE HAS 7 JOINTS ");
- else
- ROS_ERROR(" TEST CASE NOT SUCCESSFUL. ROBOT FPE HAS WRONG NUMBER OF JOINTS ");
-
- // TEST CASES FOR ALL AGGREGATION RELATIONS POINTERS
- if(o7.jo[1]||o7.jo[2]||o7.jo[3]||o7.jo[4]||o7.jo[5]||o7.jo[6]||o7.jo[7]== nullptr)
- ROS_ERROR("ERROR IN JOINTS POINTER AGGREGATION RELATIONSHIP");
- if(o8.h==nullptr)
- ROS_ERROR("ERROR IN HAND POINTER IN ROBOT FPE CLASS AGGREGATION RELATION");
- if(o9.a== nullptr)
- ROS_ERROR("ERROR IN POINTER HUMANSPACE AGGREGATION");
- if(o9.c== nullptr)
- ROS_ERROR("ERROR IN ROBOT CLASS AGGREGATION");
- if(o9.b1== nullptr)
- ROS_ERROR("ERROR IN ABSTRACT OBJECT CLASS AGGREGATION RELATION");
- if(o9.b2== nullptr)
- ROS_ERROR("ERROR IN ABSTRACT OBJECT CLASS AGGREGATION RELATION");
-
-
-/* o7.jo1->p1[0]=0.0;
- o7.jo1->p1[1]=0.0;
- o7.jo1->p1[2]=0.0;
- o7.jo2->p1[0]=0.0;
- o7.jo2->p1[1]=0.0;
- o7.jo2->p1[2]=0.0;
- o7.jo3->p1[0]=0.0;
- o7.jo3->p1[1]=0.0;
- o7.jo3->p1[2]=0.0;
- o7.jo4->p1[0]=0.0;
- o7.jo4->p1[1]=0.0;
- o7.jo4->p1[2]=0.0;
- o7.jo5->p1[0]=0.0;
- o7.jo5->p1[1]=0.0;
- o7.jo5->p1[2]=0.0;
- o7.jo6->p1[0]=0.0;
- o7.jo6->p1[1]=0.0;
- o7.jo6->p1[2]=0.0;
- o7.jo7->p1[0]=0.0;
- o7.jo7->p1[1]=0.0;
- o7.jo7->p1[2]=0.0;
-
- o7.jo1->p1[0]=transformStamped1.transform.translation.x;
- o7.jo1->p1[1]=transformStamped1.transform.translation.y;
- o7.jo1->p1[2]=transformStamped1.transform.translation.z;
- ROS_INFO_STREAM("panda_link1 Position is " <<" x = "<< o7.jo1->p1[0] << ", y = "<< o7.jo1->p1[1]<<", z = "<<o7.jo1->p1[2]);
-
- o7.jo2->p1[0]=transformStamped2.transform.translation.x;
- o7.jo2->p1[1]=transformStamped2.transform.translation.y;
- o7.jo2->p1[2]=transformStamped2.transform.translation.z;
- ROS_INFO_STREAM("panda_link2 Position is " <<" x = "<< o7.jo2->p1[0] << ", y = "<< o7.jo2->p1[1]<<", z = "<<o7.jo2->p1[2]);
-
- o7.jo3->p1[0]=transformStamped3.transform.translation.x;
- o7.jo3->p1[1]=transformStamped3.transform.translation.y;
- o7.jo3->p1[2]=transformStamped3.transform.translation.z;
- ROS_INFO_STREAM("panda_link3 Position is " <<" x = "<< o7.jo3->p1[0] << ", y = "<< o7.jo3->p1[1]<<", z = "<<o7.jo3->p1[2]);
-
- o7.jo4->p1[0]=transformStamped4.transform.translation.x;
- o7.jo4->p1[1]=transformStamped4.transform.translation.y;
- o7.jo4->p1[2]=transformStamped4.transform.translation.z;
- ROS_INFO_STREAM("panda_link4 Position is " <<" x = "<< o7.jo4->p1[0] << ", y = "<< o7.jo4->p1[1]<<", z = "<<o7.jo4->p1[2]);
-
- o7.jo5->p1[0]=transformStamped5.transform.translation.x;
- o7.jo5->p1[1]=transformStamped5.transform.translation.y;
- o7.jo5->p1[2]=transformStamped5.transform.translation.z;
- ROS_INFO_STREAM("panda_link5 Position is " <<" x = "<< o7.jo5->p1[0] << ", y = "<< o7.jo5->p1[1]<<", z = "<<o7.jo5->p1[2]);
-
- o7.jo6->p1[0]=transformStamped6.transform.translation.x;
- o7.jo6->p1[1]=transformStamped6.transform.translation.y;
- o7.jo6->p1[2]=transformStamped6.transform.translation.z;
- ROS_INFO_STREAM("panda_link6 Position is " <<" x = "<< o7.jo6->p1[0] << ", y = "<< o7.jo6->p1[1]<<", z = "<<o7.jo6->p1[2]);
-
- o7.jo7->p1[0]=transformStamped7.transform.translation.x;
- o7.jo7->p1[0]=transformStamped7.transform.translation.y;
- o7.jo7->p1[0]=transformStamped7.transform.translation.z;
- ROS_INFO_STREAM("panda_link7 Position is " <<" x = "<< o7.jo7->p1[0] << ", y = "<< o7.jo7->p1[1]<<", z = "<<o7.jo7->p1[2]);
-
- o7.jo1->or2.w=0.0;
- o7.jo1->or2.x=0.0;
- o7.jo1->or2.y=0.0;
- o7.jo1->or2.z=0.0;
- o7.jo2->or2.w=0.0;
- o7.jo2->or2.x=0.0;
- o7.jo2->or2.y=0.0;
- o7.jo2->or2.z=0.0;
- o7.jo3->or2.w=0.0;
- o7.jo3->or2.x=0.0;
- o7.jo3->or2.y=0.0;
- o7.jo3->or2.z=0.0;
- o7.jo4->or2.w=0.0;
- o7.jo4->or2.x=0.0;
- o7.jo4->or2.y=0.0;
- o7.jo4->or2.z=0.0;
- o7.jo5->or2.w=0.0;
- o7.jo5->or2.x=0.0;
- o7.jo5->or2.y=0.0;
- o7.jo5->or2.z=0.0;
- o7.jo6->or2.w=0.0;
- o7.jo6->or2.x=0.0;
- o7.jo6->or2.y=0.0;
- o7.jo6->or2.z=0.0;
- o7.jo7->or2.w=0.0;
- o7.jo7->or2.x=0.0;
- o7.jo7->or2.y=0.0;
- o7.jo7->or2.z=0.0;
-
- o7.jo1->or2.w=transformStamped1.transform.rotation.w;
- o7.jo1->or2.x=transformStamped1.transform.rotation.x;
- o7.jo1->or2.y=transformStamped1.transform.rotation.y;
- o7.jo1->or2.z=transformStamped1.transform.rotation.z;
- ROS_INFO_STREAM("panda_link1 Orientation is " <<" w = "<< o7.jo1->or2.w<<", x = "<<o7.jo1->or2.x<<", y = "<<o7.jo1->or2.y<<", z = "<<o7.jo1->or2.z);
-
- o7.jo2->or2.w=transformStamped2.transform.rotation.w;
- o7.jo2->or2.x=transformStamped2.transform.rotation.x;
- o7.jo2->or2.y=transformStamped2.transform.rotation.y;
- o7.jo2->or2.z=transformStamped2.transform.rotation.z;
- ROS_INFO_STREAM("panda_link2 Orientation is " <<" w = "<< o7.jo2->or2.w<<", x = "<< o7.jo2->or2.x<<", y = "<< o7.jo2->or2.y<<", z = "<< o7.jo2->or2.z);
-
- o7.jo3->or2.w=transformStamped3.transform.rotation.w;
- o7.jo3->or2.x=transformStamped3.transform.rotation.x;
- o7.jo3->or2.y=transformStamped3.transform.rotation.y;
- o7.jo3->or2.z=transformStamped3.transform.rotation.z;
- ROS_INFO_STREAM("panda_link3 Orientation is " <<" w = "<< o7.jo3->or2.w<<", x = "<<o7.jo3->or2.x<<", y = "<<o7.jo3->or2.y<<", z = "<<o7.jo3->or2.z);
-
- o7.jo4->or2.w=transformStamped4.transform.rotation.w;
- o7.jo4->or2.x=transformStamped4.transform.rotation.x;
- o7.jo4->or2.y=transformStamped4.transform.rotation.y;
- o7.jo4->or2.z=transformStamped4.transform.rotation.z;
- ROS_INFO_STREAM("panda_link4 Orientation is " <<" w = "<<o7.jo4->or2.w<<", x = "<<o7.jo4->or2.x<<", y = "<<o7.jo4->or2.y<<", z = "<<o7.jo4->or2.z);
-
- o7.jo5->or2.w=transformStamped5.transform.rotation.w;
- o7.jo5->or2.x=transformStamped5.transform.rotation.x;
- o7.jo5->or2.y=transformStamped5.transform.rotation.y;
- o7.jo5->or2.z=transformStamped5.transform.rotation.z;
- ROS_INFO_STREAM("panda_link5 Orientation is " <<" w = "<< o7.jo5->or2.w<<", x = "<<o7.jo5->or2.x<<", y = "<<o7.jo5->or2.y<<", z = "<<o7.jo5->or2.z);
-
- o7.jo6->or2.w=transformStamped6.transform.rotation.w;
- o7.jo6->or2.x=transformStamped6.transform.rotation.x;
- o7.jo6->or2.y=transformStamped6.transform.rotation.y;
- o7.jo6->or2.z=transformStamped6.transform.rotation.z;
- ROS_INFO_STREAM("panda_link6 Orientation is " <<" w = "<< o7.jo6->or2.w<<", x = "<<o7.jo6->or2.x<<", y = "<<o7.jo6->or2.y<<", z = "<<o7.jo6->or2.z);
-
- o7.jo7->or2.w=transformStamped7.transform.rotation.w;
- o7.jo7->or2.x=transformStamped7.transform.rotation.x;
- o7.jo7->or2.y=transformStamped7.transform.rotation.y;
- o7.jo7->or2.z=transformStamped7.transform.rotation.z;
- ROS_INFO_STREAM("panda_link7 Orientation is " <<" w = "<<o7.jo7->or2.w<<", x = "<<o7.jo7->or2.x<<", y = "<<o7.jo7->or2.y<<", z = "<<o7.jo7->or2.z);
-*/
- }
- rate.sleep();
+ graspobject o2;
+ hand o3;
+ humanspace o4;
+
+ joint o5a;
+ joint o5b;
+ joint o5c;
+ joint o5d;
+ joint o5e;
+ joint o5f;
+ joint o5g;
+
+ obstacle o6;
+ robot o7;
+ robotfpe o8;
+ world o9;
+ worldobject o10;
+
+ o7.jo.push_back(o5a);
+ o7.jo.push_back(o5b);
+ o7.jo.push_back(o5c);
+ o7.jo.push_back(o5d);
+ o7.jo.push_back(o5e);
+ o7.jo.push_back(o5f);
+ o7.jo.push_back(o5g);
+
+ o9.c->robname="Robot FPE";
+ o9.c->of=true;
+ o9.c->moving=false;
+ o9.c->grasppos=true;
+ o9.c->planninggrp= "Panda Teaching";
+
+ o8.h->grippos= {12.1, 14.3, 18.2};
+ o3.grippos={1.1,2.3,2.1};
+
+ o9.a->humannum = 1;
+ o9.b1->name="Cube";
+ o9.b1->size={5.2, 7.5, 9.1};
+ o9.b1->p1= {13.2, 14.1, 17.5};
+ o9.b1->x= 2.3;
+ o9.b1->y = 1.2;
+ o9.b1->z = 0.2;
+ o9.b1->w = 0.1;
+ o9.b2->name="Ball";
+ o9.b2->size={1.3, 2.1, 3.2};
+ o9.b2->p1= {12.5, 16.4, 17.3};
+ o9.b2->x= 1.1;
+ o9.b2->y = 1.4;
+ o9.b2->z = 1.6;
+ o9.b2->w = 1.9;
+
+ tf2_ros::Buffer tfBuffer;
+ tf2_ros::TransformListener tfListener(tfBuffer);
+
+ ros::Rate rate(10);
+ int i=0;
+ int ho=0;
+ while (node_handle.ok()) {
+ {
+ geometry_msgs::TransformStamped transformStamped[7];
+
+ try { transformStamped[++i] = tfBuffer.lookupTransform("world", o7.jo[++ho].name, ros::Time(0));
+ } catch (tf2::TransformException &ex) {
+ ROS_WARN("%s", ex.what());
+ ros::Duration(0.1).sleep();
+ continue;
+ }
+ if(ho==7)
+ ROS_INFO_STREAM(" TEST CASE SUCCESSFUL. ROBOT FPE HAS 7 JOINTS ");
+ else
+ ROS_ERROR_STREAM(" TEST CASE FAILED. ROBOT FPE HAS WRONG NUMBER OF JOINTS = "<<ho);
+
+ if(o8.h==nullptr)
+ ROS_ERROR("ERROR IN HAND POINTER IN ROBOT FPE CLASS AGGREGATION RELATION");
+
+ if(o9.a== nullptr)
+ ROS_ERROR("ERROR IN POINTER WORLD HUMANSPACE AGGREGATION");
+
+ if(o9.c== nullptr)
+ ROS_ERROR("ERROR IN ROBOT CLASS AGGREGATION");
+
+ if(o9.b1== nullptr)
+ ROS_ERROR("ERROR IN ABSTRACT OBJECT CLASS AGGREGATION RELATION");
+
+ if(o9.b2== nullptr)
+ ROS_ERROR("ERROR IN ABSTRACT OBJECT CLASS AGGREGATION RELATION");
}
+ rate.sleep();
+ }
-return 0;
+ return 0;
}