def setLidarRpm(rpm):
m = lcmmultisense.command_t()
m.utime = getUtime()
m.fps = -1
m.gain = -1
m.agc = -1
m.rpm = rpm
lcmUtils.publish('MULTISENSE_COMMAND', m)
def setLidarRpm(rpm):
m = lcmmultisense.command_t()
m.utime = getUtime()
m.fps = -1
m.gain = -1
m.agc = -1
m.rpm = rpm
lcmUtils.publish('MULTISENSE_COMMAND', m)
def setLidarRevolutionTime(secondsPerRevolution):
assert secondsPerRevolution >= 1.0
m = lcmmultisense.command_t()
m.utime = getUtime()
m.fps = -1
m.gain = -1
m.agc = -1
m.rpm = 60.0 / (secondsPerRevolution)
lcmUtils.publish('MULTISENSE_COMMAND', m)
def setLidarRevolutionTime(secondsPerRevolution):
assert secondsPerRevolution >= 1.0
m = lcmmultisense.command_t()
m.utime = getUtime()
m.fps = -1
m.gain = -1
m.agc = -1
m.rpm = 60.0 / (secondsPerRevolution)
lcmUtils.publish('MULTISENSE_COMMAND', m)
def sendMultisenseCommand(fps, gain, exposure, agc, rpm, led_flash, led_duty):
m = lcmmultisense.command_t()
m.utime = getUtime()
m.fps = fps
m.gain = gain
m.exposure_us = exposure
m.agc = agc
m.rpm = rpm
m.leds_flash = led_flash
m.leds_duty_cycle = led_duty
lcmUtils.publish('MULTISENSE_COMMAND', m)
def sendMultisenseCommand(fps, gain, exposure, agc, rpm, led_flash, led_duty):
m = lcmmultisense.command_t()
m.utime = getUtime()
m.fps = fps
m.gain = gain
m.exposure_us = exposure
m.agc = agc
m.rpm = rpm
m.leds_flash = led_flash
m.leds_duty_cycle = led_duty
lcmUtils.publish('MULTISENSE_COMMAND', m)
msg.pose.translation.z = 1.2
msg.joint_position[0] = 0
orientation = euler_to_quat([0, 0.0*np.pi/180.0,0])
msg.pose.rotation.w = orientation[0]
msg.pose.rotation.x = orientation[1]
msg.pose.rotation.y = orientation[2]
msg.pose.rotation.z = orientation[3]
lc.publish("EST_ROBOT_STATE", msg.encode())
msg2.pos = [msg.pose.translation.x, msg.pose.translation.y, msg.pose.translation.z]
msg2.orientation = [msg.pose.rotation.w, msg.pose.rotation.x, msg.pose.rotation.y, msg.pose.rotation.z]
lc.publish("POSE_BODY", msg2.encode())
if rotation_flag == 'rotate':
time.sleep(5)
msg3 = lcmmultisense.command_t()
msg3.utime = timestamp_now()
msg3.fps = 15
msg3.gain = -1
msg3.exposure_us = 10000
msg3.agc = -1
msg3.rpm = 5
msg3.leds_flash = False
msg3.leds_duty_cycle = 0
lc.publish("MULTISENSE_COMMAND", msg3.encode())
print "Publishing Multisense command to spin at 5rpm"
else:
for i in range(0,1000):
i
j=i/1000.0
msg.pose.rotation.y = orientation[2]
msg.pose.rotation.z = orientation[3]
lc.publish("EST_ROBOT_STATE", msg.encode())
msg2.pos = [
msg.pose.translation.x, msg.pose.translation.y, msg.pose.translation.z
]
msg2.orientation = [
msg.pose.rotation.w, msg.pose.rotation.x, msg.pose.rotation.y,
msg.pose.rotation.z
]
lc.publish("POSE_BODY", msg2.encode())
if rotation_flag == 'rotate':
time.sleep(5)
msg3 = lcmmultisense.command_t()
msg3.utime = timestamp_now()
msg3.fps = 15
msg3.gain = -1
msg3.exposure_us = 10000
msg3.agc = -1
msg3.rpm = 5
msg3.leds_flash = False
msg3.leds_duty_cycle = 0
lc.publish("MULTISENSE_COMMAND", msg3.encode())
print "Publishing Multisense command to spin at 5rpm"
else:
for i in range(0, 1000):
i
j = i / 1000.0
