Skip to content
Snippets Groups Projects
Commit 15f09f18 authored by Nikhil Ambardar's avatar Nikhil Ambardar
Browse files

Update appmod.cpp

parent 68df92e0
No related branches found
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
Showing
with 91 additions and 240 deletions
src/appmod.cpp
+ 91
− 240
View file @ 15f09f18
...@@ -33,7 +33,7 @@ ...@@ -33,7 +33,7 @@
int main(int argc, char** argv) { int main(int argc, char** argv) {
ros::init(argc, argv, "Application Model"); ros::init(argc, argv, "Application_Model");
ros::NodeHandle node_handle; ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1); ros::AsyncSpinner spinner(1);
spinner.start(); spinner.start();
...@@ -66,15 +66,13 @@ int main(int argc, char** argv) { ...@@ -66,15 +66,13 @@ int main(int argc, char** argv) {
else else
ROS_INFO("THE ROBOT HAS NOT MOVED TO THE POSITION AS INTENDED"); ROS_INFO("THE ROBOT HAS NOT MOVED TO THE POSITION AS INTENDED");
ros::shutdown(); ros::shutdown();
abstractobject o1; abstractobject o1;
graspobject o2; graspobject o2;
hand o3; hand o3;
humanspace o4; humanspace o4;
/*
joint o5a; joint o5a;
joint o5b; joint o5b;
joint o5c; joint o5c;
...@@ -82,38 +80,21 @@ int main(int argc, char** argv) { ...@@ -82,38 +80,21 @@ int main(int argc, char** argv) {
joint o5e; joint o5e;
joint o5f; joint o5f;
joint o5g; joint o5g;
*/
obstacle o6; obstacle o6;
robot o7; robot o7;
robotfpe o8; robotfpe o8;
world o9; world o9;
worldobject o10; worldobject o10;
int yx=0; o7.jo.push_back(o5a);
o7.jo[++yx]->name= "panda_link1"; o7.jo.push_back(o5b);
o7.jo[++yx]->name= "panda_link2"; o7.jo.push_back(o5c);
o7.jo[++yx]->name= "panda_link3"; o7.jo.push_back(o5d);
o7.jo[++yx]->name= "panda_link4"; o7.jo.push_back(o5e);
o7.jo[++yx]->name= "panda_link5"; o7.jo.push_back(o5f);
o7.jo[++yx]->name= "panda_link6"; o7.jo.push_back(o5g);
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->robname="Robot FPE";
o9.c->of=true; o9.c->of=true;
o9.c->moving=false; o9.c->moving=false;
...@@ -143,167 +124,37 @@ int main(int argc, char** argv) { ...@@ -143,167 +124,37 @@ int main(int argc, char** argv) {
tf2_ros::TransformListener tfListener(tfBuffer); tf2_ros::TransformListener tfListener(tfBuffer);
ros::Rate rate(10); ros::Rate rate(10);
int i=1; int i=0;
int h=1; int ho=0;
while (node_handle.ok()) { while (node_handle.ok()) {
//for (const auto &topic : ROSNODE1::topics)
{ {
geometry_msgs::TransformStamped transformStamped[7]; geometry_msgs::TransformStamped transformStamped[7];
try { transformStamped[i++] = tfBuffer.lookupTransform("world", o7.jo[h++]->name, ros::Time(0)); try { transformStamped[++i] = tfBuffer.lookupTransform("world", o7.jo[++ho].name, ros::Time(0));
} catch (tf2::TransformException &ex) { } catch (tf2::TransformException &ex) {
ROS_WARN("%s", ex.what()); ROS_WARN("%s", ex.what());
ros::Duration(0.1).sleep(); ros::Duration(0.1).sleep();
continue; continue;
} }
if(h==7) if(ho==7)
ROS_INFO(" TEST CASE SUCCESSFUL. ROBOT FPE HAS 7 JOINTS "); ROS_INFO_STREAM(" TEST CASE SUCCESSFUL. ROBOT FPE HAS 7 JOINTS ");
else else
ROS_ERROR(" TEST CASE NOT SUCCESSFUL. ROBOT FPE HAS WRONG NUMBER OF JOINTS "); ROS_ERROR_STREAM(" TEST CASE FAILED. ROBOT FPE HAS WRONG NUMBER OF JOINTS = "<<ho);
// 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) if(o8.h==nullptr)
ROS_ERROR("ERROR IN HAND POINTER IN ROBOT FPE CLASS AGGREGATION RELATION"); ROS_ERROR("ERROR IN HAND POINTER IN ROBOT FPE CLASS AGGREGATION RELATION");
if(o9.a== nullptr) if(o9.a== nullptr)
ROS_ERROR("ERROR IN POINTER HUMANSPACE AGGREGATION"); ROS_ERROR("ERROR IN POINTER WORLD HUMANSPACE AGGREGATION");
if(o9.c== nullptr) if(o9.c== nullptr)
ROS_ERROR("ERROR IN ROBOT CLASS AGGREGATION"); ROS_ERROR("ERROR IN ROBOT CLASS AGGREGATION");
if(o9.b1== nullptr) if(o9.b1== nullptr)
ROS_ERROR("ERROR IN ABSTRACT OBJECT CLASS AGGREGATION RELATION"); ROS_ERROR("ERROR IN ABSTRACT OBJECT CLASS AGGREGATION RELATION");
if(o9.b2== nullptr) if(o9.b2== nullptr)
ROS_ERROR("ERROR IN ABSTRACT OBJECT CLASS AGGREGATION RELATION"); 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(); rate.sleep();
} }
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment