diff --git a/cmake-build-debug/CMakeFiles/worldsafety.dir/src/worldsafety.cpp.o b/cmake-build-debug/CMakeFiles/worldsafety.dir/src/worldsafety.cpp.o index c30872c21a909016c501db3da5d00f1c1f45ef2c..8ef7f5659347b1ccaefed80b07a397e443c6b094 100644 Binary files a/cmake-build-debug/CMakeFiles/worldsafety.dir/src/worldsafety.cpp.o and b/cmake-build-debug/CMakeFiles/worldsafety.dir/src/worldsafety.cpp.o differ diff --git a/cmake-build-debug/devel/lib/robot_models/worldsafety b/cmake-build-debug/devel/lib/robot_models/worldsafety index 9bacd014d2bdf204e0dcb0720e6a3d618ab81401..9480aeac853a91ab37931945cedee719dbd381f2 100755 Binary files a/cmake-build-debug/devel/lib/robot_models/worldsafety and b/cmake-build-debug/devel/lib/robot_models/worldsafety differ diff --git a/src/robot_models_node.cpp b/src/robot_models_node.cpp index 5afa4ea8e103528533a15d061172fc3ea22fee8d..c1dd3347fc0e7872d35bd13e3ea3e8c080d40672 100644 --- a/src/robot_models_node.cpp +++ b/src/robot_models_node.cpp @@ -3,7 +3,6 @@ #include "datalink/graspobject.h" #include "datalink/hand.h" #include "datalink/humanspace.h" -#include "datalink/jo.h" #include "datalink/obstacle.h" #include "datalink/robot.h" #include "datalink/robotfpe.h" @@ -27,275 +26,277 @@ #include <tf2/LinearMath/Quaternion.h> #include <moveit/move_group_interface/move_group_interface.h> #include <moveit/planning_scene_interface/planning_scene_interface.h> - -void updateDectectedHumans(const std_msgs::uint32::ConstPtr& msg) -{ - ROS_INFO("I heard: [%s]", msg->data.c_str()); +#include <std_msgs/UInt32.h> +void updateDectectedHumans(const std_msgs::UInt32::ConstPtr &msg) { + ROS_INFO("I heard: [%i]", msg->data.c_str()); } - 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 robot1; - robotfpe o8; - world o9; - worldobject o10; - - robot1.jo.push_back(o5a); - robot1.jo.push_back(o5b); - robot1.jo.push_back(o5c); - robot1.jo.push_back(o5d); - robot1.jo.push_back(o5e); - robot1.jo.push_back(o5f); - robot1.jo.push_back(o5g); - /*robot1.jo1->name= "panda_link1"; - robot1.jo2->name= "panda_link2"; - robot1.jo3->name= "panda_link3"; - robot1.jo4->name= "panda_link4"; - robot1.jo5->name= "panda_link5"; - robot1.jo6->name= "panda_link6"; - robot1.jrobot1->name= "panda_link7"; - - robot1.jo1->or2= {1.2,5.3,6.5,7.4}; - robot1.jo2->or2= {2.5,3.6,5.8,4.9}; - robot1.jo3->or2= {5.1,1.2,9.9,6.8}; - robot1.jo4->or2= {6.3,3.4,8.5,9.6}; - robot1.jo5->or2= {8.2,2.6,7.9,5.5}; - robot1.jo6->or2= {7.3,4.4,6.7,4.0}; - robot1.jrobot1->or2= {7.3,4.4,6.7,4.0}; - - robot1.jo1->p1= {1.3, 10.1, 9.4}; - robot1.jo2->p1= {14.7, 11.3, 6.5}; - robot1.jo3->p1= {4.9, 13.8, 7.7}; - robot1.jo4->p1= {2.4, 12.5, 1.6}; - robot1.jo5->p1= {3.1, 16.2, 5.3}; - robot1.jo6->p1= {11.5, 15.4, 3.4}; - robot1.jrobot1->p1= {5.1, 18.3, 4.2};*/ - - int yx=0; - robot1.jo[++yx].name= "panda_link1"; - robot1.jo[++yx].name= "panda_link2"; - robot1.jo[++yx].name= "panda_link3"; - robot1.jo[++yx].name= "panda_link4"; - robot1.jo[++yx].name= "panda_link5"; - robot1.jo[++yx].name= "panda_link6"; - robot1.jo[++yx].name= "panda_link7"; - - - 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}; - - 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; - - ros::Publisher chatter_pub = namespace_name.advertise<std_msgs::String>("chatter", 1000); - ros::Rate loop_rate(10); - int count = 0; - while (ros::ok()) { - std_msgs::uint32 msg; - std::stringstream ss; - ss << "hello world " << count; - msg.data = ss.str(); - ROS_INFO("%s", msg.data.c_str()); - chatter_pub.publish(msg); - ros::spinOnce(); - loop_rate.sleep(); - ++count; - } - - ros::Subscriber sub = namespace_name.subscribe("chatter", 1000, updateDectectedHumans); - ros::spin(); - - 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 transformStamped1; - geometry_msgs::TransformStamped transformStamped2; - geometry_msgs::TransformStamped transformStamped3; - geometry_msgs::TransformStamped transformStamped4; - geometry_msgs::TransformStamped transformStamped5; - geometry_msgs::TransformStamped transformStamped6; - geometry_msgs::TransformStamped transformStamped7; - uint32::data updateDetectedHumans; - geometry_msgs::uint32 x; - try { - transformStamped1 = tfBuffer.lookupTransform("world", robot1.jo[1].name, ros::Time(0)); - transformStamped2 = tfBuffer.lookupTransform("world", robot1.jo[2].name, ros::Time(0)); - transformStamped3 = tfBuffer.lookupTransform("world", robot1.jo[3].name, ros::Time(0)); - transformStamped4 = tfBuffer.lookupTransform("world", robot1.jo[4].name, ros::Time(0)); - transformStamped5 = tfBuffer.lookupTransform("world", robot1.jo[5].name, ros::Time(0)); - transformStamped6 = tfBuffer.lookupTransform("world", robot1.jo[6].name, ros::Time(0)); - transformStamped7 = tfBuffer.lookupTransform("world", robot1.jo[7].name, ros::Time(0)); - updateDetectedHumans=o9.a->humannum; - } catch (tf2::TransformException &ex) { - ROS_WARN("%s", ex.what()); - ros::Duration(0.1).sleep(); - continue; - } - - robot1.jo[3].p1[0]=0.0; - robot1.jo[3].p1[1]=0.0; - robot1.jo[3].p1[2]=0.0; - robot1.jo[4].p1[0]=0.0; - robot1.jo[4].p1[1]=0.0; - robot1.jo[4].p1[2]=0.0; - robot1.jo[5].p1[0]=0.0; - robot1.jo[5].p1[1]=0.0; - robot1.jo[5].p1[2]=0.0; - robot1.jo[6].p1[0]=0.0; - robot1.jo[6].p1[1]=0.0; - robot1.jo[6].p1[2]=0.0; - robot1.jo[7].p1[0]=0.0; - robot1.jo[7].p1[1]=0.0; - robot1.jo[7].p1[2]=0.0; - - robot1.jo[1].p1[0]=transformStamped1.transform.translation.x; - robot1.jo[1].p1[1]=transformStamped1.transform.translation.y; - robot1.jo[1].p1[2]=transformStamped1.transform.translation.z; - ROS_INFO_STREAM("panda_link1 Position is " <<" x = "<< robot1.jo[1].p1[0] << ", y = "<< robot1.jo[1].p1[1]<<", z = "<<robot1.jo[1].p1[2]); - - robot1.jo[2].p1[0]=transformStamped2.transform.translation.x; - robot1.jo[2].p1[1]=transformStamped2.transform.translation.y; - robot1.jo[2].p1[2]=transformStamped2.transform.translation.z; - ROS_INFO_STREAM("panda_link2 Position is " <<" x = "<< robot1.jo[2].p1[0] << ", y = "<< robot1.jo[2].p1[1]<<", z = "<<robot1.jo[2].p1[2]); - - robot1.jo[3].p1[0]=transformStamped3.transform.translation.x; - robot1.jo[3].p1[1]=transformStamped3.transform.translation.y; - robot1.jo[3].p1[2]=transformStamped3.transform.translation.z; - ROS_INFO_STREAM("panda_link3 Position is " <<" x = "<< robot1.jo[3].p1[0] << ", y = "<< robot1.jo[3].p1[1]<<", z = "<<robot1.jo[3].p1[2]); - - robot1.jo[4].p1[0]=transformStamped4.transform.translation.x; - robot1.jo[4].p1[1]=transformStamped4.transform.translation.y; - robot1.jo[4].p1[2]=transformStamped4.transform.translation.z; - ROS_INFO_STREAM("panda_link4 Position is " <<" x = "<< robot1.jo[4].p1[0] << ", y = "<< robot1.jo[4].p1[1]<<", z = "<<robot1.jo[4].p1[2]); - - robot1.jo[5].p1[0]=transformStamped5.transform.translation.x; - robot1.jo[5].p1[1]=transformStamped5.transform.translation.y; - robot1.jo[5].p1[2]=transformStamped5.transform.translation.z; - ROS_INFO_STREAM("panda_link5 Position is " <<" x = "<< robot1.jo[5].p1[0] << ", y = "<< robot1.jo[5].p1[1]<<", z = "<<robot1.jo[5].p1[2]); - - robot1.jo[6].p1[0]=transformStamped6.transform.translation.x; - robot1.jo[6].p1[1]=transformStamped6.transform.translation.y; - robot1.jo[6].p1[2]=transformStamped6.transform.translation.z; - ROS_INFO_STREAM("panda_link6 Position is " <<" x = "<< robot1.jo[6].p1[0] << ", y = "<< robot1.jo[6].p1[1]<<", z = "<<robot1.jo[6].p1[2]); - - robot1.jo[7].p1[0]=transformStamped7.transform.translation.x; - robot1.jo[7].p1[1]=transformStamped7.transform.translation.y; - robot1.jo[7].p1[2]=transformStamped7.transform.translation.z; - ROS_INFO_STREAM("panda_link7 Position is " <<" x = "<< robot1.jo[7].p1[0] << ", y = "<< robot1.jo[7].p1[1]<<", z = "<<robot1.jo[7].p1[2]); - - robot1.jo[1].or2.w=0.0; - robot1.jo[1].or2.x=0.0; - robot1.jo[1].or2.y=0.0; - robot1.jo[1].or2.z=0.0; - robot1.jo[2].or2.w=0.0; - robot1.jo[2].or2.x=0.0; - robot1.jo[2].or2.y=0.0; - robot1.jo[2].or2.z=0.0; - robot1.jo[3].or2.w=0.0; - robot1.jo[3].or2.x=0.0; - robot1.jo[3].or2.y=0.0; - robot1.jo[3].or2.z=0.0; - robot1.jo[4].or2.w=0.0; - robot1.jo[4].or2.x=0.0; - robot1.jo[4].or2.y=0.0; - robot1.jo[4].or2.z=0.0; - robot1.jo[5].or2.w=0.0; - robot1.jo[5].or2.x=0.0; - robot1.jo[5].or2.y=0.0; - robot1.jo[5].or2.z=0.0; - robot1.jo[6].or2.w=0.0; - robot1.jo[6].or2.x=0.0; - robot1.jo[6].or2.y=0.0; - robot1.jo[6].or2.z=0.0; - robot1.jo[7].or2.w=0.0; - robot1.jo[7].or2.x=0.0; - robot1.jo[7].or2.y=0.0; - robot1.jo[7].or2.z=0.0; - - robot1.jo[1].or2.w=transformStamped1.transform.rotation.w; - robot1.jo[1].or2.x=transformStamped1.transform.rotation.x; - robot1.jo[1].or2.y=transformStamped1.transform.rotation.y; - robot1.jo[1].or2.z=transformStamped1.transform.rotation.z; - ROS_INFO_STREAM("panda_link1 Orientation is " <<" w = "<< robot1.jo[1].or2.w<<", x = "<<robot1.jo[1].or2.x<<", y = "<<robot1.jo[1].or2.y<<", z = "<<robot1.jo[1].or2.z); - - robot1.jo[2].or2.w=transformStamped2.transform.rotation.w; - robot1.jo[2].or2.x=transformStamped2.transform.rotation.x; - robot1.jo[2].or2.y=transformStamped2.transform.rotation.y; - robot1.jo[2].or2.z=transformStamped2.transform.rotation.z; - ROS_INFO_STREAM("panda_link2 Orientation is " <<" w = "<< robot1.jo[2].or2.w<<", x = "<< robot1.jo[2].or2.x<<", y = "<< robot1.jo[2].or2.y<<", z = "<< robot1.jo[2].or2.z); - - robot1.jo[3].or2.w=transformStamped3.transform.rotation.w; - robot1.jo[3].or2.x=transformStamped3.transform.rotation.x; - robot1.jo[3].or2.y=transformStamped3.transform.rotation.y; - robot1.jo[3].or2.z=transformStamped3.transform.rotation.z; - ROS_INFO_STREAM("panda_link3 Orientation is " <<" w = "<< robot1.jo[3].or2.w<<", x = "<<robot1.jo[3].or2.x<<", y = "<<robot1.jo[3].or2.y<<", z = "<<robot1.jo[3].or2.z); - - robot1.jo[4].or2.w=transformStamped4.transform.rotation.w; - robot1.jo[4].or2.x=transformStamped4.transform.rotation.x; - robot1.jo[4].or2.y=transformStamped4.transform.rotation.y; - robot1.jo[4].or2.z=transformStamped4.transform.rotation.z; - ROS_INFO_STREAM("panda_link4 Orientation is " <<" w = "<<robot1.jo[4].or2.w<<", x = "<<robot1.jo[4].or2.x<<", y = "<<robot1.jo[4].or2.y<<", z = "<<robot1.jo[4].or2.z); - - robot1.jo[5].or2.w=transformStamped5.transform.rotation.w; - robot1.jo[5].or2.x=transformStamped5.transform.rotation.x; - robot1.jo[5].or2.y=transformStamped5.transform.rotation.y; - robot1.jo[5].or2.z=transformStamped5.transform.rotation.z; - ROS_INFO_STREAM("panda_link5 Orientation is " <<" w = "<< robot1.jo[5].or2.w<<", x = "<<robot1.jo[5].or2.x<<", y = "<<robot1.jo[5].or2.y<<", z = "<<robot1.jo[5].or2.z); - - robot1.jo[6].or2.w=transformStamped6.transform.rotation.w; - robot1.jo[6].or2.x=transformStamped6.transform.rotation.x; - robot1.jo[6].or2.y=transformStamped6.transform.rotation.y; - robot1.jo[6].or2.z=transformStamped6.transform.rotation.z; - ROS_INFO_STREAM("panda_link6 Orientation is " <<" w = "<< robot1.jo[6].or2.w<<", x = "<<robot1.jo[6].or2.x<<", y = "<<robot1.jo[6].or2.y<<", z = "<<robot1.jo[6].or2.z); - - robot1.jo[7].or2.w=transformStamped7.transform.rotation.w; - robot1.jo[7].or2.x=transformStamped7.transform.rotation.x; - robot1.jo[7].or2.y=transformStamped7.transform.rotation.y; - robot1.jo[7].or2.z=transformStamped7.transform.rotation.z; - ROS_INFO_STREAM("panda_link7 Orientation is " <<" w = "<<robot1.jo[7].or2.w<<", x = "<<robot1.jo[7].or2.x<<", y = "<<robot1.jo[7].or2.y<<", z = "<<robot1.jo[7].or2.z); - } - rate.sleep(); +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 robot1; + robotfpe o8; + world o9; + worldobject o10; + + robot1.jo.push_back(o5a); + robot1.jo.push_back(o5b); + robot1.jo.push_back(o5c); + robot1.jo.push_back(o5d); + robot1.jo.push_back(o5e); + robot1.jo.push_back(o5f); + robot1.jo.push_back(o5g); + + int yx = 0; + robot1.jo[++yx].name = "panda_link1"; + robot1.jo[++yx].name = "panda_link2"; + robot1.jo[++yx].name = "panda_link3"; + robot1.jo[++yx].name = "panda_link4"; + robot1.jo[++yx].name = "panda_link5"; + robot1.jo[++yx].name = "panda_link6"; + robot1.jo[++yx].name = "panda_link7"; + + 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}; + + 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; + + ros::Publisher chatter_pub = namespace_name.advertise<std_msgs::String>("chatter", 1000); + ros::Rate loop_rate(10); + int count = 0; + while (ros::ok()) { + std_msgs::UInt32 msg; + std::stringstream ss; + ss << "hello world " << count; + msg.data = ss.str(); + ROS_INFO("%s", msg.data.c_str()); + chatter_pub.publish(msg); + ros::spinOnce(); + loop_rate.sleep(); + ++count; + } + + ros::Subscriber sub = namespace_name.subscribe("chatter", 1000, updateDectectedHumans); + ros::spin(); + + 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 transformStamped1; + geometry_msgs::TransformStamped transformStamped2; + geometry_msgs::TransformStamped transformStamped3; + geometry_msgs::TransformStamped transformStamped4; + geometry_msgs::TransformStamped transformStamped5; + geometry_msgs::TransformStamped transformStamped6; + geometry_msgs::TransformStamped transformStamped7; + UInt32::data updateDetectedHumans; + geometry_msgs::UInt32 x; + try { + transformStamped1 = tfBuffer.lookupTransform("world", robot1.jo[1].name, ros::Time(0)); + transformStamped2 = tfBuffer.lookupTransform("world", robot1.jo[2].name, ros::Time(0)); + transformStamped3 = tfBuffer.lookupTransform("world", robot1.jo[3].name, ros::Time(0)); + transformStamped4 = tfBuffer.lookupTransform("world", robot1.jo[4].name, ros::Time(0)); + transformStamped5 = tfBuffer.lookupTransform("world", robot1.jo[5].name, ros::Time(0)); + transformStamped6 = tfBuffer.lookupTransform("world", robot1.jo[6].name, ros::Time(0)); + transformStamped7 = tfBuffer.lookupTransform("world", robot1.jo[7].name, ros::Time(0)); + updateDetectedHumans = o9.a->humannum; + } catch (tf2::TransformException &ex) { + ROS_WARN("%s", ex.what()); + ros::Duration(0.1).sleep(); + continue; + } + + robot1.jo[3].p1[0] = 0.0; + robot1.jo[3].p1[1] = 0.0; + robot1.jo[3].p1[2] = 0.0; + robot1.jo[4].p1[0] = 0.0; + robot1.jo[4].p1[1] = 0.0; + robot1.jo[4].p1[2] = 0.0; + robot1.jo[5].p1[0] = 0.0; + robot1.jo[5].p1[1] = 0.0; + robot1.jo[5].p1[2] = 0.0; + robot1.jo[6].p1[0] = 0.0; + robot1.jo[6].p1[1] = 0.0; + robot1.jo[6].p1[2] = 0.0; + robot1.jo[7].p1[0] = 0.0; + robot1.jo[7].p1[1] = 0.0; + robot1.jo[7].p1[2] = 0.0; + + robot1.jo[1].p1[0] = transformStamped1.transform.translation.x; + robot1.jo[1].p1[1] = transformStamped1.transform.translation.y; + robot1.jo[1].p1[2] = transformStamped1.transform.translation.z; + ROS_INFO_STREAM( + "panda_link1 Position is " << " x = " << robot1.jo[1].p1[0] << ", y = " << robot1.jo[1].p1[1] + << ", z = " << robot1.jo[1].p1[2]); + + robot1.jo[2].p1[0] = transformStamped2.transform.translation.x; + robot1.jo[2].p1[1] = transformStamped2.transform.translation.y; + robot1.jo[2].p1[2] = transformStamped2.transform.translation.z; + ROS_INFO_STREAM( + "panda_link2 Position is " << " x = " << robot1.jo[2].p1[0] << ", y = " << robot1.jo[2].p1[1] + << ", z = " << robot1.jo[2].p1[2]); + + robot1.jo[3].p1[0] = transformStamped3.transform.translation.x; + robot1.jo[3].p1[1] = transformStamped3.transform.translation.y; + robot1.jo[3].p1[2] = transformStamped3.transform.translation.z; + ROS_INFO_STREAM( + "panda_link3 Position is " << " x = " << robot1.jo[3].p1[0] << ", y = " << robot1.jo[3].p1[1] + << ", z = " << robot1.jo[3].p1[2]); + + robot1.jo[4].p1[0] = transformStamped4.transform.translation.x; + robot1.jo[4].p1[1] = transformStamped4.transform.translation.y; + robot1.jo[4].p1[2] = transformStamped4.transform.translation.z; + ROS_INFO_STREAM( + "panda_link4 Position is " << " x = " << robot1.jo[4].p1[0] << ", y = " << robot1.jo[4].p1[1] + << ", z = " << robot1.jo[4].p1[2]); + + robot1.jo[5].p1[0] = transformStamped5.transform.translation.x; + robot1.jo[5].p1[1] = transformStamped5.transform.translation.y; + robot1.jo[5].p1[2] = transformStamped5.transform.translation.z; + ROS_INFO_STREAM( + "panda_link5 Position is " << " x = " << robot1.jo[5].p1[0] << ", y = " << robot1.jo[5].p1[1] + << ", z = " << robot1.jo[5].p1[2]); + + robot1.jo[6].p1[0] = transformStamped6.transform.translation.x; + robot1.jo[6].p1[1] = transformStamped6.transform.translation.y; + robot1.jo[6].p1[2] = transformStamped6.transform.translation.z; + ROS_INFO_STREAM( + "panda_link6 Position is " << " x = " << robot1.jo[6].p1[0] << ", y = " << robot1.jo[6].p1[1] + << ", z = " << robot1.jo[6].p1[2]); + + robot1.jo[7].p1[0] = transformStamped7.transform.translation.x; + robot1.jo[7].p1[1] = transformStamped7.transform.translation.y; + robot1.jo[7].p1[2] = transformStamped7.transform.translation.z; + ROS_INFO_STREAM( + "panda_link7 Position is " << " x = " << robot1.jo[7].p1[0] << ", y = " << robot1.jo[7].p1[1] + << ", z = " << robot1.jo[7].p1[2]); + + robot1.jo[1].or2.w = 0.0; + robot1.jo[1].or2.x = 0.0; + robot1.jo[1].or2.y = 0.0; + robot1.jo[1].or2.z = 0.0; + robot1.jo[2].or2.w = 0.0; + robot1.jo[2].or2.x = 0.0; + robot1.jo[2].or2.y = 0.0; + robot1.jo[2].or2.z = 0.0; + robot1.jo[3].or2.w = 0.0; + robot1.jo[3].or2.x = 0.0; + robot1.jo[3].or2.y = 0.0; + robot1.jo[3].or2.z = 0.0; + robot1.jo[4].or2.w = 0.0; + robot1.jo[4].or2.x = 0.0; + robot1.jo[4].or2.y = 0.0; + robot1.jo[4].or2.z = 0.0; + robot1.jo[5].or2.w = 0.0; + robot1.jo[5].or2.x = 0.0; + robot1.jo[5].or2.y = 0.0; + robot1.jo[5].or2.z = 0.0; + robot1.jo[6].or2.w = 0.0; + robot1.jo[6].or2.x = 0.0; + robot1.jo[6].or2.y = 0.0; + robot1.jo[6].or2.z = 0.0; + robot1.jo[7].or2.w = 0.0; + robot1.jo[7].or2.x = 0.0; + robot1.jo[7].or2.y = 0.0; + robot1.jo[7].or2.z = 0.0; + + robot1.jo[1].or2.w = transformStamped1.transform.rotation.w; + robot1.jo[1].or2.x = transformStamped1.transform.rotation.x; + robot1.jo[1].or2.y = transformStamped1.transform.rotation.y; + robot1.jo[1].or2.z = transformStamped1.transform.rotation.z; + ROS_INFO_STREAM( + "panda_link1 Orientation is " << " w = " << robot1.jo[1].or2.w << ", x = " << robot1.jo[1].or2.x + << ", y = " << robot1.jo[1].or2.y << ", z = " << robot1.jo[1].or2.z); + + robot1.jo[2].or2.w = transformStamped2.transform.rotation.w; + robot1.jo[2].or2.x = transformStamped2.transform.rotation.x; + robot1.jo[2].or2.y = transformStamped2.transform.rotation.y; + robot1.jo[2].or2.z = transformStamped2.transform.rotation.z; + ROS_INFO_STREAM( + "panda_link2 Orientation is " << " w = " << robot1.jo[2].or2.w << ", x = " << robot1.jo[2].or2.x + << ", y = " << robot1.jo[2].or2.y << ", z = " << robot1.jo[2].or2.z); + + robot1.jo[3].or2.w = transformStamped3.transform.rotation.w; + robot1.jo[3].or2.x = transformStamped3.transform.rotation.x; + robot1.jo[3].or2.y = transformStamped3.transform.rotation.y; + robot1.jo[3].or2.z = transformStamped3.transform.rotation.z; + ROS_INFO_STREAM( + "panda_link3 Orientation is " << " w = " << robot1.jo[3].or2.w << ", x = " << robot1.jo[3].or2.x + << ", y = " << robot1.jo[3].or2.y << ", z = " << robot1.jo[3].or2.z); + + robot1.jo[4].or2.w = transformStamped4.transform.rotation.w; + robot1.jo[4].or2.x = transformStamped4.transform.rotation.x; + robot1.jo[4].or2.y = transformStamped4.transform.rotation.y; + robot1.jo[4].or2.z = transformStamped4.transform.rotation.z; + ROS_INFO_STREAM( + "panda_link4 Orientation is " << " w = " << robot1.jo[4].or2.w << ", x = " << robot1.jo[4].or2.x + << ", y = " << robot1.jo[4].or2.y << ", z = " << robot1.jo[4].or2.z); + + robot1.jo[5].or2.w = transformStamped5.transform.rotation.w; + robot1.jo[5].or2.x = transformStamped5.transform.rotation.x; + robot1.jo[5].or2.y = transformStamped5.transform.rotation.y; + robot1.jo[5].or2.z = transformStamped5.transform.rotation.z; + ROS_INFO_STREAM( + "panda_link5 Orientation is " << " w = " << robot1.jo[5].or2.w << ", x = " << robot1.jo[5].or2.x + << ", y = " << robot1.jo[5].or2.y << ", z = " << robot1.jo[5].or2.z); + + robot1.jo[6].or2.w = transformStamped6.transform.rotation.w; + robot1.jo[6].or2.x = transformStamped6.transform.rotation.x; + robot1.jo[6].or2.y = transformStamped6.transform.rotation.y; + robot1.jo[6].or2.z = transformStamped6.transform.rotation.z; + ROS_INFO_STREAM( + "panda_link6 Orientation is " << " w = " << robot1.jo[6].or2.w << ", x = " << robot1.jo[6].or2.x + << ", y = " << robot1.jo[6].or2.y << ", z = " << robot1.jo[6].or2.z); + + robot1.jo[7].or2.w = transformStamped7.transform.rotation.w; + robot1.jo[7].or2.x = transformStamped7.transform.rotation.x; + robot1.jo[7].or2.y = transformStamped7.transform.rotation.y; + robot1.jo[7].or2.z = transformStamped7.transform.rotation.z; + ROS_INFO_STREAM( + "panda_link7 Orientation is " << " w = " << robot1.jo[7].or2.w << ", x = " << robot1.jo[7].or2.x + << ", y = " << robot1.jo[7].or2.y << ", z = " << robot1.jo[7].or2.z); } -return 0; + rate.sleep(); + } + return 0; } \ No newline at end of file diff --git a/src/worldsafety.cpp b/src/worldsafety.cpp index 468c8ec1546f03ab3638195f252c8c2391aa33f0..3060e7fc87ecdf379fb12a8140cd1d2d9562dceb 100644 --- a/src/worldsafety.cpp +++ b/src/worldsafety.cpp @@ -31,8 +31,7 @@ #include <moveit/trajectory_processing/iterative_time_parameterization.h> #include "trajectory_msgs/JointTrajectoryPoint.h" -int main(int argc, char** argv) -{ +int main(int argc, char **argv) { ros::init(argc, argv, "Safety_Model"); ros::NodeHandle node_handle; ros::AsyncSpinner spinner(1); @@ -64,46 +63,44 @@ int main(int argc, char** argv) robot1.jo.push_back(o5f); robot1.jo.push_back(o5g); - int yx=0; - robot1.jo[++yx].name= "panda_link1"; - robot1.jo[++yx].name= "panda_link2"; - robot1.jo[++yx].name= "panda_link3"; - robot1.jo[++yx].name= "panda_link4"; - robot1.jo[++yx].name= "panda_link5"; - robot1.jo[++yx].name= "panda_link6"; - robot1.jo[++yx].name= "panda_link7"; + int yx = 0; + robot1.jo[++yx].name = "panda_link1"; + robot1.jo[++yx].name = "panda_link2"; + robot1.jo[++yx].name = "panda_link3"; + robot1.jo[++yx].name = "panda_link4"; + robot1.jo[++yx].name = "panda_link5"; + robot1.jo[++yx].name = "panda_link6"; + robot1.jo[++yx].name = "panda_link7"; - o9.c->robname="Robot FPE"; - o9.c->of=true; - o9.c->moving=false; - o9.c->grasppos=true; - o9.c->planninggrp= "Panda Teaching"; + 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}; + 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->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->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; // Static Tests - World Model Safety Test Cases to check Segmentation error // test the properties of the model that do not change over time, such as the structure of you model - if (robot1.jo.size() == 7) - { ROS_INFO_STREAM(" TEST CASE SUCCESSFUL. ROBOT FPE HAS 7 JOINTS ");} - else - { ROS_ERROR_STREAM(" TEST CASE FAILED. ROBOT FPE HAS WRONG NUMBER OF JOINTS = " << robot1.jo.size());} + if (robot1.jo.size() == 7) { ROS_INFO_STREAM(" TEST CASE SUCCESSFUL. ROBOT FPE HAS 7 JOINTS "); } + else { ROS_ERROR_STREAM(" TEST CASE FAILED. ROBOT FPE HAS WRONG NUMBER OF JOINTS = " << robot1.jo.size()); } if (o8.h == nullptr) ROS_ERROR("ERROR IN HAND POINTER IN ROBOT FPE CLASS AGGREGATION RELATION"); @@ -146,33 +143,47 @@ int main(int argc, char** argv) tf2_ros::Buffer tfBuffer; tf2_ros::TransformListener tfListener(tfBuffer); ros::Rate rate(10); - //robot1.jo[7].p1[2]=0.0; - int i=0; - int ho=0; - while (node_handle.ok()) - { - geometry_msgs::TransformStamped transformStamped[7]; - try { - transformStamped[++i] = tfBuffer.lookupTransform("world", robot1.jo[++ho].name, ros::Time(0)); - } catch (tf2::TransformException &ex) { - ROS_WARN("%s", ex.what()); - ros::Duration(0.1).sleep(); - continue; - } - robot1.jo[i].p1[2] = transformStamped[ho].transform.translation.z; - }//End of taking input in program - // Begin test case to check if robot moved to its position - while (node_handle.ok()) - { - { - // update the model data like in the application model node, test the dynamic parts here, i.e., if the model data is consistent with the position of the robot - //Checking if robot moved to position or not - if (robot1.jo[7].p1[2] == 0.9) - ROS_INFO_STREAM(" THE ROBOT HAS MOVED TO POSITION AS INTENDED "); - else - ROS_ERROR_STREAM("THE ROBOT HAS NOT MOVED TO THE POSITION AS INTENDED"); - } + + while (node_handle.ok()) { + geometry_msgs::TransformStamped transformStamped1; + geometry_msgs::TransformStamped transformStamped2; + geometry_msgs::TransformStamped transformStamped3; + geometry_msgs::TransformStamped transformStamped4; + geometry_msgs::TransformStamped transformStamped5; + geometry_msgs::TransformStamped transformStamped6; + geometry_msgs::TransformStamped transformStamped7; + try { + transformStamped1 = tfBuffer.lookupTransform("world", robot1.jo[1].name, ros::Time(0)); + transformStamped2 = tfBuffer.lookupTransform("world", robot1.jo[2].name, ros::Time(0)); + transformStamped3 = tfBuffer.lookupTransform("world", robot1.jo[3].name, ros::Time(0)); + transformStamped4 = tfBuffer.lookupTransform("world", robot1.jo[4].name, ros::Time(0)); + transformStamped5 = tfBuffer.lookupTransform("world", robot1.jo[5].name, ros::Time(0)); + transformStamped6 = tfBuffer.lookupTransform("world", robot1.jo[6].name, ros::Time(0)); + transformStamped7 = tfBuffer.lookupTransform("world", robot1.jo[7].name, ros::Time(0)); + robot1.jo[7].p1[2] = transformStamped7.transform.translation.z; + if (robot1.jo[7].p1[2] == 0.9) + ROS_INFO_STREAM(" THE ROBOT HAS MOVED TO POSITION AS INTENDED "); + else + ROS_ERROR_STREAM("THE ROBOT HAS NOT MOVED TO THE POSITION AS INTENDED"); + rate.sleep(); + break; + } catch (tf2::TransformException &ex) { + ROS_WARN("%s", ex.what()); + ros::Duration(0.1).sleep(); + continue; + } + } + // Dynamic Tests + while (node_handle.ok()) { + {// update the model data like in the application model node, test the dynamic parts here, i.e., if the model data is consistent with the position of the robot + //Checking if robot moved to position or not + if (robot1.jo[7].p1[2] == 0.9) + ROS_INFO_STREAM(" THE ROBOT HAS MOVED TO POSITION AS INTENDED "); + else + ROS_ERROR_STREAM("THE ROBOT HAS NOT MOVED TO THE POSITION AS INTENDED"); rate.sleep(); + } + } - return 0; + return 0; }