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

env_manager: fix target pose loader

Browse source
This commit is contained in:
Ilya Uraev 2023-12-19 08:17:38 +03:00
parent 7637f835b1
commit 86fd3d1055
2 changed files with 3 additions and 6 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -43,9 +43,6 @@ CallbackReturn GzEnviroment::on_configure(const rclcpp_lifecycle::State &) {
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");
for (auto &place : m_transforms.transforms) {
place.child_frame_id = place.child_frame_id + "_target";
}
// if (!doGzSpawn()) // if (!doGzSpawn())
// { // {

6
rbs_utils/src/rbs_utils.cpp
View file

@ -214,7 +214,7 @@ AssemblyConfigLoader::parseJsonToTFMessage(const nlohmann::json &json,
relative_transform.header.frame_id = relative_transform.header.frame_id =
relative_part.at("relativeAt").get<std::string>(); relative_part.at("relativeAt").get<std::string>();
relative_transform.child_frame_id = relative_transform.child_frame_id =
relative_part.at("name").get<std::string>(); relative_part.at("name").get<std::string>() + "_target";
relative_transform.transform.translation.x = relative_transform.transform.translation.x =
convertToDouble(relative_part.at("position").at("x")); convertToDouble(relative_part.at("position").at("x"));
relative_transform.transform.translation.y = relative_transform.transform.translation.y =
@ -238,9 +238,9 @@ AssemblyConfigLoader::parseJsonToTFMessage(const nlohmann::json &json,
if (grasp_pose.at("name").get<std::string>() == model_name) { if (grasp_pose.at("name").get<std::string>() == model_name) {
geometry_msgs::msg::TransformStamped grasp_transform; geometry_msgs::msg::TransformStamped grasp_transform;
grasp_transform.header.frame_id = grasp_transform.header.frame_id =
grasp_pose.at("name").get<std::string>(); grasp_pose.at("name").get<std::string>() + "_target";
grasp_transform.child_frame_id = grasp_transform.child_frame_id =
grasp_pose.at("name").get<std::string>() + "_grasp"; grasp_pose.at("name").get<std::string>() + "_target" + "_grasp";
grasp_transform.transform.translation.x = grasp_transform.transform.translation.x =
convertToDouble(grasp_pose.at("position").at("x")); convertToDouble(grasp_pose.at("position").at("x"));
grasp_transform.transform.translation.y = grasp_transform.transform.translation.y =