Пример #1
0
    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()
Пример #2
0
    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: