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

Update testappm.cpp

parent 8b18ea2d
No related branches found
No related tags found
No related merge requests found
......@@ -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;
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
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment