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

Merge remote-tracking branch 'origin/main' into 90-real-robot-setup

Browse source
This commit is contained in:
Ilya Uraev 2023-12-22 19:14:18 +03:00
commit af6b20b622
4 changed files with 19 additions and 20 deletions
Download patch file Download diff file Expand all files Collapse all files

3
rbs_bt_executor/bt_trees/test_tree.xml
View file

@ -3,7 +3,7 @@
<BehaviorTree ID="PoseEstimation">
<Sequence>
<Action ID="PoseEstimation"
ObjectName="!/rbs_ws/src/robossembler-ros2/rbs_perception/config/str_param.json"
ObjectName="!/home/shalenikol/robossembler_ws/src/robossembler-ros2/rbs_perception/config/str_param.json"
ReqKind = "calibrate"
server_name="/pose_estimation/change_state"
server_timeout="1000"/>
@ -14,4 +14,3 @@
server_timeout="1000"/>
</Sequence>
</BehaviorTree>
</root>

9
rbs_bt_executor/src/PoseEstimation.cpp
View file

@ -101,6 +101,15 @@ private:
_set_params_client->set_parameters(_parameters);
}
void set_mesh_param() {
auto _package_name = ament_index_cpp::get_package_share_directory("rbs_perception");
_model_path = build_model_path(_model_name, _package_name);
RCLCPP_INFO_STREAM(_node->get_logger(), _model_path);
std::vector<rclcpp::Parameter> _parameters{rclcpp::Parameter("mesh_path", _model_name)};
_set_params_client->set_parameters(_parameters);
}
void set_mesh_param() {
auto _package_name =
ament_index_cpp::get_package_share_directory("rbs_perception");

26
rbs_perception/scripts/pose_estimation_lifecycle.py
View file

@ -197,14 +197,6 @@ class PoseEstimator(Node):
"""
gtpose = None
str_param = self.get_parameter("mesh_path").get_parameter_value().string_value
# if str_param[0] == "{": # json string
# y = json.loads(str_param)
# if "mesh_path" not in y:
# self.get_logger().info("JSon 'mesh_path' not set")
# return TransitionCallbackReturn.FAILURE
# mesh_path = y["mesh_path"]
# if "gtpose" in y:
# gtpose = y["gtpose"]
if str_param[0] == "!": # filename json
json_file = str_param[1:]
if not os.path.isfile(json_file):
@ -342,9 +334,14 @@ class PoseEstimator(Node):
[k_[3], k_[4]*2.0, data.height / 2.0], # k_[5]], #
[k_[6], k_[7], k_[8]] #self.mesh_scale]
]
# self._K = [
# [k_[0]*2.0, k_[1], data.width / 2.0], # k_[2]], #
# [k_[3], k_[4]*2.0, data.height / 2.0], # k_[5]], #
# [k_[6], k_[7], k_[8]] #self.mesh_scale]
# ]
# *** for test
tPath = self.objPath / "inputs"
#{"label": "fork", "bbox_modal": [329, 189, 430, 270]}
output_fn = tPath / "object_data.json"
output_json_dict = {
"label": self.objName,
@ -353,6 +350,7 @@ class PoseEstimator(Node):
data = []
data.append(output_json_dict)
output_fn.write_text(json.dumps(data))
# ***
#{"K": [[25.0, 0.0, 8.65], [0.0, 25.0, 6.5], [0.0, 0.0, 1.0]], "resolution": [480, 640]}
output_fn = self.objPath / "camera_data.json"
@ -371,8 +369,6 @@ class PoseEstimator(Node):
"""
Depth image callback function.
"""
#self.get_logger().info("Receiving depth image")
# Convert ROS Image message to OpenCV image
current_frame = self.br.imgmsg_to_cv2(data)
@ -414,7 +410,6 @@ class PoseEstimator(Node):
bbox = [2, 2, self._res[1]-4, self._res[0]-4]
#tPath = path_to / "inputs"
#{"label": "fork", "bbox_modal": [329, 189, 430, 270]}
output_fn = path_to / "inputs/object_data.json"
output_json_dict = {
"label": self.objName,
@ -462,8 +457,6 @@ class PoseEstimator(Node):
self.get_logger().info(f"darknet: begin {self._image_cnt}")
result_json = str(self.objPath / "res.json")
#with open("log_darknet.txt", "w") as logf:
#self.get_logger().is_enabled_for(LoggingSeverity.WARN)
subprocess.run([darknet_bin, "detector", "test",
"yolov4_objs2.data",
"yolov4_objs2.cfg",
@ -475,11 +468,9 @@ class PoseEstimator(Node):
detected = self.yolo2megapose(result_json, self.objPath)
self.get_logger().info(f"darknet: end {self._image_cnt}")
#if detected and self.megapose_model:
if self.megapose_model:
if detected and self.megapose_model:
# 6D pose estimation
self.get_logger().info(f"megapose: begin {self._image_cnt} {self.objPath}")
#run_inference(self.objPath,"megapose-1.0-RGB-multi-hypothesis")
from megapose.scripts.run_inference_on_example import run_inference_rbs
run_inference_rbs(self.megapose_model)
@ -501,4 +492,3 @@ def main():
lc_node.destroy_node()
if __name__ == '__main__':
main()

1
rbs_simulation/launch/simulation.launch.py
View file

@ -80,6 +80,7 @@ def generate_launch_description():
'/outer_rgbd_camera/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked'
],
output='screen',
condition=IfCondition(sim_gazebo)
)
rgbd_bridge_in = Node(