Skip to content
Snippets Groups Projects
Commit 30e901da authored by Nikhil Ambardar's avatar Nikhil Ambardar
Browse files

contradictoryadvice

parent 8bd84fc2
Branches
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
cmake-build-debug/CMakeFiles/worldsafety.dir/src/worldsafety.cpp.o
+ 0
− 0
View file @ 30e901da
No preview for this file type
cmake-build-debug/devel/lib/robot_models/worldsafety
+ 0
− 0
View file @ 30e901da
No preview for this file type
src/robot_models_node.cpp
+ 270
− 269
View file @ 30e901da
...@@ -3,7 +3,6 @@ ...@@ -3,7 +3,6 @@
#include "datalink/graspobject.h" #include "datalink/graspobject.h"
#include "datalink/hand.h" #include "datalink/hand.h"
#include "datalink/humanspace.h" #include "datalink/humanspace.h"
#include "datalink/jo.h"
#include "datalink/obstacle.h" #include "datalink/obstacle.h"
#include "datalink/robot.h" #include "datalink/robot.h"
#include "datalink/robotfpe.h" #include "datalink/robotfpe.h"
...@@ -27,10 +26,9 @@ ...@@ -27,10 +26,9 @@
#include <tf2/LinearMath/Quaternion.h> #include <tf2/LinearMath/Quaternion.h>
#include <moveit/move_group_interface/move_group_interface.h> #include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h> #include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <std_msgs/UInt32.h>
void updateDectectedHumans(const std_msgs::uint32::ConstPtr& msg) void updateDectectedHumans(const std_msgs::UInt32::ConstPtr &msg) {
{ ROS_INFO("I heard: [%i]", msg->data.c_str());
ROS_INFO("I heard: [%s]", msg->data.c_str());
} }
int main(int argc, char **argv) { int main(int argc, char **argv) {
...@@ -64,29 +62,6 @@ void updateDectectedHumans(const std_msgs::uint32::ConstPtr& msg) ...@@ -64,29 +62,6 @@ void updateDectectedHumans(const std_msgs::uint32::ConstPtr& msg)
robot1.jo.push_back(o5e); robot1.jo.push_back(o5e);
robot1.jo.push_back(o5f); robot1.jo.push_back(o5f);
robot1.jo.push_back(o5g); 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; int yx = 0;
robot1.jo[++yx].name = "panda_link1"; robot1.jo[++yx].name = "panda_link1";
...@@ -97,14 +72,12 @@ void updateDectectedHumans(const std_msgs::uint32::ConstPtr& msg) ...@@ -97,14 +72,12 @@ void updateDectectedHumans(const std_msgs::uint32::ConstPtr& msg)
robot1.jo[++yx].name = "panda_link6"; robot1.jo[++yx].name = "panda_link6";
robot1.jo[++yx].name = "panda_link7"; robot1.jo[++yx].name = "panda_link7";
o9.c->robname = "Robot FPE"; o9.c->robname = "Robot FPE";
o9.c->of = true; o9.c->of = true;
o9.c->moving = false; o9.c->moving = false;
o9.c->grasppos = true; o9.c->grasppos = true;
o9.c->planninggrp = "Panda Teaching"; o9.c->planninggrp = "Panda Teaching";
o8.h->grippos = {12.1, 14.3, 18.2}; o8.h->grippos = {12.1, 14.3, 18.2};
o9.a->humannum = 1; o9.a->humannum = 1;
...@@ -128,7 +101,7 @@ void updateDectectedHumans(const std_msgs::uint32::ConstPtr& msg) ...@@ -128,7 +101,7 @@ void updateDectectedHumans(const std_msgs::uint32::ConstPtr& msg)
ros::Rate loop_rate(10); ros::Rate loop_rate(10);
int count = 0; int count = 0;
while (ros::ok()) { while (ros::ok()) {
std_msgs::uint32 msg; std_msgs::UInt32 msg;
std::stringstream ss; std::stringstream ss;
ss << "hello world " << count; ss << "hello world " << count;
msg.data = ss.str(); msg.data = ss.str();
...@@ -156,8 +129,8 @@ void updateDectectedHumans(const std_msgs::uint32::ConstPtr& msg) ...@@ -156,8 +129,8 @@ void updateDectectedHumans(const std_msgs::uint32::ConstPtr& msg)
geometry_msgs::TransformStamped transformStamped5; geometry_msgs::TransformStamped transformStamped5;
geometry_msgs::TransformStamped transformStamped6; geometry_msgs::TransformStamped transformStamped6;
geometry_msgs::TransformStamped transformStamped7; geometry_msgs::TransformStamped transformStamped7;
uint32::data updateDetectedHumans; UInt32::data updateDetectedHumans;
geometry_msgs::uint32 x; geometry_msgs::UInt32 x;
try { try {
transformStamped1 = tfBuffer.lookupTransform("world", robot1.jo[1].name, ros::Time(0)); transformStamped1 = tfBuffer.lookupTransform("world", robot1.jo[1].name, ros::Time(0));
transformStamped2 = tfBuffer.lookupTransform("world", robot1.jo[2].name, ros::Time(0)); transformStamped2 = tfBuffer.lookupTransform("world", robot1.jo[2].name, ros::Time(0));
...@@ -192,37 +165,51 @@ void updateDectectedHumans(const std_msgs::uint32::ConstPtr& msg) ...@@ -192,37 +165,51 @@ void updateDectectedHumans(const std_msgs::uint32::ConstPtr& msg)
robot1.jo[1].p1[0] = transformStamped1.transform.translation.x; robot1.jo[1].p1[0] = transformStamped1.transform.translation.x;
robot1.jo[1].p1[1] = transformStamped1.transform.translation.y; robot1.jo[1].p1[1] = transformStamped1.transform.translation.y;
robot1.jo[1].p1[2] = transformStamped1.transform.translation.z; 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]); 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[0] = transformStamped2.transform.translation.x;
robot1.jo[2].p1[1] = transformStamped2.transform.translation.y; robot1.jo[2].p1[1] = transformStamped2.transform.translation.y;
robot1.jo[2].p1[2] = transformStamped2.transform.translation.z; 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]); 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[0] = transformStamped3.transform.translation.x;
robot1.jo[3].p1[1] = transformStamped3.transform.translation.y; robot1.jo[3].p1[1] = transformStamped3.transform.translation.y;
robot1.jo[3].p1[2] = transformStamped3.transform.translation.z; 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]); 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[0] = transformStamped4.transform.translation.x;
robot1.jo[4].p1[1] = transformStamped4.transform.translation.y; robot1.jo[4].p1[1] = transformStamped4.transform.translation.y;
robot1.jo[4].p1[2] = transformStamped4.transform.translation.z; 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]); 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[0] = transformStamped5.transform.translation.x;
robot1.jo[5].p1[1] = transformStamped5.transform.translation.y; robot1.jo[5].p1[1] = transformStamped5.transform.translation.y;
robot1.jo[5].p1[2] = transformStamped5.transform.translation.z; 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]); 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[0] = transformStamped6.transform.translation.x;
robot1.jo[6].p1[1] = transformStamped6.transform.translation.y; robot1.jo[6].p1[1] = transformStamped6.transform.translation.y;
robot1.jo[6].p1[2] = transformStamped6.transform.translation.z; 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]); 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[0] = transformStamped7.transform.translation.x;
robot1.jo[7].p1[1] = transformStamped7.transform.translation.y; robot1.jo[7].p1[1] = transformStamped7.transform.translation.y;
robot1.jo[7].p1[2] = transformStamped7.transform.translation.z; 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]); 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.w = 0.0;
robot1.jo[1].or2.x = 0.0; robot1.jo[1].or2.x = 0.0;
...@@ -257,43 +244,57 @@ void updateDectectedHumans(const std_msgs::uint32::ConstPtr& msg) ...@@ -257,43 +244,57 @@ void updateDectectedHumans(const std_msgs::uint32::ConstPtr& msg)
robot1.jo[1].or2.x = transformStamped1.transform.rotation.x; robot1.jo[1].or2.x = transformStamped1.transform.rotation.x;
robot1.jo[1].or2.y = transformStamped1.transform.rotation.y; robot1.jo[1].or2.y = transformStamped1.transform.rotation.y;
robot1.jo[1].or2.z = transformStamped1.transform.rotation.z; 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); 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.w = transformStamped2.transform.rotation.w;
robot1.jo[2].or2.x = transformStamped2.transform.rotation.x; robot1.jo[2].or2.x = transformStamped2.transform.rotation.x;
robot1.jo[2].or2.y = transformStamped2.transform.rotation.y; robot1.jo[2].or2.y = transformStamped2.transform.rotation.y;
robot1.jo[2].or2.z = transformStamped2.transform.rotation.z; 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); 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.w = transformStamped3.transform.rotation.w;
robot1.jo[3].or2.x = transformStamped3.transform.rotation.x; robot1.jo[3].or2.x = transformStamped3.transform.rotation.x;
robot1.jo[3].or2.y = transformStamped3.transform.rotation.y; robot1.jo[3].or2.y = transformStamped3.transform.rotation.y;
robot1.jo[3].or2.z = transformStamped3.transform.rotation.z; 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); 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.w = transformStamped4.transform.rotation.w;
robot1.jo[4].or2.x = transformStamped4.transform.rotation.x; robot1.jo[4].or2.x = transformStamped4.transform.rotation.x;
robot1.jo[4].or2.y = transformStamped4.transform.rotation.y; robot1.jo[4].or2.y = transformStamped4.transform.rotation.y;
robot1.jo[4].or2.z = transformStamped4.transform.rotation.z; 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); 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.w = transformStamped5.transform.rotation.w;
robot1.jo[5].or2.x = transformStamped5.transform.rotation.x; robot1.jo[5].or2.x = transformStamped5.transform.rotation.x;
robot1.jo[5].or2.y = transformStamped5.transform.rotation.y; robot1.jo[5].or2.y = transformStamped5.transform.rotation.y;
robot1.jo[5].or2.z = transformStamped5.transform.rotation.z; 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); 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.w = transformStamped6.transform.rotation.w;
robot1.jo[6].or2.x = transformStamped6.transform.rotation.x; robot1.jo[6].or2.x = transformStamped6.transform.rotation.x;
robot1.jo[6].or2.y = transformStamped6.transform.rotation.y; robot1.jo[6].or2.y = transformStamped6.transform.rotation.y;
robot1.jo[6].or2.z = transformStamped6.transform.rotation.z; 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); 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.w = transformStamped7.transform.rotation.w;
robot1.jo[7].or2.x = transformStamped7.transform.rotation.x; robot1.jo[7].or2.x = transformStamped7.transform.rotation.x;
robot1.jo[7].or2.y = transformStamped7.transform.rotation.y; robot1.jo[7].or2.y = transformStamped7.transform.rotation.y;
robot1.jo[7].or2.z = transformStamped7.transform.rotation.z; 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); 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(); rate.sleep();
} }
......
src/worldsafety.cpp
+ 67
− 56
View file @ 30e901da
...@@ -31,8 +31,7 @@ ...@@ -31,8 +31,7 @@
#include <moveit/trajectory_processing/iterative_time_parameterization.h> #include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include "trajectory_msgs/JointTrajectoryPoint.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::init(argc, argv, "Safety_Model");
ros::NodeHandle node_handle; ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1); ros::AsyncSpinner spinner(1);
...@@ -100,10 +99,8 @@ int main(int argc, char** argv) ...@@ -100,10 +99,8 @@ int main(int argc, char** argv)
// Static Tests - World Model Safety Test Cases to check Segmentation error // 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 // test the properties of the model that do not change over time, such as the structure of you model
if (robot1.jo.size() == 7) if (robot1.jo.size() == 7) { ROS_INFO_STREAM(" TEST CASE SUCCESSFUL. ROBOT FPE HAS 7 JOINTS "); }
{ 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()); }
else
{ ROS_ERROR_STREAM(" TEST CASE FAILED. ROBOT FPE HAS WRONG NUMBER OF JOINTS = " << robot1.jo.size());}
if (o8.h == nullptr) if (o8.h == nullptr)
ROS_ERROR("ERROR IN HAND POINTER IN ROBOT FPE CLASS AGGREGATION RELATION"); ROS_ERROR("ERROR IN HAND POINTER IN ROBOT FPE CLASS AGGREGATION RELATION");
...@@ -146,33 +143,47 @@ int main(int argc, char** argv) ...@@ -146,33 +143,47 @@ int main(int argc, char** argv)
tf2_ros::Buffer tfBuffer; tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer); tf2_ros::TransformListener tfListener(tfBuffer);
ros::Rate rate(10); ros::Rate rate(10);
//robot1.jo[7].p1[2]=0.0;
int i=0; while (node_handle.ok()) {
int ho=0; geometry_msgs::TransformStamped transformStamped1;
while (node_handle.ok()) geometry_msgs::TransformStamped transformStamped2;
{ geometry_msgs::TransformStamped transformStamped3;
geometry_msgs::TransformStamped transformStamped[7]; geometry_msgs::TransformStamped transformStamped4;
geometry_msgs::TransformStamped transformStamped5;
geometry_msgs::TransformStamped transformStamped6;
geometry_msgs::TransformStamped transformStamped7;
try { try {
transformStamped[++i] = tfBuffer.lookupTransform("world", robot1.jo[++ho].name, ros::Time(0)); 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) { } catch (tf2::TransformException &ex) {
ROS_WARN("%s", ex.what()); ROS_WARN("%s", ex.what());
ros::Duration(0.1).sleep(); ros::Duration(0.1).sleep();
continue; continue;
} }
robot1.jo[i].p1[2] = transformStamped[ho].transform.translation.z; }
}//End of taking input in program // Dynamic Tests
// Begin test case to check if robot moved to its position while (node_handle.ok()) {
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
{
{
// 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 //Checking if robot moved to position or not
if (robot1.jo[7].p1[2] == 0.9) if (robot1.jo[7].p1[2] == 0.9)
ROS_INFO_STREAM(" THE ROBOT HAS MOVED TO POSITION AS INTENDED "); ROS_INFO_STREAM(" THE ROBOT HAS MOVED TO POSITION AS INTENDED ");
else else
ROS_ERROR_STREAM("THE ROBOT HAS NOT MOVED TO THE POSITION AS INTENDED"); ROS_ERROR_STREAM("THE ROBOT HAS NOT MOVED TO THE POSITION AS INTENDED");
}
rate.sleep(); rate.sleep();
} }
}
return 0; return 0;
} }
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment