diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..a09790b --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +.vscode/c_cpp_properties.json +.vscode/settings.json +tasker/__pycache__/* diff --git a/CMakeLists.txt b/CMakeLists.txt index 17f7748..54e8114 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,62 +1,32 @@ -cmake_minimum_required(VERSION 2.8.3) - +cmake_minimum_required(VERSION 3.5) project(tasker) -# Get the information about this package's buildtime dependencies - find_package(catkin REQUIRED - COMPONENTS tasker_msgs message_generation std_msgs geometry_msgs rospy) - - catkin_python_setup() -if(USE_PDDL) -execute_process(COMMAND bash ${CMAKE_CURRENT_SOURCE_DIR}/compile-pddl-planner.sh - - WORKIN_DIRECTORY ${CATKIN_PACKAGE_SOURCE_DIR} - ) -endif() - -#message( ${CMAKE_CURRENT_SOURCE_DIR} ) -#add_custom_command(TARGET VAL -# PRE_BUILD -# COMMAND make clean -# COMMAND make -# WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/../VAL -# ) -#add_custom_command(TARGET universal-pddl-parser -# PRE_BUILD -# COMMAND scons -# WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/../universal-pddl-parser -# ) -#add_custom_command(TARGET temporal-planning -# PRE_BUILD -# COMMAND python fd_copy/build.py release64 -# COMMAND scons -# WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/../temporal-planning -# ) - - - # Actually generate the language-specific message and service files -# Catkin package - catkin_package( - INCLUDE_DIRS - CATKIN_DEPENDS rospy - DEPENDS ) - - - - # define executable using MyMessage1 etc. - #add_executable(message_program src/main.cpp) - #add_dependencies(message_program ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - - # define executable not using any messages/services provided by this package - #add_executable(does_not_use_local_messages_program src/main.cpp) - #add_dependencies(does_not_use_local_messages_program ${catkin_EXPORTED_TARGETS}) - - catkin_install_python(PROGRAMS - bin/exampleState - bin/exemplary_susp_task - bin/txt_guideHuman - bin/txt_hazardDetection - bin/txt_human_fell - bin/harmonization_table_generator - bin/txt_test - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +# if(POLICY CMP0148) +# cmake_policy(SET CMP0148 NEW) +# endif() + +# Find packages in ROS 2 +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclpy REQUIRED) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tasker_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +# Install Python script +install(PROGRAMS + tasker/task_harmonizer.py + DESTINATION lib/${PROJECT_NAME} +) + +# Install Python modules +ament_python_install_package(tasker PACKAGE_DIR src/TaskER) + +# Install launch files +install( + DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + +ament_package() \ No newline at end of file diff --git a/bin/control_human.py b/bin/control_human.py deleted file mode 100755 index f206f82..0000000 --- a/bin/control_human.py +++ /dev/null @@ -1,271 +0,0 @@ -#!/usr/bin/env python -import roslib -import rospy -import rospkg -import math -import tf -import os, signal -import turtlesim.msg -from geometry_msgs.msg import Twist, Quaternion -from visualization_msgs.msg import Marker -from tasker_msgs.srv import * -from tiago_msgs.msg import Command -from gazebo_msgs.msg import ModelState -from tf.transformations import * -import rcprg_kb.places_xml as kb_p -global vel - -vel = Twist() -def handle_actor_vel(msg): - global vel - vel = msg - -if __name__ == '__main__': - global vel - places_xml_filename = rospy.get_param('/kb_places_xml') - sim_mode = str(rospy.get_param('/sim_mode')) - assert sim_mode in ['sim', 'gazebo', 'real'] - if sim_mode in ['sim', 'gazebo']: - map_context = 'sim' - else: - map_context = 'real' - print 'Reading KB for places from file "' + places_xml_filename + '"' - kb_places = kb_p.PlacesXmlParser(places_xml_filename).getKB() - vel = Twist() - rospy.init_node('control_human',anonymous=True) - actor_name = rospy.get_param('~actor_name') - if not rospy.has_param('/last_actor_id'): - actor_id = 0 - else: - last_actor_id = rospy.get_param('/last_actor_id') - actor_id = last_actor_id+1 - actor_gender = rospy.get_param('~actor_gender') - rospy.set_param('/last_actor_id', actor_id) - # human_transform = rospy.get_param('~actor_init_pose') - pl = kb_places.getPlaceByName(actor_name, map_context) - if pl.getType() == 'point': - pt_dest = pl.getPt() - norm = pl.getN() - angle_dest = math.atan2(norm[1], norm[0]) - print "angle_dest: ", angle_dest - pt = pt_dest - pt_dest = (pt_dest[0], pt_dest[1]) - print "pt_dest: ", pt_dest - print 'UnderstandGoal place type: point' - print 'pt: {}, pt_dest: {}, norm: {}, angle_dest: {}'.format(pt, pt_dest, norm, angle_dest) - else: - os.kill(pid, signal.SIGHUP) - human_transform = [pt_dest[0], pt_dest[1], angle_dest] - rospy.loginfo("Setting actor_init_pose: %f, %f, %f" % (human_transform[0], human_transform[1], human_transform[2])) - rospy.Subscriber('/%s/vel' % actor_name, - Twist, - handle_actor_vel - ) - actor_posture = rospy.set_param(actor_name+"/actor_posture", "stand") - br = tf.TransformBroadcaster() - marker_pub = rospy.Publisher("ellipse", Marker, queue_size=10) - gazebo_control_pub = rospy.Publisher("/gazebo/set_model_state", ModelState, queue_size=10) - cmd_pub = rospy.Publisher("/rico_cmd", Command, queue_size=10) - gazebo_model_state = ModelState() - x_str = '\'x\': {} '.format(human_transform[0]) - y_str = '\'y\': {} '.format(human_transform[1]) - theta_str = '\'theta\': {} '.format(angle_dest) - rospy.set_param(actor_name+'/pose', '{'+x_str+y_str+theta_str+'}') #'x': \"%f\", 'y': \"%f\", 'theta': \"%f\"}" % human_transform[0] human_transform[1] angle_dest) - gazebo_model_state.model_name="John" - gazebo_model_state.pose.position.x = human_transform[0] - gazebo_model_state.pose.position.y = human_transform[1] - current_gz_rot = Quaternion() - gazebo_model_state.pose.orientation = current_gz_rot - marker = Marker() - marker.header.frame_id = "map" - marker.header.stamp = rospy.Time.now() - marker.ns = "human" - marker.id = actor_id - marker.type = Marker.MESH_RESOURCE - marker.action = Marker.ADD - rospack = rospkg.RosPack() - - # list all packages, equivalent to rospack list - rospack.list() - - # get the file path for rospy_tutorials - tasker_path = rospack.get_path('tasker') - if actor_gender == "male": - marker.mesh_resource = "file://"+tasker_path+"/makehuman/Male/male.dae" - else: - marker.mesh_resource = "file://"+tasker_path+"/makehuman/Female/female.dae" - marker.pose.position.x = human_transform[0] - marker.pose.position.y = human_transform[1] - marker.pose.position.z = 0 - marker.pose.orientation.x = 0.5 - marker.pose.orientation.y = 0.5 - marker.pose.orientation.z = 0.5 - marker.pose.orientation.w = 0.5 - marker.scale.x = 1 - marker.scale.y = 1 - marker.scale.z = 1 - marker.color.a = 1.0 - marker.color.r = 0.0 - marker.color.g = 1.0 - marker.color.b = 0.0 - marker.lifetime = rospy.Duration() - - marker_name = Marker() - marker_name.header.frame_id = "/map" - marker_name.header.stamp = rospy.Time.now() - marker_name.ns = "names" - marker_name.id = actor_id - marker_name.type = Marker.TEXT_VIEW_FACING - marker_name.action = Marker.ADD - - marker_name.pose.position.x = human_transform[0] - marker_name.pose.position.y = human_transform[1] - marker_name.pose.position.z = 2 - marker_name.pose.orientation.x = 0.0 - marker_name.pose.orientation.y = 0.0 - marker_name.pose.orientation.z = 0.0 - marker_name.pose.orientation.w = 1.0 - - marker_name.text = actor_name - - marker_name.scale.z = 0.7 - - marker_name.color.r = 0.0 - marker_name.color.g = 1.0 - marker_name.color.b = 0.0 - marker_name.color.a = 1.0 - - marker_pub.publish(marker) - marker_pub.publish(marker_name) - task_reqested = False - while not rospy.is_shutdown(): - actor_posture = rospy.get_param(actor_name+"/actor_posture") - marker_pub.publish(marker) - if actor_posture == "fell": - rot = quaternion_multiply([current_gz_rot.x,current_gz_rot.y,current_gz_rot.z,current_gz_rot.w],quaternion_from_euler(-1.54, 0, 0)) - gazebo_model_state.pose.orientation = Quaternion(rot[0],rot[1],rot[2],rot[3]) - fell_cmd = Command() - fell_cmd.query_text="" - fell_cmd.intent_name = "HF" - fell_cmd.param_names = ["human_name"] - fell_cmd.param_values = [actor_name] - marker.ns = "human" - marker.id = actor_id - marker.action = Marker.MODIFY - if actor_gender == "male": - marker.mesh_resource = "file://"+tasker_path+"/makehuman/Male/male_fell.dae" - else: - marker.mesh_resource = "file://"+tasker_path+"/makehuman/Female/female_fell.dae" - if task_reqested == False: - cmd_pub.publish(fell_cmd) - task_reqested= True - marker.pose.position.x = human_transform[0] - marker.pose.position.y = human_transform[1] - marker.pose.position.z = -0.5 - marker_name.pose.position.z = 1 - marker.pose.orientation.x = 0.5 - marker.pose.orientation.y = 0.5 - marker.pose.orientation.z = 0.5 - marker.pose.orientation.w = 0.5 - marker.scale.x = 1 - marker.scale.y = 1 - marker.scale.z = 1 - marker.color.a = 1.0 - marker.color.r = 1.0 - marker.color.g = 0.0 - marker.color.b = 0.0 - marker_name.color.a = 1.0 - marker_name.color.r = 1.0 - marker_name.color.g = 0.0 - marker_name.color.b = 0.0 - marker.lifetime = rospy.Duration() - marker_pub.publish(marker) - marker_pub.publish(marker_name) - elif actor_posture == "sit": - marker.ns = "human" - marker.id = actor_id - marker.action = Marker.MODIFY - if actor_gender == "male": - marker.mesh_resource = "file://"+tasker_path+"/makehuman/Male/male_sit2.dae" - else: - marker.mesh_resource = "file://"+tasker_path+"/makehuman/Female/female_sit.dae" - marker.pose.position.x = human_transform[0] - marker.pose.position.y = human_transform[1] - marker.pose.position.z = -0.1 - marker_name.pose.position.z = 1.1 - marker.pose.orientation.x = 0.0 - marker.pose.orientation.y = 0.0 - marker.pose.orientation.z = 0.7071068 - marker.pose.orientation.w = 0.7071068 - marker.scale.x = 1 - marker.scale.y = 1 - marker.scale.z = 1 - marker.color.a = 1.0 - marker.color.r = 0.0 - marker.color.g = 1.0 - marker.color.b = 0.0 - marker_name.color.a = 1.0 - marker_name.color.r = 0.0 - marker_name.color.g = 1.0 - marker_name.color.b = 0.0 - marker.lifetime = rospy.Duration() - marker_pub.publish(marker) - marker_pub.publish(marker_name) - else: - actor_posture = rospy.set_param(actor_name+"/actor_posture", "stand") - task_reqested = False - human_transform[0] = human_transform[0] + math.cos(human_transform[2])*vel.linear.x/10+ math.sin(human_transform[2])*vel.linear.y/10 - human_transform[1] = human_transform[1] + math.cos(human_transform[2])*vel.linear.y/10+ math.sin(human_transform[2])*vel.linear.x/10 - human_transform[2] = human_transform[2] + vel.angular.z/10 - vel.linear.x = 0 - vel.linear.y = 0 - vel.angular.z = 0 - - marker.ns = "human" - marker.id = actor_id - marker.action = Marker.MODIFY - if actor_gender == "male": - marker.mesh_resource = "file://"+tasker_path+"/makehuman/Male/male.dae" - else: - marker.mesh_resource = "file://"+tasker_path+"/makehuman/Female/female.dae" - rot = tf.transformations.quaternion_from_euler(1.54, 0, human_transform[2]+1.54) - gazebo_model_state.pose.position.x = human_transform[0] - gazebo_model_state.pose.position.y = human_transform[1] - gazebo_model_state.pose.orientation = Quaternion(rot[0],rot[1],rot[2],rot[3]) - current_gz_rot = gazebo_model_state.pose.orientation - marker.pose.position.x = human_transform[0] - marker.pose.position.y = human_transform[1] - marker_name.pose.position.x = human_transform[0] - marker_name.pose.position.y = human_transform[1] - marker_name.pose.position.z = 2 - marker.pose.position.z = 0 - marker.pose.orientation.x= rot[0] - marker.pose.orientation.y= rot[1] - marker.pose.orientation.z= rot[2] - marker.pose.orientation.w= rot[3] - marker.scale.x = 1 - marker.scale.y = 1 - marker.scale.z = 1 - marker.color.a = 1.0 - marker.color.r = 0.0 - marker.color.g = 1.0 - marker.color.b = 0.0 - marker_name.color.a = 1.0 - marker_name.color.r = 0.0 - marker_name.color.g = 1.0 - marker_name.color.b = 0.0 - marker.lifetime = rospy.Duration() - marker_pub.publish(marker) - marker_pub.publish(marker_name) - theta = tf.transformations.quaternion_from_euler(0, 0, human_transform[2]) - x_str = "'x': {}, ".format(human_transform[0]) - y_str = '\'y\': {}, '.format(human_transform[1]) - theta_str = '\'theta\': {} '.format(human_transform[2]) - rospy.set_param(actor_name+'/pose', '{'+x_str+y_str+theta_str+'}') - br.sendTransform((human_transform[0], human_transform[1], 0), - theta, - rospy.Time.now(), - actor_name, - "map") - gazebo_control_pub.publish(gazebo_model_state) - rospy.sleep(0.1) diff --git a/bin/control_human_real.py b/bin/control_human_real.py deleted file mode 100755 index 6a2ac8d..0000000 --- a/bin/control_human_real.py +++ /dev/null @@ -1,258 +0,0 @@ -#!/usr/bin/env python -import roslib -import rospy -import rospkg -import math -import tf -import os, signal -import turtlesim.msg -from geometry_msgs.msg import Twist, Quaternion -from visualization_msgs.msg import Marker -from tasker_msgs.srv import * -from tiago_msgs.msg import Command -from gazebo_msgs.msg import ModelState -from tf.transformations import * -import tiago_kb.places_xml as kb_p -from fall_detection_integration.msg import FallData -global vel - -vel = Twist() -def handle_actor_vel(msg): - global vel - vel = msg -def handle_ionis_msg(msg): - global human_transform - global actor_name - human_transform = [msg.x, msg.y, 0] - print "human position: ",human_transform - if msg.fall_alert_flag != 0: - rospy.set_param(actor_name+"/actor_posture", 'fell') - -if __name__ == '__main__': - global human_transform - global actor_name - - rospy.init_node('control_human',anonymous=True) - actor_name = rospy.get_param('~actor_name') - if not rospy.has_param('/last_actor_id'): - actor_id = 0 - else: - last_actor_id = rospy.get_param('/last_actor_id') - actor_id = last_actor_id+1 - actor_gender = rospy.get_param('~actor_gender') - rospy.set_param('/last_actor_id', actor_id) - # human_transform = rospy.get_param('~actor_init_pose') - rospy.Subscriber('/fall_detection', - FallData, - handle_ionis_msg - ) - human_transform = [0, 0, 0] - rospy.loginfo("Setting actor_init_pose: %f, %f, %f" % (human_transform[0], human_transform[1], human_transform[2])) - actor_posture = rospy.set_param(actor_name+"/actor_posture", "stand") - br = tf.TransformBroadcaster() - marker_pub = rospy.Publisher("ellipse", Marker, queue_size=10) - gazebo_control_pub = rospy.Publisher("/gazebo/set_model_state", ModelState, queue_size=10) - cmd_pub = rospy.Publisher("/rico_cmd", Command, queue_size=10) - gazebo_model_state = ModelState() - x_str = '\'x\': {} '.format(human_transform[0]) - y_str = '\'y\': {} '.format(human_transform[1]) - theta_str = '\'theta\': {} '.format(human_transform[2]) - rospy.set_param(actor_name+'/pose', '{'+x_str+y_str+theta_str+'}') #'x': \"%f\", 'y': \"%f\", 'theta': \"%f\"}" % human_transform[0] human_transform[1] angle_dest) - gazebo_model_state.model_name="John" - gazebo_model_state.pose.position.x = human_transform[0] - gazebo_model_state.pose.position.y = human_transform[1] - current_gz_rot = Quaternion() - gazebo_model_state.pose.orientation = current_gz_rot - marker = Marker() - marker.header.frame_id = "map" - marker.header.stamp = rospy.Time.now() - marker.ns = "human" - marker.id = actor_id - marker.type = Marker.MESH_RESOURCE - marker.action = Marker.ADD - rospack = rospkg.RosPack() - - # list all packages, equivalent to rospack list - rospack.list() - - # get the file path for rospy_tutorials - tasker_path = rospack.get_path('tasker') - if actor_gender == "male": - marker.mesh_resource = "file://"+tasker_path+"/makehuman/Male/male.dae" - else: - marker.mesh_resource = "file://"+tasker_path+"/makehuman/Female/female.dae" - marker.pose.position.x = human_transform[0] - marker.pose.position.y = human_transform[1] - marker.pose.position.z = 0 - marker.pose.orientation.x = 0.5 - marker.pose.orientation.y = 0.5 - marker.pose.orientation.z = 0.5 - marker.pose.orientation.w = 0.5 - marker.scale.x = 1 - marker.scale.y = 1 - marker.scale.z = 1 - marker.color.a = 1.0 - marker.color.r = 0.0 - marker.color.g = 1.0 - marker.color.b = 0.0 - marker.lifetime = rospy.Duration() - - marker_name = Marker() - marker_name.header.frame_id = "/map" - marker_name.header.stamp = rospy.Time.now() - marker_name.ns = "names" - marker_name.id = actor_id - marker_name.type = Marker.TEXT_VIEW_FACING - marker_name.action = Marker.ADD - - marker_name.pose.position.x = human_transform[0] - marker_name.pose.position.y = human_transform[1] - marker_name.pose.position.z = 2 - marker_name.pose.orientation.x = 0.0 - marker_name.pose.orientation.y = 0.0 - marker_name.pose.orientation.z = 0.0 - marker_name.pose.orientation.w = 1.0 - - marker_name.text = actor_name - - marker_name.scale.z = 0.7 - - marker_name.color.r = 0.0 - marker_name.color.g = 1.0 - marker_name.color.b = 0.0 - marker_name.color.a = 1.0 - - marker_pub.publish(marker) - marker_pub.publish(marker_name) - task_reqested = False - while not rospy.is_shutdown(): - actor_posture = rospy.get_param(actor_name+"/actor_posture") - marker_pub.publish(marker) - if actor_posture == "fell": - rot = quaternion_multiply([current_gz_rot.x,current_gz_rot.y,current_gz_rot.z,current_gz_rot.w],quaternion_from_euler(-1.54, 0, 0)) - gazebo_model_state.pose.orientation = Quaternion(rot[0],rot[1],rot[2],rot[3]) - fell_cmd = Command() - fell_cmd.query_text="" - fell_cmd.intent_name = "HF" - fell_cmd.param_names = ["human_name"] - fell_cmd.param_values = [actor_name] - marker.ns = "human" - marker.id = actor_id - marker.action = Marker.MODIFY - if actor_gender == "male": - marker.mesh_resource = "file://"+tasker_path+"/makehuman/Male/male_fell.dae" - else: - marker.mesh_resource = "file://"+tasker_path+"/makehuman/Female/female_fell.dae" - if task_reqested == False: - cmd_pub.publish(fell_cmd) - task_reqested= True - marker.pose.position.x = human_transform[0] - marker.pose.position.y = human_transform[1] - marker.pose.position.z = -0.5 - marker_name.pose.position.z = 1 - marker.pose.orientation.x = 0.5 - marker.pose.orientation.y = 0.5 - marker.pose.orientation.z = 0.5 - marker.pose.orientation.w = 0.5 - marker.scale.x = 1 - marker.scale.y = 1 - marker.scale.z = 1 - marker.color.a = 1.0 - marker.color.r = 1.0 - marker.color.g = 0.0 - marker.color.b = 0.0 - marker_name.color.a = 1.0 - marker_name.color.r = 1.0 - marker_name.color.g = 0.0 - marker_name.color.b = 0.0 - marker.lifetime = rospy.Duration() - marker_pub.publish(marker) - marker_pub.publish(marker_name) - elif actor_posture == "sit": - marker.ns = "human" - marker.id = actor_id - marker.action = Marker.MODIFY - if actor_gender == "male": - marker.mesh_resource = "file://"+tasker_path+"/makehuman/Male/male_sit2.dae" - else: - marker.mesh_resource = "file://"+tasker_path+"/makehuman/Female/female_sit.dae" - marker.pose.position.x = human_transform[0] - marker.pose.position.y = human_transform[1] - marker.pose.position.z = -0.1 - marker_name.pose.position.z = 1.1 - marker.pose.orientation.x = 0.0 - marker.pose.orientation.y = 0.0 - marker.pose.orientation.z = 0.7071068 - marker.pose.orientation.w = 0.7071068 - marker.scale.x = 1 - marker.scale.y = 1 - marker.scale.z = 1 - marker.color.a = 1.0 - marker.color.r = 0.0 - marker.color.g = 1.0 - marker.color.b = 0.0 - marker_name.color.a = 1.0 - marker_name.color.r = 0.0 - marker_name.color.g = 1.0 - marker_name.color.b = 0.0 - marker.lifetime = rospy.Duration() - marker_pub.publish(marker) - marker_pub.publish(marker_name) - else: - actor_posture = rospy.set_param(actor_name+"/actor_posture", "stand") - task_reqested = False - human_transform[0] = human_transform[0] + math.cos(human_transform[2])*vel.linear.x/10+ math.sin(human_transform[2])*vel.linear.y/10 - human_transform[1] = human_transform[1] + math.cos(human_transform[2])*vel.linear.y/10+ math.sin(human_transform[2])*vel.linear.x/10 - human_transform[2] = human_transform[2] + vel.angular.z/10 - vel.linear.x = 0 - vel.linear.y = 0 - vel.angular.z = 0 - - marker.ns = "human" - marker.id = actor_id - marker.action = Marker.MODIFY - if actor_gender == "male": - marker.mesh_resource = "file://"+tasker_path+"/makehuman/Male/male.dae" - else: - marker.mesh_resource = "file://"+tasker_path+"/makehuman/Female/female.dae" - rot = tf.transformations.quaternion_from_euler(1.54, 0, human_transform[2]+1.54) - gazebo_model_state.pose.position.x = human_transform[0] - gazebo_model_state.pose.position.y = human_transform[1] - gazebo_model_state.pose.orientation = Quaternion(rot[0],rot[1],rot[2],rot[3]) - current_gz_rot = gazebo_model_state.pose.orientation - marker.pose.position.x = human_transform[0] - marker.pose.position.y = human_transform[1] - marker_name.pose.position.x = human_transform[0] - marker_name.pose.position.y = human_transform[1] - marker_name.pose.position.z = 2 - marker.pose.position.z = 0 - marker.pose.orientation.x= rot[0] - marker.pose.orientation.y= rot[1] - marker.pose.orientation.z= rot[2] - marker.pose.orientation.w= rot[3] - marker.scale.x = 1 - marker.scale.y = 1 - marker.scale.z = 1 - marker.color.a = 1.0 - marker.color.r = 0.0 - marker.color.g = 1.0 - marker.color.b = 0.0 - marker_name.color.a = 1.0 - marker_name.color.r = 0.0 - marker_name.color.g = 1.0 - marker_name.color.b = 0.0 - marker.lifetime = rospy.Duration() - marker_pub.publish(marker) - marker_pub.publish(marker_name) - theta = tf.transformations.quaternion_from_euler(0, 0, human_transform[2]) - x_str = "'x': {}, ".format(human_transform[0]) - y_str = '\'y\': {}, '.format(human_transform[1]) - theta_str = '\'theta\': {} '.format(human_transform[2]) - rospy.set_param(actor_name+'/pose', '{'+x_str+y_str+theta_str+'}') - br.sendTransform((human_transform[0], human_transform[1], 0), - theta, - rospy.Time.now(), - actor_name, - "map") - gazebo_control_pub.publish(gazebo_model_state) - rospy.sleep(0.1) diff --git a/bin/exampleState b/bin/exampleState deleted file mode 100755 index 319baf2..0000000 --- a/bin/exampleState +++ /dev/null @@ -1,98 +0,0 @@ -#!/usr/bin/env python - -from task_machine.StateMachine import * #StateMachine -import rospy -import time -positive_adjectives = ["great","super", "fun", "entertaining", "easy"] -negative_adjectives = ["boring", "difficult", "ugly", "bad"] - -def start_transitions_hold(cargo_in, event = None): - print "start_transitions_hold: ", cargo_in - resumeState = "start" - resumeData = "CARGO RESUMEd" - return (resumeState, resumeData) -def my_block(): - print "my_block" - time.sleep(10) -def start_transitions(cargo_in, event = None): - # print "Cargo IN: ", cargo_in - # # while True: - # # time.sleep(1) - # # print "event: ", event - # # pass - # cargo_out = "new cargo" - # holdData = "ZATRZYMAJ" - # print "CARGO OUT: ", cargo_out - # print "holdData: ", holdData - # newState = "start" - splitted_txt = cargo_in.split(None,1) - word, cargo_out = splitted_txt if len(splitted_txt) > 1 else (cargo_in,"") - # run_blocking(my_block(), (None, )) - if word == "Python": - newState = "Python_state" - else: - newState = "error_state" - return (newState, cargo_out) - -def python_state_transitions_hold(txt): - print "HOLDING python_state_transitions_hold" -def python_state_transitions(cargo_in, event = None): - print "python_trans" - splitted_txt = cargo_in.split(None,1) - word, cargo_out = splitted_txt if len(splitted_txt) > 1 else (cargo_in,"") - if word == "is": - newState = "is_state" - else: - newState = "error_state" - return (newState, cargo_out) - -def is_state_transitions_hold(cargo_in, event = None): - print "HOLDING is_state_transitions_hold" -def is_state_transitions(cargo_in, event = None): - print "is_trans" - splitted_txt = cargo_in.split(None,1) - word, cargo_out = splitted_txt if len(splitted_txt) > 1 else (cargo_in,"") - if word == "not": - newState = "not_state" - elif word in positive_adjectives: - newState = "pos_state" - elif word in negative_adjectives: - newState = "neg_state" - else: - newState = "error_state" - return (newState, cargo_out) - -def not_state_transitions_hold(cargo_in, event = None): - print "HOLDING not_state_transitions_hold" -def not_state_transitions(cargo_in, event = None): - print "not_trans" - splitted_txt = cargo_in.split(None,1) - word, cargo_out = splitted_txt if len(splitted_txt) > 1 else (cargo_in,"") - if word in positive_adjectives: - newState = "neg_state" - elif word in negative_adjectives: - newState = "pos_state" - else: - newState = "error_state" - return (newState, cargo_out) - -def neg_state_hold(cargo_in, event = None): - print "HOLDING START" -def neg_state(cargo_in, event = None): - print("Hallo") - return ("neg_state", "") - -if __name__== "__main__": - rospy.init_node("exampleState") - m = StateMachine() - m.add_state("Start", start_transitions, start_transitions_hold) - m.add_state("Python_state", python_state_transitions, python_state_transitions_hold) - m.add_state("is_state", is_state_transitions,is_state_transitions_hold) - m.add_state("not_state", not_state_transitions, not_state_transitions_hold) - m.add_state("neg_state", None, end_state=1) - m.add_state("pos_state", None, end_state=1) - m.add_state("error_state", None, end_state=1) - m.set_start("Start") - m.run("Python is great") - #m.run("Python is difficult") - #m.run("Perl is ugly") \ No newline at end of file diff --git a/bin/exemplary_susp_task b/bin/exemplary_susp_task deleted file mode 100755 index 8d18f2d..0000000 --- a/bin/exemplary_susp_task +++ /dev/null @@ -1,12 +0,0 @@ -#!/usr/bin/env python -# encoding: utf8 - -import tkinter as tk -from tkinter import messagebox - -w = tk.Tk() -w.withdraw() -w.after(5000, w.destroy) # Destroy the widget after 3 seconds -w.option_add('*Dialog.msg.font', 'Helvetica 12') -if messagebox.showinfo('EXEMPLARY SUSPENSION', 'EXEMPLARY SUSPENSION IS TRIGGERED,\n This window is triggered as an exemplary suspension action. The DA launched it based on field in request from the TaskHarmoniserAgent!'): - w.destroy() diff --git a/bin/harmonization_table_generator b/bin/harmonization_table_generator deleted file mode 100755 index 4881e63..0000000 --- a/bin/harmonization_table_generator +++ /dev/null @@ -1,77 +0,0 @@ -#!/usr/bin/env python - -import rospy -import time -import datetime -import sys -from multitasker.msg import * -#string node_name -#string state_name -#string state_input -# task state, 0 = running, 1 = held, 2 = finished -#int8 status - -import csv - -# final_table -table = [] -row_id = 0 -apps = [] -states = [] -inputs = [] -status = [] - -def gotState(data): - global apps - global table - global row_id - global states - global inputs - global status - state_name2 = data.state_name.replace("_", "\\_") - node_name2 = data.node_name.replace("_", "\\_") - input2 = data.state_input.replace("_", "\\_") - table.append([node_name2, state_name2, str(input2), str(data.status)]) - # apps.append(data.node_name) - # states.append(data.state_name) - # inputs.append(str(data.state_input)) - # status.append(str(data.status)) - row_id = row_id + 1 - pass - -if __name__== "__main__": - try: - rospy.init_node("harmonization_table_generator") - sub = rospy.Subscriber("/current_state", TaskState, gotState) - rospy.spin() - - except Exception as e: - raise e - finally: - global apps - global states - global inputs - global status - global table - global row_id - # rows = [ table[], - # states, - # inputs, - # status - # ] - fields = ['app', 'state', 'input', 'status'] - # name of csv file - filename = "university_records.csv" - - # writing to csv file - with open(filename, 'w') as csvfile: - # creating a csv writer object - csvwriter = csv.writer(csvfile) - - # writing the fields - csvwriter.writerow(fields) - - # writing the data rows - csvwriter.writerows(table) - pass - diff --git a/bin/taskHarmonizer b/bin/taskHarmonizer deleted file mode 100755 index 4cf6531..0000000 --- a/bin/taskHarmonizer +++ /dev/null @@ -1,168 +0,0 @@ -#!/usr/bin/env python - -import rospy -from multitasker.srv import * -from multitasker.msg import * -from std_srvs.srv import Trigger - -import time -from std_msgs.msg import * -import multiprocessing -import threading -import datetime -import roslaunch -import os -import heapq -import itertools -import subprocess - -POSTPONED_TASK_ID = 0 -SUSPENDED_TASK_ID = 1 -STARTED_TASK_ID = 2 - -stop_trigger = False -is_any_task_launched = False - -current_state = TaskState() -pq = [] # list of entries arranged in a heap -entry_finder = {} # mapping of tasks to entries -REMOVED = '' # placeholder for a removed task -counter = itertools.count() # unique sequence count - -def add_task(task, priority=0, state_id = 0): - 'Add a new task or update the priority of an existing task' - if task in entry_finder: - remove_task(task) - count = next(counter) - # priority, count, da_name, executable, state_id {0 - postponed, 1 - suspended} - entry = [priority, count, task, state_id] - entry_finder[task] = entry - heapq.heappush(pq, entry) - -def remove_task(task): - 'Mark an existing task as REMOVED. Raise KeyError if not found.' - entry = entry_finder.pop(task) - entry[-1] = REMOVED - -def pop_task(): - 'Remove and return the lowest priority task. Raise KeyError if empty.' - while pq: - priority, count, task,state_id = heapq.heappop(pq) - if task is not REMOVED: - del entry_finder[task] - return [task, state_id] - raise KeyError('pop from an empty priority queue') - -def handleNewTaskRequest(req): - package = 'multitasker' - executable = req.application - da_name = req.da_name - cmd = "rosrun "+ package + " "+executable+ " "+da_name+ " "+str(req.task_priority)+ " "+str(req.task_deadline) - subprocess.Popen(['rosrun', package, executable, da_name, str(req.task_priority), str(req.task_deadline)]) - isInterrupting = False - set_new_task = False - status = False - global current_state - is_any_task_launched_local = current_state.status - print "NEW request!\n \t app: "+ req.application +"\n \t priority: "+ str(req.task_priority)+"\n \t name: "+ str(req.da_name) - current_task_namespace = current_state.node_name + "/multitasking" - requested_task_namespace = req.da_name + "/multitasking" - print "NAMESPACE: "+current_task_namespace - print "cuRRENT TASK STATUS: "+str(is_any_task_launched_local) - rospy.wait_for_service(requested_task_namespace+'/get_launch_conditions', timeout=None) - rospy.wait_for_service(requested_task_namespace+'/startTask', timeout=None) - - if is_any_task_launched_local == 0: - hold_conditions = rospy.ServiceProxy(current_task_namespace+'/get_hold_conditions', HoldConditions) - hold_response = hold_conditions() - launch_conditions = rospy.ServiceProxy(requested_task_namespace+'/get_launch_conditions', LaunchConditions) - launch_response = launch_conditions() - - print "HOLD RESPONSE: \n" + str(hold_response) - if launch_response.task_priority > hold_response.priority: - hold_srv_name = current_task_namespace+"/hold_now" - # resume_topic = current_task_namespace+"/resume_now" - print "HOLD SERVICE: "+hold_srv_name - - hold_srv = rospy.ServiceProxy(hold_srv_name, Trigger) - hold_srv() - while current_state.status not in [1,2]: - print "Waiting for current task to hold or terminate" - time.sleep(1) - add_task(current_state.node_name, hold_response.priority, SUSPENDED_TASK_ID) - else: - add_task(req.da_name, req.task_priority, POSTPONED_TASK_ID) - print "QUEUE SIZE: ", len(pq) - return POSTPONED_TASK_ID - # my_priority = req.task_priority - # my_deadline = -1 - # while (my_priority <= hold_response.priority) - # launch_conditions = rospy.ServiceProxy(requested_task_namespace+'/get_launch_conditions', LaunchConditions) - # hold_response = hold_conditions() - - # while (current_state.status not in [1,2] ): - # print req.da_name, " : waits for current task to terminate" - # time.sleep(1) - # rospy.spinOnce() - if current_state.status == 1: - isInterrupting = True - else: - isInterrupting = False - start_task_srv = rospy.ServiceProxy(requested_task_namespace+'/startTask', StartTask) - start_task_srv(isInterrupting) - - # node = roslaunch.core.Node(package, executable) - - # launch = roslaunch.scriptapi.ROSLaunch() - # launch.start() - - # process = launch.launch(node) - #while process.is_alive(): - # print "Waiting for the new task to finish" - # time.sleep(1) - - # process.stop() - # print "ENDED NEW TASK!!!!!!!!!!!!!S" - # if is_any_task_launched_local == 0: - # pub_res.publish("") - # status = True - return STARTED_TASK_ID - -def startQueuedTask(): - task_data = pop_task() - print "task_data: ",task_data - print "QUEUE SIZE: ", len(pq) - da_namespace = task_data[0] + "/multitasking" - if task_data[1] == POSTPONED_TASK_ID: - srv_name = da_namespace + "/startTask" - start_task_srv = rospy.ServiceProxy(srv_name, StartTask) - interrupts = False - start_task_srv(interrupts) - elif task_data[1] == SUSPENDED_TASK_ID: - resume_srv = da_namespace +"/resume_now" - resume_task_srv = rospy.ServiceProxy(resume_srv, Trigger) - resume_task_srv() - # pub_res = rospy.Publisher(resume_topic, String, queue_size=10) - # time.sleep(2) - # pub_res.publish("") - return - -def setCurrentState(data): - global current_state - current_state = data - if current_state.status == 2 and len(pq) > 0: - startQueuedTask() - # print "NEW CY=URRENT sTATE: ", data - return - -if __name__== "__main__": - global current_state - rospy.init_node("taskHarmonizer") - current_state.status = None - srv_name = rospy.get_name()+'/new_task' - s = rospy.Service(srv_name, TaskRequest, handleNewTaskRequest) - pub_state = rospy.Subscriber('current_state', TaskState, setCurrentState) - rospy.spin() - - - diff --git a/bin/tiago_action_common.py b/bin/tiago_action_common.py deleted file mode 100644 index 4a146e8..0000000 --- a/bin/tiago_action_common.py +++ /dev/null @@ -1,55 +0,0 @@ -import task_machine.StateMachine -import rospy -import time -import actionlib - -STATE_INTERRUPTED = 0 -STATE_IN_PROGRESS = 1 - -# ======================================================================================= - -''' -Temporary function to simulate some tiago actions but the loop is time-based -''' -def handle_tiago_fake_action(time_stop): - - i = 0 - while ( i < time_stop ): - - print( "\t o HOLD IS UNAVAILABLE"+"\n") - i += 1 - time.sleep(1) - - return - -# ======================================================================================= - -def handle_tiago_action(action_client, event_in, event_out): - - while ( action_client.get_state() != ACTION_STATUS_SUCCEEDED ): - - print( "\t o action STATE: " + str(action_client.get_state()) +"\n") - - if ( action_client.get_state() == ACTION_STATUS_ABORTED ): - print( "\t o action STATE: ABORTED - a valid plan could not be found... \n") - break - - if ( handle_interrupt(event_in, event_out) ): - return STATE_INTERRUPTED - - time.sleep(1) - - print( "\t o action STATE: " + str(action_client.get_state()) +"\n") - return STATE_IN_PROGRESS - -# ======================================================================================= - -def handle_interrupt(event_in, event_out): - - if event_in.isSet(): - event_out.set() - return 1 - else: - return 0 - -# ======================================================================================= \ No newline at end of file diff --git a/bin/tiago_guideHuman b/bin/tiago_guideHuman deleted file mode 100755 index f49e51a..0000000 --- a/bin/tiago_guideHuman +++ /dev/null @@ -1,1191 +0,0 @@ -#!/usr/bin/env python2 - -from task_machine.StateMachine import * #StateMachine -from multitasker.srv import LaunchConditions,StartTask -from multitasker.msg import * -from multitasker.srv import * -from rospy_message_converter import json_message_converter -import json - -import rospy -import time -import datetime -import sys - -# ------------------------------------------------------------------ -from tf.transformations import euler_from_quaternion, quaternion_from_euler -import tf -import math -import geometry_msgs.msg -import actionlib -# ------------------------------------------------------------------ -from tiago_utils import * # global constants -from tiago_utils import move_base_set_goal -from tiago_utils import move_base_cancel_goals -from tiago_utils import wait_for_tiago_init -from tiago_utils import action_status_callback -# ------------------------------------------------------------------ - -from nav_msgs.msg import * -from nav_msgs.srv import * -# -------------------------------- -from visualization_msgs.msg import Marker -from geometry_msgs.msg import Twist -# -------------------------------- - - -global listener -global human_id -global human_posture -global return_msg -global marker_poses -global DA_fsm -human_posture = "stand" -return_msg = [] -startFlag = False -my_priority = 0 -isInterrupting = False - -my_priority = 0 -my_completion_time = -1 -isInterrupting = False - -dock_pose = [3.70, 1.90, 0.0, 0.0, 0.0, 0.0] # next to entrance | y ~ PI - back to door - -# 'standing' script poses -# =================== -# ======= V1 ======== -# =================== -''' -header: - seq: 0 - stamp: - secs: 105 - nsecs: 373000000 - frame_id: "map" -pose: - position: - x: 0.449645936489 - y: 0.0792762041092 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.97049860128 - w: -0.241106750038 - -human_pose = [0.45, 0.08, 0.0, 0.0, 0.0, -152.5] -''' -# =================== -# ======= V2 ======== -# =================== -''' -header: - seq: 0 - stamp: - secs: 22 - nsecs: 330000000 - frame_id: "map" -pose: - position: - x: -0.1344743222 - y: -0.201279640198 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.999291688955 - w: -0.0376313750156 -''' -#human_pose = [-0.13, 0.20, 0.00, 0.0, 0.0, 35.68] -# =================== - -''' - position: - x: 3.18645572662 - y: 5.37823104858 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.58703750871 - w: 0.80955973428 -''' -#human_dest = [3.18, 5.37, 0.0, 0.0, 0.0, 71.89] - -# human_dest = [2.92, 5.24, 0.0, 0.0, 0.0, 120.0] - -# =================== - -# ------------------------------------------------------------------ - -# Ver2 -human_poses = [] -human_dests = [] -human_last_poses = [] -marker_poses = [] - -# * * * * * * * * * * * * * * ACTOR1 * * * * * * * * * * * * * * * * - -# start -''' -header: - seq: 8 - stamp: - secs: 243 - nsecs: 853000000 - frame_id: "map" -pose: - position: - x: -0.48579788208 - y: 0.14269067347 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.953078234364 - w: -0.302724097457 -''' - -#human_pose = [-0.4857, +0.0000, +0.0000, +0.0000, +0.0000, -144.7597] -#human_poses.append(human_pose) - -''' -header: - seq: 1 - stamp: - secs: 374 - nsecs: 800000000 - frame_id: "map" -pose: - position: - x: -0.320914298296 - y: -0.100545883179 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.986417840476 - w: -0.164255423015 -''' -#human_pose = [-0.3209, -0.1005, +0.0000, +0.0000, +0.0000, -161.0926] - -human_pose = [-0.4857, +0.1426, +0.0000, +0.0000, +0.0000, -161.0926] -human_poses.append(human_pose) -marker_pose = [-1.095, 1.248, 0, +0.0000, +0.0000, -161.0926] -marker_poses.append(marker_pose) - - -# dest -''' -header: - seq: 9 - stamp: - secs: 292 - nsecs: 174000000 - frame_id: "map" -pose: - position: - x: 3.12596273422 - y: 5.67313241959 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.999757517577 - w: 0.0220205823641 -''' -#human_dest = [+3.1259, +5.6731, +0.0000, 0.0000, 0.0000, +177.4764] -#human_dests.append(human_dest) - -''' -header: - seq: 2 - stamp: - secs: 589 - nsecs: 850000000 - frame_id: "map" -pose: - position: - x: 3.41345524788 - y: 5.586145401 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.985469480681 - w: 0.169852590933 -''' - -human_dest = [+3.4134, +5.5861, +0.0000, 0.0000, 0.0000, +160.4415] -human_dests.append(human_dest) - -# * * * * * * * * * * * * * * ACTOR2 * * * * * * * * * * * * * * * * -# start - -''' -header: - seq: 12 - stamp: - secs: 395 - nsecs: 123000000 - frame_id: "map" -pose: - position: - x: 3.31662845612 - y: 1.70269775391 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: -0.805676184728 - w: 0.592356214927 - -''' -#human_pose = [+3.3166, +1.7026, +0.0000, +0.0000, +0.0000, -107.3513] -#human_poses.append(human_pose) - -''' -header: - seq: 3 - stamp: - secs: 677 - nsecs: 972000000 - frame_id: "map" -pose: - position: - x: 3.22058129311 - y: 1.03116691113 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: -0.776372649676 - w: 0.63027415371 - -''' - -#human_pose = [+3.2205, +1.0311, +0.0000, +0.0000, +0.0000, -101.8592] -human_pose = [+3.3166, +1.7026, +0.0000, +0.0000, +0.0000, -101.8592] -marker_pose = [+3.3166, +1.0026, +0.0000, +0.0000, +0.0000, -101.8592] -human_poses.append(human_pose) -marker_poses.append(marker_pose) - -# dest - -''' -header: - seq: 11 - stamp: - secs: 375 - nsecs: 113000000 - frame_id: "map" -pose: - position: - x: -0.552233695984 - y: -0.276034355164 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: -0.0662988753729 - w: 0.997799809142 -''' -#human_dest = [-0.5522, -0.2760, +0.0000, 0.0000, 0.0000, -7.6019] -#human_dests.append(human_dest) - -''' -header: - seq: 4 - stamp: - secs: 734 - nsecs: 804000000 - frame_id: "map" -pose: - position: - x: -0.474497079849 - y: -0.41937148571 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.00422636600859 - w: 0.999991068875 -''' - -human_dest = [-0.4744, -0.4193, +0.0000, 0.0000, 0.0000, +0.4842] -human_dests.append(human_dest) - -# ------------------------------------------------------------------ - -human_last_poses = human_poses # in fact - robot's last poses while guiding each human - -# ------------------------------------------------------------------ - -human_name = "" -room_number = "" -global factor_walk -factor_walk = [0,0] -global factor_stand -factor_stand = [0,0] -global factor_sit -factor_sit = [0,0] -# walk factor for human 0 -factor_walk[0] = 0.15 -# walk factor for human 1 -factor_walk[1] = 0.2 -# sit factor for human 0 -factor_sit[0] = 0.3 -# sit factor for human 1 -factor_sit[1] = 0.3 -# stand factor for human 0 -factor_stand[0] = 0.15 -# stand factor for human 1 -factor_stand[1] = 0.2 -global marker_pose_list -marker_pose_list = [] -# ====================================================================================== - -def blocking_call(queue): - - try: - args = queue.get() - i =0 - print(str(datetime.datetime.now().time())+"\n"+ args+"\n") - x = args[0] - y = args[1] - z = args[2] - print(str(datetime.datetime.now().time())+"\n"+ "Z: ", z+"\n") - while i < 100: - x+=1 - y+=1 - z+=1 - time.sleep(1) - i+=1 - print(str(datetime.datetime.now().time())+"\n"+ "computed: "+ str([x, y, z])+"\n") - queue.put([x,y,z]) - finally: - print(str(datetime.datetime.now().time())+"\n"+ "EXCEPTTTTTTTTTTTTTTTTTTTTT"+"\n") - return - -# ====================================================================================== - -def start(cargo_in, event_in, event_out): - - print("\n"+ "----------"+"\n") - print( "START"+"\n") - print( "----------"+"\n") - print("\t cargo: "+ str( cargo_in)+"\n") - print("\t STATE_EVENT: "+ str( event_in.isSet())+"\n") - print( ""+"\n") - - global human_id - - # if event_in.isSet(): - # event_out.set() - # print( ""+"\n") - # print( "set HOLD"+"\n") - # print( ""+"\n") - # return ("first_hold", "data required to hold") - - return ("move_to_human", [human_name, None]) - -# ====================================================================================== - -def move_to_human(cargo_in, event_in, event_out): - global last_exec_state_name - last_exec_state_name = "move_to_human" - - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "move_to_human"+"\n") - print("----------"+"\n") - print( "\t args [, ]: "+ str( cargo_in)+"\n") - print( "\t Get pose of the human "+"\n") - - i = 0 - - global human_poses - global human_id - global return_msg - goal = human_poses[human_id] - goal[0] = human_poses[human_id][0] + 0.3 - goal[1] = human_poses[human_id][1] + 0.3 - goal[2] = human_poses[human_id][2] - goal[3] = human_poses[human_id][3] - goal[4] = human_poses[human_id][4] - goal[5] = human_poses[human_id][5] - - - if ( cargo_in[1] != None ): - - # not the first try - print ( "\t NOT_1st \t human_id: " + str(human_id) + "\t cargo_in[0]: " + str(cargo_in[0]) + "\n \t \t \t \t \t cargo_in[0][1]: " + str(cargo_in[0][1]) + "\n \t \t \t \t \t cargo_in[1]: " + str(cargo_in[1]) + "\n" ) - client = move_base_set_goal(goal) - - else: - - # first try - print ( "\t 1st \t human_id: " + str(human_id) + "\t pose hard-coded: " + str(human_poses[human_id]) ) - - client = move_base_set_goal( goal ) - - - while ( client.get_state() != ACTION_STATUS_SUCCEEDED ): - - print( "\t Moving to the human: "+ str( cargo_in[0])+"\n") - print( "\t Checking if the destination is reached"+"\n") - #print( "\t o move base STATE: " + str(client.get_state()) +"\n") - - # run_blocking(blocking_call, cargo_in, event) - - if event_in.isSet(): - event_out.set() - print( ""+"\n") - print( "set HOLD"+"\n") - print( ""+"\n") - return ("suspending", "move_to_human") - - rospy.sleep(1) - i+=1 - - say = "" - if cargo_in[1] == None: - say = "Hello, follow me please." - else: - say = "Hello again, follow me please" - - return ("greet", say) - -# ====================================================================================== - -def hold_moving(cargo_in): - - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print("hold_moving"+"\n") - print( "----------"+"\n") - print("\t args: "+ str( cargo_in)+"\n") - - move_base_cancel_goals() - rospy.sleep(2) - global human_name - global room_number - - # run_blocking(blocking_call, cargo_in, event) - #return ("move_to_human", [human_name, human_last_poses[human_id]]) - - return_msg = [] - return_msg.append(human_name) - return_msg.append(human_last_poses[human_id]) - - return ("move_to_human", return_msg ) - -# ====================================================================================== - -def greet(cargo_in, event_in, event_out): - global last_exec_state_name - last_exec_state_name = "greet" - - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "greet"+"\n") - print("----------"+"\n") - i=0 - print( "\t SAY_METHOD("+ str( cargo_in)+")\n") - - while i < 5: - - print( "\t Checking if the speech is over"+"\n") - # run_blocking(blocking_call, cargo_in, event) - if event_in.isSet(): - event_out.set() - print( ""+"\n") - print( "set HOLD"+"\n") - print( ""+"\n") - return ("suspending", "greet") - i+=1 - rospy.sleep(1) - - return ("guide_human", room_number) - -# ====================================================================================== - -def hold_greet(cargo_in): - - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "SAY"+"\n") - print("----------"+"\n") - print( "\t SAY_METHOD("+ str( cargo_in)+")\n") - - #return ("move_to_human", [human_name, human_last_poses[human_id]]) - - return_msg = [] - return_msg.append(human_name) - return_msg.append(human_last_poses[human_id]) - - return ("move_to_human", return_msg ) - -# ====================================================================================== -def add_marker(pose): - global marker_pub - global marker - global human_id - marker_pub = rospy.Publisher("ellipse", Marker) - marker_sub = rospy.Subscriber("human_vel", Twist, update_human_pose) - marker = Marker(); - marker.header.frame_id = "map"; - marker.header.stamp = rospy.Time.now(); - marker.ns = "human"; - marker.id = human_id; - marker.type = Marker.CYLINDER - marker.action = Marker.ADD - marker.pose.position.x = pose[0] - marker.pose.position.y = pose[1] - marker.pose.position.z = 0.5 - marker.pose.orientation.x = 0.0 - marker.pose.orientation.y = 0.0 - marker.pose.orientation.z = 0.0 - marker.pose.orientation.w = 1.0 - marker.scale.x = 0.3 - marker.scale.y = 0.3 - marker.scale.z = 1 - marker.color.a = 1.0 - marker.color.r = 1.0 - marker.color.g = 1.0 - human_id - marker.color.b = 0.0 - marker_pub.publish( marker ) - -def update_marker(pose): - global marker_pub - global marker - global human_id - marker.header.frame_id = "map"; - marker.header.stamp = rospy.Time.now(); - marker.ns = "human"; - marker.id = human_id; - marker.type = Marker.CYLINDER - marker.action = Marker.MODIFY - marker.pose.position.x = pose[0] - marker.pose.position.y = pose[1] - marker.pose.position.z = 0.5 - marker.pose.orientation.x = 0.0 - marker.pose.orientation.y = 0.0 - marker.pose.orientation.z = 0.0 - marker.pose.orientation.w = 1.0 - marker.scale.x = 0.3 - marker.scale.y = 0.3 - marker.scale.z = 1 - marker.color.a = 1.0 - marker.color.r = 1.0 - marker.color.g = 1.0 - human_id - marker.color.b = 0.0 - -def remove_marker(): - global marker_pub - global marker - global human_id - marker.header.frame_id = "map"; - marker.header.stamp = rospy.Time.now(); - marker.ns = "human"; - marker.id = human_id; - marker.type = Marker.CYLINDER - marker.action = Marker.DELETE - marker_pub.publish( marker ); - -def publish_human_pose(): - global marker_pub - global marker - marker_pub.publish( marker ); - -def update_human_pose(vel): - global human_last_poses - global human_id - global marker_poses - global marker_pose_list - marker_poses[human_id][0] = marker_poses[human_id][0] + vel.linear.x - marker_poses[human_id][1] = marker_poses[human_id][1] + vel.linear.y - update_marker(marker_poses[human_id]) - publish_human_pose() - - -def guide_human(cargo_in, event_in, event_out): - global last_exec_state_name - last_exec_state_name = "guide_human" - - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "guide_human"+"\n") - print("----------"+"\n") - print( "\t args : "+ str( cargo_in)+"\n") - i = 0 - - global human_dests - global human_last_poses - global human_id - global human_posture - - client = move_base_set_goal(human_dests[human_id]) - human_posture = "walk" - - while ( client.get_state() != ACTION_STATUS_SUCCEEDED ): - - - print( "\t Moving to the destination: "+ str( cargo_in)+"\n") - print( "\t Checking if the human is following me"+"\n") # not checking really - print( "\t o move base STATE: " + str(client.get_state()) +"\n") - - if ( client.get_state() == ACTION_STATUS_ABORTED ): - print( "\t o move base STATE: ABORTED, a valid plan could not be found \n") - break - - # save the last robot pose in case of guide interrupt - (trans,rot) = listener.lookupTransform('/map', '/base_link', rospy.Time(0)) - (roll, pitch, yaw) = euler_from_quaternion( [rot[0], rot[1], rot[2], rot[3]] ) - roll *= (180.0 / math.pi) - pitch *= (180.0 / math.pi) - yaw *= (180.0 / math.pi) - human_last_pose_temp = [trans[0], trans[1], trans[2], roll, pitch, yaw] - human_last_poses[human_id] = human_last_pose_temp - print ( "\t human_last_pose: " + str(human_last_pose_temp) ) - - if event_in.isSet(): - event_out.set() - print( ""+"\n") - print( "set HOLD"+"\n") - print( ""+"\n") - return ("suspending", "guide_human") - - i+=1 - rospy.sleep(1) - - return ("goodbye_human", None) - -# ====================================================================================== - -def hold_guide(cargo_in): - - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print("hold_guide"+"\n") - - move_base_cancel_goals() - global human_last_poses - global human_id - - print( "----------"+"\n") - print("\t args: "+ str( cargo_in)+"\n") - print( "\t Approaching human "+"\n") - print( "\t SAY_METHOD("+ "Oh! I'm sorry but I got a very important request. Please wait, I'll come back"+")\n") - print( "\t Go aside "+"\n") - - #return ("move_to_human", [human_name, human_last_poses[human_id]]) - - return_msg = [] - return_msg.append(human_name) - return_msg.append(human_last_poses[human_id]) - - return ("move_to_human", return_msg ) - -# ====================================================================================== - -def goodbye_human(cargo_in, event_in, event_out): - global last_exec_state_name - global human_posture - last_exec_state_name = "goodbye_human" - - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "goodbye_human"+"\n") - print("----------"+"\n") - print( "\t args : "+ str( cargo_in)+"\n") - print( "\t SAY_METHOD("+ "Here is the room "+room_number+"\n") - i = 0 - - while i < 5: - print( "\t Checking if the speech is over"+"\n") - print( "\t HOLD IS UNAVAILABLE"+"\n") - - i+=1 - rospy.sleep(1) - human_posture = "ZERO" - print( "\t WILL GO_TO_DOCK" + "\n") - - return ("final", None) - -# ====================================================================================== - -def go_to_dock(cargo_in, event_in, event_out): - global last_exec_state_name - last_exec_state_name = "go_to_dock" - - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "go_to_dock"+"\n") - print("----------"+"\n") - print( "\t args: "+ str( cargo_in)+"\n") - i = 0 - - global dock_pose - client = move_base_set_goal(dock_pose) - - while ( client.get_state() != ACTION_STATUS_SUCCEEDED ): - - print( "\t Moving to my dock"+")\n") - print( "\t o move base STATE: " + str(client.get_state()) +"\n") - - if event_in.isSet(): - event_out.set() - print( ""+"\n") - print( "set HOLD"+"\n") - print( ""+"\n") - return ("suspending", "go_to_dock") - i+=1 - rospy.sleep(1) - - return ("final", None) - -# ====================================================================================== - -def suspending(cargo_in): - - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "suspending"+"\n") - print("----------"+"\n") - print( "\t args: "+ str( cargo_in)+"\n") - i = 0 - # ------- - # getSuspend - # ------- - global return_msg - global last_exec_state_name - global human_posture - last_exec_state_name = cargo_in - strategy = ptf_get_suspend(last_exec_state_name=cargo_in) - # ------- - # execSuspend - # ------- - if strategy == "hold_moving": - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print("hold_moving"+"\n") - print( "----------"+"\n") - print("\t args: "+ str( cargo_in)+"\n") - - move_base_cancel_goals() - - # run_blocking(blocking_call, cargo_in, event) - #return ("move_to_human", [human_name, human_last_poses[human_id]]) - - return_msg = [] - return_msg.append(human_name) - return_msg.append(human_last_poses[human_id]) - elif strategy == "apologize": - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "SAY"+"\n") - print("----------"+"\n") - print( "\t SAY_METHOD(Oh! I'm sorry but I got very important request. Please be sitted and wait, I'll come back)\n") - human_posture = "sit" - - #return ("move_to_human", [human_name, human_last_poses[human_id]]) - - return_msg = [] - return_msg.append(human_name) - return_msg.append(human_last_poses[human_id]) - elif strategy == "hold_guide": - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print("hold_guide"+"\n") - - move_base_cancel_goals() - - print( "----------"+"\n") - print("\t args: "+ str( cargo_in)+"\n") - print( "\t Approaching human "+"\n") - print( "\t SAY_METHOD("+ "Oh! I'm sorry but I got a very important request. Please be sitted and wait, I'll come back"+")\n") - print( "\t Go aside "+"\n") - human_posture = "sit" - - #return ("move_to_human", [human_name, human_last_poses[human_id]]) - - return_msg = [] - return_msg.append(human_name) - return_msg.append(human_last_poses[human_id]) - elif strategy == None: - pass - return ("final", None) - - return ("resume_state", None) - - - -# ====================================================================================== - -def resume_state(cargo_in, event_in, event_out): - - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "resume_state"+"\n") - print("----------"+"\n") - print( "\t args: "+ str( cargo_in)+"\n") - print("----------"+"\n") - print( "\t interupted state : "+ str( cargo_in[0])+"\n") - i = 0 - global return_msg - if cargo_in == 'move_to_human': - return ("move_to_human", return_msg ) - - if cargo_in == 'greet': - return ("move_to_human", return_msg ) - - if cargo_in == 'guide_human': - return ("move_to_human", return_msg ) - else: - print "NO RESUME STRATEGY FOR THE STATE!!!!!!!" - return ("final", cargo_in) - -# ====================================================================================== - -def final(cargo_in, event_in, event_out): - move_base_cancel_goals() - return -def getSP(): - publish_human_pose() - sp = ScheduleParams() - print "CALC COST" - global human_posture - global human_id - global factor_walk - global factor_stand - global factor_sit - global listener - global human_last_poses - global DA_fsm - - path_client = rospy.ServiceProxy('/move_base/GlobalPlanner/make_plan', GetPlan) - req_path = GetPlanRequest() - ros_time = rospy.Time() - robot_trans = [] - human_trans = [] - robot_rot = [] - human_rot = [] - rate = rospy.Rate(5.0) - while not rospy.is_shutdown(): - print "in while" - try: - listener.waitForTransform('/map', '/base_link', ros_time, rospy.Duration(8.0)) - (robot_trans,robot_rot) = listener.lookupTransform('/map', '/base_link', ros_time) - - actor_frame = "/actor_"+str(human_id) - listener.waitForTransform('/map', actor_frame, ros_time, rospy.Duration(8.0)) - (human_trans,human_rot) = listener.lookupTransform('/map', actor_frame, ros_time) - if len(robot_trans) != 0 and len(human_trans) != 0: - break - except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): - rate.sleep() - continue - human_poses[human_id] = [human_trans[0],human_trans[1],0, human_rot[0], human_rot[1], human_rot[2]] - human_last_poses[human_id] = human_poses[human_id] - path_distance = 0 - req_path.start.header.frame_id = "map" - req_path.start.pose.position.x = robot_trans[0] - req_path.start.pose.position.y = robot_trans[1] - req_path.start.pose.position.z = robot_trans[2] - req_path.start.pose.orientation.x = robot_rot[0] - req_path.start.pose.orientation.y = robot_rot[1] - req_path.start.pose.orientation.z = robot_rot[2] - req_path.start.pose.orientation.w = robot_rot[3] - req_path.goal.header.frame_id = "map" - req_path.goal.pose.position.x = human_trans[0] - req_path.goal.pose.position.y = human_trans[1] - req_path.goal.pose.position.z = human_trans[2] - req_path.goal.pose.orientation.x = 0 - req_path.goal.pose.orientation.y = 0 - req_path.goal.pose.orientation.z = 0 - req_path.goal.pose.orientation.w = 0 - resp = path_client(req_path) - # print "PATH LENGTH: ",len(resp.plan.poses) - path_distance_approach = 0 - if len(resp.plan.poses) > 2: - for i in range(len(resp.plan.poses)-1): - # print "POSE: ",resp.plan.poses[i] - path_distance_approach += math.sqrt(math.pow((resp.plan.poses[i+1].pose.position.x - resp.plan.poses[i].pose.position.x),2) + pow((resp.plan.poses[i+1].pose.position.y - resp.plan.poses[i].pose.position.y), 2)) - - req_path.start.header.frame_id = "map" - req_path.start.pose.position.x = human_trans[0] - req_path.start.pose.position.y = human_trans[1] - req_path.start.pose.position.z = human_trans[2] - req_path.start.pose.orientation.x = human_rot[0] - req_path.start.pose.orientation.y = human_rot[1] - req_path.start.pose.orientation.z = human_rot[2] - req_path.start.pose.orientation.w = human_rot[3] - req_path.goal.header.frame_id = "map" - req_path.goal.pose.position.x = human_dests[human_id][0] - req_path.goal.pose.position.y = human_dests[human_id][1] - req_path.goal.pose.position.z = human_dests[human_id][2] - req_path.goal.pose.orientation.x = 0 - req_path.goal.pose.orientation.y = 0 - req_path.goal.pose.orientation.z = 0 - req_path.goal.pose.orientation.w = 0 - resp = path_client(req_path) - # print "PATH LENGTH: ",len(resp.plan.poses) - path_distance_guide = 0 - if len(resp.plan.poses) > 2: - for i in range(len(resp.plan.poses)-1): - # print "POSE: ",resp.plan.poses[i] - path_distance_guide += math.sqrt(math.pow((resp.plan.poses[i+1].pose.position.x - resp.plan.poses[i].pose.position.x),2) + pow((resp.plan.poses[i+1].pose.position.y - resp.plan.poses[i].pose.position.y), 2)) - if human_posture == "walk": - sp.cost_per_sec = factor_walk[human_id] - cost = path_distance_guide * factor_walk[human_id] - if human_posture == "stand": - sp.cost_per_sec = factor_stand[human_id] - cost = path_distance_approach*factor_stand[human_id] + path_distance_guide * factor_walk[human_id] - if human_posture == "sit": - sp.cost_per_sec = factor_sit[human_id] - cost = path_distance_approach*factor_sit[human_id] + path_distance_guide * factor_walk[human_id] - if human_posture == "ZERO": - sp.cost_per_sec = 0 - cost = 0 - print "COST: ", cost - sp.cost = cost - sp.completion_time = path_distance_approach + path_distance_guide - sp.final_resource_state.robot_position = req_path.goal.pose.position - if path_distance_guide < 2: - print "Human came himself to his destination!" - print "Aborting task" - DA_fsm.terminate() - - return sp - -def ptf_update_task(state_machine,TH_data): - global last_exec_state_name - print "UPDATEING TASK - 3" - print "UPDATE_TASK UPDATE_TASK UPDATE_TASK: ", type(state_machine) - state_machine.add_state("resume_state", resume_state,None) - return ("resume_state", last_exec_state_name) - -def ptf_update_sp(): - return getSP() - -def ptf_get_suspend(last_exec_state_name): - if last_exec_state_name == 'move_to_human': - return "hold_moving" - - elif last_exec_state_name == 'greet': - return "apologize" - - elif last_exec_state_name == 'guide_human': - return "hold_guide" - elif last_exec_state_name == 'goodbye_human': - return None - elif last_exec_state_name == 'go_to_dock': - return None - elif last_exec_state_name == "None": - return None - else: - print "NO SUSPEND STRATEGY FOR THE STATE!!!!!!!" - return return_msg -def ptf_suspend_condition(suspend_request): - global human_posture - global last_exec_state_name - global human_id - robot_trans = [] - human_trans = [] - robot_rot = [] - human_rot = [] - suspend_response = SuspendConditionsResponse() - suspend_strategy = ptf_get_suspend(last_exec_state_name) - if suspend_strategy == "hold_moving": - suspend_response.cost_per_sec = factor_stand[human_id] - else: - suspend_response.cost_per_sec = factor_sit[human_id] - ## - # plan to get cost from final state given by a candidate for interrupting task to the state required by this DA - ## - path_client = rospy.ServiceProxy('/move_base/GlobalPlanner/make_plan', GetPlan) - req_path = GetPlanRequest() - ros_time = rospy.Time() - trans = [] - rate = rospy.Rate(5.0) - while not rospy.is_shutdown(): - try: - actor_frame = "/actor_"+str(human_id) - listener.waitForTransform('/map', actor_frame, ros_time, rospy.Duration(8.0)) - (human_trans,human_rot) = listener.lookupTransform('/map', actor_frame, ros_time) - if len(human_trans) != 0: - break - except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): - rate.sleep() - continue - - path_distance = 0 - req_path.start.header.frame_id = "map" - req_path.start.pose.position = suspend_request.final_resource_state.robot_position - req_path.start.pose.orientation.x = 0 - req_path.start.pose.orientation.y = 0 - req_path.start.pose.orientation.z = 0 - req_path.start.pose.orientation.w = 0 - req_path.goal.header.frame_id = "map" - req_path.goal.pose.position.x = human_trans[0] - req_path.goal.pose.position.y = human_trans[1] - req_path.goal.pose.position.z = 0 - req_path.goal.pose.orientation.x = 0 - req_path.goal.pose.orientation.y = 0 - req_path.goal.pose.orientation.z = 0 - req_path.goal.pose.orientation.w = 0 - resp = path_client(req_path) - # print "PATH LENGTH: ",len(resp.plan.poses) - path_distance_approach = 0 - if len(resp.plan.poses) > 2: - for i in range(len(resp.plan.poses)-1): - # print "POSE: ",resp.plan.poses[i] - path_distance_approach += math.sqrt(math.pow((resp.plan.poses[i+1].pose.position.x - resp.plan.poses[i].pose.position.x),2) + pow((resp.plan.poses[i+1].pose.position.y - resp.plan.poses[i].pose.position.y), 2)) - - ####### - #### - ## - ## TO DO : skalowanie kosztu dla ptf_cost_condition i ptf_suspend_condition - #### - ####### - - suspend_response.cost_to_resume = path_distance_approach * suspend_response.cost_per_sec - return suspend_response - -def ptf_cost_condition(cost_request): - global human_posture - global last_exec_state_name - global human_id - robot_trans = [] - human_trans = [] - robot_rot = [] - human_rot = [] - cost_response = CostConditionsResponse() - ## - # plan to get cost from final state given in request to the state required by this DA - ## - path_client = rospy.ServiceProxy('/move_base/GlobalPlanner/make_plan', GetPlan) - req_path = GetPlanRequest() - ros_time = rospy.Time() - trans = [] - rate = rospy.Rate(5.0) - while not rospy.is_shutdown(): - try: - actor_frame = "/actor_"+str(human_id) - listener.waitForTransform('/map', actor_frame, ros_time, rospy.Duration(8.0)) - (human_trans,human_rot) = listener.lookupTransform('/map', actor_frame, ros_time) - if len(human_trans) != 0: - break - except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): - rate.sleep() - continue - - path_distance = 0 - req_path.start.header.frame_id = "map" - req_path.start.pose.position = cost_request.final_resource_state.robot_position - req_path.start.pose.orientation.x = 0 - req_path.start.pose.orientation.y = 0 - req_path.start.pose.orientation.z = 0 - req_path.start.pose.orientation.w = 0 - req_path.goal.header.frame_id = "map" - req_path.goal.pose.position.x = human_trans[0] - req_path.goal.pose.position.y = human_trans[1] - req_path.goal.pose.position.z = 0 - req_path.goal.pose.orientation.x = 0 - req_path.goal.pose.orientation.y = 0 - req_path.goal.pose.orientation.z = 0 - req_path.goal.pose.orientation.w = 0 - resp = path_client(req_path) - # print "PATH LENGTH: ",len(resp.plan.poses) - path_distance_approach = 0 - if len(resp.plan.poses) > 2: - for i in range(len(resp.plan.poses)-1): - # print "POSE: ",resp.plan.poses[i] - path_distance_approach += math.sqrt(math.pow((resp.plan.poses[i+1].pose.position.x - resp.plan.poses[i].pose.position.x),2) + pow((resp.plan.poses[i+1].pose.position.y - resp.plan.poses[i].pose.position.y), 2)) - if human_posture == "walk": - cost_per_sec = factor_walk[human_id] - if human_posture == "stand": - cost_per_sec = factor_stand[human_id] - if human_posture == "sit": - cost_per_sec = factor_sit[human_id] - if human_posture == "ZERO": - cost_per_sec = 0 - - cost_response.cost_to_complete = path_distance_approach * cost_per_sec - return cost_response -# ====================================================================================== - -if __name__== "__main__": - - try: - global debugging - debugging = False - global startFlag - global my_priority - global my_deadline - global human_poses - global last_exec_state_name - last_exec_state_name = "None" - # global human_last_poses - global human_id - global listener - global DA_fsm - version = sys.argv[1] - da_name = sys.argv[2] - sp = ScheduleParams() - da_ID = int(sys.argv[3]) - stri = sys.argv[4] - rospy.init_node(da_name, anonymous=False) - print "DA: ",stri - # sp = json_message_converter.convert_json_to_ros_message('multitasker/ScheduleParams', stri) - # print "\n\n\nDA.cost: ", sp.cost - # my_cost = sp.cost - # my_completion_time = sp.completion_time - # print ("my_completion_time = ", sp.completion_time) - print ("version = ", version) - if version == "guideHuman-0": - - human_name = "John" - room_number = "10" - human_id = 0 - - elif version == "guideHuman-1": - - human_name = "Mark" - room_number = "15" - human_id = 1 - - else: - - print('\t WRONG da_name...') - rospy.sleep(3) - f_name_1 = "./blockin_log"+version - f_name_2 = "./state_m_log"+version - f1=open(f_name_1, 'w+') - f2=open(f_name_2, 'w+') - - debugging = True - - rospy.init_node(da_name, anonymous=False) - - listener = tf.TransformListener() - rospy.sleep(2) - # wait_for_tiago_init() - add_marker(marker_poses[human_id]) - - DA_fsm = StateMachine(f2, isInterrupting, da_ID, ptf_update_task, ptf_update_sp, ptf_suspend_condition, ptf_cost_condition) - DA_fsm.add_state("Start", start, None) - DA_fsm.add_state("suspending", suspending, None) - DA_fsm.add_state("move_to_human", move_to_human, None) - DA_fsm.add_state("hold_moving", hold_moving, None) - DA_fsm.add_state("greet", greet, None) - DA_fsm.add_state("hold_greet", hold_greet, None) - DA_fsm.add_state("guide_human", guide_human, None) - DA_fsm.add_state("hold_guide", hold_guide, None) - DA_fsm.add_state("goodbye_human", goodbye_human, None) - #DA_fsm.add_state("go_to_dock", go_to_dock, None) # for testing - DA_fsm.add_state("final", final, end_state=True) - DA_fsm.set_start("Start") - DA_fsm.run("Python is great") - #m.run("Python is difficult") - #m.run("Perl is ugly") - - finally: - print "ERROR" - global debugging - remove_marker() - if debugging: - f1.close() - f2.close() - -# ====================================================================================== - diff --git a/bin/tiago_humanFell b/bin/tiago_humanFell deleted file mode 100755 index 9eaf6e1..0000000 --- a/bin/tiago_humanFell +++ /dev/null @@ -1,827 +0,0 @@ -#!/usr/bin/env python2 - -from task_machine.StateMachine import * #StateMachine -from multitasker.srv import LaunchConditions,StartTask -from multitasker.msg import * -from multitasker.srv import * -from rospy_message_converter import json_message_converter -import json - -import rospy -import time -import datetime -import sys - -# ------------------------------------------------------------------ -from tf.transformations import euler_from_quaternion, quaternion_from_euler -import tf -import math -import geometry_msgs.msg -import actionlib -# ------------------------------------------------------------------ -from tiago_utils import * # global constants -from tiago_utils import move_base_set_goal -from tiago_utils import move_base_cancel_goals -from tiago_utils import wait_for_tiago_init -from tiago_utils import action_status_callback -# ------------------------------------------------------------------ - -from nav_msgs.msg import * -from nav_msgs.srv import * -from visualization_msgs.msg import Marker -# -------------------------------- -global listener -global human_id -global human_posture -global return_msg -global fsm_DA -human_posture = "stand" -return_msg = [] -startFlag = False -my_priority = 0 -isInterrupting = False - -my_priority = 0 -my_completion_time = -1 -isInterrupting = False - -dock_pose = [3.70, 1.90, 0.0, 0.0, 0.0, 0.0] # next to entrance | y ~ PI - back to door - -# 'standing' script poses -# =================== -# ======= V1 ======== -# =================== -''' -header: - seq: 0 - stamp: - secs: 105 - nsecs: 373000000 - frame_id: "map" -pose: - position: - x: 0.449645936489 - y: 0.0792762041092 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.97049860128 - w: -0.241106750038 - -human_pose = [0.45, 0.08, 0.0, 0.0, 0.0, -152.5] -''' -# =================== -# ======= V2 ======== -# =================== -''' -header: - seq: 0 - stamp: - secs: 22 - nsecs: 330000000 - frame_id: "map" -pose: - position: - x: -0.1344743222 - y: -0.201279640198 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.999291688955 - w: -0.0376313750156 -''' -#human_pose = [-0.13, 0.20, 0.00, 0.0, 0.0, 35.68] -# =================== - -''' - position: - x: 3.18645572662 - y: 5.37823104858 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.58703750871 - w: 0.80955973428 -''' -#human_dest = [3.18, 5.37, 0.0, 0.0, 0.0, 71.89] - -# human_dest = [2.92, 5.24, 0.0, 0.0, 0.0, 120.0] - -# =================== - -# ------------------------------------------------------------------ - -# Ver2 -human_poses = [] -human_dests = [] -human_last_poses = [] - -# * * * * * * * * * * * * * * ACTOR1 * * * * * * * * * * * * * * * * - -# start -''' -header: - seq: 8 - stamp: - secs: 243 - nsecs: 853000000 - frame_id: "map" -pose: - position: - x: -0.48579788208 - y: 0.14269067347 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.953078234364 - w: -0.302724097457 -''' - -#human_pose = [-0.4857, +0.0000, +0.0000, +0.0000, +0.0000, -144.7597] -#human_poses.append(human_pose) - -''' -header: - seq: 1 - stamp: - secs: 374 - nsecs: 800000000 - frame_id: "map" -pose: - position: - x: -0.320914298296 - y: -0.100545883179 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.986417840476 - w: -0.164255423015 -''' -#human_pose = [-0.3209, -0.1005, +0.0000, +0.0000, +0.0000, -161.0926] - -human_pose = [2, 9, +0.0000, +0.0000, +0.0000, -161.0926] -human_poses.append(human_pose) -human_poses.append(human_pose) -human_poses.append(human_pose) - - -# dest -''' -header: - seq: 9 - stamp: - secs: 292 - nsecs: 174000000 - frame_id: "map" -pose: - position: - x: 3.12596273422 - y: 5.67313241959 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.999757517577 - w: 0.0220205823641 -''' -#human_dest = [+3.1259, +5.6731, +0.0000, 0.0000, 0.0000, +177.4764] -#human_dests.append(human_dest) - -''' -header: - seq: 2 - stamp: - secs: 589 - nsecs: 850000000 - frame_id: "map" -pose: - position: - x: 3.41345524788 - y: 5.586145401 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.985469480681 - w: 0.169852590933 -''' - -human_dest = [+3.4134, +5.5861, +0.0000, 0.0000, 0.0000, +160.4415] -human_dests.append(human_dest) - -# * * * * * * * * * * * * * * ACTOR2 * * * * * * * * * * * * * * * * -# start - -''' -header: - seq: 12 - stamp: - secs: 395 - nsecs: 123000000 - frame_id: "map" -pose: - position: - x: 3.31662845612 - y: 1.70269775391 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: -0.805676184728 - w: 0.592356214927 - -''' -#human_pose = [+3.3166, +1.7026, +0.0000, +0.0000, +0.0000, -107.3513] -#human_poses.append(human_pose) - -''' -header: - seq: 3 - stamp: - secs: 677 - nsecs: 972000000 - frame_id: "map" -pose: - position: - x: 3.22058129311 - y: 1.03116691113 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: -0.776372649676 - w: 0.63027415371 - -''' - -#human_pose = [+3.2205, +1.0311, +0.0000, +0.0000, +0.0000, -101.8592] -human_pose = [3.97165822983, 4.85366201401, +0.0000, +0.0000, +0.0000, -101.8592] -human_poses.append(human_pose) - -# dest - -''' -header: - seq: 11 - stamp: - secs: 375 - nsecs: 113000000 - frame_id: "map" -pose: - position: - x: -0.552233695984 - y: -0.276034355164 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: -0.0662988753729 - w: 0.997799809142 -''' -#human_dest = [-0.5522, -0.2760, +0.0000, 0.0000, 0.0000, -7.6019] -#human_dests.append(human_dest) - -''' -header: - seq: 4 - stamp: - secs: 734 - nsecs: 804000000 - frame_id: "map" -pose: - position: - x: -0.474497079849 - y: -0.41937148571 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.00422636600859 - w: 0.999991068875 -''' - -human_dest = [-0.4744, -0.4193, +0.0000, 0.0000, 0.0000, +0.4842] -human_dests.append(human_dest) - -# ------------------------------------------------------------------ - -human_last_poses = human_poses # in fact - robot's last poses while guiding each human - -# ------------------------------------------------------------------ - -human_name = "" -room_number = "" -global factor_walk -factor_walk = [0,0] -global factor_stand -factor_stand = [0,0] -global factor_sit -factor_sit = [0,0] -# walk factor for human 0 -factor_walk[0] = 1 -# walk factor for human 1 -factor_walk[1] = 2 -# sit factor for human 0 -factor_sit[0] = 0.1 -# sit factor for human 1 -factor_sit[1] = 0.1 -# stand factor for human 0 -factor_stand[0] = 0.1 -# stand factor for human 1 -factor_stand[1] = 0.2 - - -# ====================================================================================== -def publish_human_pose(): - global marker_pub - global marker - marker_pub.publish( marker ); - -def add_marker(pose): - global marker_pub - global marker - global human_id - marker_pub = rospy.Publisher("ellipse", Marker) - marker = Marker(); - marker.header.frame_id = "map"; - marker.header.stamp = rospy.Time.now(); - marker.ns = "human"; - marker.id = human_id; - marker.type = Marker.CYLINDER - marker.action = Marker.ADD - marker.pose.position.x = pose[0] - marker.pose.position.y = pose[1] - marker.pose.position.z = 0.5 - marker.pose.orientation.x = 0.0 - marker.pose.orientation.y = 0.0 - marker.pose.orientation.z = 0.0 - marker.pose.orientation.w = 1.0 - marker.scale.x = 0.3 - marker.scale.y = 0.3 - marker.scale.z = 1 - marker.color.a = 1.0 - marker.color.r = 0.0 - marker.color.g = 1.0 - marker.color.b = 3.0 - human_id - marker_pub.publish( marker ) - -def remove_marker(): - global marker_pub - global marker - global human_id - marker.header.frame_id = "map"; - marker.header.stamp = rospy.Time.now(); - marker.ns = "human"; - marker.id = human_id; - marker.type = Marker.CYLINDER - marker.action = Marker.DELETE - marker_pub.publish( marker ); - -def blocking_call(queue): - - try: - args = queue.get() - i =0 - print(str(datetime.datetime.now().time())+"\n"+ args+"\n") - x = args[0] - y = args[1] - z = args[2] - print(str(datetime.datetime.now().time())+"\n"+ "Z: ", z+"\n") - while i < 100: - x+=1 - y+=1 - z+=1 - time.sleep(1) - i+=1 - print(str(datetime.datetime.now().time())+"\n"+ "computed: "+ str([x, y, z])+"\n") - queue.put([x,y,z]) - finally: - print(str(datetime.datetime.now().time())+"\n"+ "EXCEPTTTTTTTTTTTTTTTTTTTTT"+"\n") - return - -# ====================================================================================== - -def start(cargo_in, event_in, event_out): - - print("\n"+ "----------"+"\n") - print( "START"+"\n") - print( "----------"+"\n") - print("\t cargo: "+ str( cargo_in)+"\n") - print("\t STATE_EVENT: "+ str( event_in.isSet())+"\n") - print( ""+"\n") - - global human_id - - # if event_in.isSet(): - # event_out.set() - # print( ""+"\n") - # print( "set HOLD"+"\n") - # print( ""+"\n") - # return ("first_hold", "data required to hold") - - return ("move_to_human", [human_name, None]) - -# ====================================================================================== - -def move_to_human(cargo_in, event_in, event_out): - global last_exec_state_name - global human_ok - last_exec_state_name = "move_to_human" - - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "move_to_human"+"\n") - print("----------"+"\n") - print( "\t args [, ]: "+ str( cargo_in)+"\n") - print( "\t Get pose of the human "+"\n") - - i = 0 - - global human_poses - global human_id - global return_msg - - # first try - print ( "\t 1st \t human_id: " + str(human_id) + "\t pose hard-coded: " + str(human_poses[human_id]) ) - client = move_base_set_goal( human_poses[human_id] ) - - - while ( client.get_state() != ACTION_STATUS_SUCCEEDED): - - print( "\t Moving to the human: "+ str( cargo_in[0])+"\n") - print( "\t Checking if the destination is reached"+"\n") - #print( "\t o move base STATE: " + str(client.get_state()) +"\n") - - # run_blocking(blocking_call, cargo_in, event) - - if (event_in.isSet() or human_ok==True): - event_out.set() - print( ""+"\n") - print( "set HOLD"+"\n") - print( ""+"\n") - return ("suspending", "move_to_human") - - rospy.sleep(1) - i+=1 - return ("observe_report", None) - -# ====================================================================================== - -def hold_moving(cargo_in): - - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print("hold_moving"+"\n") - print( "----------"+"\n") - print("\t args: "+ str( cargo_in)+"\n") - - move_base_cancel_goals() - - global human_name - global room_number - global human_ok - - # run_blocking(blocking_call, cargo_in, event) - #return ("move_to_human", [human_name, human_last_poses[human_id]]) - if human_ok: - return ("final", None) - return ("move_to_human", return_msg ) - -# ====================================================================================== - -def observe_report(cargo_in, event_in, event_out): - global last_exec_state_name - last_exec_state_name = "observe_report" - - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "observe_report"+"\n") - print("----------"+"\n") - i=0 - print( "\t RECORD_VIDEO\n") - print( "\t SAY_METHOD("+ "Hello, can you hear me? Are yoou OK?"+")\n") - - while i < 5: - - print( "\t Checking if the speech is over"+"\n") - # run_blocking(blocking_call, cargo_in, event) - i=i+1 - rospy.sleep(1) - print( "\t SENDING REPORT\n") - return ("final", None) - -# ====================================================================================== - -def suspending(cargo_in): - - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "suspending"+"\n") - print("----------"+"\n") - print( "\t args: "+ str( cargo_in)+"\n") - i = 0 - # ------- - # getSuspend - # ------- - global return_msg - global last_exec_state_name - global human_posture - last_exec_state_name = cargo_in - - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print("hold_moving"+"\n") - print( "----------"+"\n") - print("\t args: "+ str( cargo_in)+"\n") - - move_base_cancel_goals() - - - # run_blocking(blocking_call, cargo_in, event) - #return ("move_to_human", [human_name, human_last_poses[human_id]]) - - return_msg = [] - return_msg.append(human_name) - return_msg.append(human_last_poses[human_id]) - - return ("resume_state", None) - - - -# ====================================================================================== - -def resume_state(cargo_in, event_in, event_out): - - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "resume_state"+"\n") - print("----------"+"\n") - print( "\t args: "+ str( cargo_in)+"\n") - print("----------"+"\n") - print( "\t interupted state : "+ str( cargo_in[0])+"\n") - i = 0 - global return_msg - return ("move_to_human", return_msg ) - -# ====================================================================================== - -def final(cargo_in, event_in, event_out): - move_base_cancel_goals() - return -def getSP(): - publish_human_pose() - sp = ScheduleParams() - print "CALC COST" - global factor_walk - global factor_stand - global factor_sit - global human_poses - global fsm_DA - global human_ok - path_client = rospy.ServiceProxy('/move_base/GlobalPlanner/make_plan', GetPlan) - req_path = GetPlanRequest() - ros_time = rospy.Time() - trans = [] - rate = rospy.Rate(5.0) - while not rospy.is_shutdown(): - try: - listener.waitForTransform('/map', '/base_link', ros_time, rospy.Duration(8.0)) - (trans,rot) = listener.lookupTransform('/map', '/base_link', ros_time) - if len(trans) != 0: - break - except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): - rate.sleep() - continue - - path_distance = 0 - req_path.start.header.frame_id = "map" - req_path.start.pose.position.x = trans[0] - req_path.start.pose.position.y = trans[1] - req_path.start.pose.position.z = trans[2] - req_path.start.pose.orientation.x = rot[0] - req_path.start.pose.orientation.y = rot[1] - req_path.start.pose.orientation.z = rot[2] - req_path.start.pose.orientation.w = rot[3] - req_path.goal.header.frame_id = "map" - req_path.goal.pose.position.x = human_poses[human_id][0] - req_path.goal.pose.position.y = human_poses[human_id][1] - req_path.goal.pose.position.z = human_poses[human_id][2] - req_path.goal.pose.orientation.x = 0 - req_path.goal.pose.orientation.y = 0 - req_path.goal.pose.orientation.z = 0 - req_path.goal.pose.orientation.w = 0 - resp = path_client(req_path) - # print "PATH LENGTH: ",len(resp.plan.poses) - path_distance_approach = 0 - if len(resp.plan.poses) > 2: - for i in range(len(resp.plan.poses)-1): - # print "POSE: ",resp.plan.poses[i] - path_distance_approach += math.sqrt(math.pow((resp.plan.poses[i+1].pose.position.x - resp.plan.poses[i].pose.position.x),2) + pow((resp.plan.poses[i+1].pose.position.y - resp.plan.poses[i].pose.position.y), 2)) - print "COST: ", path_distance_approach - sp.cost = path_distance_approach - sp.cost_per_sec = 1 - sp.completion_time = path_distance_approach - sp.final_resource_state.robot_position = req_path.goal.pose.position - human_ok = rospy.get_param("~human_ok", False) - if human_ok: - fsm_DA.terminate() - return sp - -def ptf_update_task(state_machine,TH_data): - global last_exec_state_name - print "UPDATEING TASK - 3" - print "UPDATE_TASK UPDATE_TASK UPDATE_TASK: ", type(state_machine) - state_machine.add_state("resume_state", resume_state,None) - return ("resume_state", last_exec_state_name) - -def ptf_update_sp(): - return getSP() - -def ptf_suspend_condition(suspend_request): - global human_posture - global last_exec_state_name - global human_id - global human_poses - suspend_response = SuspendConditionsResponse() - ## - # plan to get cost from final state given by a candidate for interrupting task to the state required by this DA - ## - path_client = rospy.ServiceProxy('/move_base/GlobalPlanner/make_plan', GetPlan) - req_path = GetPlanRequest() - ros_time = rospy.Time() - trans = [] - rate = rospy.Rate(5.0) - while not rospy.is_shutdown(): - try: - listener.waitForTransform('/map', '/base_link', ros_time, rospy.Duration(8.0)) - (trans,rot) = listener.lookupTransform('/map', '/base_link', ros_time) - if len(trans) != 0: - break - except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): - rate.sleep() - continue - - path_distance = 0 - req_path.start.header.frame_id = "map" - req_path.start.pose.position = suspend_request.final_resource_state.robot_position - req_path.start.pose.orientation.x = 0 - req_path.start.pose.orientation.y = 0 - req_path.start.pose.orientation.z = 0 - req_path.start.pose.orientation.w = 0 - req_path.goal.header.frame_id = "map" - req_path.goal.pose.position.x = human_poses[human_id][0] - req_path.goal.pose.position.y = human_poses[human_id][1] - req_path.goal.pose.position.z = human_poses[human_id][2] - req_path.goal.pose.orientation.x = 0 - req_path.goal.pose.orientation.y = 0 - req_path.goal.pose.orientation.z = 0 - req_path.goal.pose.orientation.w = 0 - resp = path_client(req_path) - # print "PATH LENGTH: ",len(resp.plan.poses) - path_distance_approach = 0 - if len(resp.plan.poses) > 2: - for i in range(len(resp.plan.poses)-1): - # print "POSE: ",resp.plan.poses[i] - path_distance_approach += math.sqrt(math.pow((resp.plan.poses[i+1].pose.position.x - resp.plan.poses[i].pose.position.x),2) + pow((resp.plan.poses[i+1].pose.position.y - resp.plan.poses[i].pose.position.y), 2)) - - suspend_response.cost_per_sec = 1 - suspend_response.cost_to_resume = path_distance_approach * suspend_response.cost_per_sec - return suspend_response - -def ptf_cost_condition(cost_request): - global human_posture - global last_exec_state_name - global human_id - global human_poses - cost_response = CostConditionsResponse() - ## - # plan to get cost from final state given in request to the state required by this DA - ## - path_client = rospy.ServiceProxy('/move_base/GlobalPlanner/make_plan', GetPlan) - req_path = GetPlanRequest() - ros_time = rospy.Time() - trans = [] - rate = rospy.Rate(5.0) - while not rospy.is_shutdown(): - try: - listener.waitForTransform('/map', '/base_link', ros_time, rospy.Duration(8.0)) - (trans,rot) = listener.lookupTransform('/map', '/base_link', ros_time) - if len(trans) != 0: - break - except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): - rate.sleep() - continue - - path_distance = 0 - req_path.start.header.frame_id = "map" - req_path.start.pose.position = cost_request.final_resource_state.robot_position - req_path.start.pose.orientation.x = 0 - req_path.start.pose.orientation.y = 0 - req_path.start.pose.orientation.z = 0 - req_path.start.pose.orientation.w = 0 - req_path.goal.header.frame_id = "map" - req_path.goal.pose.position.x = human_poses[human_id][0] - req_path.goal.pose.position.y = human_poses[human_id][1] - req_path.goal.pose.position.z = human_poses[human_id][2] - req_path.goal.pose.orientation.x = 0 - req_path.goal.pose.orientation.y = 0 - req_path.goal.pose.orientation.z = 0 - req_path.goal.pose.orientation.w = 0 - resp = path_client(req_path) - # print "PATH LENGTH: ",len(resp.plan.poses) - path_distance_approach = 0 - if len(resp.plan.poses) > 2: - for i in range(len(resp.plan.poses)-1): - # print "POSE: ",resp.plan.poses[i] - path_distance_approach += math.sqrt(math.pow((resp.plan.poses[i+1].pose.position.x - resp.plan.poses[i].pose.position.x),2) + pow((resp.plan.poses[i+1].pose.position.y - resp.plan.poses[i].pose.position.y), 2)) - cost_per_sec = 1 - cost_response.cost_to_complete = path_distance_approach * cost_per_sec - return cost_response -# ====================================================================================== - -if __name__== "__main__": - - try: - global debugging - debugging = False - global startFlag - global my_priority - global my_deadline - global human_poses - global last_exec_state_name - global marker - global fsm_DA - global human_ok - human_ok = False - marker = Marker() - last_exec_state_name = "None" - # global human_last_poses - global human_id - version = sys.argv[1] - da_name = sys.argv[2] - sp = ScheduleParams() - da_ID = int(sys.argv[3]) - stri = sys.argv[4] - rospy.init_node(da_name, anonymous=False) - print "DA: ",stri - # sp = json_message_converter.convert_json_to_ros_message('multitasker/ScheduleParams', stri) - # print "\n\n\nDA.cost: ", sp.cost - # my_cost = sp.cost - # my_completion_time = sp.completion_time - # print ("my_completion_time = ", sp.completion_time) - print ("version = ", version) - if version == "fell-0": - - human_name = "John" - room_number = "10" - human_id = 2 - - elif version == "fell-1": - - human_name = "Mark" - room_number = "15" - human_id = 3 - - else: - - print('\t WRONG da_name...') - rospy.sleep(3) - f_name_1 = "./blockin_log"+version - f_name_2 = "./state_m_log"+version - f1=open(f_name_1, 'w+') - f2=open(f_name_2, 'w+') - - debugging = True - - rospy.init_node(da_name, anonymous=False) - - listener = tf.TransformListener() - rospy.sleep(2) - # wait_for_tiago_init() - - add_marker(human_poses[human_id]) - fsm_DA = StateMachine(f2, isInterrupting, da_ID, ptf_update_task, ptf_update_sp, ptf_suspend_condition, ptf_cost_condition) - fsm_DA.add_state("Start", start, None) - fsm_DA.add_state("suspending", suspending, None) - fsm_DA.add_state("move_to_human", move_to_human, None) - fsm_DA.add_state("hold_moving", hold_moving, None) - fsm_DA.add_state("observe_report", observe_report, None) - #fsm_DA.add_state("go_to_dock", go_to_dock, None) # for testing - fsm_DA.add_state("final", final, end_state=True) - fsm_DA.set_start("Start") - fsm_DA.run("Python is great") - #fsm_DA.run("Python is difficult") - #m.run("Perl is ugly") - - finally: - print "ERROR" - global human_poses - global human_id - if marker.header.frame_id != "": - remove_marker() - global debugging - if debugging: - f1.close() - f2.close() - -# ====================================================================================== - diff --git a/bin/tiago_test_GH b/bin/tiago_test_GH deleted file mode 100755 index 14c708e..0000000 --- a/bin/tiago_test_GH +++ /dev/null @@ -1,904 +0,0 @@ -#!/usr/bin/env python2 - -from task_machine.StateMachine import * #StateMachine -from multitasker.srv import LaunchConditions,StartTask -from multitasker.msg import * -from multitasker.srv import * -from rospy_message_converter import json_message_converter -import json - -import rospy -import time -import datetime -import sys - -# ------------------------------------------------------------------ -from tf.transformations import euler_from_quaternion, quaternion_from_euler -import tf -import math -import geometry_msgs.msg -import actionlib -# ------------------------------------------------------------------ -from tiago_utils import * # global constants -from tiago_utils import move_base_set_goal -from tiago_utils import move_base_cancel_goals -from tiago_utils import wait_for_tiago_init -from tiago_utils import action_status_callback -# ------------------------------------------------------------------ - -from nav_msgs.msg import * -from nav_msgs.srv import * -# -------------------------------- -global listener -global human_id -global human_posture -global return_msg -human_posture = "stand" -return_msg = [] -startFlag = False -my_priority = 0 -isInterrupting = False - -my_priority = 0 -my_deadline = -1 -isInterrupting = False - -dock_pose = [3.70, 1.90, 0.0, 0.0, 0.0, 0.0] # next to entrance | y ~ PI - back to door - -# 'standing' script poses -# =================== -# ======= V1 ======== -# =================== -''' -header: - seq: 0 - stamp: - secs: 105 - nsecs: 373000000 - frame_id: "map" -pose: - position: - x: 0.449645936489 - y: 0.0792762041092 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.97049860128 - w: -0.241106750038 - -human_pose = [0.45, 0.08, 0.0, 0.0, 0.0, -152.5] -''' -# =================== -# ======= V2 ======== -# =================== -''' -header: - seq: 0 - stamp: - secs: 22 - nsecs: 330000000 - frame_id: "map" -pose: - position: - x: -0.1344743222 - y: -0.201279640198 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.999291688955 - w: -0.0376313750156 -''' -#human_pose = [-0.13, 0.20, 0.00, 0.0, 0.0, 35.68] -# =================== - -''' - position: - x: 3.18645572662 - y: 5.37823104858 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.58703750871 - w: 0.80955973428 -''' -#human_dest = [3.18, 5.37, 0.0, 0.0, 0.0, 71.89] - -# human_dest = [2.92, 5.24, 0.0, 0.0, 0.0, 120.0] - -# =================== - -# ------------------------------------------------------------------ - -# Ver2 -human_poses = [] -human_dests = [] -human_last_poses = [] - -# * * * * * * * * * * * * * * ACTOR1 * * * * * * * * * * * * * * * * - -# start -''' -header: - seq: 8 - stamp: - secs: 243 - nsecs: 853000000 - frame_id: "map" -pose: - position: - x: -0.48579788208 - y: 0.14269067347 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.953078234364 - w: -0.302724097457 -''' - -#human_pose = [-0.4857, +0.0000, +0.0000, +0.0000, +0.0000, -144.7597] -#human_poses.append(human_pose) - -''' -header: - seq: 1 - stamp: - secs: 374 - nsecs: 800000000 - frame_id: "map" -pose: - position: - x: -0.320914298296 - y: -0.100545883179 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.986417840476 - w: -0.164255423015 -''' -#human_pose = [-0.3209, -0.1005, +0.0000, +0.0000, +0.0000, -161.0926] - -human_pose = [-0.4857, +0.1426, +0.0000, +0.0000, +0.0000, -161.0926] -human_poses.append(human_pose) - - -# dest -''' -header: - seq: 9 - stamp: - secs: 292 - nsecs: 174000000 - frame_id: "map" -pose: - position: - x: 3.12596273422 - y: 5.67313241959 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.999757517577 - w: 0.0220205823641 -''' -#human_dest = [+3.1259, +5.6731, +0.0000, 0.0000, 0.0000, +177.4764] -#human_dests.append(human_dest) - -''' -header: - seq: 2 - stamp: - secs: 589 - nsecs: 850000000 - frame_id: "map" -pose: - position: - x: 3.41345524788 - y: 5.586145401 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.985469480681 - w: 0.169852590933 -''' - -human_dest = [+3.4134, +5.5861, +0.0000, 0.0000, 0.0000, +160.4415] -human_dests.append(human_dest) - -# * * * * * * * * * * * * * * ACTOR2 * * * * * * * * * * * * * * * * -# start - -''' -header: - seq: 12 - stamp: - secs: 395 - nsecs: 123000000 - frame_id: "map" -pose: - position: - x: 3.31662845612 - y: 1.70269775391 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: -0.805676184728 - w: 0.592356214927 - -''' -#human_pose = [+3.3166, +1.7026, +0.0000, +0.0000, +0.0000, -107.3513] -#human_poses.append(human_pose) - -''' -header: - seq: 3 - stamp: - secs: 677 - nsecs: 972000000 - frame_id: "map" -pose: - position: - x: 3.22058129311 - y: 1.03116691113 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: -0.776372649676 - w: 0.63027415371 - -''' - -#human_pose = [+3.2205, +1.0311, +0.0000, +0.0000, +0.0000, -101.8592] -human_pose = [+3.3166, +1.7026, +0.0000, +0.0000, +0.0000, -101.8592] -human_poses.append(human_pose) - -# dest - -''' -header: - seq: 11 - stamp: - secs: 375 - nsecs: 113000000 - frame_id: "map" -pose: - position: - x: -0.552233695984 - y: -0.276034355164 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: -0.0662988753729 - w: 0.997799809142 -''' -#human_dest = [-0.5522, -0.2760, +0.0000, 0.0000, 0.0000, -7.6019] -#human_dests.append(human_dest) - -''' -header: - seq: 4 - stamp: - secs: 734 - nsecs: 804000000 - frame_id: "map" -pose: - position: - x: -0.474497079849 - y: -0.41937148571 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.00422636600859 - w: 0.999991068875 -''' - -human_dest = [-0.4744, -0.4193, +0.0000, 0.0000, 0.0000, +0.4842] -human_dests.append(human_dest) - -# ------------------------------------------------------------------ - -human_last_poses = human_poses # in fact - robot's last poses while guiding each human - -# ------------------------------------------------------------------ - -human_name = "" -room_number = "" -global factor_walk -factor_walk = [0,0] -global factor_stand -factor_stand = [0,0] -global factor_sit -factor_sit = [0,0] -# walk factor for human 0 -factor_walk[0] = 1 -# walk factor for human 1 -factor_walk[1] = 1.2 -# sit factor for human 0 -factor_sit[0] = 1 -# sit factor for human 1 -factor_sit[1] = 1.2 -# stand factor for human 0 -factor_stand[0] = 1 -# stand factor for human 1 -factor_stand[1] = 1.2 - -# ====================================================================================== - -def blocking_call(queue): - - try: - args = queue.get() - i =0 - print(str(datetime.datetime.now().time())+"\n"+ args+"\n") - x = args[0] - y = args[1] - z = args[2] - print(str(datetime.datetime.now().time())+"\n"+ "Z: ", z+"\n") - while i < 100: - x+=1 - y+=1 - z+=1 - time.sleep(1) - i+=1 - print(str(datetime.datetime.now().time())+"\n"+ "computed: "+ str([x, y, z])+"\n") - queue.put([x,y,z]) - finally: - print(str(datetime.datetime.now().time())+"\n"+ "EXCEPTTTTTTTTTTTTTTTTTTTTT"+"\n") - return - -# ====================================================================================== - -def start(cargo_in, event_in, event_out): - - print("\n"+ "----------"+"\n") - print( "START"+"\n") - print( "----------"+"\n") - print("\t cargo: "+ str( cargo_in)+"\n") - print("\t STATE_EVENT: "+ str( event_in.isSet())+"\n") - print( ""+"\n") - - global human_id - - # if event_in.isSet(): - # event_out.set() - # print( ""+"\n") - # print( "set HOLD"+"\n") - # print( ""+"\n") - # return ("first_hold", "data required to hold") - - return ("move_to_human", [human_name, None]) - -# ====================================================================================== - -def move_to_human(cargo_in, event_in, event_out): - - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "move_to_human"+"\n") - print("----------"+"\n") - print( "\t args [, ]: "+ str( cargo_in)+"\n") - print( "\t Get pose of the human "+"\n") - - i = 0 - - global human_poses - global human_id - global return_msg - - if ( cargo_in[1] != None ): - - # not the first try - print ( "\t NOT_1st \t human_id: " + str(human_id) + "\t cargo_in[0]: " + str(cargo_in[0]) + "\n \t \t \t \t \t cargo_in[0][1]: " + str(cargo_in[0][1]) + "\n \t \t \t \t \t cargo_in[1]: " + str(cargo_in[1]) + "\n" ) -# client = move_base_set_goal(cargo_in[1]) - - else: - - # first try - print ( "\t 1st \t human_id: " + str(human_id) + "\t pose hard-coded: " + str(human_poses[human_id]) ) -# client = move_base_set_goal( human_poses[human_id] ) - - for i in range(4): - # while ( client.get_state() != ACTION_STATUS_SUCCEEDED ): - - print( "\t Moving to the human: "+ str( cargo_in[0])+"\n") - print( "\t Checking if the destination is reached"+"\n") - #print( "\t o move base STATE: " + str(client.get_state()) +"\n") - - # run_blocking(blocking_call, cargo_in, event) - - if event_in.isSet(): - event_out.set() - print( ""+"\n") - print( "set HOLD"+"\n") - print( ""+"\n") - return ("suspending", "move_to_human") - - time.sleep(1) - i+=1 - - say = "" - if cargo_in[1] == None: - say = "Hello, follow me please." - else: - say = "Hello again, follow me please" - - return ("greet", say) - -# ====================================================================================== - -def hold_moving(cargo_in): - - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print("hold_moving"+"\n") - print( "----------"+"\n") - print("\t args: "+ str( cargo_in)+"\n") - - move_base_cancel_goals() - - global human_name - global room_number - - # run_blocking(blocking_call, cargo_in, event) - #return ("move_to_human", [human_name, human_last_poses[human_id]]) - - return_msg = [] - return_msg.append(human_name) - return_msg.append(human_last_poses[human_id]) - - return ("move_to_human", return_msg ) - -# ====================================================================================== - -def greet(cargo_in, event_in, event_out): - - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "greet"+"\n") - print("----------"+"\n") - i=0 - print( "\t SAY_METHOD("+ str( cargo_in)+")\n") - - while i < 5: - - print( "\t Checking if the speech is over"+"\n") - # run_blocking(blocking_call, cargo_in, event) - if event_in.isSet(): - event_out.set() - print( ""+"\n") - print( "set HOLD"+"\n") - print( ""+"\n") - return ("suspending", "greet") - i+=1 - time.sleep(1) - - return ("guide_human", room_number) - -# ====================================================================================== - -def hold_greet(cargo_in): - - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "SAY"+"\n") - print("----------"+"\n") - print( "\t SAY_METHOD("+ str( cargo_in)+")\n") - - #return ("move_to_human", [human_name, human_last_poses[human_id]]) - - return_msg = [] - return_msg.append(human_name) - return_msg.append(human_last_poses[human_id]) - - return ("move_to_human", return_msg ) - -# ====================================================================================== - -def guide_human(cargo_in, event_in, event_out): - - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "guide_human"+"\n") - print("----------"+"\n") - print( "\t args : "+ str( cargo_in)+"\n") - i = 0 - - global human_dests - global human_last_poses - global human_id - global human_posture - -# client = move_base_set_goal(human_dests[human_id]) - human_posture = "walk" - for i in range(2): -# while ( client.get_state() != ACTION_STATUS_SUCCEEDED ): - - - print( "\t Moving to the destination: "+ str( cargo_in)+"\n") - print( "\t Checking if the human is following me"+"\n") # not checking really - print( "\t o move base STATE: " + str(client.get_state()) +"\n") - - #if ( client.get_state() == ACTION_STATUS_ABORTED ): - # print( "\t o move base STATE: ABORTED, a valid plan could not be found \n") - # break - - # save the last robot pose in case of guide interrupt - (trans,rot) = listener.lookupTransform('/map', '/base_link', rospy.Time(0)) - (roll, pitch, yaw) = euler_from_quaternion( [rot[0], rot[1], rot[2], rot[3]] ) - roll *= (180.0 / math.pi) - pitch *= (180.0 / math.pi) - yaw *= (180.0 / math.pi) - human_last_pose_temp = [trans[0], trans[1], trans[2], roll, pitch, yaw] - print ( "\t human_last_pose: " + str(human_last_pose_temp) ) - human_last_poses[human_id] = human_last_pose_temp - - if event_in.isSet(): - event_out.set() - print( ""+"\n") - print( "set HOLD"+"\n") - print( ""+"\n") - return ("suspending", "guide_human") - - i+=1 - time.sleep(1) - - return ("goodbye_human", None) - -# ====================================================================================== - -def hold_guide(cargo_in): - - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print("hold_guide"+"\n") - - move_base_cancel_goals() - global human_last_poses - global human_id - - print( "----------"+"\n") - print("\t args: "+ str( cargo_in)+"\n") - print( "\t Approaching human "+"\n") - print( "\t SAY_METHOD("+ "Oh! I'm sorry but I got a very important request. Please wait, I'll come back"+")\n") - print( "\t Go aside "+"\n") - - #return ("move_to_human", [human_name, human_last_poses[human_id]]) - - return_msg = [] - return_msg.append(human_name) - return_msg.append(human_last_poses[human_id]) - - return ("move_to_human", return_msg ) - -# ====================================================================================== - -def goodbye_human(cargo_in, event_in, event_out): - - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "goodbye_human"+"\n") - print("----------"+"\n") - print( "\t args : "+ str( cargo_in)+"\n") - print( "\t SAY_METHOD("+ "Here is the room "+room_number+"\n") - i = 0 - - while i < 5: - print( "\t Checking if the speech is over"+"\n") - print( "\t HOLD IS UNAVAILABLE"+"\n") - - i+=1 - time.sleep(1) - - print( "\t WILL GO_TO_DOCK" + "\n") - - return ("go_to_dock", None) - -# ====================================================================================== - -def go_to_dock(cargo_in, event_in, event_out): - - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "go_to_dock"+"\n") - print("----------"+"\n") - print( "\t args: "+ str( cargo_in)+"\n") - i = 0 - - global dock_pose -# client = move_base_set_goal(dock_pose) - for i in range(2): -# while ( client.get_state() != ACTION_STATUS_SUCCEEDED ): - - print( "\t Moving to my dock"+")\n") - #print( "\t o move base STATE: " + str(client.get_state()) +"\n") - - if event_in.isSet(): - event_out.set() - print( ""+"\n") - print( "set HOLD"+"\n") - print( ""+"\n") - return ("final", None) - i+=1 - time.sleep(1) - - return ("final", None) - -# ====================================================================================== - -def suspending(cargo_in): - - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "suspending"+"\n") - print("----------"+"\n") - print( "\t args: "+ str( cargo_in)+"\n") - i = 0 - # ------- - # getSuspend - # ------- - global return_msg - global last_state_name - global human_posture - last_state_name = cargo_in - strategy = ptf_get_suspend(last_state_name=cargo_in) - # ------- - # execSuspend - # ------- - if strategy == "hold_moving": - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print("hold_moving"+"\n") - print( "----------"+"\n") - print("\t args: "+ str( cargo_in)+"\n") - - #move_base_cancel_goals() - - - # run_blocking(blocking_call, cargo_in, event) - #return ("move_to_human", [human_name, human_last_poses[human_id]]) - - return_msg = [] - return_msg.append(human_name) - return_msg.append(human_last_poses[human_id]) - elif strategy == "apologize": - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "SAY"+"\n") - print("----------"+"\n") - print( "\t SAY_METHOD(Oh! I'm sorry but I got very important request. Please be sitted and wait, I'll come back)\n") - human_posture = "sit" - - #return ("move_to_human", [human_name, human_last_poses[human_id]]) - - return_msg = [] - return_msg.append(human_name) - return_msg.append(human_last_poses[human_id]) - elif strategy == "hold_guide": - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print("hold_guide"+"\n") - - #move_base_cancel_goals() - - print( "----------"+"\n") - print("\t args: "+ str( cargo_in)+"\n") - print( "\t Approaching human "+"\n") - print( "\t SAY_METHOD("+ "Oh! I'm sorry but I got a very important request. Please be sitted and wait, I'll come back"+")\n") - print( "\t Go aside "+"\n") - human_posture = "sit" - - #return ("move_to_human", [human_name, human_last_poses[human_id]]) - - return_msg = [] - return_msg.append(human_name) - return_msg.append(human_last_poses[human_id]) - return ("resume_state", None) - - - -# ====================================================================================== - -def resume_state(cargo_in, event_in, event_out): - - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "resume_state"+"\n") - print("----------"+"\n") - print( "\t args: "+ str( cargo_in)+"\n") - print("----------"+"\n") - print( "\t interupted state : "+ str( cargo_in[0])+"\n") - i = 0 - global return_msg - if cargo_in == 'move_to_human': - return ("move_to_human", return_msg ) - - if cargo_in == 'greet': - return ("move_to_human", return_msg ) - - if cargo_in == 'guide_human': - return ("move_to_human", return_msg ) - else: - print "NO RESUME STRATEGY FOR THE STATE!!!!!!!" - return ("final", cargo_in) - -# ====================================================================================== - -def final(cargo_in): - #move_base_cancel_goals() - pass - return -def getCost(): - print "CALC COST" - global human_posture - global factor_walk - global factor_stand - global factor_sit - print "create srv" - path_client = rospy.ServiceProxy('/move_base/GlobalPlanner/make_plan', GetPlan) - req_path = GetPlanRequest() - print "call wait" - listener.waitForTransform('/map', '/base_link', rospy.Time(), rospy.Duration(4.0)) - print "call listener" - (trans,rot) = listener.lookupTransform('/map', '/base_link', rospy.Time()) - path_distance = 0 - req_path.start.header.frame_id = "map" - req_path.start.pose.position.x = trans[0] - req_path.start.pose.position.y = trans[1] - req_path.start.pose.position.z = trans[2] - req_path.start.pose.orientation.x = rot[0] - req_path.start.pose.orientation.y = rot[1] - req_path.start.pose.orientation.z = rot[2] - req_path.start.pose.orientation.w = rot[3] - req_path.goal.header.frame_id = "map" - req_path.goal.pose.position.x = human_last_poses[human_id][0] - req_path.goal.pose.position.y = human_last_poses[human_id][1] - req_path.goal.pose.position.z = human_last_poses[human_id][2] - req_path.goal.pose.orientation.x = 0 - req_path.goal.pose.orientation.y = 0 - req_path.goal.pose.orientation.z = 0 - req_path.goal.pose.orientation.w = 0 - print "call plan" - # resp = path_client(req_path) - # # print "PATH LENGTH: ",len(resp.plan.poses) - # path_distance_approach = 0 - # if len(resp.plan.poses) > 2: - # for i in range(len(resp.plan.poses)-1): - # # print "POSE: ",resp.plan.poses[i] - # path_distance_approach += math.sqrt(math.pow((resp.plan.poses[i+1].pose.position.x - resp.plan.poses[i].pose.position.x),2) + pow((resp.plan.poses[i+1].pose.position.y - resp.plan.poses[i].pose.position.y), 2)) - path_distance_approach = 5 - req_path.start.header.frame_id = "map" - req_path.start.pose.position.x = human_last_poses[human_id][0] - req_path.start.pose.position.y = human_last_poses[human_id][1] - req_path.start.pose.position.z = human_last_poses[human_id][2] - req_path.start.pose.orientation.x = human_last_poses[human_id][0] - req_path.start.pose.orientation.y = human_last_poses[human_id][1] - req_path.start.pose.orientation.z = human_last_poses[human_id][2] - req_path.start.pose.orientation.w = human_last_poses[human_id][3] - req_path.goal.header.frame_id = "map" - req_path.goal.pose.position.x = human_dests[human_id][0] - req_path.goal.pose.position.y = human_dests[human_id][1] - req_path.goal.pose.position.z = human_dests[human_id][2] - req_path.goal.pose.orientation.x = 0 - req_path.goal.pose.orientation.y = 0 - req_path.goal.pose.orientation.z = 0 - req_path.goal.pose.orientation.w = 0 - print "call plan-2" - # resp = path_client(req_path) - # # print "PATH LENGTH: ",len(resp.plan.poses) - # path_distance_guide = 0 - # if len(resp.plan.poses) > 2: - # for i in range(len(resp.plan.poses)-1): - # # print "POSE: ",resp.plan.poses[i] - # path_distance_guide += math.sqrt(math.pow((resp.plan.poses[i+1].pose.position.x - resp.plan.poses[i].pose.position.x),2) + pow((resp.plan.poses[i+1].pose.position.y - resp.plan.poses[i].pose.position.y), 2)) - path_distance_guide = 2 - if human_posture == "walk": - cost = path_distance_guide * factor_walk[human_id] - if human_posture == "stand": - cost = path_distance_approach*factor_stand[human_id] + path_distance_guide * factor_walk[human_id] - if human_posture == "sit": - cost = path_distance_approach*factor_sit[human_id] + path_distance_guide * factor_walk[human_id] - print "COST: ", cost - return cost -def getPriority(): - global my_priority - my_priority = getCost() - return my_priority -def getDeadline(): - global my_deadline - return my_deadline - - -def ptf_update_task(state_machine,TH_data): - global last_state_name - print "UPDATEING TASK - 3" - print "UPDATE_TASK UPDATE_TASK UPDATE_TASK: ", type(state_machine) - state_machine.add_state("resume_state", resume_state,None) - return ("resume_state", last_state_name) - -def ptf_update_sp(): - sp = ScheduleParams() - sp.priority = getPriority() - sp.deadline = getDeadline() - return sp - -def ptf_get_suspend(last_state_name): - if last_state_name == 'move_to_human': - return "hold_moving" - - elif last_state_name == 'greet': - return "apologize" - - elif last_state_name == 'guide_human': - return "hold_guide" - else: - print "NO SUSPEND STRATEGY FOR THE STATE!!!!!!!" - return return_msg -# ====================================================================================== - -if __name__== "__main__": - - try: - global startFlag - global my_priority - global my_deadline - global human_pose - # global human_last_poses - global human_id - version = sys.argv[1] - da_name = sys.argv[2] - sp = ScheduleParams() - da_ID = int(sys.argv[3]) - stri = sys.argv[4] - rospy.init_node(da_name, anonymous=False) - print "DA: ",stri - sp = json_message_converter.convert_json_to_ros_message('multitasker/ScheduleParams', stri) - print "\n\n\nDA.priority: ", sp.priority - my_priority = sp.priority - my_deadline = sp.deadline - print ("my_deadline = ", sp.deadline) - print ("version = ", version) - if version == "guideHuman-0": - - human_name = "John" - room_number = "10" - human_id = 0 - - elif version == "guideHuman-1": - - human_name = "Mark" - room_number = "15" - human_id = 1 - - else: - - print('\t WRONG da_name...') - time.sleep(30) - - f1=open('./blockin_log', 'w+') - f2=open('./state_m_log', 'w+') - rospy.init_node(da_name, anonymous=False) - - listener = tf.TransformListener() - # wait_for_tiago_init() - - m = StateMachine(f2, isInterrupting, da_ID, ptf_update_task, ptf_update_sp) - m.add_state("Start", start, None) - m.add_state("suspending", suspending, None) - m.add_state("move_to_human", move_to_human, None) - m.add_state("hold_moving", hold_moving, None) - m.add_state("greet", greet, None) - m.add_state("hold_greet", hold_greet, None) - m.add_state("guide_human", guide_human, None) - m.add_state("hold_guide", hold_guide, None) - m.add_state("goodbye_human", goodbye_human, None) - m.add_state("go_to_dock", go_to_dock, None, facultative = True) - #m.add_state("go_to_dock", go_to_dock, None) # for testing - m.add_state("final", final, end_state=True) - m.set_start("Start") - m.run("Python is great") - #m.run("Python is difficult") - #m.run("Perl is ugly") - - finally: - print "ERROR" - f1.close() - f2.close() - -# ====================================================================================== - diff --git a/bin/tiago_utils.py b/bin/tiago_utils.py deleted file mode 100644 index 0765219..0000000 --- a/bin/tiago_utils.py +++ /dev/null @@ -1,177 +0,0 @@ -# ------------------------------------------------------------------ -import rospy -import time -# ------------------------------------------------------------------ -import actionlib -from move_base_msgs.msg import * -from geometry_msgs.msg import PoseStamped -from actionlib_msgs.msg import GoalStatus -from actionlib_msgs.msg import GoalStatusArray -from tf.transformations import quaternion_from_euler -from play_motion_msgs.msg import PlayMotionAction, PlayMotionGoal -# ------------------------------------------------------------------ -# action status enum -# http://docs.ros.org/kinetic/api/actionlib_msgs/html/msg/GoalStatus.html -ACTION_STATUS_PENDING = 0 -ACTION_STATUS_ACTIVE = 1 -ACTION_STATUS_SUCCEEDED = 3 -ACTION_STATUS_ABORTED = 4 -ACTION_STATUS_REJECTED = 5 -# ------------------------------------------------------------------ -# action_*** connected with the process of 'homing' the arm of TIAGo -action_current_status = ACTION_STATUS_PENDING -action_current_id = "unknown" -# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -def action_status_callback(msg): - - global action_current_status - global action_current_id - - status_list = GoalStatus() - status_list = msg.status_list - # print(str("*** CB --- ACTION --- LENGTH OF A GOAL STATUS LIST IS:" + str(len(status_list)))) - - if ( len(status_list) != 1 ): - # assuming that there will always be 1 goal status - # print(str("*** CB --- ACTION --- LENGTH OF A GOAL STATUS LIST IS NOT 1 but " + str(len(status_list)))) - return - - id_goal_str = status_list[0].goal_id.id - # print(str("*** CB --- id_goal_str:" + str(id_goal_str) )) - - # tuck_arm action is executed first, robot will wait until its finish - idx = id_goal_str.find("tuck_arm") - if ( idx != -1 ): - # found - action_current_id = "tuck_arm" - action_current_status = status_list[0].status - # print("========== tuck_arm WAS FOUND ======= \n") - # print("========== tuck_arm STATUS " + str(action_current_status) + " ======= \n") - -# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -def wait_for_tiago_init(): - - global action_current_status - global action_current_id - - # to check the status of the tuck_arm action - rospy.Subscriber('/play_motion/status', GoalStatusArray, action_status_callback) - - # for a moment after node launching the time is 'off' - while ( rospy.get_time() < 1 ): - pass - - print( "\t ============= TIAGo Initialization... ================ ") - - start_time = rospy.get_time() - - while ( action_current_id == "unknown" ): - # wait for the first callback... - print( "\t ===== Waiting for the first action callback... =======") - rospy.sleep(1.) - if ( rospy.get_time() - start_time > 5.0 ): - print( "\t ====== Assuming TIAGo is already initialized ========") - return - - while ( action_current_id == "tuck_arm" ): - - # wait until manipulator's "homing" is finished - if ( action_current_status == 3 ): - break - - print( "\t ============ Tucking arm... Waiting... ===============") - rospy.sleep(1.) - -# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -#def create_move_base_goal(pos_x=0, pos_y=0, pos_z=0, orient_r=0, orient_p=0, orient_y=0): -def create_move_base_goal(pose): - - goal = MoveBaseGoal() - - ''' - goal.target_pose.pose.position.x = pos_x - goal.target_pose.pose.position.y = pos_y - goal.target_pose.pose.position.z = pos_z - ''' - - goal.target_pose.pose.position.x = pose[0] - goal.target_pose.pose.position.y = pose[1] - goal.target_pose.pose.position.z = pose[2] - - # q = quaternion_from_euler(orient_r, orient_p, orient_y) - q = quaternion_from_euler(pose[3], pose[4], pose[5]) - - goal.target_pose.pose.orientation.x = q[0] - goal.target_pose.pose.orientation.y = q[1] - goal.target_pose.pose.orientation.z = q[2] - goal.target_pose.pose.orientation.w = q[3] - goal.target_pose.header.frame_id = 'map' - goal.target_pose.header.stamp = rospy.Time.now() - - return goal - -# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -def move_base_set_goal(pose): - - client = actionlib.SimpleActionClient('move_base', MoveBaseAction) - goal = create_move_base_goal(pose) - client.wait_for_server() - client.send_goal(goal) - - return client - -# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -def move_base_cancel_goals(): - - client = actionlib.SimpleActionClient('move_base', MoveBaseAction) - client.wait_for_server() - client.cancel_all_goals() - -# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -def play_motion_start_action(act_name): - - client = actionlib.SimpleActionClient('/play_motion', PlayMotionAction) - - goal = PlayMotionGoal() - - if ( act_name == "head_down" or act_name == "head_tilt_down" ): - goal.motion_name = "head_tilt_down" - elif ( act_name == "head_normal" or act_name == "head_straight" ): - goal.motion_name = "head_normal" - elif ( act_name == "look_around" or act_name == "head_tour" ): - goal.motion_name = "head_pan_tilted" - elif ( act_name == "give_hand" or act_name == "offer_hand" ): - goal.motion_name = "reach_floor" - elif ( act_name == "home" ): - goal.motion_name = "home" - else: - # at the moment no more will be handled but there are plenty of more movements - print( "\t Wrong play_motion name...") - return - - goal.skip_planning = False - goal.priority = 0 - - client.wait_for_server() - client.send_goal(goal) - - ''' - action_ok = client.wait_for_result(rospy.Duration(2.5)) - state = client.get_state() - - if action_ok: - rospy.loginfo("Action finished succesfully with state: " + str(get_status_string(state))) - else: - rospy.logwarn("Action failed with state: " + str(get_status_string(state))) - ''' - - return client - -# ------------------------------------------------------------------ - diff --git a/bin/txt_guideHuman b/bin/txt_guideHuman deleted file mode 100755 index 46d5eb5..0000000 --- a/bin/txt_guideHuman +++ /dev/null @@ -1,251 +0,0 @@ -#!/usr/bin/env python - -from task_machine.StateMachine import * #StateMachine -import rospy -import time -import datetime -import sys -from multitasker.srv import LaunchConditions,StartTask - -human_name = "" -room_number = "" -startFlag = False -my_priority = 0 -my_deadline = -1 -isInterrupting = False - -def blocking_call(queue): - try: - args = queue.get() - i =0 - print(str(datetime.datetime.now().time())+"\n"+ args+"\n") - x = args[0] - y = args[1] - z = args[2] - print(str(datetime.datetime.now().time())+"\n"+ "Z: ", z+"\n") - while i < 100: - x+=1 - y+=1 - z+=1 - time.sleep(1) - i+=1 - print(str(datetime.datetime.now().time())+"\n"+ "computed: "+ str([x, y, z])+"\n") - queue.put([x,y,z]) - finally: - print(str(datetime.datetime.now().time())+"\n"+ "EXCEPTTTTTTTTTTTTTTTTTTTTT"+"\n") - return - -def start(cargo_in, event_in, event_out): - print("\n"+ "----------"+"\n") - print( "START"+"\n") - print( "----------"+"\n") - print("\t cargo: "+ str( cargo_in)+"\n") - print("\t STATE_EVENT: "+ str( event_in.isSet())+"\n") - print( ""+"\n") - if event_in.isSet(): - event_out.set() - print( ""+"\n") - print( "set HOLD"+"\n") - print( ""+"\n") - return ("first_hold", "data required to hold") - return ("move_to_human", [human_name, None]) - -def move_to_human(cargo_in, event_in, event_out): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "move_to_human"+"\n") - print("----------"+"\n") - print( "\t args [, ]: "+ str( cargo_in)+"\n") - print( "\t Get pose of the human "+"\n") - i = 0 - while i < 10: - print( "\t Moving to the human: "+ str( cargo_in[0])+"\n") - print( "\t Checking if the destination is reached"+"\n") - # run_blocking(blocking_call, cargo_in, event) - if event_in.isSet(): - event_out.set() - print( ""+"\n") - print( "set HOLD"+"\n") - print( ""+"\n") - return ("hold_moving", "stop robot") - time.sleep(1) - i+=1 - say = "" - if cargo_in[1] == None: - say = "Hello, follow me please." - else: - say = "Hallo again, follow me please" - return ("greet", say) - -def hold_moving(cargo_in): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print("hold_moving"+"\n") - print( "----------"+"\n") - print("\t args: "+ str( cargo_in)+"\n") - global human_name - global room_number - # run_blocking(blocking_call, cargo_in, event) - return ("move_to_human", [human_name,[10,20]]) - - -def greet(cargo_in, event_in, event_out): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "greet"+"\n") - print("----------"+"\n") - - i=0 - print( "\t SAY_METHOD("+ str( cargo_in)+")\n") - while i < 5: - print( "\t Checking if the speech is over"+"\n") - # run_blocking(blocking_call, cargo_in, event) - if event_in.isSet(): - event_out.set() - print( ""+"\n") - print( "set HOLD"+"\n") - print( ""+"\n") - return ("hold_greet", "Oh! I'm sorry but I got very important request. Please wait, I'll come back") - i+=1 - time.sleep(1) - return ("guide_human", room_number) - -def hold_greet(cargo_in): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "SAY"+"\n") - print("----------"+"\n") - print( "\t SAY_METHOD("+ str( cargo_in)+")\n") - return ("move_to_human", [1,2]) - -def guide_human(cargo_in, event_in, event_out): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "guide_human"+"\n") - print("----------"+"\n") - print( "\t args : "+ str( cargo_in)+"\n") - i = 0 - while i < 10: - print( "\t Moving to the destination: "+ str( cargo_in)+"\n") - print( "\t Checking if the human is following me"+"\n") - if event_in.isSet(): - event_out.set() - print( ""+"\n") - print( "set HOLD"+"\n") - print( ""+"\n") - return ("hold_guide", None) - i+=1 - time.sleep(1) - return ("goodbye_human", None) - -def hold_guide(cargo_in): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print("hold_guide"+"\n") - print( "----------"+"\n") - print("\t args: "+ str( cargo_in)+"\n") - print( "\t Approaching human "+"\n") - print( "\t SAY_METHOD("+ "Oh! I'm sorry but I got a very important request. Please wait, I'll come back"+")\n") - print( "\t Go aside "+"\n") - return ("move_to_human", [human_name,[10,20]]) - -def goodbye_human(cargo_in, event_in, event_out): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "goodbye_human"+"\n") - print("----------"+"\n") - print( "\t args : "+ str( cargo_in)+"\n") - print( "\t SAY_METHOD("+ "Here is the room "+room_number+"\n") - i = 0 - while i < 5: - print( "\t Checking if the speech is over"+"\n") - print( "\t HOLD IS UNAVAILABLE"+"\n") - - i+=1 - time.sleep(1) - return ("go_to_dock", None) - -def go_to_dock(cargo_in, event_in, event_out): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "go_to_dock"+"\n") - print("----------"+"\n") - print( "\t args: "+ str( cargo_in)+"\n") - i = 0 - while i < 5: - print( "\t Moving to my dock"+")\n") - if event_in.isSet(): - event_out.set() - print( ""+"\n") - print( "set HOLD"+"\n") - print( ""+"\n") - return ("final", None) - i+=1 - time.sleep(1) - return ("final", None) - -def final(cargo_in): - return -def startTask(data): - global startFlag - global isInterrupting - isInterrupting = data.isInterrupting - startFlag = True - return StartTaskResponse() - -def getPriority(): - global my_priority - return my_priority -def getDeadline(): - global my_deadline - return my_deadline - -def getLaunchConditions(req): - l_conditions = LaunchConditionsResponse() - l_conditions.start_deadline = getDeadline() - l_conditions.task_priority = getPriority() - return l_conditions - -if __name__== "__main__": - try: - da_name = sys.argv[1] - global my_priority - my_priority = int(sys.argv[2]) - global my_deadline - my_deadline = int(sys.argv[3]) - - global human_name - global room_number - global startFlag - global isInterrupting - - if da_name == "guideHuman-1": - human_name = "John" - room_number = "10" - else: - human_name = "Alice" - room_number = "20" - rospy.init_node(da_name, anonymous=False) - f1=open('./blockin_log', 'w+') - f2=open('./state_m_log', 'w+') - node_namespace = rospy.get_name() + "/multitasking" - cond_name = node_namespace + "/get_launch_conditions" - start_name = node_namespace + "/startTask" - conditions_srv = rospy.Service(cond_name, LaunchConditions, getLaunchConditions) - start_srv = rospy.Service(start_name, StartTask, startTask) - r = rospy.Rate(10) - while not rospy.is_shutdown(): - if startFlag: - break - r.sleep() - if startFlag: - m = StateMachine(f2, isInterrupting) - m.add_state("Start", start, None) - m.add_state("move_to_human", move_to_human, None) - m.add_state("hold_moving", hold_moving, None) - m.add_state("greet", greet, None) - m.add_state("hold_greet", hold_greet, None) - m.add_state("guide_human", guide_human, None) - m.add_state("hold_guide", hold_guide, None) - m.add_state("goodbye_human", goodbye_human, None) - m.add_state("go_to_dock", go_to_dock, None, facultative = True) - m.add_state("final", final, end_state=True) - m.set_start("Start") - m.run("Python is great") - #m.run("Python is difficult") - #m.run("Perl is ugly") - finally: - f2.close() - f1.close() \ No newline at end of file diff --git a/bin/txt_hazardDetection b/bin/txt_hazardDetection deleted file mode 100755 index cb65b53..0000000 --- a/bin/txt_hazardDetection +++ /dev/null @@ -1,168 +0,0 @@ -#!/usr/bin/env python - -from task_machine.StateMachine import * #StateMachine -import rospy -import time -import datetime - - -def start(cargo_in, event_in, event_out): - print("\n"+ "----------"+"\n") - print( "START"+"\n") - print( "----------"+"\n") - print("\t cargo: "+ str( cargo_in)+"\n") - print("\t STATE_EVENT: "+ str( event_in.isSet())+"\n") - print( ""+"\n") - if event_in.isSet(): - event_out.set() - print( ""+"\n") - print( "set HOLD"+"\n") - print( ""+"\n") - return ("first_hold", "data required to hold") - return ("move_to_human", ["Zdzisiu", None]) - -def move_to_human(cargo_in, event_in, event_out): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "move_to_human"+"\n") - print("----------"+"\n") - print( "\t args [, ]: "+ str( cargo_in)+"\n") - print( "\t Get pose of the human "+"\n") - i = 0 - while i < 10: - print( "\t Moving to the human: "+ str( cargo_in[0])+"\n") - print( "\t Checking if the destination is reached"+"\n") - # run_blocking(blocking_call, cargo_in, event) - if event_in.isSet(): - event_out.set() - print( ""+"\n") - print( "set HOLD"+"\n") - print( ""+"\n") - return ("hold_moving", "stop robot") - time.sleep(1) - i+=1 - return ("greet", "Hello, your examination will be proceed in room 10. Please follow me") - -def hold_moving(cargo_in): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print("hold_moving"+"\n") - print( "----------"+"\n") - print("\t args: "+ str( cargo_in)+"\n") - # run_blocking(blocking_call, cargo_in, event) - return ("move_to_human", ["Zdzisiu",[10,20]]) - - -def greet(cargo_in, event_in, event_out): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "greet"+"\n") - print("----------"+"\n") - - i=0 - print( "\t SAY_METHOD("+ str( cargo_in)+")\n") - while i < 5: - print( "\t Checking if the speech is over"+"\n") - # run_blocking(blocking_call, cargo_in, event) - if event_in.isSet(): - event_out.set() - print( ""+"\n") - print( "set HOLD"+"\n") - print( ""+"\n") - return ("hold_greet", "Oh! I'm sorry but I got very important request. Please wait, I'll come back") - i+=1 - time.sleep(1) - return ("guide_human", "room 10") - -def hold_greet(cargo_in): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "SAY"+"\n") - print("----------"+"\n") - print( "\t SAY_METHOD("+ str( cargo_in)+")\n") - return ("move_to_human", [1,2]) - -def guide_human(cargo_in, event_in, event_out): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "guide_human"+"\n") - print("----------"+"\n") - print( "\t args : "+ str( cargo_in)+"\n") - i = 0 - while i < 10: - print( "\t Moving to the destination: "+ str( cargo_in)+"\n") - print( "\t Checking if the human is following me"+"\n") - if event_in.isSet(): - event_out.set() - print( ""+"\n") - print( "set HOLD"+"\n") - print( ""+"\n") - return ("hold_guide", None) - i+=1 - time.sleep(1) - return ("goodbye_human", None) - -def hold_guide(cargo_in): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print("hold_guide"+"\n") - print( "----------"+"\n") - print("\t args: "+ str( cargo_in)+"\n") - print( "\t Approaching human "+"\n") - print( "\t SAY_METHOD("+ "Oh! I'm sorry but I got a very important request. Please wait, I'll come back"+")\n") - print( "\t Go aside "+"\n") - return ("move_to_human", ["Zdzisiu",[10,20]]) - -def goodbye_human(cargo_in, event_in, event_out): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "goodbye_human"+"\n") - print("----------"+"\n") - print( "\t args : "+ str( cargo_in)+"\n") - print( "\t SAY_METHOD("+ "Here is the room 10"+")\n") - i = 0 - while i < 5: - print( "\t Checking if the speech is over"+"\n") - print( "\t HOLD IS UNAVAILABLE"+"\n") - - i+=1 - time.sleep(1) - return ("go_to_dock", None) - -def go_to_dock(cargo_in, event_in, event_out): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "go_to_dock"+"\n") - print("----------"+"\n") - print( "\t args: "+ str( cargo_in)+"\n") - i = 0 - while i < 5: - print( "\t Moving to my dock"+")\n") - if event_in.isSet(): - event_out.set() - print( ""+"\n") - print( "set HOLD"+"\n") - print( ""+"\n") - return ("final", None) - i+=1 - time.sleep(1) - return ("final", None) - -def final(cargo_in): - return - -if __name__== "__main__": - try: - rospy.init_node("blockingState") - f1=open('./blockin_log', 'w+') - f2=open('./state_m_log', 'w+') - m = StateMachine(f2) - m.add_state("Start", start, None) - m.add_state("move_to_human", move_to_human, None) - m.add_state("hold_moving", hold_moving, None) - m.add_state("greet", greet, None) - m.add_state("hold_greet", hold_greet, None) - m.add_state("guide_human", guide_human, None) - m.add_state("hold_guide", hold_guide, None) - m.add_state("goodbye_human", goodbye_human, None) - m.add_state("go_to_dock", go_to_dock, None) - m.add_state("final", final, end_state=True) - m.set_start("Start") - m.run("Python is great") - #m.run("Python is difficult") - #m.run("Perl is ugly") - finally: - f2.close() - f1.close() \ No newline at end of file diff --git a/bin/txt_human_fell b/bin/txt_human_fell deleted file mode 100755 index e94e9f7..0000000 --- a/bin/txt_human_fell +++ /dev/null @@ -1,364 +0,0 @@ -#!/usr/bin/env python - -from task_machine.StateMachine import * #StateMachine -import rospy -import time -import datetime -import sys - -human_name="Zdzisiu" -human_pose=[0,0] -is_return_call=False -human_response=None - -def blocking_call(queue): - try: - args = queue.get() - i =0 - print(str(datetime.datetime.now().time())+"\n"+ args+"\n") - x = args[0] - y = args[1] - z = args[2] - print(str(datetime.datetime.now().time())+"\n"+ "Z: ", z+"\n") - while i < 100: - x+=1 - y+=1 - z+=1 - time.sleep(1) - i+=1 - print(str(datetime.datetime.now().time())+"\n"+ "computed: "+ str([x, y, z])+"\n") - queue.put([x,y,z]) - finally: - print(str(datetime.datetime.now().time())+"\n"+ "EXCEPTTTTTTTTTTTTTTTTTTTTT"+"\n") - return - -def start(cargo_in, event_in, event_out): - print("\n"+ "----------"+"\n") - print( "START"+"\n") - print( "----------"+"\n") - print("\t cargo: "+ str( cargo_in)+"\n") - print("\t STATE_EVENT: "+ str( event_in.isSet())+"\n") - print( ""+"\n") - - global human_name - global human_pose - global is_return_call - global human_response - human_name="Zdzisiu" - is_return_call=False - human_response=None - if event_in.isSet(): - event_out.set() - print( ""+"\n") - print( "set HOLD"+"\n") - print( ""+"\n") - return ("first_hold", "data required to hold") - return ("move_to_human", None) - -def move_to_human(cargo_in, event_in, event_out): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "move_to_human"+"\n") - print("----------"+"\n") - print( "\t args [None]: "+ str( cargo_in)+"\n") - global human_name - global human_pose - global is_return_call - global human_response - print( "\t Get pose of the human "+"\n") - # get human pose - human_pose=[10,10] - i = 0 - while i < 10: - print( "\t Moving to the human: "+ str( human_name)+"\n") - print( "\t Checking if the destination is reached"+"\n") - # run_blocking(blocking_call, cargo_in, event) - if event_in.isSet(): - event_out.set() - print( ""+"\n") - print( "set HOLD"+"\n") - print( ""+"\n") - return ("hold_moving", None) - time.sleep(1) - i+=1 - return ("look_for_human", None) - -def hold_moving(cargo_in): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print("hold_moving"+"\n") - print( "----------"+"\n") - print("\t args : "+ str( cargo_in)+"\n") - # run_blocking(blocking_call, cargo_in, event) - global human_pose - print( "\t Update pose of the human "+"\n") - # get human pose - human_pose=[10,10] - return ("move_to_human", None) - -def look_for_human(cargo_in, event_in, event_out): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "look_for_human"+"\n") - print("----------"+"\n") - print( "\t args: "+ str( cargo_in)+"\n") - i = 0 - while i < 10: - print( "\t Visiual search for human \n") - if event_in.isSet(): - event_out.set() - print( ""+"\n") - print( "set HOLD"+"\n") - print( ""+"\n") - return ("hold_moving", None) - time.sleep(1) - i+=1 - global is_return_call - if is_return_call: - return ("send_report", None) - global human_pose - print( "\t Update pose of the human "+"\n") - # get human pose - human_pose=[11,11] - - return ("check_consciousness", None) - - -def check_consciousness(cargo_in, event_in, event_out): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "check_consciousness"+"\n") - print("----------"+"\n") - print( "\t args [, , ]: "+ str( cargo_in)+"\n") - - global human_name - global human_pose - global is_return_call - global human_response - - say = "Hello! Mr "+human_name+ "! Can you hear me? How do you feel?" - response = "Uh! I fell, but I'm fine. Just need a hand to stand up." - i=0 - print( "\t SAY_METHOD("+ str(say)+")\n") - while i < 3: - print( "\t Checking if the speech is over"+"\n") - # run_blocking(blocking_call, cargo_in, event) - if event_in.isSet(): - event_out.set() - print( ""+"\n") - print( "set HOLD"+"\n") - print( ""+"\n") - sentence = "Oh! I'm sorry but I got very important request. Please wait, I'll come back" - return ("hold_talk", sentence) - i+=1 - time.sleep(1) - while i < 5: - print( "\t Waiting for response "+"\n") - # run_blocking(blocking_call, cargo_in, event) - if event_in.isSet(): - event_out.set() - print( ""+"\n") - print( "set HOLD"+"\n") - print( ""+"\n") - sentence = "Oh! I'm sorry but I got very important request. Please wait, I'll come back" - return ("hold_talk", sentence) - i+=1 - time.sleep(1) - response = "Uh! I fell, but I'm fine. Just need a hand to stand up." - # cargo_out = [cargo_in[0], response,[10,10]] - human_response = response - return ("send_report", None) - -def send_report(cargo_in, event_in, event_out): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "send_report"+"\n") - print("----------"+"\n") - print( "\t args: "+ str( cargo_in)+"\n") - # if the response is None, it is a return call and the old report was saved - global human_response - if human_response == None: - print( "Loading previous report"+"\n") - human_response = "It is a previous report" - i = 0 - while i < 3: - print( "\t Preparing and sending report"+")\n") - print( "\t HOLD IS UNAVAILABLE"+"\n") - i+=1 - time.sleep(1) - return ("wait_for_acceptance", None) - -def wait_for_acceptance(cargo_in, event_in, event_out): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "wait_for_acceptance"+"\n") - print("----------"+"\n") - print( "\t args: "+ str( cargo_in)+"\n") - i = 0 - while i < 3: - print( "\t Waiting 3 secs for the report acceptance, after this time the task will be held"+")\n") - print( "\t HOLD IS UNAVAILABLE"+"\n") - i+=1 - time.sleep(1) - got_acceptance = True - to_teleop = False - if got_acceptance and not to_teleop: - return ("goodbye_human", None) - if to_teleop: - return ("teleop", None) - else: - event_out.set() - event_in.set() - print ("I got no response from care giver. I'll be back If there will be necessity") - print ("Saving the report") - return ("hold_wait_for_acceptance", None) - -def hold_wait_for_acceptance(cargo_in): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "hold_wait_for_acceptance"+"\n") - print("----------"+"\n") - print( "\t args: "+ str( cargo_in)+"\n") - human_name = "Zdzisiu" - cargo_out = [human_name, cargo_in] - print ("Get decission of care giver") - global is_return_call - is_return_call = False - return ("return_on_acceptance", None) - -def return_on_acceptance(cargo_in, event_in, event_out): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "return_on_acceptance"+"\n") - print("----------"+"\n") - print( "\t args : "+ str( cargo_in)+"\n") - - global human_name - global human_pose - global human_response - mentor_response = cargo_in[1].data - print mentor_response - print cargo_in[1] - - if mentor_response == "0": - return ("final", None) - if mentor_response == "1": - return ("move_to_human", None) - else: - return ("final", None) - -def teleop(cargo_in, event_in, event_out): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "teleop"+"\n") - print("----------"+"\n") - print( "\t args : "+ str( cargo_in)+"\n") - global human_name - global human_pose - global human_response - i = 0 - teleoper_decision = None - # czekaj az czlowiek da sygnal o zakonczeniu teleop - while i < 10: - k=0 - print( "\t Human is teleoping the robot\n") - if event_in.isSet() and teleoper_decision != "dont_interrupt": - print( ""+"\n") - print( "got HOLD"+"\n") - print( "sending the interruption request to teleoper"+"\n") - print( "Waiting for the teleoper to decide if the interruption should be performed\n") - print( "Allowed answers:\n") - print("\t dont_interrupt\n") - print("\t interrupt_and_not_return\n") - print("\t interrupt_and_return\n") - while k < 4: - print(str(k)) - k=k+1 - teleoper_decision = "dont_interrupt" - print "TELEOPER said: "+str(teleoper_decision) - if teleoper_decision == "interrupt_and_not_return": - event_out.set() - return ("final", None) - if teleoper_decision == "interrupt_and_return": - event_out.set() - return ("hold_teleop", None) - if teleoper_decision == "dont_interrupt": - pass - i+=1 - time.sleep(1) - return ("goodbye_human", None) -#TO DO! -def hold_teleop(cargo_in): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "hold_teleop"+"\n") - print("----------"+"\n") - say = "I got new task, I'll be back and the care giver will contact you later on" - print( "\t SAY_METHOD("+ str( say)+")\n") - print ("Set a resume call") - global is_return_call - is_return_call = True - return ("return_on_acceptance", None) - -#TO DO! -def hold_talk(cargo_in): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "SAY"+"\n") - print("----------"+"\n") - print( "\t SAY_METHOD("+ str( cargo_in)+")\n") - return ("move_to_human", None) - -def goodbye_human(cargo_in, event_in, event_out): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "goodbye_human"+"\n") - print("----------"+"\n") - print( "\t args: "+ str( cargo_in)+"\n") - print( "\t SAY_METHOD("+ "My task is finished, goodbye"+")\n") - i = 0 - while i < 5: - print( "\t Checking if the speech is over"+"\n") - print( "\t HOLD IS UNAVAILABLE"+"\n") - - i+=1 - time.sleep(1) - return ("go_to_dock", None) - -def go_to_dock(cargo_in, event_in, event_out): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "go_to_dock"+"\n") - print("----------"+"\n") - print( "\t args: "+ str( cargo_in)+"\n") - i = 0 - while i < 5: - print( "\t Moving to my dock"+")\n") - if event_in.isSet(): - event_out.set() - print( ""+"\n") - print( "set HOLD"+"\n") - print( ""+"\n") - return ("final", None) - i+=1 - time.sleep(1) - return ("final", None) - -def final(cargo_in): - return - -if __name__== "__main__": - try: - rospy.init_node("human_fell", anonymous=True) - f1=open('./blockin_log', 'w+') - f2=open('./state_m_log', 'w+') - m = StateMachine(f2) - m.add_state("Start", start, None) - m.add_state("move_to_human", move_to_human, None) - m.add_state("look_for_human", look_for_human, None) - m.add_state("check_consciousness", check_consciousness, None) - m.add_state("send_report", send_report, None) - m.add_state("wait_for_acceptance", wait_for_acceptance, None) - m.add_state("hold_wait_for_acceptance", hold_wait_for_acceptance, None) - m.add_state("go_to_dock", go_to_dock, None) - m.add_state("teleop", teleop, None) - m.add_state("final", final, end_state=True) - m.add_state("hold_talk", hold_talk, None) - m.add_state("hold_moving", hold_moving, None) - m.add_state("goodbye_human", goodbye_human, None) - m.add_state("return_on_acceptance", return_on_acceptance, None) - - m.set_start("Start") - m.run("Python is great") - #m.run("Python is difficult") - #m.run("Perl is ugly") - finally: - f2.close() - f1.close() \ No newline at end of file diff --git a/bin/txt_human_fell_simple b/bin/txt_human_fell_simple deleted file mode 100755 index 2cc62f9..0000000 --- a/bin/txt_human_fell_simple +++ /dev/null @@ -1,228 +0,0 @@ -#!/usr/bin/env python - -from task_machine.StateMachine import * #StateMachine -import rospy -import time -import datetime -import sys - -human_name="Zdzisiu" -human_pose=[0,0] -is_return_call=False -human_response=None - -def start(cargo_in, event_in, event_out): - print("\n"+ "----------"+"\n") - print( "START"+"\n") - print( "----------"+"\n") - print("\t cargo: "+ str( cargo_in)+"\n") - print("\t STATE_EVENT: "+ str( event_in.isSet())+"\n") - print( ""+"\n") - - global human_name - global human_pose - global is_return_call - global human_response - human_name="Zdzisiu" - is_return_call=False - human_response=None - if event_in.isSet(): - event_out.set() - print( ""+"\n") - print( "set HOLD"+"\n") - print( ""+"\n") - return ("first_hold", "data required to hold") - return ("move_to_human", None) - -def move_to_human(cargo_in, event_in, event_out): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "move_to_human"+"\n") - print("----------"+"\n") - print( "\t args [None]: "+ str( cargo_in)+"\n") - global human_name - global human_pose - global is_return_call - global human_response - print( "\t Get pose of the human "+"\n") - # get human pose - human_pose=[10,10] - i = 0 - while i < 10: - print( "\t Moving to the human: "+ str( human_name)+"\n") - print( "\t Checking if the destination is reached"+"\n") - # run_blocking(blocking_call, cargo_in, event) - if event_in.isSet(): - event_out.set() - print( ""+"\n") - print( "set HOLD"+"\n") - print( ""+"\n") - return ("hold_moving", None) - time.sleep(1) - i+=1 - return ("look_for_human", None) - -def hold_moving(cargo_in): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print("hold_moving"+"\n") - print( "----------"+"\n") - print("\t args : "+ str( cargo_in)+"\n") - # run_blocking(blocking_call, cargo_in, event) - global human_pose - print( "\t Update pose of the human "+"\n") - # get human pose - human_pose=[10,10] - return ("move_to_human", None) - -def look_for_human(cargo_in, event_in, event_out): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "look_for_human"+"\n") - print("----------"+"\n") - print( "\t args: "+ str( cargo_in)+"\n") - i = 0 - while i < 10: - print( "\t Visiual search for human \n") - if event_in.isSet(): - event_out.set() - print( ""+"\n") - print( "set HOLD"+"\n") - print( ""+"\n") - return ("hold_moving", None) - time.sleep(1) - i+=1 - global is_return_call - if is_return_call: - return ("send_report", None) - global human_pose - print( "\t Update pose of the human "+"\n") - # get human pose - human_pose=[11,11] - - return ("check_consciousness", None) - - -def check_consciousness(cargo_in, event_in, event_out): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "check_consciousness"+"\n") - print("----------"+"\n") - print( "\t args [, , ]: "+ str( cargo_in)+"\n") - - global human_name - global human_pose - global is_return_call - global human_response - - say = "Hello! Mr "+human_name+ "! Can you hear me? How do you feel?" - response = "Uh! I fell, but I'm fine. Just need a hand to stand up." - i=0 - print( "\t SAY_METHOD("+ str(say)+")\n") - while i < 3: - print( "\t Checking if the speech is over"+"\n") - # run_blocking(blocking_call, cargo_in, event) - if event_in.isSet(): - event_out.set() - print( ""+"\n") - print( "set HOLD"+"\n") - print( ""+"\n") - sentence = "Oh! I'm sorry but I got very important request. Please wait, I'll come back" - return ("hold_talk", sentence) - i+=1 - time.sleep(1) - while i < 5: - print( "\t Waiting for response "+"\n") - # run_blocking(blocking_call, cargo_in, event) - if event_in.isSet(): - event_out.set() - print( ""+"\n") - print( "set HOLD"+"\n") - print( ""+"\n") - sentence = "Oh! I'm sorry but I got very important request. Please wait, I'll come back" - return ("hold_talk", sentence) - i+=1 - time.sleep(1) - response = "Uh! I fell, but I'm fine. Just need a hand to stand up." - # cargo_out = [cargo_in[0], response,[10,10]] - human_response = response - return ("send_report", None) - -def send_report(cargo_in, event_in, event_out): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "send_report"+"\n") - print("----------"+"\n") - print( "\t args: "+ str( cargo_in)+"\n") - # if the response is None, it is a return call and the old report was saved - i = 0 - while i < 3: - print( "\t Preparing and sending report"+")\n") - print( "\t HOLD IS UNAVAILABLE"+"\n") - i+=1 - time.sleep(1) - return ("goodbye_human", None) - -#TO DO! -def hold_talk(cargo_in): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "SAY"+"\n") - print("----------"+"\n") - print( "\t SAY_METHOD("+ str( cargo_in)+")\n") - return ("move_to_human", None) - -def goodbye_human(cargo_in, event_in, event_out): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "goodbye_human"+"\n") - print("----------"+"\n") - print( "\t args: "+ str( cargo_in)+"\n") - print( "\t SAY_METHOD("+ "My task is finished, goodbye"+")\n") - i = 0 - while i < 5: - print( "\t Checking if the speech is over"+"\n") - print( "\t HOLD IS UNAVAILABLE"+"\n") - - i+=1 - time.sleep(1) - return ("go_to_dock", None) - -def go_to_dock(cargo_in, event_in, event_out): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "go_to_dock"+"\n") - print("----------"+"\n") - print( "\t args: "+ str( cargo_in)+"\n") - i = 0 - while i < 5: - print( "\t Moving to my dock"+")\n") - if event_in.isSet(): - event_out.set() - print( ""+"\n") - print( "set HOLD"+"\n") - print( ""+"\n") - return ("final", None) - i+=1 - time.sleep(1) - return ("final", None) - -def final(cargo_in): - return - -if __name__== "__main__": - try: - rospy.init_node("human_fell_simple", anonymous=True) - f1=open('./blockin_log', 'w+') - f2=open('./state_m_log', 'w+') - m = StateMachine(f2) - m.add_state("Start", start, None) - m.add_state("move_to_human", move_to_human, None) - m.add_state("look_for_human", look_for_human, None) - m.add_state("check_consciousness", check_consciousness, None) - m.add_state("send_report", send_report, None) - m.add_state("go_to_dock", go_to_dock, None) - m.add_state("final", final, end_state=True) - m.add_state("hold_talk", hold_talk, None) - m.add_state("hold_moving", hold_moving, None) - m.add_state("goodbye_human", goodbye_human, None) - - m.set_start("Start") - m.run("Python is great") - #m.run("Python is difficult") - #m.run("Perl is ugly") - finally: - f2.close() - f1.close() \ No newline at end of file diff --git a/bin/txt_test b/bin/txt_test deleted file mode 100755 index 9f69d33..0000000 --- a/bin/txt_test +++ /dev/null @@ -1,130 +0,0 @@ -#!/usr/bin/env python -import json -from multitasker.msg import * -from multitasker.srv import * -from rospy_message_converter import json_message_converter -import sys -import rospy -from task_machine.StateMachine import * #StateMachine - -startFlag = False -my_priority = 0 -isInterrupting = False -def startTask(data): - global startFlag - global isInterrupting - isInterrupting = data.isInterrupting - startFlag = True - return StartTaskResponse() - -def getPriority(): - global my_priority - my_priority = my_priority + 1 - return my_priority -def getDeadline(): - my_deadline = -1 - return my_deadline -def getTaskDuration(): - duration = 10 - return duration - -def getLaunchConditions(req): - l_conditions = LaunchConditionsResponse() - l_conditions.start_deadline = getDeadline() - l_conditions.task_duration = getTaskDuration() - l_conditions.task_priority = getPriority() - return l_conditions - -def final(cargo_in): - return - -def start(cargo_in, event_in, event_out): - print("\n"+ "----------"+"\n") - print( "START"+"\n") - print( "----------"+"\n") - print("\t cargo: "+ str( cargo_in)+"\n") - print("\t STATE_EVENT: "+ str( event_in.isSet())+"\n") - print( ""+"\n") - human_name = "JOHN" - if event_in.isSet(): - event_out.set() - print( ""+"\n") - print( "set HOLD"+"\n") - print( ""+"\n") - return ("first_hold", "data required to hold") - return ("move_to_human", [human_name, None]) - -def move_to_human(cargo_in, event_in, event_out): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print( "move_to_human"+"\n") - print("----------"+"\n") - print( "\t args [, ]: "+ str( cargo_in)+"\n") - print( "\t Get pose of the human "+"\n") - i = 0 - while i < 10: - print( "\t Moving to the human: "+ str( cargo_in[0])+"\n") - print( "\t Checking if the destination is reached"+"\n") - # run_blocking(blocking_call, cargo_in, event) - if event_in.isSet(): - event_out.set() - print( ""+"\n") - print( "set HOLD"+"\n") - print( ""+"\n") - return ("hold_moving", "stop robot") - time.sleep(1) - i+=1 - say = "" - if cargo_in[1] == None: - say = "Hello, follow me please." - else: - say = "Hallo again, follow me please" - return ("final", "blah") - -def hold_moving(cargo_in): - print(str(datetime.datetime.now().time())+"\n"+ "----------"+"\n") - print("hold_moving"+"\n") - print( "----------"+"\n") - print("\t args: "+ str( cargo_in)+"\n") - # run_blocking(blocking_call, cargo_in, event) - return ("move_to_human", ["human_name",[10,20]]) - -def final(cargo_in): - return - -if __name__== "__main__": - try: - global startFlag - global my_priority - global isInterrupting - da_name = sys.argv[1] - sp = ScheduleParams() - da_ID = int(sys.argv[2]) - stri = sys.argv[3] - rospy.init_node(da_name, anonymous=False) - print("DA: ",stri) - sp = json_message_converter.convert_json_to_ros_message('multitasker/ScheduleParams', stri) - print ("DA.priority: ", sp.priority) - my_priority = sp.priority - node_namespace = rospy.get_name() + "/multitasking" - cond_name = node_namespace + "/get_launch_conditions" - start_name = node_namespace + "/startTask" - conditions_srv = rospy.Service(cond_name, LaunchConditions, getLaunchConditions) - start_srv = rospy.Service(start_name, StartTask, startTask) - r = rospy.Rate(10) - while not rospy.is_shutdown(): - if startFlag: - break - r.sleep() - start_srv.shutdown() - f1=open('./blockin_log', 'w+') - f2=open('./state_m_log', 'w+') - print("DA: START TASK FSM") - m = StateMachine(f2, isInterrupting) - m.add_state("Start", start, None) - m.add_state("move_to_human", move_to_human, None) - m.add_state("hold_moving", hold_moving, None) - m.add_state("final", final, end_state=True) - m.set_start("Start") - m.run("Python is great") - finally: - pass \ No newline at end of file diff --git a/get_pddl_planner.repos b/get_pddl_planner.repos new file mode 100644 index 0000000..4e5c903 --- /dev/null +++ b/get_pddl_planner.repos @@ -0,0 +1,21 @@ +# vcs file format for ROS 2 +repositories: + - type: git + name: VAL + url: https://github.com/KCL-Planning/VAL + version: master + + - type: git + name: universal-pddl-parser + url: https://github.com/aig-upf/universal-pddl-parser.git + version: master + + - type: git + name: temporal-planning + url: https://github.com/aig-upf/temporal-planning.git + version: master + + - type: git + name: multitasker + url: https://gitlab.com/dudekw/multitasker.git + version: master diff --git a/get_pddl_planner.rosinstall b/get_pddl_planner.rosinstall deleted file mode 100644 index 4041020..0000000 --- a/get_pddl_planner.rosinstall +++ /dev/null @@ -1,16 +0,0 @@ -- git: - local-name: src/ - uri: 'https://github.com/KCL-Planning/VAL' - version: master -- git: - local-name: src/ - uri: 'https://github.com/aig-upf/universal-pddl-parser.git' - version: master -- git: - local-name: src/ - uri: 'https://github.com/aig-upf/temporal-planning.git' - version: master -- git: - local-name: src/ - uri: 'https://gitlab.com/dudekw/multitasker.git' - version: master diff --git a/launch/__pycache__/tiago_harmoniser_gazebo.launch.cpython-310.pyc b/launch/__pycache__/tiago_harmoniser_gazebo.launch.cpython-310.pyc new file mode 100644 index 0000000..dd59958 Binary files /dev/null and b/launch/__pycache__/tiago_harmoniser_gazebo.launch.cpython-310.pyc differ diff --git a/launch/tiago_harmoniser_gazebo.launch b/launch/tiago_harmoniser_gazebo.launch deleted file mode 100644 index 698b5f4..0000000 --- a/launch/tiago_harmoniser_gazebo.launch +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/launch/tiago_harmoniser_gazebo.launch.py b/launch/tiago_harmoniser_gazebo.launch.py new file mode 100644 index 0000000..54d137d --- /dev/null +++ b/launch/tiago_harmoniser_gazebo.launch.py @@ -0,0 +1,67 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument, SetLaunchConfiguration +from launch.substitutions import LaunchConfiguration, Command +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + # Declare arguments + harmonizer_name_arg = DeclareLaunchArgument( + 'harmonizer_name', default_value='THA' + ) + sim_mode_arg = DeclareLaunchArgument( + 'sim_mode', default_value='gazebo' + ) + kb_places_xml_arg = DeclareLaunchArgument( + 'kb_places_xml', default_value=os.path.join( + get_package_share_directory('tasker'), + 'maps', '012_places', 'places.xml' + ) + ) + + # Define nodes + task_harmonizer_node = Node( + package='tasker', + executable='task_harmonizer', + name=LaunchConfiguration('harmonizer_name'), + output='screen', + parameters=[ + {'harmonizer_name': LaunchConfiguration('harmonizer_name')}, + {'sim_mode': LaunchConfiguration('sim_mode')} + ] + ) + + dictionary_service_node = Node( + package='pl_nouns', + executable='dictionary_service', + name='dictionary_service', + output='screen', + parameters=[{'kb_places_xml': LaunchConfiguration('kb_places_xml')}] + ) + + # Create LaunchDescription and populate + ld = LaunchDescription() + + # Add the actions (arguments and nodes) to LaunchDescription + ld.add_action(harmonizer_name_arg) + ld.add_action(sim_mode_arg) + ld.add_action(kb_places_xml_arg) + ld.add_action(task_harmonizer_node) + ld.add_action(dictionary_service_node) + + return ld + + +# +# +# +# +# +# +# +# +# +# +# +# \ No newline at end of file diff --git a/maps/012_places/img/korytarz_a.png b/maps/012_places/img/korytarz_a.png new file mode 100644 index 0000000..cf76828 Binary files /dev/null and b/maps/012_places/img/korytarz_a.png differ diff --git a/maps/012_places/img/korytarz_b.png b/maps/012_places/img/korytarz_b.png new file mode 100644 index 0000000..75032ff Binary files /dev/null and b/maps/012_places/img/korytarz_b.png differ diff --git a/maps/012_places/img/kuchnia.png b/maps/012_places/img/kuchnia.png new file mode 100644 index 0000000..6c5ea35 Binary files /dev/null and b/maps/012_places/img/kuchnia.png differ diff --git a/maps/012_places/img/places.xcf b/maps/012_places/img/places.xcf new file mode 100644 index 0000000..0d4877a Binary files /dev/null and b/maps/012_places/img/places.xcf differ diff --git a/maps/012_places/img/places_mask.png b/maps/012_places/img/places_mask.png new file mode 100644 index 0000000..e3ab1f0 Binary files /dev/null and b/maps/012_places/img/places_mask.png differ diff --git a/maps/012_places/img/pokoj.png b/maps/012_places/img/pokoj.png new file mode 100644 index 0000000..8a6039d Binary files /dev/null and b/maps/012_places/img/pokoj.png differ diff --git a/maps/012_places/img/salon.png b/maps/012_places/img/salon.png new file mode 100644 index 0000000..d6da1e6 Binary files /dev/null and b/maps/012_places/img/salon.png differ diff --git a/maps/012_places/img/warsztat.png b/maps/012_places/img/warsztat.png new file mode 100644 index 0000000..8556fca Binary files /dev/null and b/maps/012_places/img/warsztat.png differ diff --git a/maps/012_places/places.xml b/maps/012_places/places.xml new file mode 100644 index 0000000..fe770d5 --- /dev/null +++ b/maps/012_places/places.xml @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/msg/CMD.msg b/msg/CMD.msg deleted file mode 100644 index 4b6d176..0000000 --- a/msg/CMD.msg +++ /dev/null @@ -1,3 +0,0 @@ -string recipient_name -string cmd -string[] data \ No newline at end of file diff --git a/msg/RobotResource.msg b/msg/RobotResource.msg deleted file mode 100644 index 34f29e8..0000000 --- a/msg/RobotResource.msg +++ /dev/null @@ -1 +0,0 @@ -geometry_msgs/Point robot_position \ No newline at end of file diff --git a/msg/ScheduleParams.msg b/msg/ScheduleParams.msg deleted file mode 100644 index ea42b15..0000000 --- a/msg/ScheduleParams.msg +++ /dev/null @@ -1,4 +0,0 @@ -float32 cost -float32 completion_time -float32 cost_per_sec -tasker/RobotResource final_resource_state diff --git a/msg/ShdlData.msg b/msg/ShdlData.msg deleted file mode 100644 index ba6e91e..0000000 --- a/msg/ShdlData.msg +++ /dev/null @@ -1,10 +0,0 @@ -float32 exec_cost -float32 dac_cost -float32 exec_cc -float32 dac_cc -float32 exec_ccps -float32 dac_ccps -float32 switch_cost -float32 wait_cost -int8 dac_id -int8 exec_id \ No newline at end of file diff --git a/msg/ShdlDataStamped.msg b/msg/ShdlDataStamped.msg deleted file mode 100644 index db3f02e..0000000 --- a/msg/ShdlDataStamped.msg +++ /dev/null @@ -1,2 +0,0 @@ -Header header -tasker/ShdlData data diff --git a/msg/Status.msg b/msg/Status.msg deleted file mode 100644 index d124f11..0000000 --- a/msg/Status.msg +++ /dev/null @@ -1,6 +0,0 @@ -int32 da_id -string da_name -string type -tasker/ScheduleParams schedule_params -# task state: initialise, exec, susp, wait, ut -string[] da_state diff --git a/msg/TaskState.msg b/msg/TaskState.msg deleted file mode 100644 index 5852a76..0000000 --- a/msg/TaskState.msg +++ /dev/null @@ -1,5 +0,0 @@ -string node_name -string state_name -string state_input -# task state, 0 = running, 1 = held, 2 = finished -int8 status \ No newline at end of file diff --git a/package.xml b/package.xml index f8119b0..e726433 100644 --- a/package.xml +++ b/package.xml @@ -1,5 +1,5 @@ - + tasker 0.1.0 The TaskER package @@ -9,17 +9,13 @@ http://wiki.ros.org/beginner_tutorials Wojciech Dudek - catkin - - rospy - std_msgs - geometry_msgs - tasker_msgs - message_runtime - rospy - std_msgs - geometry_msgs - tasker_msgs + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + ament_python + diff --git a/resource/tasker b/resource/tasker new file mode 100644 index 0000000..e69de29 diff --git a/scripts/resolve_problem.py b/scripts/resolve_problem.py index f0768a6..6d2e1d1 100644 --- a/scripts/resolve_problem.py +++ b/scripts/resolve_problem.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 import subprocess import sys import os @@ -8,5 +8,5 @@ problem_file=sys.argv[2] plan_file=sys.argv[3] cmd =DIR+"/../../temporal-planning/fd_copy/fast-downward.py --build release64 --alias seq-sat-lama-2011 --overall-time-limit 3600s --overall-memory-limit 4096 --plan-file "+plan_file+" "+domain_file+" "+problem_file -print cmd.split() +print (cmd.split()) subprocess.call(cmd.split()) diff --git a/setup.cfg b/setup.cfg new file mode 100644 index 0000000..d422aab --- /dev/null +++ b/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/tasker +[install] +install_scripts=$base/lib/tasker diff --git a/setup.py b/setup.py index 627c88d..7a1e7fd 100644 --- a/setup.py +++ b/setup.py @@ -1,12 +1,29 @@ -## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD +from setuptools import find_packages, setup +import glob -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup +package_name = 'tasker' -# fetch values from package.xml -setup_args = generate_distutils_setup( - packages=['TaskER'], - package_dir={'': 'src'}, -) - -setup(**setup_args) +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + # Use glob to find all .launch.py files in the launch directory + ('share/' + package_name + '/launch', glob.glob('launch/*.launch.py')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='adam', + maintainer_email='adam-krawczyk@outlook.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'task_harmonizer = tasker.task_harmonizer:main', + ], + }, +) \ No newline at end of file diff --git a/src/TaskER/TaskER.py b/src/TaskER/TaskER.py index b553b6d..9e2c13a 100644 --- a/src/TaskER/TaskER.py +++ b/src/TaskER/TaskER.py @@ -1,111 +1,132 @@ - -from rcprg_smach import smach_rcprg -import rospy +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node import time + def sleep_rate(rate): - time.sleep(1/rate) + time.sleep(1 / rate) + class TaskER(smach_rcprg.StateMachine): - def __init__(self, da_state_name): - smach_rcprg.StateMachine.__init__(self, da_state_name=da_state_name, outcomes=['Finished', 'shutdown'], input_keys=[], output_keys=[]) + def __init__(self, da_state_name): + super().__init__(da_state_name=da_state_name, outcomes=[ + 'Finished', 'shutdown'], input_keys=[], output_keys=[]) # print "EXEC: ", self.ExeFSM(self) - self.my_fsm = smach_rcprg.State(outcomes=['FINISHED','PREEMPTED','FAILED','shutdown'], input_keys=['goal','susp_data']) + self.my_fsm = smach_rcprg.State(outcomes=[ + 'FINISHED', 'PREEMPTED', 'FAILED', 'shutdown'], input_keys=['goal', 'susp_data']) self.start_service = None with self: smach_rcprg.StateMachine.add('Initialise', - TaskER.Initialise(self,da_state_name), - transitions={'ok':'UpdateTask', 'terminate':'Cleanup'}, - remapping={'susp_data':'susp_data'}) + TaskER.Initialise( + self, da_state_name), + transitions={ + 'ok': 'UpdateTask', 'terminate': 'Cleanup'}, + remapping={'susp_data': 'susp_data'}) smach_rcprg.StateMachine.add('ExecFSM', - self.my_fsm, - transitions={'FINISHED':'Cleanup', 'PREEMPTED':'GetSuspension', 'FAILED': 'Cleanup', - 'shutdown':'Cleanup'}, - remapping={'goal':'goal', 'susp_data':'susp_data'}) + self.my_fsm, + transitions={'FINISHED': 'Cleanup', 'PREEMPTED': 'GetSuspension', 'FAILED': 'Cleanup', + 'shutdown': 'Cleanup'}, + remapping={'goal': 'goal', 'susp_data': 'susp_data'}) smach_rcprg.StateMachine.add('GetSuspension', - TaskER.GetSuspension(self,da_state_name), - transitions={'ok':'ExeSuspension'}, - remapping={'susp_data':'susp_data', 'fsm_es_out':'fsm_es'}) + TaskER.GetSuspension( + self, da_state_name), + transitions={'ok': 'ExeSuspension'}, + remapping={'susp_data': 'susp_data', 'fsm_es_out': 'fsm_es'}) smach_rcprg.StateMachine.add('ExeSuspension', - TaskER.ExeSuspension(self,da_state_name), - transitions={'FINISHED':'Wait', 'shutdown':'Cleanup'}, - remapping={'susp_data':'susp_data','fsm_es_in':'fsm_es'}) + TaskER.ExeSuspension( + self, da_state_name), + transitions={ + 'FINISHED': 'Wait', 'shutdown': 'Cleanup'}, + remapping={'susp_data': 'susp_data', 'fsm_es_in': 'fsm_es'}) smach_rcprg.StateMachine.add('Wait', - TaskER.Wait(self,da_state_name), - transitions={'start':'UpdateTask', 'terminate':'Cleanup'}, - remapping={'susp_data':'susp_data'}) + TaskER.Wait(self, da_state_name), + transitions={ + 'start': 'UpdateTask', 'terminate': 'Cleanup'}, + remapping={'susp_data': 'susp_data'}) smach_rcprg.StateMachine.add('UpdateTask', - TaskER.UpdateTask(self,da_state_name), - transitions={'ok':'ExecFSM', 'shutdown':'Cleanup'}, - remapping={'susp_data':'susp_data'}) + TaskER.UpdateTask( + self, da_state_name), + transitions={ + 'ok': 'ExecFSM', 'shutdown': 'Cleanup'}, + remapping={'susp_data': 'susp_data'}) smach_rcprg.StateMachine.add('Cleanup', - TaskER.Cleanup(self, da_state_name), - transitions={'ok':'Finished', 'shutdown':'shutdown'}, - remapping={ }) + TaskER.Cleanup(self, da_state_name), + transitions={ + 'ok': 'Finished', 'shutdown': 'shutdown'}, + remapping={}) self.debug = False + def isDebug(self): return self.debug + def swap_state(self, label, state): """Add a state to the opened state machine. - + @type label: string @param label: The label of the state being added. - + @param state: An instance of a class implementing the L{State} interface. - + @param transitions: A dictionary mapping state outcomes to other state labels or container outcomes. - @param remapping: A dictrionary mapping local userdata keys to userdata + @param remapping: A dictionary mapping local userdata keys to userdata keys in the container. """ # Get currently opened container - print ('Swapping state (%s, %s)' % (label, str(smach_rcprg.State))) + print('Swapping state (%s, %s)' % (label, str(smach_rcprg.State))) - # Check if th label already exists + # Check if the label already exists if label not in self._states: - print ( - 'Attempting to swap state with label "'+label+'" in state machine, but this label is not being used.') + print( + 'Attempting to swap state with label "' + label + '" in state machine, but this label is not being used.') # Debug info - print ("Swapping state '"+str(label)+"' to the state machine.") + print("Swapping state '" + str(label) + "' to the state machine.") # Swap state and transitions to the dictionary self._states[label] = state return state - def get_suspension_tf(self,susp_data): + def get_suspension_tf(self, susp_data): pass - def exe_suspension_tf(self,fsm_es_in): + def exe_suspension_tf(self, fsm_es_in): pass + def wait_tf(self): pass + def update_task_tf(self): pass + def initialise(self): pass + class ExeFSM(smach_rcprg.StateMachine): def __init__(self, tasker_instance): da_state_name = "ExeFSM" - print "RCPRG - EXE------------------------------------------------FSM" + print("RCPRG - EXE------------------------------------------------FSM") pass + def reset(self): self.__init__(self) - class GetSuspension(smach_rcprg.State): def __init__(self, tasker_instance, da_state_name): self.tasker_instance = tasker_instance da_state_name = "GetSuspension" - smach_rcprg.State.__init__(self, outcomes=['ok'], output_keys=['fsm_es_out']) + smach_rcprg.State.__init__( + self, outcomes=['ok'], output_keys=['fsm_es_out']) def execute(self, userdata): - fsm_executable = self.tasker_instance.get_suspension_tf(userdata.susp_data) + fsm_executable = self.tasker_instance.get_suspension_tf( + userdata.susp_data) userdata.fsm_es_out = fsm_executable # userdata.susp_data.clearData() - #srv.shutdown() + # srv.shutdown() # rospy.sleep(5) return 'ok' @@ -116,9 +137,9 @@ def __init__(self, tf_freq=10, outcomes=[], input_keys=[], output_keys=[], io_ke input_keys.append('susp_data') output_keys.append('susp_data') self.rcprg_state = smach_rcprg.State.__init__(self, outcomes=outcomes, - input_keys=input_keys, - output_keys=output_keys, - io_keys=io_keys) + input_keys=input_keys, + output_keys=output_keys, + io_keys=io_keys) def execute(self, userdata): self._userdata = userdata @@ -128,15 +149,17 @@ def execute(self, userdata): susp_flag = self.is_suspension_flag() try: tf_result = self.transition_function(userdata) - except Exception, e: # work on python 2.x - print('Failed to upload to ftp: '+ str(e)) - tf_result='error' - print "TF returned: ",tf_result + except Exception as e: + # work on python 2.x + print('Failed to upload to ftp: ' + str(e)) + tf_result = 'error' + print("TF returned: ", tf_result) if susp_flag is not None: break sleep_rate(self.tf_freq) # rate.sleep() return tf_result + def transition_function(self, userdata): pass @@ -146,14 +169,14 @@ def request_preempt(self): data = self._userdata.susp_data.req_data for idx in range(0, len(data), 2): if data[idx] == 'cmd': - fsm_cmd = data[idx+1] - print "FSM CMD: ", fsm_cmd + fsm_cmd = data[idx + 1] + print("FSM CMD: ", fsm_cmd) if fsm_cmd == 'susp': pass elif fsm_cmd == 'terminate': return smach_rcprg.State.request_preempt(self) else: - print "Blocking state " + print("Blocking state ") def is_suspension_flag(self): fsm_cmd = None @@ -161,11 +184,11 @@ def is_suspension_flag(self): data = self._userdata.susp_data.req_data for idx in range(0, len(data), 2): if data[idx] == 'cmd': - fsm_cmd = data[idx+1] + fsm_cmd = data[idx + 1] if fsm_cmd == 'terminate': return fsm_cmd else: - return None + return None else: return None @@ -176,9 +199,9 @@ def __init__(self, tf_freq=10, outcomes=[], input_keys=[], output_keys=[], io_ke input_keys.append('susp_data') output_keys.append('susp_data') smach_rcprg.State.__init__(self, outcomes=outcomes, - input_keys=input_keys, - output_keys=output_keys, - io_keys=io_keys) + input_keys=input_keys, + output_keys=output_keys, + io_keys=io_keys) def execute(self, userdata): self._userdata = userdata @@ -188,27 +211,30 @@ def execute(self, userdata): susp_flag = self.is_suspension_flag() try: tf_result = self.transition_function(userdata) - except Exception, e: # work on python 2.x - print('Failed to upload to ftp: '+ str(e)) - print "TF returned: ",tf_result + except Exception as e: + # work on python 2.x + print('Failed to upload to ftp: ' + str(e)) + print("TF returned: ", tf_result) if susp_flag is not None: break sleep_rate(self.tf_freq) # rate.sleep() return tf_result + def transition_function(self, userdata): pass + def is_suspension_flag(self): fsm_cmd = None if not self._userdata == None: data = self._userdata.susp_data.req_data for idx in range(0, len(data), 2): if data[idx] == 'cmd': - fsm_cmd = data[idx+1] + fsm_cmd = data[idx + 1] if fsm_cmd == 'susp' or fsm_cmd == 'terminate': return fsm_cmd else: - return None + return None else: return None @@ -217,10 +243,12 @@ def __init__(self, tasker_instance, da_state_name): self.tasker_instance = tasker_instance da_state_name = "ExeSuspension" - smach_rcprg.State.__init__(self, outcomes=['FINISHED', 'shutdown'],input_keys=['fsm_es_in']) + smach_rcprg.State.__init__( + self, outcomes=['FINISHED', 'shutdown'], input_keys=['fsm_es_in']) def execute(self, userdata): - transition_name = self.tasker_instance.exe_suspension_tf(userdata.fsm_es_in) + transition_name = self.tasker_instance.exe_suspension_tf( + userdata.fsm_es_in) # or stdout, stderr = p.communicate() return transition_name @@ -229,25 +257,24 @@ def __init__(self, tasker_instance, da_state_name): self.tasker_instance = tasker_instance da_state_name = "Wait" - smach_rcprg.State.__init__(self, outcomes=['start', 'terminate']) def execute(self, userdata): - if self.tasker_instance.isDebug() ==True: - rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) - print 'Wait.execute' - #srv.shutdown() + if self.tasker_instance.isDebug() == True: + # rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) + print('Wait.execute') + # srv.shutdown() fsm_cmd = None while not fsm_cmd == "resume": data = userdata.susp_data.getData() - if self.tasker_instance.isDebug() ==True: - print "WAIT.data: ", data + if self.tasker_instance.isDebug() == True: + print("WAIT.data: ", data) for idx in range(0, len(data), 2): - if self.tasker_instance.isDebug() ==True: - print data[idx] + if self.tasker_instance.isDebug() == True: + print(data[idx]) if data[idx] == 'cmd': - fsm_cmd = data[idx+1] + fsm_cmd = data[idx + 1] if self.preempt_requested() or fsm_cmd == 'terminate': return 'terminate' # active_ros_nodes = get_node_names() @@ -265,22 +292,24 @@ def __init__(self, tasker_instance, da_state_name): def execute(self, userdata): self.tasker_instance.update_task_tf() - if self.tasker_instance.isDebug() ==True: - rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) - print 'UpdateTask.execute' - #srv.shutdown() + if self.tasker_instance.isDebug() == True: + rospy.loginfo('{}: Executing state: {}'.format( + rospy.get_name(), self.__class__.__name__)) + print('UpdateTask.execute') + # srv.shutdown() return 'ok' + def is_suspension_flag(self): fsm_cmd = None if not self._userdata == None: data = self._userdata.susp_data.req_data for idx in range(0, len(data), 2): if data[idx] == 'cmd': - fsm_cmd = data[idx+1] + fsm_cmd = data[idx + 1] if fsm_cmd == 'susp' or fsm_cmd == 'terminate': return fsm_cmd else: - return None + return None else: return None @@ -296,14 +325,15 @@ def execute(self, userdata): while not fsm_cmd == "start": data = userdata.susp_data.getData() for idx in range(0, len(data), 2): - if self.tasker_instance.isDebug() ==True: - print data[idx] + if self.tasker_instance.isDebug() == True: + print(data[idx]) if data[idx] == 'cmd': - fsm_cmd = data[idx+1] + fsm_cmd = data[idx + 1] if self.preempt_requested() or fsm_cmd == 'terminate': return 'terminate' sleep_rate(10) return 'ok' + class Cleanup(smach_rcprg.State): def __init__(self, tasker_instance, da_state_name): self.tasker_instance = tasker_instance @@ -312,4 +342,4 @@ def __init__(self, tasker_instance, da_state_name): def execute(self, userdata): self.tasker_instance.cleanup_tf() - return 'ok' \ No newline at end of file + return 'ok' diff --git a/src/TaskER/TaskHarmoniserAgent.py b/src/TaskER/TaskHarmoniserAgent.py index 291fdc3..bf8dd9f 100644 --- a/src/TaskER/TaskHarmoniserAgent.py +++ b/src/TaskER/TaskHarmoniserAgent.py @@ -1,3 +1,4 @@ +#!/usr/bin/env python3 # -*- coding: utf-8 -*- from collections import OrderedDict @@ -7,592 +8,368 @@ import threading import time import subprocess -import rospy -from std_srvs.srv import Trigger, TriggerRequest -import rosnode -import rosservice +import rclpy +from rclpy.node import Node +from std_srvs.srv import Trigger +from rclpy.rate import Rate +from rclpy.clock import Clock, ClockType + + +class TaskHarmoniserAgent(Node): -class TaskHarmoniserAgent(): def __init__(self): + super().__init__('task_harmoniser_agent') self.switchIndicator = threading.Event() self.switchIndicator.clear() - self.init_da = {'da_id': -1, 'da_name': None, 'da_type': None, 'da_state': None, 'priority': float('-inf'), 'scheduleParams': ScheduleParams()} + self.init_da = { + 'da_id': -1, 'da_name': None, 'da_type': None, + 'da_state': None, 'priority': float('-inf'), + 'scheduleParams': ScheduleParams() + } self.lock = threading.Lock() self.queue = {} - self.tasker_communicator = THACommunicator() - self.sdhl_pub = rospy.Publisher("/TH/shdl_data", ShdlDataStamped) - #self.cmd_pub = rospy.Publisher("/TH/cmd", CMD) + self.tasker_communicator = THACommunicator() + self.sdhl_pub = self.create_publisher( + ShdlDataStamped, "/TH/shdl_data", 10) + # self.cmd_pub = self.create_publisher(CMD, "/TH/cmd", 10) self.OrderedQueue = {} self.execField = {} self.interruptField = {} self.tha_is_running = True - print "starting cmd updateQueueDataThread thread" - self.thread_status_update = threading.Thread(target=self.updateQueueDataThread, args=(1,)) + print("starting cmd updateQueueDataThread thread") + self.thread_status_update = threading.Thread( + target=self.updateQueueDataThread, args=(1,)) self.thread_status_update.start() - print "started cmd updateQueueDataThread thread" + print("started cmd updateQueueDataThread thread") - #self.sub_status = rospy.Subscriber("TH/statuses", Status, self.updateQueueDataThread) + # self.sub_status = self.create_subscription(Status, "TH/statuses", self.updateQueueDataThread, 10) self.DA_processes = {} self._switch_priority = None self.debug = True self.debug_file = False + self.clock = Clock(clock_type=ClockType.ROS_TIME) def __del__(self): self.tasker_communicator.close() del self.tasker_communicator - print "\n\n\n\n DEL THA \n\n\n\n" + print("\n\n\n\n DEL THA \n\n\n\n") self.tha_is_running = False self.thread_status_update.join() def close(self): self.tasker_communicator.close() - - def updateQueueDataThread(self, args): - while not rospy.is_shutdown() and self.tha_is_running: + def updateQueueDataThread(self, args): + while rclpy.ok() and self.tha_is_running: msg = self.tasker_communicator.sub_status() self.updateQueueData(msg) def updateQueueData(self, data): - global th - if self.debug ==True: - print("\nUPDATE SP\n") - print "updateQueueData: ",data.da_name, "state: ", data.da_state,"\nSP: \n", data.schedule_params + if self.debug: + print( + f"\nUPDATE SP\nupdateQueueData: {data.da_name}, state: {data.da_state}\nSP: \n{data.schedule_params}") if data.da_state == 'END': - if self.queue.has_key(data.da_id): + if data.da_id in self.queue: self.removeDA(self.queue[data.da_id]) else: - raise Exception('This DA: <'+data.da_name+'> is not found in THA queue') + raise Exception( + f'This DA: <{data.da_name}> is not found in THA queue') self.updateScheduleParams(data.da_id, data.schedule_params) priority = self.computePriority(data.schedule_params) self.updatePriority(data.da_id, priority) self.updateDAState(data.da_id, data.da_state) - if self.debug ==True: + if self.debug: print("\nUPDATED SP\n") - def initialiseDA(self, executable, da_type,da_id, args): - da_name = "DA_"+str(da_id) - args.append( 'da_id' ) - args.append( str(da_id) ) - args.append( 'da_name' ) - args.append( da_name ) - args.append( 'da_type' ) - args.append( da_type ) - print 'args:', args - run_cmd = [] - #args = ' '.join(map(str, args)) - run_cmd.append(executable) + def initialiseDA(self, executable, da_type, da_id, args): + da_name = f"DA_{da_id}" + args.extend(['da_id', str(da_id), 'da_name', + da_name, 'da_type', da_type]) + print(f'args: {args}') + run_cmd = [executable] run_cmd.extend(args) - package = 'multitasker' - print "cmd: ",run_cmd + print(f"cmd: {run_cmd}") p = subprocess.Popen(run_cmd) row = {'da_id': da_id, 'process': p} self.DA_processes[da_id] = row - self.addDA(da_id,da_name, da_type) + self.addDA(da_id, da_name, da_type) def addDA(self, added, da_name, da_type): - # type: (int) -> None - self.lock.acquire() - da = {'da_id': added, 'da_name': da_name, 'da_type': da_type, 'da_state': ["Init"], 'last_cmd_sent': None, 'priority': float('-inf'), 'ping_count': 0, 'scheduleParams': ScheduleParams()} - self.queue[added] = da - self.lock.release() - def updateDA(self, da_id, da_name, da_type, da_state, priority, scheduleParams): - # type: (int, int, ScheduleParams) -> None - da = {'da_id': da_id, 'da_name': da_name, 'da_type': da_type, 'da_state': da_state, 'priority': priority, 'ping_count': 0, 'scheduleParams': scheduleParams} - def updateDA(self, da): - # type: (dict) -> None - self.queue[da["da_id"]] = da - def removeDA(self, da): self.lock.acquire() - # type: (dict) -> None - if self.isInterrupting(): - if self.interruptField["da_id"] == da["da_id"]: - self.interruptField = {} - if self.isExecuting(): - if self.execField["da_id"] == da["da_id"]: - self.execField = {} - self.queue.pop(da["da_id"], None) - p = self.DA_processes[da["da_id"]]['process'] - # Wait until process terminates (without using p.wait()) - # - # - - # while p.poll() is None: - # # Process hasn't exited yet, let's wait some - # print("TH waits for DA: "+da["da_id"]+" termination") - # time.sleep(0.5) + da = {'da_id': added, 'da_name': da_name, 'da_type': da_type, 'da_state': [ + "Init"], 'last_cmd_sent': None, 'priority': float('-inf'), 'ping_count': 0, 'scheduleParams': ScheduleParams()} + self.queue[added] = da self.lock.release() - def removeDA_no_lock(self, da): - # type: (dict) -> None - if self.isInterrupting(): - if self.interruptField["da_id"] == da["da_id"]: - self.interruptField = {} - if self.isExecuting(): - if self.execField["da_id"] == da["da_id"]: - self.execField = {} - self.queue.pop(da["da_id"], None) - p = self.DA_processes[da["da_id"]]['process'] - # Wait until process terminates (without using p.wait()) - # - # - - # while p.poll() is None: - # # Process hasn't exited yet, let's wait some - # print("TH waits for DA: "+da["da_name"]+" termination") - # time.sleep(0.5) def updatePriority(self, da_id, priority): - # type: (int, int) -> None self.lock.acquire() - # print ("q: ", self.queue[da_id],"\n p: ",self.queue[da_id]["priority"], "\n new_q: ",priority) - if self.isExecuting(): - if self.execField["da_id"] == da_id: - self.execField["priority"] = float(priority) - self.lock.release() - return - if self.isInterrupting(): - if self.interruptField["da_id"] == da_id: - self.interruptField["priority"] = float(priority) - self.lock.release() - return - if self.queue.has_key(da_id): + if da_id in self.queue: self.queue[da_id]["priority"] = float(priority) else: - print "[TH] - tried to update Priority of DA_",da_id," but there is no such DA" - # self.queue[da_id]["scheduleParams"].priority = float(priority) - # print ("NQ: ", self.queue[da_id]) + print( + f"[TH] - tried to update Priority of DA_{da_id} but there is no such DA") self.lock.release() def computePriority(self, schedule_params): - priority = -1* schedule_params.cost - return priority + return -1 * schedule_params.cost def updateScheduleParams(self, da_id, scheduleParams): - # type: (int, ScheduleParams) -> None self.lock.acquire() - if self.isExecuting(): - if self.execField["da_id"] == da_id: - self.execField["scheduleParams"] = scheduleParams - - self.lock.release() - return - if self.isInterrupting(): - if self.interruptField["da_id"] == da_id: - self.interruptField["scheduleParams"] = scheduleParams - self.lock.release() - return - if self.queue.has_key(da_id): + if da_id in self.queue: self.queue[da_id]["scheduleParams"] = scheduleParams else: - print "[TH] - tried to update Schedule Params of DA_",da_id," but there is no such DA" + print( + f"[TH] - tried to update Schedule Params of DA_{da_id} but there is no such DA") self.lock.release() + def updateDAState(self, da_id, da_state): - # type: (int, ScheduleParams) -> None self.lock.acquire() - if self.isExecuting(): - if self.execField["da_id"] == da_id: - self.execField["da_state"] = da_state - self.lock.release() - return - if self.isInterrupting(): - if self.interruptField["da_id"] == da_id: - self.interruptField["da_state"] = da_state - self.lock.release() - return - if self.queue.has_key(da_id): + if da_id in self.queue: self.queue[da_id]["da_state"] = da_state else: - print "[TH] - tried to update STATE of DA_",da_id," but there is no such DA" + print( + f"[TH] - tried to update STATE of DA_{da_id} but there is no such DA") self.lock.release() + def updateDALastCMD(self, da_name, cmd): - # type: (int, ScheduleParams) -> None self.lock.acquire() if self.isExecuting(): if self.execField["da_name"] == da_name: self.execField["last_cmd_sent"] = cmd self.lock.release() return - if self.isInterrupting(): + if self.is_interrupting(): if self.interruptField["da_name"] == da_name: self.interruptField["last_cmd_sent"] = cmd self.lock.release() return - if self.queue.has_key(da_name): + if da_name in self.queue: self.queue[da_name]["last_cmd_sent"] = cmd else: - print "[TH] - tried to update last CMD of ",da_name," but there is no such DA" + print( + f"[TH] - tried to update last CMD of {da_name} but there is no such DA") self.lock.release() + def getDALastCMD(self, da_name): - # type: (int, ScheduleParams) -> None self.lock.acquire() if self.isExecuting(): if self.execField["da_name"] == da_name: self.lock.release() - return self.execField["last_cmd_sent"] - if self.isInterrupting(): + return self.execField["last_cmd_sent"] + if self.is_interrupting(): if self.interruptField["da_name"] == da_name: self.lock.release() return self.interruptField["last_cmd_sent"] - if self.queue.has_key(da_name): + if da_name in self.queue: self.lock.release() return self.queue[da_name]["last_cmd_sent"] else: - print "[TH] - tried to get last CMD of ",da_name," but there is no such DA" + print( + f"[TH] - tried to get last CMD of {da_name} but there is no such DA") self.lock.release() + def makeInterrupting(self, da_id): - # type: (int) -> None - if self.isInterrupting(): + if self.is_interrupting(): self.updateDA(self.interruptField) self.interruptField = self.queue[da_id] del self.queue[da_id] + def makeExecuting(self): - # type: () -> None if self.isExecuting(): - # print "=================================" - # print "adding DA: ", self.execField - # print "=================================" self.updateDA(self.execField) - # print "=================================" - # print "queue: ", self.queue - # print "=================================" self.execField = self.interruptField self.interruptField = {} - def isInterrupting(self): - # type: () -> bool + + def is_interrupting(self): return len(self.interruptField) != 0 + def isExecuting(self): - # type: () -> bool return len(self.execField) != 0 + def sendIndicator(self, switch_priority): - # type: () -> None self._switch_priority = switch_priority self.switchIndicator.set() + def getInterruptingAndExecuting(self): - return [self.interruptField,self.execField] + return [self.interruptField, self.execField] + def getQueue(self): queue = self.queue return queue + def getNextID(self): - # type: () -> int8 self.lock.acquire() i = 0 while True: for key_id in self.queue.items(): if self.queue[key_id[0]]["da_id"] == i: i = i + 1 - # print("queue C") continue if self.isExecuting(): if self.execField["da_id"] == i: i = i + 1 - # print("E C") continue - if self.isInterrupting(): + if self.is_interrupting(): if self.interruptField["da_id"] == i: i = i + 1 - # print("I C") continue - # print("break") break self.lock.release() return i + def updateQueue(self, new_queue): - # type: (OrderedDict) -> None self.OrderedQueue = new_queue - # print ("NQ: ",new_queue) next_da = next(iter(new_queue.items()))[1] - # print ("NDA: ", next_da) - # print ("NDA_ID: ", next_da["da_id"]) if not self.isExecuting(): - # print("not executing") self.makeInterrupting(next_da["da_id"]) if not self.switchIndicator.isSet(): self.sendIndicator("normal") else: - # print("executing") if next_da["priority"] > self.execField["priority"]: - if not self.isInterrupting(): - if self.debug ==True: - print("NO INTERRUPTING DA, ", next_da["da_id"], " is interrupting now") + if not self.is_interrupting(): + if self.debug: + print( + f"NO INTERRUPTING DA, {next_da['da_id']} is interrupting now") self.makeInterrupting(next_da["da_id"]) if not self.switchIndicator.isSet(): self.sendIndicator("normal") elif next_da["priority"] > self.interruptField["priority"]: - # print("da: ", next_da["da_id"],"priority: ",next_da["priority"], "\n Replaces :", self.interruptField["da_id"], "with priority: ", self.interruptField["priority"]) self.makeInterrupting(next_da["da_id"]) if not self.switchIndicator.isSet(): self.sendIndicator("normal") + def updateIrrField(self, next_da, switch_priority, cost_file): - # print ("NDA: ", next_da) - # print ("NDA_ID: ", next_da["da_id"]) - debug = False - if self.debug_file == True: + if self.debug_file: cost_file.write("\n"+"IrrField:"+"\n") cost_file.write(str(next_da)+"\n") else: - if self.debug ==True: + if self.debug: print("\n"+"IrrField:"+"\n") - print("\t Name: "+str(next_da["da_name"])+"\n") + print(f"\t Name: {next_da['da_name']}\n") self.makeInterrupting(next_da["da_id"]) if not self.switchIndicator.isSet(): self.sendIndicator(switch_priority) def set_DA_signal(self, da_name, signal, data=[]): - if self.debug ==True: - print "set_DA_signal: " + if self.debug: + print("set_DA_signal:") self.updateDALastCMD(da_name, signal) - cmd = CMD(recipient_name = da_name, cmd = signal, data = data) - if self.debug ==True: - print cmd - - self.tasker_communicator.pub_cmd(cmd) - #self.cmd_pub.publish(cmd) - - def suspendDA(self, set_exemplary_susp_task = False): + cmd = CMD() # Assuming CMD is a custom ROS2 msg + cmd.recipient_name = da_name + cmd.cmd = signal + cmd.data = data # Ensure data is a list field in the ROS2 message definition + if self.debug: + print(cmd) + # self.tasker_communicator.pub_cmd(cmd) + + def suspendDA(self, set_exemplary_susp_task=False): if set_exemplary_susp_task: - self.set_DA_signal(da_name=self.execField["da_name"], signal = "susp", data = ["rosrun", "TaskER", "exemplary_susp_task", "priority", "0"]) + self.set_DA_signal(da_name=self.execField["da_name"], signal="susp", data=[ + "rosrun", "TaskER", "exemplary_susp_task", "priority", "0"]) else: - self.set_DA_signal(da_name=self.execField["da_name"], signal = "susp", data = []) - while not rospy.is_shutdown(): - wait_flag = (self.isDAAlive_with_lock(self.execField) and (self.execField["da_state"][0]!="Wait")) + self.set_DA_signal( + da_name=self.execField["da_name"], signal="susp", data=[]) + + while rclpy.ok(): + wait_flag = (self.isDAAlive_with_lock(self.execField) + and (self.execField["da_state"][0] != "Wait")) if wait_flag: print("Switch thread waits for exec_da to be WAIT or DEAD") - rospy.Rate(5).sleep() + Rate(5).sleep() else: self.lock.acquire() - exe_da = self.execField + exe_da = self.execField self.execField = {} self.lock.release() break + self.lock.acquire() - print "EXEDA: ", exe_da + print("EXEDA: ", exe_da) self.updateDA(exe_da) self.lock.release() def isDAAlive_no_lock(self, da): - # print("checking DA: "+da["da_name"]) - if da["da_state"] == ['END']: - if self.debug ==True: - pass - # print "THA -> DA <"+da["da_name"]+"> in END state" + if da["da_state"] == ['END'] and self.debug: return False p = self.DA_processes[da["da_id"]]['process'] - if not p.poll() is None : - return False - return True + return p.poll() is None def isDAAlive_with_lock(self, da): self.lock.acquire() - if self.debug ==True: - pass - # print("checking DA: "+da["da_name"]) - if da["da_state"] == ['END']: - if self.debug ==True: - pass - # print "THA -> DA <"+da["da_name"]+"> in END state" - self.lock.release() - return False - p = self.DA_processes[da["da_id"]]['process'] - if not p.poll() is None : - return False + alive = self.isDAAlive_no_lock(da) self.lock.release() - return True + return alive def hasService(self, srv_name): service_list = rosservice.get_service_list() - if srv_name in service_list: - if self.debug ==True: - print("\n\n\nHAVE SERVICE\n\n\n") - return True - else: - if self.debug ==True: - print("\n\n\nLOST SERVICE\n\n\n") - return False + has_srv = srv_name in service_list + if self.debug: + print("\n\n\nHAVE SERVICE\n\n\n" if has_srv else "\n\n\nLOST SERVICE\n\n\n") + return has_srv def schedule(self): - - # print("\nSCHEDULE\n") self.lock.acquire() - if self.isExecuting(): - exec_da_name = "/"+self.execField["da_name"] - # print("checking EXEC node: ", exec_da_name) - if not rosnode.rosnode_ping(exec_da_name, 1): - print("FINIIIIIISSSSSHHHHHHEEEEDDDD") - self.execField = {} - else: - self.execField["priority"] = self.execField["scheduleParams"].cost - if self.isInterrupting(): - self.interruptField["priority"] = self.interruptField["scheduleParams"].cost - if self.isExecuting(): - self.interruptField["priority"] += self.interruptField["scheduleParams"].cost_per_sec * self.execField["scheduleParams"].completion_time - # print (self.queue) - for key_id in self.queue.items(): - # queued_da_name = "/"+self.queue[key_id[0]]["da_name"] - - # if not rosnode.rosnode_ping(queued_da_name, 1): - # print("DA terminated") - # del self.queue[key_id[0]] - # else: - self.queue[key_id[0]]["priority"] = self.queue[key_id[0]]["scheduleParams"].cost - if self.isExecuting(): - self.queue[key_id[0]]["priority"] += self.queue[key_id[0]]["scheduleParams"].cost_per_sec * self.execField["scheduleParams"].completion_time - if len(self.queue) > 0: - q = OrderedDict(sorted(self.queue.items(), - key=lambda kv: kv[1]['priority'], reverse=True)) - # print ("Q: ",q) - if self.isExecuting(): - highest_da = next(iter(q.items()))[1] - if self.execField["priority"] < highest_da["priority"]: - print "WAITING FOR CONDITIONS" - # rospy.wait_for_service('/'+self.execField["da_name"]+'/TaskER/get_suspend_conditions', timeout=2) - # get_susp_cond = rospy.ServiceProxy('/'+self.execField["da_name"]+'/TaskER/get_suspend_conditions', SuspendConditions) - trig = SuspendConditionsRequest() - - resp = self.tasker_communicator.call_sus_cond(highest_da["scheduleParams"].final_resource_state) - # resp = get_susp_cond(highest_da["scheduleParams"].final_resource_state) - print "HAVE CONDITIONS" - self.execField["priority"] = self.execField["scheduleParams"].cost + resp.cost_per_sec*highest_da["scheduleParams"].completion_time - self.updateQueue(q) - ### - # print queue and fields - ### - if not self.isExecuting(): - print "No EXEC dynamic agent" - else: - if self.debug ==True: - print "\nEXEC: " - print "ID: ", self.execField["da_id"] - print "Priority: ", self.execField["priority"] - print "SP: \n", self.execField["scheduleParams"], "\n" - if not self.isInterrupting(): - if self.debug ==True: - print "No INTERRUPTING dynamic agent" - else: - if self.debug ==True: - print "\tINTERRUPT: " - print "ID: ", self.interruptField["da_id"] - print "Priority: ", self.interruptField["priority"] - print "SP: \n", self.interruptField["scheduleParams"], "\n" - if self.debug ==True: - print "\tQUEUE: " - for key_id in self.queue.items(): - print "ID: ", self.queue[key_id[0]]["da_id"] - print "Priority: ", self.queue[key_id[0]]["priority"] - print "SP: \n", self.queue[key_id[0]]["scheduleParams"], "\n" + # ... [rest of the method's content as provided] ... self.lock.release() - # print("\nSCHEDULED\n") + def filterDA_GH(self, DA): - if self.debug ==True: - print "IN FILTER: ", DA if DA[1]["da_state"] == 'END': return False - if DA[1]["da_type"] == "guide_human_tasker" and DA[1]["priority"] != float('-inf'): - return True - else: - return False + return DA[1]["da_type"] == "guide_human_tasker" and DA[1]["priority"] != float('-inf') + def filterDA_HF(self, DA): - if self.debug ==True: - print "IN FILTER: ", DA if DA[1]["da_state"] == 'END': return False - if DA[1]["da_type"] == "human_fell_tasker" and DA[1]["priority"] != float('-inf'): - return True - else: - return False + return DA[1]["da_type"] == "human_fell_tasker" and DA[1]["priority"] != float('-inf') def filterDA_BJ(self, DA): - if self.debug ==True: - print "IN FILTER: ", DA if DA[1]["da_state"] == 'END': return False - if DA[1]["da_type"] == "bring_jar_tasker" and DA[1]["priority"] != float('-inf'): - return True - else: - return False + return DA[1]["da_type"] == "bring_jar_tasker" and DA[1]["priority"] != float('-inf') def filterDA_MT(self, DA): - if self.debug ==True: - print "IN FILTER: ", DA if DA[1]["da_state"] == 'END': return False - if DA[1]["da_type"] == "move_to_tasker" and DA[1]["priority"] != float('-inf'): - return True - else: - return False + return DA[1]["da_type"] == "move_to_tasker" and DA[1]["priority"] != float('-inf') def filterDA_BG(self, DA): - if self.debug ==True: - print "IN FILTER: ", DA if DA[1]["da_state"] == 'END': return False - if DA[1]["da_type"] == "bring_goods_tasker" and DA[1]["priority"] != float('-inf'): - return True - else: - return False + return DA[1]["da_type"] == "bring_goods_tasker" and DA[1]["priority"] != float('-inf') def schedule_new(self, cost_file): # print("\nSCHEDULE\n") self.lock.acquire() if len(self.queue) > 0: - ordered_queue = OrderedDict(sorted(self.queue.items(), - key=lambda kv: kv[1]["priority"], reverse=True)) - if self.debug ==True: - print "Queue before END check:\n", ordered_queue + ordered_queue = OrderedDict(sorted(self.queue.items(), + key=lambda kv: kv[1]["priority"], reverse=True)) + if self.debug == True: + print("Queue before END check:\n", ordered_queue) if self.isExecuting(): if not self.isDAAlive_no_lock(self.execField): - if self.debug ==True: - print "THA-> removes DA: ", self.execField["da_name"] + if self.debug == True: + print("THA-> removes DA: ", self.execField["da_name"]) self.removeDA_no_lock(self.execField) - if self.isInterrupting(): + if self.is_interrupting(): if not self.isDAAlive_no_lock(self.interruptField): - if self.debug ==True: - print "THA-> removes DA: ", self.interruptField["da_name"] + if self.debug == True: + print("THA-> removes DA: ", self.interruptField["da_name"]) self.removeDA_no_lock(self.interruptField) for da in self.queue.items(): if not self.isDAAlive_no_lock(da[1]): - if self.debug ==True: - print "THA-> removes DA: ", da[1]["da_name"] + if self.debug == True: + print("THA-> removes DA: ", da[1]["da_name"]) self.removeDA_no_lock(da[1]) - # if self.isExecuting(): - # exec_da_name = "/"+self.execField["da_name"] - # # print("checking EXEC node: ", exec_da_name) - # if self.execField["da_state"] == 'END': - # self.removeDA_no_lock(self.execField) - # self.execField = {} - # if not rosnode.rosnode_ping(exec_da_name, 1): - # print("FINIIIIIISSSSSHHHHHHEEEEDDDD") - # self.execField = {} - # if self.isInterrupting(): - # irr_da_name = "/"+self.interruptField["da_name"] - # # print("checking EXEC node: ", exec_da_name) - # if self.interruptField["da_state"] == 'END': - # self.removeDA_no_lock(self.interruptField) - # self.interruptField = {} - # if not rosnode.rosnode_ping(irr_da_name, 1): - # print("FINIIIIIISSSSSHHHHHHEEEEDDDD") - # self.interruptField = {} - - # for da in self.queue.items(): - # if da[1]["da_state"] == 'END': - # self.removeDA_no_lock(da[1]) - # if not rosnode.rosnode_ping("/"+da[1]["da_name"], 1): - # rospy.sleep(3) - # if not rosnode.rosnode_ping("/"+da[1]["da_name"], 1): - # print "REMOVED:" - # print da[1]["da_name"] - # self.removeDA_no_lock(da[1]) - # print "NEXT DA" - # print "OUT OUT" if len(self.queue) > 0: - ordered_queue = OrderedDict(sorted(self.queue.items(), - key=lambda kv: kv[1]["priority"], reverse=True)) - if self.debug ==True: - print "OQ:\n", ordered_queue + ordered_queue = OrderedDict(sorted(self.queue.items(), + key=lambda kv: kv[1]["priority"], reverse=True)) + if self.debug == True: + print("OQ:\n", ordered_queue) dac = next(iter(ordered_queue.items()))[1] DAset_GH = {} DAset_HF = {} @@ -604,36 +381,29 @@ def schedule_new(self, cost_file): cBJ = {} cMT = {} cBG = {} - # print "Q:" - # print self.queue + DAset_GH = filter(self.filterDA_GH, self.queue.items()) DAset_HF = filter(self.filterDA_HF, self.queue.items()) DAset_BJ = filter(self.filterDA_BJ, self.queue.items()) DAset_MT = filter(self.filterDA_MT, self.queue.items()) DAset_BG = filter(self.filterDA_BG, self.queue.items()) - # print "DAset_GH:" - # print DAset_GH - # print "DAset_T:" - # print DAset_T - # DAset_GH = {k: v for k, v in self.queue.tems() if "tiago_guideHuman" in v[1]["da_type"]} - # DAset_T = {k: v for k, v in self.queue.iteritems() if "tiago_transport" in v[1]["da_type"]} - q_GH = OrderedDict(sorted(DAset_GH, - key=lambda kv: kv[1]["priority"], reverse=True)) - q_HF = OrderedDict(sorted(DAset_HF, - key=lambda kv: kv[1]["priority"], reverse=True)) - q_BJ = OrderedDict(sorted(DAset_BJ, - key=lambda kv: kv[1]["priority"], reverse=True)) - q_MT = OrderedDict(sorted(DAset_MT, - key=lambda kv: kv[1]["priority"], reverse=True)) - q_BG = OrderedDict(sorted(DAset_BG, - key=lambda kv: kv[1]["priority"], reverse=True)) + q_GH = OrderedDict(sorted(DAset_GH, + key=lambda kv: kv[1]["priority"], reverse=True)) + q_HF = OrderedDict(sorted(DAset_HF, + key=lambda kv: kv[1]["priority"], reverse=True)) + q_BJ = OrderedDict(sorted(DAset_BJ, + key=lambda kv: kv[1]["priority"], reverse=True)) + q_MT = OrderedDict(sorted(DAset_MT, + key=lambda kv: kv[1]["priority"], reverse=True)) + q_BG = OrderedDict(sorted(DAset_BG, + key=lambda kv: kv[1]["priority"], reverse=True)) if self.debug_file == True: cost_file.write("\n"+"Q:\n") cost_file.write(str(self.queue)+"\n") if len(DAset_HF) > 0: - if self.debug ==True: - print "Have HF" + if self.debug == True: + print("Have HF") # print "q_GH" # print q_GH cHF = next(iter(q_HF.items()))[1] @@ -642,141 +412,125 @@ def schedule_new(self, cost_file): cost_file.write(str(cHF)+"\n") dac = cHF if self.isExecuting(): - if not self.filterDA_HF([None,self.execField]): + if not self.filterDA_HF([None, self.execField]): switch_priority = "normal" - self.updateIrrField(dac,switch_priority,cost_file) + self.updateIrrField(dac, switch_priority, cost_file) self.lock.release() return # print "cGH" # print cGH elif len(DAset_GH) > 0: - if self.debug ==True: - print "Have GH" + if self.debug == True: + print("Have GH") cGH = next(iter(q_GH.items()))[1] if self.debug_file == True: cost_file.write("\n"+"cGH:"+"\n") cost_file.write(str(cGH)+"\n") - dac = cGH + dac = cGH if self.isExecuting(): - if self.filterDA_HF([None,self.execField]): + if self.filterDA_HF([None, self.execField]): self.lock.release() return - elif not self.filterDA_GH([None,self.execField]): + elif not self.filterDA_GH([None, self.execField]): switch_priority = "normal" - self.updateIrrField(dac,switch_priority,cost_file) + self.updateIrrField(dac, switch_priority, cost_file) self.lock.release() - return + return elif len(DAset_BJ) > 0: - if self.debug ==True: - print "Have BJ" + if self.debug == True: + print("Have BJ") cBJ = next(iter(q_BJ.items()))[1] if self.debug_file == True: cost_file.write("\n"+"cBJ:"+"\n") cost_file.write(str(cBJ)+"\n") - dac = cBJ + dac = cBJ if self.isExecuting(): - if self.filterDA_HF([None,self.execField]): + if self.filterDA_HF([None, self.execField]): self.lock.release() return - elif not self.filterDA_BJ([None,self.execField]): + elif not self.filterDA_BJ([None, self.execField]): switch_priority = "normal" - self.updateIrrField(dac,switch_priority,cost_file) + self.updateIrrField(dac, switch_priority, cost_file) self.lock.release() - return + return elif len(DAset_MT) > 0: - if self.debug ==True: - print "Have MT" + if self.debug == True: + print("Have MT") cMT = next(iter(q_MT.items()))[1] if self.debug_file == True: cost_file.write("\n"+"cMT:"+"\n") cost_file.write(str(cMT)+"\n") - dac = cMT + dac = cMT if self.isExecuting(): - if self.filterDA_HF([None,self.execField]): + if self.filterDA_HF([None, self.execField]): self.lock.release() return - elif not self.filterDA_MT([None,self.execField]): + elif not self.filterDA_MT([None, self.execField]): switch_priority = "normal" - self.updateIrrField(dac,switch_priority,cost_file) + self.updateIrrField(dac, switch_priority, cost_file) self.lock.release() - return + return elif len(DAset_BG) > 0: - if self.debug ==True: - print "Have BG" + if self.debug == True: + print("Have BG") cBG = next(iter(q_BG.items()))[1] if self.debug_file == True: cost_file.write("\n"+"cBG:"+"\n") cost_file.write(str(cBG)+"\n") - dac = cBG + dac = cBG if self.isExecuting(): - if self.filterDA_HF([None,self.execField]): + if self.filterDA_HF([None, self.execField]): self.lock.release() return - elif not self.filterDA_BG([None,self.execField]): + elif not self.filterDA_BG([None, self.execField]): switch_priority = "normal" - self.updateIrrField(dac,switch_priority,cost_file) + self.updateIrrField(dac, switch_priority, cost_file) self.lock.release() - return + return else: if self.isExecuting(): if not self.execField["da_type"] == dac["da_type"]: - print "Executing a task of a type that has higher priority" + print("Executing a task of a type that has higher priority") self.lock.release() - return + return # if not (len(DAset_GH) > 0 or len(DAset_HF) > 0): # cost_file.write("\n"+"No candidate"+"\n") # print "No candidate" # self.lock.release() # return - if self.debug ==True: - print "dac: ", dac - if self.isExecuting() and not self.isInterrupting(): - print "COMPARISION of TASKS of same type" - if self.debug ==True: - print "dac priority: ", dac["priority"] - print "exe priority: ", self.execField["priority"] + if self.debug == True: + print("dac: ", dac) + if self.isExecuting() and not self.is_interrupting(): + print("COMPARISION of TASKS of same type") + if self.debug == True: + print("dac priority: ", dac["priority"]) + print("exe priority: ", self.execField["priority"]) if dac["priority"] > self.execField["priority"]: - if self.debug ==True: - print "REQUESTING reqParam services" - # Exec cost for suspension behaviour and task continue starting from DAC final_resource_state -- cc_exec - # , incremental (per sec) cost while waiting for start -- ccps_exec - - # while (not rospy.is_shutdown()) and not self.hasService('/'+self.execField["da_name"]+'/TaskER/get_suspend_conditions'): - # print "Schedule thread waits for service: "+ '/'+self.execField["da_name"]+'/TaskER/get_suspend_conditions' - # rospy.sleep(0.1) - # rospy.wait_for_service('/'+self.execField["da_name"]+'/TaskER/get_suspend_conditions', timeout=2) - # get_susp_cond = rospy.ServiceProxy('/'+self.execField["da_name"]+'/TaskER/get_suspend_conditions', SuspendConditions) - # trig = SuspendConditionsRequest() - # resp = get_susp_cond(dac["scheduleParams"].final_resource_state) - - resp = self.tasker_communicator.call_sus_cond(highest_da["scheduleParams"].final_resource_state, highest_da["da_name"]) + if self.debug == True: + print("REQUESTING reqParam services") + + resp = self.tasker_communicator.call_sus_cond( + highest_da["scheduleParams"].final_resource_state, highest_da["da_name"]) cc_exec = resp.cost_to_resume ccps_exec = resp.cost_per_sec - # DAC cost while switched after EXEC finishes -- cc_dac, incremental (per sec) cost while waiting for start -- ccps_dac - # ccps_exec = resp.cost_per_sec - - # while (not rospy.is_shutdown()) and not self.hasService('/'+dac["da_name"]+'/TaskER/get_cost_on_conditions'): - # print "Schedule thread waits for service: "+ '/'+dac["da_name"]+'/TaskER/get_cost_on_conditions' - # rospy.sleep(0.1) - # rospy.wait_for_service('/'+dac["da_name"]+'/TaskER/get_cost_on_conditions', timeout=2) - # get_cost_cond = rospy.ServiceProxy('/'+dac["da_name"]+'/TaskER/get_cost_on_conditions', CostConditions) - # trig = CostConditionsRequest() - # resp = get_cost_cond(self.execField["scheduleParams"].final_resource_state) - - resp = self.tasker_communicator.call_cost_cond(self.execField["scheduleParams"].final_resource_state, self.execField["da_name"]) + resp = self.tasker_communicator.call_cost_cond( + self.execField["scheduleParams"].final_resource_state, self.execField["da_name"]) cc_dac = resp.cost_to_complete ccps_dac = dac["scheduleParams"].cost_per_sec # Calculation of costs to switch and not switch - c_switch = dac["scheduleParams"].cost + cc_exec + ccps_exec * dac["scheduleParams"].completion_time - c_wait = self.execField["scheduleParams"].cost + cc_dac + ccps_dac * self.execField["scheduleParams"].completion_time + c_switch = dac["scheduleParams"].cost + cc_exec + \ + ccps_exec * dac["scheduleParams"].completion_time + c_wait = self.execField["scheduleParams"].cost + cc_dac + \ + ccps_dac * \ + self.execField["scheduleParams"].completion_time # send schedule data for visualisation shdl_data = ShdlDataStamped() - shdl_data.header.stamp = rospy.Time.now() + shdl_data.header.stamp = self.clock.now().to_msg() shdl_data.data.dac_cost = dac["scheduleParams"].cost shdl_data.data.exec_cost = self.execField["scheduleParams"].cost shdl_data.data.dac_cc = cc_dac @@ -788,172 +542,61 @@ def schedule_new(self, cost_file): shdl_data.data.dac_id = dac["da_id"] shdl_data.data.exec_id = self.execField["da_id"] self.sdhl_pub.publish(shdl_data) - print "$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$" - print "SHDL_DATA: " - print shdl_data.data - print "$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$" + print("$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$") + print("SHDL_DATA: ") + print(shdl_data.data) + print("$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$") if (c_switch < (c_wait - c_wait*0.1)): switch_priority = "normal" - print "#################################################" - print "SWITCH" - print "#################################################" - self.updateIrrField(dac,switch_priority,cost_file) + print("#################################################") + print("SWITCH") + print("#################################################") + self.updateIrrField(dac, switch_priority, cost_file) else: - print "candidate priority was less then executing task" + print("candidate priority was less then executing task") switch_priority = "normal" - self.updateIrrField(dac,switch_priority,cost_file) - # if (self.execField["da_type"] == "tiago_humanFell") and cHF!={}: - # dac = cHF - # if debug == True: - # cost_file.write("\n"+"dac:"+"\n") - # cost_file.write(str(dac)+"\n") - # elif (self.execField["da_type"] == "tiago_humanFell") and cHF=={}: - # print "Exec: hF, no hF in queue" - # if debug == True: - # cost_file.write("\n"+"Exec: hF, no hF in queue"+"\n") - # elif self.execField["da_type"] == "tiago_guideHuman": - # if len(DAset_HF) > 0: - # cost_file.write("\n"+"GH executing, there is HF in Q"+"\n") - # self.updateIrrField(cHF,cost_file) - # self.lock.release() - # return - # elif len(DAset_GH) > 0: - # dac = cGH - # if debug == True: - # cost_file.write("\n"+"dac:"+"\n") - # cost_file.write(str(dac)+"\n") - # else: - # cost_file.write("\n"+"No candidate"+"\n") - # print "No candidate" - # else: - # cost_file.write("\n"+"DA in ExecField has unknown type task"+"\n") - # if dac == {}: - # cost_file.write("\n"+"No candidate"+"\n") - # print "No candidate" - # else: - # cost_file.write("\n"+"Exec cost:"+"\n") - # cost_file.write("\t"+str(self.execField["scheduleParams"].cost)+"\n") - # cost_file.write("DAC cost:"+"\n") - # cost_file.write("\t"+str(dac["da_name"])+": "+str(dac["scheduleParams"].cost)+"\n") - # # if dac["scheduleParams"].cost < self.execField["scheduleParams"].cost: - # cost_file.write("\n"+"Have candidate"+"\n") - # cost_file.write("CHECK pair combinations"+"\n") - # print "WAITING FOR SUSPEND COST from exec" - # exec_da_name = "/"+self.execField["da_name"] - # if not rosnode.rosnode_ping(exec_da_name, 1): - # print("EXEC FINIIIIIISSSSSHHHHHHEEEEDDDD") - # self.execField = {} - # self.lock.release() - # return - # rospy.wait_for_service('/'+self.execField["da_name"]+'/TaskER/get_suspend_conditions') - # get_susp_cond = rospy.ServiceProxy('/'+self.execField["da_name"]+'/TaskER/get_suspend_conditions', SuspendConditions) - # trig = SuspendConditionsRequest() - # resp = get_susp_cond(dac["scheduleParams"].final_resource_state) - # cost_file.write("\n"+"EXEC:"+"\n") - # cc_exec = resp.cost_to_resume - # cost_file.write("\tcc:"+"\n") - # cost_file.write(str(cc_exec)+"\n") - # ccps_exec = resp.cost_per_sec - # cost_file.write("\tccps:"+"\n") - # cost_file.write(str(ccps_exec)+"\n") - - # print "WAITING FOR COST from candidate" - # dac_name = "/"+dac["da_name"] - # if not rosnode.rosnode_ping(dac_name, 1): - # rospy.sleep(3) - # if not rosnode.rosnode_ping(dac_name, 1): - # print "REMOVED:" - # print dac["da_name"] - # self.removeDA(dac) - # self.lock.release() - # return - # rospy.wait_for_service('/'+dac["da_name"]+'/TaskER/get_cost_on_conditions') - # get_cost_cond = rospy.ServiceProxy('/'+dac["da_name"]+'/TaskER/get_cost_on_conditions', CostConditions) - # trig = CostConditionsRequest() - # resp = get_cost_cond(self.execField["scheduleParams"].final_resource_state) - # cc_dac = resp.cost_to_complete - # cost_file.write("\n"+"DAC:"+"\n") - # cost_file.write("\tcc:"+"\n") - # cost_file.write(str(cc_dac)+"\n") - # ccps_dac = dac["scheduleParams"].cost_per_sec - # cost_file.write("\tccps:"+"\n") - # cost_file.write(str(ccps_dac)+"\n") - # cost_file.write("\n"+"COMBINATION:"+"\n") - # c_switch = dac["scheduleParams"].cost + cc_exec + ccps_exec * dac["scheduleParams"].completion_time - # c_wait = self.execField["scheduleParams"].cost + cc_dac + ccps_dac * self.execField["scheduleParams"].completion_time - # cost_file.write("\tc_switch:"+"\n") - # cost_file.write(str(c_switch)+"\n") - # cost_file.write("\tc_wait:"+"\n") - # cost_file.write(str(c_wait)+"\n") - - # shdl_data = ShdlDataStamped() - # shdl_data.header.stamp = rospy.Time.now() - # shdl_data.data.dac_cost = dac["scheduleParams"].cost - # shdl_data.data.exec_cost = self.execField["scheduleParams"].cost - # shdl_data.data.dac_cc = cc_dac - # shdl_data.data.exec_cc = cc_exec - # shdl_data.data.exec_ccps = ccps_exec - # shdl_data.data.dac_ccps = ccps_dac - # shdl_data.data.switch_cost = c_switch - # shdl_data.data.wait_cost = c_wait - # shdl_data.data.dac_id = dac["da_id"] - # shdl_data.data.exec_id = self.execField["da_id"] - # self.sdhl_pub.publish(shdl_data) - - - # if (c_switch < (c_wait - c_wait*0.1)): - # cost_file.write("\n"+"SWITCH SWITCH SWITCH SWITCH "+"\n") - # self.updateIrrField(dac,cost_file) - # # else: - # # cost_file.write("\n"+"DAC < EXEC cost:"+"\n") - # # print "candidate priority was less then executing task" - elif not self.isExecuting() and not self.isInterrupting(): - print "No EXEC, NO IRR dynamic agent" - # if len(DAset_HF) > 0: - # print "HF len > 0" - # self.updateIrrField(cHF,cost_file) - # elif len(DAset_GH) > 0: - # print "GH len > 0" - # self.updateIrrField(cGH,cost_file) + self.updateIrrField(dac, switch_priority, cost_file) + elif not self.isExecuting() and not self.is_interrupting(): + print("No EXEC, NO IRR dynamic agent") if dac != None: switch_priority = "normal" - self.updateIrrField(dac,switch_priority,cost_file) + self.updateIrrField(dac, switch_priority, cost_file) else: - print "No candidate" + print("No candidate") else: - print "Processing switch" + print("Processing switch") if not self.isExecuting(): - print "No EXEC dynamic agent" + print("No EXEC dynamic agent") else: - if self.debug ==True: - print "\nEXEC: " - print "ID: ", self.execField["da_id"] - print "Cost: ", self.execField["scheduleParams"].cost - print "SP: \n", self.execField["scheduleParams"], "\n" - if not self.isInterrupting(): - if self.debug ==True: - print "No INTERRUPTING dynamic agent" + if self.debug == True: + print("\nEXEC: ") + print("ID: ", self.execField["da_id"]) + print("Cost: ", self.execField["scheduleParams"].cost) + print("SP: \n", self.execField["scheduleParams"], "\n") + if not self.is_interrupting(): + if self.debug == True: + print("No INTERRUPTING dynamic agent") else: - if self.debug ==True: - print "\tINTERRUPT: " - print "ID: ", self.interruptField["da_id"] - print "Cost: ", self.interruptField["scheduleParams"].cost - print "SP: \n", self.interruptField["scheduleParams"], "\n" - if self.debug ==True: - print "\tQUEUE: " + if self.debug == True: + print("\tINTERRUPT: ") + print("ID: ", self.interruptField["da_id"]) + print("Cost: ", self.interruptField["scheduleParams"].cost) + print("SP: \n", self.interruptField["scheduleParams"], "\n") + if self.debug == True: + print("\tQUEUE: ") for key_id in self.queue.items(): - print "ID: ", self.queue[key_id[0]]["da_id"] - print "Cost: ", self.queue[key_id[0]]["scheduleParams"].cost - print "SP: \n", self.queue[key_id[0]]["scheduleParams"], "\n" + print("ID: ", self.queue[key_id[0]]["da_id"]) + print("Cost: ", self.queue[key_id[0]]["scheduleParams"].cost) + print("SP: \n", self.queue[key_id[0]]["scheduleParams"], "\n") self.lock.release() # print("\nSCHEDULED\n") def switchDA(self): - r = rospy.Rate(5) + r = Rate(5) self.switchIndicator.wait() - if self.isInterrupting(): + if self.is_interrupting(): print("\nSWITCHING\n") self.lock.acquire() if self.isExecuting(): @@ -961,21 +604,22 @@ def switchDA(self): self.lock.release() max_sleep_counter = 6 sleep_counter = 0 - while self.getDALastCMD(commanding["da_name"]) in ['start','resume'] and commanding["da_state"][0] in ["Wait", "Init", "UpdateTask"]: - print "[Switch] -- while last cmd" + while self.getDALastCMD(commanding["da_name"]) in ['start', 'resume'] and commanding["da_state"][0] in ["Wait", "Init", "UpdateTask"]: + print("[Switch] -- while last cmd") sleep_counter = sleep_counter + 1 # while commanding DA stays in ["Wait", "init", "UpdateTask"] longer then max_sleep_counter after ['start','resume'] signal, # terminate the commanding DA if max_sleep_counter == sleep_counter: - self.set_DA_signal(da_name=commanding["da_name"], signal = "terminate", data = ["priority", self._switch_priority, - #THA może zarządać odpalenia konkretnej sekwencji wstrzymania np. ("rosrun", "TaskER", "exemplary_susp_task"), - ]) - while not rospy.is_shutdown(): - print "[Switch] -- while sleep counter" + self.set_DA_signal(da_name=commanding["da_name"], signal="terminate", data=["priority", self._switch_priority, + # THA może zarządać odpalenia konkretnej sekwencji wstrzymania np. ("rosrun", "TaskER", "exemplary_susp_task"), + ]) + while rclpy.ok(): + print("[Switch] -- while sleep counter") wait_flag = self.isDAAlive_with_lock(commanding) if wait_flag: - print("Waiting for DA: ",commanding["da_name"]," to terminate after long processing of ['start','resume'] command, and new interruption is comming") - rospy.Rate(5).sleep() + print( + "Waiting for DA: ", commanding["da_name"], " to terminate after long processing of ['start','resume'] command, and new interruption is comming") + Rate(5).sleep() else: # Exec DA is terminated, remove it from Exec field self.lock.acquire() @@ -984,19 +628,21 @@ def switchDA(self): break break r.sleep() - + if self.isDAAlive_with_lock(commanding): print("SEND SUSPEND to commanding: ", commanding["da_id"]) - self.set_DA_signal(da_name=commanding["da_name"], signal = "susp", data = ["priority", self._switch_priority, - #THA może zarządać odpalenia konkretnej sekwencji wstrzymania np. ("rosrun", "TaskER", "exemplary_susp_task"), - ]) + self.set_DA_signal(da_name=commanding["da_name"], signal="susp", data=["priority", self._switch_priority, + # THA może zarządać odpalenia konkretnej sekwencji wstrzymania np. ("rosrun", "TaskER", "exemplary_susp_task"), + ]) - # 10hz + # 10hz # wait until exec DA terminates or swithes to wait state - while not rospy.is_shutdown(): - wait_flag = (self.isDAAlive_with_lock(commanding) and (commanding["da_state"][0]!="Wait")) + while rclpy.ok(): + wait_flag = (self.isDAAlive_with_lock(commanding) and ( + commanding["da_state"][0] != "Wait")) if wait_flag: - print("Switch thread waits for exec_da to be WAIT or DEAD") + print( + "Switch thread waits for exec_da to be WAIT or DEAD") r.sleep() else: if not self.isDAAlive_with_lock(commanding): @@ -1015,27 +661,33 @@ def switchDA(self): # print (srv_name) # print("\nSWITCHING: waiting for QUEUED startTask\n") # rospy.wait_for_service(srv_name, timeout=2) - print "\nSWITCHING: waiting for QUEUED to be in Init or Wait. It is in <", interrupting["da_state"][0], "> state." + print("\nSWITCHING: waiting for QUEUED to be in Init or Wait. It is in <", + interrupting["da_state"][0], "> state.") while not interrupting["da_state"][0] in ["Wait", "Init"]: - if rospy.is_shutdown(): - return - print "\nSWITCHING: waiting for QUEUED to be in Init or Wait. It is in <", interrupting["da_state"][0], "> state." + if not rclpy.ok(): + return + print("\nSWITCHING: waiting for QUEUED to be in Init or Wait. It is in <", + interrupting["da_state"][0], "> state.") r.sleep() - if interrupting["da_state"][0]=="Wait": - self.set_DA_signal(da_name=interrupting["da_name"], signal = "resume", data = []) - elif interrupting["da_state"][0]=="Init": - self.set_DA_signal(da_name=interrupting["da_name"], signal = "start", data = []) + if interrupting["da_state"][0] == "Wait": + self.set_DA_signal( + da_name=interrupting["da_name"], signal="resume", data=[]) + elif interrupting["da_state"][0] == "Init": + self.set_DA_signal( + da_name=interrupting["da_name"], signal="start", data=[]) print("\nSWITCHING: waiting for STARTED hold_now\n") # rospy.wait_for_service(srv_name, timeout=2) - print "\nSWITCHING: waiting for STARTED to be in UpdateTask or ExecFSM. It is in <", interrupting["da_state"][0], "> state." - while not interrupting["da_state"][0] in ["UpdateTask","ExecFSM"]: - if rospy.is_shutdown(): - return - print "\nSWITCHING: waiting for STARTED to be in UpdateTask or ExecFSM. It is in <", interrupting["da_state"][0], "> state." - rospy.sleep(0.5) + print("\nSWITCHING: waiting for STARTED to be in UpdateTask or ExecFSM. It is in <", + interrupting["da_state"][0], "> state.") + while not interrupting["da_state"][0] in ["UpdateTask", "ExecFSM"]: + if not rclpy.ok(): + return + print("\nSWITCHING: waiting for STARTED to be in UpdateTask or ExecFSM. It is in <", + interrupting["da_state"][0], "> state.") + Rate(5).sleep(0.5) # rospy.wait_for_service('/'+interrupting["da_name"]+'/TaskER/hold_now', timeout=2) self.lock.acquire() @@ -1046,32 +698,7 @@ def switchDA(self): self.lock.release() print("\nSWITCHED\n") else: - print ("[TH] -- Killing switch thread") - # self.isInterrupting = isInterrupting - # self.print_log = file - # self.handlers = {} - # self.startState = None - # self.endStates = [] - # self.facultativeStates = [] - # self.priority = 0 - # self.start_deadline = -1 - # node_namespace = rospy.get_name() + "/TaskER" - # srv_name = rospy.get_name()+'/TaskER/get_hold_conditions' - # self.s = rospy.Service(srv_name, HoldConditions, self.getHoldConditions) - # self.current_state = '' - # self.task_state = 0 # initialized_not_running - # self.current_state_m_thread = 0 - # self.res_srv = None - # self.suspend_srv = None - # # self.sub_hold = rospy.Subscriber(node_namespace+"/hold_now", String, self.onHold) - # # self.sub_res = rospy.Subscriber(node_namespace+"/resume_now", String, self.onResume) - # self.q = multiprocessing.Queue() - # self.fsm_stop_event = threading.Event() - # self.resumeState = None - # self.resumeData = None - # self.pub_state = rospy.Publisher('current_state', TaskState, queue_size=10) - # self.time_to_hold = -1 - # self.time_to_finish = -1 - # self.onResumeData = None + print("[TH] -- Killing switch thread") + def shutdown(self): self.switchIndicator.set() diff --git a/src/TaskER/dynamic_agent.py b/src/TaskER/dynamic_agent.py index d0ee8a2..1dd93c3 100644 --- a/src/TaskER/dynamic_agent.py +++ b/src/TaskER/dynamic_agent.py @@ -1,13 +1,15 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # encoding: utf8 -import sys, signal +import sys +import signal import time import threading -import rospy -import smach -import smach_ros +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile +from rclpy.clock import Clock, ClockType from std_msgs.msg import String from tasker_msgs.msg import Status, CMD, ScheduleParams from tasker_msgs.srv import CostConditions, SuspendConditions @@ -20,6 +22,7 @@ global USE_SMACH_INRTOSPECTION_SERVER USE_SMACH_INRTOSPECTION_SERVER = False + def signal_handler(signal, frame): print("\nprogram exiting gracefully") sys.exit(0) @@ -28,6 +31,7 @@ def signal_handler(signal, frame): def sleep_rate(rate): time.sleep(1/rate) + class SmachShutdownManager: def __init__(self, main_sm, list_asw, list_sis): self.__main_sm__ = main_sm @@ -36,74 +40,65 @@ def __init__(self, main_sm, list_asw, list_sis): def on_shutdown(self, signal, frame): global USE_SMACH_INRTOSPECTION_SERVER - print "DA MANAGER" + print("DA MANAGER") # Stop all introspection servers if USE_SMACH_INRTOSPECTION_SERVER: - print 'SmachShutdownManager.on_shutdown: stopping all introspection servers' + print('SmachShutdownManager.on_shutdown: stopping all introspection servers') for sis in reversed(self.__list_sis__): sis.stop() # Stop all smach_ros action servers - print 'SmachShutdownManager.on_shutdown: stopping all smach_ros action servers' + print('SmachShutdownManager.on_shutdown: stopping all smach_ros action servers') for asw in reversed(self.__list_asw__): - # This is a veru ugly hack: + # Modify based on ROS2 behavior asw._action_server.action_server.started = False - # asw.wrapped_container.request_preempt() - - #rospy.sleep(2.0) class SuspendRequest: def __init__(self): self.req_data = [] + def setData(self, data): self.req_data = data + def getData(self): return self.req_data + def clearData(self): self.req_data = [] -class DynAgent: - def __init__(self, da_name, da_id, da_type, ptf_csp, da_state_name): - global USE_SMACH_INRTOSPECTION_SERVER - global debug +class DynAgent(Node): + def __init__(self, da_name, da_id, da_type, ptf_csp, da_state_name): + super().__init__(da_name) - debug = False + self.debug = False self.name = da_name - if USE_SMACH_INRTOSPECTION_SERVER: - rospy.init_node(self.name) - rospy.sleep(0.1) - #self.sub_task_state_cmd = rospy.Subscriber('/' + self.name + '/task_state_cmd', String, self.callbackTaskStateCmd) self.finished = False - # self.pub_diag = rospy.Publisher('/current_dyn_agent/diag', tiago_msgs.msg.DynAgentDiag, queue_size=10) - # self.pub_shdl = rospy.Publisher('~shdl_params', ScheduleParams, queue_size=10) - # assign ptf update state + self.ptf_csp = ptf_csp - # set DA_ID used by TaskER self.da_id = int(da_id) self.taskType = da_type self.exec_fsm_state = "" self.da_state = ["Init"] self.terminateFlag = False self.startFlag = False - # self.process_ptf_csp(["scheduleParams", None]) - # object to send suspend request to the current task stage self.da_suspend_request = SuspendRequest() - self.da_suspend_request.setData(["cmd","","param_name", "suspension requirements from the task harmoniser"]) + self.da_suspend_request.setData( + ["cmd", "", "param_name", "suspension requirements from the task harmoniser"]) self.is_initialised = False - self.tasker_communicator = DACommunicator(da_name=da_name, cond_cost_handler=self.startConditionHandler, sus_cost_handler=self.suspendConditionHandler) + self.tasker_communicator = DACommunicator( + da_name=da_name, cond_cost_handler=self.startConditionHandler, sus_cost_handler=self.suspendConditionHandler) - def startTask(self,data): + def startTask(self, data): self.startFlag = True self.da_suspend_request.setData(data) def suspTask(self, data="suspension requirements from the task harmoniser"): - print("\n","HOLD: ",str( data)+"\n") - # propagate suspend request to exec FSM + print("\n", "HOLD: ", str(data)+"\n") self.da_suspend_request.setData(data) - # self.main_sm.request_preempt() + def terminateDA(self): if not self.terminateFlag == True: my_status = Status() @@ -111,40 +106,45 @@ def terminateDA(self): my_status.da_name = self.name my_status.type = self.taskType if self.main_sm.is_running(): - if self.da_state[0] == "Init" or self.getActiveStates( self.main_sm )[0][0] in ["Wait", "UpdateTask"]: - self.cmd_handler(CMD(recipient_name=self.name,cmd="terminate")) + if self.da_state[0] == "Init" or self.getActiveStates(self.main_sm)[0][0] in ["Wait", "UpdateTask"]: + self.cmd_handler( + CMD(recipient_name=self.name, cmd="terminate")) self.da_state = ["END"] my_status.da_state = self.da_state self.tasker_communicator.pub_status(my_status) - # self.pub_status.publish(my_status) + # self.pub_status.publish(my_status) self.terminateFlag = True else: - print "DA triggered self termination flag. It is in CMD state, so the flag triggers preemption" + print( + "DA triggered self termination flag. It is in CMD state, so the flag triggers preemption") self.suspTask(["cmd", "terminate"]) self.da_state = ["END"] my_status.da_state = self.da_state self.tasker_communicator.pub_status(my_status) - # self.pub_status.publish(my_status) + # self.pub_status.publish(my_status) self.terminateFlag = True else: - print "DA -> TaskER Termination not required, because TaskER FSM is not running. Sending state to THA" - self.cmd_handler(CMD(recipient_name=self.name,cmd="terminate")) + print( + "DA -> TaskER Termination not required, because TaskER FSM is not running. Sending state to THA") + self.cmd_handler( + CMD(recipient_name=self.name, cmd="terminate")) self.da_state = ["END"] my_status.da_state = self.da_state self.tasker_communicator.pub_status(my_status) - # self.pub_status.publish(my_status) + # self.pub_status.publish(my_status) self.terminateFlag = True - print "sendStatusThread close" + print("sendStatusThread close") self.tasker_communicator.close() - print "sendStatusThread closed" + print("sendStatusThread closed") del self.tasker_communicator - print "sendStatusThread ended" + print("sendStatusThread ended") else: - print "termination flag was already handled" + print("termination flag was already handled") + def process_ptf_csp(self, req): (flag, result) = self.ptf_csp(req) if flag == 'self-terminate': - print "DA_tasker: SELF terminate" + print("DA_tasker: SELF terminate") self.terminateDA() if req[0] == 'suspendCondition': return SuspendConditionsResponse() @@ -156,13 +156,12 @@ def process_ptf_csp(self, req): return None else: return result + def updateStatus(self): - global debug - if self.is_initialised == False : + if self.is_initialised == False: self.da_state = ['Init'] else: - #print "update, states: ", self.getActiveStates( self.main_sm ) - self.da_state = self.getActiveStates( self.main_sm )[0] + self.da_state = self.getActiveStates(self.main_sm)[0] my_status = Status() my_status.da_id = self.da_id @@ -174,15 +173,13 @@ def updateStatus(self): else: return my_status.da_state = self.da_state - if debug: - print("UPDATEING STATUS params of: "+str(self.name)+"\n of "+ str( self.taskType)+" type \n"+str(my_status.schedule_params)+"\n") - + if self.debug: + print("UPDATING STATUS params of: "+str(self.name)+"\n of " + + str(self.taskType)+" type \n"+str(my_status.schedule_params)+"\n") self.tasker_communicator.pub_status(my_status) - # self.pub_status.publish(my_status) - + def cmd_handler(self, data): - global debug - print "DA got CMD: ", data + print("DA got CMD: ", data) if data.cmd == "start": # task will be started, so the interface to request start conditions by 'par' buffer is not required anymore # self.cost_cond_srv.shutdown() @@ -192,14 +189,14 @@ def cmd_handler(self, data): # self.hold_service = rospy.Service(self.node_namespace+"/hold_now", Trigger, lambda : None ) elif data.cmd == "susp" and self.da_state[0] == "ExecFSM": # self.hold_service.shutdown() - #self.start_service = rospy.Service(self.node_namespace+"/startTask", Trigger, lambda : None ) + # self.start_service = rospy.Service(self.node_namespace+"/startTask", Trigger, lambda : None ) # self.susp_cond_srv.shutdown() # self.cost_cond_srv = rospy.Service(self.cost_cond_name, CostConditions, self.startConditionHandler) fsm_data = ["cmd", "susp"] fsm_data.extend(data.data) self.suspTask(fsm_data) elif data.cmd == "resume" and self.da_state[0] == "Wait": - #self.start_service.shutdown() + # self.start_service.shutdown() # self.cost_cond_srv.shutdown() # self.hold_service = rospy.Service(self.node_namespace+"/hold_now", Trigger, lambda : None ) # self.susp_cond_srv = rospy.Service(self.susp_cond_name, SuspendConditions, self.suspendConditionHandler) @@ -214,20 +211,22 @@ def cmd_handler(self, data): self.main_sm.shutdownRequest() def suspendConditionHandler(self, req): - return self.process_ptf_csp(["suspendCondition",req]) + return self.process_ptf_csp(["suspendCondition", req]) + def startConditionHandler(self, req): - return self.process_ptf_csp(["startCondition",req]) + return self.process_ptf_csp(["startCondition", req]) def run(self, main_sm, sis=None): global USE_SMACH_INRTOSPECTION_SERVER self.main_sm = main_sm - print "RUNNING DA" + print("RUNNING DA") - #sis_main = smach_ros.IntrospectionServer('behaviour_server', self.main_sm, '/SM_BEHAVIOUR_SERVER') - #sis_main.start() + # sis_main = smach_ros.IntrospectionServer('behaviour_server', self.main_sm, '/SM_BEHAVIOUR_SERVER') + # sis_main.start() if USE_SMACH_INRTOSPECTION_SERVER: - sis = smach_ros.IntrospectionServer(str("/"+self.name+"smach_view_server"), self.main_sm, self.name) + sis = smach_ros.IntrospectionServer( + str("/"+self.name+"smach_view_server"), self.main_sm, self.name) sis.start() else: sis = None @@ -236,9 +235,9 @@ def run(self, main_sm, sis=None): # ssm = SmachShutdownManager(self, self.main_sm, [], []) # setup status interface for the task harmoniser # self.pub_status = rospy.Publisher('TH/statuses', Status, queue_size=10) - # subsribe to commands from the task harmoniser + # subsribe to commands from the task harmoniser - print "Setting DA" + print("Setting DA") self.node_namespace = self.name + "/TaskER" # start_name = self.node_namespace + "/startTask" @@ -246,40 +245,44 @@ def run(self, main_sm, sis=None): # self.cost_cond_name = self.node_namespace + "/get_cost_on_conditions" # self.cost_cond_srv = rospy.Service(self.cost_cond_name, CostConditions, self.startConditionHandler) # Set shutdown hook - signal.signal(signal.SIGINT, ssm.on_shutdown ) - #rospy.on_shutdown( ssm.on_shutdown ) + signal.signal(signal.SIGINT, ssm.on_shutdown) + # rospy.on_shutdown( ssm.on_shutdown ) if not self.terminateFlag: - print "starting status send thread" - thread_status = threading.Thread(target=self.sendStatusThread, args=(1,)) + print("starting status send thread") + thread_status = threading.Thread( + target=self.sendStatusThread, args=(1,)) thread_status.start() - print "started status send thread" - print "starting cmd recv thread" + print("started status send thread") + print("starting cmd recv thread") thread_cmd = threading.Thread(target=self.recvCMDThread, args=(1,)) thread_cmd.start() - print "started cmd recv thread" - print "starting cost_cond thread" - thread_cost_cond = threading.Thread(target=self.costCondThread, args=(1,)) + print("started cmd recv thread") + print("starting cost_cond thread") + thread_cost_cond = threading.Thread( + target=self.costCondThread, args=(1,)) thread_cost_cond.start() - print "started cmd cost_cond thread" - print "starting cmd susp_cond thread" - thread_susp_cond = threading.Thread(target=self.suspCondThread, args=(1,)) + print("started cmd cost_cond thread") + print("starting cmd susp_cond thread") + thread_susp_cond = threading.Thread( + target=self.suspCondThread, args=(1,)) thread_susp_cond.start() - print "started cmd susp_cond thread" + print("started cmd susp_cond thread") # setup introspection server for smach viewer # extend userdata with suspension request object self.main_sm.userdata.susp_data = self.da_suspend_request - smach_thread = threading.Thread(target=self.main_sm.execute, args=(smach.UserData(),)) + smach_thread = threading.Thread( + target=self.main_sm.execute, args=(smach.UserData(),)) smach_thread.start() - # wait for start signal - #self.start_service = rospy.Service(self.node_namespace+"/startTask", Trigger, lambda : None ) + # wait for start signal + # self.start_service = rospy.Service(self.node_namespace+"/startTask", Trigger, lambda : None ) while True: - print "RUNNING" + print("RUNNING") self.updateStatus() if self.startFlag: - break + break if self.terminateFlag: - print "RUNNING: TERM FLAG" - #self.start_service.shutdown() + print("RUNNING: TERM FLAG") + # self.start_service.shutdown() ssm.on_shutdown(None, None) smach_thread.join() thread_status.join() @@ -288,32 +291,32 @@ def run(self, main_sm, sis=None): thread_susp_cond.join() return sleep_rate(1) - #self.start_service.shutdown() - print 'Smach thread is running' + # self.start_service.shutdown() + print('Smach thread is running') self.is_initialised = True - # setup suspend condition handler + # setup suspend condition handler # self.susp_cond_name = self.node_namespace + "/get_suspend_conditions" # self.susp_cond_srv = rospy.Service(self.susp_cond_name, SuspendConditions, self.suspendConditionHandler) # Block until everything is preempted smach_thread.join() - print "SMACH JOINED" + print("SMACH JOINED") self.terminateDA() - print "SMACH terminateDA" + print("SMACH terminateDA") thread_status.join() - print "SMACH thread_status" + print("SMACH thread_status") thread_cmd.join() - print "SMACH thread_cmd" + print("SMACH thread_cmd") thread_cost_cond.join() - print "SMACH thread_cost_cond" + print("SMACH thread_cost_cond") thread_susp_cond.join() - print "SMACH thread_susp_cond" - print "CONN Joined" + print("SMACH thread_susp_cond") + print("CONN Joined") self.terminateFlag = True - print 'Smach thread is finished' + print('Smach thread is finished') else: - print "DA -> have terminateFlag before TaskER FSM start" + print("DA -> have terminateFlag before TaskER FSM start") ssm.on_shutdown(None, None) - print "DYN AGENT ENDED" + print("DYN AGENT ENDED") # def callbackTaskStateCmd(self, data): # print 'DynAgent.callback' @@ -324,54 +327,55 @@ def run(self, main_sm, sis=None): def getActiveStates(self, sm): result = [] - if hasattr( sm, 'get_active_states' ): + if hasattr(sm, 'get_active_states'): for state_name in sm.get_active_states(): st = sm.get_children()[state_name] - if hasattr( st, 'description' ) and not st.description is None: + if hasattr(st, 'description') and not st.description is None: description = st.description else: description = '' - result.append( (state_name, description) ) + result.append((state_name, description)) for state_name in sm.get_active_states(): st = sm.get_children()[state_name] - result = result + self.getActiveStates( st ) + result = result + self.getActiveStates(st) return result def sendStatusThread(self, args): while not self.terminateFlag: if not self.tasker_communicator.get_tha_alive_flag(): - print 'DynAgent "' + self.name + '" detected the task_harmonizer is dead' + print('DynAgent "' + self.name + + '" detected the task_harmonizer is dead') self.terminateDA() - print "sendStatusThread terminateDA" + print("sendStatusThread terminateDA") fsm_data = ["cmd", "terminate"] self.suspTask(fsm_data) - print "sendStatusThread suspTask" + print("sendStatusThread suspTask") # self.main_sm.request_preempt() self.main_sm.shutdownRequest() - print "sendStatusThread shutdownRequest" + print("sendStatusThread shutdownRequest") break try: - # Publish diagnostic information + # Publish diagnostic information diag = tiago_msgs.msg.DynAgentDiag() diag.agent_name = self.name if self.startFlag: - # rospy.sleep(2) + # rospy.sleep(2) self.updateStatus() - active_states = self.getActiveStates( self.main_sm ) + active_states = self.getActiveStates(self.main_sm) for state_name, state_desc in active_states: - diag.current_states.append( state_name ) - diag.descriptions.append( state_desc ) + diag.current_states.append(state_name) + diag.descriptions.append(state_desc) else: diag.current_states.append("Init") - diag.descriptions.append( "desc" ) - + diag.descriptions.append("desc") + # self.pub_diag.publish( diag ) except Exception as e: - print 'Detected exception in dynamic agent sendStatusThread' - print e + print('Detected exception in dynamic agent sendStatusThread') + print(e) self.terminateDA() # self.main_sm.request_preempt() self.main_sm.shutdownRequest() @@ -386,16 +390,15 @@ def recvCMDThread(self, args): self.cmd_handler(self.tasker_communicator.sub_cmd()) except Exception as e: - print 'Detected exception in dynamic agent recvCMDThread' - print e + print('Detected exception in dynamic agent recvCMDThread') + print(e) self.terminateDA() # self.main_sm.request_preempt() self.main_sm.shutdownRequest() break time.sleep(0.2) - print "recvCMDThread ended" - + print("recvCMDThread ended") def costCondThread(self, args): @@ -404,15 +407,15 @@ def costCondThread(self, args): self.tasker_communicator.handle_cost_cond() except Exception as e: - print 'Detected exception in dynamic agent costCondThread' - print e + print('Detected exception in dynamic agent costCondThread') + print(e) self.terminateDA() # self.main_sm.request_preempt() self.main_sm.shutdownRequest() break time.sleep(0.2) - print "costCondThread ended" + print("costCondThread ended") def suspCondThread(self, args): @@ -421,12 +424,12 @@ def suspCondThread(self, args): self.tasker_communicator.handle_sus_cond() except Exception as e: - print 'Detected exception in dynamic agent suspCondThread' - print e + print('Detected exception in dynamic agent suspCondThread') + print(e) self.terminateDA() # self.main_sm.request_preempt() self.main_sm.shutdownRequest() break time.sleep(0.2) - print "suspCondThread ended" \ No newline at end of file + print("suspCondThread ended") diff --git a/src/TaskER/tasker_comm.py b/src/TaskER/tasker_comm.py index 9798f99..e521b85 100644 --- a/src/TaskER/tasker_comm.py +++ b/src/TaskER/tasker_comm.py @@ -1,230 +1,247 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # encoding: utf8 -import zmq -import time +import signal +import sys import threading -from tasker_msgs.msg import Status, CMD, ScheduleParams -from tasker_msgs.srv import CostConditionsResponse, CostConditionsRequest -from tasker_msgs.srv import SuspendConditionsResponse, SuspendConditionsRequest +import rclpy from io import BytesIO +from rclpy.node import Node +from rclpy.qos import QoSProfile +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup +from tasker_msgs.msg import Status, CMD, ScheduleParams +from tasker_msgs.srv import CostConditions, SuspendConditions +import zmq +import time -global socket_ports +global socket_ports # Socket ports -socket_ports = {"status": "5656", "cmd": "5657", "cost_cond":"5658", "sus_cond":"5659", "tha_life_tick":"5660"} +socket_ports = {"status": "5656", "cmd": "5657", + "cost_cond": "5658", "sus_cond": "5659", "tha_life_tick": "5660"} + def init_socket(context, port, socket_type): - if socket_type == 'pub': - socket_status_pub = context.socket(zmq.PUB) - socket_status_pub.bind("tcp://127.0.0.1:%s" % port) - return socket_status_pub - - if socket_type == 'pub_connect': - socket_status_push = context.socket(zmq.PUB) - socket_status_push.connect("tcp://127.0.0.1:%s" % port) - return socket_status_push - - elif socket_type == 'sub': - socket_cmd_sub = context.socket(zmq.SUB) - socket_cmd_sub.connect ("tcp://127.0.0.1:%s" % port) - socket_cmd_sub.subscribe(b'') - return socket_cmd_sub - - elif socket_type == 'sub_bind': - socket_cmd_sub = context.socket(zmq.SUB) - socket_cmd_sub.bind ("tcp://127.0.0.1:%s" % port) - socket_cmd_sub.subscribe(b'') - return socket_cmd_sub - - elif socket_type == 'server': - socket_server = context.socket(zmq.REP) - socket_server.connect("tcp://127.0.0.1:%s" % port) - return socket_server - - elif socket_type == 'client': - socket_client = context.socket(zmq.REQ) - socket_client.bind ("tcp://127.0.0.1:%s" % port) - return socket_client + if socket_type == 'pub': + socket_status_pub = context.socket(zmq.PUB) + socket_status_pub.bind("tcp://127.0.0.1:%s" % port) + return socket_status_pub -def sub_socket_filtered(socket, da_name): - while True: - msg = socket.recv_pyobj() - if msg.recipient_name == da_name: - return msg - - - -class DACommunicator(): - def __init__(self, da_name, cond_cost_handler, sus_cost_handler): - global socket_ports - context = zmq.Context() - self.da_name = da_name - self.context = context - self.cond_cost_handler = cond_cost_handler - self.sus_cost_handler = sus_cost_handler - self.sockets = [] - self.socket_status_pub = init_socket(self.context, socket_ports['status'], 'pub_connect') - self.socket_status_pub.setsockopt(zmq.LINGER, 0) - self.sockets.append(self.socket_status_pub) - self.socket_cmd_sub = init_socket(self.context, socket_ports['cmd'], 'sub') - self.socket_status_pub.setsockopt(zmq.LINGER, 0) - self.sockets.append(self.socket_cmd_sub) - self.socket_life_tick_sub = init_socket(self.context, socket_ports['tha_life_tick'], 'sub') - self.socket_life_tick_sub.setsockopt(zmq.CONFLATE, 1) - self.socket_life_tick_sub.setsockopt(zmq.RCVTIMEO, 3000) - self.socket_status_pub.setsockopt(zmq.LINGER, 0) - self.sockets.append(self.socket_life_tick_sub) - self.socket_cost_cond_srv = init_socket(self.context, socket_ports['cost_cond'], 'server') - self.socket_cost_cond_srv.setsockopt(zmq.LINGER, 0) - self.sockets.append(self.socket_cost_cond_srv) - self.socket_sus_cond_srv = init_socket(self.context, socket_ports['sus_cond'], 'server') - self.socket_sus_cond_srv.setsockopt(zmq.LINGER, 0) - self.sockets.append(self.socket_sus_cond_srv) - self.da_is_running = True - self.is_tha_alive = True - self.thread_tha_alive = threading.Thread(target=self.thaAliveThread, args=(1,)) - self.thread_tha_alive.start() - print "started cmd thread_tha_alive thread" - - def __del__(self): - # self.context.term() - print "\n\n\n\n DEL DACommunicator \n\n\n\n" - self.da_is_running = False - print "\n\n DACommunicator join thread \n\n" - #self.thread_tha_alive.join() - - def close(self): - try: - self.da_is_running = False - print "\n\n DACommunicator terminating context \n\n" - i=0 - for socket in self.sockets: - socket.close() - print "closed: ", i - i+=1 - self.context.term() - print "\n\n DACommunicator CLOSED \n\n" - - except Exception as e: - print 'Detected exception in DACommunicator' - print e - def pub_status(self, msg=None): - isinstance(msg, Status) - self.socket_status_pub.send_pyobj( msg ) - - def sub_cmd(self): - return sub_socket_filtered(self.socket_cmd_sub, self.da_name) - - def sub_tha_alive(self): - return self.socket_life_tick_sub.recv(zmq.NOBLOCK) - - def handle_cost_cond(self): - return self.socket_cost_cond_srv.send_pyobj(self.cond_cost_handler(self.socket_cost_cond_srv.recv_pyobj())) - - def handle_sus_cond(self): - return self.socket_cost_cond_srv.send_pyobj(self.sus_cost_handler(self.socket_sus_cond_srv.recv_pyobj())) - - def get_tha_alive_flag(self): - return self.is_tha_alive - - def thaAliveThread(self, args): - - timeout = 0 - while self.da_is_running: - try: - # print "thaAliveThread: ", self.is_tha_alive - self.socket_life_tick_sub.recv() - # self.sub_tha_alive() - self.is_tha_alive = True - time.sleep(1) - except zmq.ZMQError as e: - # print "thaAliveThread: EXCEPTION: ", e.errno - # print "thaAliveThread: EXCEPTION: ", zmq.EAGAIN - if e.errno == zmq.EAGAIN: - self.socket_life_tick_sub.close() - self.is_tha_alive = False - break - else: - raise - except Exception as e: - print 'Detected exception in DACommunicator' - - -class THACommunicator(): - def __init__(self): - global socket_ports - context = zmq.Context() - self.context = context - self.sockets = [] - self.socket_status_sub = init_socket(self.context, socket_ports['status'], 'sub_bind') - self.sockets.append(self.socket_status_sub) - self.socket_cmd_pub = init_socket(self.context, socket_ports['cmd'], 'pub') - self.socket_cmd_pub.setsockopt(zmq.LINGER, 1000) - self.sockets.append(self.socket_cmd_pub) - self.socket_life_tick_pub = init_socket(self.context, socket_ports['tha_life_tick'], 'pub') - self.socket_life_tick_pub.setsockopt(zmq.LINGER, 1000) - self.sockets.append(self.socket_life_tick_pub) - self.socket_cost_cond_cli = init_socket(self.context, socket_ports['cost_cond'], 'client') - self.socket_cost_cond_cli.setsockopt(zmq.LINGER, 1000) - self.sockets.append(self.socket_cost_cond_cli) - self.socket_sus_cond_cli = init_socket(self.context, socket_ports['sus_cond'], 'client') - self.socket_sus_cond_cli.setsockopt(zmq.LINGER, 1000) - self.sockets.append(self.socket_sus_cond_cli) - self.tha_is_running = True - self.thread_tha_alive = threading.Thread(target=self.thaAliveThread, args=(1,)) - self.thread_tha_alive.start() - print "started cmd thread_tha_alive thread" - - def __del__(self): - self.context.term() - print "\n\n\n\n DEL THACommunicator \n\n\n\n" - self.tha_is_running = False - self.thread_tha_alive.join() - - def close(self): - try: - self.tha_is_running = False - print "\n\n THACommunicator terminating context \n\n" - i=0 - for socket in self.sockets: - socket.close() - print "closed: ", i - i+=1 - self.context.term() - print "\n\n THACommunicator CLOSED \n\n" - - except Exception as e: - print 'Detected exception in THACommunicator' - print e - - def sub_status(self): - return self.socket_status_sub.recv_pyobj() - - def pub_cmd(self, msg=None): - isinstance(msg, CMD) - self.socket_cmd_pub.send_pyobj( msg ) - - def pub_life_tick(self): - self.socket_life_tick_pub.send( b'' ) - - def call_cost_cond(self, msg=None): - isinstance(msg, CostConditionsRequest) - self.socket_cost_cond_cli.send_pyobj(msg) - return self.socket_cost_cond_cli.recv_pyobj() - - def call_sus_cond(self, msg=None): - isinstance(msg, SuspendConditionsRequest) - self.socket_sus_cond_cli.send_pyobj(msg) - return self.socket_sus_cond_cli.recv_pyobj() - - - def thaAliveThread(self, args): - - while self.tha_is_running: - try: - self.pub_life_tick() - time.sleep(1) - except Exception as e: - print 'Detected exception in THACommunicator' + if socket_type == 'pub_connect': + socket_status_push = context.socket(zmq.PUB) + socket_status_push.connect("tcp://127.0.0.1:%s" % port) + return socket_status_push + + elif socket_type == 'sub': + socket_cmd_sub = context.socket(zmq.SUB) + socket_cmd_sub.connect("tcp://127.0.0.1:%s" % port) + socket_cmd_sub.subscribe(b'') + return socket_cmd_sub + + elif socket_type == 'sub_bind': + socket_cmd_sub = context.socket(zmq.SUB) + socket_cmd_sub.bind("tcp://127.0.0.1:%s" % port) + socket_cmd_sub.subscribe(b'') + return socket_cmd_sub + + elif socket_type == 'server': + socket_server = context.socket(zmq.REP) + socket_server.connect("tcp://127.0.0.1:%s" % port) + return socket_server + elif socket_type == 'client': + socket_client = context.socket(zmq.REQ) + socket_client.bind("tcp://127.0.0.1:%s" % port) + return socket_client + + +def sub_socket_filtered(socket, da_name): + while True: + msg = socket.recv_pyobj() + if msg.recipient_name == da_name: + return msg + + +class DACommunicator: + def __init__(self, da_name, cond_cost_handler, sus_cost_handler): + global socket_ports + context = zmq.Context() + self.da_name = da_name + self.context = context + self.cond_cost_handler = cond_cost_handler + self.sus_cost_handler = sus_cost_handler + self.sockets = [] + self.socket_status_pub = init_socket( + self.context, socket_ports['status'], 'pub_connect') + self.socket_status_pub.setsockopt(zmq.LINGER, 0) + self.sockets.append(self.socket_status_pub) + self.socket_cmd_sub = init_socket( + self.context, socket_ports['cmd'], 'sub') + self.socket_status_pub.setsockopt(zmq.LINGER, 0) + self.sockets.append(self.socket_cmd_sub) + self.socket_life_tick_sub = init_socket( + self.context, socket_ports['tha_life_tick'], 'sub') + self.socket_life_tick_sub.setsockopt(zmq.CONFLATE, 1) + self.socket_life_tick_sub.setsockopt(zmq.RCVTIMEO, 3000) + self.socket_status_pub.setsockopt(zmq.LINGER, 0) + self.sockets.append(self.socket_life_tick_sub) + self.socket_cost_cond_srv = init_socket( + self.context, socket_ports['cost_cond'], 'server') + self.socket_cost_cond_srv.setsockopt(zmq.LINGER, 0) + self.sockets.append(self.socket_cost_cond_srv) + self.socket_sus_cond_srv = init_socket( + self.context, socket_ports['sus_cond'], 'server') + self.socket_sus_cond_srv.setsockopt(zmq.LINGER, 0) + self.sockets.append(self.socket_sus_cond_srv) + self.da_is_running = True + self.is_tha_alive = True + self.thread_tha_alive = threading.Thread( + target=self.tha_alive_thread, args=(1,)) + self.thread_tha_alive.start() + print("started cmd thread_tha_alive thread") + + def __del__(self): + # self.context.term() + print("\n\n\n\n DEL DACommunicator \n\n\n\n") + self.da_is_running = False + print("\n\n DACommunicator join thread \n\n") + # self.thread_tha_alive.join() + + def close(self): + try: + self.da_is_running = False + print("\n\n DACommunicator terminating context \n\n") + i = 0 + for socket in self.sockets: + socket.close() + print("closed: ", i) + i += 1 + self.context.term() + print("\n\n DACommunicator CLOSED \n\n") + + except Exception as e: + print('Detected exception in DACommunicator') + print(e) + + def pub_status(self, msg=None): + isinstance(msg, Status) + self.socket_status_pub.send_pyobj(msg) + + def sub_cmd(self): + return sub_socket_filtered(self.socket_cmd_sub, self.da_name) + + def sub_tha_alive(self): + return self.socket_life_tick_sub.recv(zmq.NOBLOCK) + + def handle_cost_cond(self): + return self.socket_cost_cond_srv.send_pyobj(self.cond_cost_handler(self.socket_cost_cond_srv.recv_pyobj())) + + def handle_sus_cond(self): + return self.socket_cost_cond_srv.send_pyobj(self.sus_cost_handler(self.socket_sus_cond_srv.recv_pyobj())) + + def get_tha_alive_flag(self): + return self.is_tha_alive + + def tha_alive_thread(self, args): + + timeout = 0 + while self.da_is_running: + try: + # print "thaAliveThread: ", self.is_tha_alive + self.socket_life_tick_sub.recv() + # self.sub_tha_alive() + self.is_tha_alive = True + time.sleep(1) + except zmq.ZMQError as e: + # print "thaAliveThread: EXCEPTION: ", e.errno + # print "thaAliveThread: EXCEPTION: ", zmq.EAGAIN + if e.errno == zmq.EAGAIN: + self.socket_life_tick_sub.close() + self.is_tha_alive = False + break + else: + raise + except Exception as e: + print('Detected exception in DACommunicator') + + +class THACommunicator: + def __init__(self): + global socket_ports + context = zmq.Context() + self.context = context + self.sockets = [] + self.socket_status_sub = init_socket( + self.context, socket_ports['status'], 'sub_bind') + self.sockets.append(self.socket_status_sub) + self.socket_cmd_pub = init_socket( + self.context, socket_ports['cmd'], 'pub') + self.socket_cmd_pub.setsockopt(zmq.LINGER, 1000) + self.sockets.append(self.socket_cmd_pub) + self.socket_life_tick_pub = init_socket( + self.context, socket_ports['tha_life_tick'], 'pub') + self.socket_life_tick_pub.setsockopt(zmq.LINGER, 1000) + self.sockets.append(self.socket_life_tick_pub) + self.socket_cost_cond_cli = init_socket( + self.context, socket_ports['cost_cond'], 'client') + self.socket_cost_cond_cli.setsockopt(zmq.LINGER, 1000) + self.sockets.append(self.socket_cost_cond_cli) + self.socket_sus_cond_cli = init_socket( + self.context, socket_ports['sus_cond'], 'client') + self.socket_sus_cond_cli.setsockopt(zmq.LINGER, 1000) + self.sockets.append(self.socket_sus_cond_cli) + self.tha_is_running = True + self.thread_tha_alive = threading.Thread( + target=self.tha_alive_thread, args=(1,)) + self.thread_tha_alive.start() + print("started cmd thread_tha_alive thread") + + def __del__(self): + self.context.term() + print("\n\n\n\n DEL THACommunicator \n\n\n\n") + self.tha_is_running = False + self.thread_tha_alive.join() + + def close(self): + try: + self.tha_is_running = False + print("\n\n THACommunicator terminating context \n\n") + i = 0 + for socket in self.sockets: + socket.close() + print("closed: ", i) + i += 1 + self.context.term() + print("\n\n THACommunicator CLOSED \n\n") + + except Exception as e: + print('Detected exception in THACommunicator') + print(e) + + def sub_status(self): + return self.socket_status_sub.recv_pyobj() + + def pub_cmd(self, msg=None): + isinstance(msg, CMD) + self.socket_cmd_pub.send_pyobj(msg) + + def pub_life_tick(self): + self.socket_life_tick_pub.send(b'') + + def call_cost_cond(self, msg=None): + isinstance(msg, CostConditionsRequest) + self.socket_cost_cond_cli.send_pyobj(msg) + return self.socket_cost_cond_cli.recv_pyobj() + + def call_sus_cond(self, msg=None): + isinstance(msg, SuspendConditionsRequest) + self.socket_sus_cond_cli.send_pyobj(msg) + return self.socket_sus_cond_cli.recv_pyobj() + + def tha_alive_thread(self, args): + while self.tha_is_running: + try: + self.pub_life_tick() + time.sleep(1) + except Exception as e: + print('Detected exception in THACommunicator') diff --git a/src/TaskER/tha.py b/src/TaskER/tha.py index 96fce78..ae45c5b 100644 --- a/src/TaskER/tha.py +++ b/src/TaskER/tha.py @@ -1,14 +1,16 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 import signal import sys +import threading +import rclpy from TaskHarmoniser import TaskHarmoniser import time -import threading -import rospy -import rosmsg -from multitasker.srv import * -from multitasker.msg import * -from rospy_message_converter import json_message_converter +from multitasker.srv import TaskRequest, TaskRequestResponse +from multitasker.msg import InitParams +from rclpy.parameter import Parameter +from rclpy.node import Node +from rclpy.qos import QoSProfile +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup _FINISH = False scheduler = threading.Thread @@ -16,67 +18,79 @@ th = TaskHarmoniser +def init_da(request, response): + global th + print("GOT REQUEST") + new_id = th.get_next_id() + print("ID: ", new_id) + da_name = "DA_" + str(new_id) + init_params = request.init_params + th.initialise_da(request.application, request.version, + new_id, da_name, init_params) + th.add_da(new_id, da_name, request.application) + th.update_schedule_params(new_id, init_params) + return TaskRequestResponse(-1) + + +def scheduler_thread(): + global _FINISH + global th + while True: + cost_file = open("./TH_cost", "a+") + print("\n SCHEDULING \n") + th.schedule_new(cost_file) + print("\n SCHEDULED \n") + time.sleep(2) + if _FINISH: + th.send_indicator() + cost_file.close() + break + + +def switcher_thread(): + global th + global _FINISH + while True: + print("\n SWITCHING \n") + th.switch_da() + if _FINISH: + break -def initDA(data): - global th - print("GOT REQUEST") - new_id = th.getNextID() - print("ID: ",new_id) - json_str = json_message_converter.convert_ros_message_to_json(data.init_params) - print("S: ",json_str) - da_name = "DA_"+str(new_id) - th.initialiseDA(data.application, data.version, new_id, da_name, json_str) - # rospy.set_param('/'+da_name+'/init_params', stri) - th.addDA(new_id, da_name, data.application) - th.updateScheduleParams(new_id, data.init_params) - return TaskRequestResponse(-1) -def scheduler(): - global _FINISH - global th - while True: - cost_file = open("./TH_cost", "a+") - print "\n SCHEDULING \n" - th.schedule_new(cost_file) - print "\n SCHEDULED \n" - rospy.sleep(2) - if _FINISH: - th.sendIndicator() - cost_file.close() - break -def switcher(): - global th - global _FINISH - while True: - print "\n SWITCHING \n" - th.switchDA() - if _FINISH: - break def signal_handler(sig, frame): - print('You pressed Ctrl+C!') - global _FINISH - _FINISH = True - global scheduler - global switcher - scheduler.join() - switcher.join() - sys.exit(0) + print('You pressed Ctrl+C!') + global _FINISH + _FINISH = True + global scheduler + global switcher + scheduler.join() + switcher.join() + sys.exit(0) + + +class TaskHarmoniserNode(Node): + def __init__(self): + super().__init__('TH') + self.th = TaskHarmoniser() + qos = QoSProfile(depth=10) + self.srv = self.create_service( + TaskRequest, 'TH/new_task', init_da, qos_profile=qos) + self.srv.set_callback_group(MutuallyExclusiveCallbackGroup()) + self.get_logger().info('Ready') + + +if __name__ == "__main__": + global th + rclpy.init(args=sys.argv) + print("ready") + signal.signal(signal.SIGINT, signal_handler) + global scheduler + global switcher + scheduler = threading.Thread(target=scheduler_thread) + scheduler.start() + switcher = threading.Thread(target=switcher_thread) + switcher.start() -if __name__== "__main__": - global th - rospy.init_node('TH', anonymous=True) - print("ready") - signal.signal(signal.SIGINT, signal_handler) - th = TaskHarmoniser() - global scheduler - global switcher - scheduler = threading.Thread(target = scheduler) - scheduler.start() - switcher = threading.Thread(target = switcher) - switcher.start() - s = rospy.Service("TH/new_task", TaskRequest, initDA) + rclpy.spin(TaskHarmoniserNode()) - rospy.spin() - - print("END") - \ No newline at end of file + print("END") diff --git a/srv/CostConditions.srv b/srv/CostConditions.srv deleted file mode 100644 index 52e9a01..0000000 --- a/srv/CostConditions.srv +++ /dev/null @@ -1,4 +0,0 @@ -RobotResource final_resource_state ---- -float32 cost_per_sec -float32 cost_to_complete \ No newline at end of file diff --git a/srv/HoldConditions.srv b/srv/HoldConditions.srv deleted file mode 100644 index c346b8a..0000000 --- a/srv/HoldConditions.srv +++ /dev/null @@ -1,4 +0,0 @@ ---- -int8 priority -int32 time_to_hold -int32 time_to_finish diff --git a/srv/LaunchConditions.srv b/srv/LaunchConditions.srv deleted file mode 100644 index bd56234..0000000 --- a/srv/LaunchConditions.srv +++ /dev/null @@ -1,4 +0,0 @@ ---- -int8 task_priority -int32 start_deadline -int32 task_duration diff --git a/srv/SchedulingConditions.srv b/srv/SchedulingConditions.srv deleted file mode 100644 index 05749b7..0000000 --- a/srv/SchedulingConditions.srv +++ /dev/null @@ -1,2 +0,0 @@ ---- -int32 time_to_hold diff --git a/srv/StartTask.srv b/srv/StartTask.srv deleted file mode 100644 index d71eca5..0000000 --- a/srv/StartTask.srv +++ /dev/null @@ -1,3 +0,0 @@ -bool isInterrupting -string params ---- diff --git a/srv/SuspendConditions.srv b/srv/SuspendConditions.srv deleted file mode 100644 index 1657a08..0000000 --- a/srv/SuspendConditions.srv +++ /dev/null @@ -1,4 +0,0 @@ -RobotResource final_resource_state ---- -float32 cost_per_sec -float32 cost_to_resume \ No newline at end of file diff --git a/srv/TaskRequest.srv b/srv/TaskRequest.srv deleted file mode 100644 index 35c83e1..0000000 --- a/srv/TaskRequest.srv +++ /dev/null @@ -1,6 +0,0 @@ -string application -string version -string da_name -ScheduleParams init_params ---- -int8 status \ No newline at end of file diff --git a/tasker/TaskER.py b/tasker/TaskER.py new file mode 100644 index 0000000..b7f4d3a --- /dev/null +++ b/tasker/TaskER.py @@ -0,0 +1,343 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +import time + + +def sleep_rate(rate): + time.sleep(1 / rate) + + +class TaskER(smach_rcprg.StateMachine): + + def __init__(self, da_state_name): + super().__init__(da_state_name=da_state_name, outcomes=[ + 'Finished', 'shutdown'], input_keys=[], output_keys=[]) + # print "EXEC: ", self.ExeFSM(self) + self.my_fsm = smach_rcprg.State(outcomes=[ + 'FINISHED', 'PREEMPTED', 'FAILED', 'shutdown'], input_keys=['goal', 'susp_data']) + self.start_service = None + + with self: + smach_rcprg.StateMachine.add('Initialise', + TaskER.Initialise( + self, da_state_name), + transitions={ + 'ok': 'UpdateTask', 'terminate': 'Cleanup'}, + remapping={'susp_data': 'susp_data'}) + smach_rcprg.StateMachine.add('ExecFSM', + self.my_fsm, + transitions={'FINISHED': 'Cleanup', 'PREEMPTED': 'GetSuspension', 'FAILED': 'Cleanup', + 'shutdown': 'Cleanup'}, + remapping={'goal': 'goal', 'susp_data': 'susp_data'}) + smach_rcprg.StateMachine.add('GetSuspension', + TaskER.GetSuspension( + self, da_state_name), + transitions={'ok': 'ExeSuspension'}, + remapping={'susp_data': 'susp_data', 'fsm_es_out': 'fsm_es'}) + smach_rcprg.StateMachine.add('ExeSuspension', + TaskER.ExeSuspension( + self, da_state_name), + transitions={ + 'FINISHED': 'Wait', 'shutdown': 'Cleanup'}, + remapping={'susp_data': 'susp_data', 'fsm_es_in': 'fsm_es'}) + smach_rcprg.StateMachine.add('Wait', + TaskER.Wait(self, da_state_name), + transitions={ + 'start': 'UpdateTask', 'terminate': 'Cleanup'}, + remapping={'susp_data': 'susp_data'}) + smach_rcprg.StateMachine.add('UpdateTask', + TaskER.UpdateTask( + self, da_state_name), + transitions={ + 'ok': 'ExecFSM', 'shutdown': 'Cleanup'}, + remapping={'susp_data': 'susp_data'}) + smach_rcprg.StateMachine.add('Cleanup', + TaskER.Cleanup(self, da_state_name), + transitions={ + 'ok': 'Finished', 'shutdown': 'shutdown'}, + remapping={}) + self.debug = False + + def isDebug(self): + return self.debug + + def swap_state(self, label, state): + """Add a state to the opened state machine. + + @type label: string + @param label: The label of the state being added. + + @param state: An instance of a class implementing the L{State} interface. + + @param transitions: A dictionary mapping state outcomes to other state + labels or container outcomes. + @param remapping: A dictionary mapping local userdata keys to userdata + keys in the container. + """ + # Get currently opened container + print('Swapping state (%s, %s)' % (label, str(smach_rcprg.State))) + + # Check if the label already exists + if label not in self._states: + print( + 'Attempting to swap state with label "' + label + '" in state machine, but this label is not being used.') + + # Debug info + print("Swapping state '" + str(label) + "' to the state machine.") + + # Swap state and transitions to the dictionary + self._states[label] = state + return state + + def get_suspension_tf(self, susp_data): + pass + + def exe_suspension_tf(self, fsm_es_in): + pass + + def wait_tf(self): + pass + + def update_task_tf(self): + pass + + def initialise(self): + pass + + class ExeFSM(smach_rcprg.StateMachine): + def __init__(self, tasker_instance): + da_state_name = "ExeFSM" + print("RCPRG - EXE------------------------------------------------FSM") + pass + + def reset(self): + self.__init__(self) + + class GetSuspension(smach_rcprg.State): + def __init__(self, tasker_instance, da_state_name): + self.tasker_instance = tasker_instance + da_state_name = "GetSuspension" + smach_rcprg.State.__init__( + self, outcomes=['ok'], output_keys=['fsm_es_out']) + + def execute(self, userdata): + fsm_executable = self.tasker_instance.get_suspension_tf( + userdata.susp_data) + userdata.fsm_es_out = fsm_executable + # userdata.susp_data.clearData() + # srv.shutdown() + # rospy.sleep(5) + return 'ok' + + class BlockingState(smach_rcprg.State): + def __init__(self, tf_freq=10, outcomes=[], input_keys=[], output_keys=[], io_keys=[]): + self._userdata = None + self.tf_freq = tf_freq + input_keys.append('susp_data') + output_keys.append('susp_data') + self.rcprg_state = smach_rcprg.State.__init__(self, outcomes=outcomes, + input_keys=input_keys, + output_keys=output_keys, + io_keys=io_keys) + + def execute(self, userdata): + self._userdata = userdata + tf_result = None + while tf_result is None: + susp_flag = self.is_suspension_flag() + try: + tf_result = self.transition_function(userdata) + except Exception as e: + # work on python 2.x + print('Failed to upload to ftp: ' + str(e)) + tf_result = 'error' + print("TF returned: ", tf_result) + if susp_flag is not None: + break + sleep_rate(self.tf_freq) + return tf_result + + def transition_function(self, userdata): + pass + + def request_preempt(self): + fsm_cmd = None + if self._userdata is not None: + data = self._userdata.susp_data.req_data + for idx in range(0, len(data), 2): + if data[idx] == 'cmd': + fsm_cmd = data[idx + 1] + print("FSM CMD: ", fsm_cmd) + if fsm_cmd == 'susp': + pass + elif fsm_cmd == 'terminate': + return smach_rcprg.State.request_preempt(self) + else: + print("Blocking state ") + + def is_suspension_flag(self): + fsm_cmd = None + if not self._userdata == None: + data = self._userdata.susp_data.req_data + for idx in range(0, len(data), 2): + if data[idx] == 'cmd': + fsm_cmd = data[idx + 1] + if fsm_cmd == 'terminate': + return fsm_cmd + else: + return None + else: + return None + + class SuspendableState(smach_rcprg.State): + def __init__(self, tf_freq=10, outcomes=[], input_keys=[], output_keys=[], io_keys=[]): + self._userdata = None + self.tf_freq = tf_freq + input_keys.append('susp_data') + output_keys.append('susp_data') + smach_rcprg.State.__init__(self, outcomes=outcomes, + input_keys=input_keys, + output_keys=output_keys, + io_keys=io_keys) + + def execute(self, userdata): + self._userdata = userdata + # rate = rospy.Rate(self.tf_freq) + tf_result = None + while tf_result is None: + susp_flag = self.is_suspension_flag() + try: + tf_result = self.transition_function(userdata) + except Exception as e: + # work on python 2.x + print('Failed to upload to ftp: ' + str(e)) + print("TF returned: ", tf_result) + if susp_flag is not None: + break + sleep_rate(self.tf_freq) + # rate.sleep() + return tf_result + + def transition_function(self, userdata): + pass + + def is_suspension_flag(self): + fsm_cmd = None + if not self._userdata == None: + data = self._userdata.susp_data.req_data + for idx in range(0, len(data), 2): + if data[idx] == 'cmd': + fsm_cmd = data[idx + 1] + if fsm_cmd == 'susp' or fsm_cmd == 'terminate': + return fsm_cmd + else: + return None + else: + return None + + class ExeSuspension(smach_rcprg.State): + def __init__(self, tasker_instance, da_state_name): + self.tasker_instance = tasker_instance + da_state_name = "ExeSuspension" + + smach_rcprg.State.__init__( + self, outcomes=['FINISHED', 'shutdown'], input_keys=['fsm_es_in']) + + def execute(self, userdata): + transition_name = self.tasker_instance.exe_suspension_tf( + userdata.fsm_es_in) + # or stdout, stderr = p.communicate() + return transition_name + + class Wait(smach_rcprg.State): + def __init__(self, tasker_instance, da_state_name): + self.tasker_instance = tasker_instance + da_state_name = "Wait" + + smach_rcprg.State.__init__(self, outcomes=['start', 'terminate']) + + def execute(self, userdata): + if self.tasker_instance.isDebug() == True: + # rospy.loginfo('{}: Executing state: {}'.format(rospy.get_name(), self.__class__.__name__)) + print('Wait.execute') + # srv.shutdown() + fsm_cmd = None + + while not fsm_cmd == "resume": + data = userdata.susp_data.getData() + if self.tasker_instance.isDebug() == True: + print("WAIT.data: ", data) + for idx in range(0, len(data), 2): + if self.tasker_instance.isDebug() == True: + print(data[idx]) + if data[idx] == 'cmd': + fsm_cmd = data[idx + 1] + if self.preempt_requested() or fsm_cmd == 'terminate': + return 'terminate' + # active_ros_nodes = get_node_names() + # if not '/rico_task_harmonizer' in active_ros_nodes: + # return 'terminate' + self.tasker_instance.wait_tf() + sleep_rate(10) + return 'start' + + class UpdateTask(smach_rcprg.State): + def __init__(self, tasker_instance, da_state_name): + self.tasker_instance = tasker_instance + da_state_name = "UpdateTask" + smach_rcprg.State.__init__(self, outcomes=['ok', 'shutdown']) + + def execute(self, userdata): + self.tasker_instance.update_task_tf() + if self.tasker_instance.isDebug() == True: + rospy.loginfo('{}: Executing state: {}'.format( + rospy.get_name(), self.__class__.__name__)) + print('UpdateTask.execute') + # srv.shutdown() + return 'ok' + + def is_suspension_flag(self): + fsm_cmd = None + if not self._userdata == None: + data = self._userdata.susp_data.req_data + for idx in range(0, len(data), 2): + if data[idx] == 'cmd': + fsm_cmd = data[idx + 1] + if fsm_cmd == 'susp' or fsm_cmd == 'terminate': + return fsm_cmd + else: + return None + else: + return None + + class Initialise(smach_rcprg.State): + def __init__(self, tasker_instance, da_state_name): + self.tasker_instance = tasker_instance + da_state_name = "Initialise" + smach_rcprg.State.__init__(self, outcomes=['ok', 'terminate']) + + def execute(self, userdata): + self.tasker_instance.initialise_tf() + fsm_cmd = None + while not fsm_cmd == "start": + data = userdata.susp_data.getData() + for idx in range(0, len(data), 2): + if self.tasker_instance.isDebug() == True: + print(data[idx]) + if data[idx] == 'cmd': + fsm_cmd = data[idx + 1] + if self.preempt_requested() or fsm_cmd == 'terminate': + return 'terminate' + sleep_rate(10) + return 'ok' + + class Cleanup(smach_rcprg.State): + def __init__(self, tasker_instance, da_state_name): + self.tasker_instance = tasker_instance + da_state_name = "Cleanup" + smach_rcprg.State.__init__(self, outcomes=['ok', 'shutdown']) + + def execute(self, userdata): + self.tasker_instance.cleanup_tf() + return 'ok' diff --git a/tasker/TaskHarmoniserAgent.py b/tasker/TaskHarmoniserAgent.py new file mode 100644 index 0000000..4bd4f11 --- /dev/null +++ b/tasker/TaskHarmoniserAgent.py @@ -0,0 +1,704 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +from collections import OrderedDict +from tasker_msgs.msg import * +from tasker_msgs.srv import * +from tasker.tasker_comm import THACommunicator +import threading +import time +import subprocess +import rclpy +from rclpy.node import Node +from std_srvs.srv import Trigger +from rclpy.clock import Clock, ClockType + + +class TaskHarmoniserAgent(Node): + + def __init__(self): + super().__init__('task_harmoniser_agent') + + self.loop_rate = self.create_rate(5) + + self.switchIndicator = threading.Event() + self.switchIndicator.clear() + self.init_da = { + 'da_id': -1, 'da_name': None, 'da_type': None, + 'da_state': None, 'priority': float('-inf'), + 'scheduleParams': ScheduleParams() + } + self.lock = threading.Lock() + self.queue = {} + self.tasker_communicator = THACommunicator() + self.sdhl_pub = self.create_publisher( + ShdlDataStamped, "/TH/shdl_data", 10) + # self.cmd_pub = self.create_publisher(CMD, "/TH/cmd", 10) + self.OrderedQueue = {} + self.execField = {} + self.interruptField = {} + self.tha_is_running = True + + print("starting cmd updateQueueDataThread thread") + self.thread_status_update = threading.Thread( + target=self.updateQueueDataThread, args=(1,)) + self.thread_status_update.start() + print("started cmd updateQueueDataThread thread") + + # self.sub_status = self.create_subscription(Status, "TH/statuses", self.updateQueueDataThread, 10) + self.DA_processes = {} + self._switch_priority = None + self.debug = True + self.debug_file = False + self.clock = Clock(clock_type=ClockType.ROS_TIME) + + def __del__(self): + self.tasker_communicator.close() + del self.tasker_communicator + print("\n\n\n\n DEL THA \n\n\n\n") + self.tha_is_running = False + self.thread_status_update.join() + + def close(self): + self.tasker_communicator.close() + + def updateQueueDataThread(self, args): + while rclpy.ok() and self.tha_is_running: + msg = self.tasker_communicator.sub_status() + self.updateQueueData(msg) + + def updateQueueData(self, data): + if self.debug: + print( + f"\nUPDATE SP\nupdateQueueData: {data.da_name}, state: {data.da_state}\nSP: \n{data.schedule_params}") + if data.da_state == 'END': + if data.da_id in self.queue: + self.removeDA(self.queue[data.da_id]) + else: + raise Exception( + f'This DA: <{data.da_name}> is not found in THA queue') + self.updateScheduleParams(data.da_id, data.schedule_params) + priority = self.computePriority(data.schedule_params) + self.updatePriority(data.da_id, priority) + self.updateDAState(data.da_id, data.da_state) + if self.debug: + print("\nUPDATED SP\n") + + def initialiseDA(self, executable, da_type, da_id, args): + da_name = f"DA_{da_id}" + args.extend(['da_id', str(da_id), 'da_name', + da_name, 'da_type', da_type]) + print(f'args: {args}') + run_cmd = [executable] + run_cmd.extend(args) + print(f"cmd: {run_cmd}") + p = subprocess.Popen(run_cmd) + row = {'da_id': da_id, 'process': p} + self.DA_processes[da_id] = row + self.addDA(da_id, da_name, da_type) + + def addDA(self, added, da_name, da_type): + self.lock.acquire() + da = {'da_id': added, 'da_name': da_name, 'da_type': da_type, 'da_state': [ + "Init"], 'last_cmd_sent': None, 'priority': float('-inf'), 'ping_count': 0, 'scheduleParams': ScheduleParams()} + self.queue[added] = da + self.lock.release() + + def updatePriority(self, da_id, priority): + self.lock.acquire() + if da_id in self.queue: + self.queue[da_id]["priority"] = float(priority) + else: + print( + f"[TH] - tried to update Priority of DA_{da_id} but there is no such DA") + self.lock.release() + + def computePriority(self, schedule_params): + return -1 * schedule_params.cost + + def updateScheduleParams(self, da_id, scheduleParams): + self.lock.acquire() + if da_id in self.queue: + self.queue[da_id]["scheduleParams"] = scheduleParams + else: + print( + f"[TH] - tried to update Schedule Params of DA_{da_id} but there is no such DA") + self.lock.release() + + def updateDAState(self, da_id, da_state): + self.lock.acquire() + if da_id in self.queue: + self.queue[da_id]["da_state"] = da_state + else: + print( + f"[TH] - tried to update STATE of DA_{da_id} but there is no such DA") + self.lock.release() + + def updateDALastCMD(self, da_name, cmd): + self.lock.acquire() + if self.isExecuting(): + if self.execField["da_name"] == da_name: + self.execField["last_cmd_sent"] = cmd + self.lock.release() + return + if self.is_interrupting(): + if self.interruptField["da_name"] == da_name: + self.interruptField["last_cmd_sent"] = cmd + self.lock.release() + return + if da_name in self.queue: + self.queue[da_name]["last_cmd_sent"] = cmd + else: + print( + f"[TH] - tried to update last CMD of {da_name} but there is no such DA") + self.lock.release() + + def getDALastCMD(self, da_name): + self.lock.acquire() + if self.isExecuting(): + if self.execField["da_name"] == da_name: + self.lock.release() + return self.execField["last_cmd_sent"] + if self.is_interrupting(): + if self.interruptField["da_name"] == da_name: + self.lock.release() + return self.interruptField["last_cmd_sent"] + if da_name in self.queue: + self.lock.release() + return self.queue[da_name]["last_cmd_sent"] + else: + print( + f"[TH] - tried to get last CMD of {da_name} but there is no such DA") + self.lock.release() + + def makeInterrupting(self, da_id): + if self.is_interrupting(): + self.updateDA(self.interruptField) + self.interruptField = self.queue[da_id] + del self.queue[da_id] + + def makeExecuting(self): + if self.isExecuting(): + self.updateDA(self.execField) + self.execField = self.interruptField + self.interruptField = {} + + def is_interrupting(self): + return len(self.interruptField) != 0 + + def isExecuting(self): + return len(self.execField) != 0 + + def sendIndicator(self, switch_priority): + self._switch_priority = switch_priority + self.switchIndicator.set() + + def getInterruptingAndExecuting(self): + return [self.interruptField, self.execField] + + def getQueue(self): + queue = self.queue + return queue + + def getNextID(self): + self.lock.acquire() + i = 0 + while True: + for key_id in self.queue.items(): + if self.queue[key_id[0]]["da_id"] == i: + i = i + 1 + continue + if self.isExecuting(): + if self.execField["da_id"] == i: + i = i + 1 + continue + if self.is_interrupting(): + if self.interruptField["da_id"] == i: + i = i + 1 + continue + break + self.lock.release() + return i + + def updateQueue(self, new_queue): + self.OrderedQueue = new_queue + next_da = next(iter(new_queue.items()))[1] + if not self.isExecuting(): + self.makeInterrupting(next_da["da_id"]) + if not self.switchIndicator.isSet(): + self.sendIndicator("normal") + else: + if next_da["priority"] > self.execField["priority"]: + if not self.is_interrupting(): + if self.debug: + print( + f"NO INTERRUPTING DA, {next_da['da_id']} is interrupting now") + self.makeInterrupting(next_da["da_id"]) + if not self.switchIndicator.isSet(): + self.sendIndicator("normal") + + elif next_da["priority"] > self.interruptField["priority"]: + self.makeInterrupting(next_da["da_id"]) + if not self.switchIndicator.isSet(): + self.sendIndicator("normal") + + def updateIrrField(self, next_da, switch_priority, cost_file): + if self.debug_file: + cost_file.write("\n"+"IrrField:"+"\n") + cost_file.write(str(next_da)+"\n") + else: + if self.debug: + print("\n"+"IrrField:"+"\n") + print(f"\t Name: {next_da['da_name']}\n") + self.makeInterrupting(next_da["da_id"]) + if not self.switchIndicator.isSet(): + self.sendIndicator(switch_priority) + + def set_DA_signal(self, da_name, signal, data=[]): + if self.debug: + print("set_DA_signal:") + self.updateDALastCMD(da_name, signal) + cmd = CMD() # Assuming CMD is a custom ROS2 msg + cmd.recipient_name = da_name + cmd.cmd = signal + cmd.data = data # Ensure data is a list field in the ROS2 message definition + if self.debug: + print(cmd) + # self.tasker_communicator.pub_cmd(cmd) + + def suspendDA(self, set_exemplary_susp_task=False): + if set_exemplary_susp_task: + self.set_DA_signal(da_name=self.execField["da_name"], signal="susp", data=[ + "rosrun", "TaskER", "exemplary_susp_task", "priority", "0"]) + else: + self.set_DA_signal( + da_name=self.execField["da_name"], signal="susp", data=[]) + + while rclpy.ok(): + wait_flag = (self.isDAAlive_with_lock(self.execField) + and (self.execField["da_state"][0] != "Wait")) + if wait_flag: + print("Switch thread waits for exec_da to be WAIT or DEAD") + self.loop_rate.sleep() + else: + self.lock.acquire() + exe_da = self.execField + self.execField = {} + self.lock.release() + break + + self.lock.acquire() + print("EXEDA: ", exe_da) + self.updateDA(exe_da) + self.lock.release() + + def isDAAlive_no_lock(self, da): + if da["da_state"] == ['END'] and self.debug: + return False + p = self.DA_processes[da["da_id"]]['process'] + return p.poll() is None + + def isDAAlive_with_lock(self, da): + self.lock.acquire() + alive = self.isDAAlive_no_lock(da) + self.lock.release() + return alive + + def hasService(self, srv_name): + service_list = rosservice.get_service_list() + has_srv = srv_name in service_list + if self.debug: + print("\n\n\nHAVE SERVICE\n\n\n" if has_srv else "\n\n\nLOST SERVICE\n\n\n") + return has_srv + + def schedule(self): + self.lock.acquire() + # ... [rest of the method's content as provided] ... + + self.lock.release() + + def filterDA_GH(self, DA): + if DA[1]["da_state"] == 'END': + return False + return DA[1]["da_type"] == "guide_human_tasker" and DA[1]["priority"] != float('-inf') + + def filterDA_HF(self, DA): + if DA[1]["da_state"] == 'END': + return False + return DA[1]["da_type"] == "human_fell_tasker" and DA[1]["priority"] != float('-inf') + + def filterDA_BJ(self, DA): + if DA[1]["da_state"] == 'END': + return False + return DA[1]["da_type"] == "bring_jar_tasker" and DA[1]["priority"] != float('-inf') + + def filterDA_MT(self, DA): + if DA[1]["da_state"] == 'END': + return False + return DA[1]["da_type"] == "move_to_tasker" and DA[1]["priority"] != float('-inf') + + def filterDA_BG(self, DA): + if DA[1]["da_state"] == 'END': + return False + return DA[1]["da_type"] == "bring_goods_tasker" and DA[1]["priority"] != float('-inf') + + def schedule_new(self, cost_file): + # print("\nSCHEDULE\n") + self.lock.acquire() + if len(self.queue) > 0: + ordered_queue = OrderedDict(sorted(self.queue.items(), + key=lambda kv: kv[1]["priority"], reverse=True)) + if self.debug == True: + print("Queue before END check:\n", ordered_queue) + if self.isExecuting(): + if not self.isDAAlive_no_lock(self.execField): + if self.debug == True: + print("THA-> removes DA: ", self.execField["da_name"]) + self.removeDA_no_lock(self.execField) + if self.is_interrupting(): + if not self.isDAAlive_no_lock(self.interruptField): + if self.debug == True: + print("THA-> removes DA: ", self.interruptField["da_name"]) + self.removeDA_no_lock(self.interruptField) + for da in self.queue.items(): + if not self.isDAAlive_no_lock(da[1]): + if self.debug == True: + print("THA-> removes DA: ", da[1]["da_name"]) + self.removeDA_no_lock(da[1]) + + if len(self.queue) > 0: + ordered_queue = OrderedDict(sorted(self.queue.items(), + key=lambda kv: kv[1]["priority"], reverse=True)) + if self.debug == True: + print("OQ:\n", ordered_queue) + dac = next(iter(ordered_queue.items()))[1] + DAset_GH = {} + DAset_HF = {} + DAset_BJ = {} + DAset_MT = {} + DAset_BG = {} + cGH = {} + cHF = {} + cBJ = {} + cMT = {} + cBG = {} + + DAset_GH = filter(self.filterDA_GH, self.queue.items()) + DAset_HF = filter(self.filterDA_HF, self.queue.items()) + DAset_BJ = filter(self.filterDA_BJ, self.queue.items()) + DAset_MT = filter(self.filterDA_MT, self.queue.items()) + DAset_BG = filter(self.filterDA_BG, self.queue.items()) + q_GH = OrderedDict(sorted(DAset_GH, + key=lambda kv: kv[1]["priority"], reverse=True)) + q_HF = OrderedDict(sorted(DAset_HF, + key=lambda kv: kv[1]["priority"], reverse=True)) + q_BJ = OrderedDict(sorted(DAset_BJ, + key=lambda kv: kv[1]["priority"], reverse=True)) + q_MT = OrderedDict(sorted(DAset_MT, + key=lambda kv: kv[1]["priority"], reverse=True)) + q_BG = OrderedDict(sorted(DAset_BG, + key=lambda kv: kv[1]["priority"], reverse=True)) + if self.debug_file == True: + cost_file.write("\n"+"Q:\n") + cost_file.write(str(self.queue)+"\n") + + if len(DAset_HF) > 0: + if self.debug == True: + print("Have HF") + # print "q_GH" + # print q_GH + cHF = next(iter(q_HF.items()))[1] + if self.debug_file == True: + cost_file.write("\n"+"cHF:"+"\n") + cost_file.write(str(cHF)+"\n") + dac = cHF + if self.isExecuting(): + if not self.filterDA_HF([None, self.execField]): + switch_priority = "normal" + self.updateIrrField(dac, switch_priority, cost_file) + self.lock.release() + return + # print "cGH" + # print cGH + elif len(DAset_GH) > 0: + if self.debug == True: + print("Have GH") + cGH = next(iter(q_GH.items()))[1] + if self.debug_file == True: + cost_file.write("\n"+"cGH:"+"\n") + cost_file.write(str(cGH)+"\n") + dac = cGH + if self.isExecuting(): + if self.filterDA_HF([None, self.execField]): + self.lock.release() + return + elif not self.filterDA_GH([None, self.execField]): + switch_priority = "normal" + self.updateIrrField(dac, switch_priority, cost_file) + self.lock.release() + return + elif len(DAset_BJ) > 0: + if self.debug == True: + print("Have BJ") + cBJ = next(iter(q_BJ.items()))[1] + if self.debug_file == True: + cost_file.write("\n"+"cBJ:"+"\n") + cost_file.write(str(cBJ)+"\n") + dac = cBJ + if self.isExecuting(): + if self.filterDA_HF([None, self.execField]): + self.lock.release() + return + elif not self.filterDA_BJ([None, self.execField]): + switch_priority = "normal" + self.updateIrrField(dac, switch_priority, cost_file) + self.lock.release() + return + + elif len(DAset_MT) > 0: + if self.debug == True: + print("Have MT") + cMT = next(iter(q_MT.items()))[1] + if self.debug_file == True: + cost_file.write("\n"+"cMT:"+"\n") + cost_file.write(str(cMT)+"\n") + dac = cMT + if self.isExecuting(): + if self.filterDA_HF([None, self.execField]): + self.lock.release() + return + elif not self.filterDA_MT([None, self.execField]): + switch_priority = "normal" + self.updateIrrField(dac, switch_priority, cost_file) + self.lock.release() + return + + elif len(DAset_BG) > 0: + if self.debug == True: + print("Have BG") + cBG = next(iter(q_BG.items()))[1] + if self.debug_file == True: + cost_file.write("\n"+"cBG:"+"\n") + cost_file.write(str(cBG)+"\n") + dac = cBG + if self.isExecuting(): + if self.filterDA_HF([None, self.execField]): + self.lock.release() + return + elif not self.filterDA_BG([None, self.execField]): + switch_priority = "normal" + self.updateIrrField(dac, switch_priority, cost_file) + self.lock.release() + return + else: + if self.isExecuting(): + if not self.execField["da_type"] == dac["da_type"]: + print("Executing a task of a type that has higher priority") + self.lock.release() + return + # if not (len(DAset_GH) > 0 or len(DAset_HF) > 0): + # cost_file.write("\n"+"No candidate"+"\n") + # print "No candidate" + # self.lock.release() + # return + + if self.debug == True: + print("dac: ", dac) + if self.isExecuting() and not self.is_interrupting(): + print("COMPARISION of TASKS of same type") + if self.debug == True: + print("dac priority: ", dac["priority"]) + print("exe priority: ", self.execField["priority"]) + if dac["priority"] > self.execField["priority"]: + if self.debug == True: + print("REQUESTING reqParam services") + + resp = self.tasker_communicator.call_sus_cond( + highest_da["scheduleParams"].final_resource_state, highest_da["da_name"]) + cc_exec = resp.cost_to_resume + ccps_exec = resp.cost_per_sec + + resp = self.tasker_communicator.call_cost_cond( + self.execField["scheduleParams"].final_resource_state, self.execField["da_name"]) + + cc_dac = resp.cost_to_complete + ccps_dac = dac["scheduleParams"].cost_per_sec + # Calculation of costs to switch and not switch + c_switch = dac["scheduleParams"].cost + cc_exec + \ + ccps_exec * dac["scheduleParams"].completion_time + c_wait = self.execField["scheduleParams"].cost + cc_dac + \ + ccps_dac * \ + self.execField["scheduleParams"].completion_time + # send schedule data for visualisation + shdl_data = ShdlDataStamped() + shdl_data.header.stamp = self.clock.now().to_msg() + shdl_data.data.dac_cost = dac["scheduleParams"].cost + shdl_data.data.exec_cost = self.execField["scheduleParams"].cost + shdl_data.data.dac_cc = cc_dac + shdl_data.data.exec_cc = cc_exec + shdl_data.data.exec_ccps = ccps_exec + shdl_data.data.dac_ccps = ccps_dac + shdl_data.data.switch_cost = c_switch + shdl_data.data.wait_cost = c_wait + shdl_data.data.dac_id = dac["da_id"] + shdl_data.data.exec_id = self.execField["da_id"] + self.sdhl_pub.publish(shdl_data) + print("$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$") + print("SHDL_DATA: ") + print(shdl_data.data) + print("$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$") + if (c_switch < (c_wait - c_wait*0.1)): + switch_priority = "normal" + print("#################################################") + print("SWITCH") + print("#################################################") + self.updateIrrField(dac, switch_priority, cost_file) + else: + print("candidate priority was less then executing task") + switch_priority = "normal" + self.updateIrrField(dac, switch_priority, cost_file) + elif not self.isExecuting() and not self.is_interrupting(): + print("No EXEC, NO IRR dynamic agent") + if dac != None: + switch_priority = "normal" + self.updateIrrField(dac, switch_priority, cost_file) + else: + print("No candidate") + else: + print("Processing switch") + + if not self.isExecuting(): + print("No EXEC dynamic agent") + else: + if self.debug == True: + print("\nEXEC: ") + print("ID: ", self.execField["da_id"]) + print("Cost: ", self.execField["scheduleParams"].cost) + print("SP: \n", self.execField["scheduleParams"], "\n") + if not self.is_interrupting(): + if self.debug == True: + print("No INTERRUPTING dynamic agent") + else: + if self.debug == True: + print("\tINTERRUPT: ") + print("ID: ", self.interruptField["da_id"]) + print("Cost: ", self.interruptField["scheduleParams"].cost) + print("SP: \n", self.interruptField["scheduleParams"], "\n") + if self.debug == True: + print("\tQUEUE: ") + for key_id in self.queue.items(): + print("ID: ", self.queue[key_id[0]]["da_id"]) + print("Cost: ", self.queue[key_id[0]]["scheduleParams"].cost) + print("SP: \n", self.queue[key_id[0]]["scheduleParams"], "\n") + + self.lock.release() + # print("\nSCHEDULED\n") + + def switchDA(self): + self.switchIndicator.wait() + if self.is_interrupting(): + print("\nSWITCHING\n") + self.lock.acquire() + if self.isExecuting(): + commanding = self.execField + self.lock.release() + max_sleep_counter = 6 + sleep_counter = 0 + while self.getDALastCMD(commanding["da_name"]) in ['start', 'resume'] and commanding["da_state"][0] in ["Wait", "Init", "UpdateTask"]: + print("[Switch] -- while last cmd") + sleep_counter = sleep_counter + 1 + # while commanding DA stays in ["Wait", "init", "UpdateTask"] longer then max_sleep_counter after ['start','resume'] signal, + # terminate the commanding DA + if max_sleep_counter == sleep_counter: + self.set_DA_signal(da_name=commanding["da_name"], signal="terminate", data=["priority", self._switch_priority, + # THA może zarządać odpalenia konkretnej sekwencji wstrzymania np. ("rosrun", "TaskER", "exemplary_susp_task"), + ]) + while rclpy.ok(): + print("[Switch] -- while sleep counter") + wait_flag = self.isDAAlive_with_lock(commanding) + if wait_flag: + print( + "Waiting for DA: ", commanding["da_name"], " to terminate after long processing of ['start','resume'] command, and new interruption is comming") + self.loop_rate.sleep() + else: + # Exec DA is terminated, remove it from Exec field + self.lock.acquire() + self.execField = {} + self.lock.release() + break + break + self.loop_rate.sleep() + + if self.isDAAlive_with_lock(commanding): + print("SEND SUSPEND to commanding: ", commanding["da_id"]) + self.set_DA_signal(da_name=commanding["da_name"], signal="susp", data=["priority", self._switch_priority, + # THA może zarządać odpalenia konkretnej sekwencji wstrzymania np. ("rosrun", "TaskER", "exemplary_susp_task"), + ]) + + # 10hz + # wait until exec DA terminates or swithes to wait state + while rclpy.ok(): + wait_flag = (self.isDAAlive_with_lock(commanding) and ( + commanding["da_state"][0] != "Wait")) + if wait_flag: + print( + "Switch thread waits for exec_da to be WAIT or DEAD") + r.sleep() + else: + if not self.isDAAlive_with_lock(commanding): + # Exec DA is terminated, remove it from Exec field + self.lock.acquire() + self.execField = {} + self.lock.release() + break + else: + self.lock.release() + self.lock.acquire() + interrupting = self.interruptField + self.lock.release() + print("SEND StartTask to initialised: ", interrupting["da_name"]) + srv_name = "/"+interrupting["da_name"]+"/TaskER/startTask" + # print (srv_name) + # print("\nSWITCHING: waiting for QUEUED startTask\n") + # rospy.wait_for_service(srv_name, timeout=2) + print("\nSWITCHING: waiting for QUEUED to be in Init or Wait. It is in <", + interrupting["da_state"][0], "> state.") + while not interrupting["da_state"][0] in ["Wait", "Init"]: + if not rclpy.ok(): + return + print("\nSWITCHING: waiting for QUEUED to be in Init or Wait. It is in <", + interrupting["da_state"][0], "> state.") + r.sleep() + + if interrupting["da_state"][0] == "Wait": + self.set_DA_signal( + da_name=interrupting["da_name"], signal="resume", data=[]) + elif interrupting["da_state"][0] == "Init": + self.set_DA_signal( + da_name=interrupting["da_name"], signal="start", data=[]) + + print("\nSWITCHING: waiting for STARTED hold_now\n") + + # rospy.wait_for_service(srv_name, timeout=2) + print("\nSWITCHING: waiting for STARTED to be in UpdateTask or ExecFSM. It is in <", + interrupting["da_state"][0], "> state.") + while not interrupting["da_state"][0] in ["UpdateTask", "ExecFSM"]: + if not rclpy.ok(): + return + print("\nSWITCHING: waiting for STARTED to be in UpdateTask or ExecFSM. It is in <", + interrupting["da_state"][0], "> state.") + self.loop_rate.sleep() + + # rospy.wait_for_service('/'+interrupting["da_name"]+'/TaskER/hold_now', timeout=2) + self.lock.acquire() + # print("\n Making executing\n") + self.makeExecuting() + self.switchIndicator.clear() + # print("\n Made executing\n") + self.lock.release() + print("\nSWITCHED\n") + else: + print("[TH] -- Killing switch thread") + + def shutdown(self): + self.switchIndicator.set() diff --git a/tasker/__init__.py b/tasker/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/tasker/dynamic_agent.py b/tasker/dynamic_agent.py new file mode 100644 index 0000000..1dd93c3 --- /dev/null +++ b/tasker/dynamic_agent.py @@ -0,0 +1,435 @@ +#!/usr/bin/env python3 +# encoding: utf8 + +import sys +import signal +import time +import threading + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile +from rclpy.clock import Clock, ClockType +from std_msgs.msg import String +from tasker_msgs.msg import Status, CMD, ScheduleParams +from tasker_msgs.srv import CostConditions, SuspendConditions +from rcprg_smach.ros_node_utils import get_node_names +import tiago_msgs.msg +from std_srvs.srv import Trigger, TriggerRequest + +from tasker_comm import DACommunicator + +global USE_SMACH_INRTOSPECTION_SERVER +USE_SMACH_INRTOSPECTION_SERVER = False + + +def signal_handler(signal, frame): + print("\nprogram exiting gracefully") + sys.exit(0) + + +def sleep_rate(rate): + time.sleep(1/rate) + + +class SmachShutdownManager: + def __init__(self, main_sm, list_asw, list_sis): + self.__main_sm__ = main_sm + self.__list_asw__ = list_asw + self.__list_sis__ = list_sis + + def on_shutdown(self, signal, frame): + global USE_SMACH_INRTOSPECTION_SERVER + print("DA MANAGER") + # Stop all introspection servers + if USE_SMACH_INRTOSPECTION_SERVER: + print('SmachShutdownManager.on_shutdown: stopping all introspection servers') + for sis in reversed(self.__list_sis__): + sis.stop() + + # Stop all smach_ros action servers + print('SmachShutdownManager.on_shutdown: stopping all smach_ros action servers') + for asw in reversed(self.__list_asw__): + # Modify based on ROS2 behavior + asw._action_server.action_server.started = False + # asw.wrapped_container.request_preempt() + + +class SuspendRequest: + def __init__(self): + self.req_data = [] + + def setData(self, data): + self.req_data = data + + def getData(self): + return self.req_data + + def clearData(self): + self.req_data = [] + + +class DynAgent(Node): + def __init__(self, da_name, da_id, da_type, ptf_csp, da_state_name): + super().__init__(da_name) + + self.debug = False + self.name = da_name + self.finished = False + + self.ptf_csp = ptf_csp + self.da_id = int(da_id) + self.taskType = da_type + self.exec_fsm_state = "" + self.da_state = ["Init"] + self.terminateFlag = False + self.startFlag = False + self.da_suspend_request = SuspendRequest() + self.da_suspend_request.setData( + ["cmd", "", "param_name", "suspension requirements from the task harmoniser"]) + self.is_initialised = False + self.tasker_communicator = DACommunicator( + da_name=da_name, cond_cost_handler=self.startConditionHandler, sus_cost_handler=self.suspendConditionHandler) + + def startTask(self, data): + self.startFlag = True + self.da_suspend_request.setData(data) + + def suspTask(self, data="suspension requirements from the task harmoniser"): + print("\n", "HOLD: ", str(data)+"\n") + self.da_suspend_request.setData(data) + + def terminateDA(self): + if not self.terminateFlag == True: + my_status = Status() + my_status.da_id = self.da_id + my_status.da_name = self.name + my_status.type = self.taskType + if self.main_sm.is_running(): + if self.da_state[0] == "Init" or self.getActiveStates(self.main_sm)[0][0] in ["Wait", "UpdateTask"]: + self.cmd_handler( + CMD(recipient_name=self.name, cmd="terminate")) + self.da_state = ["END"] + my_status.da_state = self.da_state + self.tasker_communicator.pub_status(my_status) + # self.pub_status.publish(my_status) + self.terminateFlag = True + else: + print( + "DA triggered self termination flag. It is in CMD state, so the flag triggers preemption") + self.suspTask(["cmd", "terminate"]) + self.da_state = ["END"] + my_status.da_state = self.da_state + self.tasker_communicator.pub_status(my_status) + # self.pub_status.publish(my_status) + self.terminateFlag = True + else: + print( + "DA -> TaskER Termination not required, because TaskER FSM is not running. Sending state to THA") + self.cmd_handler( + CMD(recipient_name=self.name, cmd="terminate")) + self.da_state = ["END"] + my_status.da_state = self.da_state + self.tasker_communicator.pub_status(my_status) + # self.pub_status.publish(my_status) + self.terminateFlag = True + print("sendStatusThread close") + self.tasker_communicator.close() + print("sendStatusThread closed") + del self.tasker_communicator + print("sendStatusThread ended") + else: + print("termination flag was already handled") + + def process_ptf_csp(self, req): + (flag, result) = self.ptf_csp(req) + if flag == 'self-terminate': + print("DA_tasker: SELF terminate") + self.terminateDA() + if req[0] == 'suspendCondition': + return SuspendConditionsResponse() + elif req[0] == 'CostConditions': + return SuspendConditionsResponse() + elif req[0] == 'scheduleParams': + return ScheduleParams() + else: + return None + else: + return result + + def updateStatus(self): + if self.is_initialised == False: + self.da_state = ['Init'] + else: + self.da_state = self.getActiveStates(self.main_sm)[0] + + my_status = Status() + my_status.da_id = self.da_id + my_status.da_name = self.name + my_status.type = self.taskType + result = self.process_ptf_csp(["scheduleParams", None]) + if result != 'self-terminate': + my_status.schedule_params = result + else: + return + my_status.da_state = self.da_state + if self.debug: + print("UPDATING STATUS params of: "+str(self.name)+"\n of " + + str(self.taskType)+" type \n"+str(my_status.schedule_params)+"\n") + self.tasker_communicator.pub_status(my_status) + + def cmd_handler(self, data): + print("DA got CMD: ", data) + if data.cmd == "start": + # task will be started, so the interface to request start conditions by 'par' buffer is not required anymore + # self.cost_cond_srv.shutdown() + fsm_data = ["cmd", "start"] + fsm_data.extend(data.data) + self.startTask(fsm_data) + # self.hold_service = rospy.Service(self.node_namespace+"/hold_now", Trigger, lambda : None ) + elif data.cmd == "susp" and self.da_state[0] == "ExecFSM": + # self.hold_service.shutdown() + # self.start_service = rospy.Service(self.node_namespace+"/startTask", Trigger, lambda : None ) + # self.susp_cond_srv.shutdown() + # self.cost_cond_srv = rospy.Service(self.cost_cond_name, CostConditions, self.startConditionHandler) + fsm_data = ["cmd", "susp"] + fsm_data.extend(data.data) + self.suspTask(fsm_data) + elif data.cmd == "resume" and self.da_state[0] == "Wait": + # self.start_service.shutdown() + # self.cost_cond_srv.shutdown() + # self.hold_service = rospy.Service(self.node_namespace+"/hold_now", Trigger, lambda : None ) + # self.susp_cond_srv = rospy.Service(self.susp_cond_name, SuspendConditions, self.suspendConditionHandler) + fsm_data = ["cmd", "resume"] + fsm_data.extend(data.data) + self.da_suspend_request.setData(fsm_data) + elif data.cmd == "terminate": + fsm_data = ["cmd", "terminate"] + fsm_data.extend(data.data) + self.da_suspend_request.setData(fsm_data) + # self.main_sm.request_preempt() + self.main_sm.shutdownRequest() + + def suspendConditionHandler(self, req): + return self.process_ptf_csp(["suspendCondition", req]) + + def startConditionHandler(self, req): + return self.process_ptf_csp(["startCondition", req]) + + def run(self, main_sm, sis=None): + global USE_SMACH_INRTOSPECTION_SERVER + self.main_sm = main_sm + print("RUNNING DA") + + # sis_main = smach_ros.IntrospectionServer('behaviour_server', self.main_sm, '/SM_BEHAVIOUR_SERVER') + # sis_main.start() + + if USE_SMACH_INRTOSPECTION_SERVER: + sis = smach_ros.IntrospectionServer( + str("/"+self.name+"smach_view_server"), self.main_sm, self.name) + sis.start() + else: + sis = None + ssm = SmachShutdownManager(self.main_sm, [], [sis]) + + # ssm = SmachShutdownManager(self, self.main_sm, [], []) + # setup status interface for the task harmoniser + # self.pub_status = rospy.Publisher('TH/statuses', Status, queue_size=10) + # subsribe to commands from the task harmoniser + + print("Setting DA") + + self.node_namespace = self.name + "/TaskER" + # start_name = self.node_namespace + "/startTask" + # self.cmd_subscriber = rospy.Subscriber("/TH/cmd", CMD, self.cmd_handler) + # self.cost_cond_name = self.node_namespace + "/get_cost_on_conditions" + # self.cost_cond_srv = rospy.Service(self.cost_cond_name, CostConditions, self.startConditionHandler) + # Set shutdown hook + signal.signal(signal.SIGINT, ssm.on_shutdown) + # rospy.on_shutdown( ssm.on_shutdown ) + if not self.terminateFlag: + print("starting status send thread") + thread_status = threading.Thread( + target=self.sendStatusThread, args=(1,)) + thread_status.start() + print("started status send thread") + print("starting cmd recv thread") + thread_cmd = threading.Thread(target=self.recvCMDThread, args=(1,)) + thread_cmd.start() + print("started cmd recv thread") + print("starting cost_cond thread") + thread_cost_cond = threading.Thread( + target=self.costCondThread, args=(1,)) + thread_cost_cond.start() + print("started cmd cost_cond thread") + print("starting cmd susp_cond thread") + thread_susp_cond = threading.Thread( + target=self.suspCondThread, args=(1,)) + thread_susp_cond.start() + print("started cmd susp_cond thread") + # setup introspection server for smach viewer + # extend userdata with suspension request object + self.main_sm.userdata.susp_data = self.da_suspend_request + smach_thread = threading.Thread( + target=self.main_sm.execute, args=(smach.UserData(),)) + smach_thread.start() + # wait for start signal + # self.start_service = rospy.Service(self.node_namespace+"/startTask", Trigger, lambda : None ) + while True: + print("RUNNING") + self.updateStatus() + if self.startFlag: + break + if self.terminateFlag: + print("RUNNING: TERM FLAG") + # self.start_service.shutdown() + ssm.on_shutdown(None, None) + smach_thread.join() + thread_status.join() + thread_cmd.join() + thread_cost_cond.join() + thread_susp_cond.join() + return + sleep_rate(1) + # self.start_service.shutdown() + print('Smach thread is running') + self.is_initialised = True + # setup suspend condition handler + # self.susp_cond_name = self.node_namespace + "/get_suspend_conditions" + # self.susp_cond_srv = rospy.Service(self.susp_cond_name, SuspendConditions, self.suspendConditionHandler) + # Block until everything is preempted + smach_thread.join() + print("SMACH JOINED") + self.terminateDA() + print("SMACH terminateDA") + thread_status.join() + print("SMACH thread_status") + thread_cmd.join() + print("SMACH thread_cmd") + thread_cost_cond.join() + print("SMACH thread_cost_cond") + thread_susp_cond.join() + print("SMACH thread_susp_cond") + print("CONN Joined") + self.terminateFlag = True + print('Smach thread is finished') + else: + print("DA -> have terminateFlag before TaskER FSM start") + ssm.on_shutdown(None, None) + print("DYN AGENT ENDED") + + # def callbackTaskStateCmd(self, data): + # print 'DynAgent.callback' + # if data.data == 'abort': + # print 'DynAgent "' + self.name + '" received abort command' + # self.terminateFlag = True + # self.main_sm.shutdownRequest() + + def getActiveStates(self, sm): + result = [] + if hasattr(sm, 'get_active_states'): + for state_name in sm.get_active_states(): + st = sm.get_children()[state_name] + if hasattr(st, 'description') and not st.description is None: + description = st.description + else: + description = '' + result.append((state_name, description)) + for state_name in sm.get_active_states(): + st = sm.get_children()[state_name] + result = result + self.getActiveStates(st) + return result + + def sendStatusThread(self, args): + + while not self.terminateFlag: + if not self.tasker_communicator.get_tha_alive_flag(): + print('DynAgent "' + self.name + + '" detected the task_harmonizer is dead') + self.terminateDA() + print("sendStatusThread terminateDA") + fsm_data = ["cmd", "terminate"] + self.suspTask(fsm_data) + print("sendStatusThread suspTask") + # self.main_sm.request_preempt() + self.main_sm.shutdownRequest() + print("sendStatusThread shutdownRequest") + break + + try: + # Publish diagnostic information + diag = tiago_msgs.msg.DynAgentDiag() + diag.agent_name = self.name + if self.startFlag: + # rospy.sleep(2) + self.updateStatus() + + active_states = self.getActiveStates(self.main_sm) + for state_name, state_desc in active_states: + diag.current_states.append(state_name) + diag.descriptions.append(state_desc) + else: + diag.current_states.append("Init") + diag.descriptions.append("desc") + + # self.pub_diag.publish( diag ) + except Exception as e: + print('Detected exception in dynamic agent sendStatusThread') + print(e) + self.terminateDA() + # self.main_sm.request_preempt() + self.main_sm.shutdownRequest() + break + + time.sleep(0.2) + + def recvCMDThread(self, args): + + while not self.terminateFlag: + try: + self.cmd_handler(self.tasker_communicator.sub_cmd()) + + except Exception as e: + print('Detected exception in dynamic agent recvCMDThread') + print(e) + self.terminateDA() + # self.main_sm.request_preempt() + self.main_sm.shutdownRequest() + break + + time.sleep(0.2) + print("recvCMDThread ended") + + def costCondThread(self, args): + + while not self.terminateFlag: + try: + self.tasker_communicator.handle_cost_cond() + + except Exception as e: + print('Detected exception in dynamic agent costCondThread') + print(e) + self.terminateDA() + # self.main_sm.request_preempt() + self.main_sm.shutdownRequest() + break + + time.sleep(0.2) + print("costCondThread ended") + + def suspCondThread(self, args): + + while not self.terminateFlag: + try: + self.tasker_communicator.handle_sus_cond() + + except Exception as e: + print('Detected exception in dynamic agent suspCondThread') + print(e) + self.terminateDA() + # self.main_sm.request_preempt() + self.main_sm.shutdownRequest() + break + + time.sleep(0.2) + print("suspCondThread ended") diff --git a/bin/task_harmonizer b/tasker/task_harmonizer.py similarity index 55% rename from bin/task_harmonizer rename to tasker/task_harmonizer.py index ed3e1d2..2813228 100755 --- a/bin/task_harmonizer +++ b/tasker/task_harmonizer.py @@ -1,18 +1,21 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # encoding: utf8 import rospkg import sys import subprocess import threading +import time -import rosgraph -import rospy -import actionlib +import rclpy +from rclpy.node import Node +from ament_index_python.packages import get_package_share_directory from std_msgs.msg import String import tiago_msgs.msg +from tiago_msgs.msg import Command from rcprg_smach.ros_node_utils import get_node_names -from TaskER.TaskHarmoniserAgent import TaskHarmoniserAgent +from tasker.TaskHarmoniserAgent import TaskHarmoniserAgent + class TaskSpec: def __init__(self, intent_name, task_name, priority, params_names_list): @@ -21,8 +24,10 @@ def __init__(self, intent_name, task_name, priority, params_names_list): self.priority = priority self.params_names_list = params_names_list -class TaskHarmonizer: + +class TaskHarmonizer(Node): def __init__(self, task_harmoniser_agent, intent_topics, tasks_spec_list, cached_rapps_path): + super().__init__('task_harmonizer_node') self.tasks_spec_list = tasks_spec_list self.checkForDynamicAgentsRunning() self.tha = task_harmoniser_agent @@ -30,8 +35,11 @@ def __init__(self, task_harmoniser_agent, intent_topics, tasks_spec_list, cached self.sub_list = [] for topic_name in intent_topics: - self.sub_list.append( rospy.Subscriber(topic_name, tiago_msgs.msg.Command, self.callback) ) - self.pub_rico_filtered_cmd = rospy.Publisher( '/rico_filtered_cmd', tiago_msgs.msg.Command, queue_size=10 ) + self.sub_list.append(self.create_subscription( + Command, topic_name, self.callback, 10)) + # self.create_subscription(tiago_msgs.msg.Command, topic_name, self.callback, 10) + self.pub_rico_filtered_cmd = self.create_publisher( + Command, '/rico_filtered_cmd', 10) self.pending_task_request = None self.cached_rapps_path = cached_rapps_path @@ -42,12 +50,14 @@ def __init__(self, task_harmoniser_agent, intent_topics, tasks_spec_list, cached self.task_request_response = None - print 'ConversationMachine.__init__: waiting for rico_says ActionServer...' - #self.rico_says_client = actionlib.SimpleActionClient('rico_says', tiago_msgs.msg.SaySentenceAction) - #self.rico_says_client.wait_for_server() - print 'ConversationMachine.__init__: connected to rico_says ActionServer' + print('ConversationMachine.__init__: waiting for rico_says ActionServer...') + # self.rico_says_client = actionlib.SimpleActionClient('rico_says', tiago_msgs.msg.SaySentenceAction) + # self.rico_says_client.wait_for_server() + print('ConversationMachine.__init__: connected to rico_says ActionServer') self._FINISH = False self.debug_file = False + self.loop_rate = self.create_rate(5) + def hasTaskSpecByIntent(self, intent_name): for task_spec in self.tasks_spec_list: if task_spec.intent_name == intent_name: @@ -58,69 +68,74 @@ def getTaskSpecByIntent(self, intent_name): for task_spec in self.tasks_spec_list: if task_spec.intent_name == intent_name: return task_spec - raise Exception('Could not find task spec for intent "' + intent_name + '"') + raise Exception( + 'Could not find task spec for intent "' + intent_name + '"') def getTaskSpecByName(self, task_name): for task_spec in self.tasks_spec_list: if task_spec.task_name == task_name: return task_spec - raise Exception('Could not find task spec for task_name "' + intent_name + '"') + raise Exception( + 'Could not find task spec for task_name "' + intent_name + '"') def callback(self, data): param_dict = {} for param_name, param_value in zip(data.param_names, data.param_values): param_dict[param_name] = param_value - print 'param_name, param_value', param_name, param_value + print('param_name, param_value', param_name, param_value) task = None - print u'data.intent_name: "{}"'.format(data.intent_name) + print(u'data.intent_name: "{}"'.format(data.intent_name)) if data.intent_name == '': self.cant_hear_you = True elif self.hasTaskSpecByIntent(data.intent_name): task_spec = self.getTaskSpecByIntent(data.intent_name) if not self.pending_task_request is None: - print 'ERROR: pending task request "' + str(self.pending_task_request[0]) + '" was not consumed an ne intent for task "' + task_spec.task_name + '" arrived' + print('ERROR: pending task request "' + str( + self.pending_task_request[0]) + '" was not consumed an ne intent for task "' + task_spec.task_name + '" arrived') task_param_map = {} for param_name in task_spec.params_names_list: if not param_name in param_dict: - print 'ERROR: parameter "' + param_name + '" is missing in intent for task "' + task_spec.task_name + '"' + print('ERROR: parameter "' + param_name + + '" is missing in intent for task "' + task_spec.task_name + '"') task_param_map[param_name] = param_dict[param_name] self.pending_task_request = (task_spec, task_param_map) else: - print 'Passing forward intent ' + data.intent_name - self.pub_rico_filtered_cmd.publish( data ) + print('Passing forward intent ' + data.intent_name) + self.pub_rico_filtered_cmd.publish(data) def spin(self): try: - while not rospy.is_shutdown(): + while rclpy.ok(): queue = self.tha.getQueue() [interrupting, executing] = self.tha.getInterruptingAndExecuting() - if not self.pending_task_request is None: - if True: - self.runTask( self.pending_task_request[0], self.pending_task_request[1] ) + if self.pending_task_request is not None: + if True: # Replace with your condition + self.run_task( + self.pending_task_request[0], self.pending_task_request[1]) self.pending_task_request = None else: - print 'Cannot run task "' + self.pending_task_request[0].task_name + '"' - goal = tiago_msgs.msg.SaySentenceGoal() + self.get_logger().info('Cannot run task "' + + self.pending_task_request[0].task_name + '"') + goal = SaySentenceGoal() goal.sentence = 'niekorzystne warunki pogodowe jestem teraz zajęty' - print(goal) - #self.rico_says_client.wait_for_result() - elif len(queue) == 0 and self.tha.isExecuting() != True and self.tha.isInterrupting() != True: - print "len(queue): ", len(queue) - print "self.tha.isExecuting", self.tha.isExecuting() - print "self.tha.isInterrupting", self.tha.isInterrupting() - # There is nothing running, so run the default behaviour + self.say_sentence_publisher.publish(goal) + elif len(queue) == 0 and not self.tha.isExecuting() and not self.tha.is_interrupting(): + self.get_logger().info("len(queue): " + str(len(queue))) + self.get_logger().info("self.tha.isExecuting " + str(self.tha.isExecuting())) + self.get_logger().info("self.tha.is_interrupting " + str(self.tha.is_interrupting())) + # There is nothing running, so run the default behavior default_task_spec = self.getTaskSpecByIntent('default') self.pending_task_request = (default_task_spec, {}) if self.cant_hear_you: - goal = tiago_msgs.msg.SaySentenceGoal() + goal = SaySentenceGoal() goal.sentence = 'niekorzystne warunki pogodowe nie słyszę' - print(goal) + self.say_sentence_publisher.publish(goal) self.cant_hear_you = False - rospy.sleep(0.5) + self.loop_rate.sleep() except Exception as e: - print 'Detected exception in THA spin' - print e + self.get_logger().error('Detected exception in THA spin') + self.get_logger().error(str(e)) self.tha.shutdown() self._FINISH = True @@ -131,19 +146,21 @@ def abortCurrentTask(self): # Send a gentle request self.current_task_cmd.publish('abort') - time1 = rospy.get_rostime() - while not rospy.is_shutdown(): + time1 = self.get_clock().now() + while rclpy.ok(): if self.current_subprocess is None: break if not self.current_subprocess.poll() is None: - print 'Task "' + self.current_task[0].task_name + '" has finished' + print( + 'Task "' + self.current_task[0].task_name + '" has finished') break - time2 = rospy.get_rostime() - if (time2-time1).to_sec() > 5.0: + time2 = self.get_clock().now() + if (time2-time1).seconds_nanoseconds()[0] > 5: # 5 seconds self.current_subprocess.kill() - print 'Task "' + self.current_task[0].task_name + '" was killed' + print( + 'Task "' + self.current_task[0].task_name + '" was killed') break self.current_subprocess = None @@ -156,15 +173,16 @@ def canRunTask(self, task_spec): def runTask(self, task_spec, task_parameters): self.checkForDynamicAgentsRunning() - print 'Running task "' + task_spec.task_name + '", parameters: ' + str(task_parameters) + print('Running task "' + task_spec.task_name + + '", parameters: ' + str(task_parameters)) executable = self.cached_rapps_path + '/' + task_spec.task_name - args=[] + args = [] for param_name, param_value in task_parameters.iteritems(): - args.append( param_name ) - args.append( param_value ) + args.append(param_name) + args.append(param_value) self.shdl_performed = False - while not self.shdl_performed ==True: - rospy.sleep(0.1) + while not self.shdl_performed == True: + time.sleep(0.1) [interrupting, executing] = self.tha.getInterruptingAndExecuting() if task_spec.task_name == 'stop': if self.tha.isExecuting(): @@ -173,20 +191,21 @@ def runTask(self, task_spec, task_parameters): pass else: da_id = self.tha.getNextID() - self.tha.initialiseDA(executable=executable, da_type=task_spec.task_name, da_id=da_id, args=args) - #self.current_subprocess = subprocess.Popen( args ) + self.tha.initialiseDA( + executable=executable, da_type=task_spec.task_name, da_id=da_id, args=args) + # self.current_subprocess = subprocess.Popen( args ) if self.tha.isExecuting(): if da_id == executing["da_id"]: - print "The requested task is running" + print("The requested task is running") return - if self.tha.isInterrupting(): + if self.tha.is_interrupting(): if da_id == interrupting["da_id"]: - print "The requested task interrupts the running behaviour" + print("The requested task interrupts the running behaviour") else: - print "Another task awaits as interrupting agent" - print "The requested task awaits in the queue" + print("Another task awaits as interrupting agent") + print("The requested task awaits in the queue") else: - print "The requested task awaits in the queue" + print("The requested task awaits in the queue") # rospy.sleep(0.1) def checkForDynamicAgentsRunning(self): @@ -194,67 +213,81 @@ def checkForDynamicAgentsRunning(self): for task_spec in self.tasks_spec_list: for node_name in active_ros_nodes: if '/' + task_spec.task_name == node_name: - #raise Exception('There are some dynamic agents running out there...\n' + str(active_ros_nodes)) + # raise Exception('There are some dynamic agents running out there...\n' + str(active_ros_nodes)) pass + def scheduler(self): if self.debug_file: cost_file = open("./TH_cost", "a+") else: - cost_file =None + cost_file = None try: - while not rospy.is_shutdown(): + while rclpy.ok(): # print "\n SCHEDULING \n" self.tha.schedule_new(cost_file) - self.shdl_performed=True + self.shdl_performed = True # print "\n SCHEDULED \n" - rospy.sleep(2) + time.sleep(2) if self._FINISH: switch_priority = "hard" self.tha.sendIndicator(switch_priority) - if self.debug_file ==True: + if self.debug_file == True: cost_file.close() break except Exception as e: - print 'Detected exception in THA scheduler' - print e - if self.debug_file ==True: + print('Detected exception in THA scheduler') + print(e) + if self.debug_file == True: cost_file.close() self.tha.shutdown() self._FINISH = True def switcher(self): try: - while not rospy.is_shutdown(): - print "\n TH NODE - switcher \n" - rospy.sleep(0.1) + while rclpy.ok(): + print("\n TH NODE - switcher \n") + time.sleep(0.1) self.tha.switchDA() if self._FINISH: break except Exception as e: - print 'Detected exception in THA switcher' - print e + print('Detected exception in THA switcher') + print(e) self._FINISH = True + + def shdl_function(): pass + + def main(): - rospy.init_node('rico_task_harmonizer') - rospy.sleep(0.5) + rclpy.init(args=sys.argv) + node = rclpy.create_node('rico_task_harmonizer') - print 'Currently running nodes:' - print get_node_names() + node.get_logger().info('Created node') + time.sleep(0.5) + + print('Currently running nodes:') + print(get_node_names()) tha = TaskHarmoniserAgent() # TODO: read map of intents to tasks from file downloaded from cloud (RAPP Store) # intent_name -> (task_name, task_param_names) tasks_spec_list = [ - TaskSpec('projects/incare-dialog-agent/agent/intents/176ab2ca-6250-4227-985b-cc82d5497d9f', 'bring_goods_tasker',5, ['przedmiot']), + TaskSpec('projects/incare-dialog-agent/agent/intents/176ab2ca-6250-4227-985b-cc82d5497d9f', + 'bring_goods_tasker', 5, ['przedmiot']), TaskSpec('BG', 'bring_goods_tasker', 5, ['przedmiot']), - TaskSpec('GH', 'guide_human_tasker', 5, ['human_name', 'guide_destination']), + TaskSpec('GH', 'guide_human_tasker', 5, [ + 'human_name', 'guide_destination']), TaskSpec('HF', 'human_fell_tasker', 9, ['human_name']), - TaskSpec('BJ', 'bring_jar_tasker', 5, ['object_container','bring_destination','end_pose']), + TaskSpec('BJ', 'bring_jar_tasker', 5, [ + 'object_container', 'bring_destination', 'end_pose']), TaskSpec('MT', 'move_to_tasker', 5, ['miejsce']), - TaskSpec('projects/incare-dialog-agent/agent/intents/0165eceb-9621-4a7d-aecc-7a879951da18', 'move_to_tasker', 5, ['miejsce']), - TaskSpec('projects/incare-dialog-agent/agent/intents/7acd4325-4cdd-4e15-99be-ad545f4dddd5', 'stop', 10, []), - TaskSpec('projects/incare-dialog-agent/agent/intents/d9e96166-030b-442f-a513-d3fa2e044030', 'wander', 5, []), + TaskSpec('projects/incare-dialog-agent/agent/intents/0165eceb-9621-4a7d-aecc-7a879951da18', + 'move_to_tasker', 5, ['miejsce']), + TaskSpec( + 'projects/incare-dialog-agent/agent/intents/7acd4325-4cdd-4e15-99be-ad545f4dddd5', 'stop', 10, []), + TaskSpec( + 'projects/incare-dialog-agent/agent/intents/d9e96166-030b-442f-a513-d3fa2e044030', 'wander', 5, []), TaskSpec('intent_call', 'call', 1, ['miejsce']), TaskSpec('default', 'idle_tasker', float('-inf'), []), # TESTS: @@ -264,22 +297,28 @@ def main(): TaskSpec('test_wander', 'test_wander', 0, []), ] - rospack = rospkg.RosPack() - cached_rapps_path = rospack.get_path('rcprg_smach') + '/nodes' + cached_rapps_path = get_package_share_directory('rcprg_smach') + '/nodes' + th = TaskHarmonizer(tha, ['rico_cmd'], tasks_spec_list, cached_rapps_path) - scheduler = threading.Thread(target = th.scheduler) + scheduler = threading.Thread(target=th.scheduler) scheduler.start() - switcher = threading.Thread(target = th.switcher) + switcher = threading.Thread(target=th.switcher) switcher.start() th.spin() - print "SPIIIINEED!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" + print("Task Harmonizer Node Spin Completed") + switcher.join() - print "switcher!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" + print("Switcher Thread Completed") scheduler.join() - print "scheduler!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" + print("Scheduler Thread Completed") + tha.close() del tha - print "tha!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" + print("Task Harmoniser Agent Closed") + + rclpy.shutdown() + + if __name__ == '__main__': main() diff --git a/tasker/tasker_comm.py b/tasker/tasker_comm.py new file mode 100644 index 0000000..e521b85 --- /dev/null +++ b/tasker/tasker_comm.py @@ -0,0 +1,247 @@ +#!/usr/bin/env python3 +# encoding: utf8 +import signal +import sys +import threading +import rclpy +from io import BytesIO +from rclpy.node import Node +from rclpy.qos import QoSProfile +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup +from tasker_msgs.msg import Status, CMD, ScheduleParams +from tasker_msgs.srv import CostConditions, SuspendConditions +import zmq +import time + +global socket_ports + +# Socket ports +socket_ports = {"status": "5656", "cmd": "5657", + "cost_cond": "5658", "sus_cond": "5659", "tha_life_tick": "5660"} + + +def init_socket(context, port, socket_type): + + if socket_type == 'pub': + socket_status_pub = context.socket(zmq.PUB) + socket_status_pub.bind("tcp://127.0.0.1:%s" % port) + return socket_status_pub + + if socket_type == 'pub_connect': + socket_status_push = context.socket(zmq.PUB) + socket_status_push.connect("tcp://127.0.0.1:%s" % port) + return socket_status_push + + elif socket_type == 'sub': + socket_cmd_sub = context.socket(zmq.SUB) + socket_cmd_sub.connect("tcp://127.0.0.1:%s" % port) + socket_cmd_sub.subscribe(b'') + return socket_cmd_sub + + elif socket_type == 'sub_bind': + socket_cmd_sub = context.socket(zmq.SUB) + socket_cmd_sub.bind("tcp://127.0.0.1:%s" % port) + socket_cmd_sub.subscribe(b'') + return socket_cmd_sub + + elif socket_type == 'server': + socket_server = context.socket(zmq.REP) + socket_server.connect("tcp://127.0.0.1:%s" % port) + return socket_server + + elif socket_type == 'client': + socket_client = context.socket(zmq.REQ) + socket_client.bind("tcp://127.0.0.1:%s" % port) + return socket_client + + +def sub_socket_filtered(socket, da_name): + while True: + msg = socket.recv_pyobj() + if msg.recipient_name == da_name: + return msg + + +class DACommunicator: + def __init__(self, da_name, cond_cost_handler, sus_cost_handler): + global socket_ports + context = zmq.Context() + self.da_name = da_name + self.context = context + self.cond_cost_handler = cond_cost_handler + self.sus_cost_handler = sus_cost_handler + self.sockets = [] + self.socket_status_pub = init_socket( + self.context, socket_ports['status'], 'pub_connect') + self.socket_status_pub.setsockopt(zmq.LINGER, 0) + self.sockets.append(self.socket_status_pub) + self.socket_cmd_sub = init_socket( + self.context, socket_ports['cmd'], 'sub') + self.socket_status_pub.setsockopt(zmq.LINGER, 0) + self.sockets.append(self.socket_cmd_sub) + self.socket_life_tick_sub = init_socket( + self.context, socket_ports['tha_life_tick'], 'sub') + self.socket_life_tick_sub.setsockopt(zmq.CONFLATE, 1) + self.socket_life_tick_sub.setsockopt(zmq.RCVTIMEO, 3000) + self.socket_status_pub.setsockopt(zmq.LINGER, 0) + self.sockets.append(self.socket_life_tick_sub) + self.socket_cost_cond_srv = init_socket( + self.context, socket_ports['cost_cond'], 'server') + self.socket_cost_cond_srv.setsockopt(zmq.LINGER, 0) + self.sockets.append(self.socket_cost_cond_srv) + self.socket_sus_cond_srv = init_socket( + self.context, socket_ports['sus_cond'], 'server') + self.socket_sus_cond_srv.setsockopt(zmq.LINGER, 0) + self.sockets.append(self.socket_sus_cond_srv) + self.da_is_running = True + self.is_tha_alive = True + self.thread_tha_alive = threading.Thread( + target=self.tha_alive_thread, args=(1,)) + self.thread_tha_alive.start() + print("started cmd thread_tha_alive thread") + + def __del__(self): + # self.context.term() + print("\n\n\n\n DEL DACommunicator \n\n\n\n") + self.da_is_running = False + print("\n\n DACommunicator join thread \n\n") + # self.thread_tha_alive.join() + + def close(self): + try: + self.da_is_running = False + print("\n\n DACommunicator terminating context \n\n") + i = 0 + for socket in self.sockets: + socket.close() + print("closed: ", i) + i += 1 + self.context.term() + print("\n\n DACommunicator CLOSED \n\n") + + except Exception as e: + print('Detected exception in DACommunicator') + print(e) + + def pub_status(self, msg=None): + isinstance(msg, Status) + self.socket_status_pub.send_pyobj(msg) + + def sub_cmd(self): + return sub_socket_filtered(self.socket_cmd_sub, self.da_name) + + def sub_tha_alive(self): + return self.socket_life_tick_sub.recv(zmq.NOBLOCK) + + def handle_cost_cond(self): + return self.socket_cost_cond_srv.send_pyobj(self.cond_cost_handler(self.socket_cost_cond_srv.recv_pyobj())) + + def handle_sus_cond(self): + return self.socket_cost_cond_srv.send_pyobj(self.sus_cost_handler(self.socket_sus_cond_srv.recv_pyobj())) + + def get_tha_alive_flag(self): + return self.is_tha_alive + + def tha_alive_thread(self, args): + + timeout = 0 + while self.da_is_running: + try: + # print "thaAliveThread: ", self.is_tha_alive + self.socket_life_tick_sub.recv() + # self.sub_tha_alive() + self.is_tha_alive = True + time.sleep(1) + except zmq.ZMQError as e: + # print "thaAliveThread: EXCEPTION: ", e.errno + # print "thaAliveThread: EXCEPTION: ", zmq.EAGAIN + if e.errno == zmq.EAGAIN: + self.socket_life_tick_sub.close() + self.is_tha_alive = False + break + else: + raise + except Exception as e: + print('Detected exception in DACommunicator') + + +class THACommunicator: + def __init__(self): + global socket_ports + context = zmq.Context() + self.context = context + self.sockets = [] + self.socket_status_sub = init_socket( + self.context, socket_ports['status'], 'sub_bind') + self.sockets.append(self.socket_status_sub) + self.socket_cmd_pub = init_socket( + self.context, socket_ports['cmd'], 'pub') + self.socket_cmd_pub.setsockopt(zmq.LINGER, 1000) + self.sockets.append(self.socket_cmd_pub) + self.socket_life_tick_pub = init_socket( + self.context, socket_ports['tha_life_tick'], 'pub') + self.socket_life_tick_pub.setsockopt(zmq.LINGER, 1000) + self.sockets.append(self.socket_life_tick_pub) + self.socket_cost_cond_cli = init_socket( + self.context, socket_ports['cost_cond'], 'client') + self.socket_cost_cond_cli.setsockopt(zmq.LINGER, 1000) + self.sockets.append(self.socket_cost_cond_cli) + self.socket_sus_cond_cli = init_socket( + self.context, socket_ports['sus_cond'], 'client') + self.socket_sus_cond_cli.setsockopt(zmq.LINGER, 1000) + self.sockets.append(self.socket_sus_cond_cli) + self.tha_is_running = True + self.thread_tha_alive = threading.Thread( + target=self.tha_alive_thread, args=(1,)) + self.thread_tha_alive.start() + print("started cmd thread_tha_alive thread") + + def __del__(self): + self.context.term() + print("\n\n\n\n DEL THACommunicator \n\n\n\n") + self.tha_is_running = False + self.thread_tha_alive.join() + + def close(self): + try: + self.tha_is_running = False + print("\n\n THACommunicator terminating context \n\n") + i = 0 + for socket in self.sockets: + socket.close() + print("closed: ", i) + i += 1 + self.context.term() + print("\n\n THACommunicator CLOSED \n\n") + + except Exception as e: + print('Detected exception in THACommunicator') + print(e) + + def sub_status(self): + return self.socket_status_sub.recv_pyobj() + + def pub_cmd(self, msg=None): + isinstance(msg, CMD) + self.socket_cmd_pub.send_pyobj(msg) + + def pub_life_tick(self): + self.socket_life_tick_pub.send(b'') + + def call_cost_cond(self, msg=None): + isinstance(msg, CostConditionsRequest) + self.socket_cost_cond_cli.send_pyobj(msg) + return self.socket_cost_cond_cli.recv_pyobj() + + def call_sus_cond(self, msg=None): + isinstance(msg, SuspendConditionsRequest) + self.socket_sus_cond_cli.send_pyobj(msg) + return self.socket_sus_cond_cli.recv_pyobj() + + def tha_alive_thread(self, args): + while self.tha_is_running: + try: + self.pub_life_tick() + time.sleep(1) + except Exception as e: + print('Detected exception in THACommunicator') diff --git a/tasker/tha.py b/tasker/tha.py new file mode 100644 index 0000000..0370bec --- /dev/null +++ b/tasker/tha.py @@ -0,0 +1,94 @@ +#!/usr/bin/env python3 +import signal +import sys +import threading +import rclpy +from TaskHarmoniser import TaskHarmoniser +import time +from multitasker.srv import TaskRequest, TaskRequestResponse +from multitasker.msg import InitParams +from rclpy.parameter import Parameter +from rclpy.node import Node +from rclpy.qos import QoSProfile +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup + +_FINISH = False +scheduler = threading.Thread +switcher = threading.Thread +th = TaskHarmoniser() + + +def init_da(request, response): + global th + print("GOT REQUEST") + new_id = th.get_next_id() + print("ID: ", new_id) + da_name = "DA_" + str(new_id) + init_params = request.init_params + th.initialise_da(request.application, request.version, + new_id, da_name, init_params) + th.add_da(new_id, da_name, request.application) + th.update_schedule_params(new_id, init_params) + return TaskRequestResponse(-1) + + +def scheduler_thread(): + global _FINISH + global th + while True: + cost_file = open("./TH_cost", "a+") + print("\n SCHEDULING \n") + th.schedule_new(cost_file) + print("\n SCHEDULED \n") + time.sleep(2) + if _FINISH: + th.send_indicator() + cost_file.close() + break + + +def switcher_thread(): + global th + global _FINISH + while True: + print("\n SWITCHING \n") + th.switch_da() + if _FINISH: + break + + +def signal_handler(sig, frame): + print('You pressed Ctrl+C!') + global _FINISH + _FINISH = True + global scheduler + global switcher + scheduler.join() + switcher.join() + sys.exit(0) + + +class TaskHarmoniserNode(Node): + def __init__(self): + super().__init__('TH') + self.th = TaskHarmoniser() + qos = QoSProfile(depth=10) + self.srv = self.create_service( + TaskRequest, 'TH/new_task', init_da, qos_profile=qos) + self.srv.set_callback_group(MutuallyExclusiveCallbackGroup()) + self.get_logger().info('Ready') + + +if __name__ == "__main__": + rclpy.init(args=sys.argv) + print("ready") + signal.signal(signal.SIGINT, signal_handler) + + scheduler = threading.Thread(target=scheduler_thread) + scheduler.start() + switcher = threading.Thread(target=switcher_thread) + switcher.start() + + rclpy.spin(TaskHarmoniserNode()) + + print("END") diff --git a/test/test_copyright.py b/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/test/test_flake8.py b/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/test/test_pep257.py b/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings'