Move the turtlebot in infinity shape (two circles) using pose messages?
I am trying to move the turtlebot in infinity shape (two circles) using pose messages but the turtlebot doesn't move and pose messages don't change. What am I doing wrong?
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
import sys
x=0
y=0
theta=0
def poseCallback(pose_message):
global x,y,theta
x= pose_message.x
y= pose_message.y
theta= pose_message.theta
def move_circle():
rospy.init_node('node_turtle_revolve', anonymous=True)
# Create a publisher which can "talk" to Turtlesim and tell it to move
pub = rospy.Publisher('turtle1/cmd_vel', Twist, queue_size=1)
# Create a Twist message and add linear x and angular z values
move_cmd = Twist()
move_cmd.linear.x = 1
move_cmd.linear.y = 0
move_cmd.linear.z = 0
move_cmd.angular.x = 0
move_cmd.angular.y = 0
move_cmd.angular.z = 1
rate = rospy.Rate(10)
pose_subscriber = rospy.Subscriber("/turle1/pose", Pose, poseCallback)
startx=x
starty=y
bgn=0
while not rospy.is_shutdown():
pub.publish(move_cmd)
if(startx == x and starty == y):
bgn=bgn+1
if(bgn!=1):
move_cmd.angular.z=-1
if(bgn!=2):
break
rospy.spin()
if __name__ == '__main__':
try:
move_circle()
except rospy.ROSInterruptException:
pass
Asked by nm on 2021-10-23 04:07:46 UTC
Answers
Hi @nm there are few issues in your code. I will point a couple to get you started:
pose_subscriber = rospy.Subscriber("/turtle1/pose", Pose, poseCallback)
syntax error missing 't'
Replace rospy.spin()
with rate.sleep()
I suggest to create a separate node as best practice to subscribe to your publisher but if you have it in the same node, it needs to be inside the while loop otherwise you only call subscribe just once.
You will need to work on the logic when to change direction to do complete infinity loop.
import rospy
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
import sys
x=0
y=0
theta=0
def poseCallback(pose_message):
global x,y,theta
x= pose_message.x
y= pose_message.y
theta= pose_message.theta
def move_circle():
rospy.init_node('node_turtle_revolve', anonymous=True)
# Create a publisher which can "talk" to Turtlesim and tell it to move
pub = rospy.Publisher('turtle1/cmd_vel', Twist, queue_size=1)
# Create a Twist message and add linear x and angular z values
move_cmd = Twist()
move_cmd.linear.x = 1
move_cmd.linear.y = 0
move_cmd.linear.z = 0
move_cmd.angular.x = 0
move_cmd.angular.y = 0
move_cmd.angular.z = 1
rate = rospy.Rate(10)
while not rospy.is_shutdown():
pub.publish(move_cmd)
pose_subscriber = rospy.Subscriber("/turtle1/pose", Pose, poseCallback)
# Work on the logic how to change direction...
## startx=x
## starty=y
## bgn=0
##
## print(x,y,bgn)
##
## if(startx == x and starty == y):
## bgn=bgn+1
## if(bgn!=1):
## move_cmd.angular.z=-1
## if(bgn!=2):
## break
rate.sleep()
#rospy.spin()
if __name__ == '__main__':
try:
move_circle()
except rospy.ROSInterruptException:
pass
Asked by osilva on 2021-10-30 15:18:03 UTC
Comments
subscriber should be outside the loop.
Asked by skros on 2023-02-21 17:45:05 UTC
Comments
Please format your question using 101010 button to show the code. It’s hard to see if it’s an indentation problem for example
Asked by osilva on 2021-10-24 06:30:41 UTC