...
 
Commits (2)
......@@ -6,7 +6,7 @@
<arg name="datadir" default="$(optenv HOME /home/soccred)/data" />
<!-- load previous experiments' metadata -->
<rosparam command="load" file="$(arg datadir)/params_all.yaml" />
<rosparam command="load" file="$(arg datadir)/params_all.yaml" ns="/safety_security" />
<!-- recording -->
<include file="$(find record_ros)/launch/record_ros.launch">
......
......@@ -120,25 +120,25 @@ class ExperimentControl(object):
self.experiment_id = None
self.condition = None
rospy.loginfo("Subscribing to /safety_security/cmd")
rospy.Subscriber('/safety_security/cmd', String, self._callback)
rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, self._posecb)
self.notify_pub = rospy.Publisher('/safety_security/notify', PopupMsg, queue_size=1)
self.cmd_pub = rospy.Publisher('/safety_security/cmd', String, queue_size=1)
self.actuators = Actuators()
self.sound_path = rospy.get_param("~sound_path", "/tmp")
rospy.loginfo("[SCRIPT] Ready.")
def _callback(self, msgdata):
cmd = msgdata.data
if "stop" in self.signal and not "init" in self.signal:
rospy.logwarn("Ignored signal '%s' while in state '%s'", cmd, self.signal)
rospy.logwarn("[SCRIPT] Ignored signal '%s' while in state '%s'", cmd, self.signal)
else:
rospy.loginfo("Received signal, changing state from '%s' -> '%s'", self.signal, cmd)
rospy.loginfo("[SCRIPT] Received signal, changing state from '%s' -> '%s'", self.signal, cmd)
self.signal = cmd
def _posecb(self, msgdata):
self.position = msgdata.pose.pose.position
rospy.logdebug("Updated position to '%s,%s'.", self.position.x, self.position.y)
rospy.logdebug("[SCRIPT] Updated position to '%s,%s'.", self.position.x, self.position.y)
def say_item(self, item, wait=2):
......@@ -158,7 +158,7 @@ class ExperimentControl(object):
if not phrase is None:
self.say_item(phrase + "/" + self.condition)
rospy.loginfo("EXPERIMENT PHASE ID '%s', COND '%s': PUZZLE/INSPECT(%s).", self.experiment_id, self.condition, item)
rospy.loginfo("[SCRIPT] EXPERIMENT PHASE ID '%s', COND '%s': PUZZLE/INSPECT(%s).", self.experiment_id, self.condition, item)
self.check_result(self.head.look_at("straight"), "head")
self.check_result(self.arm.tuck_position(retries=3), "arm")
self.check_result(self.base.goto("standby"), "base")
......@@ -166,10 +166,10 @@ class ExperimentControl(object):
def check_result(self, result, component="robot"):
if not result is None:
if type(result) == bool and not result:
rospy.logwarn("Error moving '%s', aborting.", component)
rospy.logwarn("[SCRIPT] Error moving '%s', aborting.", component)
raise StoppedException()
elif type(result) == MoveBaseActionResult and not result.status == GoalStatus.SUCCEEDED:
rospy.logwarn("Error moving '%s: '%s', aborting.", component, result.status)
rospy.logwarn("[SCRIPT] Error moving '%s: '%s', aborting.", component, result.status)
raise StoppedException()
assert (not "stop" in self.signal)
......@@ -187,13 +187,13 @@ class ExperimentControl(object):
self.await_signal(["init"])
if not rospy.has_param("/safety_security/experiment_id"):
rospy.logerr("parameter '/safety_security/experiment_id' not present, aborting.")
rospy.logerr("[SCRIPT] Parameter '/safety_security/experiment_id' not present, aborting.")
raise InsufficientInformationException()
else:
self.experiment_id = rospy.get_param('/safety_security/experiment_id')
if not rospy.has_param("/safety_security/experiment_condition"):
rospy.logerr("parameter '/safety_security/experiment_condition' not present, aborting.")
rospy.logerr("[SCRIPT] Parameter '/safety_security/experiment_condition' not present, aborting.")
raise InsufficientInformationException()
else:
self.condition = rospy.get_param('/safety_security/experiment_condition')
......@@ -228,7 +228,7 @@ class ExperimentControl(object):
self.await_signal()
def set_state(self, cur_state):
rospy.loginfo("EXPERIMENT ID '%s', COND '%s' ENTER PHASE: %s -> %s.", self.experiment_id, self.condition, self.state, cur_state)
rospy.loginfo("[SCRIPT] EXPERIMENT ID '%s', COND '%s' ENTER PHASE: %s -> %s.", self.experiment_id, self.condition, self.state, cur_state)
rospy.set_param('/safety_security/experiment_trials/' + self.experiment_id + '/state', cur_state)
self.state = cur_state
......@@ -322,7 +322,7 @@ class ExperimentControl(object):
assert (not "stop" in self.signal)
def await_signal(self, sig=["ignored", "acted"]):
rospy.loginfo("Awaiting signal '%s'...", sig)
rospy.loginfo("[SCRIPT] Awaiting signal '%s'...", sig)
while not rospy.is_shutdown():
if "stop" in self.signal:
raise StoppedException()
......@@ -331,7 +331,7 @@ class ExperimentControl(object):
else:
for si in sig:
if si in self.signal:
rospy.loginfo("Accepted signal '%s'.", self.signal)
rospy.loginfo("[SCRIPT] Accepted signal '%s'.", self.signal)
self.signal = "";
return si
......@@ -371,12 +371,12 @@ class ExperimentControl(object):
except (AssertionError, StoppedException, InsufficientInformationException, Exception):
self.signal = ""
if not self.experiment_id is None:
rospy.logwarn("Aborting experiment id '%s', condition '%s'", self.experiment_id, self.condition)
rospy.logwarn("[SCRIPT] Aborting experiment id '%s', condition '%s'", self.experiment_id, self.condition)
self.set_state("ABORTED(%s)" % self.state)
msg.data = "abort/script"
self.cmd_pub.publish(msg)
else:
rospy.logwarn("No experiment active, ignoring abort.")
rospy.logwarn("[SCRIPT] No experiment active, ignoring abort.")
if __name__ == "__main__":
rospy.init_node("safety_security_experiment_flow")
......
......@@ -28,10 +28,6 @@ utterance_param({"/script_server/utterances/"})
setMsgList(empty_list);
setSoundEnabled(true);
setRunning(false);
std::string trial_data_file = get_dump_file(false);
ROS_INFO("[CTRLIF] Loading previous trial parameters. Executing 'rosparam load %s /safety_security'", trial_data_file.c_str());
system(("rosparam load " + trial_data_file + " /safety_security").c_str());
}
void ROSControl::cmd_received(const std_msgs::String::ConstPtr& msg)
......