ROS pub/sub node gotogoal.py

asked 2019-06-16 18:03:57 -0500

abeckd gravatar image

Hi guys, so I'm kind of new trying to make a new node. I have the turtlebot3 burger and found a code called "gotogoal.py". I want to use this code but have to make some changes since it is designed for the turtle simulator packages, topics, msgs, etc. Can someone please explain to me what do I need to change in the code in order to make it work? CODE:

#!/usr/bin/env python
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
from math import pow, atan2, sqrt


class TurtleBot:

def __init__(self):
    # Creates a node with name 'turtlebot_controller' and make sure it is a
    # unique node (using anonymous=True).
    rospy.init_node('turtlebot_controller', anonymous=True)

    # Publisher which will publish to the topic '/turtle1/cmd_vel'.
    self.velocity_publisher = rospy.Publisher('/turtle1/cmd_vel',
                                              Twist, queue_size=10)

    # A subscriber to the topic '/turtle1/pose'. self.update_pose is called
    # when a message of type Pose is received.
    self.pose_subscriber = rospy.Subscriber('/turtle1/pose',
                                            Pose, self.update_pose)

    self.pose = Pose()
    self.rate = rospy.Rate(10)

def update_pose(self, data):
    """Callback function which is called when a new message of type Pose is
    received by the subscriber."""
    self.pose = data
    self.pose.x = round(self.pose.x, 4)
    self.pose.y = round(self.pose.y, 4)

def euclidean_distance(self, goal_pose):
    """Euclidean distance between current pose and the goal."""
    return sqrt(pow((goal_pose.x - self.pose.x), 2) +
                pow((goal_pose.y - self.pose.y), 2))

def linear_vel(self, goal_pose, constant=1.5):
    """See video: https://www.youtube.com/watch?v=Qh15Nol5htM."""
    return constant * self.euclidean_distance(goal_pose)

def steering_angle(self, goal_pose):
    """See video: https://www.youtube.com/watch?v=Qh15Nol5htM."""
    return atan2(goal_pose.y - self.pose.y, goal_pose.x - self.pose.x)

def angular_vel(self, goal_pose, constant=6):
    """See video: https://www.youtube.com/watch?v=Qh15Nol5htM."""
    return constant * (self.steering_angle(goal_pose) - self.pose.theta)

def move2goal(self):
    """Moves the turtle to the goal."""
    goal_pose = Pose()

    # Get the input from the user.
    goal_pose.x = input("Set your x goal: ")
    goal_pose.y = input("Set your y goal: ")

    # Please, insert a number slightly greater than 0 (e.g. 0.01).
    distance_tolerance = input("Set your tolerance: ")

    vel_msg = Twist()

    while self.euclidean_distance(goal_pose) >= distance_tolerance:

        # Porportional controller.
        # https://en.wikipedia.org/wiki/Proportional_control

        # Linear velocity in the x-axis.
        vel_msg.linear.x = self.linear_vel(goal_pose)
        vel_msg.linear.y = 0
        vel_msg.linear.z = 0

        # Angular velocity in the z-axis.
        vel_msg.angular.x = 0
        vel_msg.angular.y = 0
        vel_msg.angular.z = self.angular_vel(goal_pose)

        # Publishing our vel_msg
        self.velocity_publisher.publish(vel_msg)

        # Publish at the desired rate.
        self.rate.sleep()

    # Stopping our robot after the movement is over.
    vel_msg.linear.x = 0
    vel_msg.angular.z = 0
    self.velocity_publisher.publish(vel_msg)

    # If we press control + C, the node will stop.
    rospy.spin()

if __name__ == '__main__':
try:
    x = TurtleBot()
    x.move2goal()
except rospy.ROSInterruptException:
    pass

I tried to run it once and this error came up:

 $ ~/catkinTFM_ws/src/location_monitor/src$ rosrun location_monitor gotogoal.py 
Set your x goal: 0 ...
(more)
edit retag flag offensive close merge delete

Comments

I understand I need to change from turtlesim.msg import Pose to from geometry_msgs.msg import Pose seen from the following command:

$ /opt/ros/kinetic/share/geometry_msgs/msg$ cat Pose.msg 
# A representation of pose in free space, composed of position and orientation. 
Point position
Quaternion orientation
abeckd gravatar imageabeckd ( 2019-06-16 18:10:44 -0500 )edit

Can you please update your question with the output of

rosmsg show turtlesim/Pose
jayess gravatar imagejayess ( 2019-06-16 20:20:21 -0500 )edit

Since I am not using turtlesim package it doesn't work, instead I got this output

$ rosmsg show Pose
[geometry_msgs/Pose]:
geometry_msgs/Point position
  float64 x
  float64 y
  float64 z
geometry_msgs/Quaternion orientation
  float64 x
  float64 y
  float64 z
  float64 w
abeckd gravatar imageabeckd ( 2019-06-18 16:34:54 -0500 )edit