from PickandPlace import PickAndPlace
import moveit_msgs.msg
import geometry_msgs.msg
from std_msgs.msg import String, Int8MultiArray
from operator import mod
import random
from gazebo_msgs.msg import ModelState
from gazebo_msgs.srv import GetModelState
import sys
import rospy
from sawyer_irl_project.msg import onions_blocks_poses
from gazebo_ros_link_attacher.srv import Attach, AttachRequest, AttachResponse
# Global initializations
pnp = PickAndPlace()
flag = False
good_onion = False
def callback_poses(onions_poses_msg):
global pnp, good_onion
onion_index = pnp.onion_index
if good_onion:
# print("I'm waiting for a bad onion!")
return
if (onion_index == -1):
print("No more onions to sort!")
rospy.signal_shutdown("Shutting down node, work is done")
else:
if (onion_index == len(onions_poses_msg.x)):
from sawyer_irl_project.msg import onions_blocks_poses
from gazebo_ros_link_attacher.srv import Attach, AttachRequest, AttachResponse
import numpy as np
''' Picked/AtHome - means Sawyer is in hover plane at home position
onionLoc = {0: 'OnConveyor', 1: 'InFront',
2: 'InBin', 3: 'Picked/AtHome'}
eefLoc = {0: 'OnConveyor', 1: 'InFront', 2: 'InBin', 3: 'Picked/AtHome'}
predictions = {0: 'Bad', 1: 'Good', 2: 'Unknown'}
listIDstatus = {0: 'Empty', 1: 'Not Empty', 2: 'Unavailable'}
actList = {0: 'InspectAfterPicking', 1: 'PlaceOnConveyor', 2: 'PlaceInBin', 3: 'Pick',
4: 'ClaimNewOnion', 5: 'InspectWithoutPicking', 6: 'ClaimNextInList'} '''
# Global initializations
flag = False
good_onion = False
pnp = PickAndPlace()
idx = -1
policy = np.genfromtxt(
'/home/psuresh/catkin_ws/src/sawyer_irl_project/scripts/learned_policy.csv',
delimiter=' ')
# policy = np.genfromtxt('/home/psuresh/catkin_ws/src/sawyer_irl_project/scripts/expert_policy.csv', delimiter=' ')
# policy = np.genfromtxt('/home/psuresh/catkin_ws/src/sawyer_irl_project/scripts/test_expert_policy.csv', delimiter=' ')
def sid2vals(s, nOnionLoc=4, nEEFLoc=4, nPredict=3, nlistIDStatus=3):
sid = s
onionloc = int(mod(sid, nOnionLoc))
sid = (sid - onionloc) / nOnionLoc
eefloc = int(mod(sid, nEEFLoc))
sid = (sid - eefloc) / nEEFLoc
predic = int(mod(sid, nPredict))
from sanet_onionsorting.srv import yolo_srv
from smach import *
from smach_ros import *
from smach_msgs.msg import *
import rospkg
rospack = rospkg.RosPack() # get an instance of RosPack with the default search paths
# get the file path for sanet_onionsorting
path = rospack.get_path('sanet_onionsorting')
sys.path.append(path + '/scripts/')
# from rgbd_imgpoint_to_tf import Camera
# Global initializations
pnp = PickAndPlace(init_node=False)
current_state = 140
done_onions = 0
# camera = None
# def getCameraInstance(camInstance):
# global camera
# camera = camInstance
# return
def sid2vals(s, nOnionLoc=4, nEEFLoc=4, nPredict=3, nlistIDStatus=3):
sid = s
onionloc = int(mod(sid, nOnionLoc))
sid = (sid - onionloc)/nOnionLoc
eefloc = int(mod(sid, nEEFLoc))
#!/usr/bin/env python
from PickandPlace import PickAndPlace
import moveit_msgs.msg
import geometry_msgs.msg
from std_msgs.msg import String, Int8MultiArray
from operator import mod
import random
import sys
import rospy
from gripper_to_position import reset_gripper, activate_gripper, gripper_to_pos
import numpy as np
pnp = PickAndPlace(target_location_x=0.9, target_location_y=0.25)
def main():
try:
# reset_gripper()
# activate_gripper()
# # 255 = closed, 0 = open
# gripper_to_pos(0, 60, 200, False) # OPEN GRIPPER
# rospy.sleep(1.0)
# gripper_to_pos(255, 255, 200, False) # GRIPPER TO POSITION 50
group = pnp.group
current_pose = group.get_current_pose().pose
from smach import *
from smach_ros import *
from smach_msgs.msg import *
import time
import rospkg
from collections import Counter
rospack = rospkg.RosPack() # get an instance of RosPack with the default search paths
# get the file path for sanet_onionsorting
path = rospack.get_path('sanet_onionsorting')
sys.path.append(path + '/scripts/')
# from rgbd_imgpoint_to_tf import Camera
# Global initializations
pnp = PickAndPlace(init_node = False, z_tf = -0.693) # z_tf changes z dist from world origin to robot origin for eef dip.
current_state = 140
done_onions = 0
# camera = None
# def getCameraInstance(camInstance):
# global camera
# camera = camInstance
# return
def sid2vals(s, nOnionLoc=4, nEEFLoc=4, nPredict=3, nlistIDStatus=3):
sid = s
onionloc = int(mod(sid, nOnionLoc))
sid = (sid - onionloc)/nOnionLoc
eefloc = int(mod(sid, nEEFLoc))