Exemplo n.º 1
0
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)
Exemplo n.º 2
0
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)
Exemplo n.º 3
0
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)