def __init__(self):
self.num_channels = 2
self.is_running = False
#Create the servoabsolute msg publisher
self.servo_msg = Servo()
for i in range(self.num_channels):
self.servo_msg.servos.append(Servo())
self.servo_msg.servos(i).servo = i + 1
self.pubmsg = rospy.Publisher("/servos_absolute",
ServoArray,
queue_size=1)
def __init__(self):
rospy.loginfo("Setting up the Node...")
rospy.init_node('dk_llc')
##--- Create an actuator dictionary
self.actuators = {}
self.actuators['throttle'] = ServoConvert(id = 1)
self.actuators['steering'] = ServoConvert(id = 2, direction = 1) ##- Positive left
rospy.loginfo("> Actuators correctly initialized!")
##--- Create the servo array publisher
##-- Create the message
self._servo_msg = ServoArray()
for i in range(2):
self._servo_msg.servos.append(Servo())
##--- Create the servo array publisher
self.ros_pub_servo_array = rospy.Publisher("/servo_absolute", ServoArray, queue_size = 1)
rospy.loginfo("> Publisher correctly initialized!")
##--- Create the Subscriber to Twist commands - /cmd_vel topic
self.ros_sub_twist = rospy.Subscriber("/cmd_vel", Twist, self.set_actuators_from_cmdvel)
rospy.loginfo("> Subscriber correctly initalized!")
##--- Save the last time we got a reference. Stop the vehicle
self._last_time_cmd_rcv = time.time()
self._timeout_s = 5 ##-- Stop after 5 seconds
rospy.loginfo("Initialization complete!")
def __init__(self):
rospy.loginfo("Setting up the node ...")
rospy.init_node("kbrd_act")
#--- Create an actuator dictionary
self.actuators = {}
self.actuators['throttle'] = ServoConvert(id=1)
self.actuators['steering'] = ServoConvert(id=2,
direction=1) #-positive left
#--- Create servo array publisher
#-- Create the message
self._servo_msg = ServoArray()
for i in range(2):
self._servo_msg.servos.append(Servo())
self._ros_pub_servo_array = rospy.Publisher("servos_absolute",
ServoArray,
queue_size=1)
rospy.loginfo("> Publisher correctly initialized")
#--- Create the subscriber to the /cmd_vel topic
self.ros_sub_twist = rospy.Subscriber("/cmd_vel", Twist,
self.set_actuators_from_cmdvel)
rospy.loginfo("> Subscriber correctly initialized")
#--- Save the last time we got a correspondence. Stop the vehicle
self._last_time_cmd_rcv = time.time()
self._timeout_s = 5 #-- stop after 5 sec
rospy.loginfo("Initialization complete")
def __init__(self):
rospy.loginfo("Setting Up the Node...")
rospy.init_node('robot_llc')
self.actuators = {}
self.actuators['shoulder_1'] = ServoConvert(id=1)
self.actuators['shoulder_2'] = ServoConvert(id=2)
self.actuators['elbow_1'] = ServoConvert(id=3)
self.actuators['elbow_2'] = ServoConvert(id=4)
self.actuators['wrist_1'] = ServoConvert(id=5)
self.actuators['wrist_2'] = ServoConvert(id=6)
rospy.loginfo("> Actuators corrrectly initialized")
self._servo_msg = ServoArray()
for i in range(16): self._servo_msg.servos.append(Servo())
#--- Create the servo array publisher
self.ros_pub_servo_array = rospy.Publisher("/servos_absolute", ServoArray, queue_size=1)
rospy.loginfo("> Publisher corrrectly initialized")
#--- Create the Subscriber to Twist commands
self.ros_sub_value = rospy.Subscriber("/cmd_vel", Twist, self.set_value)
rospy.loginfo("> Subscriber corrrectly initialized")
self.ros_sub_motions=rospy.Subscriber("/execute_motion", String ,self.set_motion)
rospy.loginfo("Initialization complete")
def __init__(self):
rospy.loginfo("Setting Up the Spot Micro Servo Control Node...")
# Set up and title the ros node for this code
rospy.init_node('spot_micro_servo_control')
# Intialize empty servo dictionary
self.servos = {}
# Create a servo dictionary with 12 ServoConvert objects
# keys: integers 0 through 12
# values: ServoConvert objects
for i in range(numServos):
self.servos[i] = ServoConvert(id=i)
rospy.loginfo("> Servos corrrectly initialized")
# Create empty ServoArray message with n number of Servos in its array
self._servo_msg = ServoArray()
for i in range(numServos):
self._servo_msg.servos.append(Servo())
# Create the servo array publisher
self.ros_pub_servo_array = rospy.Publisher("/servos_absolute", ServoArray, queue_size=1)
rospy.loginfo("> Publisher corrrectly initialized")
rospy.loginfo("Initialization complete")
# Setup terminal input reading, taken from teleop_twist_keyboard
self.settings = termios.tcgetattr(sys.stdin)
def __init__(self):
rospy.loginfo("Setting Up the Node...")
rospy.init_node('dk_llc')
self.actuators = {}
self.actuators['throttle'] = ServoConvert(id=1)
self.actuators['steering'] = ServoConvert(
id=2, direction=1) #-- positive left
rospy.loginfo("> Actuators corrrectly initialized")
self._servo_msg = ServoArray()
for i in range(2):
self._servo_msg.servos.append(Servo())
#--- Create the servo array publisher
self.ros_pub_servo_array = rospy.Publisher("/servos_absolute",
ServoArray,
queue_size=1)
rospy.loginfo("> Publisher corrrectly initialized")
#--- Create the Subscriber to Twist commands
self.ros_sub_twist = rospy.Subscriber("/cmd_vel", Twist,
self.set_actuators_from_cmdvel)
rospy.loginfo("> Subscriber corrrectly initialized")
#--- Get the last time e got a commands
self._last_time_cmd_rcv = time.time()
self._timeout_s = 5
rospy.loginfo("Initialization complete")
def __init__(self):
rospy.loginfo("Setting up Donkeycar Node...")
rospy.init_node('donkeycar')
"""
Create actuator dictionary
{
throttle: ServoConvert(id=1)
steer: ServoConvert(id=2)
}
"""
self.actuators = {}
self.actuators['throttle'] = ServoConvert(id=1)
self.actuators['steering'] = ServoConvert(id=2)
rospy.loginfo("> Actuators corrrectly initialized")
# Create servo array
# 2 servos - 1 = Throttle | 2 = Steer
self._servo_msg = ServoArray()
for i in range(2):
self._servo_msg.servos.append(Servo())
# Create the servo array publisher
self.ros_pub_servo_array = rospy.Publisher("/servos_absolute",
ServoArray,
queue_size=1)
rospy.loginfo("> Publisher corrrectly initialized")
# Create the Subscriber to Joystick commands
self.ros_sub_twist = rospy.Subscriber("/joy", Joy,
self.set_actuators_from_joystick)
rospy.loginfo("> Subscriber corrrectly initialized")
rospy.loginfo("Initialization complete")
def __init__(self):
rospy.loginfo("Setting up low-level control node...")
rospy.init_node("dk_llc")
# Create an actuator dictionary
self.actuators = {}
self.actuators['throttle'] = ServoConvert(id=1)
self.actuators['steering'] = ServoConvert(
id=2, direction=1) # Sets positive direction left
# Create the servo arra publisher and message
self._servo_msg = ServoArray()
for i in range(2): # One for speed, one for steering
self._servo_msg.servos.append(Servo())
self.pub = rospy.Publisher('/servos_absolute',
ServoArray,
queue_size=1)
rospy.loginfo('> LLC Publisher correctly initialized')
# Create subscriber to the /cmd_vel topic
self.sub_cmd_vel = rospy.Subscriber('/cmd_vel', Twist,
self.set_actuators_from_cmdvel)
rospy.loginfo('> LLC Subscriber correctly initialized')
self._last_time_cmd_rcv = time.time()
self._timeout_s = 5 # Stop after this amount of seconds if no additional commands received
rospy.loginfo('LLC initialization complete!')
class SpotMicroServoControl():
def __init__(self):
# Create a ServoConfig messag for one servo
self._servo_config_msg = ServoConfigArray()
self._servo1_config = ServoConfig()
self._servo1_config.center = 300
self._servo1_config.range = 400
self._servo1_config.servo = 1
self._servo1_config.direction = 1
self._servo_config_msg.servos.append(self._servo1_config)
rospy.loginfo("> Waiting for config_servos service...")
print('test1')
rospy.wait_for_service('config_servos')
rospy.loginfo("> Config_servos service found!")
print('test2')
try:
servoConfigService = rospy.ServiceProxy('config_servos',
ServosConfig)
resp = servoConfigService(self._servo_config_msg.servos)
print("Config servos done!!, returned value: %i" % resp.error)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
rospy.loginfo("Setting Up the Spot Micro Servo Control Node...")
# Set up and title the ros node for this code
rospy.init_node('spot_micro_servo_control')
# Intialize empty servo dictionary
self.servos = {}
# Create a servo dictionary with 12 ServoConvert objects
# keys: integers 0 through 12
# values: ServoConvert objects
for i in range(numServos):
self.servos[i] = ServoConvert(id=i)
rospy.loginfo("> Servos corrrectly initialized")
# Create empty ServoArray message with n number of Servos in its array
self._servo_msg = ServoArray()
for i in range(numServos):
self._servo_msg.servos.append(Servo())
# Create the servo array publisher
self.ros_pub_servo_array = rospy.Publisher("/servos_proportional",
ServoArray,
queue_size=1)
rospy.loginfo("> Publisher corrrectly initialized")
rospy.loginfo("Initialization complete")
# Setup terminal input reading, taken from teleop_twist_keyboard
self.settings = termios.tcgetattr(sys.stdin)
def __init__(self, pwm1, pwm2, pwm3):
self.pub = rospy.Publisher('/servos_absolute',
ServoArray,
queue_size=100)
rospy.init_node('ControleMoteur', anonymous=True)
rospy.on_shutdown(self.arret)
self.rate = rospy.Rate(50) # 10hz
self.msg = ServoArray()
self.Servo1 = Servo()
self.Servo2 = Servo()
self.Servo3 = Servo()
# DC moteurs
self.Servo1.servo = pwm1
self.Servo1.value = 0
self.Servo2.servo = pwm2
self.Servo2.value = 0
# Servo moteur
self.Servo3.servo = pwm3
self.Servo3.value = 0
self.msg.servos = [self.Servo1, self.Servo2, self.Servo3]
class donkey_driver:
#initiate the servo control board and set the dc driver to zero
def __init__(self):
self.num_channels = 2
self.is_running = False
#Create the servoabsolute msg publisher
self.servo_msg = Servo()
for i in range(self.num_channels):
self.servo_msg.servos.append(Servo())
self.servo_msg.servos(i).servo = i+1
self.pubmsg = rospy.Publisher("/servos_absolute", ServoArray, queue_size=1)
class donkey_driver:
#initiate the servo control board and set the dc driver to zero
def __init__(self):
self.num_channels = 2
self.is_running = False
#Create the servoabsolute msg publisher
self.servo_msg = Servo()
for i in range(self.num_channels):
self.servo_msg.servos.append(Servo())
self.servo_msg.servos(i).servo = i + 1
self.pubmsg = rospy.Publisher("/servos_absolute",
ServoArray,
queue_size=1)
def pub(self):
self.pubmsg.publish(self.servo_msg)
#Turn the wheels to the left
def left_turn(self):
self.servo_msg.servos[1].value = 345
#Turn the wheels to the right
def right_turn(self):
self.servo_msg.servos[1].value = 290
#Center the wheels for straight movement
def center_wheels(self):
self.servo_msg.servos[1].value = 318
#Set the servo to spin the wheels forward
def drive_fwd(self):
self.servo_msg.servos[0].value = 352
time.sleep(0.2)
self.servo_msg.servos[0].value = 349
#Set the servo to spin the wheels backwards
def drive_bwd(self):
self.servo_msg.servos[0].value = 0
time.sleep(0.2)
self.servo_msg.servos[0].value = 307
time.sleep(0.2)
self.servo_msg.servos[0].value = 0
time.sleep(0.2)
self.servo_msg.servos[0].value = 307
time.sleep(0.2)
self.servo_msg.servos[0].value = 312
#Stop the wheels and center/full stop
def drive_stop(self):
self.servo_msg.servos[0].value = 333
self.servo_msg.servos[1].value = 318
def __init__(self):
rospy.loginfo("Setting Up the Node...")
#--- Initialize the node
rospy.init_node('dk_llc')
self.actuators = {}
self.actuators['throttle'] = ServoConvert(id=1)
self.actuators['steering'] = ServoConvert(
id=2, center_value=328, direction=1) #-- positive left
rospy.loginfo("> Actuators corrrectly initialized")
self._servo_msg = ServoArray()
for i in range(2):
self._servo_msg.servos.append(Servo())
#--- Create the servo array publisher
self.ros_pub_servo_array = rospy.Publisher("/servos_absolute",
ServoArray,
queue_size=1)
rospy.loginfo("> Publisher corrrectly initialized")
#--- Create a debug publisher for resulting cmd_vel
self.ros_pub_debug_command = rospy.Publisher("/dkcar/debug/cmd_vel",
Twist,
queue_size=1)
rospy.loginfo("> Publisher corrrectly initialized")
#--- Create the Subscriber to Twist commands
self.ros_sub_twist = rospy.Subscriber("/cmd_vel", Twist,
self.update_message_from_command)
rospy.loginfo("> Subscriber corrrectly initialized")
#--- Create the Subscriber to obstacle_avoidance commands
self.ros_sub_twist = rospy.Subscriber("/dkcar/control/cmd_vel", Twist,
self.update_message_from_avoid)
rospy.loginfo("> Subscriber corrrectly initialized")
self.throttle_cmd = 0.
self.throttle_avoid = 1.
self.steer_cmd = 0.
self.steer_avoid = 0.
self._debud_command_msg = Twist()
#--- Get the last time e got a commands
self._last_time_cmd_rcv = time.time()
self._last_time_avoid_rcv = time.time()
self._timeout_s = 5
rospy.loginfo("Initialization complete")
def __init__(self):
#Inicializar motores y servo
self.actuators = {}
self.actuators['MotorDerecho'] = ServoConvert(id=14)
self.actuators['MotorIzquierdo'] = ServoConvert(id=13)
self.actuators['ServoBasura']= ServoConvert(id=16)
self._servo_msg = ServoArray()
for i in range(16): self._servo_msg.servos.append(Servo())
self._ros_sub_aruco = rospy.Subscriber("/fiducial_vertices", FiducialArray, self.callback)
self.ros_pub_servo_array = rospy.Publisher("/servos_absolute", ServoArray, queue_size=1)
self.idAruco=0
#Inicializar seguidor de linea
GPIO.setmode(GPIO.BCM)
self.wp = wp
self.lastError=0
self.errorI=0
self.lastProporcional=0
self.integral=0
self.KP=2
#self.KI=0.001
self.KD=5
self.velMinIzquierda=1500
self.velMinDerecha=1500
self.velMaxIzquierda=2500
self.velMaxDerecha=2500
self.Foco=20
self.Boton=6
GPIO.setup(self.Foco, GPIO.OUT)
GPIO.output(self.Foco, GPIO.HIGH)
self.LEDON_PIN = 5
self.SENSOR_PINS = [26, 23, 24, 25, 12]#6
self.NUM_SENSORS = len(self.SENSOR_PINS)
self.CHARGE_TIME = 10 #us to charge the capacitors
self.READING_TIMEOUT = 1000 #us, assume reading is black
self.sensorValues = []
self.calibratedMax = []
self.calibratedMin = []
self.lastValue = 0
self.init_pins()
def ControleMoteur(Va,Vb):
pub = rospy.Publisher('/servos_absolute', ServoArray,queue_size=100)
rospy.init_node('ControleMoteur', anonymous=True)
rate = rospy.Rate(50) # 10hz
msg=ServoArray()
MoteurA=Servo()
MoteurA.servo=16
MoteurA.value=0
MoteurB=Servo()
MoteurB.servo=15
MoteurB.value=0
msg.servos=[MoteurA, MoteurB]
while not rospy.is_shutdown():
rospy.loginfo(msg)
pub.publish(msg)
rate.sleep()
def __init__(self):
rospy.loginfo("Setting Up the Node...")
rospy.init_node('dk_llc')
self._servo_msg = ServoArray()
for i in range(16): self._servo_msg.servos.append(Servo())
#--- Create the servo array publisher
self.ros_pub_servo_array = rospy.Publisher("/servos_absolute", ServoArray, queue_size=1)
rospy.loginfo("> Publisher corrrectly initialized")
#--- Create the Subscriber to Twist commands
self.ros_sub_value = rospy.Subscriber("/cmd_vel", Twist, self.set_value)
rospy.loginfo("> Subscriber corrrectly initialized")
rospy.loginfo("Initialization complete")
def __init__(self):
rospy.loginfo("Setting up the node...")
rospy.init_node("ts_llc")
motor.setup()
motor.ctrl(1)
#--- Create the actuator dictionary
self.actuators = {}
self.actuators['throttle'] = ServoConvert(id=1, centre_value=0)
motor.setSpeed(0)
self.actuators['steering'] = ServoConvert(id=2,
direction=1) #-positive left
#--- Create the servo array publisher
#-- Create the message
self._servo_msg = ServoArray()
for i in range(2):
self._servo_msg.servos.append(Servo())
self.ros_pub_servo_array = rospy.Publisher("/servos_absolute",
ServoArray,
queue_size=1)
rospy.loginfo("> Publisher correctly initialized")
#--- Create the subscriber to the /cmd-vel topic
self.ros_sub_twist = rospy.Subscriber("/cmd_vel", Twist,
self.set_actuator_from_cmdvel)
#self.motor_sub = rospy.Subscriber("/cmd_vel", Twist, self.move_dc_from_cmdvel)
rospy.loginfo("> subscriber correctly initialized")
#--- save the last time we got a reference. Stop the vehicle if no commands given
self._last_time_cmd_rcv = time.time()
self._timeout_s = 5 #-- stop after 5 seconds
rospy.loginfo("Initialization complete")
def __init__(self):
'''Constructor'''
# Create speed, body rate, and state command data variables
self.x_speed_cmd_mps = 0
self.y_speed_cmd_mps = 0
self.yaw_rate_cmd_rps = 0
self.trot_event_cmd = False
self.prev_trot_event_cmd = False
# Create and publish servo config message
# Initialize servo_config_msg
self._servo_config_msg = ServoConfigArray()
for s in servo_dict.values():
temp_servo = ServoConfig()
temp_servo.center = s['center']
temp_servo.range = s['range']
temp_servo.servo = s['num']
temp_servo.direction = s['direction']
# Append servo to servo config message
self._servo_config_msg.servos.append(temp_servo)
# Publish servo configuration
# rospy.loginfo("> Waiting for config_servos service...")
# rospy.wait_for_service('config_servos')
# rospy.loginfo("> Config_servos service found!")
# try:
# servoConfigService = rospy.ServiceProxy('config_servos',ServosConfig)
# resp = servoConfigService(self._servo_config_msg.servos)
# print("Config servos done!!, returned value: %i"%resp.error)
# except rospy.ServiceException, e:
# print "Service call failed: %s"%e
# Set up and initialize ros node
# rospy.loginfo("Setting Up the Spot Micro Simple Command Node...")
# Set up and title the ros node for this code
# rospy.init_node('spot_micro_walk')
# Create a servo command dictionary in the same order as angles are received back from
# SpotMicroStickFigure.get_leg_angles
self.servo_cmds_rad = {'RB_1':0,'RB_2':0,'RB_3':0,
'RF_1':0,'RF_2':0,'RF_3':0,
'LF_1':0,'LF_2':0,'LF_3':0,
'LB_1':0,'LB_2':0,'LB_3':0}
# Create empty ServoArray message with n number of Servos in its array
self._servo_msg = ServoArray()
for _ in range(len(servo_dict)):
self._servo_msg.servos.append(Servo())
# Create the servo array publisher
# self.ros_pub_servo_array = rospy.Publisher("/servos_proportional", ServoArray, queue_size=1)
# rospy.loginfo("> Publisher corrrectly initialized")
# Create subsribers for speed and body rate command topics, both using vector3
# rospy.Subscriber('x_speed_cmd',Float32,self.update_x_speed_cmd)
# rospy.Subscriber('/y_speed_cmd',Float32,self.update_y_speed_cmd)
# rospy.Subscriber('/yaw_rate_cmd',Float32,self.update_yaw_rate_cmd)
# rospy.Subscriber('/state_cmd',Bool,self.update_state_cmd)
# rospy.loginfo("Initialization complete")
# Create a spot micro stick figure object to encapsulate robot state
self.default_height = 0.18
self.sm = SpotMicroStickFigure(y=self.default_height)
# Set absolute foot positions for default stance,
# foot order: RB, RF, LF, LB
l = self.sm.body_length
w = self.sm.body_width
l1 = self.sm.hip_length
self.default_sm_foot_position = np.array([ [-l/2, 0, w/2 + l1],
[ l/2 , 0, w/2 + l1],
[ l/2 , 0, -w/2 - l1],
[-l/2 , 0, -w/2 - l1] ])
self.sm.set_absolute_foot_coordinates(self.default_sm_foot_position)
# Create configuration object and update values to reflect spot micro configuration
self.config = Configuration()
self.config.delta_x = l/2
self.config.delta_y = w/2 + l1
self.default_z_ref = -self.default_height
# Create controller object
self.controller = Controller(self.config)
# Create state object
self.state = State()
self.state.foot_locations = (self.config.default_stance + np.array([0,0,-self.default_height])[:, np.newaxis])
# Create Command object
self.command = Command()
self.command.height = -self.default_height
class SpotMicroSimpleCommand():
'''Class to encapsulate simple command of spot micro robot'''
def __init__(self):
'''Constructor'''
# Create and publish servo config message
# Initialize servo_config_msg
self._servo_config_msg = ServoConfigArray()
for s in servo_dict.values():
temp_servo = ServoConfig()
temp_servo.center = s['center']
temp_servo.range = s['range']
temp_servo.servo = s['num']
temp_servo.direction = s['direction']
# Append servo to servo config message
self._servo_config_msg.servos.append(temp_servo)
# Publish servo configuration
rospy.loginfo("> Waiting for config_servos service...")
rospy.wait_for_service('config_servos')
rospy.loginfo("> Config_servos service found!")
try:
servoConfigService = rospy.ServiceProxy('config_servos',ServosConfig)
resp = servoConfigService(self._servo_config_msg.servos)
print("Config servos done!!, returned value: %i"%resp.error)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
# Set up and initialize ros node
rospy.loginfo("Setting Up the Spot Micro Simple Command Node...")
# Set up and title the ros node for this code
rospy.init_node('spot_micro_simple_command')
# Create a servo command dictionary in the same order as angles are received back from
# SpotMicroStickFigure.get_leg_angles
self.servo_cmds_rad = {'RB_1':0,'RB_2':0,'RB_3':0,
'RF_1':0,'RF_2':0,'RF_3':0,
'LF_1':0,'LF_2':0,'LF_3':0,
'LB_1':0,'LB_2':0,'LB_3':0}
# Create empty ServoArray message with n number of Servos in its array
self._servo_msg = ServoArray()
for _ in range(len(servo_dict)):
self._servo_msg.servos.append(Servo())
# Create the servo array publisher
self.ros_pub_servo_array = rospy.Publisher("/servos_proportional", ServoArray, queue_size=1)
rospy.loginfo("> Publisher corrrectly initialized")
rospy.loginfo("Initialization complete")
# Create a spot micro stick figure object to encapsulate robot state
self.sm = SpotMicroStickFigure(y=0.18)
# Set absolute foot positions
l = self.sm.body_length
w = self.sm.body_width
l1 = self.sm.hip_length
desired_p4_points = np.array([ [-l/2, 0, w/2 + l1],
[ l/2 , 0, w/2 + l1],
[ l/2 , 0, -w/2 - l1],
[-l/2 , 0, -w/2 - l1] ])
self.sm.set_absolute_foot_coordinates(desired_p4_points)
def __init__(self):
self.num_channels = 2
self.is_running = False
#Create the servoabsolute msg publisher
self.servo_msg = Servo()
# 1. Direction and velocity commands for Planning (geometry
# and chaosbot hardward specific).
# 2. Button commands for actions like Emergency stop
# and gripper movements
# 3. Button commands for poses or autonomous motions
#
# Direction and velocity geometry will be provided by
# the following joystick axes.
# NOTE: Using MODE where the MODE button light is ON
# axis 3 aka right stick vertical controls linear speed
# axis 2 aka right stick horizontal controls angular speed
ARMMOTORS_IN_CONTROL = 4
joystick = Twist()
chaosbot_cmds = [Servo() for i in range(ARMMOTORS_IN_CONTROL)]
# Initialize a inAutonomous variable and initialize [Toggled with Button 10]
inAutonomous = False
global pos_wrist, pos_hand
def callback_allcurrentinputs(data):
rospy.loginfo('Received a joystick input')
# Initialize the wrist and hand to neutral (middle) position
global pos_wrist, pos_hand
# Stop all motors and don't send servo positions if Button 10
if data.buttons[9] <> 0:
toggleAutonomous()
def __init__(self, wheel_distance=0.6, wheel_diameter=0.4):
rospy.loginfo("Setting Up the Node...")
rospy.init_node('scooby_llc')
GPIO.setmode(GPIO.BCM)
self.PIN_LIGHT = 5
self.PIN_HORN = 6
self.gain = 0.75
GPIO.setup(self.PIN_LIGHT, GPIO.OUT)
GPIO.setup(self.PIN_HORN, GPIO.OUT)
GPIO.output(self.PIN_LIGHT, GPIO.HIGH)
GPIO.output(self.PIN_HORN, GPIO.HIGH)
self._wheel_distance = wheel_distance
self._wheel_radius = wheel_diameter / 2.0
self.actuators = {}
self.actuators['left_a'] = ServoConvert(id=1)
self.actuators['left_b'] = ServoConvert(id=2,
direction=1) #-- positive left
self.actuators['right_a'] = ServoConvert(id=3)
self.actuators['right_b'] = ServoConvert(id=4, direction=1)
rospy.loginfo("> Actuators corrrectly initialized")
self._servo_msg = ServoArray()
#self._imu_data_msg = Imu()
self._joint_stat = JointState()
self._velocity_msg = Float32MultiArray()
for i in range(4):
self._servo_msg.servos.append(Servo())
#--- Create the servo array publisher
self.ros_pub_velocity_array = rospy.Publisher("/wheel_velocity",
Float32MultiArray,
queue_size=1)
self.ros_pub_servo_array = rospy.Publisher("/servos_absolute",
ServoArray,
queue_size=1)
#self.ros_pub_imu_data = rospy.Publisher("/imu/data_raw", Imu, queue_size=1)
self.ros_pub_joint_stat = rospy.Publisher("/joint_stat",
JointState,
queue_size=10)
rospy.loginfo("> Publisher corrrectly initialized")
#--- Create imu subscriber
#self.ros_sub_imu = rospy.Subscriber("/rtimulib_node/imu", Imu, self.send_imu_data_msg)
#--- Create the Subscriber to Joy commands
self.ros_sub_joy = rospy.Subscriber("/joy_teleop/joy", Joy,
self.set_actuators_from_joy)
#--- Create the Subscriber to Twist commands
self.ros_sub_twist = rospy.Subscriber("/joy_teleop/cmd_vel", Twist,
self.set_actuators_from_cmdvel)
rospy.loginfo("> Subscriber corrrectly initialized")
#--- Get the last time e got a commands
self._last_time_cmd_rcv = time.time()
self._timeout_s = 5
self.anglez = 0.00
rospy.loginfo("Initialization complete")