class wrench_generator:
def __init__(self, name):
# Set up publishers
self.wrench_pub = rospy.Publisher('wrench',
WrenchStamped,
queue_size=10)
# Initilize kill
self.killed = False
self.kill_listener = KillListener(self.set_kill, self.clear_kill)
self.kill_broadcaster = KillBroadcaster(
id=name, description='Tank steer wrench_generator shutdown')
rospy.on_shutdown(self.on_shutdown)
# Clear in case it was previously killed
try:
self.kill_broadcaster.clear()
except rospy.service.ServiceException, e:
rospy.logwarn(str(e))
# Current pose
self.carrot_position = self.current_position = np.zeros(3)
self.carrot_position = self.current_orientation = np.zeros(3)
self.current_velocity = np.zeros(3)
self.last_perp_velocity = 0
# Grab gains
self.p = rospy.get_param('p', 15)
self.i = rospy.get_param('i', 5)
self.d = rospy.get_param('d', 0)
# Distance before we only orient to goal
self.orientation_radius = rospy.get_param('orientation_radius', 0.5)
# Controller vars
self.last_time = rospy.get_rostime()
# Tf stuff
self.tf_listener = tf.TransformListener()
#self.tf_listener.waitForTransform('/enu', '/base_link', rospy.Time(0), rospy.Time(10000))
# Wait for current position and set as desired position
rospy.loginfo('Waiting for /odom')
self.odom_sub = rospy.Subscriber('/odom',
Odometry,
self.odom_cb,
queue_size=10)
while not self.current_position.any(
): # Will be 0 until an odom publishes (if its still 0 it will drift very very soon)
pass
# Start traj subscriber
self.traj_sub = rospy.Subscriber('/trajectory',
PoseTwistStamped,
self.traj_cb,
queue_size=10)
class wrench_generator:
def __init__(self, name):
# Set up publishers
self.wrench_pub = rospy.Publisher('wrench', WrenchStamped, queue_size = 10)
# Initilize kill
self.killed = False
self.kill_listener = KillListener(self.set_kill, self.clear_kill)
self.kill_broadcaster = KillBroadcaster(id=name, description='Tank steer wrench_generator shutdown')
rospy.on_shutdown(self.on_shutdown)
# Clear in case it was previously killed
try:
self.kill_broadcaster.clear()
except rospy.service.ServiceException, e:
rospy.logwarn(str(e))
# Current pose
self.carrot_position = self.current_position = np.zeros(3)
self.carrot_position = self.current_orientation = np.zeros(3)
self.current_velocity = np.zeros(3)
self.last_perp_velocity = 0
# Grab gains
self.p = rospy.get_param('p', 15)
self.i = rospy.get_param('i', 5)
self.d = rospy.get_param('d', 0)
# Distance before we only orient to goal
self.orientation_radius = rospy.get_param('orientation_radius', 0.5)
# Controller vars
self.last_time = rospy.get_rostime()
# Tf stuff
self.tf_listener = tf.TransformListener()
#self.tf_listener.waitForTransform('/enu', '/base_link', rospy.Time(0), rospy.Time(10000))
# Wait for current position and set as desired position
rospy.loginfo('Waiting for /odom')
self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_cb, queue_size = 10)
while not self.current_position.any(): # Will be 0 until an odom publishes (if its still 0 it will drift very very soon)
pass
# Start traj subscriber
self.traj_sub = rospy.Subscriber('/trajectory', PoseTwistStamped, self.traj_cb, queue_size = 10)
def thrusterCtrl():
global port_current
global starboard_current
global kill_listener
global kill_broadcaster
#Setup ros
rospy.init_node('thruster_control')
rospy.Subscriber("thruster_config", thrusterNewtons, motorConfigCallback)
r = rospy.Rate(UPDATE_RATE) #1000 hz(1ms Period)... I think
pub_timer = rospy.Timer(PUB_RATE, pubStatus)
# Initilize kill
kill_listener = KillListener(set_kill, clear_kill)
kill_broadcaster = KillBroadcaster(id=rospy.get_name(), description='Newtons to PWM shutdown')
# Clear in case it was previously killed
try:
kill_broadcaster.clear()
except rospy.service.ServiceException, e:
rospy.logwarn(str(e))
def start():
global kill_listener
global kill_broadcaster
#Init node
rospy.init_node('remote_control')
# Register xbox_rc as a controller
rospy.wait_for_service('/controller/register_controller', 30.0)
register_controller_proxy('xbox_rc')
# Request to switch to xbox_rc
request_controller_proxy('xbox_rc')
kill_broadcaster = KillBroadcaster(id=rospy.get_name(), description='Remote control kill')
kill_listener = KillListener(killed_cb, unkilled_cb)
try:
kill_broadcaster.clear()
except rospy.service.ServiceException, e:
rospy.logwarn(str(e))
def thrusterCtrl():
global port_current
global starboard_current
global kill_listener
global kill_broadcaster
global control_kill
#Setup ros
rospy.init_node('thruster_control')
#rospy.Subscriber("control_arbiter", Bool, control_callback)
rospy.Subscriber("thruster_config", thrusterNewtons, motorConfigCallback)
r = rospy.Rate(UPDATE_RATE) #1000 hz(1ms Period)... I think
pub_timer = rospy.Timer(PUB_RATE, pubStatus)
# Initilize kill
kill_listener = KillListener(set_kill, clear_kill)
kill_broadcaster = KillBroadcaster(id=rospy.get_name(), description='Newtons to PWM shutdown')
# Clear in case it was previously killed
try:
kill_broadcaster.clear()
except rospy.service.ServiceException, e:
rospy.logwarn(str(e))
class control_arbiter:
def __init__(self):
# Initilize ros
rospy.init_node('control_arbiter')
# Vars
self.controller = 'none'
self.controllers = []
self.floating = False
self.killed = False
self.continuous_angle_lock = threading.Lock()
self.joint_lock = threading.Lock()
self.full_lock = threading.Lock()
self.wheel_lock = threading.Lock()
self.thrust_lock = threading.Lock()
# Services
self.request_controller_srv = rospy.Service('request_controller',
request_controller,
self.request_controller_cb)
self.register_controller_srv = rospy.Service(
'register_controller', register_controller,
self.register_controller_cb)
self.float_srv = rospy.Service('float_mode', FloatMode,
self.set_float_mode)
self.get_all_controllers_srv = rospy.Service(
'get_all_controllers', get_all_controllers,
self.get_all_controllers_cb)
# Publishers
self.continuous_angle_pub = rospy.Publisher(
'/dynamixel/dynamixel_continuous_angle_config',
DynamixelContinuousAngleConfig,
queue_size=10)
self.full_pub = rospy.Publisher('/dynamixel/dynamixel_full_config',
DynamixelFullConfig,
queue_size=10)
self.joint_pub = rospy.Publisher('/dynamixel/dynamixel_joint_config',
DynamixelJointConfig,
queue_size=10)
self.wheel_pub = rospy.Publisher('/dynamixel/dynamixel_wheel_config',
DynamixelWheelConfig,
queue_size=10)
self.port_pub = rospy.Publisher('/stm32f3discovery_imu_driver/pwm1',
Float64,
queue_size=10)
self.starboard_pub = rospy.Publisher(
'/stm32f3discovery_imu_driver/pwm2', Float64, queue_size=10)
self.float_status_pub = rospy.Publisher('float_status',
Bool,
queue_size=1)
self.current_controller_pub = rospy.Publisher('current_controller',
String,
queue_size=1)
# Subscribers
self.continuous_angle_sub = rospy.Subscriber(
'dynamixel_continuous_angle_config',
ControlDynamixelContinuousAngleConfig,
self.continuous_angle_cb,
queue_size=10)
self.full_sub = rospy.Subscriber('dynamixel_full_config',
ControlDynamixelFullConfig,
self.full_cb,
queue_size=10)
self.joint_sub = rospy.Subscriber('dynamixel_joint_config',
ControlDynamixelJointConfig,
self.joint_cb,
queue_size=10)
self.wheel_sub = rospy.Subscriber('dynamixel_wheel_config',
ControlDynamixelWheelConfig,
self.wheel_cb,
queue_size=10)
self.thruster_sub = rospy.Subscriber('thruster_config',
ControlThrustConfig,
self.thruster_cb,
queue_size=10)
# Timers
self.update_timer = rospy.Timer(rospy.Duration(0.1),
self.status_update)
# Kill
self.kill_listener = KillListener(self.set_kill_cb, self.clear_kill_cb)
self.kill_broadcaster = KillBroadcaster(
id='control_arbiter', description='control_arbiter has shutdown')
try:
self.kill_broadcaster.clear()
except rospy.service.ServiceException, e:
rospy.logwarn(str(e))
rospy.on_shutdown(self.shutdown)
class trajectory_generator:
def __init__(self, name):
# Desired pose
self.desired_position = self.current_position = np.zeros(3)
self.desired_orientation = self.current_orientation = np.zeros(3)
#self.desired_twist = self.current_twist = Twist()
# Goal tolerances before seting succeded
self.linear_tolerance = rospy.get_param('linear_tolerance', 0.5)
self.angular_tolerance = rospy.get_param('angular_tolerance', np.pi / 10)
self.orientation_radius = rospy.get_param('orientation_radius', 0.75)
self.slow_down_radius = rospy.get_param('slow_down_radius', 5)
# Speed parameters
self.max_tracking_distance = rospy.get_param('max_tracking_distance', 5)
self.min_tracking_distance = rospy.get_param('min_tracking_distance', 1)
self.tracking_to_speed_conv = rospy.get_param('tracking_to_speed_conv', 1)
self.tracking_slope = (self.max_tracking_distance - self.min_tracking_distance) / (self.slow_down_radius - self.orientation_radius)
self.tracking_intercept = self.tracking_slope * self.orientation_radius + self.min_tracking_distance
# Publishers
self.traj_pub = rospy.Publisher('/trajectory', PoseTwistStamped, queue_size = 10)
# Set desired twist to 0
#self.desired_twist.linear.x = self.desired_twist.linear.y = self.desired_twist.linear.z = 0
#self.desired_twist.angular.x = self.desired_twist.angular.y = self.desired_twist.angular.z = 0
# Initilize a line
self.line = line(np.zeros(3), np.ones(3))
# Wait for current position and set as desired position
rospy.loginfo('Waiting for /odom')
self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_cb, queue_size = 10)
while not self.current_position.any(): # Will be 0 until an odom publishes (if its still 0 it will drift very very soon)
pass
# Initlize Trajectory generator with current position as goal
# Set the line to be along our current orientation
self.desired_position = self.current_position
self.desired_orientation = self.current_orientation
# Make a line along the orientation
self.line = line(self.current_position, tools.normal_vector_from_rotvec(self.current_orientation) + self.current_position)
self.redraw_line = False
rospy.loginfo('Got current pose from /odom')
# Kill handling
self.killed = False
self.kill_listener = KillListener(self.set_kill, self.clear_kill)
self.kill_broadcaster = KillBroadcaster(id=name, description='Tank steer trajectory_generator shutdown')
try:
self.kill_broadcaster.clear() # In case previously killed
except rospy.service.ServiceException, e:
rospy.logwarn(str(e))
rospy.on_shutdown(self.on_shutdown)
# RC handling
# Start the main update loop
rospy.Timer(rospy.Duration(0.1), self.update)
# Implement the moveto action server
self.moveto_as = actionlib.SimpleActionServer('moveto', MoveToAction, None, False)
self.moveto_as.register_goal_callback(self.new_goal)
self.moveto_as.register_preempt_callback(self.goal_preempt)
self.moveto_as.start()
class azi_waypoint:
def __init__(self, name):
# Implement the moveto action server
self.moveto_as = actionlib.SimpleActionServer('moveto', MoveToAction, None, False)
# Thread Locking
self.as_lock = self.moveto_as.action_server.lock
self.moveto_as.register_goal_callback(self.new_goal)
self.moveto_as.register_preempt_callback(self.goal_preempt)
# Trajectory generator (using stratagy design pattern where traj_gen is the stratagy)
self.traj_gen = None
# Desired pose
self.desired_position = self.current_position = np.zeros(3)
self.desired_orientation = self.current_orientation = np.zeros(3)
#self.desired_twist = self.current_twist = Twist()
# Goal tolerances before seting succeded
self.linear_tolerance = rospy.get_param('linear_tolerance', 1.25)
self.angular_tolerance = rospy.get_param('angular_tolerance', 20 * np.pi / 180)
# Publishers
self.traj_pub = rospy.Publisher('/trajectory', PoseTwistStamped, queue_size = 10)
self.traj_debug_pub = rospy.Publisher('/trajectory_debug', PoseStamped, queue_size = 10)
self.goal_debug_pub = rospy.Publisher('/goal_debug', PoseStamped, queue_size = 10)
# Set desired twist to 0
#self.desired_twist.linear.x = self.desired_twist.linear.y = self.desired_twist.linear.z = 0
#self.desired_twist.angular.x = self.desired_twist.angular.y = self.desired_twist.angular.z = 0
# Wait for current position and set as desired position
rospy.loginfo('Waiting for /odom')
self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_cb, queue_size = 10)
while not self.current_position.any(): pass # Will be 0 until an odom publishes (if its still 0 it will drift very very soon)
self.desired_position = self.current_position
self.desired_orientation = self.current_orientation
rospy.loginfo('Got current pose from /odom')
# Kill handling
self.killed = False
self.kill_listener = KillListener(self.set_kill, self.clear_kill)
self.kill_broadcaster = KillBroadcaster(id=name, description='Azi waypoint shutdown')
try:
self.kill_broadcaster.clear() # In case previously killed
except rospy.service.ServiceException, e:
rospy.logwarn(str(e))
rospy.on_shutdown(self.on_shutdown)
# Trajectory mode dictionary
self.modes ={
trajectory_modeRequest.POINT_SHOOT_PP: point_shoot_pp,
trajectory_modeRequest.POINT_SHOOT_2_PP: point_shoot_2_pp,
trajectory_modeRequest.A_STAR_RPP: a_star_rpp,
trajectory_modeRequest.GOAL_PP: goal_pp
}
# Initilize Trajectory generator with current position as goal
self.mode = trajectory_modeRequest.POINT_SHOOT_2_PP
self.traj_gen = point_shoot_2_pp()
self.traj_gen.start(self.get_current_posetwist())
# Initilize the switch modes service
mode_srv = rospy.Service('azi_waypoint_mode', trajectory_mode, self.switch_mode)
# Start the main update loop
rospy.Timer(rospy.Duration(0.1), self.update)
self.moveto_as.start()
class control_arbiter:
def __init__(self):
# Initilize ros
rospy.init_node("control_arbiter")
# Vars
self.controller = "none"
self.controllers = []
self.floating = False
self.killed = False
self.continuous_angle_lock = threading.Lock()
self.joint_lock = threading.Lock()
self.full_lock = threading.Lock()
self.wheel_lock = threading.Lock()
self.thrust_lock = threading.Lock()
# Services
self.request_controller_srv = rospy.Service(
"request_controller", request_controller, self.request_controller_cb
)
self.register_controller_srv = rospy.Service(
"register_controller", register_controller, self.register_controller_cb
)
self.float_srv = rospy.Service("float_mode", FloatMode, self.set_float_mode)
self.get_all_controllers_srv = rospy.Service(
"get_all_controllers", get_all_controllers, self.get_all_controllers_cb
)
# Publishers
self.continuous_angle_pub = rospy.Publisher(
"/dynamixel/dynamixel_continuous_angle_config", DynamixelContinuousAngleConfig, queue_size=10
)
self.full_pub = rospy.Publisher("/dynamixel/dynamixel_full_config", DynamixelFullConfig, queue_size=10)
self.joint_pub = rospy.Publisher("/dynamixel/dynamixel_joint_config", DynamixelJointConfig, queue_size=10)
self.wheel_pub = rospy.Publisher("/dynamixel/dynamixel_wheel_config", DynamixelWheelConfig, queue_size=10)
self.port_pub = rospy.Publisher("/stm32f3discovery_imu_driver/pwm1", Float64, queue_size=10)
self.starboard_pub = rospy.Publisher("/stm32f3discovery_imu_driver/pwm2", Float64, queue_size=10)
self.float_status_pub = rospy.Publisher("float_status", Bool, queue_size=1)
self.current_controller_pub = rospy.Publisher("current_controller", String, queue_size=1)
# Subscribers
self.continuous_angle_sub = rospy.Subscriber(
"dynamixel_continuous_angle_config",
ControlDynamixelContinuousAngleConfig,
self.continuous_angle_cb,
queue_size=10,
)
self.full_sub = rospy.Subscriber(
"dynamixel_full_config", ControlDynamixelFullConfig, self.full_cb, queue_size=10
)
self.joint_sub = rospy.Subscriber(
"dynamixel_joint_config", ControlDynamixelJointConfig, self.joint_cb, queue_size=10
)
self.wheel_sub = rospy.Subscriber(
"dynamixel_wheel_config", ControlDynamixelWheelConfig, self.wheel_cb, queue_size=10
)
self.thruster_sub = rospy.Subscriber("thruster_config", ControlThrustConfig, self.thruster_cb, queue_size=10)
# Timers
self.update_timer = rospy.Timer(rospy.Duration(0.1), self.status_update)
# Kill
self.kill_listener = KillListener(self.set_kill_cb, self.clear_kill_cb)
self.kill_broadcaster = KillBroadcaster(id="control_arbiter", description="control_arbiter has shutdown")
try:
self.kill_broadcaster.clear()
except rospy.service.ServiceException, e:
rospy.logwarn(str(e))
rospy.on_shutdown(self.shutdown)
class Controller(object):
def __init__(self, rate=100):
'''I can't swim on account of my ass-mar'''
# Register azi_drive as a controller
rospy.wait_for_service('/controller/register_controller', 30.0)
rospy.ServiceProxy('/controller/register_controller',
register_controller)('azi_drive')
self.rate = rate
self.servo_max_rotation = 6.0
self.controller_max_rotation = self.servo_max_rotation / self.rate
# rospy.init_node('azi_drive', log_level=rospy.WARN)
# Set up kill handling
self.kill_listener = KillListener(self.on_kill, self.on_unkill)
self.kill_broadcaster = KillBroadcaster(
id='Azi_drive', description='Azi drive node shutdown')
try:
self.kill_broadcaster.clear()
except rospy.service.ServiceException, e:
rospy.logwarn(str(e))
self.killed = False
rospy.logwarn("Setting maximum rotation speed to {} rad/s".format(
self.controller_max_rotation))
Azi_Drive.set_delta_alpha_max(self.controller_max_rotation)
#Azi_Drive.set_thrust_bound((-50,50))
# These should not be queued! Old commands are garbage.
# Unfortunately, we have to queue these, because the subscriber cannot process two sequential
# thrust messages as quickly as they are sent
self.thrust_pub = rospy.Publisher('thruster_config',
thrusterNewtons,
queue_size=4)
self.servo_pub = rospy.Publisher('controller/dynamixel_full_config',
ControlDynamixelFullConfig,
queue_size=4)
self.next_wrench = WrenchStamped().wrench
rospy.Subscriber('wrench',
WrenchStamped,
self._wrench_cb,
queue_size=1)
# Thrust topic id's for each thruster
self.left_id = 3
self.right_id = 2
# Time between messages before azi_drive shuts off
# self.control_timeout = 2 # secs
self.control_timeout = np.inf
self.default_angles = np.array([0.0, 0.0], dtype=np.float32)
self.default_forces = np.array([0.0, 0.0], dtype=np.float32)
self.pwm_forces = np.array([0.0, 0.0], dtype=np.float64)
# Left, Right
self.cur_angles = self.default_angles[:]
self.cur_forces = self.default_forces[:]
self.prev_angles = None
self.set_servo_angles(self.cur_angles)
self.set_forces(self.cur_forces)
# Initializations
self.last_msg_time = time()
self.des_fx, self.des_fy, self.des_torque = 0.0, 0.0, 0.0
self.MAX_NEWTONS = 100.0 #Full forward Jacksons motors
self.MIN_NEWTONS = -100.0 #Full reverse Jacksons
self.ABS_MAX_PW = .002
self.ABS_MIN_PW = .001
self.ZERO_NEWTONS = 0
self.REV_CONV = (self.ZERO_NEWTONS -
self.MIN_NEWTONS) / (0 - self.ABS_MIN_PW)
self.FWD_CONV = (self.MAX_NEWTONS -
self.ZERO_NEWTONS) / self.ABS_MAX_PW
class trajectory_generator:
def __init__(self, name):
# Desired pose
self.desired_position = self.current_position = np.zeros(3)
self.desired_orientation = self.current_orientation = np.zeros(3)
#self.desired_twist = self.current_twist = Twist()
# Goal tolerances before seting succeded
self.linear_tolerance = rospy.get_param('linear_tolerance', 0.5)
self.angular_tolerance = rospy.get_param('angular_tolerance',
np.pi / 10)
self.orientation_radius = rospy.get_param('orientation_radius', 0.75)
self.slow_down_radius = rospy.get_param('slow_down_radius', 5)
# Speed parameters
self.max_tracking_distance = rospy.get_param('max_tracking_distance',
5)
self.min_tracking_distance = rospy.get_param('min_tracking_distance',
1)
self.tracking_to_speed_conv = rospy.get_param('tracking_to_speed_conv',
1)
self.tracking_slope = (
self.max_tracking_distance - self.min_tracking_distance) / (
self.slow_down_radius - self.orientation_radius)
self.tracking_intercept = self.tracking_slope * self.orientation_radius + self.min_tracking_distance
# Publishers
self.traj_pub = rospy.Publisher('/trajectory',
PoseTwistStamped,
queue_size=10)
# Set desired twist to 0
#self.desired_twist.linear.x = self.desired_twist.linear.y = self.desired_twist.linear.z = 0
#self.desired_twist.angular.x = self.desired_twist.angular.y = self.desired_twist.angular.z = 0
# Initilize a line
self.line = line(np.zeros(3), np.ones(3))
# Wait for current position and set as desired position
rospy.loginfo('Waiting for /odom')
self.odom_sub = rospy.Subscriber('/odom',
Odometry,
self.odom_cb,
queue_size=10)
while not self.current_position.any(
): # Will be 0 until an odom publishes (if its still 0 it will drift very very soon)
pass
# Initlize Trajectory generator with current position as goal
# Set the line to be along our current orientation
self.desired_position = self.current_position
self.desired_orientation = self.current_orientation
# Make a line along the orientation
self.line = line(
self.current_position,
tools.normal_vector_from_rotvec(self.current_orientation) +
self.current_position)
self.redraw_line = False
rospy.loginfo('Got current pose from /odom')
# Kill handling
self.killed = False
self.kill_listener = KillListener(self.set_kill, self.clear_kill)
self.kill_broadcaster = KillBroadcaster(
id=name, description='Tank steer trajectory_generator shutdown')
try:
self.kill_broadcaster.clear() # In case previously killed
except rospy.service.ServiceException, e:
rospy.logwarn(str(e))
rospy.on_shutdown(self.on_shutdown)
# RC handling
# Start the main update loop
rospy.Timer(rospy.Duration(0.1), self.update)
# Implement the moveto action server
self.moveto_as = actionlib.SimpleActionServer('moveto', MoveToAction,
None, False)
self.moveto_as.register_goal_callback(self.new_goal)
self.moveto_as.register_preempt_callback(self.goal_preempt)
self.moveto_as.start()
global s
data = s.readline()
return data
if __name__ == '__main__':
rospy.init_node('kill_switch')
kill_broadcaster = KillBroadcaster(rospy.get_name(),"kill switch kill")
try:
kill_broadcaster.clear()
except rospy.service.ServiceException, e:
rospy.logwarn(str(e))
kill_switch_pub = rospy.Publisher('kill_switch', String, queue_size = 10)
while not rospy.is_shutdown():
global status
status = ReadTerminal()
status = str(status)
class Controller(object):
def __init__(self, rate=100):
'''I can't swim on account of my ass-mar'''
# Register azi_drive as a controller
rospy.wait_for_service('/controller/register_controller', 30.0)
rospy.ServiceProxy('/controller/register_controller', register_controller)('azi_drive')
self.rate = rate
self.servo_max_rotation = 6.0
self.controller_max_rotation = self.servo_max_rotation / self.rate
# rospy.init_node('azi_drive', log_level=rospy.WARN)
# Set up kill handling
self.kill_listener = KillListener(self.on_kill, self.on_unkill)
self.kill_broadcaster = KillBroadcaster(id='Azi_drive', description='Azi drive node shutdown')
try:
self.kill_broadcaster.clear()
except rospy.service.ServiceException, e:
rospy.logwarn(str(e))
self.killed = False
rospy.logwarn("Setting maximum rotation speed to {} rad/s".format(self.controller_max_rotation))
Azi_Drive.set_delta_alpha_max(self.controller_max_rotation)
#Azi_Drive.set_thrust_bound((-50,50))
# These should not be queued! Old commands are garbage.
# Unfortunately, we have to queue these, because the subscriber cannot process two sequential
# thrust messages as quickly as they are sent
self.thrust_pub = rospy.Publisher('thruster_config', thrusterNewtons, queue_size=4)
self.servo_pub = rospy.Publisher('controller/dynamixel_full_config', ControlDynamixelFullConfig, queue_size=4)
self.next_wrench = WrenchStamped().wrench
rospy.Subscriber('wrench', WrenchStamped, self._wrench_cb, queue_size=1)
# Thrust topic id's for each thruster
self.left_id = 3
self.right_id = 2
# Time between messages before azi_drive shuts off
# self.control_timeout = 2 # secs
self.control_timeout = np.inf
self.default_angles = np.array([0.0, 0.0], dtype=np.float32)
self.default_forces = np.array([0.0, 0.0], dtype=np.float32)
self.pwm_forces = np.array([0.0, 0.0], dtype=np.float64)
# Left, Right
self.cur_angles = self.default_angles[:]
self.cur_forces = self.default_forces[:]
self.prev_angles = None
self.set_servo_angles(self.cur_angles)
self.set_forces(self.cur_forces)
# Initializations
self.last_msg_time = time()
self.des_fx, self.des_fy, self.des_torque = 0.0, 0.0, 0.0
self.MAX_NEWTONS = 100.0 #Full forward Jacksons motors
self.MIN_NEWTONS = -100.0 #Full reverse Jacksons
self.ABS_MAX_PW = .002
self.ABS_MIN_PW = .001
self.ZERO_NEWTONS = 0
self.REV_CONV = (self.ZERO_NEWTONS - self.MIN_NEWTONS) / (0 - self.ABS_MIN_PW)
self.FWD_CONV = (self.MAX_NEWTONS - self.ZERO_NEWTONS) / self.ABS_MAX_PW
class Node(object):
# Constructor
def __init__(self):
rospy.init_node('tank_steer_pd', anonymous=False)
# Grab params
if rospy.has_param('~simulate'):
self.simulate = rospy.get_param('~simulate')
else:
self.simulate = 0
self.max_thrust = rospy.get_param('/max_thrust', 100.0)
self.min_thrust = rospy.get_param('/min_thrust', -100.0)
self.L = rospy.get_param('distance_between_thrusters', 0.6096) #meters
# Set up publishers for servo position and prop speed
self.thrust_pub = rospy.Publisher('thruster_config', thrusterNewtons, queue_size = 10)
self.servo_pub = rospy.Publisher('dynamixel/dynamixel_full_config', DynamixelFullConfig, queue_size = 10)
# Initilize kill
self.killed = False
self.kill_listener = KillListener(self.set_kill, self.clear_kill)
self.kill_broadcaster = KillBroadcaster(id=rospy.get_name(), description='Tank steer PD shutdown')
rospy.on_shutdown(self.on_shutdown)
# Clear in case it was previously killed
try:
self.kill_broadcaster.clear()
except rospy.service.ServiceException, e:
rospy.logwarn(str(e))
# Get subscriber to wrench topic (from trajectory generator)
self.wrench_sub = rospy.Subscriber('wrench', WrenchStamped, self._wrench_cb, queue_size = 10)
# Setup some constants
# Define thrustors and there position
# TODO: should be retrieved and not hard coded
"""
self.thrusters = [
dict( # port
id=3,
pwm_pub= rospy.Publisher('stm32f3discovery_imu_driver/pwm1', Float64, queue_size = 10),
angle_pub= rospy.Publisher('port_angle', Float64, queue_size = 10),
),
dict( # starboard
id=2,
position= np.array([-.5239, -.3048, -.5]),
angle= 0,
#thrust_range= (min(calib_data[0]), max(calib_data[0])),
dangle_range= (-3.2e-0, +3.2e-0),
angle_range= (-math.pi*.75, +math.pi*.75), # at most (-6*math.pi, +6*math.pi)
effort= 0,
dangle= 0,
pwm_pub= rospy.Publisher('stm32f3discovery_imu_driver/pwm2', Float64, queue_size = 10),
angle_pub= rospy.Publisher('starboard_angle', Float64, queue_size = 10),
),
]
"""
# See Wrench Callback for deffinition of wrench_tf
self.wrench_tf = np.matrix([[0.5, -1 / self.L], [0.5, 1 / self.L]])
# Wait for the dynamixel node to initilze
if not self.simulate:
rospy.loginfo('Checking/waiting for dynamixel server')
while(self.servo_pub.get_num_connections() <= 0 and not rospy.is_shutdown):
pass
rospy.loginfo('dynamixel server found')
# Zero servos
for x in range(2,4):
self.servo_pub.publish(DynamixelFullConfig(
id= x,
goal_position= math.pi,
moving_speed= 12, # near maximum, not actually achievable ...
torque_limit= 1023,
goal_acceleration= 38,
control_mode= DynamixelFullConfig.JOINT,
goal_velocity= 10,
))
# Wait for wrench msgs
rospy.spin()