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

temp

parent b37291c6
Branches
No related tags found
No related merge requests found
...@@ -4,8 +4,8 @@ ...@@ -4,8 +4,8 @@
class humanspace class humanspace
{ {
public: public:
int humannum; //int humannum;
uint32 humannum;
//constructor //constructor
......
...@@ -26,6 +26,12 @@ ...@@ -26,6 +26,12 @@
#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>
void updateDectectedHumans(const std_msgs::uint32::ConstPtr& msg)
{
ROS_INFO("I heard: [%s]", msg->data.c_str());
}
int main(int argc, char** argv) { int main(int argc, char** argv) {
ros::init(argc, argv, "ROSNODE1"); ros::init(argc, argv, "ROSNODE1");
ros::NodeHandle node_handle("namespace_name"); ros::NodeHandle node_handle("namespace_name");
...@@ -117,9 +123,26 @@ ...@@ -117,9 +123,26 @@
o9.b2->z = 1.6; o9.b2->z = 1.6;
o9.b2->w = 1.9; 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::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer); tf2_ros::TransformListener tfListener(tfBuffer);
ros::Rate rate(10); ros::Rate rate(10);
while (node_handle.ok()) { while (node_handle.ok()) {
...@@ -132,7 +155,8 @@ ...@@ -132,7 +155,8 @@
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;
geometry_msgs::uint32 x;
try { try {
transformStamped1 = tfBuffer.lookupTransform("world", o7.jo[1].name, ros::Time(0)); transformStamped1 = tfBuffer.lookupTransform("world", o7.jo[1].name, ros::Time(0));
transformStamped2 = tfBuffer.lookupTransform("world", o7.jo[2].name, ros::Time(0)); transformStamped2 = tfBuffer.lookupTransform("world", o7.jo[2].name, ros::Time(0));
...@@ -141,6 +165,7 @@ ...@@ -141,6 +165,7 @@
transformStamped5 = tfBuffer.lookupTransform("world", o7.jo[5].name, ros::Time(0)); transformStamped5 = tfBuffer.lookupTransform("world", o7.jo[5].name, ros::Time(0));
transformStamped6 = tfBuffer.lookupTransform("world", o7.jo[6].name, ros::Time(0)); transformStamped6 = tfBuffer.lookupTransform("world", o7.jo[6].name, ros::Time(0));
transformStamped7 = tfBuffer.lookupTransform("world", o7.jo[7].name, ros::Time(0)); transformStamped7 = tfBuffer.lookupTransform("world", o7.jo[7].name, ros::Time(0));
updateDetectedHumans=o9.a->humannum;
} 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();
...@@ -200,8 +225,8 @@ ...@@ -200,8 +225,8 @@
ROS_INFO_STREAM("panda_link6 Position is " <<" x = "<< o7.jo[6].p1[0] << ", y = "<< o7.jo[6].p1[1]<<", z = "<<o7.jo[6].p1[2]); ROS_INFO_STREAM("panda_link6 Position is " <<" x = "<< o7.jo[6].p1[0] << ", y = "<< o7.jo[6].p1[1]<<", z = "<<o7.jo[6].p1[2]);
o7.jo[7].p1[0]=transformStamped7.transform.translation.x; o7.jo[7].p1[0]=transformStamped7.transform.translation.x;
o7.jo[7].p1[0]=transformStamped7.transform.translation.y; o7.jo[7].p1[1]=transformStamped7.transform.translation.y;
o7.jo[7].p1[0]=transformStamped7.transform.translation.z; o7.jo[7].p1[2]=transformStamped7.transform.translation.z;
ROS_INFO_STREAM("panda_link7 Position is " <<" x = "<< o7.jo[7].p1[0] << ", y = "<< o7.jo[7].p1[1]<<", z = "<<o7.jo[7].p1[2]); ROS_INFO_STREAM("panda_link7 Position is " <<" x = "<< o7.jo[7].p1[0] << ", y = "<< o7.jo[7].p1[1]<<", z = "<<o7.jo[7].p1[2]);
o7.jo[1].or2.w=0.0; o7.jo[1].or2.w=0.0;
......
...@@ -64,12 +64,12 @@ int main(int argc, char** argv) ...@@ -64,12 +64,12 @@ int main(int argc, char** argv)
o7.jo.push_back(o5f); o7.jo.push_back(o5f);
o7.jo.push_back(o5g); o7.jo.push_back(o5g);
// test if the robot contains 7 joints static test /* // test if the robot contains 7 joints static test
if (o7.jo.size() == 5) { if (o7.jo.size() == 5) {
ROS_INFO("SIZE IS 5"); ROS_INFO("SIZE IS 5");
} else { } else {
ROS_INFO_STREAM("SIZE IS NOT 5 but was " << o7.jo.size()); ROS_INFO_STREAM("SIZE IS NOT 5 but was " << o7.jo.size());
} }*/
int yx=0; int yx=0;
o7.jo[++yx].name= "panda_link1"; o7.jo[++yx].name= "panda_link1";
...@@ -129,35 +129,6 @@ int main(int argc, char** argv) ...@@ -129,35 +129,6 @@ int main(int argc, char** argv)
// Move the (simulated) robot in gazebo // Move the (simulated) robot in gazebo
move_group.move();*/ move_group.move();*/
tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer);
ros::Rate rate(10);
int i=0;
int ho=0;
while (node_handle.ok()) {
{
geometry_msgs::TransformStamped transformStamped[7];
try {
transformStamped[++i] = tfBuffer.lookupTransform("world", o7.jo[++ho].name, ros::Time(0));
} catch (tf2::TransformException &ex) {
ROS_WARN("%s", ex.what());
ros::Duration(0.1).sleep();
continue;
}
//Checking if robot moved to position or not
if (o7.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");
ros::shutdown();
// World Model Safety Test Cases to check Segmentation errors
ros::Rate rate(10);
// test the properties of the model that do not change over time, such as the structure of you model
if (o7.jo.size() == 7) if (o7.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 else
...@@ -178,12 +149,6 @@ int main(int argc, char** argv) ...@@ -178,12 +149,6 @@ int main(int argc, char** argv)
if (o9.b2 == nullptr) if (o9.b2 == nullptr)
ROS_ERROR("ERROR IN ABSTRACT OBJECT CLASS AGGREGATION RELATION"); ROS_ERROR("ERROR IN ABSTRACT OBJECT CLASS AGGREGATION RELATION");
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
static const std::string PLANNING_GROUP = "panda_arm"; static const std::string PLANNING_GROUP = "panda_arm";
moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP); moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
moveit::planning_interface::PlanningSceneInterface planning_scene_interface; moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
...@@ -205,6 +170,45 @@ int main(int argc, char** argv) ...@@ -205,6 +170,45 @@ int main(int argc, char** argv)
// Move the (simulated) robot in gazebo // Move the (simulated) robot in gazebo
move_group.move(); move_group.move();
tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer);
ros::Rate rate(10);
int i=0;
int ho=0;
while (node_handle.ok()) {
{
geometry_msgs::TransformStamped transformStamped[7];
try {
transformStamped[++i] = tfBuffer.lookupTransform("world", o7.jo[++ho].name, ros::Time(0));
} catch (tf2::TransformException &ex) {
ROS_WARN("%s", ex.what());
ros::Duration(0.1).sleep();
continue;
}
o7.jo[7].p1[2]=transformStamped[7].transform.translation.z;
// World Model Safety Test Cases to check Segmentation errors
ros::Rate rate(10);
// test the properties of the model that do not change over time, such as the structure of you model
// Static 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 (o7.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");
ros::shutdown();
} }
} }
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment