From e175c92d4d6671332b9c31c16c4ff88887537df2 Mon Sep 17 00:00:00 2001 From: Nikhil Ambardar <202-nikaviator@users.noreply.git-st.inf.tu-dresden.de> Date: Mon, 9 Nov 2020 11:27:48 +0100 Subject: [PATCH] temp --- src/datalink/humanspace.h | 4 +- src/robot_models_node.cpp | 33 +++++++++++-- src/worldsafety.cpp | 100 ++++++++++++++++++++------------------ 3 files changed, 83 insertions(+), 54 deletions(-) diff --git a/src/datalink/humanspace.h b/src/datalink/humanspace.h index 5e681f4..610a334 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 10294f4..9b032a7 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 c212bcb..6323304 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(); } } -- GitLab