diff --git a/src/testappm.cpp b/src/testappm.cpp index 06edc6c8671a7f9e4e3bf628b3c5c8e4603251bc..24a8853d885aa2fa972a638221ccc501177329b9 100644 --- a/src/testappm.cpp +++ b/src/testappm.cpp @@ -24,9 +24,8 @@ #include "geometry_msgs/PointStamped.h" #include <tf2_ros/transform_broadcaster.h> #include <tf2/LinearMath/Quaternion.h> - - - +#include <moveit/move_group_interface/move_group_interface.h> +#include <moveit/planning_scene_interface/planning_scene_interface.h> int main(int argc, char** argv) { ros::init(argc, argv, "TestNode"); ros::NodeHandle node_handle("namespace_name"); @@ -34,18 +33,12 @@ int main(int argc, char** argv) { spinner.start(); ROS_INFO("Welcome to Test Code"); - 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; + abstractobject o1; graspobject o2; hand o3; humanspace o4; -/* + joint o5a; joint o5b; joint o5c; @@ -53,38 +46,37 @@ int main(int argc, char** argv) { 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.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); + + // test if the robot contains 7 joints + if (o7.jo.size() == 5) { + ROS_INFO("SIZE IS 5"); + } else { + ROS_INFO_STREAM("SIZE IS NOT 5 but was " << o7.jo.size()); + } - /* 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}; + 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->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; @@ -114,171 +106,40 @@ int main(int argc, char** argv) { tf2_ros::TransformListener tfListener(tfBuffer); ros::Rate rate(10); - int i=1; - int h=1; + int i=0; + int ho=0; 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) { + 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(h==7) - ROS_INFO(" TEST CASE SUCCESSFUL. ROBOT FPE HAS 7 JOINTS "); + if(ho==7) + ROS_INFO_STREAM(" TEST CASE SUCCESSFUL. ROBOT FPE HAS 7 JOINTS "); 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) ROS_ERROR("ERROR IN HAND POINTER IN ROBOT FPE CLASS AGGREGATION RELATION"); + if(o9.a== nullptr) - ROS_ERROR("ERROR IN POINTER HUMANSPACE AGGREGATION"); + 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"); - - -/* 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