From c91b08bb2d4281828d19ed84e90904bdbad99baa Mon Sep 17 00:00:00 2001
From: armar-user <armar6@kit>
Date: Fri, 17 Feb 2023 09:42:10 +0100
Subject: [PATCH] complete working draft for LookForObjectWithKinematicUnit
---
.../Component.cpp | 6 +-
.../view_selection_skill_provider/Component.h | 5 +
.../skills/LookForObjectWithKinematicUnit.cpp | 119 +++++++-------
.../view_selection/skills/ScanLocation.cpp | 152 +++++++++---------
.../view_selection/skills/ScanLocation.h | 14 +-
5 files changed, 152 insertions(+), 144 deletions(-)
diff --git a/source/armarx/view_selection/components/view_selection_skill_provider/Component.cpp b/source/armarx/view_selection/components/view_selection_skill_provider/Component.cpp
index b9dd6cd..d527d9f 100644
--- a/source/armarx/view_selection/components/view_selection_skill_provider/Component.cpp
+++ b/source/armarx/view_selection/components/view_selection_skill_provider/Component.cpp
@@ -69,6 +69,8 @@ namespace armarx::view_selection::components::view_selection_skill_provider
// Add an optionalproperty.
+ def->topic(this->textToSpeech, "TextToSpeech");
+
return def;
}
@@ -112,8 +114,10 @@ namespace armarx::view_selection::components::view_selection_skill_provider
skills::LookForObjectWithKinematicUnit::Services srv{
.kinematicUnit = getRobotUnit()->getKinematicUnit(),
- .objectPoseClient = getClient()
+ .objectPoseClient = getClient(),
+ .textToSpeech = this->textToSpeech
};
+
skills_.lookForObjectWithKinematicUnit->connect(srv);
}
diff --git a/source/armarx/view_selection/components/view_selection_skill_provider/Component.h b/source/armarx/view_selection/components/view_selection_skill_provider/Component.h
index 5f650a4..a9e3805 100644
--- a/source/armarx/view_selection/components/view_selection_skill_provider/Component.h
+++ b/source/armarx/view_selection/components/view_selection_skill_provider/Component.h
@@ -34,6 +34,8 @@
#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
#include <RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h>
+#include <RobotAPI/interface/speech/SpeechInterface.h>
+
// #include <armarx/view_selection/components/view_selection_skill_provider/ComponentInterface.h>
namespace armarx::view_selection::components::view_selection_skill_provider
@@ -120,6 +122,9 @@ namespace armarx::view_selection::components::view_selection_skill_provider
{
};
Properties properties;
+
+
+ armarx::TextListenerInterfacePrx textToSpeech;
/* Use a mutex if you access variables from different threads
* (e.g. ice functions and RemoteGui_update()).
std::mutex propertiesMutex;
diff --git a/source/armarx/view_selection/skills/LookForObjectWithKinematicUnit.cpp b/source/armarx/view_selection/skills/LookForObjectWithKinematicUnit.cpp
index 7d83cca..4fa6f4f 100644
--- a/source/armarx/view_selection/skills/LookForObjectWithKinematicUnit.cpp
+++ b/source/armarx/view_selection/skills/LookForObjectWithKinematicUnit.cpp
@@ -1,5 +1,6 @@
#include "LookForObjectWithKinematicUnit.h"
#include "RobotAPI/libraries/ArmarXObjects/ObjectFinder.h"
+#include "RobotAPI/libraries/ArmarXObjects/ObjectInfo.h"
#include "RobotAPI/libraries/ArmarXObjects/aron_conversions.h"
#include "RobotAPI/libraries/aron/core/aron_conversions.h"
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
@@ -10,14 +11,17 @@
#include <Eigen/Geometry>
#include <chrono>
+#include <optional>
#include <thread>
+#include <vector>
namespace armarx::view_selection::skills
{
+
std::map<std::string, float> neck_joint_angles = {
- {"Neck_1_Yaw", 0.0}, {"Neck_2_Pitch", 0.3}};
+ {"Neck_1_Yaw", 0.0}, {"Neck_2_Pitch", 0.3}};
VirtualRobot::RobotPtr localRobot;
-
+
LookForObjectWithKinematicUnit::LookForObjectWithKinematicUnit() : Base(DefaultSkillDescription())
{
}
@@ -34,42 +38,24 @@ namespace armarx::view_selection::skills
return ::armarx::skills::Skill::InitResult{
.status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
}
-
- // std::optional<objpose::ObjectPose>
- // findObjectByClass(const objpose::ObjectPoseSeq& objectPoses, const armarx::ObjectID& objectID)
- // {
- // const auto matchesId = [&objectID](const objpose::ObjectPose& objectPose) -> bool
- // { return objectPose.objectID.equalClass(objectID); };
-
- // const auto it = std::find_if(objectPoses.begin(), objectPoses.end(), matchesId);
- // if (it != objectPoses.end())
- // {
- // return *it;
- // }
-
- // return std::nullopt;
- // }
+
void
LookForObjectWithKinematicUnit::setNeckJointAngles(float goal_yaw, float goal_pitch, int waypoints_num)
{
float yaw_step_size = (goal_yaw - neck_joint_angles["Neck_1_Yaw"]) / waypoints_num;
float pitch_step_size = (goal_pitch - neck_joint_angles["Neck_2_Pitch"]) / waypoints_num;
+
for (int i = 1; i < waypoints_num; i++)
{
neck_joint_angles["Neck_1_Yaw"] = neck_joint_angles["Neck_1_Yaw"] + yaw_step_size;
neck_joint_angles["Neck_2_Pitch"] = neck_joint_angles["Neck_2_Pitch"] + pitch_step_size;
srv_->kinematicUnit->setJointAngles({
{"Neck_1_Yaw", neck_joint_angles["Neck_1_Yaw"]}, {"Neck_2_Pitch", neck_joint_angles["Neck_2_Pitch"]}});
- std::this_thread::sleep_for(std::chrono::milliseconds(200));
- // while (localRobot->getRobotNode("Neck_1_Yaw")->getJointValue() != neck_joint_angles["Neck_1_Yaw"]) {
- // srv_->kinematicUnit->setJointAngles({
- // {"Neck_1_Yaw", neck_joint_angles["Neck_1_Yaw"]}, {"Neck_2_Pitch", neck_joint_angles["Neck_2_Pitch"]}});
-
- // armarx::Clock::WaitFor(armarx::Duration::MilliSeconds(200));
- // }
+ std::this_thread::sleep_for(std::chrono::milliseconds(300));
+
}
- std::this_thread::sleep_for(std::chrono::milliseconds(200));
- //armarx::Clock::WaitFor(armarx::Duration::MilliSeconds(200));
+ std::this_thread::sleep_for(std::chrono::milliseconds(300));
+
}
void
LookForObjectWithKinematicUnit::speak(const std::string& text)
@@ -81,40 +67,26 @@ namespace armarx::view_selection::skills
::armarx::skills::Skill::MainResult
LookForObjectWithKinematicUnit::main(const Base::SpecializedMainInput& in)
{
- // TODO(Nagi): implement
- // objpose::ObjectPoseSeq objectPoses = srv_->objectPoseClient.fetchObjectPoses();
-
- // armarx::ObjectID objectId;
- // fromAron(in.params.objectID, objectId);
- // std::vector<std::string> predefined_poses = {"r003-door", "r003-pillar", "clean-box", "dirty-box", "beam", "hund-table-120x80",
- // "hund-table-180x60", "hund-table-180x80", "hund-table-200x80", "mobile-dishwasher",
- // "mobile-fridge", "mobile-kitchen-counter", "guard-support-full-with-light", "ladder-closed",
- // "mounting-adhesive", "spraybottle", "workbench"};
- // std::vector<std::string> all_objects_poses;
- // for (auto o : objectPoses) {
- // all_objects_poses.push_back(o.objectID.className());
- // }
- // std::vector<std::string> objects_names;
- // for (auto const& object : all_objects_poses) {
-
- // if (std::find(predefined_poses.begin(), predefined_poses.end(), object) == predefined_poses.end() &&
- // std::find(objects_names.begin(), objects_names.end(), object) == objects_names.end()) {
- // objects_names.push_back(object);
- // }
- // }
- // std::optional<objpose::ObjectPose> objectPose = findObjectByClass(objectPoses, objectId);
-
- armarx::RemoteRobot::synchronizeLocalClone(localRobot, srv_->robotStateComponent);
- srv_->kinematicUnit->switchControlMode({{"Neck_1_Yaw", armarx::ControlMode::ePositionControl},{"Neck_2_Pitch", armarx::ControlMode::ePositionControl} });
+
+
+ srv_->kinematicUnit->switchControlMode({{"Neck_1_Yaw", armarx::ControlMode::ePositionControl},{"Neck_2_Pitch", armarx::ControlMode::ePositionControl}, {"TorsoJoint", armarx::ControlMode::ePositionControl} });
+ srv_->kinematicUnit->setJointAngles({
+ {"TorsoJoint", -109.76679992675781}});
+ std::this_thread::sleep_for(std::chrono::milliseconds(300));
+
ARMARX_INFO << "start the running";
// doing the neck movement routine
// move neck to start position
setNeckJointAngles(0.0, 0.2, 10);
ARMARX_INFO << "finished first movement'";
// move the neck to the max right
- setNeckJointAngles(-0.593412, 0.7609636, 10);
+ setNeckJointAngles(-0.593412, 0.66654124, 10);
+ std::this_thread::sleep_for(std::chrono::milliseconds(450));
// move the neck to the max left
- setNeckJointAngles(0.593412, 0.7609636, 20);
+ setNeckJointAngles(0.0, 0.66654124, 10);
+ std::this_thread::sleep_for(std::chrono::milliseconds(450));
+ setNeckJointAngles(0.593412, 0.66654124, 10);
+ std::this_thread::sleep_for(std::chrono::milliseconds(450));
// move the neck back to the normal
setNeckJointAngles(0.0, 0.2, 10);
ARMARX_INFO << "no problem with the neck";
@@ -126,22 +98,41 @@ namespace armarx::view_selection::skills
std::vector<std::string> predefined_poses = {"r003-door", "r003-pillar", "clean-box", "dirty-box", "beam", "hund-table-120x80",
"hund-table-180x60", "hund-table-180x80", "hund-table-200x80", "mobile-dishwasher",
"mobile-fridge", "mobile-kitchen-counter", "guard-support-full-with-light", "ladder-closed",
- "mounting-adhesive", "spraybottle", "workbench"};
- std::vector<std::string> all_objects_poses;
- for (auto o : objectPoses) {
- all_objects_poses.push_back(o.objectID.className());
- }
+ "mounting-adhesive", "spraybottle", "workbench", "box"};
+
std::vector<std::string> objects_names;
- for (auto const& object : all_objects_poses) {
-
- if (std::find(predefined_poses.begin(), predefined_poses.end(), object) == predefined_poses.end() &&
- std::find(objects_names.begin(), objects_names.end(), object) == objects_names.end()) {
- objects_names.push_back(object);
+
+ armarx::ObjectFinder objFind;
+
+ for (auto o : objectPoses) {
+
+ const std::string className = o.objectID.className();
+
+ if (std::find(predefined_poses.begin(), predefined_poses.end(), className) == predefined_poses.end() &&
+ std::find(objects_names.begin(), objects_names.end(), className) == objects_names.end())
+ {
+ std::string objectName;
+ std::optional<armarx::ObjectInfo> info = objFind.findObject(o.objectID);
+ if (info)
+ {
+ std::optional<std::vector<std::string>> names = info->loadSpokenNames();
+ if (names.has_value() and not names->empty())
+ {
+ objectName = names->front();
+ }
+ }
+
+ if (objectName.empty())
+ {
+ objectName = className;
+ }
+
+ objects_names.push_back(objectName);
+
}
}
ARMARX_INFO << "no problem with the poses";
-
for (int i = 0; i < static_cast<int>(objects_names.size()); i++) {
if (i == 0) {
speak("I can see the following objects");
diff --git a/source/armarx/view_selection/skills/ScanLocation.cpp b/source/armarx/view_selection/skills/ScanLocation.cpp
index d10e88d..1e36d9a 100644
--- a/source/armarx/view_selection/skills/ScanLocation.cpp
+++ b/source/armarx/view_selection/skills/ScanLocation.cpp
@@ -13,9 +13,9 @@
namespace armarx::view_selection::skills
{
- std::map<std::string, float> neck_joint_angles = {
- {"Neck_1_Yaw", 0.0}, {"Neck_2_Pitch", 0.3}};
- VirtualRobot::RobotPtr localRobot;
+ // std::map<std::string, float> neck_joint_angles = {
+ // {"Neck_1_Yaw", 0.0}, {"Neck_2_Pitch", 0.3}};
+ // VirtualRobot::RobotPtr localRobot;
ScanLocation::ScanLocation() : Base(DefaultSkillDescription())
{
}
@@ -33,81 +33,89 @@ namespace armarx::view_selection::skills
.status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
}
- void
- ScanLocation::setNeckJointAngles(float goal_yaw, float goal_pitch, int waypoints_num)
- {
- float yaw_step_size = (goal_yaw - neck_joint_angles["Neck_1_Yaw"]) / waypoints_num;
- float pitch_step_size = (goal_pitch - neck_joint_angles["Neck_2_Pitch"]) / waypoints_num;
- for (int i = 1; i < waypoints_num; i++)
- {
- neck_joint_angles["Neck_1_Yaw"] = neck_joint_angles["Neck_1_Yaw"] + yaw_step_size;
- neck_joint_angles["Neck_2_Pitch"] = neck_joint_angles["Neck_2_Pitch"] + pitch_step_size;
- srv_->kinematicUnit->setJointAngles({
- {"Neck_1_Yaw", neck_joint_angles["Neck_1_Yaw"]}, {"Neck_2_Pitch", neck_joint_angles["Neck_2_Pitch"]}});
- std::this_thread::sleep_for(std::chrono::milliseconds(200));
- }
- std::this_thread::sleep_for(std::chrono::milliseconds(200));
- }
- void
- ScanLocation::speak(const std::string& text)
- {
- ARMARX_INFO << "Saying '" << text.substr(0, 50) << "...'";
- srv_->textToSpeech->reportText(text);
- }
+ // void
+ // ScanLocation::setNeckJointAngles(float goal_yaw, float goal_pitch, int waypoints_num)
+ // {
+ // float yaw_step_size = (goal_yaw - neck_joint_angles["Neck_1_Yaw"]) / waypoints_num;
+ // float pitch_step_size = (goal_pitch - neck_joint_angles["Neck_2_Pitch"]) / waypoints_num;
+ // for (int i = 1; i < waypoints_num; i++)
+ // {
+ // neck_joint_angles["Neck_1_Yaw"] = neck_joint_angles["Neck_1_Yaw"] + yaw_step_size;
+ // neck_joint_angles["Neck_2_Pitch"] = neck_joint_angles["Neck_2_Pitch"] + pitch_step_size;
+ // srv_->kinematicUnit->setJointAngles({
+ // {"Neck_1_Yaw", neck_joint_angles["Neck_1_Yaw"]}, {"Neck_2_Pitch", neck_joint_angles["Neck_2_Pitch"]}});
+ // std::this_thread::sleep_for(std::chrono::milliseconds(200));
+ // }
+ // std::this_thread::sleep_for(std::chrono::milliseconds(200));
+ // }
+ // void
+ // ScanLocation::speak(const std::string& text)
+ // {
+ // ARMARX_INFO << "Saying '" << text.substr(0, 50) << "...'";
+ // srv_->textToSpeech->reportText(text);
+ // }
::armarx::skills::Skill::MainResult
ScanLocation::main(const Base::SpecializedMainInput& in)
{
- armarx::RemoteRobot::synchronizeLocalClone(localRobot, srv_->robotStateComponent);
- srv_->kinematicUnit->switchControlMode({{"Neck_1_Yaw", armarx::ControlMode::ePositionControl},{"Neck_2_Pitch", armarx::ControlMode::ePositionControl} });
- ARMARX_INFO << "start the running";
- // doing the neck movement routine
- // move neck to start position
- setNeckJointAngles(0.0, 0.2, 10);
- ARMARX_INFO << "finished first movement'";
- // move the neck to the max right
- setNeckJointAngles(-0.593412, 0.7609636, 10);
- // move the neck to the max left
- setNeckJointAngles(0.593412, 0.7609636, 20);
- // move the neck back to the normal
- setNeckJointAngles(0.0, 0.2, 10);
- ARMARX_INFO << "no problem with the neck";
-
- // getting the
- objpose::ObjectPoseSeq objectPoses = srv_->objectPoseClient.fetchObjectPoses();
- // these objects are always in the memory so, we need to not consider them for what can u see now scenario
- armarx::ObjectID objectId;
- std::vector<std::string> predefined_poses = {"r003-door", "r003-pillar", "clean-box", "dirty-box", "beam", "hund-table-120x80",
- "hund-table-180x60", "hund-table-180x80", "hund-table-200x80", "mobile-dishwasher",
- "mobile-fridge", "mobile-kitchen-counter", "guard-support-full-with-light", "ladder-closed",
- "mounting-adhesive", "spraybottle", "workbench"};
- std::vector<std::string> all_objects_poses;
- for (auto o : objectPoses) {
- all_objects_poses.push_back(o.objectID.className());
- }
- std::vector<std::string> objects_names;
- for (auto const& object : all_objects_poses) {
+ // armarx::RemoteRobot::synchronizeLocalClone(localRobot, srv_->robotStateComponent);
+ // srv_->kinematicUnit->switchControlMode({{"Neck_1_Yaw", armarx::ControlMode::ePositionControl},{"Neck_2_Pitch", armarx::ControlMode::ePositionControl} });
+ // ARMARX_INFO << "start the running";
+ // // doing the neck movement routine
+ // // move neck to start position
+ // setNeckJointAngles(0.0, 0.2, 10);
+ // ARMARX_INFO << "finished first movement'";
+ // // move the neck to the max right
+ // setNeckJointAngles(-0.593412, 0.7609636, 10);
+ // // move the neck to the max left
+ // setNeckJointAngles(0.593412, 0.7609636, 20);
+ // // move the neck back to the normal
+ // setNeckJointAngles(0.0, 0.2, 10);
+ // ARMARX_INFO << "no problem with the neck";
+
+ // // getting the
+ // objpose::ObjectPoseSeq objectPoses = srv_->objectPoseClient.fetchObjectPoses();
+ // // these objects are always in the memory so, we need to not consider them for what can u see now scenario
+ // armarx::ObjectID objectId;
+ // std::vector<std::string> predefined_poses = {"r003-door", "r003-pillar", "clean-box", "dirty-box", "beam", "hund-table-120x80",
+ // "hund-table-180x60", "hund-table-180x80", "hund-table-200x80", "mobile-dishwasher",
+ // "mobile-fridge", "mobile-kitchen-counter", "guard-support-full-with-light", "ladder-closed",
+ // "mounting-adhesive", "spraybottle", "workbench"};
+ // std::vector<std::string> all_objects_poses;
+ // for (auto o : objectPoses) {
+ // all_objects_poses.push_back(o.objectID.className());
+ // }
+ // std::vector<std::string> objects_names;
+ // for (auto const& object : all_objects_poses) {
- if (std::find(predefined_poses.begin(), predefined_poses.end(), object) == predefined_poses.end() &&
- std::find(objects_names.begin(), objects_names.end(), object) == objects_names.end()) {
- objects_names.push_back(object);
- }
- }
-
- ARMARX_INFO << "no problem with the poses";
-
- for (int i = 0; i < static_cast<int>(objects_names.size()); i++) {
- if (i == 0) {
- speak("I can see the following objects");
- std::this_thread::sleep_for(std::chrono::milliseconds(200));
- } else if (i == (static_cast<int>(objects_names.size()) - 1)) {
- speak("and");
- std::this_thread::sleep_for(std::chrono::milliseconds(200));
- }
- speak(objects_names[i]);
- std::this_thread::sleep_for(std::chrono::milliseconds(200));
- }
+ // if (std::find(predefined_poses.begin(), predefined_poses.end(), object) == predefined_poses.end() &&
+ // std::find(objects_names.begin(), objects_names.end(), object) == objects_names.end()) {
+ // objects_names.push_back(object);
+ // }
+ // }
+
+ // ARMARX_INFO << "no problem with the poses";
+
+ // for (int i = 0; i < static_cast<int>(objects_names.size()); i++) {
+ // if (i == 0) {
+ // if (static_cast<int>(objects_names.size()) == 1) {
+ // speak("I can see the following object");
+ // std::this_thread::sleep_for(std::chrono::milliseconds(200));
+ // }
+ // else{
+
+ // speak("I can see the following objects");
+ // std::this_thread::sleep_for(std::chrono::milliseconds(200));
+ // }
+
+ // } else if (i == (static_cast<int>(objects_names.size()) - 1)) {
+ // speak("and");
+ // std::this_thread::sleep_for(std::chrono::milliseconds(200));
+ // }
+ // speak(objects_names[i]);
+ // std::this_thread::sleep_for(std::chrono::milliseconds(200));
+ // }
// ScanLocation::setJointAngles();
return ::armarx::skills::Skill::MainResult{
diff --git a/source/armarx/view_selection/skills/ScanLocation.h b/source/armarx/view_selection/skills/ScanLocation.h
index c5df8af..643b735 100644
--- a/source/armarx/view_selection/skills/ScanLocation.h
+++ b/source/armarx/view_selection/skills/ScanLocation.h
@@ -48,11 +48,11 @@ namespace armarx::view_selection::skills
{
KinematicUnitInterfacePrx kinematicUnit;
- objpose::ObjectPoseClient objectPoseClient;
+ // objpose::ObjectPoseClient objectPoseClient;
- armarx::TextListenerInterfacePrx textToSpeech;
+ // armarx::TextListenerInterfacePrx textToSpeech;
- armarx::RobotStateComponentInterfacePrx robotStateComponent;
+ // armarx::RobotStateComponentInterfacePrx robotStateComponent;
// armem::client::MemoryNameSystem& memoryNameSystem;
};
@@ -81,10 +81,10 @@ namespace armarx::view_selection::skills
}
void
setJointAngles();
- void
- setNeckJointAngles(float goal_yaw, float goal_pitch, int waypoints_num);
- void
- speak(const std::string& text);
+ // void
+ // setNeckJointAngles(float goal_yaw, float goal_pitch, int waypoints_num);
+ // void
+ // speak(const std::string& text);
protected:
private:
--
GitLab