From 137a3aa7c0ff44d71ac6ea1eff22c78a5de556b9 Mon Sep 17 00:00:00 2001 From: Adnan Munawar Date: Wed, 26 Jun 2024 01:14:23 +0500 Subject: [PATCH] Implemented Contact Sensors and GhostObject ROS Comm --- CHANGELOG.md | 5 + adf_loader/version_1_0/adf_loader_1_0.cpp | 57 ++++- adf_loader/version_1_0/adf_loader_1_0.h | 3 + ambf_framework/afAttributes.h | 16 ++ ambf_framework/afEnums.h | 3 +- ambf_framework/afFramework.cpp | 229 ++++++++++++++---- ambf_framework/afFramework.h | 87 ++++++- .../core/ros_comm_plugin/ObjectCommPlugin.cpp | 142 ++++++++++- .../core/ros_comm_plugin/ObjectCommPlugin.h | 6 + .../ambf_client/python/ambf_client.py | 45 +++- .../ambf_client/python/ambf_ghost_object.py | 92 +++++++ .../ambf_client/python/ambf_sensor.py | 71 +++++- ambf_ros_modules/ambf_msgs/CMakeLists.txt | 6 + .../ambf_msgs/msg/ContactData.msg | 6 + .../ambf_msgs/msg/ContactEvent.msg | 3 + .../ambf_msgs/msg/ContactSensorCmd.msg | 1 + .../ambf_msgs/msg/ContactSensorState.msg | 9 + .../ambf_msgs/msg/GhostObjectCmd.msg | 1 + .../ambf_msgs/msg/GhostObjectState.msg | 9 + ambf_ros_modules/ambf_server/CMakeLists.txt | 4 + .../include/ambf_server/GhostObject.h | 67 +++++ .../include/ambf_server/GhostObjectRosCom.h | 63 +++++ .../ambf_server/include/ambf_server/Sensor.h | 11 + .../include/ambf_server/SensorRosCom.h | 26 +- .../ambf_server/src/GhostObject.cpp | 89 +++++++ .../ambf_server/src/GhostObjectRosCom.cpp | 66 +++++ .../ambf_server/src/RosComBase.cpp | 8 + ambf_ros_modules/ambf_server/src/Sensor.cpp | 17 ++ .../ambf_server/src/SensorRosCom.cpp | 33 ++- 29 files changed, 1089 insertions(+), 86 deletions(-) create mode 100644 CHANGELOG.md create mode 100644 ambf_ros_modules/ambf_client/python/ambf_ghost_object.py create mode 100644 ambf_ros_modules/ambf_msgs/msg/ContactData.msg create mode 100644 ambf_ros_modules/ambf_msgs/msg/ContactEvent.msg create mode 100644 ambf_ros_modules/ambf_msgs/msg/ContactSensorCmd.msg create mode 100644 ambf_ros_modules/ambf_msgs/msg/ContactSensorState.msg create mode 100644 ambf_ros_modules/ambf_msgs/msg/GhostObjectCmd.msg create mode 100644 ambf_ros_modules/ambf_msgs/msg/GhostObjectState.msg create mode 100644 ambf_ros_modules/ambf_server/include/ambf_server/GhostObject.h create mode 100644 ambf_ros_modules/ambf_server/include/ambf_server/GhostObjectRosCom.h create mode 100644 ambf_ros_modules/ambf_server/src/GhostObject.cpp create mode 100644 ambf_ros_modules/ambf_server/src/GhostObjectRosCom.cpp diff --git a/CHANGELOG.md b/CHANGELOG.md new file mode 100644 index 000000000..0a0573810 --- /dev/null +++ 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 \ No newline at end of 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 index 61cf330fc..cce636900 100644 --- 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(); + } + + if (visibleSizeNode.IsDefined()){ + attribs->m_visibleSize = visibleSizeNode.as(); + } + + if (distanceThresholdNode.IsDefined()){ + attribs->m_distanceThreshold = distanceThresholdNode.as(); + } + + if (processContactDetailsNode.IsDefined()){ + attribs->m_processContactDetails = processContactDetailsNode.as(); + } + + 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 index 154b1e7d7..7ffbdb79d 100644 --- 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 index 77b676437..a9f1fab82 100644 --- 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 index 302580a2a..1d51acdd0 100644 --- 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 }; diff --git a/ambf_framework/afFramework.cpp b/ambf_framework/afFramework.cpp index fcfc63c54..de6b1ea85 100644 --- a/ambf_framework/afFramework.cpp +++ b/ambf_framework/afFramework.cpp @@ -2625,7 +2625,6 @@ void afRigidBody::estimateCartesianControllerGains(afCartesianController &contro } } - /// /// \brief afRigidBody::updatePositionFromDynamics /// @@ -7425,6 +7424,14 @@ bool afModel::createFromAttribs(afModelAttributes *a_attribs) valid = ((afResistanceSensor*)sensorPtr)->createFromAttribs(senAttribs); break; } + case afSensorType::CONTACT: + { + sensorPtr = new afContactSensor(m_afWorld, this); + type_str = "CONTACT"; + afContactSensorAttributes* senAttribs = (afContactSensorAttributes*) a_attribs->m_sensorAttribs[i]; + valid = ((afContactSensor*)sensorPtr)->createFromAttribs(senAttribs); + break; + } default: continue; } @@ -8024,7 +8031,7 @@ void afGhostObject::update(double dt) // trans << m_bulletGhostObject->getWorldTransform(); // setLocalTransform(trans); m_bulletGhostObject->setWorldTransform(to_btTransform(m_globalTransform)); - vector localSensedBodies; + m_sensedObjectsMaps.clear(); btManifoldArray* manifoldArray = new btManifoldArray(); btBroadphasePairArray pairArray = m_bulletGhostObject->getOverlappingPairCache()->getOverlappingPairArray(); @@ -8044,25 +8051,23 @@ void afGhostObject::update(double dt) collisionPair->m_algorithm->getAllContactManifolds(*manifoldArray); } - for (int j = 0; j < manifoldArray->size(); j++) - { + for (int j = 0; j < manifoldArray->size(); j++){ btPersistentManifold* manifold = manifoldArray->at(j); - btCollisionObject* co; + btCollisionObject* coB; if (manifold->getBody0() == m_bulletGhostObject){ - co = const_cast(manifold->getBody1()); + coB = const_cast(manifold->getBody1()); } else{ - co = const_cast(manifold->getBody0()); + coB = const_cast(manifold->getBody0()); } + afBaseObjectPtr aoB = (afBaseObjectPtr)coB->getUserPointer(); - for (int p = 0; p < manifold->getNumContacts(); p++) - { - btManifoldPoint pt = manifold->getContactPoint(p); - if (pt.getDistance() < 0.0f) - { - btRigidBody* pBody = dynamic_cast(co); - if (pBody){ - localSensedBodies.push_back(pBody); + if(aoB){ + for (int p = 0; p < manifold->getNumContacts(); p++){ + btManifoldPoint pt = manifold->getContactPoint(p); + if (pt.getDistance() < 0.0f) { + m_sensedObjectsMaps[aoB] = pt.getDistance(); // All we care is if any point of object intersects. + break; } } } @@ -8070,42 +8075,6 @@ void afGhostObject::update(double dt) } delete manifoldArray; - - for (int i = 0 ; i < m_sensedBodies.size() ; i++){ - m_sensedBodies[i]->setGravity(m_afWorld->m_bulletWorld->getGravity()); - m_sensedBodies[i]->applyCentralForce(btVector3(0, 0, 0)); - m_sensedBodies[i]->applyTorque(btVector3(0, 0, 0)); - } - - m_sensedBodies.clear(); - m_sensedBodies = localSensedBodies; - - - for (int i = 0 ; i < m_sensedBodies.size() ; i++){ - if (m_sensedBodies[i]){ - m_sensedBodies[i]->setGravity(btVector3(0, 0, 0)); - double damping_factor = 1.0 - 0.1; - btVector3 va(0, 0, 0); - if (getParentObject()){ - afRigidBodyPtr parentBody = dynamic_cast(getParentObject()); - va = parentBody->m_bulletRigidBody->getLinearVelocity(); - } - - btVector3 vb = m_sensedBodies[i]->getLinearVelocity(); - double mag_va = va.length(); - btVector3 proj_vb_va(0, 0, 0); - - if (mag_va > 0.00001){ - proj_vb_va = va.normalized() * (vb.dot(va) / mag_va); - } - btVector3 orth_vb_va = vb - proj_vb_va; - - btVector3 wb = m_sensedBodies[i]->getAngularVelocity(); - - m_sensedBodies[i]->setLinearVelocity(damping_factor * orth_vb_va + va); - m_sensedBodies[i]->setAngularVelocity(damping_factor * wb); - } - } } bool afGhostObject::createFromAttribs(afGhostObjectAttributes *a_attribs) @@ -8216,6 +8185,8 @@ bool afGhostObject::createFromAttribs(afGhostObjectAttributes *a_attribs) loadPlugins(this, a_attribs, &a_attribs->m_pluginAttribs); + loadCommunicationPlugin(this, a_attribs); + return valid; } @@ -8639,3 +8610,159 @@ cTexture3dPtr afVolume::copy3DTexture(cTexture1dPtr tex3D) copyTex->m_image = static_pointer_cast(tex3D->m_image)->copy(); return copyTex; } + +afContactSensorCallback::~afContactSensorCallback(){ +} + +bool afContactSensorCallback::needsCollision(btBroadphaseProxy *proxy) const { + // superclass will check m_collisionFilterGroup and m_collisionFilterMask + if(!btCollisionWorld::ContactResultCallback::needsCollision(proxy)) + return false; + // if passed filters, may also want to avoid contacts between constraints + assert(m_parentObject->getType() == afType::RIGID_BODY && "ERROR! NOT IMPLEMENTED FOR OTHER AMBF OBJECT TYPES"); // TODO: GENERALIZE TO OTHER AF OBJECTS + btRigidBody* rb = ((afRigidBodyPtr)m_parentObject)->m_bulletRigidBody; + return rb->checkCollideWithOverride(static_cast(proxy->m_clientObject)); +} + +btScalar afContactSensorCallback::addSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0, int partId0, int index0, const btCollisionObjectWrapper *colObj1, int partId1, int index1) +{ + if (cp.getDistance() <= m_distanceThreshold){ + afBaseObjectPtr boA, boB; + cVector3d P_a_w, P_b_w, N_b_w; + if(colObj0->m_collisionObject->getUserPointer() == m_parentObject) { + boA = (afBaseObjectPtr)colObj0->m_collisionObject->getUserPointer(); + boB = (afBaseObjectPtr)colObj1->m_collisionObject->getUserPointer(); + P_a_w << cp.m_positionWorldOnA; + P_b_w << cp.m_positionWorldOnB; + N_b_w << cp.m_normalWorldOnB; + } else { + assert(colObj1->m_collisionObject->getUserPointer() == m_parentObject && "body does not match either collision object"); + boA = (afBaseObjectPtr)colObj0->m_collisionObject->getUserPointer(); + boB = (afBaseObjectPtr)colObj0->m_collisionObject->getUserPointer(); + P_a_w << cp.m_positionWorldOnB; + P_b_w << cp.m_positionWorldOnA; + N_b_w << -cp.m_normalWorldOnB; + } + if (m_contactEventMap.find(boB) == m_contactEventMap.end()){ + m_contactEventMap[boB] = afContactEvent(boA, boB); + } + m_contactEventMap[boB].m_contactData.push_back(afContactData(P_a_w, P_b_w, N_b_w, cp.m_distance1)); + } + return 0; +} + +/// +/// \brief afContactSensor::afContactSensor +/// \param a_afWorld +/// \param a_modelPtr +/// +afContactSensor::afContactSensor(afWorldPtr a_afWorld, afModelPtr a_modelPtr): afSensor(a_afWorld, a_modelPtr){ + m_pointCloud = nullptr; +} + +/// +/// \brief afContactSensor::createFromAttribs +/// \param a_attribs +/// \return +/// +bool afContactSensor::createFromAttribs(afContactSensorAttributes *a_attribs){ + storeAttributes(a_attribs); + afContactSensorAttributes &attribs = *a_attribs; + m_sensorType = afSensorType::CONTACT; + + bool result = true; + setIdentifier(a_attribs->m_identifier); + setName(a_attribs->m_identificationAttribs.m_name); + setNamespace(a_attribs->m_identificationAttribs.m_namespace); + + m_parentName = a_attribs->m_hierarchyAttribs.m_parentName; + m_processContactDetails = a_attribs->m_processContactDetails; + + setMinPublishFrequency(a_attribs->m_communicationAttribs.m_minPublishFreq); + setMaxPublishFrequency(a_attribs->m_communicationAttribs.m_maxPublishFreq); + setPassive(a_attribs->m_communicationAttribs.m_passive); + + if (a_attribs->m_visible){ + m_pointCloud = new cMultiPoint; + m_pointCloud->setPointSize(a_attribs->m_visibleSize); + cColorf orange; orange.setOrangeCoral(); + m_pointCloud->setPointColor(orange); + m_pointCloud->setUseVertexColors(true); + addChildSceneObject(m_pointCloud, cTransform()); + } + + // First search in the local space. + m_parentBody = m_modelPtr->getRigidBody(m_parentName, true); + + string remap_idx = afUtils::getNonCollidingIdx(getQualifiedIdentifier(), m_afWorld->getSensorMap()); + setGlobalRemapIdx(remap_idx); + + if(m_parentBody == nullptr){ + m_parentBody = m_afWorld->getRigidBody(m_parentName + getGlobalRemapIdx()); + if (m_parentBody == nullptr){ + cerr << "ERROR! SENSOR'S "<< m_parentName + remap_idx << " NOT FOUND, IGNORING SENSOR\n"; + return 0; + } + } + + m_parentBody->addSensor(this); + m_parentBody->addChildObject(this); + + m_contactSensorCallback = new afContactSensorCallback(m_parentBody); + + m_contactSensorCallback->m_distanceThreshold = a_attribs->m_distanceThreshold; + + if (m_contactSensorCallback->m_distanceThreshold < 0.0){ + cerr << "WARNING! For Sensor " << getQualifiedName() << ", contact sensor threshold set to < 0.0. Contact test may be unseccessful " << endl; + } + + loadPlugins(this, a_attribs, &a_attribs->m_pluginAttribs); + + loadCommunicationPlugin(this, a_attribs); + + return true; +} + +void afContactSensor::updateSceneObjects(){ + if (m_pointCloud && m_processContactDetails){ + m_pointCloud->clear(); + m_mutex.acquire(); + for (auto it: m_contactSensorCallback->m_contactEventMap){ + for (auto pit: it.second.m_contactData){ + uint i = m_pointCloud->newPoint(pit.m_P_a_w); + if (it.second.m_contactObjectB->getType() == afType::RIGID_BODY){ + afInertialObjectPtr rbPtr = (afInertialObjectPtr)it.second.m_contactObjectB; + m_pointCloud->m_vertices->setColor(i, rbPtr->m_visualMesh->m_material->m_diffuse); + } + } + } + m_mutex.release(); + } +} + +void afContactSensor::update(double dt){ + m_mutex.acquire(); + m_contactSensorCallback->m_contactEventMap.clear(); + m_afWorld->m_bulletWorld->contactTest(m_parentBody->m_bulletRigidBody, *m_contactSensorCallback); + +// cerr << "INFO! Testing contact for: " << m_parentBody->getQualifiedName(); +// cerr << "INFO! Object Name " << m_name << endl; +// cerr << " Number of objects in contact " << m_contactSensorCallback->m_contactDataMap.size() << endl; +// for (auto obj: m_contactSensorCallback->m_contactDataMap){ +// cerr << "\t " << obj.first->getName() << ", Number of contacts: " << obj.second.size() << " Normal[0]: " << obj.second[0].m_globalContactNormalOnB << endl; +// } + m_mutex.release(); +} + + +afContactData::afContactData(cVector3d &P_a_w, cVector3d &P_b_w, cVector3d &N_b_w, double &distance){ + m_P_a_w = P_a_w; + m_P_b_w = P_b_w; + m_N_b_w = N_b_w; + m_distance = distance; +} + +afContactEvent::afContactEvent(afBaseObjectPtr objA, afBaseObjectPtr objB){ + m_contactObjectA = objA; + m_contactObjectB = objB; +} diff --git a/ambf_framework/afFramework.h b/ambf_framework/afFramework.h index de8c536c6..c229b0f40 100644 --- a/ambf_framework/afFramework.h +++ b/ambf_framework/afFramework.h @@ -98,6 +98,9 @@ class afJointController; class afConstraintActuator; class afRayTracerSensor; class afResistanceSensor; +class afContactSensor; +class afContactData; +class afContactEvent; typedef afBaseObject* afBaseObjectPtr; @@ -111,6 +114,9 @@ typedef afWorld* afWorldPtr; typedef afConstraintActuator* afConstraintActuatorPtr; typedef afRayTracerSensor* afRayTracerSensorPtr; typedef afResistanceSensor* afResistanceSensorPtr; +typedef afContactData* afContactDataPtr; +typedef afContactEvent* afContactEventPtr; +typedef afContactSensor* afContactSensorPtr; typedef map afRigidBodyMap; typedef map afSoftBodyMap; @@ -120,6 +126,7 @@ typedef vector afRigidBodyVec; typedef vector afSoftBodyVec; typedef vector afGhostObjectVec; typedef vector afJointVec; +typedef map afContactEventMap; //------------------------------------------------------------------------------ class afCamera; typedef afCamera* afCameraPtr; @@ -1154,7 +1161,32 @@ class afRigidBody: public afInertialObject{ afGeometryType m_collisionGeometryType; }; +struct afContactData{ +public: + + afContactData(cVector3d& gpA, cVector3d& gpB, cVector3d& gnB, double& distance); + + cVector3d m_P_a_w; // Point on A in world coords + + cVector3d m_P_b_w; // Point on B in world coords + cVector3d m_N_b_w;// Normal on A in world coords + + double m_distance; // Separating distance. Or penetration depth. +}; + + +struct afContactEvent{ + afContactEvent(){} + afContactEvent(afBaseObjectPtr objA, afBaseObjectPtr objB); + afBaseObjectPtr m_contactObjectA; + afBaseObjectPtr m_contactObjectB; + vector m_contactData; +}; + +/// +/// \brief The afVertexTree struct +/// struct afVertexTree{ std::vector triangleIdx; std::vector vertexIdx; @@ -1246,10 +1278,9 @@ class afGhostObject: public afInertialObject{ btPairCachingGhostObject* m_bulletGhostObject; -protected: - - std::vector m_sensedBodies; + map m_sensedObjectsMaps; +protected: static btGhostPairCallback* m_bulletGhostPairCallback; }; @@ -1600,6 +1631,54 @@ class afProximitySensor: public afRayTracerSensor{ }; +struct afContactSensorCallback : public btCollisionWorld::ContactResultCallback { + + ~afContactSensorCallback(); + + //! Constructor, pass whatever context you want to have available when processing contacts + /*! You may also want to set m_collisionFilterGroup and m_collisionFilterMask + * (supplied by the superclass) for needsCollision() */ + afContactSensorCallback(afBaseObjectPtr a_parentObject) : btCollisionWorld::ContactResultCallback(), m_parentObject(a_parentObject){ } + + afBaseObjectPtr m_parentObject; //!< The object the sensor is monitoring + + //! If you don't want to consider collisions where the bodies are joined by a constraint, override needsCollision: + /*! However, if you use a btCollisionObject for #body instead of a btRigidBody, + * then this is unnecessary—checkCollideWithOverride isn't available */ + virtual bool needsCollision(btBroadphaseProxy* proxy) const; + + //! Called with each contact for your own processing (e.g. test if contacts fall in within sensor parameters) + virtual btScalar addSingleResult(btManifoldPoint& cp, + const btCollisionObjectWrapper* colObj0,int partId0,int index0, + const btCollisionObjectWrapper* colObj1,int partId1,int index1); + + + + afContactEventMap m_contactEventMap; + double m_distanceThreshold = 0.0; +}; + + +class afContactSensor: public afSensor{ +public: + afContactSensor(afWorldPtr a_afWorld, afModelPtr a_modelPtr); + + virtual bool createFromAttribs(afContactSensorAttributes* a_attribs); + + virtual void updateSceneObjects(); + + virtual void update(double dt); + + bool m_processContactDetails; + + afContactSensorCallback* m_contactSensorCallback; + + cMultiPointPtr m_pointCloud; + + cMutex m_mutex; +}; + + /// /// \brief This is an implementation of Sleep function that tries to adjust sleep between each cycle to maintain /// the desired loop frequency. This class has been inspired from ROS Rate Sleep written by Eitan Marder-Eppstein @@ -2307,7 +2386,7 @@ class afWorld: public afIdentification, public afComm, public afModelManager{ cVector3d m_pickedOffset; - cMultiPoint* m_pickMultiPoint = nullptr; + cMultiPointPtr m_pickMultiPoint = nullptr; cPrecisionClock m_wallClock; diff --git a/ambf_plugins/core/ros_comm_plugin/ObjectCommPlugin.cpp b/ambf_plugins/core/ros_comm_plugin/ObjectCommPlugin.cpp index db3a61052..5a482dec4 100644 --- a/ambf_plugins/core/ros_comm_plugin/ObjectCommPlugin.cpp +++ b/ambf_plugins/core/ros_comm_plugin/ObjectCommPlugin.cpp @@ -1,5 +1,25 @@ #include "ObjectCommPlugin.h" +void copyVec(cVector3d* in, geometry_msgs::Vector3* out){ + out->x = in->x(); out->y = in->x(); out->z = in->z(); +} + +void fillContactData(afContactEventMap* conEventMap, vector* conEventMsgVec){ + afContactEventMap& eventMap = *conEventMap; + for (auto it : eventMap){ + ambf_msgs::ContactEvent conEventMsg; + conEventMsg.object_name.data = it.first->getQualifiedIdentifier(); + ambf_msgs::ContactData contDataMsg; + for (int in = 0 ; in < it.second.m_contactData.size() ; in ++){ + contDataMsg.distance.data = it.second.m_contactData[in].m_distance; + copyVec(&it.second.m_contactData[in].m_P_b_w, &contDataMsg.contact_point); + copyVec(&it.second.m_contactData[in].m_N_b_w, &contDataMsg.contact_normal); + conEventMsg.contact_data.push_back(contDataMsg); + } + conEventMsgVec->push_back(conEventMsg); + } +} + void afRigidBodyState::setChildrenNames(afRigidBodyPtr afRBPtr){ int num_children = afRBPtr->m_CJ_PairsActive.size(); if (num_children > 0){ @@ -127,11 +147,31 @@ int afObjectCommunicationPlugin::init(const afBaseObjectPtr a_afObjectPtr, const success = true; } break; + case afType::GHOST_OBJECT: + { + m_ghostObjectCommPtr.reset(new ambf_comm::GhostObject(objName, objNamespace, minFreq, maxFreq, timeOut)); + m_ghostObjectCommPtr->set_identifier(objQualifiedIdentifier); + success = true; + } + break; case afType::SENSOR: { - m_sensorCommPtr.reset(new ambf_comm::Sensor(objName, objNamespace, minFreq, maxFreq, timeOut)); - m_sensorCommPtr->set_identifier(objQualifiedIdentifier); afSensorPtr senPtr = (afSensorPtr) m_objectPtr; + switch (senPtr->m_sensorType){ + case afSensorType::RAYTRACER: + case afSensorType::RESISTANCE: + { + m_sensorCommPtr.reset(new ambf_comm::Sensor(objName, objNamespace, minFreq, maxFreq, timeOut)); + m_sensorCommPtr->set_identifier(objQualifiedIdentifier); + } + break; + case afSensorType::CONTACT: + { + m_contactSensorCommPtr.reset(new ambf_comm::ContactSensor(objName, objNamespace, minFreq, maxFreq, timeOut)); + m_contactSensorCommPtr->set_identifier(objQualifiedIdentifier); + } + break; + } switch (senPtr->m_sensorType) { case afSensorType::RAYTRACER: m_sensorCommPtr->set_type("PROXIMITY"); @@ -139,6 +179,9 @@ int afObjectCommunicationPlugin::init(const afBaseObjectPtr a_afObjectPtr, const case afSensorType::RESISTANCE: m_sensorCommPtr->set_type("RESISTANCE"); break; + case afSensorType::CONTACT: + m_contactSensorCommPtr->set_type("CONTACT"); + break; default: break; } @@ -217,6 +260,13 @@ void afObjectCommunicationPlugin::physicsUpdate(double dt) rigidBodyUpdateState(rbPtr, dt); } break; + case afType::GHOST_OBJECT: + { + afGhostObjectPtr goPtr = (afGhostObjectPtr)m_objectPtr; + ghostObjectFetchCommand(goPtr, dt); + ghostObjectUpdateState(goPtr, dt); + } + break; case afType::SENSOR:{ afSensorPtr senPtr = (afSensorPtr)m_objectPtr; sensorFetchCommand(senPtr, dt); @@ -279,11 +329,35 @@ void afObjectCommunicationPlugin::setTimeStamps(const double a_wall_time, const m_rigidBodyCommPtr->set_time_stamp(a_system_time); } break; + case afType::GHOST_OBJECT: + { + m_ghostObjectCommPtr->set_wall_time(a_wall_time); + m_ghostObjectCommPtr->set_sim_time(a_sim_time); + m_ghostObjectCommPtr->set_time_stamp(a_system_time); + } + break; case afType::SENSOR: { - m_sensorCommPtr->set_wall_time(a_wall_time); - m_sensorCommPtr->set_sim_time(a_sim_time); - m_sensorCommPtr->set_time_stamp(a_system_time); + afSensorPtr sensorPtr = (afSensorPtr) m_objectPtr; + switch (sensorPtr->m_sensorType) { + case afSensorType::RAYTRACER: + case afSensorType::RESISTANCE: + { + m_sensorCommPtr->set_wall_time(a_wall_time); + m_sensorCommPtr->set_sim_time(a_sim_time); + m_sensorCommPtr->set_time_stamp(a_system_time); + } + break; + case afSensorType::CONTACT: + { + m_contactSensorCommPtr->set_wall_time(a_wall_time); + m_contactSensorCommPtr->set_sim_time(a_sim_time); + m_contactSensorCommPtr->set_time_stamp(a_system_time); + } + break; + default: + break; + } } break; case afType::VEHICLE: @@ -820,20 +894,45 @@ void afObjectCommunicationPlugin::rigidBodyUpdateState(afRigidBodyPtr afRBPtr, d m_write_count++; } -void afObjectCommunicationPlugin::sensorFetchCommand(afSensorPtr senPtr, double dt) -{ +void afObjectCommunicationPlugin::ghostObjectFetchCommand(afGhostObjectPtr, double){ + +} + +void afObjectCommunicationPlugin::ghostObjectUpdateState(afGhostObjectPtr goPtr, double){ + + m_ghostObjectCommPtr->m_writeMtx.lock(); + setTimeStamps(m_objectPtr->m_afWorld->getWallTime(), m_objectPtr->m_afWorld->getSimulationTime(), m_objectPtr->getCurrentTimeStamp()); + m_ghostObjectCommPtr->set_parent_name(goPtr->m_parentName); + cVector3d localPos = goPtr->getLocalPos(); + m_ghostObjectCommPtr->cur_position(localPos.x(), localPos.y(), localPos.z()); + cQuaternion q; + q.fromRotMat(goPtr->getLocalRot()); + m_ghostObjectCommPtr->cur_orientation(q.x, q.y, q.z, q.w); + m_ghostObjectCommPtr->reset_sensed_objects(); + for (auto it : goPtr->m_sensedObjectsMaps){ + m_ghostObjectCommPtr->add_sensed_object(it.first->getQualifiedIdentifier()); + } + m_ghostObjectCommPtr->m_writeMtx.unlock(); + m_ghostObjectCommPtr->enableComm(); + + m_write_count++; +} + +void afObjectCommunicationPlugin::sensorFetchCommand(afSensorPtr senPtr, double dt){ } void afObjectCommunicationPlugin::sensorUpdateState(afSensorPtr senPtr, double dt) { - m_sensorCommPtr->m_writeMtx.lock(); - setTimeStamps(m_objectPtr->m_afWorld->getWallTime(), m_objectPtr->m_afWorld->getSimulationTime(), m_objectPtr->getCurrentTimeStamp()); switch (senPtr->m_sensorType) { case afSensorType::RAYTRACER: case afSensorType::RESISTANCE: case afSensorType::RANGE: { + + m_sensorCommPtr->m_writeMtx.lock(); + setTimeStamps(m_objectPtr->m_afWorld->getWallTime(), m_objectPtr->m_afWorld->getSimulationTime(), m_objectPtr->getCurrentTimeStamp()); + afRayTracerSensorPtr raySenPtr = (afRayTracerSensorPtr) senPtr; m_sensorCommPtr->set_count(raySenPtr->getCount()); m_sensorCommPtr->set_parent_name(raySenPtr->m_parentName); @@ -860,10 +959,10 @@ void afObjectCommunicationPlugin::sensorUpdateState(afSensorPtr senPtr, double d if (triggers[i]){ switch (raySenPtr->getSensedBodyType(i)) { case afBodyType::RIGID_BODY: - sensed_obj_names[i] = raySenPtr->getSensedRigidBody(i)->getName(); + sensed_obj_names[i] = raySenPtr->getSensedRigidBody(i)->getQualifiedName(); break; case afBodyType::SOFT_BODY: - sensed_obj_names[i] = raySenPtr->getSensedSoftBody(i)->getName(); + sensed_obj_names[i] = raySenPtr->getSensedSoftBody(i)->getQualifiedName(); default: sensed_obj_names[i] = ""; break; @@ -876,13 +975,30 @@ void afObjectCommunicationPlugin::sensorUpdateState(afSensorPtr senPtr, double d m_sensorCommPtr->set_measurements(measurements); m_sensorCommPtr->set_sensed_objects(sensed_obj_names); + m_sensorCommPtr->m_writeMtx.unlock(); + m_sensorCommPtr->enableComm(); + + } + break; + case afSensorType::CONTACT: + { + m_contactSensorCommPtr->m_writeMtx.lock(); + m_contactSensorCommPtr->reset_contact_events(); + setTimeStamps(m_objectPtr->m_afWorld->getWallTime(), m_objectPtr->m_afWorld->getSimulationTime(), m_objectPtr->getCurrentTimeStamp()); + afContactSensorPtr conSenPtr = (afContactSensorPtr) senPtr; + m_contactSensorCommPtr->set_parent_name(conSenPtr->m_parentName); + vector conEvents; + fillContactData(&conSenPtr->m_contactSensorCallback->m_contactEventMap, &conEvents); + for (auto v: conEvents){ + m_contactSensorCommPtr->add_contact_event(v); + } + m_contactSensorCommPtr->m_writeMtx.unlock(); + m_contactSensorCommPtr->enableComm(); } break; default: break; } - m_sensorCommPtr->m_writeMtx.unlock(); - m_sensorCommPtr->enableComm(); m_write_count++; } diff --git a/ambf_plugins/core/ros_comm_plugin/ObjectCommPlugin.h b/ambf_plugins/core/ros_comm_plugin/ObjectCommPlugin.h index 725edb278..4f51e6fbe 100644 --- a/ambf_plugins/core/ros_comm_plugin/ObjectCommPlugin.h +++ b/ambf_plugins/core/ros_comm_plugin/ObjectCommPlugin.h @@ -52,6 +52,7 @@ #include "ambf_server/Light.h" #include "ambf_server/Object.h" #include "ambf_server/RigidBody.h" +#include "ambf_server/GhostObject.h" #include "ambf_server/Sensor.h" #include "ambf_server/Vehicle.h" #include "ambf_server/World.h" @@ -104,6 +105,9 @@ class afObjectCommunicationPlugin: public afObjectPlugin{ void rigidBodyFetchCommand(afRigidBodyPtr, double); void rigidBodyUpdateState(afRigidBodyPtr, double); + void ghostObjectFetchCommand(afGhostObjectPtr, double); + void ghostObjectUpdateState(afGhostObjectPtr, double); + void sensorFetchCommand(afSensorPtr, double); void sensorUpdateState(afSensorPtr, double); @@ -124,7 +128,9 @@ class afObjectCommunicationPlugin: public afObjectPlugin{ std::shared_ptr m_lightCommPtr; std::shared_ptr m_objectCommPtr; std::shared_ptr m_rigidBodyCommPtr; + std::shared_ptr m_ghostObjectCommPtr; std::shared_ptr m_sensorCommPtr; + std::shared_ptr m_contactSensorCommPtr; std::shared_ptr m_vehicleCommPtr; std::shared_ptr m_pointCloudCommPtr; protected: diff --git a/ambf_ros_modules/ambf_client/python/ambf_client.py b/ambf_ros_modules/ambf_client/python/ambf_client.py index 3af55f9a3..81f7f0fe2 100755 --- a/ambf_ros_modules/ambf_client/python/ambf_client.py +++ b/ambf_ros_modules/ambf_client/python/ambf_client.py @@ -48,8 +48,9 @@ from ambf_msgs.msg import LightState, LightCmd from ambf_msgs.msg import ObjectState, ObjectCmd from ambf_msgs.msg import RigidBodyState, RigidBodyCmd +from ambf_msgs.msg import GhostObjectState,GhostObjectCmd from ambf_msgs.msg import WorldState, WorldCmd -from ambf_msgs.msg import SensorState, SensorCmd +from ambf_msgs.msg import SensorState, SensorCmd, ContactSensorState, ContactSensorCmd from ambf_msgs.msg import VehicleState, VehicleCmd from std_msgs.msg import Empty import threading @@ -59,7 +60,8 @@ from ambf_light import Light from ambf_object import Object from ambf_rigid_body import RigidBody -from ambf_sensor import Sensor +from ambf_ghost_object import GhostObject +from ambf_sensor import Sensor, ContactSensor from ambf_vehicle import Vehicle from ambf_world import World from difflib import SequenceMatcher @@ -77,6 +79,7 @@ def __init__(self, client_name='ambf_client'): self._client_name = client_name self._world_handle = None self._rate = None + self._default_queue_size = 10 pass def set_publish_rate(self, rate): @@ -103,7 +106,9 @@ def create_objs_from_rostopics(self, publish_rate): 'ambf_msgs/LightState', 'ambf_msgs/ObjectState', 'ambf_msgs/RigidBodyState', + 'ambf_msgs/GhostObjectState', 'ambf_msgs/SensorState', + 'ambf_msgs/ContactSensorState', 'ambf_msgs/VehicletState']: if first_run: first_run = False @@ -127,7 +132,7 @@ def create_objs_from_rostopics(self, publish_rate): world_obj = World(self._world_name) world_obj._sub = rospy.Subscriber(topic_name, WorldState, world_obj.ros_cb) world_obj._pub = rospy.Publisher(name=topic_name.replace('/State', '/Command'), data_class=WorldCmd, - queue_size=10) + queue_size=self._default_queue_size) world_obj._reset_pub = rospy.Publisher(name=topic_name.replace('/State', '/Command/Reset'), data_class=Empty, queue_size=1) world_obj._reset_bodies_pub = rospy.Publisher( @@ -142,7 +147,7 @@ def create_objs_from_rostopics(self, publish_rate): base_obj._cmd = ActuatorCmd() base_obj._sub = rospy.Subscriber(topic_name, ActuatorState, base_obj.ros_cb) base_obj._pub = rospy.Publisher(name=topic_name.replace('/State', '/Command'), data_class=ActuatorCmd, - tcp_nodelay=True, queue_size=10) + tcp_nodelay=True, queue_size=self._default_queue_size) self._objects_dict[base_obj.get_name()] = base_obj elif msg_type == 'ambf_msgs/CameraState': # pre_trimmed_name = topic_niyme.replace(self._common_obj_namespace, '') @@ -152,7 +157,7 @@ def create_objs_from_rostopics(self, publish_rate): base_obj._cmd = CameraCmd() base_obj._sub = rospy.Subscriber(topic_name, CameraState, base_obj.ros_cb) base_obj._pub = rospy.Publisher(name=topic_name.replace('/State', '/Command'), data_class=CameraCmd, - tcp_nodelay=True, queue_size=10) + tcp_nodelay=True, queue_size=self._default_queue_size) self._objects_dict[base_obj.get_name()] = base_obj elif msg_type == 'ambf_msgs/LightState': # pre_trimmed_name = topic_niyme.replace(self._common_obj_namespace, '') @@ -162,7 +167,7 @@ def create_objs_from_rostopics(self, publish_rate): base_obj._cmd = LightCmd() base_obj._sub = rospy.Subscriber(topic_name, LightState, base_obj.ros_cb) base_obj._pub = rospy.Publisher(name=topic_name.replace('/State', '/Command'), data_class=LightCmd, - tcp_nodelay=True, queue_size=10) + tcp_nodelay=True, queue_size=self._default_queue_size) self._objects_dict[base_obj.get_name()] = base_obj elif msg_type == 'ambf_msgs/ObjectState': # pre_trimmed_name = topic_niyme.replace(self._common_obj_namespace, '') @@ -172,7 +177,7 @@ def create_objs_from_rostopics(self, publish_rate): base_obj._cmd = ObjectCmd() base_obj._sub = rospy.Subscriber(topic_name, ObjectState, base_obj.ros_cb) base_obj._pub = rospy.Publisher(name=topic_name.replace('/State', '/Command'), data_class=ObjectCmd, - tcp_nodelay=True, queue_size=10) + tcp_nodelay=True, queue_size=self._default_queue_size) self._objects_dict[base_obj.get_name()] = base_obj elif msg_type == 'ambf_msgs/RigidBodyState': # pre_trimmed_name = topic_niyme.replace(self._common_obj_namespace, '') @@ -182,7 +187,17 @@ def create_objs_from_rostopics(self, publish_rate): base_obj._cmd = RigidBodyCmd() base_obj._sub = rospy.Subscriber(topic_name, RigidBodyState, base_obj.ros_cb) base_obj._pub = rospy.Publisher(name=topic_name.replace('/State', '/Command'), data_class=RigidBodyCmd, - tcp_nodelay=True, queue_size=10) + tcp_nodelay=True, queue_size=self._default_queue_size) + self._objects_dict[base_obj.get_name()] = base_obj + elif msg_type == 'ambf_msgs/GhostObjectState': + # pre_trimmed_name = topic_niyme.replace(self._common_obj_namespace, '') + post_trimmed_name = topic_name.replace('/State', '') + base_obj = GhostObject(post_trimmed_name) + base_obj._state = GhostObjectState() + base_obj._cmd = GhostObjectCmd() + base_obj._sub = rospy.Subscriber(topic_name, GhostObjectState, base_obj.ros_cb) + base_obj._pub = rospy.Publisher(name=topic_name.replace('/State', '/Command'), data_class=GhostObjectCmd, + tcp_nodelay=True, queue_size=self._default_queue_size) self._objects_dict[base_obj.get_name()] = base_obj elif msg_type == 'ambf_msgs/SensorState': # pre_trimmed_name = topic_niyme.replace(self._common_obj_namespace, '') @@ -192,7 +207,17 @@ def create_objs_from_rostopics(self, publish_rate): base_obj._cmd = SensorCmd() base_obj._sub = rospy.Subscriber(topic_name, SensorState, base_obj.ros_cb) base_obj._pub = rospy.Publisher(name=topic_name.replace('/State', '/Command'), data_class=SensorCmd, - tcp_nodelay=True, queue_size=10) + tcp_nodelay=True, queue_size=self._default_queue_size) + self._objects_dict[base_obj.get_name()] = base_obj + elif msg_type == 'ambf_msgs/ContactSensorState': + # pre_trimmed_name = topic_niyme.replace(self._common_obj_namespace, '') + post_trimmed_name = topic_name.replace('/State', '') + base_obj = ContactSensor(post_trimmed_name) + base_obj._state = ContactSensorState() + base_obj._cmd = ContactSensorCmd() + base_obj._sub = rospy.Subscriber(topic_name, ContactSensorState, base_obj.ros_cb) + base_obj._pub = rospy.Publisher(name=topic_name.replace('/State', '/Command'), data_class=ContactSensorCmd, + tcp_nodelay=True, queue_size=self._default_queue_size) self._objects_dict[base_obj.get_name()] = base_obj elif msg_type == 'ambf_msgs/VehicleState': # pre_trimmed_name = topic_niyme.replace(self._common_obj_namespace, '') @@ -202,7 +227,7 @@ def create_objs_from_rostopics(self, publish_rate): base_obj._cmd = VehicleCmd() base_obj._sub = rospy.Subscriber(topic_name, VehicleState, base_obj.ros_cb) base_obj._pub = rospy.Publisher(name=topic_name.replace('/State', '/Command'), data_class=VehicleCmd, - tcp_nodelay=True, queue_size=10) + tcp_nodelay=True, queue_size=self._default_queue_size) self._objects_dict[base_obj.get_name()] = base_obj def connect(self, default_publish_rate=120): diff --git a/ambf_ros_modules/ambf_client/python/ambf_ghost_object.py b/ambf_ros_modules/ambf_client/python/ambf_ghost_object.py new file mode 100644 index 000000000..2deb20683 --- /dev/null +++ b/ambf_ros_modules/ambf_client/python/ambf_ghost_object.py @@ -0,0 +1,92 @@ +#!/usr/bin/env python +# //============================================================================== +# /* +# Software License Agreement (BSD License) +# Copyright (c) 2020, AMBF +# (https://github.com/WPI-AIM/ambf) +# +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# +# * Neither the name of authors nor the names of its contributors may +# be used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# \author +# \author Adnan Munawar +# \version 1.0 +# */ +# //============================================================================== + +from ambf_msgs.msg import GhostObjectState +from ambf_msgs.msg import GhostObjectCmd +from ambf_base_object import BaseObject + + +class GhostObject(BaseObject): + def __init__(self, a_name, time_out=0.1): + """ + Constructor + :param a_name: + """ + super(GhostObject, self).__init__(a_name, time_out) # Set duration of Watchdog expiry + self.object_type = "GHOST" + self.body_type = "KINEMATIC" + + def _clear_command(self): + """ + Clear cmd if watchdog is expired + :return: + """ + + def get_sensed_obj_name(self, idx): + """ + Get the name of sensed object by a specific sensor element. + :param idx: + :return: + """ + if idx < len(self._state.sensed_objects): + return self._state.sensed_objects[idx].data + + + def get_all_sensed_obj_names(self): + """ + Get the name of sensed object by a specific sensor element. + :param idx: + :return: + """ + so = [v.data for v in self._state.sensed_objects] + return so + + def get_pose(self): + + """ + Get the pose of the sensor + :return geometry_msgs/Pose + """ + return self._state.pose + diff --git a/ambf_ros_modules/ambf_client/python/ambf_sensor.py b/ambf_ros_modules/ambf_client/python/ambf_sensor.py index 14faa34ac..202be6061 100755 --- a/ambf_ros_modules/ambf_client/python/ambf_sensor.py +++ b/ambf_ros_modules/ambf_client/python/ambf_sensor.py @@ -45,17 +45,32 @@ from ambf_msgs.msg import SensorState from ambf_msgs.msg import SensorCmd from ambf_base_object import BaseObject +from enum import Enum +class SensorType(Enum): + RAYTRACER=1 + CONTACT=2 -class Sensor(BaseObject): + +class SensorBase(BaseObject): + def __init__(self, sensor_type, a_name, time_out): + super().__init__(a_name, time_out) + self.object_type = "SENSOR" + self.body_type = "KINEMATIC" + self.sensor_type = sensor_type + + def get_type(self): + return self.sensor_type + + +class Sensor(SensorBase): def __init__(self, a_name, time_out=0.1): """ Constructor :param a_name: """ - super(Sensor, self).__init__(a_name, time_out) # Set duration of Watchdog expiry - self.object_type = "SENSOR" - self.body_type = "KINEMATIC" + super().__init__(SensorType.RAYTRACER, a_name, time_out) # Set duration of Watchdog expiry + def _clear_command(self): """ @@ -130,3 +145,51 @@ def get_pose(self): """ return self._state.pose + + +class ContactSensor(SensorBase): + def __init__(self, a_name, time_out=0.1): + """ + Constructor + :param a_name: + """ + super().__init__(SensorType.CONTACT, a_name, time_out) # Set duration of Watchdog expiry + + def _clear_command(self): + """ + Clear wrench if watchdog is expired + :return: + """ + + def get_contact_event(self, idx): + """ + Get the name of sensed object by a specific sensor element. + :param idx: + :return: + """ + if idx < len(self._state.contact_events): + return self._state.contact_events[idx] + + + def get_all_contact_event(self): + return self._state.contact_events + + + def get_contact_object_name(self, idx): + """ + Get the name of sensed object by a specific sensor element. + :param idx: + :return: + """ + if idx < len(self._state.contact_events): + return self._state.contact_events[idx].object_name.data + + + def get_all_contact_object_names(self): + contact_object_names = [nm.object_name.data for nm in self._state.contact_events] + return contact_object_names + + + def get_num_contact_events(self): + return len(self._state.contact_events) + diff --git a/ambf_ros_modules/ambf_msgs/CMakeLists.txt b/ambf_ros_modules/ambf_msgs/CMakeLists.txt index ffdf5aa6b..e21b0fb3b 100644 --- a/ambf_ros_modules/ambf_msgs/CMakeLists.txt +++ b/ambf_ros_modules/ambf_msgs/CMakeLists.txt @@ -59,6 +59,12 @@ find_package(catkin REQUIRED COMPONENTS ActuatorCmd.msg SensorState.msg SensorCmd.msg + ContactData.msg + ContactEvent.msg + ContactSensorState.msg + ContactSensorCmd.msg + GhostObjectState.msg + GhostObjectCmd.msg CameraState.msg CameraCmd.msg LightState.msg diff --git a/ambf_ros_modules/ambf_msgs/msg/ContactData.msg b/ambf_ros_modules/ambf_msgs/msg/ContactData.msg new file mode 100644 index 000000000..389586616 --- /dev/null +++ b/ambf_ros_modules/ambf_msgs/msg/ContactData.msg @@ -0,0 +1,6 @@ +# Separation or penetration distance between object a (parent) and b +std_msgs/Float64 distance +# Contact point in world coordinates on object b +geometry_msgs/Vector3 contact_point +# Contact normal in world coordinates on object b +geometry_msgs/Vector3 contact_normal diff --git a/ambf_ros_modules/ambf_msgs/msg/ContactEvent.msg b/ambf_ros_modules/ambf_msgs/msg/ContactEvent.msg new file mode 100644 index 000000000..31119dbe8 --- /dev/null +++ b/ambf_ros_modules/ambf_msgs/msg/ContactEvent.msg @@ -0,0 +1,3 @@ +# Name of object in contact +std_msgs/String object_name +ContactData[] contact_data \ No newline at end of file diff --git a/ambf_ros_modules/ambf_msgs/msg/ContactSensorCmd.msg b/ambf_ros_modules/ambf_msgs/msg/ContactSensorCmd.msg new file mode 100644 index 000000000..1f83c0ba2 --- /dev/null +++ b/ambf_ros_modules/ambf_msgs/msg/ContactSensorCmd.msg @@ -0,0 +1 @@ +Header header \ No newline at end of file diff --git a/ambf_ros_modules/ambf_msgs/msg/ContactSensorState.msg b/ambf_ros_modules/ambf_msgs/msg/ContactSensorState.msg new file mode 100644 index 000000000..e9aacc05d --- /dev/null +++ b/ambf_ros_modules/ambf_msgs/msg/ContactSensorState.msg @@ -0,0 +1,9 @@ +Header header +uint32 sim_step +std_msgs/String type +std_msgs/String parent_name +std_msgs/String identifier +std_msgs/String name +float32 wall_time +float32 sim_time +ContactEvent[] contact_events \ No newline at end of file diff --git a/ambf_ros_modules/ambf_msgs/msg/GhostObjectCmd.msg b/ambf_ros_modules/ambf_msgs/msg/GhostObjectCmd.msg new file mode 100644 index 000000000..1f83c0ba2 --- /dev/null +++ b/ambf_ros_modules/ambf_msgs/msg/GhostObjectCmd.msg @@ -0,0 +1 @@ +Header header \ No newline at end of file diff --git a/ambf_ros_modules/ambf_msgs/msg/GhostObjectState.msg b/ambf_ros_modules/ambf_msgs/msg/GhostObjectState.msg new file mode 100644 index 000000000..6455ad61d --- /dev/null +++ b/ambf_ros_modules/ambf_msgs/msg/GhostObjectState.msg @@ -0,0 +1,9 @@ +Header header +uint32 sim_step +std_msgs/String parent_name +std_msgs/String identifier +std_msgs/String name +float32 wall_time +float32 sim_time +geometry_msgs/Pose pose +std_msgs/String[] sensed_objects \ No newline at end of file diff --git a/ambf_ros_modules/ambf_server/CMakeLists.txt b/ambf_ros_modules/ambf_server/CMakeLists.txt index 7c5c3fa98..7a97b4ddc 100644 --- a/ambf_ros_modules/ambf_server/CMakeLists.txt +++ b/ambf_ros_modules/ambf_server/CMakeLists.txt @@ -80,6 +80,8 @@ set (HDR_FILES include/${PROJECT_NAME}/ObjectRosCom.h include/${PROJECT_NAME}/RigidBody.h include/${PROJECT_NAME}/RigidBodyRosCom.h + include/${PROJECT_NAME}/GhostObject.h + include/${PROJECT_NAME}/GhostObjectRosCom.h include/${PROJECT_NAME}/Sensor.h include/${PROJECT_NAME}/SensorRosCom.h include/${PROJECT_NAME}/Vehicle.h @@ -99,6 +101,8 @@ set (SRC_FILES src/ObjectRosCom.cpp src/RigidBody.cpp src/RigidBodyRosCom.cpp + src/GhostObject.cpp + src/GhostObjectRosCom.cpp src/Sensor.cpp src/SensorRosCom.cpp src/Vehicle.cpp diff --git a/ambf_ros_modules/ambf_server/include/ambf_server/GhostObject.h b/ambf_ros_modules/ambf_server/include/ambf_server/GhostObject.h new file mode 100644 index 000000000..bfd401a4e --- /dev/null +++ b/ambf_ros_modules/ambf_server/include/ambf_server/GhostObject.h @@ -0,0 +1,67 @@ +//============================================================================== +/* + Software License Agreement (BSD License) + Copyright (c) 2019-2021, AMBF + (https://github.com/WPI-AIM/ambf) + + All rights reserved. + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions + are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above + copyright notice, this list of conditions and the following + disclaimer in the documentation and/or other materials provided + with the distribution. + + * Neither the name of authors nor the names of its contributors may + be used to endorse or promote products derived from this software + without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + POSSIBILITY OF SUCH DAMAGE. + + \author + \author Adnan Munawar +*/ +//============================================================================== + +#ifndef AFGHOSTCOMM_H +#define AFGHOSTCOMM_H + +#include +#include "ambf_server/GhostObjectRosCom.h" + +namespace ambf_comm{ + +class GhostObject: public GhostObjectRosCom{ +public: + GhostObject(std::string a_name, std::string a_namespace, int a_freq_min, int a_freq_max, double time_out); + void cur_position(double px, double py, double pz); + void cur_orientation(double roll, double pitch, double yaw); + void cur_orientation(double qx, double qy, double qz, double qw); + inline void set_parent_name(std::string parent_name){m_State.parent_name.data = parent_name;} + + void reset_sensed_objects(); + void add_sensed_object(std::string sensed_object); + void set_sensed_objects(std::vector& sensed_objects); + +}; + +} + +#endif diff --git a/ambf_ros_modules/ambf_server/include/ambf_server/GhostObjectRosCom.h b/ambf_ros_modules/ambf_server/include/ambf_server/GhostObjectRosCom.h new file mode 100644 index 000000000..ec432c6ac --- /dev/null +++ b/ambf_ros_modules/ambf_server/include/ambf_server/GhostObjectRosCom.h @@ -0,0 +1,63 @@ +//============================================================================== +/* + Software License Agreement (BSD License) + Copyright (c) 2019-2021, AMBF + (https://github.com/WPI-AIM/ambf) + + All rights reserved. + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions + are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above + copyright notice, this list of conditions and the following + disclaimer in the documentation and/or other materials provided + with the distribution. + + * Neither the name of authors nor the names of its contributors may + be used to endorse or promote products derived from this software + without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + POSSIBILITY OF SUCH DAMAGE. + + \author + \author Adnan Munawar +*/ +//============================================================================== + +#ifndef GHOSTOBJECTROSCOM_H +#define GHOSTOBJECTROSCOM_H + +#include "ambf_server/RosComBase.h" +#include "ambf_msgs/GhostObjectState.h" +#include "ambf_msgs/GhostObjectCmd.h" + + +class GhostObjectRosCom: public RosComBase{ +public: + GhostObjectRosCom(std::string a_name, std::string a_namespace, int a_freq_min, int a_freq_max, double time_out); + virtual void init(); + +protected: + virtual void reset_cmd(); + void sub_cb(ambf_msgs::GhostObjectCmdConstPtr msg); +}; + + + +#endif diff --git a/ambf_ros_modules/ambf_server/include/ambf_server/Sensor.h b/ambf_ros_modules/ambf_server/include/ambf_server/Sensor.h index 24fc75ed4..972dd6d9e 100644 --- a/ambf_ros_modules/ambf_server/include/ambf_server/Sensor.h +++ b/ambf_ros_modules/ambf_server/include/ambf_server/Sensor.h @@ -47,6 +47,7 @@ #include "ambf_server/SensorRosCom.h" namespace ambf_comm{ + class Sensor: public SensorRosCom{ public: Sensor(std::string a_name, std::string a_namespace, int a_freq_min, int a_freq_max, double time_out); @@ -79,6 +80,16 @@ class Sensor: public SensorRosCom{ void set_sensed_object_map(int sensed_objects_map); }; + +class ContactSensor: public ContactSensorRosCom{ +public: + ContactSensor(std::string a_name, std::string a_namespace, int a_freq_min, int a_freq_max, double time_out); + inline void set_parent_name(std::string parent_name){m_State.parent_name.data = parent_name;} + + void set_type(std::string type); + void add_contact_event(ambf_msgs::ContactEvent& a_contact_event); + void reset_contact_events(); +}; } #endif diff --git a/ambf_ros_modules/ambf_server/include/ambf_server/SensorRosCom.h b/ambf_ros_modules/ambf_server/include/ambf_server/SensorRosCom.h index 544c38cb1..e99896224 100644 --- a/ambf_ros_modules/ambf_server/include/ambf_server/SensorRosCom.h +++ b/ambf_ros_modules/ambf_server/include/ambf_server/SensorRosCom.h @@ -46,9 +46,23 @@ #include "ambf_server/RosComBase.h" #include "ambf_msgs/SensorState.h" #include "ambf_msgs/SensorCmd.h" +#include "ambf_msgs/ContactSensorState.h" +#include "ambf_msgs/ContactSensorCmd.h" +#include "ambf_msgs/ContactEvent.h" +#include "ambf_msgs/ContactData.h" -class SensorRosCom: public RosComBase{ +template +class SensorRosComBase: public RosComBase{ +public: + SensorRosComBase(std::string a_name, std::string a_namespace, int a_freq_min, int a_freq_max, double time_out); + virtual void init(){} + +protected: + virtual void reset_cmd(){} +}; + +class SensorRosCom: public SensorRosComBase{ public: SensorRosCom(std::string a_name, std::string a_namespace, int a_freq_min, int a_freq_max, double time_out); virtual void init(); @@ -58,5 +72,15 @@ class SensorRosCom: public RosComBase{ +public: + ContactSensorRosCom(std::string a_name, std::string a_namespace, int a_freq_min, int a_freq_max, double time_out); + virtual void init(); + +protected: + virtual void reset_cmd(); + void sub_cb(ambf_msgs::ContactSensorCmdConstPtr msg); +}; + #endif diff --git a/ambf_ros_modules/ambf_server/src/GhostObject.cpp b/ambf_ros_modules/ambf_server/src/GhostObject.cpp new file mode 100644 index 000000000..97e65ef4b --- /dev/null +++ b/ambf_ros_modules/ambf_server/src/GhostObject.cpp @@ -0,0 +1,89 @@ +//============================================================================== +/* + Software License Agreement (BSD License) + Copyright (c) 2019-2021, AMBF + (https://github.com/WPI-AIM/ambf) + + All rights reserved. + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions + are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above + copyright notice, this list of conditions and the following + disclaimer in the documentation and/or other materials provided + with the distribution. + + * Neither the name of authors nor the names of its contributors may + be used to endorse or promote products derived from this software + without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + POSSIBILITY OF SUCH DAMAGE. + + \author + \author Adnan Munawar +*/ +//============================================================================== + +#include "ambf_server/GhostObject.h" +namespace ambf_comm{ + +GhostObject::GhostObject(std::string a_name, std::string a_namespace, int a_freq_min, int a_freq_max, double time_out): GhostObjectRosCom(a_name, a_namespace, a_freq_min, a_freq_max, time_out){ +} + +void GhostObject::cur_position(double px, double py, double pz){ + m_trans.setOrigin(tf::Vector3(px, py, pz)); + m_State.pose.position.x = px; + m_State.pose.position.y = py; + m_State.pose.position.z = pz; +} + +void GhostObject::cur_orientation(double roll, double pitch, double yaw){ + tf::Quaternion rot_quat; + rot_quat.setRPY(roll, pitch, yaw); + m_trans.setRotation(rot_quat); + tf::quaternionTFToMsg(rot_quat, m_State.pose.orientation); +} + +void GhostObject::cur_orientation(double qx, double qy, double qz, double qw){ + tf::Quaternion rot_quat(qx, qy, qz, qw); + m_trans.setRotation(rot_quat); + tf::quaternionTFToMsg(rot_quat, m_State.pose.orientation); +} + +void GhostObject::reset_sensed_objects(){ + m_State.sensed_objects.clear(); +} + +void GhostObject::add_sensed_object(std::string sensed_object){ + std_msgs::String obj; + obj.data = sensed_object; + m_State.sensed_objects.push_back(obj); +} + +void GhostObject::set_sensed_objects(std::vector& sensed_objects){ + if (m_State.sensed_objects.size() != sensed_objects.size()){ + m_State.sensed_objects.resize(sensed_objects.size()); + } + + for (int i = 0 ; i < sensed_objects.size() ; i++){ + m_State.sensed_objects[i].data = sensed_objects[i]; + } +} + +} diff --git a/ambf_ros_modules/ambf_server/src/GhostObjectRosCom.cpp b/ambf_ros_modules/ambf_server/src/GhostObjectRosCom.cpp new file mode 100644 index 000000000..1def68d91 --- /dev/null +++ b/ambf_ros_modules/ambf_server/src/GhostObjectRosCom.cpp @@ -0,0 +1,66 @@ +//============================================================================== +/* + Software License Agreement (BSD License) + Copyright (c) 2019-2021, AMBF + (https://github.com/WPI-AIM/ambf) + + All rights reserved. + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions + are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above + copyright notice, this list of conditions and the following + disclaimer in the documentation and/or other materials provided + with the distribution. + + * Neither the name of authors nor the names of its contributors may + be used to endorse or promote products derived from this software + without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + POSSIBILITY OF SUCH DAMAGE. + + \author + \author Adnan Munawar +*/ +//============================================================================== + +#include "ambf_server/GhostObjectRosCom.h" + +GhostObjectRosCom::GhostObjectRosCom(std::string a_name, std::string a_namespace, int a_freq_min, int a_freq_max, double time_out): RosComBase(a_name, a_namespace, a_freq_min, a_freq_max, time_out){ + init(); +} + +void GhostObjectRosCom::init(){ + m_State.name.data = m_name; + m_State.sim_step = 0; + + m_pub = nodePtr->advertise("/" + m_namespace + "/" + m_name + "/State", 10); + m_sub = nodePtr->subscribe("/" + m_namespace + "/" + m_name + "/Command", 10, &GhostObjectRosCom::sub_cb, this); + + m_thread = boost::thread(boost::bind(&GhostObjectRosCom::run_publishers, this)); + std::cerr << "INFO! Thread Joined: " << m_name << std::endl; +} + +void GhostObjectRosCom::reset_cmd(){ +} + +void GhostObjectRosCom::sub_cb(ambf_msgs::GhostObjectCmdConstPtr msg){ + m_Cmd = *msg; + m_watchDogPtr->acknowledge_wd(); +} diff --git a/ambf_ros_modules/ambf_server/src/RosComBase.cpp b/ambf_ros_modules/ambf_server/src/RosComBase.cpp index eee8fef89..e679cc12a 100644 --- a/ambf_ros_modules/ambf_server/src/RosComBase.cpp +++ b/ambf_ros_modules/ambf_server/src/RosComBase.cpp @@ -52,8 +52,12 @@ #include "ambf_msgs/ObjectState.h" #include "ambf_msgs/RigidBodyCmd.h" #include "ambf_msgs/RigidBodyState.h" +#include "ambf_msgs/GhostObjectCmd.h" +#include "ambf_msgs/GhostObjectState.h" #include "ambf_msgs/SensorCmd.h" #include "ambf_msgs/SensorState.h" +#include "ambf_msgs/ContactSensorState.h" +#include "ambf_msgs/ContactSensorCmd.h" #include "ambf_msgs/VehicleCmd.h" #include "ambf_msgs/VehicleState.h" #include "ambf_msgs/WorldCmd.h" @@ -92,7 +96,9 @@ template void RosComBase::cleanUp( template void RosComBase::cleanUp(); template void RosComBase::cleanUp(); template void RosComBase::cleanUp(); +template void RosComBase::cleanUp(); template void RosComBase::cleanUp(); +template void RosComBase::cleanUp(); template void RosComBase::cleanUp(); template void RosComBase::cleanUp(); @@ -101,6 +107,8 @@ template RosComBase::RosComBase(st template RosComBase::RosComBase(std::string a_name, std::string a_namespace, int a_freq_min, int a_freq_max, double time_out); template RosComBase::RosComBase(std::string a_name, std::string a_namespace, int a_freq_min, int a_freq_max, double time_out); template RosComBase::RosComBase(std::string a_name, std::string a_namespace, int a_freq_min, int a_freq_max, double time_out); +template RosComBase::RosComBase(std::string a_name, std::string a_namespace, int a_freq_min, int a_freq_max, double time_out); template RosComBase::RosComBase(std::string a_name, std::string a_namespace, int a_freq_min, int a_freq_max, double time_out); +template RosComBase::RosComBase(std::string a_name, std::string a_namespace, int a_freq_min, int a_freq_max, double time_out); template RosComBase::RosComBase(std::string a_name, std::string a_namespace, int a_freq_min, int a_freq_max, double time_out); template RosComBase::RosComBase(std::string a_name, std::string a_namespace, int a_freq_min, int a_freq_max, double time_out); diff --git a/ambf_ros_modules/ambf_server/src/Sensor.cpp b/ambf_ros_modules/ambf_server/src/Sensor.cpp index 0a6ffa78d..5311a62d2 100644 --- a/ambf_ros_modules/ambf_server/src/Sensor.cpp +++ b/ambf_ros_modules/ambf_server/src/Sensor.cpp @@ -170,4 +170,21 @@ void destroy_sensor(Sensor* obj){ } } + +ContactSensor::ContactSensor(std::string a_name, std::string a_namespace, int a_freq_min, int a_freq_max, double time_out): ContactSensorRosCom(a_name, a_namespace, a_freq_min, a_freq_max, time_out){ + +} + +void ContactSensor::set_type(std::string type){ + m_State.type.data = type; +} + +void ContactSensor::add_contact_event(ambf_msgs::ContactEvent& a_contact_event){ + m_State.contact_events.push_back(a_contact_event); +} + +void ContactSensor::reset_contact_events(){ + m_State.contact_events.clear(); +} + } diff --git a/ambf_ros_modules/ambf_server/src/SensorRosCom.cpp b/ambf_ros_modules/ambf_server/src/SensorRosCom.cpp index 93335b7f0..21b52c775 100644 --- a/ambf_ros_modules/ambf_server/src/SensorRosCom.cpp +++ b/ambf_ros_modules/ambf_server/src/SensorRosCom.cpp @@ -42,7 +42,7 @@ #include "ambf_server/SensorRosCom.h" -SensorRosCom::SensorRosCom(std::string a_name, std::string a_namespace, int a_freq_min, int a_freq_max, double time_out): RosComBase(a_name, a_namespace, a_freq_min, a_freq_max, time_out){ +SensorRosCom::SensorRosCom(std::string a_name, std::string a_namespace, int a_freq_min, int a_freq_max, double time_out): SensorRosComBase(a_name, a_namespace, a_freq_min, a_freq_max, time_out){ init(); } @@ -67,3 +67,34 @@ void SensorRosCom::sub_cb(ambf_msgs::SensorCmdConstPtr msg){ m_Cmd = *msg; m_watchDogPtr->acknowledge_wd(); } + + +ContactSensorRosCom::ContactSensorRosCom(std::string a_name, std::string a_namespace, int a_freq_min, int a_freq_max, double time_out): SensorRosComBase(a_name, a_namespace, a_freq_min, a_freq_max, time_out){ + init(); +} + +void ContactSensorRosCom::init(){ + m_State.name.data = m_name; + m_State.sim_step = 0; + + m_pub = nodePtr->advertise("/" + m_namespace + "/" + m_name + "/State", 10); + m_sub = nodePtr->subscribe("/" + m_namespace + "/" + m_name + "/Command", 10, &ContactSensorRosCom::sub_cb, this); + + m_thread = boost::thread(boost::bind(&ContactSensorRosCom::run_publishers, this)); + std::cerr << "INFO! Thread Joined: " << m_name << std::endl; +} + +void ContactSensorRosCom::reset_cmd(){ + +} + +void ContactSensorRosCom::sub_cb(ambf_msgs::ContactSensorCmdConstPtr msg){ + m_Cmd = *msg; + m_watchDogPtr->acknowledge_wd(); +} + +template +SensorRosComBase::SensorRosComBase(std::string a_name, std::string a_namespace, int a_freq_min, int a_freq_max, double time_out): RosComBase(a_name, a_namespace, a_freq_min, a_freq_max, time_out){ +} + +//template SensorRosComBase::SensorRosComBase(std::string a_name, std::string a_namespace, int a_freq_min, int a_freq_max, double time_out);