Skip to content
Snippets Groups Projects
Select Git revision
  • 9e25d3ed07f0815607dd94aba6523607256bad07
  • master default
2 results

testappm.cpp

Blame
    • testappm.cpp 4.41 KiB 
    #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;
}