def allocate(self):
''' Solve for thrust vectors after creating trans matrix '''
# create thrust matrix
A = self.thrust_matrix()
b = self.des_force
# solve Ax = b
# solutions are given respectively by thruster pose
bl, br, fl, fr = np.linalg.lstsq(A, b)[0]
# Temporarily sending the left thruster command to the motor driver
BL_msg, BR_msg, FL_msg, FR_msg = Command(), Command(), Command(), Command()
# convert thrusts (in newtons) to effort values (in firmware units)
# clip them to firmware limits
# assign to ROS messages
BL_msg.setpoint = np.clip(bl * self.effort_ratio, -self.effort_limit, self.effort_limit)
BR_msg.setpoint = np.clip(br * self.effort_ratio, -self.effort_limit, self.effort_limit)
FL_msg.setpoint = np.clip(fl * self.effort_ratio, -self.effort_limit, self.effort_limit)
FR_msg.setpoint = np.clip(fr * self.effort_ratio, -self.effort_limit, self.effort_limit)
# publish ROS messages
if self.kill is True:
self.BL_pub.publish(Command(setpoint=0))
self.BR_pub.publish(Command(setpoint=0))
self.FL_pub.publish(Command(setpoint=0))
self.FR_pub.publish(Command(setpoint=0))
else:
self.BL_pub.publish(BL_msg)
self.BR_pub.publish(BR_msg)
self.FL_pub.publish(FL_msg)
self.FR_pub.publish(FR_msg)
def publish_thrusts(self):
'''
Use the mapper to find the individual thrusts needed to acheive the current body wrench.
If killed, publish 0 to all thrusters just to be safe.
'''
BL, BR, FL, FR = Command(), Command(), Command(), Command()
if not self.kill: # Only get thrust of not killed, otherwise publish default 0 thrust
BL.setpoint, BR.setpoint, FL.setpoint, FR.setpoint = self.thruster_map.wrench_to_thrusts(self.wrench)
self.BL_pub.publish(BL)
self.BR_pub.publish(BR)
self.FL_pub.publish(FL)
self.FR_pub.publish(FR)
def callback(data):
rospy.loginfo("Received a /cmd_vel message!")
rospy.loginfo("Linear Components: [%f, %f, %f]"%(data.linear.x, data.linear.y, data.linear.z))
rospy.loginfo("Angular Components: [%f, %f, %f]"%(data.angular.x, data.angular.y, data.angular.z))
pub = rospy.Publisher('/right/cmd', Command, queue_size=10)
pub2 = rospy.Publisher('/left/cmd', Command, queue_size=10)
msg = Command()
msg.commanded_velocity = ((data.linear.x - data.angular.z * (width / 2)) / radius) * 50
msg2 = Command()
msg2.commanded_velocity = - ((data.linear.x + data.angular.z * (width /2)) / radius) * 50
rospy.loginfo("Conversion to drive command for each wheel")
rospy.loginfo("Right wheel command %f", msg.commanded_velocity)
rospy.loginfo("Left wheel command %f", msg2.commanded_velocity)
pub.publish(msg)
pub2.publish(msg2)
def __init__(self): #sets up GPIO pins as outputs
#!/usr/bin/env python
c=Command()
pub_lin=rospy.Publisher('linear/cmd',Command, queue_size=10)
pub_ang=rospy.Publisher('angular/cmd',Command, queue_size=10)
rospy.init_node('motor_control',anonymous=True)
rate = rospy.Rate(10)
def deploy_grinch(self):
'''
Deploy the grinch mechanism
'''
winch_defer = self.spin_winch(speed=1.0)
yield self.nh.sleep(self._grinch_lower_time)
self._winch_motor_pub.publish(Command(setpoint=0))
winch_defer.cancel()
def spin_winch(self, speed=-1.0, interval=0.1):
'''
Spin the grinch raise/lower mechanism. To avoid watchdog timeout, this is sent
in a loop at the specified interface. So to stop spinning,
cancel the defer
'''
while True:
self._winch_motor_pub.publish(Command(setpoint=speed * self.max_grinch_effort))
yield self.nh.sleep(interval)
def allocate(self):
''' Solve for thrust vectors after creating trans matrix '''
# create thrust matrix
A = self.thrust_matrix()
b = self.des_force
# solve Ax = b
# solutions are given respectively by thruster pose
bl, br, fl, fr = np.linalg.lstsq(A, b)[0]
# Temporarily sending the left thruster command to the motor driver
BL_msg, BR_msg, FL_msg, FR_msg = Command(), Command(), Command(
), Command()
# convert thrusts (in newtons) to effort values (in firmware units)
# clip them to firmware limits
# assign to ROS messages
BL_msg.setpoint = np.clip(bl * self.effort_ratio, -self.effort_limit,
self.effort_limit)
BR_msg.setpoint = np.clip(br * self.effort_ratio, -self.effort_limit,
self.effort_limit)
FL_msg.setpoint = np.clip(fl * self.effort_ratio, -self.effort_limit,
self.effort_limit)
FR_msg.setpoint = np.clip(fr * self.effort_ratio, -self.effort_limit,
self.effort_limit)
# publish ROS messages
if self.kill is True:
self.BL_pub.publish(Command(setpoint=0))
self.BR_pub.publish(Command(setpoint=0))
self.FL_pub.publish(Command(setpoint=0))
self.FR_pub.publish(Command(setpoint=0))
else:
self.BL_pub.publish(BL_msg)
self.BR_pub.publish(BR_msg)
self.FL_pub.publish(FL_msg)
self.FR_pub.publish(FR_msg)
def spin_grinch(self, speed=1.0, interval=0.1):
'''
Spin the grinch mechnaism. To avoid watchdog timeout, this is sent
in a loop at the specified interface. So to stop spinning,
cancel the defer
Example:
# start spinning
d = self.spin_grinch()
# do some stuf....
...
# Stop spinning
d.cancel()
'''
while True:
self._grind_motor_pub.publish(Command(setpoint=speed * self.max_grinch_effort))
yield self.nh.sleep(interval)
def publish_thrusts(self):
'''
Use the mapper to find the individual thrusts needed to acheive the current body wrench.
If killed, publish 0 to all thrusters just to be safe.
'''
commands = [Command() for i in range(len(self.publishers))]
if not self.kill:
thrusts = self.thruster_map.wrench_to_thrusts(self.wrench)
if len(thrusts) != len(self.publishers):
rospy.logfatal(
"Number of thrusts does not equal number of publishers")
return
for i in range(len(self.publishers)):
commands[i].setpoint = thrusts[i]
for i in range(len(self.publishers)):
self.joint_state_msg.effort[i] = commands[i].setpoint
self.publishers[i].publish(commands[i])
self.joint_state_pub.publish(self.joint_state_msg)
def retract_grinch(self):
'''
Retract the grinch mechanism
'''
now = yield self.nh.get_time()
end = now + genpy.Duration(self._grinch_raise_time)
winch_defer = self.spin_winch(speed=-1.0)
while True:
now = yield self.nh.get_time()
if self.grinch_limit_switch_pressed:
print 'limit switch pressed, stopping'
break
elif now >= end:
print 'retract timed out'
break
yield self.nh.sleep(0.1)
winch_defer.cancel()
self._winch_motor_pub.publish(Command(setpoint=0))
def publish_thrusts(self):
'''
Use the mapper to find the individual thrusts needed to acheive the current body wrench.
If killed, publish 0 to all thrusters just to be safe.
'''
BL, BR, FL, FR = Command(), Command(), Command(), Command()
if not self.kill: # Only get thrust of not killed, otherwise publish default 0 thrust
BL.setpoint, BR.setpoint, FL.setpoint, FR.setpoint = self.thruster_map.wrench_to_thrusts(
self.wrench)
self.BL_pub.publish(BL)
self.BR_pub.publish(BR)
self.FL_pub.publish(FL)
self.FR_pub.publish(FR)
def callback(data):
rospy.loginfo("Received a /cmd_vel message!")
rospy.loginfo("Linear Components: [%f, %f, %f]" %
(data.linear.x, data.linear.y, data.linear.z))
rospy.loginfo("Angular Components: [%f, %f, %f]" %
(data.angular.x, data.angular.y, data.angular.z))
pub = rospy.Publisher('/right/cmd', Command, queue_size=10)
pub2 = rospy.Publisher('/left/cmd', Command, queue_size=10)
msg = Command()
msg.commanded_velocity = ((data.linear.x + data.angular.z *
(width / 2)) / radius) * 50 * 0.6
msg2 = Command()
msg2.commanded_velocity = -((data.linear.x - data.angular.z *
(width / 2)) / radius) * 50 * 0.6
rospy.loginfo("Conversion to drive command for each wheel")
rospy.loginfo("Right wheel command %f", msg.commanded_velocity)
rospy.loginfo("Left wheel command %f", msg2.commanded_velocity)
pub.publish(msg)
pub2.publish(msg2)
def send_command(self, setpoint):
command = Command()
command.mode = self.get_mode()
command.setpoint = setpoint
def send_command(self, setpoint):
command = Command()
command.setpoint = setpoint
self.cmd_publisher.publish(command)
def callback(self, twist):
FLW_cmd = Command()
FRW_cmd = Command()
RLW_cmd = Command()
RRW_cmd = Command()
# linear velocity
vLinear = sqrt(twist.linear.x ** 2 + twist.linear.y ** 2)
if vLinear > self.maxLinearVelocity:
vLinear = self.maxLinearVelocity
# movement orientation
Heading = atan2(twist.linear.y, twist.linear.x)
# x axis linear velocity
xVel = vLinear * cos(Heading)
# y axis linear velocity
yVel = vLinear * sin(Heading)
# YAW axis rotational velocity
yawVel = twist.angular.z
if yawVel ** 2 > self.maxAngularVelocity ** 2:
yawVel = self.maxAngularVelocity * yawVel / abs(yawVel)
w = self.InverseKinematics(xVel, yVel, yawVel)
FLW_cmd.mode = 0
FLW_cmd.setpoint = w[0]
FRW_cmd.mode = 0
FRW_cmd.setpoint = w[1]
RLW_cmd.mode = 0
RLW_cmd.setpoint = w[2]
RRW_cmd.mode = 0
RRW_cmd.setpoint = w[3]
self.pubFLW.publish(FLW_cmd)
self.pubFRW.publish(FRW_cmd)
self.pubRLW.publish(RLW_cmd)
self.pubRRW.publish(RRW_cmd)
def callback(self, twist):
FLW_cmd = Command()
FRW_cmd = Command()
RLW_cmd = Command()
RRW_cmd = Command()
# linear velocity
vLinear = sqrt(twist.linear.x**2 + twist.linear.y**2)
if vLinear > self.maxLinearVelocity:
vLinear = self.maxLinearVelocity
# movement orientation
Heading = atan2(twist.linear.y, twist.linear.x)
# x axis linear velocity
xVel = vLinear * cos(Heading)
# y axis linear velocity
yVel = vLinear * sin(Heading)
# YAW axis rotational velocity
yawVel = twist.angular.z
if yawVel**2 > self.maxAngularVelocity**2:
yawVel = self.maxAngularVelocity * yawVel / abs(yawVel)
w = self.InverseKinematics(xVel, yVel, yawVel)
FLW_cmd.mode = 0
FLW_cmd.setpoint = w[0]
FRW_cmd.mode = 0
FRW_cmd.setpoint = w[1]
RLW_cmd.mode = 0
RLW_cmd.setpoint = w[2]
RRW_cmd.mode = 0
RRW_cmd.setpoint = w[3]
self.pubFLW.publish(FLW_cmd)
self.pubFRW.publish(FRW_cmd)
self.pubRLW.publish(RLW_cmd)
self.pubRRW.publish(RRW_cmd)
the controller node won't read the imu topic, so we have a
temprary work around by publishing the heading here to the
/heading topic. Please resolve this if there is solution.
3. Prior to using pyserial, we used a motor driver called roboteq_driver.
Hence there are commented out line for publish topic to /roboteq_driver
The drivers can extract voltage level from the battery, however
they're not super easy to use. Hence we left it out.
4. We used two custom defined message. MotorCommand, and Arduino_imu,
the latter is essentially a time stamped float32
"""
strboard_ser = None
port_ser = None
port_command = Command()
star_command = Command()
servo_command = 1600
servo_pub = rospy.Publisher('servo_cmd', UInt32, queue_size=10)
port_pub = rospy.Publisher('starboard_driver/cmd', Command, queue_size=10)
star_pub = rospy.Publisher('port_driver/cmd', Command, queue_size=10)
heading_pub = rospy.Publisher('heading', Float32, queue_size=10)
#port_pub = rospy.Publisher('roboteq_driver/port/cmd', Command, queue_size=10)
#star_pub = rospy.Publisher('roboteq_driver/star/cmd', Command, queue_size=10)
# note that the servo is on the arduino, connected via rosserial
# therefore we need to publish to this topic for servo control.
def setup_serial():
def send_command(self, setpoint):
command = Command()
command.mode = self.get_mode()
command.setpoint = setpoint
