Skip to content
Snippets Groups Projects
Select Git revision
  • e68116121835e2ac69b987c10857fcd3a7875d8c
  • master default protected
2 results

garage.cpp

Blame
  • appmod.cpp 13.16 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>
    #include <moveit/move_group_interface/move_group_interface.h>
    #include <moveit/planning_scene_interface/planning_scene_interface.h>
    #include <moveit_msgs/CollisionObject.h>
    #include <moveit_visual_tools/moveit_visual_tools.h>
    #include <moveit/trajectory_processing/iterative_time_parameterization.h>
    #include <trajectory_msgs/JointTrajectoryPoint.h>
    
    int main(int argc, char** argv) {
    
        ros::init(argc, argv, "Application Model");
        ros::NodeHandle node_handle;
        ros::AsyncSpinner spinner(1);
        spinner.start();
    
        // setup the planning environment
        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();
    
        move_group.setStartStateToCurrentState();
    
        geometry_msgs::Pose another_pose;
        another_pose.orientation.w = 1.0;
        another_pose.position.x = 0.4;
        another_pose.position.y = -0.4;
        another_pose.position.z = 0.9;
        move_group.setPoseTarget(another_pose);
    
        moveit::planning_interface::MoveGroupInterface::Plan my_plan;
        bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
        ROS_INFO("Visualizing plan %s", success ? "" : "FAILED");
    
        // Move the (simulated) robot in gazebo
        move_group.move();
    
        if(another_pose.position.z==0.9)
        ROS_INFO(" THE ROBOT HAS MOVED TO POSITION AS INTENDED ");
        else
            ROS_INFO("THE ROBOT HAS NOT MOVED TO THE POSITION AS INTENDED");
    
    
        ros::shutdown();
    
    
        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;
    
            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->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=1;
            int h=1;
            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) {
                        ROS_WARN("%s", ex.what());
                        ros::Duration(0.1).sleep();
                        continue;
                    }
                    if(h==7)
                        ROS_INFO(" TEST CASE SUCCESSFUL. ROBOT FPE HAS 7 JOINTS ");
                    else
                        ROS_ERROR(" TEST CASE NOT SUCCESSFUL. ROBOT FPE HAS WRONG NUMBER OF JOINTS ");
    
                    // 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");
                    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;
    }