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

update main

Browse source
This commit is contained in:
Ilya Uraev 2024-04-22 15:06:46 +03:00
parent d5e7768d90
commit e8ea09e020
20 changed files with 315 additions and 168 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -97,4 +97,4 @@ private:
std::shared_ptr<pluginlib::ClassLoader<env_interface::EnvInterface>> m_loader;
};
} // namespace env_manager
#endif // __ENV_MANAGER_HPP__
#endif // __ENV_MANAGER_HPP__

6
env_manager/gz_enviroment/CMakeLists.txt
View file

@ -67,6 +67,12 @@ install(TARGETS ${PROJECT_NAME}
LIBRARY DESTINATION lib
)
install(PROGRAMS
scripts/publish_asm_config.py
scripts/test_tf.py
DESTINATION lib/${PROJECT_NAME}
)
ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
# ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
ament_package()

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

@ -2,12 +2,13 @@
#include "rclcpp_lifecycle/lifecycle_node.hpp"
// #include "ros_gz_bridge/convert.hpp"
#include "env_interface/env_interface.hpp"
#include "geometry_msgs/msg/pose_array.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 <rclcpp/timer.hpp>
#include <string>
#include <tf2_msgs/msg/detail/tf_message__struct.hpp>
#include <tf2_msgs/msg/tf_message.hpp>
@ -16,6 +17,7 @@
#include <gz/sim/Entity.hh>
#include <gz/sim/EntityComponentManager.hh>
#include <gz/sim/components/Model.hh>
#include <thread>
namespace gz_enviroment {
using CallbackReturn =
@ -37,7 +39,6 @@ protected:
private:
std::unique_ptr<tf2_ros::TransformBroadcaster> m_tf2_broadcaster;
std::unique_ptr<tf2_ros::TransformBroadcaster> m_tf2_broadcaster_target;
std::shared_ptr<gz::transport::Node> m_gz_node;
std::vector<std::string> m_follow_frames;
std::string m_topic_name;
@ -46,6 +47,9 @@ private:
std::shared_ptr<rbs_utils::AssemblyConfigLoader> m_config_loader;
tf2_msgs::msg::TFMessage m_transforms;
tf2_msgs::msg::TFMessage m_target_places;
rclcpp::TimerBase::SharedPtr m_timer;
std::vector<geometry_msgs::msg::TransformStamped> m_all_transforms{};
std::string getGzWorldName();
std::string createGzModelString(const std::vector<double> &pose,
@ -53,5 +57,14 @@ private:
const double &mass,
const std::string &mesh_filepath,
const std::string &name);
void broadcast_timer_callback() {
if (!m_transforms.transforms.empty()) {
for (auto &transform : m_transforms.transforms) {
m_tf2_broadcaster->sendTransform(std::move(transform));
}
// m_transforms.transforms.clear();
}
}
};
} // namespace gz_enviroment

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

