add workspace base functional

Workspace functional includes basic read from workspace and trajectory calculation for inspection
This commit is contained in:
Ilya Uraev 2023-12-30 23:39:43 +03:00
parent e5e8110697
commit 192887ab8b
7 changed files with 240 additions and 101 deletions

View file

@ -11,6 +11,6 @@ public:
EnvInterface() = default; EnvInterface() = default;
virtual ~EnvInterface() = default; virtual ~EnvInterface() = default;
protected: protected:
std::shared_ptr<std::vector<rbs_utils::EnvModels>> m_env_models; std::shared_ptr<std::vector<rbs_utils::EnvModel>> m_env_models;
}; };
} // namespace env_interface } // namespace env_interface

View file

@ -1,37 +1,37 @@
#include "rclcpp/rclcpp.hpp" #include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp"
// #include "ros_gz_bridge/convert.hpp" // #include "ros_gz_bridge/convert.hpp"
#include "env_interface/env_interface.hpp"
#include "gz/msgs/pose_v.pb.h"
#include <geometry_msgs/msg/transform_stamped.hpp>
#include "geometry_msgs/msg/pose_array.hpp"
#include <gz/transport/Node.hh>
#include <memory> #include <memory>
#include <mutex> #include <mutex>
#include <string> #include <string>
#include <tf2_msgs/msg/detail/tf_message__struct.hpp> #include <tf2_msgs/msg/detail/tf_message__struct.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_msgs/msg/tf_message.hpp> #include <tf2_msgs/msg/tf_message.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp> #include <tf2_ros/transform_broadcaster.h>
#include <gz/transport/Node.hh>
#include "gz/msgs/pose_v.pb.h"
#include "env_interface/env_interface.hpp"
#include <gz/sim/components/Model.hh>
#include <gz/sim/Entity.hh> #include <gz/sim/Entity.hh>
#include <gz/sim/EntityComponentManager.hh> #include <gz/sim/EntityComponentManager.hh>
#include <gz/sim/components/Model.hh>
namespace gz_enviroment namespace gz_enviroment {
{ using CallbackReturn =
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
class GzEnviroment : public env_interface::EnvInterface class GzEnviroment : public env_interface::EnvInterface {
{
public: public:
GzEnviroment(); GzEnviroment();
CallbackReturn on_init() override; CallbackReturn on_init() override;
CallbackReturn on_configure(const rclcpp_lifecycle::State&) override; CallbackReturn on_configure(const rclcpp_lifecycle::State &) override;
CallbackReturn on_activate(const rclcpp_lifecycle::State&) override; CallbackReturn on_activate(const rclcpp_lifecycle::State &) override;
CallbackReturn on_deactivate(const rclcpp_lifecycle::State&) override; CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override;
CallbackReturn on_cleanup(const rclcpp_lifecycle::State&) override; CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) override;
protected: protected:
void onGzPoseSub(const gz::msgs::Pose_V& pose_v); void onGzPoseSub(const gz::msgs::Pose_V &pose_v);
bool doGzSpawn(); bool doGzSpawn();
@ -47,9 +47,11 @@ private:
tf2_msgs::msg::TFMessage m_transforms; tf2_msgs::msg::TFMessage m_transforms;
tf2_msgs::msg::TFMessage m_target_places; tf2_msgs::msg::TFMessage m_target_places;
std::string getGzWorldName(); std::string getGzWorldName();
std::string createGzModelString(const std::vector<double>& pose, const std::vector<double>& inertia, const double &mass, const std::string& mesh_filepath, std::string createGzModelString(const std::vector<double> &pose,
const std::string& name); const std::vector<double> &inertia,
const double &mass,
const std::string &mesh_filepath,
const std::string &name);
}; };
} // namespace gz_enviroment } // namespace gz_enviroment

View file

