def move2goal(self):
goal_pose = Odometry()
goal_pose.x = input("Set your x goal:")
goal_pose.y = input("Set your y goal:")
distance_tolerance = input("Set your tolerance:")
vel_msg = Twist()
while sqrt(
pow((goal_pose.x - self.pose.x), 2) +
pow((goal_pose.y - self.pose.y), 2)) >= distance_tolerance:
#Porportional Controller
#linear velocity in the x-axis:
vel_msg.linear.x = 1.5 * sqrt(
pow((goal_pose.x - self.pose.x), 2) +
pow((goal_pose.y - self.pose.y), 2))
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 = 4 * (
atan2(goal_pose.y - self.pose.y, goal_pose.x - self.pose.x) -
self.pose.theta)
#Publishing our vel_msg
self.velocity_publisher.publish(vel_msg)
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)
rospy.spin()
def move2goal(self):
""""
This functions behaves as a Proportional controller
Set the linear velocity in X-axis and y-axis
Publish a message
Return:
Stop the robot after the movement is over
"""
goal_pose = Odometry()
goal_pose.x = input("Set x goal:")
goal_pose.y = input("Set y goal:")
distance_tolerance = input("Set your tolerance:")
vel_msg = Twist()
while sqrt(pow((goal_pose.x - self.pose.x), 2) + pow((goal_pose.y - self.pose.y), 2)) >= distance_tolerance: