diff --git a/robossembler_scene_monitor/scene_monitor/CMakeLists.txt b/robossembler_scene_monitor/scene_monitor/CMakeLists.txt index e76072f..2ae81e6 100644 --- a/robossembler_scene_monitor/scene_monitor/CMakeLists.txt +++ b/robossembler_scene_monitor/scene_monitor/CMakeLists.txt @@ -32,12 +32,18 @@ add_library(component_state_lib SHARED src/component.cpp src/component_state_monitor.cpp ) + ament_target_dependencies(component_state_lib ${dependencies}) add_executable(component_state_monitor_node src/component_state_monitor_node.cpp) target_link_libraries(component_state_monitor_node component_state_lib) ament_target_dependencies(component_state_monitor_node ${dependencies}) +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME} +) + install(TARGETS component_state_lib ARCHIVE DESTINATION lib diff --git a/robossembler_scene_monitor/scene_monitor/include/component.hpp b/robossembler_scene_monitor/scene_monitor/include/component.hpp index edb55aa..4fd2d9f 100644 --- a/robossembler_scene_monitor/scene_monitor/include/component.hpp +++ b/robossembler_scene_monitor/scene_monitor/include/component.hpp @@ -3,8 +3,6 @@ #include #include #include "scene_monitor_interfaces/msg/grasp_pose.hpp" -// #include "scene_monitor_interfaces/msg/bounding_box.hpp" -// #include "scene_monitor_interfaces/msg/principal_properties.hpp" #include "nlohmann_json/json.hpp" namespace component_state_monitor @@ -44,25 +42,16 @@ public: void setFrameName(const std::string &frame_name); void initializeFromJson(json &input); - /* - TODO - - create methods for get all recently needed component - from metadata class - */ void updateState(const ComponentState &state); private: - // float _volume; std::string _frame_name; ComponentState _current_state; geometry_msgs::msg::Pose _placement; - // geometry_msgs::msg::Point _center_of_mass; std::map _grasp_poses; - // scene_monitor_interfaces::msg::BoundingBox _bounding_box; - // scene_monitor_interfaces::msg::PrincipalProperties _principal_properties; }; diff --git a/robossembler_scene_monitor/scene_monitor/include/component_state_monitor.hpp b/robossembler_scene_monitor/scene_monitor/include/component_state_monitor.hpp index 24e20a0..9858517 100644 --- a/robossembler_scene_monitor/scene_monitor/include/component_state_monitor.hpp +++ b/robossembler_scene_monitor/scene_monitor/include/component_state_monitor.hpp @@ -25,6 +25,7 @@ public: ~ComponentStateMonitor(); void initializeComponents(const std::string &package_name); + void initializeParameters(); private: @@ -40,5 +41,8 @@ private: rclcpp::Service::SharedPtr _get_grasp_pose_service; rclcpp::Service::SharedPtr _get_grasp_poses_service; + std::string _models_package; + rclcpp::TimerBase::SharedPtr _timer; + }; } \ No newline at end of file diff --git a/robossembler_scene_monitor/scene_monitor/include/scene_state_monitor.hpp b/robossembler_scene_monitor/scene_monitor/include/scene_state_monitor.hpp index e69de29..8f67de2 100644 --- a/robossembler_scene_monitor/scene_monitor/include/scene_state_monitor.hpp +++ b/robossembler_scene_monitor/scene_monitor/include/scene_state_monitor.hpp @@ -0,0 +1,20 @@ +#include "component_state_monitor.hpp" + +namespace scene_state_monitor +{ + + +class SceneStateMonitor: public rclcpp::Node +{ + +public: + + SceneStateMonitor(); + ~SceneStateMonitor(); + +private: + + component_state_monitor::ComponentStateMonitor _component_state_monitor_node; + +}; +} \ No newline at end of file diff --git a/robossembler_scene_monitor/scene_monitor/launch/scene_monitor.launch.py b/robossembler_scene_monitor/scene_monitor/launch/scene_monitor.launch.py new file mode 100644 index 0000000..8e69521 --- /dev/null +++ b/robossembler_scene_monitor/scene_monitor/launch/scene_monitor.launch.py @@ -0,0 +1,17 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + ld = LaunchDescription() + component_state_monitor_node = Node( + package="robossembler_scene_monitor", + executable="component_state_monitor_node", + name="component_state_monitor_node", + output="screen", + emulate_tty=True, + parameters=[ + {"models-package": "sdf_models"} + ] + ) + ld.add_action(component_state_monitor_node) + return ld \ No newline at end of file diff --git a/robossembler_scene_monitor/scene_monitor/src/component_state_monitor.cpp b/robossembler_scene_monitor/scene_monitor/src/component_state_monitor.cpp index f3a1a4e..68f7ca7 100644 --- a/robossembler_scene_monitor/scene_monitor/src/component_state_monitor.cpp +++ b/robossembler_scene_monitor/scene_monitor/src/component_state_monitor.cpp @@ -1,6 +1,7 @@ #include #include #include +#include #include @@ -8,8 +9,11 @@ namespace component_state_monitor { + using namespace std::chrono_literals; ComponentStateMonitor::ComponentStateMonitor(): Node("component_state_monitor_node") - { + { + this->declare_parameter("models-package", "sdf_models"); + RCLCPP_INFO(this->get_logger(), "Initialize [get_part] service"); _get_part_service = this->create_service("scene_monitor/get_part", std::bind(&ComponentStateMonitor::getComponent, this, @@ -25,8 +29,15 @@ namespace component_state_monitor std::bind(&ComponentStateMonitor::getGraspPose, this, std::placeholders::_1, std::placeholders::_2)); - std::string package_name = "sdf_models"; - initializeComponents(package_name); + + initializeParameters(); + initializeComponents(_models_package); + } + + void ComponentStateMonitor::initializeParameters() + { + this->get_parameter("models-package", _models_package); + RCLCPP_INFO(this->get_logger(), "Initialize with models-package (%s)", _models_package.c_str()); } ComponentStateMonitor::~ComponentStateMonitor() {} @@ -131,7 +142,7 @@ namespace component_state_monitor component.setFrameName(frame_name); } else { - RCLCPP_INFO(this->get_logger(), "Start read frame metadata (%s)", json_path.c_str()); + RCLCPP_INFO(this->get_logger(), "Start read frame metadata (%s/frames.json)", frame_name.c_str()); std::ifstream json_file(json_path); auto input = json::parse(json_file); diff --git a/robossembler_scene_monitor/scene_monitor/src/component_state_monitor_node.cpp b/robossembler_scene_monitor/scene_monitor/src/component_state_monitor_node.cpp index 0b79361..f3a4764 100644 --- a/robossembler_scene_monitor/scene_monitor/src/component_state_monitor_node.cpp +++ b/robossembler_scene_monitor/scene_monitor/src/component_state_monitor_node.cpp @@ -1,5 +1,4 @@ #include - // #include "SceneMonitor.hpp" #include "component_state_monitor.hpp" @@ -10,6 +9,7 @@ int main(int argc, char **argv) rclcpp::executors::SingleThreadedExecutor exec; auto component_state_monitor_node = std::make_shared(); + exec.add_node(component_state_monitor_node); exec.spin(); diff --git a/robossembler_scene_monitor/scene_monitor/utils/pose_estimation/src/pose_estimation.cpp b/robossembler_scene_monitor/scene_monitor/utils/pose_estimation/src/pose_estimation.cpp index fff8727..e0a62b7 100644 --- a/robossembler_scene_monitor/scene_monitor/utils/pose_estimation/src/pose_estimation.cpp +++ b/robossembler_scene_monitor/scene_monitor/utils/pose_estimation/src/pose_estimation.cpp @@ -29,7 +29,6 @@ geometry_msgs::msg::Pose PoseEstimation::frame_pose() auto future_response = _gz_state_client->async_send_request( request, [this, request](rclcpp::Client::SharedFuture response) { - RCLCPP_INFO(this->get_logger(), "hi from response!"); auto result = response.get(); if (!result->success) { diff --git a/robossembler_scene_monitor/scene_monitor_interfaces/msg/GraspPose.msg b/robossembler_scene_monitor/scene_monitor_interfaces/msg/GraspPose.msg index 13694f7..f95dd4f 100644 --- a/robossembler_scene_monitor/scene_monitor_interfaces/msg/GraspPose.msg +++ b/robossembler_scene_monitor/scene_monitor_interfaces/msg/GraspPose.msg @@ -1,11 +1,3 @@ -# float64 radius string label -# string part -# string shape_type -# string positioning -geometry_msgs/Pose placement_pose -float64 distance -# geometry_msgs/Point axis -# geometry_msgs/Point center -# geometry_msgs/Pose feature_placement -# float64[] parameter_range \ No newline at end of file +geometry_msgs/Pose placement_pose # TODO: tf2 +float64 distance \ No newline at end of file