def DM_client(json_parameters):
client = actionlib.SimpleActionClient('srs_decision_making_actions', xmsg.ExecutionAction)
client.wait_for_server()
# Creates a goal to send to the action server.
_goal=xmsg.ExecutionGoal()
#_goal.action="move"
#_goal.parameter=target_pos
#_goal.priority=0
_goal.json_parameters = json_parameters
# Sends the goal to the action server.
client.send_goal(_goal)
# Waits for the server to finish performing the action.
client.wait_for_result()
# Prints out the result of executing the action
return client.get_result()
def DM_client(target_pos):
# Creates the SimpleActionClient, passing the type of the action
# constructor.
client = actionlib.SimpleActionClient('srs_decision_making_actions', xmsg.ExecutionAction)
# Waits until the action server has started up and started
# listening for goals.
client.wait_for_server()
# Creates a goal to send to the action server.
_goal=xmsg.ExecutionGoal()
_goal.action="move"
_goal.parameter=target_pos
_goal.priority=0
# Sends the goal to the action server.
client.send_goal(_goal)
# Waits for the server to finish performing the action.
client.wait_for_result()
# Prints out the result of executing the action
return client.get_result()
move_milk = rospy.ServiceProxy("/gazebo/set_model_state", SetModelState)
move_milk(modelstate)
except rospy.ServiceException, e:
error_message = "%s"%e
rospy.logerr("have you put the milk into the simulation?")
#setmodelstate = gazebo
#setmodelstate.request.model_state = modelstate
handle_tray = sss.move("tray", "down", False)
handle_tray.wait()
_goal=xmsg.ExecutionGoal()
_goal.action="move"
_goal.parameter="[0.5 -1.6 1.57]"
_goal.priority=0
client.send_goal(_goal)
client.wait_for_result()
sss.say(["back to charging station, I am ready for new tasks"],False)
return client.get_result()
if __name__ == '__main__':
try:
def DM_client():
# Creates the SimpleActionClient, passing the type of the action
# constructor.
client = actionlib.SimpleActionClient('srs_decision_making_actions',
xmsg.ExecutionAction)
# Waits until the action server has started up and started
# listening for goals.
client.wait_for_server()
# creates action sequence for the action server
# Creates a goal to send to the action server.
_goal = xmsg.ExecutionGoal()
_goal.action = "move"
_goal.parameter = "[0.5 -1.6 1.57]"
_goal.priority = 0
client.send_goal(_goal)
client.wait_for_result()
print("ready to start")
sss.wait_for_input()
_goal.action = "get" # msg imports
_goal.parameter = "milk"
_goal.priority = 0
client.send_goal(_goal)
client.wait_for_result()
print("milk ready")
sss.wait_for_input()
_goal.action = "move"
_goal.parameter = "[1.47 -0.7 0.75]"
_goal.priority = 0
client.send_goal(_goal)
client.wait_for_result()
print("milk delivered")
sss.say(["Here is your milk, Please help yourself."], False)
sss.wait_for_input()
start_pose = Pose()
start_pose.position.x = -3.0
start_pose.position.y = -0.2
start_pose.position.z = 1.02
start_pose.orientation.x = 0.0
start_pose.orientation.y = 0.0
start_pose.orientation.z = 0.0
start_pose.orientation.w = 0.0
start_twist = Twist()
start_twist.linear.x = 0.0
start_twist.linear.y = 0.0
start_twist.linear.z = 0.0
start_twist.angular.x = 0.0
start_twist.angular.y = 0.0
start_twist.angular.z = 0.0
modelstate = gazebo.ModelState
modelstate.model_name = "milk_box"
modelstate.reference_frame = "world"
modelstate.pose = start_pose
modelstate.twist = start_twist
move_milk = rospy.ServiceProxy("/gazebo/set_model_state", SetModelState)
#setmodelstate = gazebo
#setmodelstate.request.model_state = modelstate
move_milk(modelstate)
print("confirm milk has been taken")
sss.wait_for_input()
client.wait_for_result()
handle_tray = sss.move("tray", "down", False)
handle_tray.wait()
_goal.action = "move"
_goal.parameter = "[1 -1.6 1.57]"
_goal.priority = 0
client.send_goal(_goal)
#client.wait_for_result()
print("back to charge")
return client.get_result()
