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(); } }