@ -4,6 +4,7 @@
#include "ros_gz_bridge/convert.hpp" #include "ros_gz_bridge/convert.hpp"
#include <memory> #include <memory>
#include <rclcpp/logging.hpp> #include <rclcpp/logging.hpp>
#include <rclcpp_lifecycle/lifecycle_node.hpp>
#include <ros_gz_interfaces/msg/detail/entity__builder.hpp> #include <ros_gz_interfaces/msg/detail/entity__builder.hpp>
#include <gz/sim/components.hh> #include <gz/sim/components.hh>
@ -25,7 +26,8 @@ CallbackReturn GzEnviroment::on_init() {
for (const auto entity : modelEntities) { for (const auto entity : modelEntities) {
const auto modelName = ecm.Component<gz::sim::components::Name>(entity); const auto modelName = ecm.Component<gz::sim::components::Name>(entity);
if (modelName) { if (modelName) {
RCLCPP_INFO_STREAM(getNode()->get_logger(), "Model Name: " << modelName->Data()); RCLCPP_INFO_STREAM(getNode()->get_logger(),
"Model Name: " << modelName->Data());
} }
} }
return CallbackReturn::SUCCESS; return CallbackReturn::SUCCESS;
@ -38,11 +40,13 @@ CallbackReturn GzEnviroment::on_configure(const rclcpp_lifecycle::State &) {
m_world_name = getGzWorldName(); m_world_name = getGzWorldName();
m_topic_name = std::string("/world/") + m_world_name + "/dynamic_pose/info"; m_topic_name = std::string("/world/") + m_world_name + "/dynamic_pose/info";
m_service_spawn = std::string("/world/") + m_world_name + "/create"; m_service_spawn = std::string("/world/") + m_world_name + "/create";
m_config_loader = m_config_loader = std::make_shared<rbs_utils::AssemblyConfigLoader>(
std::make_shared<rbs_utils::AssemblyConfigLoader>("asp-example2"); "asp-example2", getNode());
m_follow_frames = m_config_loader->getSceneModelNames(); m_follow_frames = m_config_loader->getSceneModelNames();
// m_target_places = std::make_shared<tf2_msgs::msg::TFMessage>(); // m_target_places = std::make_shared<tf2_msgs::msg::TFMessage>();
m_transforms = m_config_loader->getTfData("bishop"); m_transforms = m_config_loader->getTfData("bishop");
// TODO add workspace viewer in Rviz
// m_config_loader->printWorkspace();
// if (!doGzSpawn()) // if (!doGzSpawn())
// { // {
@ -67,11 +71,7 @@ CallbackReturn GzEnviroment::on_activate(const rclcpp_lifecycle::State &) {
CallbackReturn GzEnviroment::on_cleanup(const rclcpp_lifecycle::State &) { CallbackReturn GzEnviroment::on_cleanup(const rclcpp_lifecycle::State &) {
RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_cleanup"); RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_cleanup");
// Clear tf2 broadcasters // Clear variables on_cleanup
// m_tf2_broadcaster.reset();
// m_tf2_broadcaster_target.reset();
// Clear other variables
m_gz_node.reset(); m_gz_node.reset();
m_follow_frames.clear(); m_follow_frames.clear();
m_topic_name.clear(); m_topic_name.clear();
@ -86,10 +86,7 @@ CallbackReturn GzEnviroment::on_cleanup(const rclcpp_lifecycle::State &) {
CallbackReturn GzEnviroment::on_deactivate(const rclcpp_lifecycle::State &) { CallbackReturn GzEnviroment::on_deactivate(const rclcpp_lifecycle::State &) {
RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_deactivate"); RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_deactivate");
// m_tf2_broadcaster.reset(); // What we should use here?
// m_tf2_broadcaster_target.reset();
// m_gz_node.reset();
return CallbackReturn::SUCCESS; return CallbackReturn::SUCCESS;
} }
@ -133,7 +130,6 @@ void GzEnviroment::onGzPoseSub(const gz::msgs::Pose_V &pose_v) {
} }
} }
std::string GzEnviroment::getGzWorldName() { std::string GzEnviroment::getGzWorldName() {
bool executed{false}; bool executed{false};
bool result{false}; bool result{false};
@ -212,4 +208,4 @@ std::string GzEnviroment::createGzModelString(
#include "pluginlib/class_list_macros.hpp" #include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(gz_enviroment::GzEnviroment, PLUGINLIB_EXPORT_CLASS(gz_enviroment::GzEnviroment,
env_interface::EnvInterface); env_interface::EnvInterface);

View file

@ -8,12 +8,22 @@ endif()
# find dependencies # find dependencies
find_package(ament_cmake REQUIRED) find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED) find_package(rclcpp REQUIRED)
find_package(rclcpp_lifecycle REQUIRED)
find_package(tf2_ros REQUIRED) find_package(tf2_ros REQUIRED)
find_package(tf2_eigen REQUIRED) find_package(tf2_eigen REQUIRED)
find_package(rviz_visual_tools REQUIRED)
find_package(geometry_msgs REQUIRED)
set(deps
rclcpp
rclcpp_lifecycle
tf2_ros
tf2_eigen
rviz_visual_tools
geometry_msgs
)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
add_library(${PROJECT_NAME} SHARED src/rbs_utils.cpp) add_library(${PROJECT_NAME} SHARED src/rbs_utils.cpp)
@ -22,7 +32,7 @@ install(
DESTINATION include DESTINATION include
) )
ament_target_dependencies(${PROJECT_NAME} PUBLIC rclcpp tf2_ros tf2_eigen) ament_target_dependencies(${PROJECT_NAME} PUBLIC ${deps})
# target_include_directories(asm_folder_process # target_include_directories(asm_folder_process
# PUBLIC # PUBLIC
@ -57,4 +67,7 @@ if(BUILD_TESTING)
endif() endif()
ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
ament_export_dependencies(
${deps}
)
ament_package() ament_package()

View file

@ -1,24 +1,25 @@
#include <Eigen/Core>
#include "geometry_msgs/msg/pose.hpp"
#include "rclcpp/rclcpp.hpp" #include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "rviz_visual_tools/rviz_visual_tools.hpp"
#include "tf2_msgs/msg/tf_message.hpp" #include "tf2_msgs/msg/tf_message.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2_ros/buffer.h" #include "tf2_ros/buffer.h"
#include <tf2/convert.h>
#include "tf2_ros/static_transform_broadcaster.h" #include "tf2_ros/static_transform_broadcaster.h"
#include <fstream> #include <fstream>
#include <memory>
#include <nlohmann/json.hpp> #include <nlohmann/json.hpp>
#include <rclcpp/logger.hpp>
#include <rclcpp/node.hpp> #include <rclcpp/node.hpp>
#include <string>
#include <tf2/LinearMath/Vector3.h>
#include <tf2_eigen/tf2_eigen.hpp> #include <tf2_eigen/tf2_eigen.hpp>
#include <tf2_msgs/msg/detail/tf_message__struct.hpp>
#include <unordered_map> #include <unordered_map>
#include <vector>
const std::string env_dir = std::getenv("RBS_ASSEMBLY_DIR"); const std::string env_dir = std::getenv("RBS_ASSEMBLY_DIR");
namespace rbs_utils { namespace rbs_utils {
struct EnvModels { struct EnvModel {
std::string model_name; std::string model_name;
std::string mesh_path; std::string mesh_path;
std::vector<double> model_inertia; std::vector<double> model_inertia;
@ -26,10 +27,23 @@ struct EnvModels {
double mass; double mass;
}; };
class AssemblyConfigLoader { struct Workspace {
std::string name;
geometry_msgs::msg::PoseArray poses;
};
class AssemblyConfigLoader {
public: public:
AssemblyConfigLoader(const std::string &t_assembly_dir); template <typename NodePtr>
explicit AssemblyConfigLoader(const std::string &t_assembly_dir,
const NodePtr &t_node)
: AssemblyConfigLoader(t_assembly_dir,
t_node->get_node_logging_interface()) {}
explicit AssemblyConfigLoader(
const std::string &t_assembly_dir,
const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr
&t_logger_node_interface);
inline std::shared_ptr<std::unordered_map<std::string, nlohmann::json>> inline std::shared_ptr<std::unordered_map<std::string, nlohmann::json>>
getAssemblyFileData() { getAssemblyFileData() {
@ -37,31 +51,40 @@ public:
} }
tf2_msgs::msg::TFMessage getTfData(const std::string &model_name); tf2_msgs::msg::TFMessage getTfData(const std::string &model_name);
inline std::shared_ptr<std::vector<EnvModels>> getEnvModels() { inline std::shared_ptr<std::vector<EnvModel>> getEnvModels() {
return m_env_models; return m_env_models;
} }
std::vector<std::string> getSceneModelNames(); std::vector<std::string> getSceneModelNames();
void printWorkspace();
geometry_msgs::msg::PoseArray getWorkspaceInspectorTrajectory();
geometry_msgs::msg::Pose
transformTrajectory(const geometry_msgs::msg::TransformStamped &pose);
private: private:
std::shared_ptr<std::unordered_map<std::string, nlohmann::json>> m_env_vars; std::shared_ptr<std::unordered_map<std::string, nlohmann::json>> m_env_vars;
std::vector<std::string> m_env_files; std::vector<std::string> m_env_files;
std::vector<std::ifstream> m_env_paths; std::vector<std::ifstream> m_env_paths;
std::shared_ptr<std::vector<EnvModels>> m_env_models{}; std::shared_ptr<std::vector<EnvModel>> m_env_models{};
std::string m_assembly_dir; std::string m_assembly_dir;
std::unique_ptr<tf2_ros::Buffer> m_tf_buffer; std::unique_ptr<tf2_ros::Buffer> m_tf_buffer;
// rviz_visual_tools::RvizVisualToolsPtr m_visual_tools;
rclcpp::Logger m_logger;
void readAssemblyFileData(const std::string &filename, void readAssemblyFileData(const std::string &filename,
const std::string &filepath); const std::string &filepath);
tf2_msgs::msg::TFMessage readWorkspaceJson(const nlohmann::json &json);
tf2_msgs::msg::TFMessage parseJsonToTFMessage(const nlohmann::json &json, tf2_msgs::msg::TFMessage parseJsonToTFMessage(const nlohmann::json &json,
const std::string &model_name); const std::string &model_name);
void publishWorkspace(const tf2_msgs::msg::TFMessage &tf_msg);
void setTfFromDb(const std::string &filename); void setTfFromDb(const std::string &filename);
double convertToDouble(const nlohmann::json &value); double convertToDouble(const nlohmann::json &value);
void readModelConfigs(); void readModelConfigs();
EnvModels parseModelData(const std::string &jsonFilePath); EnvModel parseModelData(const std::string &jsonFilePath);
}; };
class StaticFramePublisher : public rclcpp::Node { class StaticFramePublisher : public rclcpp::Node {

View file

@ -8,7 +8,7 @@
<license>Apache-2.0</license> <license>Apache-2.0</license>
<buildtool_depend>ament_cmake</buildtool_depend> <buildtool_depend>ament_cmake</buildtool_depend>
<depend>rviz_visual_tools</depend>
<test_depend>ament_lint_auto</test_depend> <test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend> <test_depend>ament_lint_common</test_depend>

View file

@ -1,26 +1,24 @@
#include "rbs_utils/rbs_utils.hpp" #include "rbs_utils/rbs_utils.hpp"
#include <fstream> #include <geometry_msgs/msg/detail/pose__struct.hpp>
#include <iostream> #include <tf2/LinearMath/Transform.h>
#include <iterator> #include <tf2/convert.h>
#include <memory> #include <tf2_eigen/tf2_eigen.hpp>
#include <nlohmann/json_fwd.hpp> #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <ostream> #include <tf2_ros/buffer_interface.h>
#include <rclcpp/clock.hpp>
#include <rclcpp/logger.hpp>
#include <rclcpp/logging.hpp>
#include <string>
#include <tf2_msgs/msg/detail/tf_message__struct.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <unordered_map>
#include <vector>
namespace rbs_utils { namespace rbs_utils {
AssemblyConfigLoader::AssemblyConfigLoader(
AssemblyConfigLoader::AssemblyConfigLoader(const std::string &t_assembly_dir) const std::string &t_assembly_dir,
const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr
&t_logging_interface)
: m_env_vars( : m_env_vars(
std::make_shared<std::unordered_map<std::string, nlohmann::json>>()), std::make_shared<std::unordered_map<std::string, nlohmann::json>>()),
m_env_models(std::make_shared<std::vector<EnvModels>>()), m_env_models(std::make_shared<std::vector<EnvModel>>()),
m_assembly_dir(t_assembly_dir) { m_assembly_dir(t_assembly_dir),
m_logger(t_logging_interface->get_logger()) {
// m_visual_tools.reset(new
// rviz_visual_tools::RvizVisualTools("world","/rviz_visual_markers",
// m_node));
if (!m_assembly_dir.empty()) { if (!m_assembly_dir.empty()) {
std::vector<std::string> filenames = {"robossembler_db", "sequences"}; std::vector<std::string> filenames = {"robossembler_db", "sequences"};
for (auto &filename : filenames) { for (auto &filename : filenames) {
@ -31,9 +29,8 @@ AssemblyConfigLoader::AssemblyConfigLoader(const std::string &t_assembly_dir)
readAssemblyFileData(filename, filepath); readAssemblyFileData(filename, filepath);
} }
readModelConfigs(); readModelConfigs();
} else { } else {
RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"), "Assembly dir is not set"); RCLCPP_ERROR(m_logger, "Assembly dir is not set");
} }
} }
@ -42,44 +39,39 @@ void AssemblyConfigLoader::readAssemblyFileData(const std::string &filename,
try { try {
std::ifstream i(filepath); std::ifstream i(filepath);
if (!i.is_open()) { if (!i.is_open()) {
RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"), "Failed to open file: %s", RCLCPP_ERROR(m_logger, "Failed to open file: %s", filepath.c_str());
filepath.c_str());
return; return;
} }
nlohmann::json json = nlohmann::json::parse(i); nlohmann::json json = nlohmann::json::parse(i);
m_env_vars->insert({filename, json}); m_env_vars->insert({filename, json});
} catch (const nlohmann::json::parse_error &e) { } catch (const nlohmann::json::parse_error &e) {
RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"), RCLCPP_ERROR(m_logger, "Error parsing JSON in file %s: %s",
"Error parsing JSON in file %s: %s", filepath.c_str(), filepath.c_str(), e.what());
e.what());
} catch (const std::exception &e) { } catch (const std::exception &e) {
RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"), RCLCPP_ERROR(m_logger, "Exception reading file %s: %s", filepath.c_str(),
"Exception reading file %s: %s", filepath.c_str(), e.what()); e.what());
} }
} }
EnvModels EnvModel
AssemblyConfigLoader::parseModelData(const std::string &json_file_path) { AssemblyConfigLoader::parseModelData(const std::string &json_file_path) {
nlohmann::json json_content{}; nlohmann::json json_content{};
try { try {
std::ifstream file(json_file_path); std::ifstream file(json_file_path);
if (!file.is_open()) { if (!file.is_open()) {
RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"), "Failed to open file: %s", RCLCPP_ERROR(m_logger, "Failed to open file: %s", json_file_path.c_str());
json_file_path.c_str()); return EnvModel{};
return EnvModels{};
} }
json_content = nlohmann::json::parse(file); json_content = nlohmann::json::parse(file);
} catch (const nlohmann::json::parse_error &e) { } catch (const nlohmann::json::parse_error &e) {
RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"), RCLCPP_ERROR(m_logger, "Error parsing JSON in file %s: %s",
"Error parsing JSON in file %s: %s", json_file_path.c_str(), json_file_path.c_str(), e.what());
e.what());
} catch (const std::exception &e) { } catch (const std::exception &e) {
RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"), RCLCPP_ERROR(m_logger, "Exception reading file %s: %s",
"Exception reading file %s: %s", json_file_path.c_str(), json_file_path.c_str(), e.what());
e.what());
} }
EnvModels envModel; EnvModel envModel;
if (!json_content.empty()) { if (!json_content.empty()) {
envModel.model_name = json_content.at("name").get<std::string>(); envModel.model_name = json_content.at("name").get<std::string>();
envModel.mesh_path = env_dir + "/" + m_assembly_dir + "/" + envModel.mesh_path = env_dir + "/" + m_assembly_dir + "/" +
@ -114,7 +106,7 @@ void AssemblyConfigLoader::readModelConfigs() {
for (const auto &entry : for (const auto &entry :
std::filesystem::directory_iterator(assets_folder)) { std::filesystem::directory_iterator(assets_folder)) {
if (entry.is_regular_file() && entry.path().extension() == ".json") { if (entry.is_regular_file() && entry.path().extension() == ".json") {
EnvModels model = parseModelData(entry.path().string()); EnvModel model = parseModelData(entry.path().string());
m_env_models->push_back(model); m_env_models->push_back(model);
} }
} }
@ -142,8 +134,7 @@ std::vector<std::string> AssemblyConfigLoader::getSceneModelNames() {
} }
} }
} else { } else {
RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"), RCLCPP_ERROR(m_logger, "Key 'robossembler_db' not found in m_env_vars.");
"Key 'robossembler_db' not found in m_env_vars.");
return model_names; return model_names;
} }
@ -165,7 +156,7 @@ AssemblyConfigLoader::getTfData(const std::string &model_name) {
// Output position information to console // Output position information to console
for (const auto &transform : tf_msg.transforms) { for (const auto &transform : tf_msg.transforms) {
RCLCPP_INFO_STREAM( RCLCPP_INFO_STREAM(
rclcpp::get_logger("rbs_utils"), m_logger,
"Frame ID: " << transform.header.frame_id << ", Child Frame ID: " "Frame ID: " << transform.header.frame_id << ", Child Frame ID: "
<< transform.child_frame_id << ", Translation: [" << transform.child_frame_id << ", Translation: ["
<< transform.transform.translation.x << ", " << transform.transform.translation.x << ", "
@ -175,8 +166,7 @@ AssemblyConfigLoader::getTfData(const std::string &model_name) {
// auto r = std::make_shared<StaticFramePublisher>(tf_msg); // auto r = std::make_shared<StaticFramePublisher>(tf_msg);
} else { } else {
RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"), RCLCPP_ERROR(m_logger, "Key 'robossembler_db' not found in m_env_vars.");
"Key 'robossembler_db' not found in m_env_vars.");
} }
return tf_msg; return tf_msg;
} }
@ -262,6 +252,122 @@ AssemblyConfigLoader::parseJsonToTFMessage(const nlohmann::json &json,
return tf_msg; return tf_msg;
} }
tf2_msgs::msg::TFMessage
AssemblyConfigLoader::readWorkspaceJson(const nlohmann::json &json) {
tf2_msgs::msg::TFMessage tf_msg;
int i{0};
if (json.contains("workspace") && json.at("workspace").is_array()) {
for (auto &point : json.at("workspace")) {
if (point.contains("position") && point.at("position").is_object()) {
geometry_msgs::msg::TransformStamped workspace_point;
workspace_point.header.frame_id = "world";
workspace_point.child_frame_id = "bbox_" + std::to_string(i);
i++;
if (point["position"].contains("x") &&
point["position"].contains("y") &&
point["position"].contains("z")) {
workspace_point.transform.translation.x =
point["position"]["x"].get<double>();
workspace_point.transform.translation.y =
point["position"]["y"].get<double>();
workspace_point.transform.translation.z =
point["position"]["z"].get<double>();
tf_msg.transforms.push_back(workspace_point);
} else {
RCLCPP_ERROR(m_logger,
"Cannot find key 'x', 'y', 'z' in 'position' or 'name' "
"isn't found in key 'workspace'");
}
} else {
RCLCPP_ERROR(m_logger,
"Cannot find key 'position', 'name', or 'position' isn't "
"an object in key 'workspace'");
}
}
} else {
RCLCPP_ERROR(m_logger, "Cannot find key 'workspace' in robossembler_db");
}
return tf_msg;
}
void AssemblyConfigLoader::publishWorkspace(
const tf2_msgs::msg::TFMessage &tf_msg) {
// if (tf_msg.transforms.size() >= 4) {
// auto pose1 =
// Eigen::Vector3d(tf_msg.transforms[0].transform.translation.x,
// tf_msg.transforms[0].transform.translation.y,
// tf_msg.transforms[0].transform.translation.z);
//
// auto pose2 =
// Eigen::Vector3d(tf_msg.transforms[1].transform.translation.x,
// tf_msg.transforms[1].transform.translation.y,
// tf_msg.transforms[1].transform.translation.z);
//
// auto pose3 =
// Eigen::Vector3d(tf_msg.transforms[2].transform.translation.x,
// tf_msg.transforms[2].transform.translation.y,
// tf_msg.transforms[2].transform.translation.z);
//
// auto pose4 =
// Eigen::Vector3d(tf_msg.transforms[3].transform.translation.x,
// tf_msg.transforms[3].transform.translation.y,
// tf_msg.transforms[3].transform.translation.z);
//
// // Connect workspace points as box
// m_visual_tools->publishLine(pose1, pose2, rviz_visual_tools::GREEN);
// m_visual_tools->publishLine(pose2, pose3, rviz_visual_tools::GREEN);
// m_visual_tools->publishLine(pose3, pose4, rviz_visual_tools::GREEN);
// m_visual_tools->publishLine(pose4, pose1, rviz_visual_tools::GREEN);
// } else {
// RCLCPP_ERROR(m_logger,
// "Not enough points to form a square");
// }
}
void AssemblyConfigLoader::printWorkspace() {
if (m_env_vars->find("robossembler_db") != m_env_vars->end()) {
const nlohmann::json json = (*m_env_vars)["robossembler_db"];
if (json.contains("workspace")) {
auto workspace = readWorkspaceJson(json);
publishWorkspace(workspace);
} else {
RCLCPP_WARN(m_logger, "Workspace isn't set in robossembler_db");
}
} else {
RCLCPP_ERROR(m_logger, "Cannot find robossembler_db");
}
}
geometry_msgs::msg::PoseArray
AssemblyConfigLoader::getWorkspaceInspectorTrajectory() {
geometry_msgs::msg::PoseArray pose_array;
pose_array.header.frame_id = "world";
if (m_env_vars->find("robossembler_db") != m_env_vars->end()) {
const nlohmann::json json = (*m_env_vars)["robossembler_db"];
if (json.contains("workspace")) {
auto workspace = readWorkspaceJson(json);
for (auto &point : workspace.transforms) {
geometry_msgs::msg::Pose pose;
pose = transformTrajectory(point);
pose_array.poses.push_back(pose);
}
}
}
return pose_array;
}
geometry_msgs::msg::Pose AssemblyConfigLoader::transformTrajectory(
const geometry_msgs::msg::TransformStamped &pose) {
auto pose_eigen = tf2::transformToEigen(pose.transform);
Eigen::AngleAxisd rotation(M_PI, Eigen::Vector3d::UnitX());
pose_eigen.rotate(rotation);
pose_eigen.translation().z() += 0.5;
auto pose_msg = tf2::toMsg(pose_eigen);
return pose_msg;
}
double AssemblyConfigLoader::convertToDouble(const nlohmann::json &value) { double AssemblyConfigLoader::convertToDouble(const nlohmann::json &value) {
if (value.is_number()) { if (value.is_number()) {
return value.get<double>(); return value.get<double>();
@ -269,8 +375,7 @@ double AssemblyConfigLoader::convertToDouble(const nlohmann::json &value) {
try { try {
return std::stod(value.get<std::string>()); return std::stod(value.get<std::string>());
} catch (const std::exception &e) { } catch (const std::exception &e) {
RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"), RCLCPP_ERROR(m_logger, "Error converting string to double: %s", e.what());
"Error converting string to double: %s", e.what());
throw std::runtime_error("Error converting string to double"); throw std::runtime_error("Error converting string to double");
} }
} }
@ -278,4 +383,4 @@ double AssemblyConfigLoader::convertToDouble(const nlohmann::json &value) {
throw std::runtime_error("Invalid JSON type for conversion to double"); throw std::runtime_error("Invalid JSON type for conversion to double");
} }
} // namespace rbs_utils } // namespace rbs_utils