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