ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

How do I send a signal when my robot is within a given range of a goal?

asked 2021-02-01 08:37:11 -0500

liambroek gravatar image

I want to send my robot multiple goals (waypoints really) to pass through. Right now it stops at each goal that is sent. I thought of sending a new goal once it comes within range of the previous goal (assuming the sending of a new goal will mean it skips the old goal, as with sending 2D_nav_goal in Rviz).

This is my current code for sending two goals.

import time import rospy import actionlib from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal from std_msgs.msg import Int8

def movebase_client(): rate = rospy.Rate(1)

client = actionlib.SimpleActionClient('move_base',MoveBaseAction)
client.wait_for_server()

goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "map"
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = 0.746
goal.target_pose.pose.position.y = 1.44
goal.target_pose.pose.orientation.x = 0
goal.target_pose.pose.orientation.y = 0
goal.target_pose.pose.orientation.z = 1

goal.target_pose.pose.orientation.w = -0.9

client.send_goal(goal)
wait = client.wait_for_result()
if not wait:
    rospy.logerr("Action server not available!")
    rospy.signal_shutdown("Action server not available!")

#goal2
goal.target_pose.header.frame_id = "map"
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = 0.586
goal.target_pose.pose.position.y = 0.851
goal.target_pose.pose.orientation.x = 0
goal.target_pose.pose.orientation.y = 0
goal.target_pose.pose.orientation.z = 1

goal.target_pose.pose.orientation.w = -0.9

client.send_goal(goal)
wait = client.wait_for_result()
if not wait:
    rospy.logerr("Action server not available!")
    rospy.signal_shutdown("Action server not available!")

else:
    return client.get_result()

if __name__ == '__main__': try: rospy.init_node('movebase_client_py') result = movebase_client() if result: rospy.loginfo("Goal execution done!")

except rospy.ROSInterruptException:
    rospy.loginfo("Navigation test finished.")

I imagine that I would monitor the amcl_pose topic and then use the location of the robot from that, but I dont know how to access the x, y, from the topic.

I'm using ROS melodic. Any help would be greatly appreciated. Thanks!

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2021-02-02 11:10:49 -0500

You can get your robot position 2 different way.

  1. Your robot transform to map (this is using generally for navigation - unlimited frequency)
  2. amcl_pose (this is corrected with scan topic - low frequency)

If you need high frequency position data you need transform listener but specially if you need to you amcl_pose you can use at the bellow code

#! /usr/bin/env python 
import rospy
from geometry_msgs.msg import PoseWithCovarianceStamped,PoseStamped
def PoseCallBack(msg):
    data=""
    #Subscribed coordinate information
    x = msg.pose.pose.position.x
    y = msg.pose.pose.position.y
    #Subscribed quaternion information, used to indicate the direction
    orien_z = msg.pose.pose.orientation.z
    orien_w = msg.pose.pose.orientation.w

    data = str(x) + "," + str(y)+ "," + str(orien_z)+ "," + str(orien_w)
    rospy.loginfo(data)

def PoseSub():
    rospy.init_node('pose_sub',anonymous=False)
    #Monitor topics and handle them in the callback function
    rospy.Subscriber('/amcl_pose',PoseWithCovarianceStamped,PoseCallBack)
    rospy.spin()
if __name__=='__main__':
    try:
        PoseSub()
    except:
        rospy.loginfo("Error, exiting...")
edit flag offensive delete link more

Question Tools

2 followers

Stats

Asked: 2021-02-01 07:54:24 -0500

Seen: 243 times

Last updated: Feb 02 '21