diff --git a/env_manager/env_manager/include/env_manager/env_manager.hpp b/env_manager/env_manager/include/env_manager/env_manager.hpp index 2333fbd..912c168 100644 --- a/env_manager/env_manager/include/env_manager/env_manager.hpp +++ b/env_manager/env_manager/include/env_manager/env_manager.hpp @@ -97,4 +97,4 @@ private: std::shared_ptr> m_loader; }; } // namespace env_manager -#endif // __ENV_MANAGER_HPP__ \ No newline at end of file +#endif // __ENV_MANAGER_HPP__ diff --git a/env_manager/gz_enviroment/CMakeLists.txt b/env_manager/gz_enviroment/CMakeLists.txt index bf496f1..42c7027 100644 --- a/env_manager/gz_enviroment/CMakeLists.txt +++ b/env_manager/gz_enviroment/CMakeLists.txt @@ -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() diff --git a/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp b/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp index d33e147..34f84cb 100644 --- a/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp +++ b/env_manager/gz_enviroment/include/gz_enviroment/gz_enviroment.hpp @@ -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 -#include "geometry_msgs/msg/pose_array.hpp" #include #include #include +#include #include #include #include @@ -16,6 +17,7 @@ #include #include #include +#include namespace gz_enviroment { using CallbackReturn = @@ -37,7 +39,6 @@ protected: private: std::unique_ptr m_tf2_broadcaster; - std::unique_ptr m_tf2_broadcaster_target; std::shared_ptr m_gz_node; std::vector m_follow_frames; std::string m_topic_name; @@ -46,6 +47,9 @@ private: std::shared_ptr m_config_loader; tf2_msgs::msg::TFMessage m_transforms; tf2_msgs::msg::TFMessage m_target_places; + rclcpp::TimerBase::SharedPtr m_timer; + + std::vector m_all_transforms{}; std::string getGzWorldName(); std::string createGzModelString(const std::vector &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 diff --git a/env_manager/gz_enviroment/src/gz_enviroment.cpp b/env_manager/gz_enviroment/src/gz_enviroment.cpp index 6ba4578..084bd50 100644 --- a/env_manager/gz_enviroment/src/gz_enviroment.cpp +++ b/env_manager/gz_enviroment/src/gz_enviroment.cpp @@ -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 +#include +#include +#include #include #include +#include #include #include @@ -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(); m_world_name = getGzWorldName(); @@ -43,15 +52,9 @@ CallbackReturn GzEnviroment::on_configure(const rclcpp_lifecycle::State &) { m_config_loader = std::make_shared( "asp-example", getNode()); m_follow_frames = m_config_loader->getSceneModelNames(); - // m_target_places = std::make_shared(); - 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(getNode()); - m_tf2_broadcaster_target = - std::make_unique(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 &pose, const std::vector &inertia, -// const double &mass, const std::string &mesh_filepath, -// const std::string &name) { -// std::string model_template = -// std::string("") + "" + -// "" + "1" + "" + -// 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]) + -// "" -// // + "" -// // + "" -// // + "" + std::to_string(inertia[0]) + "" -// // + "" + std::to_string(inertia[1]) + "" -// // + "" + std::to_string(inertia[2]) + "" -// // + "" + std::to_string(inertia[3]) + "" -// // + "" + std::to_string(inertia[4]) + "" -// // + "" + std::to_string(inertia[5]) + "" -// // + "" -// // + "" + std::to_string(mass) + "" -// // + "" -// + "" + "file://" + -// mesh_filepath + "" + "" + -// "" + -// "file://" + mesh_filepath + -// "" + "" + "" + "" + -// ""; -// return model_template; -// } - } // namespace gz_enviroment #include "pluginlib/class_list_macros.hpp" diff --git a/rbs_bringup/config/roboclone.yaml b/rbs_bringup/config/roboclone.yaml new file mode 100644 index 0000000..32c2155 --- /dev/null +++ b/rbs_bringup/config/roboclone.yaml @@ -0,0 +1,23 @@ +scene_config: + - name: arm1 + type: rbs_arm + pose: + position: + x: -0.45 + y: -2.0 + z: 1.6 + orientation: + x: 3.14159 + y: 0.0 + z: 0.0 + - name: arm2 + type: rbs_arm + pose: + position: + x: 0.45 + y: -2.0 + z: 1.6 + orientation: + x: 3.14159 + y: 0.0 + z: 0.0 diff --git a/rbs_bringup/launch/multi_robot.launch.py b/rbs_bringup/launch/multi_robot.launch.py index fe95db7..572838b 100644 --- a/rbs_bringup/launch/multi_robot.launch.py +++ b/rbs_bringup/launch/multi_robot.launch.py @@ -73,7 +73,7 @@ def launch_setup(context, *args, **kwargs): convert_types=False, ).merge_yamls() for robot in robots["scene_config"]: - namespace = "/" + robot["name"] + namespace: str = "/" + robot["name"] ld.append(IncludeLaunchDescription( PythonLaunchDescriptionSource([ PathJoinSubstitution([ @@ -106,9 +106,13 @@ def launch_setup(context, *args, **kwargs): "hardware": hardware, "launch_controllers": launch_controllers, "gazebo_gui": gazebo_gui, + "namespace": namespace, "x": str(robot["pose"]["position"]["x"]), "y": str(robot["pose"]["position"]["y"]), - "z": str(robot["pose"]["position"]["z"]) + "z": str(robot["pose"]["position"]["z"]), + "roll": str(robot["pose"]["orientation"]["x"]), + "pitch": str(robot["pose"]["orientation"]["y"]), + "yaw": str(robot["pose"]["orientation"]["z"]), }.items() )) @@ -117,9 +121,12 @@ def launch_setup(context, *args, **kwargs): executable='create', arguments=[ '-name', robot_name, - '-x', "0",#str(robot["spawn_point"][0]), - '-y', "0",#str(robot["spawn_point"][1]), - '-z', "0",#str(robot["spawn_point"][2]), + # '-x', str(robot["pose"]["position"]["x"]), + # '-y', str(robot["pose"]["position"]["y"]), + # '-z', str(robot["pose"]["position"]["z"]), + # '-R', str(robot["pose"]["orientation"]["x"]), + # '-P', str(robot["pose"]["orientation"]["y"]), + # '-Y', str(robot["pose"]["orientation"]["z"]), '-topic', namespace + '/robot_description'], output='screen', condition=IfCondition(sim_gazebo)) diff --git a/rbs_bringup/launch/rbs_robot.launch.py b/rbs_bringup/launch/rbs_robot.launch.py index b043dc5..ab48fa4 100644 --- a/rbs_bringup/launch/rbs_robot.launch.py +++ b/rbs_bringup/launch/rbs_robot.launch.py @@ -39,14 +39,18 @@ def launch_setup(context, *args, **kwargs): x_pos = LaunchConfiguration("x") y_pos = LaunchConfiguration("y") z_pos = LaunchConfiguration("z") + roll = LaunchConfiguration("roll") + pitch = LaunchConfiguration("pitch") + yaw = LaunchConfiguration("yaw") + namespace = LaunchConfiguration("namespace") robot_name = robot_name.perform(context) - namespace = "/" + robot_name + namespace = namespace.perform(context) robot_type = robot_type.perform(context) description_package = description_package.perform(context) description_file = description_file.perform(context) controllers_file = controllers_file.perform(context) - remappings = [("/tf", "tf"), ("/tf_static", "tf_static")] + # remappings = [("/tf", "tf"), ("/tf_static", "tf_static")] xacro_file = os.path.join(get_package_share_directory(description_package), "urdf", description_file) robot_description_doc = xacro.process_file( @@ -89,8 +93,27 @@ def launch_setup(context, *args, **kwargs): executable="robot_state_publisher", namespace=namespace, output="both", - remappings=remappings, - parameters=[{"use_sim_time": True}, robot_description], + # remappings=remappings, + parameters=[{"use_sim_time": use_sim_time}, robot_description], + ) + + rviz_config_file = PathJoinSubstitution( + [FindPackageShare("rbs_bringup"), "config", "rbs.rviz"] + ) + + rviz = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + namespace=namespace, + arguments=["-d", rviz_config_file], + parameters=[ + robot_description, + robot_description_semantic, + robot_description_kinematics, + {'use_sim_time': use_sim_time} + ] ) control = IncludeLaunchDescription( @@ -182,7 +205,8 @@ def launch_setup(context, *args, **kwargs): moveit, skills, task_planner, - perception + perception, + # rviz ] return nodes_to_start @@ -337,6 +361,11 @@ def generate_launch_description(): default_value="false", description="Launch gazebo with gui?") ) + declared_arguments.append( + DeclareLaunchArgument("namespace", + default_value="", + description="The ROS2 namespace of a robot") + ) declared_arguments.append( DeclareLaunchArgument("x", default_value="0.0", diff --git a/rbs_bringup/launch/single_robot.launch.py b/rbs_bringup/launch/single_robot.launch.py index 08e1fb8..450dd5e 100644 --- a/rbs_bringup/launch/single_robot.launch.py +++ b/rbs_bringup/launch/single_robot.launch.py @@ -58,27 +58,31 @@ def launch_setup(context, *args, **kwargs): condition=IfCondition(launch_simulation) ) - configured_params = RewrittenYaml( - source_file=os.path.join( - get_package_share_directory( - description_package.perform(context)), - "config", - controllers_file.perform(context)), - root_key=robot_name.perform(context), - param_rewrites={}, - convert_types=True, + # FIXME: namespaces + # configured_params = RewrittenYaml( + # source_file=os.path.join( + # get_package_share_directory( + # description_package.perform(context)), + # "config", + # controllers_file.perform(context)), + # root_key=robot_name.perform(context), + # param_rewrites={}, + # convert_types=True, + # ) + + initial_joint_controllers_file_path = os.path.join( + get_package_share_directory('rbs_arm'), 'config', 'rbs_arm0_controllers.yaml' ) - controller_paramfile = configured_params.perform(context) - namespace = "/" + robot_name.perform(context) + + # controller_paramfile = configured_params.perform(context) + # namespace = "/" + robot_name.perform(context) + namespace = "" gz_spawner = Node( package='ros_gz_sim', executable='create', arguments=[ '-name', robot_name.perform(context), - '-x', "0.5", - '-y', "0.5", - '-z', "0.0", '-topic', namespace + '/robot_description'], output='screen', condition=IfCondition(sim_gazebo)) @@ -97,11 +101,11 @@ def launch_setup(context, *args, **kwargs): "gripper_name": gripper_name, "controllers_file": controllers_file, "robot_type": robot_type, - "controllers_file": controller_paramfile, + "controllers_file": initial_joint_controllers_file_path, "cartesian_controllers": cartesian_controllers, "description_package": description_package, "description_file": description_file, - "robot_name": robot_name, + "robot_name": robot_type, "start_joint_controller": start_joint_controller, "initial_joint_controller": initial_joint_controller, "launch_simulation": launch_simulation, @@ -115,9 +119,9 @@ def launch_setup(context, *args, **kwargs): "hardware": hardware, "launch_controllers": "true", "gazebo_gui": gazebo_gui, - "x": "0.5", - "y": "0.5", - "z": "0.5" + # "x": "0.5", + # "y": "0.5", + # "z": "0.5" }.items() ) diff --git a/rbs_bt_executor/bt_trees/test_tree.xml b/rbs_bt_executor/bt_trees/test_tree.xml index 6e59ee7..381562a 100644 --- a/rbs_bt_executor/bt_trees/test_tree.xml +++ b/rbs_bt_executor/bt_trees/test_tree.xml @@ -14,19 +14,19 @@ + robot_name="{robot_name}" server_name="/cartesian_move_to_pose" server_timeout="5000" /> + robot_name="{robot_name}" server_name="/cartesian_move_to_pose" server_timeout="5000" /> + robot_name="{robot_name}" server_name="/cartesian_move_to_pose" server_timeout="5000" /> + robot_name="{robot_name}" server_name="/cartesian_move_to_pose" server_timeout="5000" /> + robot_name="{robot_name}" server_name="/cartesian_move_to_pose" server_timeout="5000" /> diff --git a/rbs_bt_executor/launch/rbs_executor.launch.py b/rbs_bt_executor/launch/rbs_executor.launch.py index e4c06dc..68f26f1 100644 --- a/rbs_bt_executor/launch/rbs_executor.launch.py +++ b/rbs_bt_executor/launch/rbs_executor.launch.py @@ -27,11 +27,16 @@ def generate_launch_description(): Node( package='behavior_tree', executable='bt_engine', - # prefix=['gdbserver localhost:3000'], + # prefix=['gdbserver localhost:1234'], parameters=[ btfile_param, - bt_skills_param - ] + bt_skills_param, + {'use_sim_time': True} + ], + # arguments=[ + # "--ros-args", + # "--log-level", "debug", + # ], ) ] diff --git a/rbs_bt_executor/src/EnvManager.cpp b/rbs_bt_executor/src/EnvManager.cpp index 8752520..70da1dd 100644 --- a/rbs_bt_executor/src/EnvManager.cpp +++ b/rbs_bt_executor/src/EnvManager.cpp @@ -35,7 +35,7 @@ public: if (response->ok) { // TODO We need better perfomance for call it in another place for all BT nodes // - Make it via shared_ptr forward throught blackboard - auto t = std::make_shared("asp-example2", _node); + auto t = std::make_shared("asp-example", _node); auto p = t->getWorkspaceInspectorTrajectory(); setOutput("workspace", p); return BT::NodeStatus::SUCCESS; diff --git a/rbs_bt_executor/src/MoveToPose.cpp b/rbs_bt_executor/src/MoveToPose.cpp index a2de744..bed2dfe 100644 --- a/rbs_bt_executor/src/MoveToPose.cpp +++ b/rbs_bt_executor/src/MoveToPose.cpp @@ -29,8 +29,8 @@ class MoveToPose : public BtAction { goal.robot_name = robot_name_; goal.target_pose = pose_des; - goal.end_effector_acceleration = 0.5; - goal.end_effector_velocity = 0.5; + goal.end_effector_acceleration = 1.0; + goal.end_effector_velocity = 1.0; RCLCPP_INFO(_node->get_logger(), "Goal populated"); diff --git a/rbs_simulation/launch/simulation_gazebo.launch.py b/rbs_simulation/launch/simulation_gazebo.launch.py index a3025a3..1e3457b 100644 --- a/rbs_simulation/launch/simulation_gazebo.launch.py +++ b/rbs_simulation/launch/simulation_gazebo.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): env_manager = Node( package="env_manager", executable="run_env_manager", + parameters=[{'use_sim_time': True}], condition=IfCondition(launch_env_manager) ) diff --git a/rbs_simulation/worlds/asm2.sdf b/rbs_simulation/worlds/asm2.sdf index 83d0f58..3360d57 100644 --- a/rbs_simulation/worlds/asm2.sdf +++ b/rbs_simulation/worlds/asm2.sdf @@ -92,16 +92,15 @@ - + Manipulating objects board model://board 0.45 0.0 0.0 0.0 0.0 0.0 - - bishop - model://bishop - 0.35 0.0 0.0 0.0 0.0 0.0 - + + + + + - \ No newline at end of file diff --git a/rbs_skill_servers/CMakeLists.txt b/rbs_skill_servers/CMakeLists.txt index 473faf4..6054d82 100644 --- a/rbs_skill_servers/CMakeLists.txt +++ b/rbs_skill_servers/CMakeLists.txt @@ -152,6 +152,7 @@ ament_export_include_directories(include) ament_python_install_package(${PROJECT_NAME}) install(PROGRAMS scripts/test_cartesian_controller.py + scripts/move_to_pose.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/rbs_skill_servers/launch/skills.launch.py b/rbs_skill_servers/launch/skills.launch.py index 16f51ad..73f3994 100644 --- a/rbs_skill_servers/launch/skills.launch.py +++ b/rbs_skill_servers/launch/skills.launch.py @@ -38,6 +38,12 @@ def launch_setup(context, *args, **kwargs): ] ) + move_to_pose = Node( + package="rbs_skill_servers", + executable="move_to_pose.py", + namespace=namespace + ) + gripper_control_node = Node( package="rbs_skill_servers", executable="gripper_control_action_server", @@ -89,6 +95,7 @@ def launch_setup(context, *args, **kwargs): gripper_control_node, move_cartesian_path_action_server, move_joint_state_action_server, + move_to_pose, # grasp_pose_loader ] return nodes_to_start diff --git a/rbs_skill_servers/scripts/test_cartesian_controller.py b/rbs_skill_servers/scripts/test_cartesian_controller.py old mode 100644 new mode 100755 index f733258..5d96555 --- a/rbs_skill_servers/scripts/test_cartesian_controller.py +++ b/rbs_skill_servers/scripts/test_cartesian_controller.py @@ -1,3 +1,4 @@ +#!/usr/bin/python import rclpy from rclpy.node import Node import argparse diff --git a/rbs_utils/rbs_utils/include/rbs_utils/rbs_utils.hpp b/rbs_utils/rbs_utils/include/rbs_utils/rbs_utils.hpp index 8bedf71..b0ad8ba 100644 --- a/rbs_utils/rbs_utils/include/rbs_utils/rbs_utils.hpp +++ b/rbs_utils/rbs_utils/include/rbs_utils/rbs_utils.hpp @@ -12,13 +12,17 @@ #include "tf2_ros/static_transform_broadcaster.h" #include #include +#include +#include #include #include #include #include #include +#include #include #include +#include #include const std::string env_dir = std::getenv("RBS_ASSEMBLY_DIR"); @@ -50,14 +54,17 @@ public: &t_clock_node_interface); tf2_msgs::msg::TFMessage getTfData(const std::string &model_name); + tf2_msgs::msg::TFMessage getTfDataAllPossible(); + std::vector getSceneModelNames(); void printWorkspace(); geometry_msgs::msg::PoseArray getWorkspaceInspectorTrajectory(); geometry_msgs::msg::Pose - transformTrajectory(const geometry_msgs::msg::TransformStamped &pose); + transformTrajectory(const geometry_msgs::msg::Pose &pose); void saveRbsConfig(); + tf2_msgs::msg::TFMessage getAdditionalPoses(); private: std::vector m_env_files; @@ -69,7 +76,7 @@ private: rclcpp::Clock::SharedPtr m_clock; void parseRbsDb(const std::string &filepath); - tf2_msgs::msg::TFMessage getWorkspace(); + geometry_msgs::msg::PoseArray getWorkspace(); geometry_msgs::msg::Transform createTransform(const geometry_msgs::msg::Pose &pose); }; diff --git a/rbs_utils/rbs_utils/src/rbs_utils.cpp b/rbs_utils/rbs_utils/src/rbs_utils.cpp index e67e2d3..c2fec9f 100644 --- a/rbs_utils/rbs_utils/src/rbs_utils.cpp +++ b/rbs_utils/rbs_utils/src/rbs_utils.cpp @@ -1,12 +1,15 @@ #include "rbs_utils/rbs_utils.hpp" +#include #include #include #include +#include #include #include #include #include #include +#include #include #include #include @@ -27,7 +30,7 @@ AssemblyConfigLoader::AssemblyConfigLoader( m_logger(t_logging_interface->get_logger()), m_clock(t_clock_interface->get_clock()) { if (!m_assembly_dir.empty()) { - std::vector filenames = {"robossembler_db"}; + std::vector filenames = {"rbs_db"}; for (auto &filename : filenames) { std::string filepath = env_dir + "/" + m_assembly_dir + "/" + filename + ".yaml"; @@ -49,18 +52,22 @@ void AssemblyConfigLoader::parseRbsDb(const std::string &filepath) { InterfaceTypeName interface{"rbs_utils_interfaces", "AssemblyConfig"}; rosmsg.type_info = dynmsg::cpp::get_type_info(interface); - void * ros_msg = reinterpret_cast(&m_assembly_config); - dynmsg::cpp::yaml_and_typeinfo_to_rosmsg(rosmsg.type_info, asm_config_string, ros_msg); - + void *ros_msg = reinterpret_cast(&m_assembly_config); + dynmsg::cpp::yaml_and_typeinfo_to_rosmsg(rosmsg.type_info, + asm_config_string, ros_msg); + // auto file_cont = rbs_utils_interfaces::msg::to_yaml(m_assembly_config); + // std::ofstream file("rbs_db.yaml"); + // if (file.is_open()) { + // file << file_cont; + // file.close(); + // } } catch (const std::exception &e) { RCLCPP_ERROR(m_logger, "Exception reading file %s: %s", filepath.c_str(), e.what()); } } -[[deprecated("Not Implemented")]] void AssemblyConfigLoader::saveRbsConfig() { - -} +[[deprecated("Not Implemented")]] void AssemblyConfigLoader::saveRbsConfig() {} std::vector AssemblyConfigLoader::getSceneModelNames() { std::vector model_names; @@ -79,37 +86,121 @@ std::vector AssemblyConfigLoader::getSceneModelNames() { return model_names; } +tf2_msgs::msg::TFMessage AssemblyConfigLoader::getAdditionalPoses() { + tf2_msgs::msg::TFMessage tp; + if (m_assembly_config.extra_poses.empty()) { + RCLCPP_ERROR(m_logger, "Extra poses is empty"); + return tp; + } + + for (auto &point : m_assembly_config.extra_poses) { + tp.transforms.emplace_back().child_frame_id = point.name; + tp.transforms.emplace_back().transform = createTransform(point.pose); + tp.transforms.emplace_back().header.stamp = m_clock->now(); + tp.transforms.emplace_back().header.frame_id = "world"; + } + + return tp; +} + +tf2_msgs::msg::TFMessage AssemblyConfigLoader::getTfDataAllPossible() { + tf2_msgs::msg::TFMessage tp; + + if (!m_assembly_config.absolute_part.empty()) { + for (auto &abs_part : m_assembly_config.absolute_part) { + geometry_msgs::msg::TransformStamped abs_transrorm_stamped; + abs_transrorm_stamped.transform = createTransform(abs_part.pose); + abs_transrorm_stamped.child_frame_id = abs_part.name; + abs_transrorm_stamped.header.frame_id = "world"; + abs_transrorm_stamped.header.stamp = m_clock->now(); + tp.transforms.push_back(abs_transrorm_stamped); + } + } else { + RCLCPP_ERROR(m_logger, "Absolute parts is empty size: %zu", + m_assembly_config.absolute_part.size()); + } + + if (!m_assembly_config.relative_part.empty()) { + for (const auto &relative_part : m_assembly_config.relative_part) { + geometry_msgs::msg::TransformStamped relative_transform_stamped; + relative_transform_stamped.transform = + createTransform(relative_part.pose); + relative_transform_stamped.child_frame_id = relative_part.name; + relative_transform_stamped.header.frame_id = relative_part.relative_at; + relative_transform_stamped.header.stamp = m_clock->now(); + tp.transforms.push_back(relative_transform_stamped); + RCLCPP_INFO(m_logger, "Model name [%s]", + relative_part.relative_at.c_str()); + } + } + + if (!m_assembly_config.grasp_pose.empty()) { + for (const auto &grasp_pose : m_assembly_config.grasp_pose) { + geometry_msgs::msg::TransformStamped grasp_transform_stamped; + grasp_transform_stamped.transform = createTransform(grasp_pose.pose); + grasp_transform_stamped.child_frame_id = grasp_pose.name; + grasp_transform_stamped.header.stamp = m_clock->now(); + grasp_transform_stamped.header.frame_id = grasp_pose.relative_at; + tp.transforms.push_back(grasp_transform_stamped); + } + } + + return tp; +} + tf2_msgs::msg::TFMessage AssemblyConfigLoader::getTfData(const std::string &model_name) { tf2_msgs::msg::TFMessage tp; + if (!m_assembly_config.absolute_part.empty()) { + for (auto &abs_part : m_assembly_config.absolute_part) { + geometry_msgs::msg::TransformStamped abs_transrorm_stamped; + abs_transrorm_stamped.transform = createTransform(abs_part.pose); + abs_transrorm_stamped.child_frame_id = abs_part.name; + abs_transrorm_stamped.header.frame_id = "world"; + abs_transrorm_stamped.header.stamp = m_clock->now(); + tp.transforms.push_back(abs_transrorm_stamped); + } + } else { + RCLCPP_ERROR(m_logger, "Absolute parts is empty size: %zu", + m_assembly_config.absolute_part.size()); + } + bool found_model = false; bool found_grasp_pose = false; if (!m_assembly_config.relative_part.empty()) { - const auto &absolute_part_pose = m_assembly_config.absolute_part.pose; - tp.transforms.emplace_back().transform = createTransform(absolute_part_pose); - tp.transforms.emplace_back().child_frame_id = "world"; - tp.transforms.emplace_back().header.stamp = m_clock->now(); - for (const auto &relative_part : m_assembly_config.relative_part) { + // Find our model data if (relative_part.name == model_name) { - tp.transforms.emplace_back().transform = createTransform(relative_part.pose); - tp.transforms.emplace_back().child_frame_id = relative_part.relative_at; - tp.transforms.emplace_back().header.stamp = m_clock->now(); + geometry_msgs::msg::TransformStamped relative_transform_stamped; + relative_transform_stamped.transform = + createTransform(relative_part.pose); + relative_transform_stamped.child_frame_id = relative_part.name; + relative_transform_stamped.header.frame_id = relative_part.relative_at; + relative_transform_stamped.header.stamp = m_clock->now(); + tp.transforms.push_back(relative_transform_stamped); found_model = true; } - RCLCPP_INFO(m_logger, "Model name [%s]", relative_part.name.c_str()); + RCLCPP_INFO(m_logger, "Model name [%s]", + relative_part.relative_at.c_str()); } } else { - RCLCPP_ERROR(m_logger, "Relative parts is empty size: %zu", m_assembly_config.relative_part.size()); + RCLCPP_ERROR(m_logger, "Relative parts is empty size: %zu", + m_assembly_config.relative_part.size()); } - for (const auto &grasp_pose : m_assembly_config.grasp_pose) { - if (grasp_pose.name == model_name) { - tp.transforms.emplace_back().transform = createTransform(grasp_pose.pose); - tp.transforms.emplace_back().child_frame_id = grasp_pose.name; - tp.transforms.emplace_back().header.stamp = m_clock->now(); - found_grasp_pose = true; + if (!m_assembly_config.grasp_pose.empty()) { + for (const auto &grasp_pose : m_assembly_config.grasp_pose) { + if (grasp_pose.name == model_name) { + geometry_msgs::msg::TransformStamped grasp_transform_stamped; + grasp_transform_stamped.transform = createTransform(grasp_pose.pose); + grasp_transform_stamped.child_frame_id = grasp_pose.name + "_grasp"; + grasp_transform_stamped.header.stamp = m_clock->now(); + grasp_transform_stamped.header.frame_id = grasp_pose.name; + tp.transforms.push_back(grasp_transform_stamped); + + found_grasp_pose = true; + } } } @@ -117,7 +208,8 @@ AssemblyConfigLoader::getTfData(const std::string &model_name) { RCLCPP_ERROR(m_logger, "Model %s not found in config", model_name.c_str()); } if (!found_grasp_pose) { - RCLCPP_ERROR(m_logger, "Grasp pose not found for model %s", model_name.c_str()); + RCLCPP_ERROR(m_logger, "Grasp pose not found for model %s", + model_name.c_str()); } return tp; @@ -136,42 +228,41 @@ AssemblyConfigLoader::createTransform(const geometry_msgs::msg::Pose &pose) { return transform; } -tf2_msgs::msg::TFMessage AssemblyConfigLoader::getWorkspace() { - tf2_msgs::msg::TFMessage tf_msg; +geometry_msgs::msg::PoseArray AssemblyConfigLoader::getWorkspace() { + geometry_msgs::msg::PoseArray pose_array; + pose_array.header.frame_id = "world"; + if (m_assembly_config.workspace.empty()) { RCLCPP_WARN(m_logger, "Workspace is empty, check your robossembler_db"); - return tf_msg; + return pose_array; } - const std::string child_frame_id = "world"; + pose_array.poses.reserve(m_assembly_config.workspace.size()); + const double default_rotation_value = 0.0; const double default_rotation_w = 1.0; - tf_msg.transforms.reserve(m_assembly_config.workspace.size()); - for (std::size_t i = 0; i < m_assembly_config.workspace.size(); ++i) { - tf_msg.transforms[i].transform.translation.x = - m_assembly_config.workspace[i].x; - tf_msg.transforms[i].transform.translation.y = - m_assembly_config.workspace[i].y; - tf_msg.transforms[i].transform.translation.z = - m_assembly_config.workspace[i].z; - tf_msg.transforms[i].child_frame_id = child_frame_id; - tf_msg.transforms[i].transform.rotation.x = default_rotation_value; - tf_msg.transforms[i].transform.rotation.y = default_rotation_value; - tf_msg.transforms[i].transform.rotation.z = default_rotation_value; - tf_msg.transforms[i].transform.rotation.w = default_rotation_w; - tf_msg.transforms[i].header.stamp = m_clock->now(); + for (const auto &point : m_assembly_config.workspace) { + geometry_msgs::msg::Pose pose; + pose.position.x = point.x; + pose.position.y = point.y; + pose.position.z = point.z; + pose.orientation.x = default_rotation_value; + pose.orientation.y = default_rotation_value; + pose.orientation.z = default_rotation_value; + pose.orientation.w = default_rotation_w; + pose_array.poses.push_back(pose); } - return tf_msg; -} + return pose_array; +} geometry_msgs::msg::PoseArray AssemblyConfigLoader::getWorkspaceInspectorTrajectory() { geometry_msgs::msg::PoseArray pose_array; pose_array.header.frame_id = "world"; auto workspace = getWorkspace(); - for (auto &point : workspace.transforms) { + for (auto &point : workspace.poses) { auto pose = transformTrajectory(point); pose_array.poses.push_back(pose); } @@ -179,13 +270,15 @@ AssemblyConfigLoader::getWorkspaceInspectorTrajectory() { } geometry_msgs::msg::Pose AssemblyConfigLoader::transformTrajectory( - const geometry_msgs::msg::TransformStamped &pose) { - auto pose_eigen = tf2::transformToEigen(pose.transform); + const geometry_msgs::msg::Pose &pose) { + + Eigen::Isometry3d pose_eigen; + tf2::fromMsg(pose, pose_eigen); + Eigen::AngleAxisd rotation(M_PI, Eigen::Vector3d::UnitY()); pose_eigen.rotate(rotation); pose_eigen.translation().z() += 0.35; - auto pose_msg = tf2::toMsg(pose_eigen); - return pose_msg; + return tf2::toMsg(pose_eigen); } } // namespace rbs_utils diff --git a/rbs_utils/rbs_utils_interfaces/msg/AssemblyConfig.msg b/rbs_utils/rbs_utils_interfaces/msg/AssemblyConfig.msg index be4df5a..5172c78 100644 --- a/rbs_utils/rbs_utils_interfaces/msg/AssemblyConfig.msg +++ b/rbs_utils/rbs_utils_interfaces/msg/AssemblyConfig.msg @@ -1,6 +1,7 @@ string assets_db geometry_msgs/Point[] workspace -rbs_utils_interfaces/NamedPose absolute_part +rbs_utils_interfaces/NamedPose[] absolute_part rbs_utils_interfaces/RelativeNamedPose[] relative_part -rbs_utils_interfaces/NamedPose[] grasp_pose +rbs_utils_interfaces/RelativeNamedPose[] grasp_pose +rbs_utils_interfaces/NamedPose[] extra_poses