From 15f09f1808e66ba9efffc5c41c34d633f9aa9999 Mon Sep 17 00:00:00 2001 From: Nikhil Ambardar <202-nikaviator@users.noreply.git-st.inf.tu-dresden.de> Date: Wed, 28 Oct 2020 14:32:13 +0000 Subject: [PATCH] Update appmod.cpp --- src/appmod.cpp | 331 ++++++++++++++----------------------------------- 1 file changed, 91 insertions(+), 240 deletions(-) diff --git a/src/appmod.cpp b/src/appmod.cpp index f234a72..29d8a65 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; } -- GitLab