robossembler/runtime
7
0
Fork 0
Code Issues 4 Pull requests Projects Releases Packages Wiki Activity Actions

add workspace base functional

Browse source 
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
Download patch file Download diff file Expand all files Collapse all files

2
env_manager/env_interface/include/env_interface/env_interface.hpp
View file

@ -11,6 +11,6 @@ public:
EnvInterface() = default;
virtual ~EnvInterface() = default;
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

42
env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp
View file

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

24
env_manager/gz_enviroment/src/gz_enviroment.cpp
View file

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

21
rbs_utils/CMakeLists.txt
View file

@ -8,12 +8,22 @@ endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_lifecycle REQUIRED)
find_package(tf2_ros 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)
@ -22,7 +32,7 @@ install(
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
# PUBLIC
@ -57,4 +67,7 @@ if(BUILD_TESTING)
endif()
ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
ament_export_dependencies(
${deps}
)
ament_package()

51
rbs_utils/include/rbs_utils/rbs_utils.hpp
View file

@ -1,24 +1,25 @@
#include <Eigen/Core>
#include "geometry_msgs/msg/pose.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_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2_ros/buffer.h"
#include <tf2/convert.h>
#include "tf2_ros/static_transform_broadcaster.h"
#include <fstream>
#include <memory>
#include <nlohmann/json.hpp>
#include <rclcpp/logger.hpp>
#include <rclcpp/node.hpp>
#include <string>
#include <tf2/LinearMath/Vector3.h>
#include <tf2_eigen/tf2_eigen.hpp>
#include <tf2_msgs/msg/detail/tf_message__struct.hpp>
#include <unordered_map>
#include <vector>
const std::string env_dir = std::getenv("RBS_ASSEMBLY_DIR");
namespace rbs_utils {
struct EnvModels {
struct EnvModel {
std::string model_name;
std::string mesh_path;
std::vector<double> model_inertia;
@ -26,10 +27,23 @@ struct EnvModels {
double mass;
};
class AssemblyConfigLoader {
struct Workspace {
std::string name;
geometry_msgs::msg::PoseArray poses;
};
class AssemblyConfigLoader {
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>>
getAssemblyFileData() {
@ -37,31 +51,40 @@ public:
}
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;
}
std::vector<std::string> getSceneModelNames();
void printWorkspace();
geometry_msgs::msg::PoseArray getWorkspaceInspectorTrajectory();
geometry_msgs::msg::Pose
transformTrajectory(const geometry_msgs::msg::TransformStamped &pose);
private:
std::shared_ptr<std::unordered_map<std::string, nlohmann::json>> m_env_vars;
std::vector<std::string> m_env_files;
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::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,
const std::string &filepath);
tf2_msgs::msg::TFMessage readWorkspaceJson(const nlohmann::json &json);
tf2_msgs::msg::TFMessage parseJsonToTFMessage(const nlohmann::json &json,
const std::string &model_name);
void publishWorkspace(const tf2_msgs::msg::TFMessage &tf_msg);
void setTfFromDb(const std::string &filename);
double convertToDouble(const nlohmann::json &value);
void readModelConfigs();
EnvModels parseModelData(const std::string &jsonFilePath);
EnvModel parseModelData(const std::string &jsonFilePath);
};
class StaticFramePublisher : public rclcpp::Node {

2
rbs_utils/package.xml
View file

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

199
rbs_utils/src/rbs_utils.cpp
View file

@ -1,26 +1,24 @@
#include "rbs_utils/rbs_utils.hpp"
#include <fstream>
#include <iostream>
#include <iterator>
#include <memory>
#include <nlohmann/json_fwd.hpp>
#include <ostream>
#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>
#include <geometry_msgs/msg/detail/pose__struct.hpp>
#include <tf2/LinearMath/Transform.h>
#include <tf2/convert.h>
#include <tf2_eigen/tf2_eigen.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2_ros/buffer_interface.h>
namespace rbs_utils {
AssemblyConfigLoader::AssemblyConfigLoader(const std::string &t_assembly_dir)
AssemblyConfigLoader::AssemblyConfigLoader(
const std::string &t_assembly_dir,
const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr
&t_logging_interface)
: m_env_vars(
std::make_shared<std::unordered_map<std::string, nlohmann::json>>()),
m_env_models(std::make_shared<std::vector<EnvModels>>()),
m_assembly_dir(t_assembly_dir) {
m_env_models(std::make_shared<std::vector<EnvModel>>()),
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()) {
std::vector<std::string> filenames = {"robossembler_db", "sequences"};
for (auto &filename : filenames) {
@ -31,9 +29,8 @@ AssemblyConfigLoader::AssemblyConfigLoader(const std::string &t_assembly_dir)
readAssemblyFileData(filename, filepath);
}
readModelConfigs();
} 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 {
std::ifstream i(filepath);
if (!i.is_open()) {
RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"), "Failed to open file: %s",
filepath.c_str());
RCLCPP_ERROR(m_logger, "Failed to open file: %s", filepath.c_str());
return;
}
nlohmann::json json = nlohmann::json::parse(i);
m_env_vars->insert({filename, json});
} catch (const nlohmann::json::parse_error &e) {
RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"),
"Error parsing JSON in file %s: %s", filepath.c_str(),
e.what());
RCLCPP_ERROR(m_logger, "Error parsing JSON in file %s: %s",
filepath.c_str(), e.what());
} catch (const std::exception &e) {
RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"),
"Exception reading file %s: %s", filepath.c_str(), e.what());
RCLCPP_ERROR(m_logger, "Exception reading file %s: %s", filepath.c_str(),
e.what());
}
}
EnvModels
EnvModel
AssemblyConfigLoader::parseModelData(const std::string &json_file_path) {
nlohmann::json json_content{};
try {
std::ifstream file(json_file_path);
if (!file.is_open()) {
RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"), "Failed to open file: %s",
json_file_path.c_str());
return EnvModels{};
RCLCPP_ERROR(m_logger, "Failed to open file: %s", json_file_path.c_str());
return EnvModel{};
}
json_content = nlohmann::json::parse(file);
} catch (const nlohmann::json::parse_error &e) {
RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"),
"Error parsing JSON in file %s: %s", json_file_path.c_str(),
e.what());
RCLCPP_ERROR(m_logger, "Error parsing JSON in file %s: %s",
json_file_path.c_str(), e.what());
} catch (const std::exception &e) {
RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"),
"Exception reading file %s: %s", json_file_path.c_str(),
e.what());
RCLCPP_ERROR(m_logger, "Exception reading file %s: %s",
json_file_path.c_str(), e.what());
}
EnvModels envModel;
EnvModel envModel;
if (!json_content.empty()) {
envModel.model_name = json_content.at("name").get<std::string>();
envModel.mesh_path = env_dir + "/" + m_assembly_dir + "/" +
@ -114,7 +106,7 @@ void AssemblyConfigLoader::readModelConfigs() {
for (const auto &entry :
std::filesystem::directory_iterator(assets_folder)) {
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);
}
}
@ -142,8 +134,7 @@ std::vector<std::string> AssemblyConfigLoader::getSceneModelNames() {
}
}
} else {
RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"),
"Key 'robossembler_db' not found in m_env_vars.");
RCLCPP_ERROR(m_logger, "Key 'robossembler_db' not found in m_env_vars.");
return model_names;
}
@ -165,7 +156,7 @@ AssemblyConfigLoader::getTfData(const std::string &model_name) {
// Output position information to console
for (const auto &transform : tf_msg.transforms) {
RCLCPP_INFO_STREAM(
rclcpp::get_logger("rbs_utils"),
m_logger,
"Frame ID: " << transform.header.frame_id << ", Child Frame ID: "
<< transform.child_frame_id << ", Translation: ["
<< transform.transform.translation.x << ", "
@ -175,8 +166,7 @@ AssemblyConfigLoader::getTfData(const std::string &model_name) {
// auto r = std::make_shared<StaticFramePublisher>(tf_msg);
} else {
RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"),
"Key 'robossembler_db' not found in m_env_vars.");
RCLCPP_ERROR(m_logger, "Key 'robossembler_db' not found in m_env_vars.");
}
return tf_msg;
}
@ -262,6 +252,122 @@ AssemblyConfigLoader::parseJsonToTFMessage(const nlohmann::json &json,
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) {
if (value.is_number()) {
return value.get<double>();
@ -269,8 +375,7 @@ double AssemblyConfigLoader::convertToDouble(const nlohmann::json &value) {
try {
return std::stod(value.get<std::string>());
} catch (const std::exception &e) {
RCLCPP_ERROR(rclcpp::get_logger("rbs_utils"),
"Error converting string to double: %s", e.what());
RCLCPP_ERROR(m_logger, "Error converting string to double: %s", e.what());
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");
}
} // namespace rbs_utils
} // namespace rbs_utils