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

Update testappm.cpp

parent 0769ee2d
No related branches found
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
Showing
with 168 additions and 30 deletions
src/testappm.cpp
+ 168
− 30
View file @ 85bc26d4
...@@ -25,6 +25,8 @@ ...@@ -25,6 +25,8 @@
#include <tf2_ros/transform_broadcaster.h> #include <tf2_ros/transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h> #include <tf2/LinearMath/Quaternion.h>
int main(int argc, char** argv) { int main(int argc, char** argv) {
ros::init(argc, argv, "TestNode"); ros::init(argc, argv, "TestNode");
ros::NodeHandle node_handle("namespace_name"); ros::NodeHandle node_handle("namespace_name");
...@@ -32,11 +34,18 @@ ...@@ -32,11 +34,18 @@
spinner.start(); spinner.start();
ROS_INFO("Welcome to Test Code"); 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; graspobject o2;
hand o3; hand o3;
humanspace o4; humanspace o4;
/*
joint o5a; joint o5a;
joint o5b; joint o5b;
joint o5c; joint o5c;
...@@ -44,20 +53,21 @@ ...@@ -44,20 +53,21 @@
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;
o7.jo1->name= "panda_link1"; int yx=0;
o7.jo2->name= "panda_link2"; o7.jo[++yx]->name= "panda_link1";
o7.jo3->name= "panda_link3"; o7.jo[++yx]->name= "panda_link2";
o7.jo4->name= "panda_link4"; o7.jo[++yx]->name= "panda_link3";
o7.jo5->name= "panda_link5"; o7.jo[++yx]->name= "panda_link4";
o7.jo6->name= "panda_link6"; o7.jo[++yx]->name= "panda_link5";
o7.jo7->name= "panda_link7"; o7.jo[++yx]->name= "panda_link6";
o7.jo[++yx]->name= "panda_link7";
/* o7.jo1->or2= {1.2,5.3,6.5,7.4}; /* o7.jo1->or2= {1.2,5.3,6.5,7.4};
o7.jo2->or2= {2.5,3.6,5.8,4.9}; o7.jo2->or2= {2.5,3.6,5.8,4.9};
...@@ -104,39 +114,167 @@ ...@@ -104,39 +114,167 @@
tf2_ros::TransformListener tfListener(tfBuffer); tf2_ros::TransformListener tfListener(tfBuffer);
ros::Rate rate(10); ros::Rate rate(10);
int i=0; int i=1;
int h=1;
while (node_handle.ok()) { while (node_handle.ok()) {
//for (const auto &topic : ROSNODE1::topics) //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.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));
} 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(i==7) if(h==7)
ROS_INFO(" TEST CASE SUCCESSFUL. ROBOT FPE HAS 7 JOINTS "); ROS_INFO(" TEST CASE SUCCESSFUL. ROBOT FPE HAS 7 JOINTS ");
else 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(); rate.sleep();
} }
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment