Skip to content
Snippets Groups Projects
Select Git revision
  • 95a40135a7745f3aebe64f236922be9dcde95a8a
  • master default
2 results

robot_models_node.cpp

Blame
    • robot_models_node.cpp 4.13 KiB 
    #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.x = 1.7;
        o5b.y = 1.8;
        o5c.z = 1.9;
        o5d.w = 2.0;
        o5a.p1 = {12, 11, 9, 18};

    
        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;


        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("panda_joint1", o5a.name, ros::Time(0));
                    transformStamped = tfBuffer.lookupTransform("panda_joint2", o5b.name, ros::Time(0));
                    transformStamped = tfBuffer.lookupTransform("panda_joint3", o5c.name, ros::Time(0));
                    transformStamped = tfBuffer.lookupTransform("panda_joint4", o5d.name, ros::Time(0));
                    transformStamped = tfBuffer.lookupTransform("panda_joint5", o5e.name, ros::Time(0));
                    transformStamped = tfBuffer.lookupTransform("panda_joint6", o5f.name, ros::Time(0));
                    transformStamped = tfBuffer.lookupTransform("panda_joint7", o5g.name, ros::Time(0));
                } catch (tf2::TransformException &ex) {
                    ROS_WARN("%s", ex.what());
                    ros::Duration(0.1).sleep();
                    continue;

                }
            }
            rate.sleep();
        }


return 0;
}