Custom controller in the usv_base_ctrl does not have any effects

[ulicioara]

Hello,

I would like to customise the control used on the USVs.

I started with just an openloop analysis (so no feedback, just sending msgs with a constant hardcoded value e.g. for rudder and velocity), based on the control_heading scripts from usv_base_ctrl and changing in the spawner launch files from usv_sim/launch/models the name of the script for the heading control node. ( <node name="heading_control" pkg="usv_base_ctrl" type="airboat_openloop.py" unless="$(arg gui)" output="screen" />)

However there seems to be no difference between that and the default run. I added code that was tested on the airboat.

Another thing to be mentioned is that besides the diffboat, all boats drift to the right. Is this expected behaviour? Disturbances for wind and water were set to none in the _scenario1 launch files. Should that have been changed in other files as well? (segmentation, j1, j2 etc)

Also, how is PID in the freefloating_gazebo package affecting the simulation? Should anything be changed there as well?

I am working on kinetic.

I'd appreciate any input.

#!/usr/bin/env python
import rospy
import math
import tf
from sensor_msgs.msg import JointState
from std_msgs.msg import Header
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Quaternion
from geometry_msgs.msg import Twist, Point, Quaternion
from std_msgs.msg import Float64

initial_pose = Odometry()
target_pose = Odometry()
target_distance = 0
actuator_vel = 15
Ianterior = 0
rate_value = 50
Kp = 2
Ki = 0
result = Float64()
result.data = 0
f_distance = 3
rudder_min = -0.5
rudder_max = 0.5
rudder_med = (rudder_min + rudder_max)/2


def get_pose(initial_pose_tmp):
    global initial_pose 
    initial_pose = initial_pose_tmp

def get_target(target_pose_tmp):
    global target_pose 
    target_pose = target_pose_tmp

def thruster_ctrl_msg():
    global actuator_vel
    msg = JointState()
    msg.header = Header()
    msg.header.stamp = rospy.Time.now()
    msg.name = ['fwd']
    msg.position = [actuator_vel]
    msg.velocity = []
    msg.effort = []
    return msg


def talker_ctrl():
    global rate_value
    rospy.init_node('usv_simple_ctrl', anonymous=True)
    rate = rospy.Rate(rate_value) # 10h
    # publishes to thruster and rudder topics
    pub_motor = rospy.Publisher('thruster_command', JointState, queue_size=10)
    pub_rudder = rospy.Publisher('joint_setpoint', JointState, queue_size=10)
    pub_result = rospy.Publisher('move_usv/result', Float64, queue_size=10)
    
    # subscribe to state and targer point topics
    rospy.Subscriber("state", Odometry, get_pose)  # get usv position (add 'gps' position latter)
    rospy.Subscriber("move_usv/goal", Odometry, get_target)  # get target position

    while not rospy.is_shutdown():
        try:  
            pub_motor.publish(thruster_ctrl_msg())
            pub_rudder.publish(rudder_ctrl_msg())
            rate.sleep()
        except rospy.ROSInterruptException:
	    rospy.logerr("ROS Interrupt Exception! Just ignore the exception!")
        except rospy.ROSTimeMovedBackwardsException:
	    rospy.logerr("ROS Time Backwards! Just ignore the exception!")


def rudder_ctrl():

    global actuator_vel
    actuator_vel = 0
    rudder_angle = 10 # degrees    

    return rudder_angle

def rudder_ctrl_msg():
    msg = JointState()
    msg.header = Header()
    msg.header.stamp = rospy.Time.now()
    msg.name = ['fwd_joint']
    msg.position = [rudder_ctrl()]
    msg.velocity = []
    msg.effort = []
    return msg

if __name__ == '__main__':
    try:
        talker_ctrl()
    except rospy.ROSInterruptException:
        pass