robot_models_node.cpp

#include <ros/ros.h>
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/abstractobject.h>
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/graspobject.h>
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/hand.h>
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/humanspace.h>
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/joint.h>
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/obstacle.h>
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/robot.h>
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/robotfpe.h>
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/world.h>
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/datalink/worldobject.h>
#include </home/nikhil/panda_gazebo_workspace/src/zero/src/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, "ROSNODE1");
        ros::NodeHandle node_handle("namespace_name");
        ros::AsyncSpinner spinner(1);
        spinner.start();


        ROS_INFO("HELLO, WORLD");


        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;


        o5a.name = "panda_joint1";
        o5b.name = "panda_joint2";
        o5c.name = "panda_joint3";
        o5d.name = "panda_joint4";
        o5e.name = "panda_joint5";
        o5f.name = "panda_joint6";
        o5g.name = "panda_joint7";
        o5a.or2 = {1,5,6,7};
        o5b.or2 = {2,3,5,4};
        o5c.or2 = {5,1,9,6};
        o5d.or2 = {6,3,8,9};
        o5e.or2 = {8,2,7,5};
        o5f.or2 = {7,4,6,4};
        o5g.or2 = {9,5,5,8};
        o5a.p1 = {1, 10, 9};
        o5b.p1 = {14, 11, 6};
        o5c.p1 = {4, 13, 7};
        o5d.p1 = {2, 12, 1};
        o5e.p1 = {3, 16, 5};
        o5f.p1 = {11, 15, 3};
        o5g.p1 = {5, 18, 4};

        o8.robname = "Robot FPE";
        o8.of = true;
        o8.moving = false;
        o8.grasppos = true;
        o8.planninggrp = "Panda Teaching";

        o3.grippos = {12, 14, 18};

        o4.humannum = 1;

        o2.name = "Cube";
        o2.size = {5, 7, 9};
        o2.p1 = {13, 14, 17};
        o2.x = 2.3;
        o2.y = 1.2;
        o2.z = 0.2;
        o2.w = 0.1;


        o6.name = "Ball";
        o6.size = {1, 2, 3};
        o6.p1 = {12, 16, 17};
        o6.x = 1.1;
        o6.y = 1.4;
        o6.z = 1.6;
        o6.w = 1.9;

        //ros::Publisher turtle_vel
             //=node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);

        tf2_ros::Buffer tfBuffer;
        tf2_ros::TransformListener tfListener(tfBuffer);

        ros::Rate rate(10);

        while (node_handle.ok()) {
            //for (const auto &topic : ROSNODE1::topics)
            {
                geometry_msgs::TransformStamped transformStamped;
                try {
                    transformStamped = tfBuffer.lookupTransform("world", o5a.name, ros::Time(0));
                    transformStamped = tfBuffer.lookupTransform("world", o5b.name, ros::Time(0));
                    transformStamped = tfBuffer.lookupTransform("world", o5c.name, ros::Time(0));
                    transformStamped = tfBuffer.lookupTransform("world", o5d.name, ros::Time(0));
                    transformStamped = tfBuffer.lookupTransform("world", o5e.name, ros::Time(0));
                    transformStamped = tfBuffer.lookupTransform("world", o5f.name, ros::Time(0));
                    transformStamped = tfBuffer.lookupTransform("world", o5g.name, ros::Time(0));
                } catch (tf2::TransformException &ex) {
                    ROS_WARN("%s", ex.what());
                    ros::Duration(0.1).sleep();
                    continue;

                }
               // I did for one joint .If logically correct then will do for other joints .so far program is compiling

               o5a.p1.push_back(transformStamped.transform.translation.x);
               o5a.p1.push_back(transformStamped.transform.translation.y);
               o5a.p1.push_back(transformStamped.transform.translation.y);

               o5a.or2.w=transformStamped.transform.rotation.w;
               o5a.or2.x=transformStamped.transform.rotation.x;
               o5a.or2.y=transformStamped.transform.rotation.y;
               o5a.or2.z=transformStamped.transform.rotation.z;

              }

            rate.sleep();
        }


return 0;
}