Skip to content

Commit d61b843

Browse files
authored
Update Goal.py
1 parent 9258038 commit d61b843

File tree

1 file changed

+26
-6
lines changed

1 file changed

+26
-6
lines changed

src/assignment/src/Goal.py

+26-6
Original file line numberDiff line numberDiff line change
@@ -6,19 +6,32 @@
66
from math import pow,atan2,sqrt
77

88
class turtlebot():
9-
10-
11-
def __init__(self):
12-
13-
9+
"""
10+
Documentation of class
11+
12+
More details
13+
"""
14+
15+
def __init__(self):
16+
"""
17+
Initializes the node
18+
Publish to a topic "/cmd_vel"
19+
Subscribe to a topic "/odom" and calls a "callback" function
20+
"""
21+
1422
rospy.init_node('turtlebot_controller', anonymous=True)
1523
self.velocity_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
1624
self.pose_subscriber = rospy.Subscriber('/odom', Odometry, self.callback)
1725
self.pose = Pose()
1826
self.rate = rospy.Rate(10)
1927

2028

21-
def callback(self, data):
29+
def callback(self, data):
30+
"""
31+
After being called by the subscriber
32+
Returns:
33+
Implement the recieved pose values
34+
"""
2235

2336
self.pose = data
2437
self.pose.x = round(self.position.x, 4)
@@ -29,6 +42,13 @@ def get_distance(self, goal_x, goal_y):
2942
return distance
3043

3144
def move2goal(self):
45+
""""
46+
This functions behaves as a Proportional controller
47+
Set the linear velocity in X-axis and y-axis
48+
Publish a message
49+
Return:
50+
Stop the robot after the movement is over
51+
"""
3252

3353
goal_pose = Odometry()
3454
goal_pose.x = input("Set x goal:")

0 commit comments

Comments
 (0)