GNC Controller - riplaboratory/Kanaloa GitHub Wiki

Controller for GNC System

Table of Contents

  • Context
  • Implementation
  • Testing
  • Conclusion
  • Appendix

Context

This project will focus on the Control aspect of GNC. The purpose of this project is to develop a controller for the GNC system that will be implemented on team Kanaloa's Wave Adaptive Modular Vessel or WAM-V. This controller is important because it will control the movement of the WAM-V and by doing so assist the system in moving in a stable manner. Currently the WAM-V does not have a controller like this implemented. The WAM-V having the ability to control its movement autonomously would greatly impact its current performance and allow it to participate and complete tasks in the RobotX competition. This controller shall control the WAM-V's twist and ensure the WAM-V achieves the twist given by the Guidance system. This controller shall prioritize stability and this controller shall be adaptable to various thruster configurations and team Kanaloa's other vessels.

Implementation

For this controller, it will be implemented in Robot Operating System (ROS) and coded in Python. This controller will utilize the Robot Localization ROS package and the Simple-pid python package from the Python Package Index. The Robot Localization files that need to be edited once downloading the package can be found in the Appendix section. The following command will install robot localization package to your system.

git clone https://github.com/cra-ros-pkg/robot_localization.git

Testing

For testing this file, open at least 3 command prompts. To simply run this file, be sure to be in the vrx_ws directory and

source/devel/setup.bash

Do this for all 3 command prompts, in 1 of the command prompts run from the vrx_ws directory

roslaunch vrx_gazebo sandisland.launch urdf:=$HOME/my_wamv/differential.urdf

This will open the gazebo environment with the WAMV configured to the differential.urdf specifications, then in the 2nd command prompt from the vrx_ws directory

cd src
cd robot_localization
cd launch
roslaunch vrx_localization.launch

This will run the robot localization files that you have set up and will give you access to the map frame to base link transform. This transform is important to have working because without it, the speed the controller is monitoring would be incorrect. Then in the last of the command prompts from the vrx_ws directory

cd src
cd thrust_control
cd scripts

Where Thrust Control is a name of a directory you make, once in the scripts directory run this command to run the file, replace "newthrust.py" with the name of your file.

rosrun thrust_control newthrust.py

The urdf file for the WAM-V, along with the vrx_localization.launch and the vrx_localization.yaml (which is found in the params folder of the robot localization directory) can be found here:GNC Required Files! These files are important to have configured because the urdf file is the setup of the WAM-V being used in the virtual environment, and the vrx_localization files basically set up the transform you will need to convert the filtered map odometry to body frame.

Conclusion

The controller is properly implemented into the WAM-V’s system architecture (ROS and Python), and the controller also effectively controls twist. Currently testing has only been done with the WAM-V moving forward which corresponds to the linear y component of the twist message. The current thruster configuration does not allow the WAM-V to move side to side freely (linear x component of the twist message), the readings from the linear x component that are being generated are due to wind and current pushing the WAM-V in that direction.

Appendix

Control System Code

#!/usr/bin/env python 
#^ is what makes sure this script is executed as a python script
#THIS IS THE CONTROL SYSTEM CODE
import sys
import time
import tf
import rospy #import rospy for writing a ROS node
from std_msgs.msg import Float32
from geometry_msgs.msg import Vector3Stamped
#ros topic info for the wamv/thrusters/right_thrust_cmd is type std_msgs/Float32 same for the left_thrust_cmd
from nav_msgs.msg import Odometry #ros topic info for the odometry
from simple_pid import PID #simple pid package 

class Controller:
    def __init__(self):
        self.pid = PID(1, 0.25, .1)
        self.pid.output_limits = (-1, 1)
        self.pid.sample_time = .1
        self.base_twist = Vector3Stamped()
        self.map_twist = Vector3Stamped()
        #node is publishing to the thrust cmd topic using msg type Float32 
        #queue size limits the amount of queued messages in case the subscriber is not receiving them fast enough
        self.left_thruster_pub = rospy.Publisher('/wamv/thrusters/left_thrust_cmd', Float32, queue_size=10) 
        self.right_thruster_pub = rospy.Publisher('/wamv/thrusters/right_thrust_cmd', Float32, queue_size=10) 
        self.left_thruster = Float32
        self.right_thruster = Float32
        self.target_velocity = Vector3Stamped() 
        self.target_velocity.vector.x = 0
        self.target_velocity.vector.y = 1
        self.target_velocity.vector.z = 0
        self.current_meters_per_second = Float32
        #create transform listener
        self.listener = tf.TransformListener()
        self.output = Float32
        self.pid.setpoint = self.target_velocity.vector.y
        self.count = Float32
        self.count = 0
    

    def thrust(self): #the input is the target velocity
        #listen to transform from map to base_link frame
        self.listener.waitForTransform("map", "wamv/base_link", rospy.Time(), rospy.Duration(10.0))
        rospy.Subscriber('/odometry/filtered_map', Odometry, self.update_current_twist)  
        #rate at which the the code loops, 1 = 1hz = 1 time per second
        rate = rospy.Rate(1)
        while not rospy.is_shutdown():  
            rate.sleep()

    def update_current_twist(self, data):
        self.map_twist.vector = data.twist.twist.linear
        self.map_twist.header.frame_id = data.header.frame_id
        #convert from map to base_link
        self.base_twist.vector = self.listener.transformVector3("wamv/base_link", self.map_twist)
        self.base_twist.header.frame_id = "wamv/base_link"
        #rospy.loginfo('Base Twist Vector: %s', self.base_twist.vector)
        self.velocity_output()

    def velocity_output(self):  
        self.current_meters_per_second = self.base_twist.vector.vector.y
        rospy.loginfo("1-------------------------------------------1")
        rospy.loginfo("current mps: %s", self.current_meters_per_second)
        f = open("output.txt", "a")
        f.write(str(self.count) + "," + str(self.current_meters_per_second) + "\n")
        f.close()
        self.count = self.count + 1
        self.output = self.pid(self.current_meters_per_second)
        rospy.loginfo("2-------------------------------------------2")
        rospy.loginfo("output value put into publish: %s", self.output)
        self.left_thruster_pub.publish(self.output)
        self.right_thruster_pub.publish(self.output)


if __name__ == '__main__': #when script is run, this is what is run after the imports. The controller class is instantiated, and the thrust function is called
    try:
        rospy.init_node('velocity_controller', anonymous=True)
        controller1 = Controller()
        controller1.thrust() 
    except rospy.ROSInterruptException:
        pass

Plot Generation Code

import matplotlib.pyplot as plt
import numpy as np

x, y = np.loadtxt('output.txt', delimiter=',', unpack=True)
plt.plot(x,y,label='Speed m/s')

plt.xlabel('Messages')
plt.ylabel('Speed')
plt.title('Speed vs. Messages')
plt.legend()
plt.show()
⚠️ **GitHub.com Fallback** ⚠️