@ -2,8 +2,13 @@
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "ros_gz_bridge/convert.hpp"
#include <algorithm>
#include <chrono>
#include <functional>
#include <iterator>
#include <memory>
#include <rclcpp/logging.hpp>
#include <rclcpp/utilities.hpp>
#include <rclcpp_lifecycle/lifecycle_node.hpp>
#include <ros_gz_interfaces/msg/detail/entity__builder.hpp>
@ -30,11 +35,15 @@ CallbackReturn GzEnviroment::on_init() {
"Model Name: " << modelName->Data());
}
}
// getNode()->declare_parameter("use_sim_time", true);
return CallbackReturn::SUCCESS;
}
CallbackReturn GzEnviroment::on_configure(const rclcpp_lifecycle::State &) {
// Configuration of parameters
using namespace std::chrono_literals;
RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_configure");
m_gz_node = std::make_shared<gz::transport::Node>();
m_world_name = getGzWorldName();
@ -43,15 +52,9 @@ CallbackReturn GzEnviroment::on_configure(const rclcpp_lifecycle::State &) {
m_config_loader = std::make_shared<rbs_utils::AssemblyConfigLoader>(
"asp-example", 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())
// {
// return CallbackReturn::ERROR;
// }
m_transforms = m_config_loader->getTfDataAllPossible();
m_timer = getNode()->create_wall_timer(
100ms, std::bind(&GzEnviroment::broadcast_timer_callback, this));
return CallbackReturn::SUCCESS;
}
@ -60,11 +63,10 @@ CallbackReturn GzEnviroment::on_activate(const rclcpp_lifecycle::State &) {
// Setting up the subscribers and publishers
RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_activate");
m_gz_node->Subscribe(m_topic_name, &GzEnviroment::onGzPoseSub, this);
m_tf2_broadcaster =
std::make_unique<tf2_ros::TransformBroadcaster>(getNode());
m_tf2_broadcaster_target =
std::make_unique<tf2_ros::TransformBroadcaster>(getNode());
m_gz_node->Subscribe(m_topic_name, &GzEnviroment::onGzPoseSub, this);
return CallbackReturn::SUCCESS;
}
@ -86,7 +88,6 @@ 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");
// What we should use here?
return CallbackReturn::SUCCESS;
}
@ -123,9 +124,7 @@ void GzEnviroment::onGzPoseSub(const gz::msgs::Pose_V &pose_v) {
all_transforms.push_back(place);
}
for (auto &transform : all_transforms) {
m_tf2_broadcaster->sendTransform(transform);
}
// m_all_transforms.swap(all_transforms);
}
std::string GzEnviroment::getGzWorldName() {
@ -153,55 +152,6 @@ std::string GzEnviroment::getGzWorldName() {
return worlds_msg.data(0);
}
// bool GzEnviroment::doGzSpawn() {
// gz::msgs::EntityFactory req;
// gz::msgs::Boolean rep;
// bool result;
// unsigned int timeout = 5000;
// bool executed;
//
// auto env_models = m_config_loader->getEnvModels();
// for (auto &model : *env_models) {
// std::string sdf_string =
// createGzModelString(model.model_pose, model.model_inertia, model.mass,
// model.mesh_path, model.model_name);
// req.set_sdf(sdf_string);
// executed = m_gz_node->Request(m_service_spawn, req, timeout, rep, result);
// }
// return executed;
// }
// std::string GzEnviroment::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 model_template =
// std::string("<sdf version='1.7'>") + "<model name='" + name + "'>" +
// "<link name='link_" + name + "'>" + "<static>1</static>" + "<pose>" +
// std::to_string(pose[0]) + " " + std::to_string(pose[1]) + " " +
// std::to_string(pose[2]) + " " + std::to_string(pose[3]) + " " +
// std::to_string(pose[4]) + " " + std::to_string(pose[5]) +
// "</pose>"
// // + "<inertial>"
// // + "<inertia>"
// // + "<ixx>" + std::to_string(inertia[0]) + "</ixx>"
// // + "<ixy>" + std::to_string(inertia[1]) + "</ixy>"
// // + "<ixz>" + std::to_string(inertia[2]) + "</ixz>"
// // + "<iyy>" + std::to_string(inertia[3]) + "</iyy>"
// // + "<iyz>" + std::to_string(inertia[4]) + "</iyz>"
// // + "<izz>" + std::to_string(inertia[5]) + "</izz>"
// // + "</inertia>"
// // + "<mass>" + std::to_string(mass) + "</mass>"
// // + "</inertial>"
// + "<visual name='visual_" + name + "'>" + "<geometry><mesh><uri>file://" +
// mesh_filepath + "</uri></mesh></geometry>" + "</visual>" +
// "<collision name='collision_" + name + "'>" +
// "<geometry><mesh><uri>file://" + mesh_filepath +
// "</uri></mesh></geometry>" + "</collision>" + "</link>" + "</model>" +
// "</sdf>";
// return model_template;
// }
} // namespace gz_enviroment
#include "pluginlib/class_list_macros.hpp"