Upload New File

src/testappm.cpp

+#include <ros/ros.h>
+#include "datalink/abstractobject.h"
+#include "datalink/graspobject.h"
+#include "datalink/hand.h"
+#include "datalink/humanspace.h"
+#include "datalink/joint.h"
+#include "datalink/obstacle.h"
+#include "datalink/robot.h"
+#include "datalink/robotfpe.h"
+#include "datalink/world.h"
+#include "datalink/worldobject.h"
+#include "datalink/quaternion.h"
+#include <vector>
+#include <iostream>
+#include <tf2_ros/transform_listener.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <tf2_ros/static_transform_broadcaster.h>
+#include <cstdio>
+#include <tf2/LinearMath/Quaternion.h>
+#include "tf2_ros/message_filter.h"
+#include "message_filters/subscriber.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#include "geometry_msgs/PointStamped.h"
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2/LinearMath/Quaternion.h>
+
+    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;
+        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.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";
+
+      /*  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=0;
+        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));
+
+
+                } 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");
+                else
+                    ROS_INFO("TEST CASE NOT SUCCESSFUL.ROBOT FPE HAS WRONG NUMBER OF JOINTS");
+
+
+
+
+              }
+            rate.sleep();
+        }
+
+
+return 0;
+}