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

contradictoryadvice

parent 8bd84fc2
No related branches found
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 @@
#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,10 +26,9 @@
#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) {
......@@ -64,29 +62,6 @@ void updateDectectedHumans(const std_msgs::uint32::ConstPtr& msg)
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";
......@@ -97,14 +72,12 @@ void updateDectectedHumans(const std_msgs::uint32::ConstPtr& msg)
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;
......@@ -128,7 +101,7 @@ void updateDectectedHumans(const std_msgs::uint32::ConstPtr& msg)
ros::Rate loop_rate(10);
int count = 0;
while (ros::ok()) {
std_msgs::uint32 msg;
std_msgs::UInt32 msg;
std::stringstream ss;
ss << "hello world " << count;
msg.data = ss.str();
......@@ -156,8 +129,8 @@ void updateDectectedHumans(const std_msgs::uint32::ConstPtr& msg)
geometry_msgs::TransformStamped transformStamped5;
geometry_msgs::TransformStamped transformStamped6;
geometry_msgs::TransformStamped transformStamped7;
uint32::data updateDetectedHumans;
geometry_msgs::uint32 x;
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));
......@@ -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[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]);
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]);
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]);
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]);
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]);
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]);
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]);
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;
......@@ -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.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);
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);
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);
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);
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);
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);
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);
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();
}
......
src/worldsafety.cpp
+ 67
− 56
View file @ 30e901da
......@@ -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);
......@@ -100,10 +99,8 @@ int main(int argc, char** argv)
// 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];
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 {
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) {
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
}
// 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;
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment