def speech(req):
global client
goal = TtsGoal()
goal.rawtext.text = str("please take the cube")
goal.rawtext.lang_id = "EN"
goal.wait_before_speaking = float(0)
client.send_goal(goal, feedback_cb="")
client.wait_for_result()
return ser_messageResponse(True)
def moveToGoal_room(xGoal, yGoal, euler_z):
global d
#define a client for to send goal requests to the move_base server through a SimpleActionClient
ac = actionlib.SimpleActionClient("move_base", MoveBaseAction)
#wait for the action server to come up
while (not ac.wait_for_server(rospy.Duration.from_sec(5.0))):
rospy.loginfo("Waiting for the move_base action server to come up")
'''while(not ac_gaz.wait_for_server(rospy.Duration.from_sec(5.0))):
rospy.loginfo("Waiting for the move_base_simple action server to come up")'''
goal = MoveBaseGoal()
#set up the frame parameters
goal.target_pose.header.frame_id = "/map"
goal.target_pose.header.stamp = rospy.Time.now()
# moving towards the goal*/
qaut = transformations.quaternion_from_euler(0, 0, euler_z)
goal.target_pose.pose.position = Point(xGoal, yGoal, 0)
goal.target_pose.pose.orientation.x = qaut[0]
goal.target_pose.pose.orientation.y = qaut[1]
goal.target_pose.pose.orientation.z = qaut[2]
goal.target_pose.pose.orientation.w = qaut[3]
rospy.loginfo("Sending goal location ...")
ac.send_goal(goal)
ac.wait_for_result(rospy.Duration(60))
stat = MoveBaseActionResult()
stat.status.status = 0
stat_pub.publish(stat)
print d, "d_before"
if (ac.get_state() == GoalStatus.SUCCEEDED and d == 1):
rospy.loginfo("You have reached the destination")
return ser_messageResponse(True)
else:
rospy.loginfo("The robot failed to reach the destination")
d = 1
print d, "d_aftr"
return ser_messageResponse(False)
def give_human(req):
p.publish("start give")
data = rospy.wait_for_message("/arm_controller/state",
JointTrajectoryControllerState, 5)
positions_arm = data.actual.positions
global client
goal = TtsGoal()
goal.rawtext.text = str("please take the cup")
goal.rawtext.lang_id = "EN"
goal.wait_before_speaking = float(0)
moveit_commander.roscpp_initialize(sys.argv)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group_name = "arm"
group = moveit_commander.MoveGroupCommander(group_name)
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
waypoints = []
wpose = group.get_current_pose().pose
scaley = -0.2 - wpose.position.y
# wpose.position.z -= scale * 0.1 # First move up (z)
wpose.position.y += scaley * 1.0 # and sideways (y)
waypoints.append(copy.deepcopy(wpose))
scalex = 0.7 - wpose.position.x
print(wpose.position.x)
wpose.position.x += scalex * 1.0 # Second move forward/backwards in (x)
waypoints.append(copy.deepcopy(wpose))
scalez = 1.0 - wpose.position.z
wpose.position.z += scalez * 1.0 # Second move forward/backwards in (x)
waypoints.append(copy.deepcopy(wpose))
# We want the Cartesian path to be interpolated at a resolution of 1 cm
# which is why we will specify 0.01 as the eef_step in Cartesian
# translation. We will disable the jump threshold by setting it to 0.0 disabling:
(plan, fraction) = group.compute_cartesian_path(
waypoints, # waypoints to follow
0.005, # eef_step
0.0) # jump_threshold
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan)
# Publish
display_trajectory_publisher.publish(display_trajectory)
group.execute(plan, wait=True)
# group.execute(plan, wait=True)
# group.execute(plan, wait=True)
client.send_goal(goal, feedback_cb="")
client.wait_for_result()
p.publish("finish give")
return ser_messageResponse(True)
