Ask Your Question
0

Help with some code. Send a navigation goal.

asked 2014-07-10 07:05:08 -0500

azeta gravatar image

updated 2014-07-11 04:24:17 -0500

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 ...
(more)
edit retag flag offensive close merge delete

Comments

1

Can you clarify what the actual problem is?

David Lu gravatar imageDavid Lu ( 2014-07-10 13:44:09 -0500 )edit

Hi @azeta, when you have comments or new information on a question, please use the comments section or edit the question instead of posting it as a new answer. Thank you for helping us keep the house tidy!! (I have converted your answer to a comment, so you don't need to do anything this time)

Martin Peris gravatar imageMartin Peris ( 2014-07-10 19:25:08 -0500 )edit

1 Answer

Sort by » oldest newest most voted
0

answered 2014-07-11 10:35:05 -0500

David Lu gravatar image

You can create an ActionServer in Python just like you did in C++. http://wiki.ros.org/actionlib_tutoria...

edit flag offensive delete link more

Comments

Hi David, I would want to modify the programme but I don´t know how. How I could introduce by keyboard the coordinates of the navigation goal? [CODE] rospy.loginfo("Creating navigation goal...") nav_goal = create_nav_goal(0.5, 0.0, 0.0) [/CODE] I want something similar to this program (http://wiki.ros.org/ROS/Tutorials/WritingServiceClient%28python%29) "rosrun beginner_tutorials add_two_ints_client.py 4 5 "

azeta gravatar imageazeta ( 2014-07-17 07:57:08 -0500 )edit

The preferred way to set navigation goals is by using actions instead of services. If you replace FibbonacciAction in the tutorial I linked to with MoveBaseAction, it will work.

David Lu gravatar imageDavid Lu ( 2014-07-19 11:59:28 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2014-07-10 07:05:08 -0500

Seen: 3,886 times

Last updated: Jul 11 '14