def test_cb(self, msg): self.hello() self.forward_DH() br = TransformBroadcaster() ls = tf.TransformListener() tar_2_msg = g_msg.TransformStamped() tar_3_msg = g_msg.TransformStamped() pos, ros = None, None try: ls.waitForTransform("tool_target", "iiwa_base_link", rospy.Time(0), rospy.Duration(4.0)) (pos, rot) = ls.lookupTransform('iiwa_base_link', 'tool_target', rospy.Time()) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): print("can't get iiwa_base_link <- tool_target transform") return -1 pos[0] += 0.4 #br.sendTransform( pos, rot, rospy.Time.now(), 'tool_target_2', 'iiwa_base_link' ) tar_2_msg.header.stamp = rospy.Time.now() tar_2_msg.header.frame_id = 'iiwa_base_link' tar_2_msg.child_frame_id = 'tool_target_2' tar_2_msg.transform.translation.x = pos[0] tar_2_msg.transform.translation.y = pos[1] tar_2_msg.transform.translation.z = pos[2] tar_2_msg.transform.rotation.x = rot[0] tar_2_msg.transform.rotation.y = rot[1] tar_2_msg.transform.rotation.z = rot[2] tar_2_msg.transform.rotation.w = rot[3] br.sendTransformMessage(tar_2_msg)
def main(): rospy.init_node("imu") port = rospy.get_param("~port", "GPG3_AD1") sensor = InertialMeasurementUnit(bus=port) pub_imu = rospy.Publisher("~imu", Imu, queue_size=10) pub_temp = rospy.Publisher("~temp", Temperature, queue_size=10) pub_magn = rospy.Publisher("~magnetometer", MagneticField, queue_size=10) br = TransformBroadcaster() msg_imu = Imu() msg_temp = Temperature() msg_magn = MagneticField() hdr = Header(stamp=rospy.Time.now(), frame_id="IMU") rate = rospy.Rate(rospy.get_param('~hz', 30)) while not rospy.is_shutdown(): q = sensor.read_quaternion() # x,y,z,w mag = sensor.read_magnetometer() # micro Tesla (µT) gyro = sensor.read_gyroscope() # deg/second accel = sensor.read_accelerometer() # m/s² temp = sensor.read_temperature() # °C msg_imu.header = hdr hdr.stamp = rospy.Time.now() msg_temp.header = hdr msg_temp.temperature = temp pub_temp.publish(msg_temp) msg_imu.header = hdr msg_imu.linear_acceleration.x = accel[0] msg_imu.linear_acceleration.y = accel[1] msg_imu.linear_acceleration.z = accel[2] msg_imu.angular_velocity.x = math.radians(gyro[0]) msg_imu.angular_velocity.y = math.radians(gyro[1]) msg_imu.angular_velocity.z = math.radians(gyro[2]) msg_imu.orientation.w = q[3] msg_imu.orientation.x = q[0] msg_imu.orientation.y = q[1] msg_imu.orientation.z = q[2] pub_imu.publish(msg_imu) msg_magn.header = hdr msg_magn.magnetic_field.x = mag[0] * 1e-6 msg_magn.magnetic_field.y = mag[1] * 1e-6 msg_magn.magnetic_field.z = mag[2] * 1e-6 pub_magn.publish(msg_magn) transform = TransformStamped(header=Header(stamp=rospy.Time.now(), frame_id="world"), child_frame_id="IMU") transform.transform.rotation = msg_imu.orientation br.sendTransformMessage(transform) rate.sleep()
class Robot: # short variables ML = gopigo3.GoPiGo3.MOTOR_LEFT MR = gopigo3.GoPiGo3.MOTOR_RIGHT S1 = gopigo3.GoPiGo3.SERVO_1 S2 = gopigo3.GoPiGo3.SERVO_2 BL = gopigo3.GoPiGo3.LED_BLINKER_LEFT BR = gopigo3.GoPiGo3.LED_BLINKER_RIGHT EL = gopigo3.GoPiGo3.LED_EYE_LEFT ER = gopigo3.GoPiGo3.LED_EYE_RIGHT EW = gopigo3.GoPiGo3.LED_WIFI WIDTH = gopigo3.GoPiGo3.WHEEL_BASE_WIDTH * 1e-3 CIRCUMFERENCE = gopigo3.GoPiGo3.WHEEL_CIRCUMFERENCE * 1e-3 POWER_PIN = "23" PULSE_RANGE = [575, 2425] def __init__(self): #### GoPiGo3 power management # export pin if not os.path.isdir("/sys/class/gpio/gpio" + self.POWER_PIN): gpio_export = os.open("/sys/class/gpio/export", os.O_WRONLY) os.write(gpio_export, self.POWER_PIN.encode()) os.close(gpio_export) time.sleep(0.1) # set pin direction gpio_direction = os.open( "/sys/class/gpio/gpio" + self.POWER_PIN + "/direction", os.O_WRONLY) os.write(gpio_direction, "out".encode()) os.close(gpio_direction) # activate power management self.gpio_value = os.open( "/sys/class/gpio/gpio" + self.POWER_PIN + "/value", os.O_WRONLY) os.write(self.gpio_value, "1".encode()) # GoPiGo3 and ROS setup self.g = gopigo3.GoPiGo3() print("GoPiGo3 info:") print("Manufacturer : ", self.g.get_manufacturer()) print("Board : ", self.g.get_board()) print("Serial Number : ", self.g.get_id()) print("Hardware version: ", self.g.get_version_hardware()) print("Firmware version: ", self.g.get_version_firmware()) self.reset_odometry() rospy.init_node("gopigo3") self.br = TransformBroadcaster() # subscriber rospy.Subscriber("motor/dps/left", Int16, lambda msg: self.g.set_motor_dps(self.ML, msg.data)) rospy.Subscriber("motor/dps/right", Int16, lambda msg: self.g.set_motor_dps(self.MR, msg.data)) rospy.Subscriber("motor/pwm/left", Int8, lambda msg: self.g.set_motor_power(self.ML, msg.data)) rospy.Subscriber("motor/pwm/right", Int8, lambda msg: self.g.set_motor_power(self.MR, msg.data)) rospy.Subscriber( "motor/position/left", Int16, lambda msg: self.g.set_motor_position(self.ML, msg.data)) rospy.Subscriber( "motor/position/right", Int16, lambda msg: self.g.set_motor_position(self.MR, msg.data)) rospy.Subscriber("servo/pulse_width/1", Int16, lambda msg: self.g.set_servo(self.S1, msg.data)) rospy.Subscriber("servo/pulse_width/2", Int16, lambda msg: self.g.set_servo(self.S2, msg.data)) rospy.Subscriber("servo/position/1", Float64, lambda msg: self.set_servo_angle(self.S1, msg.data)) rospy.Subscriber("servo/position/2", Float64, lambda msg: self.set_servo_angle(self.S2, msg.data)) rospy.Subscriber("cmd_vel", Twist, self.on_twist) rospy.Subscriber("led/blinker/left", UInt8, lambda msg: self.g.set_led(self.BL, msg.data)) rospy.Subscriber("led/blinker/right", UInt8, lambda msg: self.g.set_led(self.BR, msg.data)) rospy.Subscriber( "led/eye/left", ColorRGBA, lambda c: self.g.set_led( self.EL, int(c.r * 255), int(c.g * 255), int(c.b * 255))) rospy.Subscriber( "led/eye/right", ColorRGBA, lambda c: self.g.set_led( self.ER, int(c.r * 255), int(c.g * 255), int(c.b * 255))) rospy.Subscriber( "led/wifi", ColorRGBA, lambda c: self.g.set_led( self.EW, int(c.r * 255), int(c.g * 255), int(c.b * 255))) # publisher self.pub_enc_l = rospy.Publisher('motor/encoder/left', Float64, queue_size=10) self.pub_enc_r = rospy.Publisher('motor/encoder/right', Float64, queue_size=10) self.pub_battery = rospy.Publisher('battery_voltage', Float64, queue_size=10) self.pub_motor_status = rospy.Publisher('motor/status', MotorStatusLR, queue_size=10) self.pub_odometry = rospy.Publisher("odom", Odometry, queue_size=10) self.pub_joints = rospy.Publisher("joint_states", JointState, queue_size=10) # services self.srv_reset = rospy.Service('reset', Trigger, self.reset) self.srv_spi = rospy.Service( 'spi', SPI, lambda req: SPIResponse( data_in=self.g.spi_transfer_array(req.data_out))) self.srv_pwr_on = rospy.Service('power/on', Trigger, self.power_on) self.srv_pwr_off = rospy.Service('power/off', Trigger, self.power_off) # main loop rate = rospy.Rate(rospy.get_param('hz', 30)) # in Hz while not rospy.is_shutdown(): self.pub_battery.publish( Float64(data=self.g.get_voltage_battery())) # publish motor status, including encoder value (flags, power, encoder, speed) = self.g.get_motor_status(self.ML) status_left = MotorStatus(low_voltage=(flags & (1 << 0)), overloaded=(flags & (1 << 1)), power=power, encoder=encoder, speed=speed) self.pub_enc_l.publish(Float64(data=encoder)) (flags, power, encoder, speed) = self.g.get_motor_status(self.MR) status_right = MotorStatus(low_voltage=(flags & (1 << 0)), overloaded=(flags & (1 << 1)), power=power, encoder=encoder, speed=speed) self.pub_enc_r.publish(Float64(data=encoder)) self.pub_motor_status.publish( MotorStatusLR(header=Header(stamp=rospy.Time.now()), left=status_left, right=status_right)) # publish current pose (odom, transform) = self.odometry(status_left, status_right) self.pub_odometry.publish(odom) self.br.sendTransformMessage(transform) rate.sleep() self.g.reset_all() self.g.offset_motor_encoder(self.ML, self.g.get_motor_encoder(self.ML)) self.g.offset_motor_encoder(self.MR, self.g.get_motor_encoder(self.MR)) # deactivate power management os.write(self.gpio_value, "0".encode()) os.close(self.gpio_value) # unexport pin if os.path.isdir("/sys/class/gpio/gpio" + self.POWER_PIN): gpio_export = os.open("/sys/class/gpio/unexport", os.O_WRONLY) os.write(gpio_export, self.POWER_PIN.encode()) os.close(gpio_export) def set_servo_angle(self, servo, angle): # map angle from [-pi/2,+pi/2] to pulse width [pulse_min,pulse_max] angle = np.clip(angle, -np.pi / 2, np.pi / 2) # normalise to range [0,1] pos_norm = (1 + angle / (np.pi / 2)) / 2.0 pulse = self.PULSE_RANGE[0] + pos_norm * (self.PULSE_RANGE[1] - self.PULSE_RANGE[0]) # set servo position by pulse width pulse = np.rint(pulse).astype(np.int) pulse = np.clip(pulse, self.PULSE_RANGE[0], self.PULSE_RANGE[1]) self.g.set_servo(servo, pulse) # publish servo position as joint servo_names = {self.S1: "servo1", self.S2: "servo2"} self.pub_joints.publish( JointState(name=[servo_names[servo]], position=[angle])) def reset_odometry(self): self.g.offset_motor_encoder(self.ML, self.g.get_motor_encoder(self.ML)) self.g.offset_motor_encoder(self.MR, self.g.get_motor_encoder(self.MR)) self.last_encoders = {'l': 0, 'r': 0} self.pose = PoseWithCovariance() self.pose.pose.orientation.w = 1 def reset(self, req): self.g.reset_all() self.reset_odometry() return [True, ""] def power_on(self, req): os.write(self.gpio_value, "1".encode()) return [True, "Power ON"] def power_off(self, req): os.write(self.gpio_value, "0".encode()) return [True, "Power OFF"] def on_twist(self, twist): # Compute left and right wheel speed from a twist, which is the combination # of a linear speed (m/s) and an angular speed (rad/s). # In the coordinate frame of the GoPiGo3, the x-axis is pointing forward # and the z-axis is pointing upwards. Since the GoPiGo3 is only moving within # the x-y-plane, we are only using the linear velocity in x direction (forward) # and the angular velocity around the z-axis (yaw). # source: # https://opencurriculum.org/5481/circular-motion-linear-and-angular-speed/ # http://www.euclideanspace.com/physics/kinematics/combinedVelocity/index.htm right_speed = twist.linear.x + twist.angular.z * self.WIDTH / 2 left_speed = twist.linear.x - twist.angular.z * self.WIDTH / 2 self.g.set_motor_dps(self.ML, left_speed / self.CIRCUMFERENCE * 360) self.g.set_motor_dps(self.MR, right_speed / self.CIRCUMFERENCE * 360) def odometry(self, left, right): lspeed = left.speed / 360.0 * self.CIRCUMFERENCE rspeed = right.speed / 360.0 * self.CIRCUMFERENCE # Compute current linear and angular speed from wheel speed twist = TwistWithCovariance() twist.twist.linear.x = (rspeed + lspeed) / 2.0 twist.twist.angular.z = (rspeed - lspeed) / self.WIDTH # Compute position and orientation from travelled distance per wheel # http://www8.cs.umu.se/kurser/5DV122/HT13/material/Hellstrom-ForwardKinematics.pdf # position change per wheel in meter dl = (left.encoder - self.last_encoders['l']) / 360.0 * self.CIRCUMFERENCE dr = (right.encoder - self.last_encoders['r']) / 360.0 * self.CIRCUMFERENCE # set previous encoder state self.last_encoders['l'] = left.encoder self.last_encoders['r'] = right.encoder angle = (dr - dl) / self.WIDTH linear = 0.5 * (dl + dr) if dr != dl: radius = self.WIDTH / 2.0 * (dl + dr) / (dr - dl) else: radius = 0 # old state old_angle = 2 * np.arccos(self.pose.pose.orientation.w) old_pos = np.array( [self.pose.pose.position.x, self.pose.pose.position.y]) # update state new_angle = (old_angle + angle) % (2 * np.pi) new_q = quaternion_about_axis(new_angle, (0, 0, 1)) new_angle2 = 2 * np.arccos(self.pose.pose.orientation.w) print("new_angle2", new_angle2) new_pos = np.zeros((2, )) if abs(angle) < 1e-6: direction = old_angle + angle * 0.5 dx = linear * np.cos(direction) dy = linear * np.sin(direction) else: dx = +radius * (np.sin(new_angle) - np.sin(old_angle)) dy = -radius * (np.cos(new_angle) - np.cos(old_angle)) new_pos[0] = old_pos[0] + dx new_pos[1] = old_pos[1] + dy self.pose.pose.orientation.x = new_q[0] self.pose.pose.orientation.y = new_q[1] self.pose.pose.orientation.z = new_q[2] self.pose.pose.orientation.w = new_q[3] self.pose.pose.position.x = new_pos[0] self.pose.pose.position.y = new_pos[1] odom = Odometry(header=Header(stamp=rospy.Time.now(), frame_id="odom"), child_frame_id="base_link", pose=self.pose, twist=twist) transform = TransformStamped(header=Header(stamp=rospy.Time.now(), frame_id="odom"), child_frame_id="base_link") transform.transform.translation.x = self.pose.pose.position.x transform.transform.translation.y = self.pose.pose.position.y transform.transform.translation.z = self.pose.pose.position.z transform.transform.rotation = self.pose.pose.orientation return odom, transform
class Robot: # short variables ML = gopigo3.GoPiGo3.MOTOR_LEFT MR = gopigo3.GoPiGo3.MOTOR_RIGHT S1 = gopigo3.GoPiGo3.SERVO_1 S2 = gopigo3.GoPiGo3.SERVO_2 BL = gopigo3.GoPiGo3.LED_BLINKER_LEFT BR = gopigo3.GoPiGo3.LED_BLINKER_RIGHT EL = gopigo3.GoPiGo3.LED_EYE_LEFT ER = gopigo3.GoPiGo3.LED_EYE_RIGHT EW = gopigo3.GoPiGo3.LED_WIFI WIDTH = gopigo3.GoPiGo3.WHEEL_BASE_WIDTH CIRCUMFERENCE = gopigo3.GoPiGo3.WHEEL_CIRCUMFERENCE*1e-3 POWER_PIN = "23" def __init__(self): #### GoPiGo3 power management # export pin if not os.path.isdir("/sys/class/gpio/gpio"+self.POWER_PIN): gpio_export = os.open("/sys/class/gpio/export", os.O_WRONLY) os.write(gpio_export, self.POWER_PIN.encode()) os.close(gpio_export) time.sleep(0.1) # set pin direction gpio_direction = os.open("/sys/class/gpio/gpio"+self.POWER_PIN+"/direction", os.O_WRONLY) os.write(gpio_direction, "out".encode()) os.close(gpio_direction) # activate power management self.gpio_value = os.open("/sys/class/gpio/gpio"+self.POWER_PIN+"/value", os.O_WRONLY) os.write(self.gpio_value, "1".encode()) # GoPiGo3 and ROS setup self.g = gopigo3.GoPiGo3() print("GoPiGo3 info:") print("Manufacturer : ", self.g.get_manufacturer()) print("Board : ", self.g.get_board()) print("Serial Number : ", self.g.get_id()) print("Hardware version: ", self.g.get_version_hardware()) print("Firmware version: ", self.g.get_version_firmware()) self.reset_odometry() rospy.init_node("gopigo3") self.br = TransformBroadcaster() # subscriber rospy.Subscriber("motor/dps/left", Int16, lambda msg: self.g.set_motor_dps(self.ML, msg.data)) rospy.Subscriber("motor/dps/right", Int16, lambda msg: self.g.set_motor_dps(self.MR, msg.data)) rospy.Subscriber("motor/pwm/left", Int8, lambda msg: self.g.set_motor_power(self.ML, msg.data)) rospy.Subscriber("motor/pwm/right", Int8, lambda msg: self.g.set_motor_power(self.MR, msg.data)) rospy.Subscriber("motor/position/left", Int16, lambda msg: self.g.set_motor_position(self.ML, msg.data)) rospy.Subscriber("motor/position/right", Int16, lambda msg: self.g.set_motor_position(self.MR, msg.data)) rospy.Subscriber("servo/1", Float64, lambda msg: self.g.set_servo(self.S1, msg.data*16666)) rospy.Subscriber("servo/2", Float64, lambda msg: self.g.set_servo(self.S2, msg.data*16666)) rospy.Subscriber("cmd_vel", Twist, self.on_twist) rospy.Subscriber("led/blinker/left", UInt8, lambda msg: self.g.set_led(self.BL, msg.data)) rospy.Subscriber("led/blinker/right", UInt8, lambda msg: self.g.set_led(self.BR, msg.data)) rospy.Subscriber("led/eye/left", ColorRGBA, lambda c: self.g.set_led(self.EL, int(c.r*255), int(c.g*255), int(c.b*255))) rospy.Subscriber("led/eye/right", ColorRGBA, lambda c: self.g.set_led(self.ER, int(c.r*255), int(c.g*255), int(c.b*255))) rospy.Subscriber("led/wifi", ColorRGBA, lambda c: self.g.set_led(self.EW, int(c.r * 255), int(c.g * 255), int(c.b * 255))) # publisher self.pub_enc_l = rospy.Publisher('motor/encoder/left', Float64, queue_size=10) self.pub_enc_r = rospy.Publisher('motor/encoder/right', Float64, queue_size=10) self.pub_battery = rospy.Publisher('battery_voltage', Float64, queue_size=10) self.pub_motor_status = rospy.Publisher('motor/status', MotorStatusLR, queue_size=10) self.pub_odometry = rospy.Publisher("odometry", Odometry, queue_size=10) # services self.srv_reset = rospy.Service('reset', Trigger, self.reset) self.srv_spi = rospy.Service('spi', SPI, lambda req: SPIResponse(data_in=self.g.spi_transfer_array(req.data_out))) self.srv_pwr_on = rospy.Service('power/on', Trigger, self.power_on) self.srv_pwr_off = rospy.Service('power/off', Trigger, self.power_off) # main loop rate = rospy.Rate(10) # in Hz while not rospy.is_shutdown(): self.pub_enc_l.publish(Float64(data=self.g.get_motor_encoder(self.ML))) self.pub_enc_r.publish(Float64(data=self.g.get_motor_encoder(self.MR))) self.pub_battery.publish(Float64(data=self.g.get_voltage_battery())) # publish motor status, including encoder value (flags, power, encoder, speed) = self.g.get_motor_status(self.ML) status_left = MotorStatus(low_voltage=(flags & (1<<0)), overloaded=(flags & (1<<1)), power=power, encoder=encoder, speed=speed) (flags, power, encoder, speed) = self.g.get_motor_status(self.MR) status_right = MotorStatus(low_voltage=(flags & (1<<0)), overloaded=(flags & (1<<1)), power=power, encoder=encoder, speed=speed) self.pub_motor_status.publish(MotorStatusLR(header=Header(stamp=rospy.Time.now()), left=status_left, right=status_right)) # publish current pose (odom, transform)= self.odometry(status_left, status_right) self.pub_odometry.publish(odom) self.br.sendTransformMessage(transform) rate.sleep() self.g.reset_all() # deactivate power management os.write(self.gpio_value, "0".encode()) os.close(self.gpio_value) # unexport pin if os.path.isdir("/sys/class/gpio/gpio" + self.POWER_PIN): gpio_export = os.open("/sys/class/gpio/unexport", os.O_WRONLY) os.write(gpio_export, self.POWER_PIN.encode()) os.close(gpio_export) def reset_odometry(self): self.last_encoders = {'l': 0, 'r': 0} self.pose = PoseWithCovariance() self.pose.pose.orientation.w = 1 def reset(self, req): self.g.reset_all() self.reset_odometry() return [True, ""] def power_on(self, req): os.write(self.gpio_value, "1".encode()) return [True, "Power ON"] def power_off(self, req): os.write(self.gpio_value, "0".encode()) return [True, "Power OFF"] def on_twist(self, twist): # Compute left and right wheel speed from a twist, which is the combination # of a linear speed (m/s) and an angular speed (rad/s). # In the coordinate frame of the GoPiGo3, the x-axis is pointing forward # and the z-axis is pointing upwards. Since the GoPiGo3 is only moving within # the x-y-plane, we are only using the linear velocity in x direction (forward) # and the angular velocity around the z-axis (yaw). # source: # https://opencurriculum.org/5481/circular-motion-linear-and-angular-speed/ # http://www.euclideanspace.com/physics/kinematics/combinedVelocity/index.htm right_speed = twist.linear.x + twist.angular.z * self.WIDTH / 2 left_speed = twist.linear.x - twist.angular.z * self.WIDTH / 2 self.g.set_motor_dps(self.ML, left_speed/self.CIRCUMFERENCE*360) self.g.set_motor_dps(self.MR, right_speed/self.CIRCUMFERENCE*360) def odometry(self, left, right): # Compute current linear and angular speed from wheel speed twist = TwistWithCovariance() twist.twist.linear.x = (right.speed - left.speed) / 2.0 twist.twist.angular.z = (right.speed - left.speed) / self.WIDTH # Compute position and orientation from travelled distance per wheel # http://www8.cs.umu.se/kurser/5DV122/HT13/material/Hellstrom-ForwardKinematics.pdf # position change per wheel in meter dl = (left.encoder - self.last_encoders['l']) / 360.0 * self.CIRCUMFERENCE dr = (right.encoder - self.last_encoders['r']) / 360.0 * self.CIRCUMFERENCE if dr!=dl: radius = self.WIDTH/2.0 * (dl+dr) / (dr-dl) angle = (dr-dl) / self.WIDTH else: radius = 0 angle = 0 # new position pos = np.array([self.pose.pose.position.x, self.pose.pose.position.y]) icc = pos + radius * np.array([-np.sin(angle), np.cos(angle)]) new_pos = np.dot(np.array([[np.cos(angle), -np.sin(angle)],[np.sin(angle), np.cos(angle)]]), pos-icc) + icc # update pose new_q = quaternion_multiply( quaternion_about_axis(angle, (0, 0, 1)), [self.pose.pose.orientation.x, self.pose.pose.orientation.y, self.pose.pose.orientation.z, self.pose.pose.orientation.w]) self.pose.pose.orientation.x = new_q[0] self.pose.pose.orientation.y = new_q[1] self.pose.pose.orientation.z = new_q[2] self.pose.pose.orientation.w = new_q[3] self.pose.pose.position.x = new_pos[0] self.pose.pose.position.y = new_pos[1] # set previous encoder state self.last_encoders['l'] = left.encoder self.last_encoders['r'] = right.encoder odom = Odometry(header=Header(frame_id="gopigo"), child_frame_id="world", pose=self.pose, twist=twist) transform = TransformStamped(header=Header(frame_id="gopigo"), child_frame_id="world") transform.transform.translation.x = self.pose.pose.position.x transform.transform.translation.y = self.pose.pose.position.y transform.transform.translation.z = self.pose.pose.position.z transform.transform.rotation = self.pose.pose.orientation return odom, transform