diff --git a/src/datalink/humanspace.h b/src/datalink/humanspace.h
index 5e681f457ae615be680821e5ee63c7a9ae26cce6..610a3341ad3ffdbbb00b3cb63eb366ee51d3ce9e 100644
--- a/src/datalink/humanspace.h
+++ b/src/datalink/humanspace.h
@@ -4,8 +4,8 @@
 class humanspace
 {
 public:
-    int humannum;
-
+    //int humannum;
+    uint32 humannum;
 
 //constructor
 
diff --git a/src/robot_models_node.cpp b/src/robot_models_node.cpp
index 10294f478cd4381cf7f761fa846f68e20729800c..9b032a769426b4e25e5ad60655ac4f4df0b12495 100644
--- a/src/robot_models_node.cpp
+++ b/src/robot_models_node.cpp
@@ -26,6 +26,12 @@
 #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());
+}
+
     int main(int argc, char** argv) {
         ros::init(argc, argv, "ROSNODE1");
         ros::NodeHandle node_handle("namespace_name");
@@ -117,9 +123,26 @@
         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()) {
@@ -132,7 +155,8 @@
                 geometry_msgs::TransformStamped transformStamped5;
                 geometry_msgs::TransformStamped transformStamped6;
                 geometry_msgs::TransformStamped transformStamped7;
-
+                uint32::data updateDetectedHumans;
+                geometry_msgs::uint32 x;
                 try {
                     transformStamped1 = tfBuffer.lookupTransform("world", o7.jo[1].name, ros::Time(0));
                     transformStamped2 = tfBuffer.lookupTransform("world", o7.jo[2].name, ros::Time(0));
@@ -141,6 +165,7 @@
                     transformStamped5 = tfBuffer.lookupTransform("world", o7.jo[5].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));
+                    updateDetectedHumans=o9.a->humannum;
                 } catch (tf2::TransformException &ex) {
                     ROS_WARN("%s", ex.what());
                     ros::Duration(0.1).sleep();
@@ -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]);
 
                 o7.jo[7].p1[0]=transformStamped7.transform.translation.x;
-                o7.jo[7].p1[0]=transformStamped7.transform.translation.y;
-                o7.jo[7].p1[0]=transformStamped7.transform.translation.z;
+                o7.jo[7].p1[1]=transformStamped7.transform.translation.y;
+                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]);
 
                 o7.jo[1].or2.w=0.0;
diff --git a/src/worldsafety.cpp b/src/worldsafety.cpp
index c212bcb551dba27520ba684aff57d1c70d1c7457..6323304439c51ce7c6b7fd141af17b46b55763fe 100644
--- a/src/worldsafety.cpp
+++ b/src/worldsafety.cpp
@@ -64,12 +64,12 @@ int main(int argc, char** argv)
     o7.jo.push_back(o5f);
     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) {
         ROS_INFO("SIZE IS 5");
     } else {
         ROS_INFO_STREAM("SIZE IS NOT 5 but was " << o7.jo.size());
-    }
+    }*/
 
     int yx=0;
     o7.jo[++yx].name= "panda_link1";
@@ -129,6 +129,47 @@ int main(int argc, char** argv)
 
     // Move the (simulated) robot in gazebo
     move_group.move();*/
+    if (o7.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 = " << o7.jo.size());
+
+    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 WORLD 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");
+
+    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();
 
     tf2_ros::Buffer tfBuffer;
     tf2_ros::TransformListener tfListener(tfBuffer);
@@ -148,63 +189,26 @@ int main(int argc, char** argv)
                 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();
-
+            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
-            if (o7.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 = " << o7.jo.size());
+          // Static Tests
 
-            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 WORLD 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");
 
             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();
 
-                    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();
                 }
 
             }