Help with some code. Send a navigation goal.

edit

Hi everybody, I´m really beginner on ROS. We have a Reem h3 humanoid robot unit in the University and I am trying to send the robot to a POI by speech Recognition. What I what is to send the robot to an specific nav goal when the correct topic is published. Eg: 'I say point 1', so point1 is published and the robot is send to point 1. This is an example of code where the command is shown in the screen: Edit: Oh, sorry! The problem is that I dont know how to relate the topic with each poi. I need to make the code in cpp but im confused having code in pyton. Do you know any example about this? Thanks again

#include <ros/ros.h>
#include <pal_interaction_msgs/asrresult.h>


// muestra el contenido por pantalla

void asrResultsCallback (const pal_interaction_msgs:: asrresult  result )

{
        ROS_INFO_STREAM("You said:" << result.text);
}

ros :: NodeHandle n;
ros :: Subscriber subscriber = n.subscribe(topicName, 0, &asrResultsCallback );
//en bucle
ros :: spin();

And this is the code in Python on how to send a navigation goal

#! /usr/bin/env python
# -*- coding: utf-8 -*-
"""
Created on Jan 10 11:16:00 2014

@author: Sam Pfeiffer

Snippet of code on how to send a navigation goal and how to get the current robot position in map

Navigation actionserver: /move_base/goal
Type of message: move_base_msgs/MoveBaseActionGoal

Actual robot pose topic: /amcl_pose
Type of message: geometry_msgs/PoseWithCovarianceStamped

"""

import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from geometry_msgs.msg import PoseWithCovarianceStamped, Quaternion
from tf.transformations import quaternion_from_euler, euler_from_quaternion
from math import radians, degrees

def create_nav_goal(x, y, yaw):
    """Create a MoveBaseGoal with x, y position and yaw rotation (in degrees).
Returns a MoveBaseGoal"""
    mb_goal = MoveBaseGoal()
    mb_goal.target_pose.header.frame_id = '/map' # Note: the frame_id must be map
    mb_goal.target_pose.pose.position.x = x
    mb_goal.target_pose.pose.position.y = y
    mb_goal.target_pose.pose.position.z = 0.0 # z must be 0.0 (no height in the map)

    # Orientation of the robot is expressed in the yaw value of euler angles
    angle = radians(yaw) # angles are expressed in radians
    quat = quaternion_from_euler(0.0, 0.0, angle) # roll, pitch, yaw
    mb_goal.target_pose.pose.orientation = Quaternion(*quat.tolist())

    return mb_goal

def callback_pose(data):
    """Callback for the topic subscriber.
Prints the current received data on the topic."""
    x = data.pose.pose.position.x
    y = data.pose.pose.position.y
    roll, pitch, yaw = euler_from_quaternion([data.pose.pose.orientation.x,
                                             data.pose.pose.orientation.y,
                                             data.pose.pose.orientation.z,
                                             data.pose.pose.orientation.w])
    rospy.loginfo("Current robot pose: x=" + str(x) + "y=" + str(y) + " yaw=" + str(degrees(yaw)) + "º")

if __name__=='__main__':
    rospy.init_node("navigation_snippet")

    # Read the current pose topic
    rospy.Subscriber("/amcl_pose", PoseWithCovarianceStamped, callback_pose)

    # Connect to the navigation action server
    nav_as = actionlib.SimpleActionClient('/move_base', MoveBaseAction)
    rospy.loginfo("Connecting to /move_base AS...")
    nav_as.wait_for_server()
    rospy.loginfo("Connected.")

    rospy.loginfo("Creating navigation goal...")
    nav_goal = create_nav_goal(0.5, 0.0, 0.0)#4.72333594438, -0.377168390489, 45) # 3.925197124481201, -3.026911973953247, 0.6259599924087524 livingroom
    rospy.loginfo("Sending goal to x ...