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

fix rbs_utils

Browse source
This commit is contained in:
Ilya Uraev 2024-05-07 14:07:08 +03:00
parent 44b927f9a9
commit fa03887099
3 changed files with 93 additions and 80 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -51,10 +51,9 @@ CallbackReturn GzEnviroment::on_configure(const rclcpp_lifecycle::State &) {
m_service_spawn = std::string("/world/") + m_world_name + "/create";
m_config_loader = std::make_shared<rbs_utils::AssemblyConfigLoader>(
"asp-example", getNode());
m_follow_frames = m_config_loader->getSceneModelNames();
m_transforms = m_config_loader->getTfDataAllPossible();
m_timer = getNode()->create_wall_timer(
100ms, std::bind(&GzEnviroment::broadcast_timer_callback, this));
m_follow_frames = m_config_loader->getUniqueSceneModelNames();
m_transforms = m_config_loader->getGraspTfData("bishop");
// m_transforms = m_config_loader->getAllPossibleTfData();
return CallbackReturn::SUCCESS;
}
@ -63,9 +62,9 @@ CallbackReturn GzEnviroment::on_activate(const rclcpp_lifecycle::State &) {
// Setting up the subscribers and publishers
RCLCPP_INFO(getNode()->get_logger(), "GzEnviroment is on_activate");
// Initialize tf broadcaster before use in subscriber
m_tf2_broadcaster =
std::make_unique<tf2_ros::TransformBroadcaster>(getNode());
m_gz_node->Subscribe(m_topic_name, &GzEnviroment::onGzPoseSub, this);
return CallbackReturn::SUCCESS;
@ -121,6 +120,7 @@ void GzEnviroment::onGzPoseSub(const gz::msgs::Pose_V &pose_v) {
}
for (auto &place : m_transforms.transforms) {
place.header.stamp = getNode()->get_clock()->now();
all_transforms.push_back(place);
}