Program getting stuck at line - "sac.wait_for_server()". Unable to send goals to turtlebot robo.
Hello All,
I have connected to turtlebot using a separate machine. Now using that separate machine, I am trying to pass goals to robo but while running code, program waits at line "sac.waitforserver()" so goals are not able to publish to robo. But while I am using the machine and inside gazebo simulated environment(not connecting to external robo) then the code is working fine and all the goals are send, also Robo inside gazebo moving. But I am unable to figure out why the real robo is not working. Please help me in fixing the issue. also if any one of you have working python code to send goals to move turtlebot.. pls share.
Thanks for any help.
I am attaching the code here:::
#!/usr/bin/env python
import roslib; roslib.load_manifest('beginner_tutorials')
import rospy
import actionlib
from geometry_msgs.msg import *
#move_base_msgs
from move_base_msgs.msg import *
def simple_move():
try:
rospy.loginfo("Start")
rospy.init_node('simple_move')
#Simple Action Client
sac = actionlib.SimpleActionClient('move_base', MoveBaseAction )
#create goal
goal = MoveBaseGoal()
x = ['1.500' , '0.000']
location = Pose(Point(float(x[0]),float(x[1]), 0.000), Quaternion(0.000, 0.000, 0.000, 0.777))
goal.target_pose.pose = location
#use self?
#set goal
#goal.target_pose.pose.position.x = 1.0
#goal.target_pose.pose.orientation.w = 1.0
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
rospy.loginfo("Setted Pose")
rospy.loginfo("wait for server")
#start listner
sac.wait_for_server()
rospy.loginfo("send goal")
#send goal
sac.send_goal(goal)
rospy.loginfo("wait for result")
#finish
sac.wait_for_result()
rospy.loginfo("printing result"+str(sac.get_result()))
#print result
print sac.get_result()
except Exception, inst:
print "Error"+str(inst)
if __name__ == '__main__':
try:
rospy.loginfo("1")
simple_move()
except rospy.ROSInterruptException:
print "Keyboard Interrupt"
Asked by Brijendra Singh on 2013-09-30 21:15:28 UTC
Comments
Bringup, mapping and move_base all are running on the turtlebot, right?
Asked by McMurdo on 2014-07-26 04:35:48 UTC
Does the server run in a namespace?
Asked by BennyRe on 2014-07-28 04:03:42 UTC
Can you see the server topics being published? type $ rostopic list -v and check that it appears something like move_base/goal, etc.. Since you are working through separate machine.. Did you do the export ROS_MASTER_URI?
Asked by maik_mico on 2016-01-11 14:22:34 UTC