Adding the value to the object messages

dkurzaj · Sep 16, 2016 · 6900310 · 6900310
1 parent 6717819
commit 6900310
Show file tree

Hide file tree

Showing 4 changed files with 9 additions and 34 deletions.
diff --git a/pdg/src/OM2MObjectReader.cpp b/pdg/src/OM2MObjectReader.cpp
@@ -27,7 +27,6 @@ void OM2MObjectReader::newValueCallBack(const iot_bridge::IoTDataWithCoordinates
         curObject = new MovableIoTObject(msg->iot_data.data.key);
         curObject->setName(msg->iot_data.data.key);
     } else {
-        ROS_INFO("[pdg][OM2MObjectReader] New value from existing %s", (msg->iot_data.data.key).c_str());
         curObject = (MovableIoTObject*)lastConfig_[msg->iot_data.data.key];
     }
 

diff --git a/pdg/src/main.cpp b/pdg/src/main.cpp
@@ -60,6 +60,11 @@ Entity newPoseEnt_("");
 // Service client
 ros::ServiceClient* setPoseClient_;
 
+void fillValue(MovableIoTObject* srcObject, toaster_msgs::Object& msgObject) {
+    ROS_INFO("[pdg][main] New value from %s: %s", (srcObject->getId()).c_str(), (srcObject->getValue()).c_str());
+    msgObject.value = srcObject->getValue();
+}
+
 void fillEntity(Entity* srcEntity, toaster_msgs::Entity& msgEntity) {
     msgEntity.id = srcEntity->getId();
     msgEntity.time = srcEntity->getTime();
@@ -699,6 +704,7 @@ int main(int argc, char** argv) {
                     ROS_INFO("got true");
                 }
 
+
                 // If in hand, modify position:
                 if (objectInAgent_.find(it->first) != objectInAgent_.end()) {
                     bool addFactHand = true;
@@ -747,39 +753,8 @@ int main(int argc, char** argv) {
                     ROS_INFO("got true");
                 }
 
-                // If in hand, modify position:
-                if (objectInAgent_.find(it->first) != objectInAgent_.end()) {
-                    bool addFactHand = true;
-                    if (!putAtJointPosition(it->second, objectInAgent_[it->first], objectInHand_[it->first],
-                                            humanList_msg, true))
-                        if (!putAtJointPosition(it->second, objectInAgent_[it->first], objectInHand_[it->first],
-                                                robotList_msg, true)) {
-                            ROS_INFO("[pdg][put_in_hand] couldn't find joint %s for agent %s \n",
-                                     objectInHand_[it->first].c_str(), objectInAgent_[it->first].c_str());
-                            addFactHand = false;
-                        }
-
-                    if (addFactHand) {
-
-                        //Fact message
-                        fact_msg.property = "IsInHand";
-                        fact_msg.propertyType = "position";
-                        fact_msg.subProperty = "object";
-                        fact_msg.subjectId = it->first;
-                        fact_msg.targetId = objectInAgent_[it->first];
-                        fact_msg.targetOwnerId = objectInAgent_[it->first];
-                        fact_msg.confidence = 1.0;
-                        fact_msg.factObservability = 0.8;
-                        fact_msg.time = it->second->getTime();
-                        fact_msg.valueType = 0;
-                        fact_msg.stringValue = "true";
-
-
-                        factList_msg.factList.push_back(fact_msg);
-                    }
-
-                }
                 //Message for object
+                fillValue(static_cast<MovableIoTObject*>(it->second), object_msg);
                 fillEntity(it->second, object_msg.meEntity);
                 objectList_msg.objectList.push_back(object_msg);
             }

diff --git a/toaster_msgs/msg/Object.msg b/toaster_msgs/msg/Object.msg
@@ -3,5 +3,6 @@ uint32 container
 uint32[] containedObjects
 uint32[] aboveObjects
 uint32[] closeObjects
+string value
 Entity meEntity
 
diff --git a/tools/toaster_scripts/shell/MaRDi/area_init2_iot.sh b/tools/toaster_scripts/shell/MaRDi/area_init2_iot.sh
@@ -8,7 +8,7 @@
 
 # spark (branch genom / mardi_dev2)
 rosservice call /pdg/manage_stream "{morseHuman: false, niutHuman: false, groupHuman: false, mocapHuman: false, pr2Robot: false,
-  spencerRobot: false, toasterSimuObject: true, arObject: true, om2mObject: true}"
+  spencerRobot: false, toasterSimuHuman: true, toasterSimuObject: true, arObject: true, om2mObject: true}"
 
 # no genom (branch master)
 #rosservice call /pdg/manage_stream "{morseHuman: false, niutHuman: false, groupHuman: false, mocapHuman: true, pr2Robot: true,