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

json with parameters for calibrate

Browse source
This commit is contained in:
shalenikol 2023-12-01 15:09:08 +03:00
parent cb3082ca44
commit 8a1f2aa4dc
4 changed files with 40 additions and 2 deletions
Download patch file Download diff file Expand all files Collapse all files

4
rbs_bt_executor/bt_trees/test_tree.xml
View file

@ -3,8 +3,8 @@
<BehaviorTree ID="PoseEstimation">
<Sequence>
<Action ID="PoseEstimation"
ObjectName="fork"
ReqKind = "configure"
ObjectName="!/home/$USERNAME/robossembler_ws/src/robossembler-ros2/rbs_perception/config/str_param.json"
ReqKind = "calibrate"
server_name="/pose_estimation/change_state"
server_timeout="1000"/>
<Action ID="PoseEstimation"

17
rbs_bt_executor/bt_trees/test_tree_pe_configure.xml Normal file
View file

@ -0,0 +1,17 @@
<?xml version="1.0"?>
<root main_tree_to_execute="PoseEstimation">
<BehaviorTree ID="PoseEstimation">
<Sequence>
<Action ID="PoseEstimation"
ObjectName="fork"
ReqKind = "configure"
server_name="/pose_estimation/change_state"
server_timeout="1000"/>
<Action ID="PoseEstimation"
ObjectName=""
ReqKind = "activate"
server_name="/pose_estimation/change_state"
server_timeout="1000"/>
</Sequence>
</BehaviorTree>
</root>

4
rbs_perception/config/str_param.json Normal file
View file

@ -0,0 +1,4 @@
{
"mesh_path":"/home/$USERNAME/robossembler_ws/src/robossembler-ros2/rbs_perception/config/fork.ply",
"gtpose":[1.3,0.0,0.0,0.0,0.0,0.0]
}

17
rbs_perception/scripts/pose_estimation_lifecycle.py
View file

@ -199,6 +199,23 @@ class PoseEstimator(Node):
mesh_path = y["mesh_path"]
if "gtpose" in y:
gtpose = y["gtpose"]
elif str_param[0] == "!": # filename json
json_file = str_param[1:]
if not os.path.isfile(json_file):
self.get_logger().info("not JSon '"+ json_file +"'")
return TransitionCallbackReturn.FAILURE
str_param = Path(json_file).read_text()
try:
y = json.loads(str_param)
except json.JSONDecodeError as e:
self.get_logger().info(f"JSon error: {e}")
return TransitionCallbackReturn.FAILURE
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"]
else:
mesh_path = str_param