diff --git a/src/testappm.cpp b/src/testappm.cpp index 8e790e152aed43363cb46eb2aa6cf95409bf22ea..06edc6c8671a7f9e4e3bf628b3c5c8e4603251bc 100644 --- a/src/testappm.cpp +++ b/src/testappm.cpp @@ -25,18 +25,27 @@ #include <tf2_ros/transform_broadcaster.h> #include <tf2/LinearMath/Quaternion.h> - int main(int argc, char** argv) { + + +int main(int argc, char** argv) { ros::init(argc, argv, "TestNode"); ros::NodeHandle node_handle("namespace_name"); ros::AsyncSpinner spinner(1); spinner.start(); ROS_INFO("Welcome to Test Code"); - abstractobject o1; + static const std::string PLANNING_GROUP = "panda_arm"; + moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP); + moveit::planning_interface::PlanningSceneInterface planning_scene_interface; + + ros::Duration(3.0).sleep(); + + + abstractobject o1; graspobject o2; hand o3; humanspace o4; - +/* joint o5a; joint o5b; joint o5c; @@ -44,20 +53,21 @@ joint o5e; joint o5f; joint o5g; - +*/ obstacle o6; robot o7; robotfpe o8; world o9; worldobject o10; - o7.jo1->name= "panda_link1"; - o7.jo2->name= "panda_link2"; - o7.jo3->name= "panda_link3"; - o7.jo4->name= "panda_link4"; - o7.jo5->name= "panda_link5"; - o7.jo6->name= "panda_link6"; - o7.jo7->name= "panda_link7"; + 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}; @@ -104,43 +114,171 @@ tf2_ros::TransformListener tfListener(tfBuffer); ros::Rate rate(10); - int i=0; + 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.jo1->name, ros::Time(0)); - transformStamped[i++] = tfBuffer.lookupTransform("world", o7.jo2->name, ros::Time(0)); - transformStamped[i++] = tfBuffer.lookupTransform("world", o7.jo3->name, ros::Time(0)); - transformStamped[i++] = tfBuffer.lookupTransform("world", o7.jo4->name, ros::Time(0)); - transformStamped[i++] = tfBuffer.lookupTransform("world", o7.jo5->name, ros::Time(0)); - transformStamped[i++] = tfBuffer.lookupTransform("world", o7.jo6->name, ros::Time(0)); - transformStamped[i++] = tfBuffer.lookupTransform("world", o7.jo7->name, ros::Time(0)); - - + 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(i==7) - ROS_INFO("TEST CASE SUCCESSFUL.ROBOT FPE HAS 7 JOINTS"); + if(h==7) + ROS_INFO(" TEST CASE SUCCESSFUL. ROBOT FPE HAS 7 JOINTS "); else - ROS_INFO("TEST CASE NOT SUCCESSFUL.ROBOT FPE HAS WRONG NUMBER OF JOINTS"); + 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(); } return 0; -} +} \ No newline at end of file