def clean_shutdown():
if self.firstShutdown:
print("STOPPING TRAJECTORY")
traj.stop_trajectory()
traj.clear_waypoints()
l = Lights()
l.set_light_state('head_green_light', False)
self.firstShutdown = False
class Head_Light(object):
BLINK_DURATION = 0.7 # Second of blinking on and off
def __init__(self):
rospy.on_shutdown(self.light_off)
self.l = Lights()
#self.robot_light_fear = ['head_green_light', 'head_red_light']
self.robot_light_fear = ['head_blue_light']
self.robot_light_hope = ['head_green_light']
self.robot_light_error = ['head_red_light']
self.robot_all_leds = [
'head_red_light', 'head_blue_light', 'head_green_light'
]
self.state_error_light = False
self.robot_state = 0 # normal
rospy.Subscriber("cs_sawyer/head_light", UInt8,
self.callback_update_light)
def on_off(self, leds, on=True):
for light in self.robot_all_leds:
if light not in leds:
self.l.set_light_state(light, False)
else:
self.l.set_light_state(light, on)
def blink(self, leds):
self.state_error_light = not self.state_error_light
self.on_off(leds, self.state_error_light)
time.sleep(self.BLINK_DURATION)
def callback_update_light(self, msg):
self.robot_state = msg.data
def update_leds(self):
if self.robot_state == 0: # normal
self.on_off(self.robot_all_leds)
elif self.robot_state == 1: # error
self.blink(self.robot_light_error)
elif self.robot_state == 2: # hope
self.on_off(self.robot_light_hope)
elif self.robot_state == 3: #fear
self.on_off(self.robot_light_fear)
def run(self):
rate = rospy.Rate(5)
while not rospy.is_shutdown():
self.update_leds()
rate.sleep()
def light_off(self):
self.on_off(self.robot_all_leds, False)
def test_light_interface(light_name='head_green_light'):
"""Blinks a desired Light on then off."""
l = Lights()
rospy.loginfo("All available lights on this robot:\n{0}\n".format(
', '.join(l.list_all_lights())))
rospy.loginfo("Blinking Light: {0}".format(light_name))
initial_state = l.get_light_state(light_name)
on_off = lambda x: 'ON' if l.get_light_state(x) else 'OFF'
rospy.loginfo("Initial state: {0}".format(on_off(light_name)))
# invert light
state = not initial_state
l.set_light_state(light_name, state)
rospy.sleep(1)
rospy.loginfo("New state: {0}".format(on_off(light_name)))
# invert light
state = not state
l.set_light_state(light_name, state)
rospy.sleep(1)
rospy.loginfo("New state: {0}".format(on_off(light_name)))
# invert light
state = not state
l.set_light_state(light_name, state)
rospy.sleep(1)
rospy.loginfo("New state: {0}".format(on_off(light_name)))
# reset output
l.set_light_state(light_name, initial_state)
rospy.sleep(1)
rospy.loginfo("Final state: {0}".format(on_off(light_name)))
class GripperConnect(object):
"""
Connects wrist button presses to gripper open/close commands.
Uses the Navigator callback feature to make callbacks to connected
action functions when the button values change.
"""
def __init__(self, arm, lights=True):
"""
@type arm: str
@param arm: arm of gripper to control
@type lights: bool
@param lights: if lights should activate on cuff grasp
"""
self._arm = arm
# inputs
self._cuff = Cuff(limb=arm)
# connect callback fns to signals
self._lights = None
if lights:
self._lights = Lights()
self._cuff.register_callback(self._light_action,
'{0}_cuff'.format(arm))
try:
self._gripper = get_current_gripper_interface()
self._is_clicksmart = isinstance(self._gripper,
SimpleClickSmartGripper)
if self._is_clicksmart:
if self._gripper.needs_init():
self._gripper.initialize()
else:
if not (self._gripper.is_calibrated()
or self._gripper.calibrate() == True):
raise
self._cuff.register_callback(self._close_action,
'{0}_button_upper'.format(arm))
self._cuff.register_callback(self._open_action,
'{0}_button_lower'.format(arm))
rospy.loginfo("{0} Cuff Control initialized...".format(
self._gripper.name))
except:
self._gripper = None
self._is_clicksmart = False
msg = ("{0} Gripper is not connected to the robot."
" Running cuff-light connection only.").format(
arm.capitalize())
rospy.logwarn(msg)
def _open_action(self, value):
if value and self._gripper.is_ready():
rospy.logdebug("gripper open triggered")
if self._is_clicksmart:
self._gripper.set_ee_signal_value('grip', False)
else:
self._gripper.open()
if self._lights:
self._set_lights('red', False)
self._set_lights('green', True)
def _close_action(self, value):
if value and self._gripper.is_ready():
rospy.logdebug("gripper close triggered")
if self._is_clicksmart:
self._gripper.set_ee_signal_value('grip', True)
else:
self._gripper.close()
if self._lights:
self._set_lights('green', False)
self._set_lights('red', True)
def _light_action(self, value):
if value:
rospy.logdebug("cuff grasp triggered")
else:
rospy.logdebug("cuff release triggered")
if self._lights:
self._set_lights('red', False)
self._set_lights('green', False)
self._set_lights('blue', value)
def _set_lights(self, color, value):
self._lights.set_light_state('head_{0}_light'.format(color),
on=bool(value))
self._lights.set_light_state('{0}_hand_{1}_light'.format(
self._arm, color),
on=bool(value))
class RecordMotion(object):
def __init__(self, arm, name, start):
uri = "mongodb://*****:*****@sawyer-mongo.documents.azure.com:10255/?ssl=true"
client = MongoClient(uri)
self.db = client.SawyerDB
self.collection = self.db.Commands
self.commandName = name
self.commandNumber = start
rp = RobotParams()
self._lastJoin = {}
self.lastButtonPress = datetime.datetime.now()
self._rs = intera_interface.RobotEnable()
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
self._navigator_io = intera_interface.Navigator()
head_display = intera_interface.HeadDisplay()
head_display.display_image(
"/home/microshak/Pictures/Sawyer_Navigator_Main.png", False, 1.0)
valid_limbs = rp.get_limb_names()
self._arm = rp.get_limb_names()[0]
self._limb = intera_interface.Limb(arm)
if start == None:
limb = intera_interface.Limb("right")
limb.move_to_neutral()
print(self._arm)
# inputs
self._cuff = Cuff(limb='right')
self._navigator = Navigator()
# connect callback fns to signals
self._lights = None
self._lights = Lights()
self._cuff.register_callback(self._light_action,
'{0}_cuff'.format('right'))
try:
self._gripper = Gripper(self._arm)
#if not (self._gripper.is_calibrated() or self._gripper.calibrate() == True):
# rospy.logerr("({0}_gripper) calibration failed.".format(self._gripper.name))
# raise
except:
self._gripper = None
msg = ("{0} Gripper is not connected to the robot."
" Running cuff-light connection only.").format(
arm.capitalize())
rospy.logwarn(msg)
def clean_shutdown(self):
print("\nExiting example...")
if not self._init_state:
print("Disabling robot...")
# self._rs.disable()
return True
def record(self):
self._cuff.register_callback(self._close_action,
'{0}_button_upper'.format(self._arm))
self._cuff.register_callback(self._open_action,
'{0}_button_lower'.format(self._arm))
print "Registering COntrols"
ok_id = self._navigator.register_callback(
self._record_spot, 'right_button_ok'
) # The circular button in the middle of the navigator.
circle_id = self._navigator.register_callback(
self._record_OK, 'right_button_circle'
) #The button above the OK button, typically with a 'Back' arrow symbol.
show_id = self._navigator.register_callback(
self._record_start, 'right_button_show'
) #The "Rethink Button", is above the OK button, next to back button and typically is labeled with the Rethink logo.
while not rospy.is_shutdown():
rospy.sleep(0.1)
self._navigator.deregister_callback(ok_id)
self._navigator.deregister_callback(circle_id)
self._navigator.deregister_callback(show_id)
def _record_start(self, value):
self.headLight("green")
def _record_OK(self, value):
self.headLight("red")
print "we cool"
def _record_spot(self, value):
time = (datetime.datetime.now() - self.lastButtonPress).seconds
print time
print "!!!!!!!!!!!!!"
if time < 2:
print "time to bail"
return
self.lastButtonPress = datetime.datetime.now()
print "spot record"
self.headLight("red")
self.commandNumber += 1
posts = self.db.Command
joints = {}
names = self._limb.joint_names()
for join in names:
joints.update({join: self._limb.joint_angle(join)})
if joints == self._lastJoin:
return 0
self._lastJoin = joints
post = {
"Name": self.commandName,
"Order": self.commandNumber,
"Action": "Move",
"Joints": joints
}
posts.insert(post)
print post
self.headLight("green")
def _open_action(self, value):
self.headLight("red")
if value and self._gripper.is_ready():
self._gripper.open()
self.commandNumber += 1
posts = self.db.Command
post = {
"Name": self.commandName,
"Order": self.commandNumber,
"Action": "Gripper",
"Open": True
}
posts.insert(post)
self.headLight("green")
def _close_action(self, value):
self.headLight("red")
if value and self._gripper.is_ready():
rospy.logdebug("gripper close triggered")
self._gripper.close()
self.commandNumber += 1
posts = self.db.Command
post = {
"Name": self.commandName,
"Order": self.commandNumber,
"Action": "Gripper",
"Open": False
}
posts.insert(post)
self.headLight("green")
def _light_action(self, value):
if value:
rospy.logdebug("cuff grasp triggered")
else:
rospy.logdebug("cuff release triggered")
if self._lights:
self._set_lights('red', False)
self._set_lights('green', False)
self._set_lights('blue', value)
def _set_lights(self, color, value):
self._lights.set_light_state('head_{0}_light'.format(color),
on=bool(value))
self._lights.set_light_state('{0}_hand_{1}_light'.format(
self._arm, color),
on=bool(value))
def headLight(self, value):
colors = ["red", "blue", "green"]
for color in colors:
self._lights.set_light_state('head_{0}_light'.format(color),
on=bool(value == color))
def finalize(self):
print "ENDING!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
class GripperConnect(object):
def __init__(self, arm, lights=True):
self._arm = arm
self._cuff = Cuff(limb=arm)
# connect callback fns to signals
self._lights = None
if lights:
self._lights = Lights()
self._cuff.register_callback(self._light_action,
'{0}_cuff'.format(arm))
try:
self._gripper = get_current_gripper_interface()
self._is_clicksmart = isinstance(self._gripper,
SimpleClickSmartGripper)
if self._is_clicksmart:
if self._gripper.needs_init():
self._gripper.initialize()
else:
if not (self._gripper.is_calibrated()
or self._gripper.calibrate() == True):
raise
self._cuff.register_callback(self._close_action,
'{0}_button_upper'.format(arm))
self._cuff.register_callback(self._open_action,
'{0}_button_lower'.format(arm))
rospy.loginfo("{0} Cuff Control initialized...".format(
self._gripper.name))
except:
self._gripper = None
self._is_clicksmart = False
msg = ("{0} Gripper is not connected to the robot."
" Running cuff-light connection only.").format(
arm.capitalize())
rospy.logwarn(msg)
def _open_action(self, value):
if value and self._gripper.is_ready():
rospy.logdebug("gripper open triggered")
if self._is_clicksmart:
self._gripper.set_ee_signal_value('grip', False)
else:
self._gripper.open()
if self._lights:
self._set_lights('red', False)
self._set_lights('green', True)
def _close_action(self, value):
if value and self._gripper.is_ready():
rospy.logdebug("gripper close triggered")
if self._is_clicksmart:
self._gripper.set_ee_signal_value('grip', True)
else:
self._gripper.close()
if self._lights:
self._set_lights('green', False)
self._set_lights('red', True)
def _light_action(self, value):
if value:
rospy.logdebug("cuff grasp triggered")
else:
rospy.logdebug("cuff release triggered")
if self._lights:
self._set_lights('red', False)
self._set_lights('green', False)
self._set_lights('blue', value)
def _set_lights(self, color, value):
self._lights.set_light_state('head_{0}_light'.format(color),
on=bool(value))
self._lights.set_light_state('{0}_hand_{1}_light'.format(
self._arm, color),
on=bool(value))
def _handle_message(self, msg):
flag_close = msg.status
rospy.loginfo(rospy.get_caller_id() + " got message : %i", flag_close)
if flag_close:
print('action - close gripper')
self._close_action(1)
else:
print('action - open gripper')
self._open_action(1)
def run(self):
rate = rospy.Rate(100)
limb = Limb()
traj_options = TrajectoryOptions()
traj_options.interpolation_type = TrajectoryOptions.CARTESIAN
traj = MotionTrajectory(trajectory_options=traj_options, limb=limb)
wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=0.5,
max_joint_accel=0.5,
corner_distance=0.05)
waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb)
self.pose.header = Header(stamp=rospy.Time.now(), frame_id='base')
self.pose.pose.position.x = 0.0
self.pose.pose.position.y = -0.6
self.pose.pose.position.z = 0.5
self.pose.pose.orientation.x = 0.5
self.pose.pose.orientation.y = -0.5
self.pose.pose.orientation.z = 0.5
self.pose.pose.orientation.w = 0.5
joint_angles = limb.joint_ordered_angles()
waypoint.set_cartesian_pose(self.pose, "right_hand", joint_angles)
self.waypoints.append(waypoint)
rospy.loginfo("Sending inital waypoint: %s",
self.waypoints[0].to_string())
traj.append_waypoint(self.waypoints[0].to_msg())
result = traj.send_trajectory()
if result is None:
rospy.logerr('Trajectory FAILED to send')
elif result.result:
rospy.loginfo(
'Motion controller successfully finished the trajectory!')
else:
rospy.logerr(
'Motion controller failed to complete the trajectory with error %s',
result.errorId)
traj.clear_waypoints()
l = Lights()
l_name = 'head_green_light'
initial_state = l.get_light_state(l_name)
for i in range(0, 2):
state = not initial_state
l.set_light_state(l_name, state)
rospy.sleep(0.5)
state = not state
l.set_light_state(l_name, state)
rospy.sleep(0.5)
l.set_light_state(l_name, True)
for i in range(0, 19):
self.gen_rand_waypoint()
sendCommand = True
while not rospy.is_shutdown():
traj.clear_waypoints()
for i in range(0, 19):
self.waypoints.pop(i)
self.gen_rand_waypoint()
for point in self.waypoints:
traj.append_waypoint(point.to_msg())
print(len(self.waypoints))
result = traj.send_trajectory(wait_for_result=True)
self.firstShutdown = True
def clean_shutdown():
if self.firstShutdown:
print("STOPPING TRAJECTORY")
traj.stop_trajectory()
traj.clear_waypoints()
l = Lights()
l.set_light_state('head_green_light', False)
self.firstShutdown = False
rospy.on_shutdown(clean_shutdown)
rate.sleep()
return
class PlayCommands(object):
def __init__(self, Name):
CONNECTION_STRING = "HostName=RobotForman.azure-devices.net;DeviceId=PythonTest;SharedAccessKey=oh9Fj0mAMWJZpNNyeJ+bSecVH3cBQwbzjDnoVmeSV5g="
self.protocol = IoTHubTransportProvider.MQTT
self.client = IoTHubClient(CONNECTION_STRING, self.protocol)
self.client.set_option("messageTimeout", MESSAGE_TIMEOUT)
self.client.set_message_callback(receive_message_callback,
RECEIVE_CONTEXT)
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('mid' + str(uuid.uuid4().hex), anonymous=True)
print 'mid' + str(uuid.uuid4().hex)
self.head_display = intera_interface.HeadDisplay()
self.head_display.display_image("/home/microshak/Pictures/Ready.png",
False, 1.0)
self.head = intera_interface.Head()
rp = RobotParams()
valid_limbs = rp.get_limb_names()
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
self.group = moveit_commander.MoveGroupCommander("right_arm")
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
self.light = Lights()
self.headLight("green")
# rs = intera_interface.RobotEnable(CHECK_VERSION)
self.group.clear_pose_targets()
self.endeffector = intera_interface.Gripper("right")
self.uri = "mongodb://*****:*****@sawyer-mongo.documents.azure.com:10255/?ssl=true"
self.Mongoclient = MongoClient(self.uri)
if Name == None:
self.poleIoTHub()
else:
self.completeCommands(Name)
def poleIoTHub(self):
while True:
if len(IoTHubMessages) > 0:
self.handleIoT()
rospy.sleep(2)
def completeCommands(self, Name):
db = self.Mongoclient.SawyerDB
collection = db.Command
data = collection.find({"Name": Name}).sort("Order", pymongo.ASCENDING)
switch = {
"Move": lambda x: self.move(x),
"Gripper": lambda x: self.gripper(x)
}
print "============Start"
rospy.sleep(1)
ps = self.group.get_current_pose("right_gripper")
self.neutral()
for record in data:
print IoTHubMessages
if len(IoTHubMessages) > 0:
self.handleIoT()
self.headLight("green")
rospy.sleep(1)
switch[record["Action"]](record)
## When finished shut down moveit_commander.
moveit_commander.roscpp_shutdown()
def neutral(self):
limb = intera_interface.Limb("right")
limb.move_to_neutral()
self.headLight("blue")
self.head_display.display_image("/home/microshak/Pictures/Neutral.png",
False, 1.0)
def headLight(self, value):
colors = ["red", "blue", "green"]
for color in colors:
self.light.set_light_state('head_{0}_light'.format(color),
on=bool(value == color))
def gripper(self, data):
if data["Open"]:
self.endeffector.open()
self.head_display.display_image(
"/home/microshak/Pictures/GripperO.png", False, 1.0)
else:
self.endeffector.close()
self.head_display.display_image(
"/home/microshak/Pictures/GripperC.png", False, 1.0)
def handleIoT(self):
while len(IoTHubMessages) > 0:
self.headLight("red")
rospy.sleep(1)
message = IoTHubMessages.pop()
print message
if (message["Action"] == "Run"):
# message = IoTHubMessages.pop()
self.completeCommands("Demo")
if (message["Action"] == "Neutral"):
self.neutral()
if (message["Action"] == "Stop"):
stop = True
self.head_display.display_image(
"/home/microshak/Pictures/Stop.png", False, 1.0)
while stop:
if len(IoTHubMessages) > 0:
message = IoTHubMessages.pop()
if message["Action"] == "Continue":
stop = False
self.head_display.display_image(
"/home/microshak/Pictures/Resume.png", False,
1.0)
'''
def receive_message_callback(message, counter):
global RECEIVE_CALLBACKS
print "Listening"
message_buffer = message.get_bytearray()
size = len(message_buffer)
lit = ast.literal_eval(message_buffer[:size].decode('utf-8'))
for key in lit:
self.IotHubMessages.insert({key:lit[key]})
print key +"---" +str(lit[key])
counter += 1
RECEIVE_CALLBACKS += 1
return IoTHubMessageDispositionResult.ACCEPTED
'''
'''
def receive_message_callback(message):
global RECEIVE_CALLBACKS
message_buffer = message.get_bytearray()
size = len(message_buffer)
print ( "Received Message [%d]:" % counter )
print ( " Data: <<<%s>>> & Size=%d" % (message_buffer[:size].decode('utf-8'), size) )
map_properties = message.properties()
key_value_pair = map_properties.get_internals()
print ( " Properties: %s" % key_value_pair )
counter += 1
RECEIVE_CALLBACKS += 1
print ( " Total calls received: %d" % RECEIVE_CALLBACKS )
return IoTHubMessageDispositionResult.ACCEPTED
'''
def move(self, jointpos):
self.head_display.display_image("/home/microshak/Pictures/Moving.png",
False, 1.0)
print "MOVING!!!!!!!!!!!!!!!!!"
position = ast.literal_eval(json.dumps(jointpos['Joints']))
group = moveit_commander.MoveGroupCommander("right_arm")
group.set_joint_value_target(position)
plan2 = group.plan()
group.go(wait=True)
'''
def headLight(value):
colors = ["red", "blue", "green"]
light = Lights()
for color in colors:
light.set_light_state('head_{0}_light'.format(color),
on=bool(value == color))
class PlayCommands(object):
def __init__(self, Name):
CONNECTION_STRING = "HostName=RobotForman.azure-devices.net;DeviceId=PythonTest;SharedAccessKey=oh9Fj0mAMWJZpNNyeJ+bSecVH3cBQwbzjDnoVmeSV5g="
self.protocol = IoTHubTransportProvider.HTTP
self.client = IoTHubClient(CONNECTION_STRING, self.protocol)
self.client.set_option("messageTimeout", MESSAGE_TIMEOUT)
self.client.set_option("MinimumPollingTime", MINIMUM_POLLING_TIME)
self.client.set_message_callback(receive_message_callback,
RECEIVE_CONTEXT)
moveit_commander.roscpp_initialize(sys.argv)
print 'ROS INIT'
rospy.init_node('mid' + str(uuid.uuid4().hex), anonymous=True)
print 'mid' + str(uuid.uuid4().hex)
self.head_display = intera_interface.HeadDisplay()
self.headText("Hi")
self.head = intera_interface.Head()
rp = RobotParams()
valid_limbs = rp.get_limb_names()
robot = moveit_commander.RobotCommander()
rp.max_velocity_scaling_factor = .5
scene = moveit_commander.PlanningSceneInterface()
self.group = moveit_commander.MoveGroupCommander("right_arm")
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
self.light = Lights()
self.headLight("green")
# rs = intera_interface.RobotEnable(CHECK_VERSION)
self.group.clear_pose_targets()
self.endeffector = intera_interface.Gripper("right")
self.uri = "mongodb://*****:*****@sawyer-mongo.documents.azure.com:10255/?ssl=true"
self.Mongoclient = MongoClient(self.uri)
if Name == None:
self.poleIoTHub()
else:
self.completeCommands(Name)
def poleIoTHub(self):
while True:
if len(IoTHubMessages) > 0:
self.handleIoT()
#rospy.sleep(2)
def headText(self, text):
img = np.zeros((600, 1024, 3), np.uint8)
cv2.putText(img, "Hello World!!!", (50, 50), cv2.FONT_HERSHEY_SIMPLEX,
2, 255)
cv2.imwrite("/home/microshak/Pictures/head.png", img)
self.head_display.display_image("/home/microshak/Pictures/head.png",
False, 1.0)
def completeCommands(self, Name):
db = self.Mongoclient.SawyerDB
collection = db.Command
data = collection.find({"Name": Name}).sort("Order", pymongo.ASCENDING)
switch = {
"Move": lambda x: self.move(x),
"Gripper": lambda x: self.gripper(x)
}
print "============Start"
#rospy.sleep(1)
ps = self.group.get_current_pose("right_gripper")
self.neutral()
for record in data:
print IoTHubMessages
if len(IoTHubMessages) > 0:
self.handleIoT()
self.headLight("green")
#rospy.sleep(1)
switch[record["Action"]](record)
## When finished shut down moveit_commander.
moveit_commander.roscpp_shutdown()
def neutral(self):
limb = intera_interface.Limb("right")
limb.move_to_neutral()
self.headLight("blue")
self.head_display.display_image("/home/microshak/Pictures/Neutral.png",
False, 1.0)
def headLight(self, value):
colors = ["red", "blue", "green"]
for color in colors:
self.light.set_light_state('head_{0}_light'.format(color),
on=bool(value == color))
def gripper(self, data):
if data["Open"]:
self.endeffector.open()
self.head_display.display_image(
"/home/microshak/Pictures/GripperO.png", False, 1.0)
else:
self.endeffector.close()
self.head_display.display_image(
"/home/microshak/Pictures/GripperC.png", False, 1.0)
def handleIoT(self):
while len(IoTHubMessages) > 0:
self.headLight("red")
rospy.sleep(1)
temp = IoTHubMessages.pop()
message = {}
message["Action"] = temp["Action"]
message["Order"] = temp["Order"]
message["FriendlyName"] = temp["FriendlyName"]
# print(message)
print("Executing " + message["FriendlyName"])
if (message["Action"] == "Run"):
# message = IoTHubMessages.pop()
self.completeCommands("Test2")
if (message["Action"] == "Neutral"):
self.neutral()
if (message["Action"] == "Move"):
message["Cartisian"] = {}
message["Cartisian"]["orientation"] = {}
message["Cartisian"]["position"] = {}
message["Action"] = temp["Action"]
message["Speed"] = temp["Speed"]
message["Cartisian"]["orientation"]["x"] = float(
temp["Cartisian.orientation.x"])
message["Cartisian"]["orientation"]["y"] = float(
temp["Cartisian.orientation.y"])
message["Cartisian"]["orientation"]["z"] = float(
temp["Cartisian.orientation.z"])
message["Cartisian"]["orientation"]["w"] = float(
temp["Cartisian.orientation.w"])
message["Cartisian"]["position"]["x"] = float(
temp["Cartisian.position.x"])
message["Cartisian"]["position"]["y"] = float(
temp["Cartisian.position.y"])
message["Cartisian"]["position"]["z"] = float(
temp["Cartisian.position.z"])
self.move(message)
if (message["Action"] == "Stop"):
stop = True
self.head_display.display_image(
"/home/microshak/Pictures/Stop.png", False, 1.0)
while stop:
if len(IoTHubMessages) > 0:
message = IoTHubMessages.pop()
if message["Action"] == "Continue":
stop = False
self.head_display.display_image(
"/home/microshak/Pictures/Resume.png", False,
1.0)
def receive_message_callback(message, counter):
global RECEIVE_CALLBACKS
print "Listening"
message_buffer = message.get_bytearray()
size = len(message_buffer)
lit = ast.literal_eval(message_buffer[:size].decode('utf-8'))
for key in lit:
self.IotHubMessages.insert({key: lit[key]})
print key + "---" + str(lit[key])
counter += 1
RECEIVE_CALLBACKS += 1
return IoTHubMessageDispositionResult.ACCEPTED
def receive_message_callback(message):
global RECEIVE_CALLBACKS
message_buffer = message.get_bytearray()
size = len(message_buffer)
print("Received Message [%d]:" % counter)
print(" Data: <<<%s>>> & Size=%d" %
(message_buffer[:size].decode('utf-8'), size))
map_properties = message.properties()
key_value_pair = map_properties.get_internals()
print(" Properties: %s" % key_value_pair)
counter += 1
RECEIVE_CALLBACKS += 1
print(" Total calls received: %d" % RECEIVE_CALLBACKS)
return IoTHubMessageDispositionResult.ACCEPTED
def move(self, message):
self.head_display.display_image("/home/microshak/Pictures/Moving.png",
False, 1.0)
print "MOVING!!!!!!!!!!!!!!!!!"
print(message)
position = ast.literal_eval(json.dumps(message['Cartisian']))
speed = message['Speed']
print(position)
p = position["position"]
o = position["orientation"]
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.w = o["w"]
pose_target.orientation.x = o["x"]
pose_target.orientation.y = o["y"]
pose_target.orientation.z = o["z"]
pose_target.position.x = p["x"]
pose_target.position.y = p["y"]
pose_target.position.z = p["z"]
if (speed == '' or speed == '1'):
print pose_target
group = moveit_commander.MoveGroupCommander("right_arm")
group.set_pose_target(pose_target)
plan2 = group.plan()
group.go(wait=True)
else:
ik = IK.IK()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
pose_stamp = geometry_msgs.msg.PoseStamped()
pose_stamp.pose = pose_target
success, joints = ik.ik_service_client(pose_stamp, rospy)
limb_joints = dict(
zip(joints.joints[0].name, joints.joints[0].position))
limb = intera_interface.Limb("right")
limb.set_joint_position_speed(float(speed))
limb.move_to_joint_positions(
limb_joints,
timeout=20.0,
threshold=intera_interface.settings.JOINT_ANGLE_TOLERANCE)
'''
class Drone:
def __init__(self):
print("Getting robot state... ")
if ACCESS_ROBOT:
self.rs = RobotEnable(CHECK_VERSION)
self.rs.enable()
self.STOP = False
self.pose = PoseStamped()
self.prevX = 0.65
self.prevY = 0.0
self.l = Lights()
self.l_name = 'head_green_light'
self.r_name = 'head_red_light'
ap = argparse.ArgumentParser()
ap.add_argument("-c", "--condition", type=str, help="Condition of the environment: calm, average, rough")
ap.add_argument("-r", "--randomize", action='store_true', help="Randomly generate trajectory, overrides condition (true/false)")
ap.add_argument("-b", "--box", action='store_true', help="Test box edges at progam startup (true/false)")
ap.add_argument("--reset", action='store_true', help="Reset the arm to it's beginning position")
self.args = vars(ap.parse_args())
if self.args["condition"] is None:
self.args["condition"] = "calm"
if not self.args["randomize"]:
self.args["randomize"] = False
if not self.args["box"]:
self.args["box"] = False
outargs = dict(zip(self.args.keys(), self.args.values()))
print('')
print('')
print(outargs)
print('')
print('')
print('')
print('')
self.parser = InputData()
# ENABLE RED LIGHT WHILE WAITING FOR USER INPUT
self.l.set_light_state(self.r_name, on=True)
try:
input(" ----- ROBOT ENABLED, PLEASE PRESS 'ENTER' TO CONTINUE ----- ")
except Exception:
print("")
# ENABLE GREEN LIGHT WHEN PROGRAM IS RUNNING
self.l.set_light_state(self.r_name, on=False)
self.l.set_light_state(self.l_name, on=True)
# ENSURE THE WAYPOINTS LIST IS CLEARED AT TERMINATION OF PROGRAM
def clean_shutdown(self):
print("Stopping arm...")
try:
self.STOP = True
self.move(point_list = None)
self.l.set_light_state(self.l_name, on=False)
except:
print("There may have been an error exiting")
print("Stop successful, exiting...")
return
# INPUT THE SIMULATED TRAJECTORY AND PUSH TO OUTPUT TO move()
def sim_drone(self):
M = self.parser.inputMatrix()
point_list = list()
for x in range(0,350,3): # TIME IN FLIGHT SIM = 0.1*RANGE
point = [ M[x][2][0],-M[x][2][1],-M[x][2][2],-M[x][3][0],M[x][3][1],M[x][3][2] ]
#point = [ M[x][2][0],-M[x][2][1],-M[x][2][2],0,0,0 ]
point_list.append(point)
print(point)
self.move(wait=True, point_list=point_list, MAX_SPD_RATIO=0.8, MAX_JOINT_ACCL=[10.0, 8.0, 10.0, 10.0, 12.0, 12.0, 12.0])
# CONTAINS WAYPOINTS TO TRACE BOX
def trace_box(self):
print("I am tracing a box")
point_list = list()
point_list.append([0.0, 0.25, 0.0, 0.0, 0.0, 0.0])
self.move(point_list=point_list)
point_list = list()
point_list.append([0.0, 0.25, 0.1, 0.0, 0.0, 0.0])
self.move(point_list=point_list)
point_list = list()
point_list.append([0.0, -0.25, 0.1, 0.0, 0.0, 0.0])
self.move(point_list=point_list)
point_list = list()
point_list.append([0.0, -0.25, -0.1, 0.0, 0.0, 0.0])
self.move(point_list=point_list)
point_list = list()
point_list.append([0.0, 0.25, -0.1, 0.0, 0.0, 0.0])
self.move(point_list=point_list)
point_list = list()
point_list.append([0.0, 0.25, 0.0, 0.0, 0.0, 0.0])
self.move(point_list=point_list)
return
# MOVE TO ORIGIN DEFINED IN move()
def moveToNeutral(self):
print("\n --- Returning to neutral position (0.65, 0.0, 0.5, 0.0, 0.0, 0.0) ---")
point = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
point_list = [point]
success = self.move(wait=True, point_list=point_list, MAX_SPD_RATIO=0.2)
return success
# FUNCTION TO ABSTRACT CONTORL OF ARM
def move(self, point_list = None, wait = True, MAX_SPD_RATIO=0.4, MAX_JOINT_ACCL=[7.0, 5.0, 8.0, 8.0, 8.0, 8.0, 8.0]): # one point in point_list = [x_coord, y_coord, z_coord, x_deg, y_deg, z_deg]
try:
limb = Limb() # point_list = [pointA, pointB, pointC, ...]
traj_options = TrajectoryOptions()
traj_options.interpolation_type = TrajectoryOptions.JOINT
traj = MotionTrajectory(trajectory_options=traj_options, limb=limb)
if self.STOP:
traj.stop_trajectory()
return True
wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=MAX_SPD_RATIO, max_joint_accel=MAX_JOINT_ACCL)
waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb)
angles = limb.joint_ordered_angles()
waypoint.set_joint_angles(joint_angles=angles)
traj.append_waypoint(waypoint.to_msg())
for point in point_list:
#q_base = quaternion_from_euler(0, math.pi/2, 0)
q_base = quaternion_from_euler(0, 0, 0)
#q_rot = quaternion_from_euler(math.radians(point[3]), math.radians(point[4]), math.radians(point[5])) # USE THIS IF ANGLES ARE IN DEGREES
q_rot = quaternion_from_euler(point[3], point[4], point[5]) # USE THIS IF ANGLES ARE IN RADIANS
q = quaternion_multiply(q_rot, q_base)
# DEFINE ORIGIN
origin = {
'x' : 0.65,
'y' : 0.0,
'z' : 0.4
}
# CREATE CARTESIAN POSE FOR IK REQUEST
newPose = PoseStamped()
newPose.header = Header(stamp=rospy.Time.now(), frame_id='base')
newPose.pose.position.x = point[0] + origin['x']
newPose.pose.position.y = point[1] + origin['y']
newPose.pose.position.z = point[2] + origin['z']
newPose.pose.orientation.x = q[0]
newPose.pose.orientation.y = q[1]
newPose.pose.orientation.z = q[2]
newPose.pose.orientation.w = q[3]
# REQUEST IK SERVICE FROM ROS
ns = "ExternalTools/right/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
# SET THE NEW POSE AS THE IK REQUEST
ikreq.pose_stamp.append(newPose)
ikreq.tip_names.append('right_hand')
# ATTEMPT TO CALL THE SERVICE
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
except:
print("IK SERVICE CALL FAILED")
return
# HANDLE RETURN
if (resp.result_type[0] > 0):
joint_angles = resp.joints[0].position
# APPEND JOINT ANGLES TO NEW WAYPOINT
waypoint.set_joint_angles(joint_angles=joint_angles)
traj.append_waypoint(waypoint.to_msg())
else:
print("INVALID POSE")
print(resp.result_type[0])
if(wait):
print(" \n --- Sending trajectory and waiting for finish --- \n")
result = traj.send_trajectory(wait_for_result=wait)
if result is None:
rospy.logerr('Trajectory FAILED to send')
success = False
elif result.result:
rospy.loginfo('Motion controller successfully finished the trajcetory')
success = True
else:
rospy.logerr('Motion controller failed to complete the trajectory. Error: %s', result.errorId)
success = False
else:
print("\n --- Sending trajector w/out waiting --- \n")
traj.send_trajectory(wait_for_result=wait)
success = True
return success
except rospy.ROSInterruptException:
print("User interrupt detected. Exiting before trajectory completes")
# MAIN CONTROL LOOP
def fly(self, weather="calm"):
print("Flying")
self.moveToNeutral()
if self.args["reset"]:
self.l.set_light_state(self.l_name, on=False)
return
if self.args["box"]:
self.trace_box()
self.moveToNeutral()
self.sim_drone()
#self.move(wait=True, point_list=[[0,0,0,1.159/2,0,0]])
#rate = rospy.Rate(RATE)
#while not rospy.is_shutdown():
# rate.sleep()
self.l.set_light_state(self.l_name, on=False)
return