Simulation_turtlebot3 - Autorace(melodic) - 제어

Simulation_turtlebot3 - Autorace(melodic) - 제어

코드 출처

https://github.com/ROBOTIS-GIT/turtlebot3_autorace_2020

실행

roslaunch turtlebot3_gazebo turtlebot3_autorace.launch

roslaunch turtlebot3_autorace_camera intrinsic_camera_calibration.launch

roslaunch turtlebot3_autorace_camera extrinsic_camera_calibration.launch

roslaunch turtlebot3_autorace_detect detect_lane.launch

roslaunch turtlebot3_autorace_driving turtlebot3_autorace_control_lane.launch

rosrun rqt_reconfigure rqt_reconfigure

---

주행 제어 파트

roslaunch turtlebot3_autorace_driving turtlebot3_autorace_control_lane.launch

turtlebot3_autorace_control_lane

<launch>
  <!-- launch turtlebot3_autorace_detect_lane.launch before launch this file -->

  <!-- lane control -->
  <node pkg="turtlebot3_autorace_driving" type="control_lane" name="control_lane" output="screen">
    <remap from="/control/lane" to="/detect/lane" />
    <remap from="/control/cmd_vel" to="/cmd_vel" />
  </node>
</launch>

control_lane

#!/usr/bin/env python
# -*- coding: utf-8 -*-

################################################################################
# Copyright 2018 ROBOTIS CO., LTD.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     <http://www.apache.org/licenses/LICENSE-2.0>
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
################################################################################

# Author: Leon Jung, Gilbert, Ashe Kim
 
import rospy
import numpy as np
from std_msgs.msg import Float64
from geometry_msgs.msg import Twist

class ControlLane():
    def __init__(self):
        self.sub_lane = rospy.Subscriber('/control/lane', Float64, self.cbFollowLane, queue_size = 1)
        self.sub_max_vel = rospy.Subscriber('/control/max_vel', Float64, self.cbGetMaxVel, queue_size = 1)
        self.pub_cmd_vel = rospy.Publisher('/control/cmd_vel', Twist, queue_size = 1)

        self.lastError = 0
        self.MAX_VEL = 0.1

        rospy.on_shutdown(self.fnShutDown)

    def cbGetMaxVel(self, max_vel_msg):
        self.MAX_VEL = max_vel_msg.data

    def cbFollowLane(self, desired_center):
        center = desired_center.data

        error = center - 500

        Kp = 0.0025
        Kd = 0.007

        angular_z = Kp * error + Kd * (error - self.lastError)
        self.lastError = error
        
        twist = Twist()
        # twist.linear.x = 0.05        
        twist.linear.x = min(self.MAX_VEL * ((1 - abs(error) / 500) ** 2.2), 0.05)
        twist.linear.y = 0
        twist.linear.z = 0
        twist.angular.x = 0
        twist.angular.y = 0
        twist.angular.z = -max(angular_z, -2.0) if angular_z < 0 else -min(angular_z, 2.0)
        self.pub_cmd_vel.publish(twist)

    def fnShutDown(self):
        rospy.loginfo("Shutting down. cmd_vel will be 0")

        twist = Twist()                
        twist.linear.x = 0
        twist.linear.y = 0
        twist.linear.z = 0
        twist.angular.x = 0
        twist.angular.y = 0
        twist.angular.z = 0
        self.pub_cmd_vel.publish(twist) 

    def main(self):
        rospy.spin()

if __name__ == '__main__':
    rospy.init_node('control_lane')
    node = ControlLane()
    node.main()

💡 angular_z = Kp * error + Kd * (error - self.lastError)

• PD 제어로 주행하고 있는것을 확인 가능