add workspace base functional
Workspace functional includes basic read from workspace and trajectory calculation for inspection
This commit is contained in:
parent
e5e8110697
commit
192887ab8b
7 changed files with 240 additions and 101 deletions
|
@ -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
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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()
|
||||||
|
|
|
@ -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 {
|
||||||
|
|
|
@ -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>
|
||||||
|
|
||||||
|
|
|
@ -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
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue