Implemented Contact Sensors and GhostObject ROS Comm

WPI-AIM · Jun 25, 2024 · 137a3aa · 137a3aa
1 parent 89a50f7
commit 137a3aa
Show file tree

Hide file tree

Showing 29 changed files with 1,089 additions and 86 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -0,0 +1,5 @@
+## June 26 2024
+1. Removed setting forces and torques in the Ghost Object update
+2. Added ROS Communication Support for Ghost Objects
+3. Create Contact Sensors and implemented their ROS communication
+4. Created CHANGELOG file
diff --git a/adf_loader/version_1_0/adf_loader_1_0.cpp b/adf_loader/version_1_0/adf_loader_1_0.cpp
@@ -475,6 +475,9 @@ afSensorType ADFUtils::getSensorTypeFromString(const string &a_str)
     else if (a_str.compare("Resistance") == 0 || a_str.compare("resistance") == 0 || a_str.compare("RESISTANCE") == 0){
         type = afSensorType::RESISTANCE;
     }
+    else if (a_str.compare("Contact") == 0 || a_str.compare("contact") == 0 || a_str.compare("CONTACT") == 0){
+        type = afSensorType::CONTACT;
+    }
 
     return type;
 }
@@ -1982,6 +1985,9 @@ bool ADFLoader_1_0::loadSensorAttribs(YAML::Node *a_node, afSensorAttributes *at
     }
     case afSensorType::RESISTANCE:{
         return loadResistanceSensorAttribs(a_node, (afResistanceSensorAttributes*)attribs);
+    }
+    case afSensorType::CONTACT:{
+        return loadContactSensorAttribs(a_node, (afContactSensorAttributes*)attribs);
     }
         break;
     default:{
@@ -2117,7 +2123,7 @@ bool ADFLoader_1_0::loadResistanceSensorAttribs(YAML::Node *a_node, afResistance
 {
     YAML::Node& node = *a_node;
     if (node.IsNull()){
-        cerr << "ERROR! ACTUATOR'S YAML CONFIG DATA IS NULL\n";
+        cerr << "ERROR! SENSOR'S YAML CONFIG DATA IS NULL\n";
         return 0;
     }
     ADFUtils::saveRawData(a_node, attribs);
@@ -2163,6 +2169,51 @@ bool ADFLoader_1_0::loadResistanceSensorAttribs(YAML::Node *a_node, afResistance
         return result;
 }
 
+bool ADFLoader_1_0::loadContactSensorAttribs(YAML::Node *a_node, afContactSensorAttributes *attribs)
+{
+    YAML::Node& node = *a_node;
+    if (node.IsNull()){
+        cerr << "ERROR! SENSOR'S YAML CONFIG DATA IS NULL\n";
+        return 0;
+    }
+    ADFUtils::saveRawData(a_node, attribs);
+
+    bool result = true;
+
+    YAML::Node nameNode = node["name"];
+    YAML::Node namespaceNode = node["namespace"];
+    YAML::Node parentNameNode = node["parent"];
+    YAML::Node visibleNode = node["visible"];
+    YAML::Node visibleSizeNode = node["visible size"];
+    YAML::Node publishFrequencyNode = node["publish frequency"];
+    YAML::Node distanceThresholdNode = node["distance threshold"];
+    YAML::Node processContactDetailsNode = node["process contact details"];
+
+    ADFUtils::getIdentificationAttribsFromNode(a_node, &attribs->m_identificationAttribs);
+    ADFUtils::getHierarchyAttribsFromNode(a_node, &attribs->m_hierarchyAttribs);
+    ADFUtils::getCommunicationAttribsFromNode(a_node, &attribs->m_communicationAttribs);
+    ADFUtils::getPluginAttribsFromNode(a_node, &attribs->m_pluginAttribs);
+
+
+    if (visibleNode.IsDefined()){
+        attribs->m_visible = visibleNode.as<bool>();
+    }
+
+    if (visibleSizeNode.IsDefined()){
+        attribs->m_visibleSize = visibleSizeNode.as<double>();
+    }
+
+    if (distanceThresholdNode.IsDefined()){
+        attribs->m_distanceThreshold = distanceThresholdNode.as<double>();
+    }
+
+    if (processContactDetailsNode.IsDefined()){
+        attribs->m_processContactDetails = processContactDetailsNode.as<bool>();
+    }
+
+    return result;
+}
+
 bool ADFLoader_1_0::loadActuatorAttribs(YAML::Node *a_node, afActuatorAttributes *attribs)
 {
     YAML::Node& node = *a_node;
@@ -2729,6 +2780,10 @@ bool ADFLoader_1_0::loadModelAttribs(YAML::Node *a_node, afModelAttributes *attr
                 senAttribs = new afResistanceSensorAttributes();
                 break;
             }
+            case afSensorType::CONTACT:{
+                senAttribs = new afContactSensorAttributes();
+                break;
+            }
             default:
                 break;
             }

diff --git a/adf_loader/version_1_0/adf_loader_1_0.h b/adf_loader/version_1_0/adf_loader_1_0.h
@@ -151,6 +151,9 @@ class ADFLoader_1_0: public ADFLoaderBase{
     // Load joint from a YAML::Node
     virtual bool loadResistanceSensorAttribs(YAML::Node* a_node, afResistanceSensorAttributes* attribs);
 
+    // Load joint from a YAML::Node
+    virtual bool loadContactSensorAttribs(YAML::Node* a_node, afContactSensorAttributes* attribs);
+
     // Load actuator from a YAML::Node
     virtual bool loadActuatorAttribs(YAML::Node* a_node, afActuatorAttributes* attribs);
 

diff --git a/ambf_framework/afAttributes.h b/ambf_framework/afAttributes.h
@@ -1104,6 +1104,22 @@ struct afResistanceSensorAttributes: public afRayTracerSensorAttributes{
 };
 
 
+struct afContactSensorAttributes: public afSensorAttributes{
+public:
+    afContactSensorAttributes(){
+        m_distanceThreshold = 0.0;
+        m_processContactDetails = true;
+        m_visible = false;
+        m_visibleSize = 10;
+    }
+
+    double m_distanceThreshold; // Distance threshold between objects for contact to count
+    bool m_processContactDetails; // If true, process contact ponits, normals, etc. Otherwise just names of objects in contact
+    bool m_visible;
+    double m_visibleSize;
+};
+
+
 struct afFileObjectAttributes{
 public:
     afFileObjectAttributes(){}

diff --git a/ambf_framework/afEnums.h b/ambf_framework/afEnums.h
@@ -174,7 +174,8 @@ enum class afSensorType{
     RAYTRACER = 0,
     RANGE = 1,
     RESISTANCE = 2,
-    INVALID = 3
+    CONTACT = 3,
+    INVALID = 4
 };