Skip to content

Commit 6f11575

Browse files
committed
✨ reading manifest path also from rosparam
1 parent ea5e2e4 commit 6f11575

File tree

1 file changed

+15
-14
lines changed

1 file changed

+15
-14
lines changed

jason_ros/src/jason_ros/hw_bridge.py

Lines changed: 15 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
#!/usr/bin/env python
22
from jason_ros.hw_controller import *
33
import rospy
4+
import rosparam
45
import argparse
56
import std_msgs.msg
67
import jason_ros_msgs.msg
@@ -15,7 +16,7 @@ def arg_parser():
1516
parser.add_argument("-p", "--perception",
1617
help="Perception manifest path", nargs=1, type=str)
1718
parser.add_argument(
18-
"-r", "--param", help="Rosparam .yaml", nargs=1, type=str)
19+
"-r", "--param", help="Rosparam.yaml", nargs=1, type=str)
1920

2021
args, unknown = parser.parse_known_args()
2122
return vars(args)
@@ -38,22 +39,27 @@ def main():
3839
print("Starting HwBridge node.")
3940
rospy.init_node('HwBridge')
4041

42+
actions_manifest_file = rospy.get_param(rospy.get_name() + "/actions_manifest", "actions_manifest")
43+
perceptions_manifest_file = rospy.get_param(rospy.get_name() + "/perceptions_manifest", "perceptions_manifest")
44+
agent_name = rospy.get_param(rospy.get_name() + "/agent_name", "")
45+
4146
args = arg_parser()
4247
if args["param"] is not None:
4348
import yaml
44-
import rosparam
4549
with open(args["param"][0], 'r') as stream:
4650
try:
4751
yaml_file = yaml.safe_load(stream)
4852
rosparam.upload_params("/", yaml_file)
4953
except yaml.YAMLError as exc:
5054
print(exc)
5155

52-
action_controller = ActionController()
5356
if args["action"] is not None:
54-
action_controller.read_manifest(args["action"][0])
55-
else:
56-
action_controller.read_manifest()
57+
actions_manifest_file = args["action"][0]
58+
if args["perception"] is not None:
59+
perceptions_manifest_file = args["perception"][0]
60+
61+
action_controller = ActionController()
62+
action_controller.read_manifest(actions_manifest_file)
5763

5864
jason_actions_status_pub = rospy.Publisher(
5965
'jason/actions_status',
@@ -64,15 +70,10 @@ def main():
6470
jason_action_sub = rospy.Subscriber(
6571
'jason/actions',
6672
jason_ros_msgs.msg.Action,
67-
act, (action_controller, jason_actions_status_pub))
68-
69-
perception_controller = PerceptionController()
70-
71-
if args["perception"] is not None:
72-
perception_controller.read_manifest(args["perception"][0])
73-
else:
74-
perception_controller.read_manifest()
73+
act, (action_controller, jason_actions_status_pub, agent_name))
7574

75+
perception_controller = PerceptionController(agent_name)
76+
perception_controller.read_manifest(perceptions_manifest_file)
7677
perception_controller.start_perceiving()
7778

7879
jason_percepts_pub = rospy.Publisher(

0 commit comments

Comments
 (0)