/*
* This file is part of ArmarX.
*
* ArmarX is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* ArmarX is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @author Fabian Reister ( fabian dot reister at kit dot edu )
* @date 2024
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
#include "FamiliarObjectDetectionExample.h"
#include <Eigen/Geometry>
#include <pcl/point_types.h>
#include <opencv2/core/mat.hpp>
#include <IceUtil/Time.h>
#include <SimoxUtility/math/pose/pose.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/RobotIO.h>
#include "ArmarXCore/core/time/Clock.h"
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include "RobotAPI/libraries/ArmarXObjects/ObjectID.h"
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem_objects/aron/FamiliarObjectInstance.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/types.h>
// #include <RobotAPI/libraries/armem_objects/aron_conversions.h>
// #include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
namespace armarx::familiar_objects
{
FamiliarObjectDetectionExample::FamiliarObjectDetectionExample()
{
addPlugin(familiarObjectInstanceReaderPlugin);
p.exemplaryFamiliarObjectID.dataset = "myDataset";
p.exemplaryFamiliarObjectID.className = "sphere";
p.exemplaryFamiliarObjectID.instanceName = "0";
}
armarx::PropertyDefinitionsPtr
FamiliarObjectDetectionExample::createPropertyDefinitions()
{
armarx::PropertyDefinitionsPtr defs =
new ComponentPropertyDefinitions(getConfigIdentifier());
defs->required(p.robotName, "p.robotName");
defs->optional(p.exemplaryFamiliarObjectID.dataset, "p.objectId.dataset");
defs->optional(p.exemplaryFamiliarObjectID.className, "p.objectId.className");
defs->optional(p.exemplaryFamiliarObjectID.instanceName, "p.objectId.instanceName");
defs->component(familiarObjectPoseStoragePrx, "ObjectMemory");
return defs;
}
std::string
FamiliarObjectDetectionExample::getDefaultName() const
{
return "FamiliarObjectDetectionExample";
}
void
FamiliarObjectDetectionExample::onInitComponent()
{
}
void
FamiliarObjectDetectionExample::onConnectComponent()
{
//
// First, we create a familiar object and send it to the memory.
//
ARMARX_IMPORTANT << "Storing exemplary familiar object in memory";
storeExemplaryFamiliarObjectInMemory();
//
// Then, we read the familiar object from the memory.
//
ARMARX_IMPORTANT << "Reading familiar object from memory";
readExemplaryFamiliarObjectFromMemory();
}
void
FamiliarObjectDetectionExample::storeExemplaryFamiliarObjectInMemory() const
{
::armarx::objpose::ProvidedFamiliarObjectPoseSeq data;
armem::arondto::FamiliarObjectInstance familiarObject;
familiarObject.timestamp = armarx::Clock::Now();
familiarObject.poseSensFrame.pose =
Eigen::Isometry3f{Eigen::Translation3f{0, 0, 1000}}.matrix();
familiarObject.poseSensFrame.header.frame = "DepthCameraSim";
familiarObject.poseSensFrame.header.agent = p.robotName;
// familiarObject.poseSensFrame.header.frame = "AzureKinect_RGB";
familiarObject.objectID = p.exemplaryFamiliarObjectID;
familiarObject.confidence = 1.0;
// familiarObject.bounding_box =
// familiarObject.depth_image_patch = cv::Mat1f(cv::Size(20, 20));
// familiarObject.rgb_image_patch = cv::Mat3b(cv::Size(20, 20));
const int numPoints = 100;
// x in red
for (int i = 0; i < numPoints; i++)
{
pcl::PointXYZRGBA point;
point.x = static_cast<float>(i) - numPoints / 2;
point.y = 0;
point.z = 0;
point.r = 255;
point.g = 0;
point.b = 0;
familiarObject.points.points.push_back(point);
}
// y in green
for (int i = 0; i < numPoints; i++)
{
pcl::PointXYZRGBA point;
point.x = 0;
point.y = static_cast<float>(i) - numPoints / 2;
point.z = 0;
point.r = 0;
point.g = 255;
point.b = 0;
familiarObject.points.points.push_back(point);
}
// z in blue
for (int i = 0; i < numPoints; i++)
{
pcl::PointXYZRGBA point;
point.y = 0;
point.y = 0;
point.z = static_cast<float>(i) - numPoints / 2;
point.r = 0;
point.g = 0;
point.b = 255;
familiarObject.points.points.push_back(point);
}
familiarObject.points.header.frame_id = "DepthCameraSim";
familiarObject.points.is_dense = true;
familiarObject.points.width = familiarObject.points.points.size();
familiarObject.points.height = 1;
familiarObject.bounding_box.center.setZero();
familiarObject.bounding_box.extents.setConstant(numPoints);
data.push_back(familiarObject.toAronDTO());
ARMARX_INFO << "Sending " << data.size() << " familiar object to the memory";
familiarObjectPoseStoragePrx->reportFamiliarObjectPoses(getName(), data);
}
void
FamiliarObjectDetectionExample::readExemplaryFamiliarObjectFromMemory() const
{
ARMARX_CHECK_NOT_NULL(familiarObjectInstanceReaderPlugin);
const armem::obj::familiar_object_instance::Reader& familiarObjectInstanceReader =
familiarObjectInstanceReaderPlugin->get();
//
// 1. Read from all providers
//
ARMARX_IMPORTANT << "Reading from all providers";
{
const auto allFamiliarObjectInstances =
familiarObjectInstanceReader.queryAllLatestFamiliarObjectInstances();
// print
for (const auto& [providerName, instances] : allFamiliarObjectInstances)
{
ARMARX_INFO << "Provider name: " << providerName;
for (const auto& instance : instances)
{
ARMARX_INFO << "- Instance: " << instance.objectID.dataset << "/"
<< instance.objectID.className << "/"
<< instance.objectID.instanceName;
}
}
}
//
// 2. Read from a specific provider
//
ARMARX_IMPORTANT << "Reading from a specific provider";
{
const std::map<std::string, std::vector<armem::arondto::FamiliarObjectInstance>>
familiarObjectInstances =
familiarObjectInstanceReader.queryAllLatestFamiliarObjectInstances(getName());
ARMARX_INFO << "Provider name: " << getName();
ARMARX_CHECK_EQUAL(familiarObjectInstances.size(), 1);
ARMARX_CHECK_EQUAL(familiarObjectInstances.begin()->first, getName());
const auto& thisFamiliarObjectInstances = familiarObjectInstances.begin()->second;
// print
for (const auto& instance : thisFamiliarObjectInstances)
{
ARMARX_INFO << "- Instance: " << instance.objectID.dataset << "/"
<< instance.objectID.className << "/" << instance.objectID.instanceName;
}
}
//
// 3. Read all instances of a specific class
//
ARMARX_IMPORTANT << "Reading all instances of a specific class";
{
armarx::ObjectID objectId;
fromAron(p.exemplaryFamiliarObjectID, objectId);
const auto instances =
familiarObjectInstanceReader.queryLatestFamiliarObjectInstancesFromClass(
objectId.getClassID());
for (const auto& [instanceName, instancesForProvider] : instances)
{
for (const auto& instance : instancesForProvider)
{
ARMARX_INFO << "- Instance: " << instance.objectID.dataset << "/"
<< instance.objectID.className << "/"
<< instance.objectID.instanceName;
}
}
}
//
// 4. Read a specific instance
//
ARMARX_IMPORTANT << "Reading a specific instance";
{
armarx::ObjectID objectId;
fromAron(p.exemplaryFamiliarObjectID, objectId);
const std::optional<std::map<std::string, armem::arondto::FamiliarObjectInstance>>
instances =
familiarObjectInstanceReader.queryLatestFamiliarObjectInstance(objectId);
ARMARX_CHECK(instances.has_value());
for (const auto& [instanceName, instance] : instances.value())
{
ARMARX_INFO << "- Instance: " << instance.objectID.dataset << "/"
<< instance.objectID.className << "/" << instance.objectID.instanceName;
}
}
}
void
FamiliarObjectDetectionExample::onDisconnectComponent()
{
// task->stop();
}
void
FamiliarObjectDetectionExample::onExitComponent()
{
}
} // namespace armarx::familiar_objects