add launch file

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

robossembler_scene_monitor/scene_monitor/include/component.hpp

@@ -3,8 +3,6 @@
 #include <string>
 #include <geometry_msgs/msg/pose.hpp>
 #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<std::string, scene_monitor_interfaces::msg::GraspPose> _grasp_poses;
-    // scene_monitor_interfaces::msg::BoundingBox _bounding_box;
-    // scene_monitor_interfaces::msg::PrincipalProperties _principal_properties;
 
 };
 

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<GetGraspPoseSrv>::SharedPtr _get_grasp_pose_service;
     rclcpp::Service<GetGraspPosesSrv>::SharedPtr _get_grasp_poses_service;
 
+    std::string _models_package;
+    rclcpp::TimerBase::SharedPtr _timer;
+
 };
 }

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;
+
+};
+}

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

robossembler_scene_monitor/scene_monitor/src/component_state_monitor.cpp

@@ -1,6 +1,7 @@
 #include <filesystem>
 #include <fstream>
 #include <sstream>
+#include <chrono>
 
 #include <ament_index_cpp/get_package_share_directory.hpp>
 
@@ -8,8 +9,11 @@
 
 namespace component_state_monitor
 {
+    using namespace std::chrono_literals;
     ComponentStateMonitor::ComponentStateMonitor(): Node("component_state_monitor_node")
-    {
+    {   
+        this->declare_parameter<std::string>("models-package", "sdf_models");
+
         RCLCPP_INFO(this->get_logger(), "Initialize [get_part] service");
         _get_part_service = this->create_service<GetPartSrv>("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);

robossembler_scene_monitor/scene_monitor/src/component_state_monitor_node.cpp

@@ -1,5 +1,4 @@
 #include <rclcpp/rclcpp.hpp>
-
 // #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<component_state_monitor::ComponentStateMonitor>();
+
     exec.add_node(component_state_monitor_node);
     exec.spin();
 

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<gazebo_msgs::srv::GetEntityState>::SharedFuture response) 
         {
-            RCLCPP_INFO(this->get_logger(), "hi from response!");
             auto result = response.get();
             if (!result->success)
             {

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
+geometry_msgs/Pose placement_pose # TODO: tf2
+float64 distance