Пример #1
0
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)
Пример #2
0
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)
Пример #3
0
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))
Пример #4
0
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))
Пример #5
0
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))
Пример #6
0
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)
Пример #7
0
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()
Пример #8
0
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()
Пример #9
0
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)
Пример #10
0
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
Пример #11
0
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()
Пример #12
0
	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)
Пример #13
0
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
Пример #14
0
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()