import geometry_msgs.msg
if __name__ == '__main__':
# sleep_time = float(sys.argv[1])
sleep_time = 1.0
print "sleep_time", sleep_time
time.sleep(sleep_time)
node = rospy.init_node('action_test_1')
# ===============================================
'''
1.Test unit for platform moving
'''
# ===============================================
client = SimpleActionClient(
'/position_arm_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
client.wait_for_server()
print 'OK'
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = [
'move_1_joint', 'move_2_joint', 'move_3_joint'
]
point1 = JointTrajectoryPoint()
point1.time_from_start = rospy.Duration.from_sec(0.3)
print point1.time_from_start
point1.positions = [0, 0, 0]
# point2 = JointTrajectoryPoint()
# point2.time_from_start = rospy.Duration.from_sec(0.6)
def __init__(self, name):
ActionServer.__init__(self, name, DockAction, self.__goal_callback,
self.__cancel_callback, False)
self.__active = False
self.__docked = False
self.__mutex = threading.Lock()
self.__dock_speed = rospy.get_param('~dock/dock_speed', 0.05)
self.__dock_distance = rospy.get_param('~dock/dock_distance', 1.0)
self.__map_frame = rospy.get_param('~map_frame', 'map')
self.__odom_frame = rospy.get_param('~odom_frame', 'odom')
self.__base_frame = rospy.get_param('~base_frame', 'base_footprint')
self.__dock_ready_pose = Pose()
self.__dock_ready_pose.position.x = rospy.get_param('~dock/pose_x')
self.__dock_ready_pose.position.y = rospy.get_param('~dock/pose_y')
self.__dock_ready_pose.position.z = rospy.get_param('~dock/pose_z')
self.__dock_ready_pose.orientation.x = rospy.get_param(
'~dock/orientation_x')
self.__dock_ready_pose.orientation.y = rospy.get_param(
'~dock/orientation_y')
self.__dock_ready_pose.orientation.z = rospy.get_param(
'~dock/orientation_z')
self.__dock_ready_pose.orientation.w = rospy.get_param(
'~dock/orientation_w')
self.__dock_ready_pose_2 = PoseStamped()
rospy.loginfo('param: dock_spped, %s, dock_distance %s' %
(self.__dock_speed, self.__dock_distance))
rospy.loginfo('param: map_frame %s, odom_frame %s, base_frame %s' %
(self.__map_frame, self.__odom_frame, self.__base_frame))
rospy.loginfo('dock_pose:')
rospy.loginfo(self.__dock_ready_pose)
self.__movebase_client = SimpleActionClient('move_base',
MoveBaseAction)
rospy.loginfo('wait for movebase server...')
self.__movebase_client.wait_for_server()
rospy.loginfo('movebase server connected')
self.__approach_path = Path()
self.__path_tracker = PathTracker()
self.__approach_path_pub = rospy.Publisher('dock_approach_path',
Path,
queue_size=10)
self.__cmd_pub = rospy.Publisher(
'yocs_cmd_vel_mux/input/navigation_cmd', Twist, queue_size=10)
rospy.Subscriber('dock_pose', PoseStamped, self.__dock_pose_callback)
self.__tf_listener = tf.TransformListener()
self.__docked = False
# self.__saved_goal = MoveBaseGoal()
self.__no_goal = True
self.__current_goal_handle = ServerGoalHandle()
self.__exec_condition = threading.Condition()
self.__exec_thread = threading.Thread(None, self.__exec_loop)
self.__exec_thread.start()
rospy.loginfo('Creating ActionServer [%s]\n', name)
def __init__(self):
"""
This constructor initialises the different necessary connections to the topics and services
and resets the world to start in a good position.
"""
rospy.init_node("smart_grasper")
self.__joint_state_sub = rospy.Subscriber("/joint_states",
JointState,
self.__joint_state_cb,
queue_size=1)
rospy.wait_for_service("/gazebo/get_model_state", 10.0)
rospy.wait_for_service("/gazebo/reset_world", 10.0)
self.__reset_world = rospy.ServiceProxy("/gazebo/reset_world", Empty)
self.__get_pose_srv = rospy.ServiceProxy("/gazebo/get_model_state",
GetModelState)
rospy.wait_for_service("/gazebo/pause_physics")
self.__pause_physics = rospy.ServiceProxy("/gazebo/pause_physics",
Empty)
rospy.wait_for_service("/gazebo/unpause_physics")
self.__unpause_physics = rospy.ServiceProxy("/gazebo/unpause_physics",
Empty)
rospy.wait_for_service("/controller_manager/switch_controller")
self.__switch_ctrl = rospy.ServiceProxy(
"/controller_manager/switch_controller", SwitchController)
rospy.wait_for_service("/gazebo/set_model_configuration")
self.__set_model = rospy.ServiceProxy(
"/gazebo/set_model_configuration", SetModelConfiguration)
rospy.wait_for_service('/get_planning_scene', 10.0)
self.__get_planning_scene = rospy.ServiceProxy('/get_planning_scene',
GetPlanningScene)
self.__pub_planning_scene = rospy.Publisher('/planning_scene',
PlanningScene,
queue_size=10,
latch=True)
self.arm_commander = MoveGroupCommander("arm")
self.hand_commander = MoveGroupCommander("hand")
self.__hand_traj_client = SimpleActionClient(
"/hand_controller/follow_joint_trajectory",
FollowJointTrajectoryAction)
self.__arm_traj_client = SimpleActionClient(
"/arm_controller/follow_joint_trajectory",
FollowJointTrajectoryAction)
if self.__hand_traj_client.wait_for_server(
timeout=rospy.Duration(4.0)) is False:
rospy.logfatal(
"Failed to connect to /hand_controller/follow_joint_trajectory in 4sec."
)
raise Exception(
"Failed to connect to /hand_controller/follow_joint_trajectory in 4sec."
)
if self.__arm_traj_client.wait_for_server(
timeout=rospy.Duration(4.0)) is False:
rospy.logfatal(
"Failed to connect to /arm_controller/follow_joint_trajectory in 4sec."
)
raise Exception(
"Failed to connect to /arm_controller/follow_joint_trajectory in 4sec."
)
self.reset_world()
if __name__ == '__main__':
argv = rospy.myargv()
if len(argv) != 3:
print("Usage:")
print(
argv[0] +
" <command executer action server> <yaml file with list of commands>"
)
print("")
print("Example:")
print(argv[0] + " /main_computer/command list_of_commands.yaml")
exit(0)
print("Loading commands from " + argv[2])
msgs = yaml_to_msgs(argv[2])
print("Loaded " + str(len(msgs)) + " commands.")
print("Connecting to action server: " + argv[1])
rospy.init_node('commands_from_yaml')
ac = SimpleActionClient(argv[1], ExecuteCommandAction)
d = rospy.Duration(5.0)
while not rospy.is_shutdown() and not ac.wait_for_server(d):
print("Waiting for action server " + argv[1] + " ...")
for msg in msgs:
print("\nSending command:\n" + str(msg)[:200] + "...")
ac.send_goal(msg)
time.sleep(0.1)
print("All commands sent.")
def __init__(self, x, y, z):
self._x = x
self._y = y
self._z = z
# Retrieve params:
self._table_object_name = rospy.get_param('~table_object_name',
'Grasp_Table')
self._grasp_object_name = rospy.get_param('~grasp_object_name',
'Grasp_Object')
self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01)
self._arm_group = rospy.get_param('~manipulator', 'manipulator')
self._gripper_group = rospy.get_param('~gripper', 'gripper')
self._approach_retreat_desired_dist = rospy.get_param(
'~approach_retreat_desired_dist', 0.2)
self._approach_retreat_min_dist = rospy.get_param(
'~approach_retreat_min_dist', 0.1)
# Create (debugging) publishers:创建（调试）发布者：
self._grasps_pub = rospy.Publisher('grasps',
PoseArray,
queue_size=1,
latch=True)
self._places_pub = rospy.Publisher('places',
PoseArray,
queue_size=1,
latch=True)
# Create planning scene and robot commander:创建规划场景和机器人命令：
self._scene = PlanningSceneInterface() #未知
self._robot = RobotCommander() #未知
self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name,
self._x, self._y, self._z)
rospy.sleep(0.5)
# Define target place pose:定义目标位置姿势
self._pose_place = Pose()
self._pose_place.position.x = 0 #self._pose_coke_can.position.x
self._pose_place.position.y = -0.85 #self._pose_coke_can.position.y - 0.20
self._pose_place.position.z = 0.7 #self._pose_coke_can.position.z
self._pose_place.orientation = Quaternion(
*quaternion_from_euler(0.0, 0.0, 0.0))
self._pose_coke_can.position.z = self._pose_coke_can.position.z - 0.9 #self._pose_coke_can.position.z - 0.9 # base_link is 0.9 above
self._pose_place.position.z = self._pose_place.position.z + 0.05 # target pose a little above
# Retrieve groups (arm and gripper):
self._arm = self._robot.get_group(self._arm_group)
self._gripper = self._robot.get_group(self._gripper_group)
self._arm.set_named_target('up')
self._arm.go(wait=True)
print("up pose")
print("第一部分恢复位置初始")
# Create grasp generator 'generate' action client:
# #创建抓取生成器“生成”动作客户端：
print("开始Action通信")
self._grasps_ac = SimpleActionClient(
'/moveit_simple_grasps_server/generate', GenerateGraspsAction)
if not self._grasps_ac.wait_for_server(rospy.Duration(0.5)):
rospy.logerr('Grasp generator action client not available!')
rospy.signal_shutdown(
'Grasp generator action client not available!')
return
print("结束Action通信")
# Create move group 'pickup' action client:
# 创建移动组“抓取”操作客户端：
print("开始pickup通信")
self._pickup_ac = SimpleActionClient('/pickup', PickupAction)
if not self._pickup_ac.wait_for_server(rospy.Duration(0.5)):
rospy.logerr('Pick up action client not available!')
rospy.signal_shutdown('Pick up action client not available!')
return
print("结束pickup通信")
# Create move group 'place' action client:
# 创建移动组“放置”动作客户端：
print("开始place通信")
self._place_ac = SimpleActionClient('/place', PlaceAction)
if not self._place_ac.wait_for_server(rospy.Duration(0.5)):
rospy.logerr('Place action client not available!')
rospy.signal_shutdown('Place action client not available!')
return
print("结束place通信")
# Pick Coke can object:抓取快
while not self._pickup(self._arm_group, self._grasp_object_name,
self._grasp_object_width):
rospy.logwarn('Pick up failed! Retrying ...')
rospy.sleep(0.5)
print("抓取物体")
rospy.loginfo('Pick up successfully')
print("pose_place: ", self._pose_place)
# Place Coke can object on another place on the support surface (table):
while not self._place(self._arm_group, self._grasp_object_name,
self._pose_place):
rospy.logwarn('Place failed! Retrying ...')
rospy.sleep(0.5)
rospy.loginfo('Place successfully')
def __init__(self, ns, action_spec):
self._client = SimpleActionClient(ns, action_spec)
self._client.wait_for_server()
header.stamp = rospy.Time.now()
grasp_PA.header = header
for graspmsg in grasps:
p = Pose(graspmsg.grasp_pose.pose.position,
graspmsg.grasp_pose.pose.orientation)
grasp_PA.poses.append(p)
grasp_publisher.publish(grasp_PA)
rospy.loginfo('Published ' + str(len(grasp_PA.poses)) + ' poses')
rospy.sleep(2)
if __name__ == '__main__':
name = 'grasp_object_server'
rospy.init_node(name, anonymous=False)
rospy.loginfo("Connecting to grasp generator AS")
grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate',
GenerateGraspsAction)
gripper_group = rospy.get_param('~gripper', 'gripper')
grasp_publisher = rospy.Publisher('grasps',
PoseArray,
queue_size=1,
latch=True)
object_pose = geometry_msgs.msg.PoseStamped()
object_pose.pose.position.x = 0.0
object_pose.pose.position.y = 0.0
object_pose.pose.position.z = 0.0
object_pose.pose.orientation.w = 1.0
object_pose.pose.orientation.x = 0.0
object_pose.pose.orientation.y = 0.0
object_pose.pose.orientation.z = 0.0
grasp_list = generate_grasps(object_pose, 0.02)
rospy.loginfo('Generated ' + str(len(grasp_list)) + ' grasps.')
def __init__(self):
super(NavigateToGoalState, self).__init__(outcomes=['ok', 'err'],
input_keys=['target_pose'])
self.client = SimpleActionClient('move_base', MoveBaseAction)
def __init__(self):
self.gripper_client = SimpleActionClient('wubble_gripper_action', WubbleGripperAction)
self.gripper_client.wait_for_server()
#! /usr/bin/env python
import sys
import rospy
from actionlib import SimpleActionClient
from actionlib_msgs.msg import GoalStatus
from diagnostic_msgs.msg import KeyValue
from mir_planning_msgs.msg import GenericExecuteAction, GenericExecuteGoal
if __name__ == "__main__":
rospy.init_node("perceive_location_client_tester")
client = SimpleActionClient("perceive_location_server",
GenericExecuteAction)
client.wait_for_server()
goal = GenericExecuteGoal()
if len(sys.argv) > 1:
location = str(sys.argv[1]).upper()
goal.parameters.append(KeyValue(key="location", value=location))
rospy.loginfo("Sending following goal to perceive location server")
rospy.loginfo(goal)
client.send_goal(goal)
timeout = 15.0
finished_within_time = client.wait_for_result(
rospy.Duration.from_sec(int(timeout)))
if not finished_within_time:
exit(1)
joint_names = ('shoulder_pitch_joint',
'shoulder_pan_joint',
'upperarm_roll_joint',
'elbow_flex_joint',
'forearm_roll_joint',
'wrist_pitch_joint',
'wrist_roll_joint')
# pose that should be executed before anything else to allow opening gripper
joint_positions = (-1.760, -1.760, 0.659, -0.795, -2.521, 0.619, -0.148)
if not move_arm_to_grasping_joint_pose(joint_names, joint_positions):
exit(1)
posture_controller = SimpleActionClient('wubble_gripper_grasp_action', GraspHandPostureExecutionAction)
posture_controller.wait_for_server()
rospy.loginfo('connected to gripper posture controller')
pg = GraspHandPostureExecutionGoal()
pg.goal = GraspHandPostureExecutionGoal.RELEASE
posture_controller.send_goal(pg)
posture_controller.wait_for_result()
# define allowed contacts
table = segmentation_result.table
table_contact = AllowedContactSpecification()
table_contact.name = coll_map_res.collision_support_surface_name
table_contact.shape = Shape(type=Shape.BOX, dimensions=[abs(table.x_max - table.x_min), abs(table.y_max - table.y_min), 1e-6])
table_contact.pose_stamped = table.pose
2 - CancelGoal
3 - Exit
'''
def done_cb(terminal_state, result):
print "DONE:", terminal_state, result
def active_cb():
print "GOAL RCV"
if __name__ == "__main__":
rospy.init_node("nav_to_client")
ac_ = SimpleActionClient("nav_to_node_action", NavToAction)
ac_.wait_for_server()
print 'sever connected!'
help()
while not rospy.is_shutdown():
cmd = raw_input('cmd:')
if cmd == '1':
x = input('target_x:')
y = input('target_y:')
yaw = input('yaw:')
g = NavToActionGoal()
q = tf.transformations.quaternion_from_euler(0, 0, yaw)
g.goal.navgoal.pose.position.x = x
g.goal.navgoal.pose.position.y = y
g.goal.navgoal.pose.orientation.z = q[2]
g.goal.navgoal.pose.orientation.w = q[3]
def __init__(self, context):
self.prompt_width = 170
self.input_width = 250
super(SimpleGUI, self).__init__(context)
self.setObjectName('SimpleGUI')
self._widget = QWidget()
self._sound_client = SoundClient()
#find relative path for files to load
self.local_dir = os.path.dirname(__file__)
self.dir = os.path.join(self.local_dir, './lib/rqt_simplegui/')
if not os.path.isdir(self.dir):
os.makedirs(self.dir)
#need to add any additional subfolders as needed
if not os.path.isdir(self.dir + 'animations/'):
os.makedirs(self.dir + 'animations/')
# Creates a subscriber to the ROS topic, having msg type SoundRequest
rospy.Subscriber('robotsound', SoundRequest, self.sound_cb)
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
self.sound_sig.connect(self.sound_sig_cb)
# Code used for saving/ loading arm poses for the robot
switch_srv_name = 'pr2_controller_manager/switch_controller'
rospy.loginfo('Waiting for switch controller service...')
rospy.wait_for_service(switch_srv_name)
self.switch_service_client = rospy.ServiceProxy(
switch_srv_name, SwitchController)
self.r_joint_names = [
'r_shoulder_pan_joint', 'r_shoulder_lift_joint',
'r_upper_arm_roll_joint', 'r_elbow_flex_joint',
'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'
]
self.l_joint_names = [
'l_shoulder_pan_joint', 'l_shoulder_lift_joint',
'l_upper_arm_roll_joint', 'l_elbow_flex_joint',
'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'
]
self.all_joint_names = []
self.all_joint_poses = []
# Hash tables storing the name of the pose and the
# associated positions for that pose, initially empty
self.saved_l_poses = {}
self.saved_r_poses = {}
# Hash tables for storing name of animations and the associated pose list
self.saved_animations = {}
self.lock = threading.Lock()
rospy.Subscriber('joint_states', JointState, self.joint_states_cb)
#parsing for animations
dir = os.path.dirname(__file__)
qWarning(dir)
filename = os.path.join(self.dir, 'animations/')
ani_path = filename
ani_listing = os.listdir(ani_path)
for infile in ani_listing:
pose_left = []
pose_right = []
left_gripper_states = []
right_gripper_states = []
line_num = 1
for line in fileinput.input(ani_path + infile):
if (line_num % 2 == 1):
pose = [float(x) for x in line.split()]
pose_left.append(pose[:len(pose) / 2])
pose_right.append(pose[len(pose) / 2:])
else:
states = line.split()
left_gripper_states.append(states[0])
right_gripper_states.append(states[1])
line_num += 1
self.saved_animations[os.path.splitext(infile)[0]] = Quad(
pose_left, pose_right, left_gripper_states,
right_gripper_states)
# Create a trajectory action client
r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
self.r_traj_action_client = SimpleActionClient(r_traj_controller_name,
JointTrajectoryAction)
rospy.loginfo(
'Waiting for a response from the trajectory action server for RIGHT arm...'
)
self.r_traj_action_client.wait_for_server()
l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
self.l_traj_action_client = SimpleActionClient(l_traj_controller_name,
JointTrajectoryAction)
rospy.loginfo(
'Waiting for a response from the trajectory action server for LEFT arm...'
)
self.l_traj_action_client.wait_for_server()
# Navigation functionality initialization
self.roomNav = RoomNavigator()
self._tf_listener = TransformListener()
self.animPlay = AnimationPlayer(None, None, None, None)
# Detection and pickup functionality
self.pap = PickAndPlaceManager(self._tf_listener, self.roomNav,
self.animPlay)
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
self.joint_sig.connect(self.joint_sig_cb)
# Create a large vertical box that is top aligned
large_box = QtGui.QVBoxLayout()
large_box.setAlignment(QtCore.Qt.AlignTop)
large_box.setMargin(0)
large_box.addItem(QtGui.QSpacerItem(10, 0))
# Buttons for controlling the head of the robot
head_box = QtGui.QHBoxLayout()
head_box.addItem(QtGui.QSpacerItem(230, 0))
head_box.addWidget(self.create_pressed_button('Head Up'))
head_box.addStretch(1)
large_box.addLayout(head_box)
button_box = QtGui.QHBoxLayout()
button_box.addItem(QtGui.QSpacerItem(80, 0))
button_box.addWidget(self.create_pressed_button('Head Turn Left'))
button_box.addWidget(self.create_pressed_button('Head Down'))
button_box.addWidget(self.create_pressed_button('Head Turn Right'))
button_box.addStretch(1)
button_box.setMargin(0)
button_box.setSpacing(0)
large_box.addLayout(button_box)
# Shows what the robot says
speech_box = QtGui.QHBoxLayout()
self.speech_label = QtGui.QLabel('Robot has not spoken yet')
palette = QtGui.QPalette()
palette.setColor(QtGui.QPalette.Foreground, QtCore.Qt.blue)
self.speech_label.setPalette(palette) #
speech_box.addItem(QtGui.QSpacerItem(100, 0))
#speech_box.addWidget(self.speech_label) #
large_box.addLayout(speech_box)
# Speak button
speak_button_box = QtGui.QHBoxLayout()
speech_prompt = QtGui.QLabel('Enter Speech Text:')
speech_prompt.setFixedWidth(self.prompt_width)
speak_button_box.addWidget(speech_prompt)
robot_says = QtGui.QLineEdit(self._widget)
robot_says.setFixedWidth(self.input_width)
robot_says.textChanged[str].connect(self.onChanged) #
speak_button_box.addWidget(robot_says)
speak_button_box.addWidget(self.create_button('Speak'))
speak_button_box.addStretch(1)
large_box.addLayout(speak_button_box)
large_box.addItem(QtGui.QSpacerItem(0, 50))
# Buttons for arm poses
pose_button_box1 = QtGui.QHBoxLayout()
pose_button_box1.addItem(QtGui.QSpacerItem(150, 0))
pose_button_box1.addWidget(self.create_button('Relax Left Arm'))
pose_button_box1.addWidget(self.create_button('Relax Right Arm'))
pose_button_box1.addStretch(1)
large_box.addLayout(pose_button_box1)
# Buttons for grippers
gripper_button_box = QtGui.QHBoxLayout()
gripper_button_box.addItem(QtGui.QSpacerItem(150, 0))
gripper_button_box.addWidget(self.create_button('Open Left Gripper'))
gripper_button_box.addWidget(self.create_button('Open Right Gripper'))
gripper_button_box.addStretch(1)
large_box.addLayout(gripper_button_box)
large_box.addItem(QtGui.QSpacerItem(0, 25))
# Buttons for animation
animation_box = QtGui.QHBoxLayout()
play_anim_label = QtGui.QLabel('Select Animation:')
play_anim_label.setFixedWidth(self.prompt_width)
animation_box.addWidget(play_anim_label)
self.saved_animations_list = QtGui.QComboBox(self._widget)
animation_box.addWidget(self.saved_animations_list)
pose_time_label = QtGui.QLabel('Duration(sec):')
pose_time_label.setFixedWidth(100)
animation_box.addWidget(pose_time_label)
self.pose_time = QtGui.QLineEdit(self._widget)
self.pose_time.setFixedWidth(50)
self.pose_time.setText('2.0')
animation_box.addWidget(self.pose_time)
animation_box.addWidget(self.create_button('Play Animation'))
animation_box.addStretch(1)
large_box.addLayout(animation_box)
animation_box2 = QtGui.QHBoxLayout()
animation_name_label = QtGui.QLabel('Enter Animation Name:')
animation_name_label.setFixedWidth(self.prompt_width)
animation_box2.addWidget(animation_name_label)
self.animation_name = QtGui.QLineEdit(self._widget)
self.animation_name.setFixedWidth(self.input_width)
animation_box2.addWidget(self.animation_name)
animation_box2.addWidget(self.create_button('Save Animation'))
animation_box2.addStretch(1)
large_box.addLayout(animation_box2)
animation_box3 = QtGui.QHBoxLayout()
pose_name_label = QtGui.QLabel('Enter Pose Name:')
pose_name_label.setFixedWidth(self.prompt_width)
animation_box3.addWidget(pose_name_label)
self.pose_name_temp = QtGui.QLineEdit(self._widget)
self.pose_name_temp.setFixedWidth(self.input_width)
animation_box3.addWidget(self.pose_name_temp)
animation_box3.addWidget(self.create_button('Add Current Pose'))
animation_box3.addStretch(1)
large_box.addLayout(animation_box3)
# Playing around with UI stuff
play_box = QtGui.QHBoxLayout()
pose_sequence_label = QtGui.QLabel('Current Pose Sequence:')
pose_sequence_label.setFixedWidth(self.prompt_width)
pose_sequence_label.setAlignment(QtCore.Qt.AlignTop)
self.list_widget = QListWidget()
self.list_widget.setDragDropMode(QAbstractItemView.InternalMove)
self.list_widget.setMaximumSize(self.input_width, 200)
play_box.addWidget(pose_sequence_label)
play_box.addWidget(self.list_widget)
play_box.addStretch(1)
large_box.addLayout(play_box)
large_box.addItem(QtGui.QSpacerItem(0, 50))
# Buttons for first row of base controls
first_base_button_box = QtGui.QHBoxLayout()
first_base_button_box.addItem(QtGui.QSpacerItem(70, 0))
first_base_button_box.addWidget(
self.create_pressed_button('Rotate Left'))
first_base_button_box.addWidget(self.create_pressed_button('^'))
first_base_button_box.addWidget(
self.create_pressed_button('Rotate Right'))
first_base_button_box.addStretch(1)
large_box.addLayout(first_base_button_box)
# Buttons for second row of base controls
second_base_button_box = QtGui.QHBoxLayout()
second_base_button_box.addItem(QtGui.QSpacerItem(70, 0))
second_base_button_box.addWidget(self.create_pressed_button('<'))
second_base_button_box.addWidget(self.create_pressed_button('v'))
second_base_button_box.addWidget(self.create_pressed_button('>'))
second_base_button_box.addWidget(self.create_button('Move to Bin'))
second_base_button_box.addWidget(self.create_button('Object Detect'))
second_base_button_box.addWidget(self.create_button('Clean Room'))
second_base_button_box.addStretch(1)
large_box.addLayout(second_base_button_box)
# Animation related items to store intermediate pose co-ordinates to save
self.animation_map = {}
self.create_state = False
self._widget.setObjectName('SimpleGUI')
self._widget.setLayout(large_box)
context.add_widget(self._widget)
# Look straight and down when launched
self.head_x = 1.0
self.head_y = 0.0
self.head_z = 0.5
self.head_action(self.head_x, self.head_y, self.head_z)
# Set grippers to closed on initialization
self.left_gripper_state = ''
self.right_gripper_state = ''
self.gripper_action('l', 0.0)
self.gripper_action('r', 0.0)
# Lab 6
self.marker_publisher = rospy.Publisher('visualization_marker', Marker)
# Saved states for poses
saved_pose_box = QtGui.QHBoxLayout()
self.saved_left_poses = QtGui.QLabel('')
self.saved_right_poses = QtGui.QLabel('')
saved_pose_box.addWidget(self.saved_left_poses)
saved_pose_box.addWidget(self.saved_right_poses)
large_box.addLayout(saved_pose_box)
# Preload the map of animations
self.saved_animations_list.addItems(self.saved_animations.keys())
# Move the torso all the way down
# self.torso_down(True)
# Autonomous navigation stuff
'''
self.locations = [Pose(Point(2.48293590546, 3.90075874329, 0.000), Quaternion(0.000, 0.000, -0.783917630973, 0.620864838632)),
Pose(Point(3.70106744766, 0.304672241211, 0.000), Quaternion(0.000, 0.000, 0.950186880196, -0.311680754463)),
Pose(Point(2.57326722145, 1.51304531097, 0.000), Quaternion(0.000, 0.000, 0.96127194482, -0.275601611212)),
Pose(Point(1.28060531616, 1.52380752563, 0.000), Quaternion(0.000, 0.000, 0.946345258806, -0.323157316388)),
Pose(Point(2.11048603058, 0.420155525208, 0.000), Quaternion(0.000, 0.000, 0.945222393391, -0.326427062346)),
Pose(Point(2.82733058929, -0.739856719971, 0.000), Quaternion(0.000, 0.000, 0.945473998362, -0.325697587373)),
Pose(Point(1.29184818268, -1.90485572815, 0.000), Quaternion(0.000, 0.000, 0.942275557345, -0.334838429739)),
Pose(Point(0.722846984863, -1.0921459198, 0.000), Quaternion(0.000, 0.000, 0.949330143731, -0.314280572424))]
'''
self.locations = [
Pose(Point(2.04748392105, 2.04748010635, 0.000),
Quaternion(0.000, 0.000, -0.776968030817, 0.629540053601)),
Pose(Point(2.34193611145, 1.43208932877, 0),
Quaternion(0, 0, -0.841674385779, 0.539985396398)),
Pose(Point(3.43202018738, -0.258297920227, 0.000),
Quaternion(0.000, 0.000, 0.996656413122, -0.0817067572629)),
Pose(Point(0.931655406952, -1.96435832977, 0.000),
Quaternion(0.000, 0.000, 0.691187586713, 0.722675390458)),
Pose(Point(-0.369112968445, 0.0330476760864, 0.000),
Quaternion(0.000, 0.000, 0.0275340398899, 0.999620866453))
]
self.index = 1
rospy.loginfo("Completed GUI initialization")
if __name__ == '__main__':
try:
if len(sys.argv) > 1:
app_name = sys.argv[1]
rospy.init_node(app_name + '_key_ctrl')
else:
rospy.init_node('app_key_ctrl_smach')
# window.mainloop() will block, ignoring Ctrl+C signal, so we stop it manually on ROS shutdown
rospy.on_shutdown(quit_tk)
# Creates the SimpleActionClient, passing the type of the action
# (UserCommandAction) to the constructor.
client = SimpleActionClient('user_commands_action_server',
thorp_msgs.UserCommandAction)
# Waits until the action server has started up and started listening for goals.
client.wait_for_server()
# Create a simple GUI to show available commands and capture keyboard inputs
window = tk.Tk()
window.geometry(rospy.get_param('~window_geometry', '300x200'))
window.wm_title(rospy.get_param('~window_caption', 'User input'))
text = tk.Text(window,
background='black',
foreground='white',
font=(rospy.get_param('~text_font', 'Comic Sans MS'),
rospy.get_param('~font_size', 12)))
valid_cmds = rospy.get_param('~valid_commands', [])
def run(self):
self.event_type = 'uncontrollable' # Event type selected on Radio
self.enabled_e = [] # List of enabled events
self.param = [] # Parameters of the event
trace = Thread(
target=self.events_trace) # Thread for the tracer monitor
trace.start() # Start event tracer as a thread
while True:
#Get data from the Window
event, values = self.window.Read(timeout=10)
if event in (None,
'Cancel'): # If user closes window or clicks cancel
print('\nCLOSING EVENT INTERFACE ...\n')
break
# New event occured on the Prodct System
if self.new_trace == True:
self.new_trace = False
#Update tracer screen
if self.trace.tail(1).index[0] > 0:
if self.__events[self.trace.tail(1)
['event'].values[0]].is_controllable():
color = 'blue'
else:
color = 'red'
text = self.trace.tail(1).drop(
columns=['enabled_events', 'states']).to_string(
header=False, justify='left')
self.window['tracer'].print(text, text_color=color)
#Update the Automaton Image
try:
self.window.Element("_IMAGE_").update(
filename="output/" + values['option'] + ".png")
except:
pass
if event == 'option':
#Update the Automaton Image
try:
self.window.Element("_IMAGE_").update(
filename="output/" + values['option'] + ".png")
except:
pass
elif event == 'controllable':
#Show list of enabled controllable events
self.window.Element('selected_event').update(
values=self.enabled_e)
self.event_type = 'controllable'
elif event == 'uncontrollable':
#Show list of uncontrollable events
self.window.Element('selected_event').update(
values=self.__not_cont_e)
self.event_type = 'uncontrollable'
if event == 'trigger':
# An event is triggered
if values['controllable'] == True:
trigger_event(
values['selected_event'], self.param
) # Call the execution of the controllable event
else:
# Trigger uncontrollable events
event = values['selected_event']
# Translate non-controllable events to low-level call
ll_event = self.translation_table[(
self.translation_table['high-level'] == event
)]['low-level'].array[0]
topic = self.translation_table[(
self.translation_table['high-level'] == event
)]['topic'].array[0]
# Fake uncontrollable events
if 'erro' in ll_event:
if 'maneuvers/out' in topic:
# Fake maneuver error
move_base_client = SimpleActionClient(
"{}move_base".format(rospy.get_namespace()),
MoveBaseAction) # Get move_base service
move_base_client.wait_for_server()
move_base_client.cancel_all_goals(
) # Cancel the current motion
elif ('gas_sensor/out'
in topic) or ('victim_sensor/out' in topic):
# Fake sensor error
topic = topic.replace(
'/out',
'/in') # Get sensor/in topic to fake a failure
pub = rospy.Publisher("/{}".format(topic),
events_message,
queue_size=10)
msg = events_message()
msg.event = ll_event
pub.publish(msg) # Publish to the sensor topic
elif 'bat_' in event:
# Fake battery state change
topic = topic.replace(
'/out', '/in') # Get battery_monitor/in topic
pub = rospy.Publisher("/{}".format(topic),
events_message,
queue_size=10)
msg = events_message()
msg.event = ll_event
if event == 'bat_OK':
msg.param.append(
60.0
) # At level = 60 the system consider bat_OK
elif event == 'bat_L':
msg.param.append(
30.0
) # At level = 30 the system consider bat_L
elif event == 'bat_LL':
msg.param.append(
9.0) # At level = 9 the system consider bat_LL
pub.publish(
msg) # Publish to the battery_monitor topic
elif 'failure' in ll_event:
# Fake failures
topic = topic.replace(
'/out', '/in') # Get failures_monitor/in topic
pub = rospy.Publisher("/{}".format(topic),
events_message,
queue_size=10)
msg = events_message()
msg.event = ll_event
pub.publish(
msg) # Publish to the failures_monitor topic
elif 'ihm' in topic:
# Fake commander comands
pub = rospy.Publisher("/{}".format(topic),
events_message,
queue_size=10)
msg = events_message()
msg.event = ll_event
pub.publish(msg) # Publish to the IHM/out topic
self.param = [] # Clear param
self.window['param_list'].Update(
values=self.param) # Clear param screen
elif event == 'refresh':
self.window['tracer'].update('')
#Refresh tracer
if not self.trace.empty:
for i in self.trace.iloc[1:].index:
if self.__events[self.trace.at[
i, 'event']].is_controllable():
color = 'blue'
else:
color = 'red'
text = self.trace.loc[[i]].drop(
columns=['enabled_events', 'states']).to_string(
header=False, justify='left')
self.window['tracer'].print(text, text_color=color)
elif event == 'save':
# Save content of tracer into a csv file
filename = values['save']
if filename:
if '.' not in filename:
filename += ".csv"
if '.csv' in filename:
self.trace.drop(columns=['enabled_events', 'states'
]).to_csv(filename)
else:
sg.Popup('Wrong file extension!',
title='Saving failure!')
self.window['save'].update(value='')
elif event == 'add_param':
# Add a new item as parameter for the event
if values['new_param']:
self.param.append(eval(values['new_param']))
self.window['new_param'].update('')
self.window['param_list'].Update(values=self.param)
elif event == 'remove_param':
# Remove an item from the list of parameters
if self.window['param_list'].GetIndexes():
item = self.window['param_list'].GetIndexes()[0]
self.param.pop(item)
self.window['param_list'].Update(values=self.param)
self.window.Close()
def __init__(self,parent,id):
the_size=(700, 570)
wx.Frame.__init__(self,parent,id,'Elfin Control Panel',pos=(250,100))
self.panel=wx.Panel(self)
font=self.panel.GetFont()
font.SetPixelSize((12, 24))
self.panel.SetFont(font)
self.listener = tf.TransformListener()
self.robot=moveit_commander.RobotCommander()
self.scene=moveit_commander.PlanningSceneInterface()
self.group=moveit_commander.MoveGroupCommander('elfin_arm')
self.controller_ns='elfin_arm_controller/'
self.elfin_driver_ns='elfin_ros_control/elfin/'
self.elfin_basic_api_ns='elfin_basic_api/'
self.joint_names=rospy.get_param(self.controller_ns+'joints', [])
self.ref_link_name=self.group.get_planning_frame()
self.end_link_name=self.group.get_end_effector_link()
self.ref_link_lock=threading.Lock()
self.end_link_lock=threading.Lock()
self.js_display=[0]*6 # joint_states
self.jm_button=[0]*6 # joints_minus
self.jp_button=[0]*6 # joints_plus
self.js_label=[0]*6 # joint_states
self.ps_display=[0]*6 # pcs_states
self.pm_button=[0]*6 # pcs_minus
self.pp_button=[0]*6 # pcs_plus
self.ps_label=[0]*6 # pcs_states
self.display_init()
self.key=[]
btn_height=390
btn_lengths=[]
self.power_on_btn=wx.Button(self.panel, label=' Servo On ', name='Servo On',
pos=(20, btn_height))
btn_lengths.append(self.power_on_btn.GetSize()[0])
btn_total_length=btn_lengths[0]
self.power_off_btn=wx.Button(self.panel, label=' Servo Off ', name='Servo Off')
btn_lengths.append(self.power_off_btn.GetSize()[0])
btn_total_length+=btn_lengths[1]
self.reset_btn=wx.Button(self.panel, label=' Clear Fault ', name='Clear Fault')
btn_lengths.append(self.reset_btn.GetSize()[0])
btn_total_length+=btn_lengths[2]
self.home_btn=wx.Button(self.panel, label='Home', name='home_btn')
btn_lengths.append(self.home_btn.GetSize()[0])
btn_total_length+=btn_lengths[3]
self.stop_btn=wx.Button(self.panel, label='Stop', name='Stop')
btn_lengths.append(self.stop_btn.GetSize()[0])
btn_total_length+=btn_lengths[4]
btn_interstice=(550-btn_total_length)/4
btn_pos_tmp=btn_lengths[0]+btn_interstice+20
self.power_off_btn.SetPosition((btn_pos_tmp, btn_height))
btn_pos_tmp+=btn_lengths[1]+btn_interstice
self.reset_btn.SetPosition((btn_pos_tmp, btn_height))
btn_pos_tmp+=btn_lengths[2]+btn_interstice
self.home_btn.SetPosition((btn_pos_tmp, btn_height))
btn_pos_tmp+=btn_lengths[3]+btn_interstice
self.stop_btn.SetPosition((btn_pos_tmp, btn_height))
self.servo_state_label=wx.StaticText(self.panel, label='Servo state:',
pos=(590, btn_height-10))
self.servo_state_show=wx.TextCtrl(self.panel, style=(wx.TE_CENTER |wx.TE_READONLY),
value='', pos=(600, btn_height+10))
self.servo_state=bool()
self.fault_state_label=wx.StaticText(self.panel, label='Fault state:',
pos=(590, btn_height+60))
self.fault_state_show=wx.TextCtrl(self.panel, style=(wx.TE_CENTER |wx.TE_READONLY),
value='', pos=(600, btn_height+80))
self.fault_state=bool()
self.reply_show_label=wx.StaticText(self.panel, label='Result:',
pos=(20, btn_height+120))
self.reply_show=wx.TextCtrl(self.panel, style=(wx.TE_CENTER |wx.TE_READONLY),
value='', size=(670, 30), pos=(20, btn_height+140))
link_textctrl_length=(btn_pos_tmp-40)/2
self.ref_links_show_label=wx.StaticText(self.panel, label='Ref. link:',
pos=(20, btn_height+60))
self.ref_link_show=wx.TextCtrl(self.panel, style=(wx.TE_READONLY),
value=self.ref_link_name, size=(link_textctrl_length, 30),
pos=(20, btn_height+80))
self.end_link_show_label=wx.StaticText(self.panel, label='End link:',
pos=(link_textctrl_length+30, btn_height+60))
self.end_link_show=wx.TextCtrl(self.panel, style=(wx.TE_READONLY),
value=self.end_link_name, size=(link_textctrl_length, 30),
pos=(link_textctrl_length+30, btn_height+80))
self.set_links_btn=wx.Button(self.panel, label='Set links', name='Set links')
self.set_links_btn.SetPosition((btn_pos_tmp, btn_height+75))
# the variables about velocity scaling
velocity_scaling_init=rospy.get_param(self.elfin_basic_api_ns+'velocity_scaling',
default=0.4)
default_velocity_scaling=str(round(velocity_scaling_init, 2))
self.velocity_setting_label=wx.StaticText(self.panel, label='Velocity Scaling',
pos=(20, btn_height-70))
self.velocity_setting=wx.Slider(self.panel, value=int(velocity_scaling_init*100),
minValue=1, maxValue=100,
style = wx.SL_HORIZONTAL,
size=(500, 30),
pos=(45, btn_height-50))
self.velocity_setting_txt_lower=wx.StaticText(self.panel, label='1%',
pos=(20, btn_height-45))
self.velocity_setting_txt_upper=wx.StaticText(self.panel, label='100%',
pos=(550, btn_height-45))
self.velocity_setting_show=wx.TextCtrl(self.panel,
style=(wx.TE_CENTER|wx.TE_READONLY),
value=default_velocity_scaling,
pos=(600, btn_height-55))
self.velocity_setting.Bind(wx.EVT_SLIDER, self.velocity_setting_cb)
self.teleop_api_dynamic_reconfig_client=dynamic_reconfigure.client.Client(self.elfin_basic_api_ns,
config_callback=self.basic_api_reconfigure_cb)
self.dlg=wx.Dialog(self.panel, title='messag')
self.dlg.Bind(wx.EVT_CLOSE, self.closewindow)
self.dlg_panel=wx.Panel(self.dlg)
self.dlg_label=wx.StaticText(self.dlg_panel, label='hello', pos=(15, 15))
self.set_links_dlg=wx.Dialog(self.panel, title='Set links', size=(400, 100))
self.set_links_dlg_panel=wx.Panel(self.set_links_dlg)
self.sld_ref_link_show=wx.TextCtrl(self.set_links_dlg_panel, style=wx.TE_PROCESS_ENTER,
value='', pos=(20, 20), size=(link_textctrl_length, 30))
self.sld_end_link_show=wx.TextCtrl(self.set_links_dlg_panel, style=wx.TE_PROCESS_ENTER,
value='', pos=(20, 70), size=(link_textctrl_length, 30))
self.sld_set_ref_link_btn=wx.Button(self.set_links_dlg_panel, label='Update ref. link',
name='Update ref. link')
self.sld_set_ref_link_btn.SetPosition((link_textctrl_length+30, 15))
self.sld_set_end_link_btn=wx.Button(self.set_links_dlg_panel, label='Update end link',
name='Update end link')
self.sld_set_end_link_btn.SetPosition((link_textctrl_length+30, 65))
self.set_links_dlg.SetSize((link_textctrl_length+self.sld_set_ref_link_btn.GetSize()[0]+50, 120))
self.call_teleop_joint=rospy.ServiceProxy(self.elfin_basic_api_ns+'joint_teleop',
SetInt16)
self.call_teleop_joint_req=SetInt16Request()
self.call_teleop_cart=rospy.ServiceProxy(self.elfin_basic_api_ns+'cart_teleop',
SetInt16)
self.call_teleop_cart_req=SetInt16Request()
self.call_teleop_stop=rospy.ServiceProxy(self.elfin_basic_api_ns+'stop_teleop',
SetBool)
self.call_teleop_stop_req=SetBoolRequest()
self.call_stop=rospy.ServiceProxy(self.elfin_basic_api_ns+'stop_teleop',
SetBool)
self.call_stop_req=SetBoolRequest()
self.call_stop_req.data=True
self.stop_btn.Bind(wx.EVT_BUTTON,
lambda evt, cl=self.call_stop,
rq=self.call_stop_req :
self.call_set_bool_common(evt, cl, rq))
self.call_reset=rospy.ServiceProxy(self.elfin_driver_ns+'clear_fault', SetBool)
self.call_reset_req=SetBoolRequest()
self.call_reset_req.data=True
self.reset_btn.Bind(wx.EVT_BUTTON,
lambda evt, cl=self.call_reset,
rq=self.call_reset_req :
self.call_set_bool_common(evt, cl, rq))
self.call_power_on=rospy.ServiceProxy(self.elfin_driver_ns+'enable_robot', SetBool)
self.call_power_on_req=SetBoolRequest()
self.call_power_on_req.data=True
self.power_on_btn.Bind(wx.EVT_BUTTON,
lambda evt, cl=self.call_power_on,
rq=self.call_power_on_req :
self.call_set_bool_common(evt, cl, rq))
self.call_power_off=rospy.ServiceProxy(self.elfin_driver_ns+'disable_robot', SetBool)
self.call_power_off_req=SetBoolRequest()
self.call_power_off_req.data=True
self.power_off_btn.Bind(wx.EVT_BUTTON,
lambda evt, cl=self.call_power_off,
rq=self.call_power_off_req :
self.call_set_bool_common(evt, cl, rq))
self.call_move_homing=rospy.ServiceProxy(self.elfin_basic_api_ns+'home_teleop',
SetBool)
self.call_move_homing_req=SetBoolRequest()
self.call_move_homing_req.data=True
self.home_btn.Bind(wx.EVT_LEFT_DOWN,
lambda evt, cl=self.call_move_homing,
rq=self.call_move_homing_req :
self.call_set_bool_common(evt, cl, rq))
self.home_btn.Bind(wx.EVT_LEFT_UP,
lambda evt, mark=100:
self.release_button(evt, mark) )
self.call_set_ref_link=rospy.ServiceProxy(self.elfin_basic_api_ns+'set_reference_link', SetString)
self.call_set_end_link=rospy.ServiceProxy(self.elfin_basic_api_ns+'set_end_link', SetString)
self.set_links_btn.Bind(wx.EVT_BUTTON, self.show_set_links_dialog)
self.sld_set_ref_link_btn.Bind(wx.EVT_BUTTON, self.update_ref_link)
self.sld_set_end_link_btn.Bind(wx.EVT_BUTTON, self.update_end_link)
self.sld_ref_link_show.Bind(wx.EVT_TEXT_ENTER, self.update_ref_link)
self.sld_end_link_show.Bind(wx.EVT_TEXT_ENTER, self.update_end_link)
self.action_client=SimpleActionClient(self.controller_ns+'follow_joint_trajectory',
FollowJointTrajectoryAction)
self.action_goal=FollowJointTrajectoryGoal()
self.action_goal.trajectory.joint_names=self.joint_names
self.SetMinSize(the_size)
self.SetMaxSize(the_size)
def __init__(self, script_path):
rospy.init_node('show_time')
rospy.on_shutdown(self.cleanup)
# Initialize a number of parameters and variables for nav locations
# Set the default TTS voice to use
self.voice = rospy.get_param("~voice", "voice_en1_mbrola")
self.robot = rospy.get_param("~robot", "robbie")
self.start = (Pose(Point(1.7, 0, 0.0),
Quaternion(0.0, 0.0, 1.000, 0.018)))
self.left = (Pose(Point(1.960, -1.854, 0.0),
Quaternion(0.0, 0.0, 0.916, 0.401)))
self.right = (Pose(Point(2.123, 1.592, 0.0),
Quaternion(0.0, 0.0, 0.877, -0.480)))
self.auto_dock = (Pose(Point(0.5, 0, 0.0),
Quaternion(0.0, 0.0, 0.002, 1.000)))
self.client = SimpleActionClient("move_base", MoveBaseAction)
self.client.wait_for_server()
# Create the sound client object
self.soundhandle = SoundClient()
# Wait a moment to let the client connect to the
# sound_play server
rospy.sleep(2)
# Make sure any lingering sound_play processes are stopped.
self.soundhandle.stopAll()
# Set the wave file path if used
self.wavepath = rospy.get_param("~wavepath",
script_path + "/../sounds")
#define afternoon and morning
self.noon = strftime("%p:", localtime())
if self.noon == "AM:":
self.noon1 = "Goood Morning "
else:
self.noon1 = "Good After Noon "
self.local = strftime("%H:%M:", localtime())
#local time
self.local = strftime("%H:%M:", localtime())
self.soundhandle.say(
self.noon1 + self.robot + " is on line" + " the time is " +
self.local, self.voice)
rospy.sleep(6)
self.move_to(self.start)
self.soundhandle.say(
"Welcome to SHOW time. This is where I get to demonstrate my capabilities",
self.voice)
rospy.sleep(5)
self.soundhandle.say("I can move to my left", self.voice)
rospy.sleep(2)
self.move_to(self.left)
self.soundhandle.say("now back to the start position", self.voice)
rospy.sleep(2)
self.move_to(self.start)
self.soundhandle.say("I can move to my right", self.voice)
rospy.sleep(2)
self.move_to(self.right)
self.soundhandle.say("And again back to the start position",
self.voice)
rospy.sleep(2)
self.move_to(self.start)
rospy.sleep(2)
self.soundhandle.say("Thank you for your time ", self.voice)
rospy.sleep(2)
#rospy.sleep(2)
self.move_to(self.auto_dock)
def __init__(self, controller_name):
topic = ''.join((controller_name, '/joint_trajectory_action'))
self.client = SimpleActionClient(topic, JointTrajectoryAction)
rospy.loginfo('waiting for action client: %s' % topic)
self.client.wait_for_server()
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
# Create a dictionary to hold object colors
self.colors = dict()
#move_base action
self.fridge = (Pose(Point(X_FRIDGE, Y_FRIDGE, 0.0),
Quaternion(0.0, 0.0, 0, 1))) #location of the beer
self.person = (Pose(Point(X_PERSON, Y_PERSON, 0.0),
Quaternion(0.0, 0.0, 0,
1))) #person requesting the beer
self.station = (Pose(Point(0.5, 0.0, 0.0),
Quaternion(0.0, 0.0, 0,
1))) #person requesting the beer
self.client = SimpleActionClient("move_base", MoveBaseAction)
self.client.wait_for_server()
# Initialize the move group for the right arm
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
left_arm = MoveGroupCommander('left_arm')
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.05)
right_arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 10 seconds per planning attempt
right_arm.set_planning_time(10)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 10
# Set a limit on the number of place attempts
max_place_attempts = 5
# Give the scene a chance to catch up
rospy.sleep(2)
# Give each of the scene objects a unique name
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
target_id = 'target'
tool_id = 'tool'
person1_id = 'person1'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(target_id)
scene.remove_world_object(tool_id)
scene.remove_world_object(person1_id)
# Remove any attached objects from a previous session
scene.remove_attached_object(GRIPPER_FRAME, target_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_start')
right_arm.go()
left_arm.set_named_target('left_start')
left_arm.go()
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Set the height of the table off the ground
table_ground = 0.65
# Set the dimensions of the scene objects [l, w, h]
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
person1_size = [0.3, 0.7, 0.01]
# Set the target size [l, w, h]
target_size = [0.02, 0.01, 0.12]
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = X_FRIDGE + 0.55
table_pose.pose.position.y = Y_FRIDGE + 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = REFERENCE_FRAME
box1_pose.pose.position.x = X_FRIDGE + 0.55
box1_pose.pose.position.y = Y_FRIDGE + -0.1
box1_pose.pose.position.z = table_ground + table_size[
2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = REFERENCE_FRAME
box2_pose.pose.position.x = X_FRIDGE + 0.54
box2_pose.pose.position.y = Y_FRIDGE + 0.13
box2_pose.pose.position.z = table_ground + table_size[
2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
#add the person to the scene
person1_pose = PoseStamped()
person1_pose.header.frame_id = REFERENCE_FRAME
person1_pose.pose.position.x = X_PERSON + 0.54
person1_pose.pose.position.y = Y_PERSON + 0.13
person1_pose.pose.position.z = table_ground + table_size[
2] + person1_size[2] / 2.0
person1_pose.pose.orientation.w = 1.0
scene.add_box(person1_id, person1_pose, person1_size)
# Set the target pose in between the boxes and on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = X_FRIDGE + 0.50
target_pose.pose.position.y = Y_FRIDGE + 0.0
target_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
target_pose.pose.orientation.w = 1.0
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
# Make the table red and the boxes orange
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
self.setColor(person1_id, 0.8, 0, 0, 1.0)
# Make the target yellow
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the support surface name to the table object
right_arm.set_support_surface_name(table_id)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = X_PERSON + 0.50
place_pose.pose.position.y = Y_PERSON + -0.25
place_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
place_pose.pose.orientation.w = 1.0
#move to target
self.move_to(self.fridge)
# Initialize the grasp pose to the target pose
grasp_pose = target_pose
# Shift the grasp pose by half the width of the target to center it
grasp_pose.pose.position.y -= target_size[1] / 2.0
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id])
# Publish the grasp poses so they can be viewed in RViz
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# Track success/failure and number of attempts for pick operation
result = None
n_attempts = 0
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
result = right_arm.pick(target_id, grasps)
rospy.sleep(0.2)
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
#_------------------------now we move to the other table__________-------------------------------------------
right_arm.set_named_target('r_travel')
right_arm.go()
self.move_to(self.person)
#_------------------------now we move to the other table__________-------------------------------------------
# Generate valid place poses
places = self.make_places(place_pose)
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
for place in places:
result = right_arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
rospy.sleep(0.2)
if result != MoveItErrorCodes.SUCCESS:
rospy.loginfo("Place operation failed after " +
str(n_attempts) + " attempts.")
else:
rospy.loginfo("Pick operation failed after " + str(n_attempts) +
" attempts.")
# Return the arm to the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_start')
right_arm.go()
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
#move to station
self.move_to(self.station)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
#! /usr/bin/env python
import sys
import rospy
from actionlib import SimpleActionClient
from actionlib_msgs.msg import GoalStatus
from diagnostic_msgs.msg import KeyValue
from mir_planning_msgs.msg import GenericExecuteAction, GenericExecuteGoal
if __name__ == "__main__":
rospy.init_node("place_object_client_tester")
client = SimpleActionClient("place_object_server", GenericExecuteAction)
client.wait_for_server()
goal = GenericExecuteGoal()
if len(sys.argv) > 1:
location = str(sys.argv[1]).upper()
goal.parameters.append(KeyValue(key="location", value=location))
rospy.loginfo("Sending following goal to place object server")
rospy.loginfo(goal)
client.send_goal(goal)
timeout = 15.0
finished_within_time = client.wait_for_result(
rospy.Duration.from_sec(int(timeout)))
if not finished_within_time:
client.cancel_goal()
def execute_inner(self, userdata):
global ms
self.last_mv = rospy.Time.now()
rospy.loginfo('Beginning Exploration')
client = SimpleActionClient('explore_server', ExploreTaskAction)
rospy.loginfo('WAITING FOR EXPLORE SERVER...')
client.wait_for_server()
rospy.loginfo('EXPLORE SERVER IS NOW UP!')
boundary = PolygonStamped()
boundary.header.frame_id = "map"
boundary.header.stamp = rospy.Time.now()
r = userdata.boundary / 2.0
rospy.loginfo('boundary : {}'.format(r))
x, y, _ = userdata.initial_point
boundary.polygon.points.append(Point(x + r, y + r, 0))
boundary.polygon.points.append(Point(x - r, y + r, 0))
boundary.polygon.points.append(Point(x - r, y - r, 0))
boundary.polygon.points.append(Point(x + r, y - r, 0))
center = PointStamped()
center.header.frame_id = "map"
center.header.stamp = rospy.Time.now()
center.point.x = x
center.point.y = y
center.point.z = 0.0
goal = ExploreTaskGoal()
goal.explore_boundary = boundary
goal.explore_center = center
client.send_goal(goal)
while True:
if rospy.is_shutdown():
exit()
if client.wait_for_result(
rospy.Duration(0.3)): # 0.3 sec. timeout to check for cans
# The exploration node has finished
res = client.get_state()
rospy.loginfo('EXPLORE SERVER STATE:{}'.format(res))
if res == 3: ## SUCCEEDED
# if exploration is complete...
userdata.boundary += 1.0 # explore a larger area
return 'succeeded' # finished! yay!
else:
print client.get_goal_status_text()
print 'explore server failed : {}'.format(res)
# when explore server gives up, can't explore
return 'stuck'
now = rospy.Time.now()
if self.objective == 'discovery':
t = ms.dis_data.time
p = ms.dis_pt()
elif self.objective == 'delivery':
t = ms.del_data.time
p = ms.del_pt()
else:
rospy.logerr('Invalid objective passed to Explore state')
return 'aborted'
# if we're here, exploration is not complete yet...
if t != None: # check initialized
dt = (now - t).to_sec()
discovered = (dt < 2.0) # last seen within the last 2 seconds
if discovered:
# if can was found ...
client.cancel_all_goals()
userdata.destination = p
return 'discovered'
# more than 20 seconds have passed while completely still
# we're probably stuck
if (now - self.last_mv).to_sec() > 20.0:
print 'haven\'t been moving for a while!'
client.cancel_all_goals()
return 'stuck' # bad name... "stuck" would be better
def do_it(msg):
rospy.loginfo('We are DOING IT!')
# connect to cluster bounding box finding service
rospy.loginfo('waiting for find_cluster_bounding_box service')
rospy.wait_for_service('/find_cluster_bounding_box')
find_bounding_box_srv = rospy.ServiceProxy('/find_cluster_bounding_box',
FindClusterBoundingBox)
rospy.loginfo('connected to find_cluster_bounding_box')
target_location = msg.target_location
t_p1 = msg.object_bbox.points[0]
t_p2 = msg.object_bbox.points[1]
t_p1.y, t_p2.y = t_p2.y, t_p1.y
reset = ResetRobot()
reset.execute()
rospy.loginfo('Robot reset')
segment = SegmentScene()
res = segment.execute()
if not res: exit(1)
rospy.loginfo('Scene segmented')
# find a cluster that intersects with a provided bounding box
for idx, cluster in enumerate(res[1]['segmentation_result'].clusters):
bbox_response = find_bounding_box_srv(cluster)
if bbox_response.error_code != FindClusterBoundingBoxResponse.SUCCESS:
rospy.logwarn('unable to find for cluster %d bounding box' % idx)
continue
o_center = bbox_response.pose.pose.position
o_p1 = Point32(o_center.x - bbox_response.box_dims.x,
o_center.y - bbox_response.box_dims.y, o_center.z)
o_p2 = Point32(o_center.x + bbox_response.box_dims.x,
o_center.y + bbox_response.box_dims.y, o_center.z)
print idx, t_p1, t_p2, o_p1, o_p2
if ((t_p1.x < o_p2.x and t_p1.y < o_p2.y)
and (o_p1.x < t_p2.x and o_p1.y < t_p2.y)):
closest_index = idx
break
grasp_client = SimpleActionClient('grasp_object', GraspObjectAction)
grasp_client.wait_for_server()
grasp_goal = GraspObjectGoal()
grasp_goal.category_id = -1
grasp_goal.graspable_object = res[0].graspable_objects[closest_index]
grasp_goal.collision_object_name = res[0].collision_object_names[
closest_index]
grasp_goal.collision_support_surface_name = res[
0].collision_support_surface_name
result = grasp_client.send_goal_and_wait(grasp_goal)
if result != GoalStatus.SUCCEEDED:
rospy.logerr('failed to grasp')
reset.execute()
exit(1)
rospy.loginfo('Grasped an object')
lift_client = SimpleActionClient('lift_object', LiftObjectAction)
lift_client.wait_for_server()
lift_goal = LiftObjectGoal()
lift_goal.category_id = 0
lift_goal.graspable_object = res[0].graspable_objects[closest_index]
lift_goal.collision_object_name = res[0].collision_object_names[
closest_index]
lift_goal.collision_support_surface_name = res[
0].collision_support_surface_name
result = lift_client.send_goal_and_wait(lift_goal)
if result != GoalStatus.SUCCEEDED:
rospy.logerr('failed to lift')
exit(1)
rospy.loginfo('Lifted an object')
pudown_client = SimpleActionClient('put_down_at', PutDownAtAction)
pudown_client.wait_for_server()
putdown_goal = PutDownAtGoal()
putdown_goal.category_id = -1
putdown_goal.graspable_object = res[0].graspable_objects[closest_index]
putdown_goal.collision_object_name = res[0].collision_object_names[
closest_index]
putdown_goal.collision_support_surface_name = res[
0].collision_support_surface_name
putdown_goal.target_location = Point(
target_location.x, target_location.y,
target_location.z) #Point(0.468, 0.226, 0.251)
result = pudown_client.send_goal_and_wait(putdown_goal)
if result != GoalStatus.SUCCEEDED:
rospy.logerr('failed to put down')
reset.execute()
exit(1)
rospy.loginfo('Pu down the object')
reset.execute()
def __init__(self, timeout=5.0):
self.__ac = SimpleActionClient("/talk_request_action",
TalkRequestAction)
self.__ac.wait_for_server(rospy.Duration(timeout))
#! /usr/bin/env python
import sys
import rospy
from actionlib import SimpleActionClient
from actionlib_msgs.msg import GoalStatus
from diagnostic_msgs.msg import KeyValue
from mir_planning_msgs.msg import GenericExecuteAction, GenericExecuteGoal
if __name__ == "__main__":
rospy.init_node("unstage_object_client_tester")
client = SimpleActionClient("unstage_object_server", GenericExecuteAction)
client.wait_for_server()
if len(sys.argv) > 1:
platform = str(sys.argv[1]).upper()
if len(sys.argv) > 2:
obj = str(sys.argv[2]).upper()
else:
obj = "M20"
else:
platform = "PLATFORM_MIDDLE"
goal = GenericExecuteGoal()
goal.parameters.append(KeyValue(key="platform", value=platform))
goal.parameters.append(KeyValue(key="object", value=obj))
rospy.loginfo("Sending following goal to unstage object server")
rospy.loginfo(goal)
import rospy
from actionlib import SimpleActionClient, GoalStatus
from play_motion_msgs.msg import PlayMotionAction, PlayMotionGoal, PlayMotionResult
PLAY_MOTION_AS = "/play_motion"
from helper_functions import goal_status_dict
def test_done_cb():
print "We got a done!"
if __name__ == '__main__':
rospy.init_node("test_donecb")
rospy.loginfo("Connecting to AS: '" + PLAY_MOTION_AS + "'")
play_motion_as = SimpleActionClient(PLAY_MOTION_AS, PlayMotionAction)
play_motion_as.wait_for_server()
rospy.loginfo("Connected.")
play_motion_as.done_cb = test_done_cb # This is never called
pm_goal = PlayMotionGoal("home", False, 0)
play_motion_as.send_goal(pm_goal)
rospy.loginfo("Sent goal.")
#play_motion_as.wait_for_result() # I thought this was necessary to actually start the goal, but it's not
done = False
while not done:
state = play_motion_as.get_state()
rospy.loginfo("State: " + str(state) + " Which is: " +
goal_status_dict[state])
if state == GoalStatus.ABORTED or state == GoalStatus.SUCCEEDED:
done = True
rospy.loginfo("Finished goal")
def __init__(self,
name,
rate=100,
fk='robot',
ik='trac',
default_kv_max=1.,
default_ka_max=0.5):
"""
:param name: 'left' or 'right'
:param rate: Rate of the control loop for execution of motions
:param fk: Name of the Forward Kinematics solver, "robot", "kdl", "trac" or "ros"
:param ik: Name of the Inverse Kinematics solver, "robot", "kdl", "trac" or "ros"
:param default_kv_max: Default K maximum for velocity
:param default_ka_max: Default K maximum for acceleration
"""
Limb.__init__(self, name)
self._world = 'base'
self.kv_max = default_kv_max
self.ka_max = default_ka_max
self._gripper = Gripper(name)
self._rate = rospy.Rate(rate)
self._tf_listener = TransformListener()
self.recorder = Recorder(name)
# Kinematics services: names and services (if relevant)
self._kinematics_names = {
'fk': {
'ros': 'compute_fk'
},
'ik': {
'ros':
'compute_ik',
'robot':
'ExternalTools/{}/PositionKinematicsNode/IKService'.format(
name),
'trac':
'trac_ik_{}'.format(name)
}
}
self._kinematics_services = {
'fk': {
'ros': {
'service':
rospy.ServiceProxy(self._kinematics_names['fk']['ros'],
GetPositionFK),
'func':
self._get_fk_ros
},
'kdl': {
'func': self._get_fk_pykdl
},
'robot': {
'func': self._get_fk_robot
}
},
'ik': {
'ros': {
'service':
rospy.ServiceProxy(self._kinematics_names['ik']['ros'],
GetPositionIK),
'func':
self._get_ik_ros
},
'robot': {
'service':
rospy.ServiceProxy(self._kinematics_names['ik']['robot'],
SolvePositionIK),
'func':
self._get_ik_robot
},
'trac': {
'service':
rospy.ServiceProxy(self._kinematics_names['ik']['trac'],
GetConstrainedPositionIK),
'func':
self._get_ik_trac
},
'kdl': {
'func': self._get_ik_pykdl
}
}
}
self._selected_ik = ik
self._selected_fk = fk
# Kinematics services: PyKDL
self._kinematics_pykdl = baxter_kinematics(name)
if self._selected_ik in self._kinematics_names['ik']:
rospy.wait_for_service(
self._kinematics_names['ik'][self._selected_ik])
if self._selected_fk in self._kinematics_names['fk']:
rospy.wait_for_service(
self._kinematics_names['fk'][self._selected_fk])
# Execution attributes
rospy.Subscriber(
'/robot/limb/{}/collision_detection_state'.format(name),
CollisionDetectionState,
self._cb_collision,
queue_size=1)
rospy.Subscriber('/robot/digital_io/{}_lower_cuff/state'.format(name),
DigitalIOState,
self._cb_dig_io,
queue_size=1)
self._stop_reason = '' # 'cuff' or 'collision' could cause a trajectory to be stopped
self._stop_lock = Lock()
action_server_name = "/robot/limb/{}/follow_joint_trajectory".format(
self.name)
self.client = SimpleActionClient(action_server_name,
FollowJointTrajectoryAction)
self._display_traj = rospy.Publisher(
"/move_group/display_planned_path",
DisplayTrajectory,
queue_size=1)
self._gripper.calibrate()
self.client.wait_for_server()
def __init__(self):
# Retrieve params:
#self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table')
self._grasp_object_name = rospy.get_param('~grasp_object_name',
'object_8')
self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.08)
self._arm_group = rospy.get_param('~arm', 'arm')
self._gripper_group = rospy.get_param('~gripper', 'gripper')
self._approach_retreat_desired_dist = rospy.get_param(
'~approach_retreat_desired_dist', 0.2)
self._approach_retreat_min_dist = rospy.get_param(
'~approach_retreat_min_dist', 0.1)
# Create (debugging) publishers:
self._grasps_pub = rospy.Publisher('grasps',
PoseArray,
queue_size=1,
latch=True)
self._places_pub = rospy.Publisher('places',
PoseArray,
queue_size=1,
latch=True)
# Create planning scene and robot commander:
self._scene = PlanningSceneInterface()
self._robot = RobotCommander()
rospy.sleep(1.0)
# Clean the scene:
#self._scene.remove_world_object(self._table_object_name)
self._scene.remove_world_object(self._grasp_object_name)
# Add table and Coke can objects to the planning scene:
#self._pose_table = self._add_table(self._table_object_name)
self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name)
rospy.sleep(1.0)
# Define target place pose:
self._pose_place = self._pose_coke_can
#self._pose_place.position.x = self._pose_coke_can.position.x
#self._pose_place.position.y = self._pose_coke_can.position.y - 0.10
#self._pose_place.position.z = self._pose_coke_can.position.z + 0.08
#self._pose_place.orientation = Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0))
# Retrieve groups (arm and gripper):
self._arm = self._robot.get_group(self._arm_group)
self._gripper = self._robot.get_group(self._gripper_group)
# Create grasp generator 'generate' action client:
self._grasps_ac = SimpleActionClient(
'/moveit_simple_grasps_server/generate', GenerateGraspsAction)
if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Grasp generator action client not available!')
rospy.signal_shutdown(
'Grasp generator action client not available!')
return
# Create move group 'pickup' action client:
self._pickup_ac = SimpleActionClient('/pickup', PickupAction)
if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Pick up action client not available!')
rospy.signal_shutdown('Pick up action client not available!')
return
# Create move group 'place' action client:
self._place_ac = SimpleActionClient('/place', PlaceAction)
if not self._place_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Place action client not available!')
rospy.signal_shutdown('Place action client not available!')
return
# Pick Coke can object:
while not self._pickup(self._arm_group, self._grasp_object_name,
self._grasp_object_width):
rospy.logwarn('Pick up failed! Retrying ...')
rospy.sleep(1.0)
rospy.loginfo('Pick up successfully')
# Place Coke can object on another place on the support surface (table):
while not self._place(self._arm_group, self._grasp_object_name,
self._pose_place):
rospy.logwarn('Place failed! Retrying ...')
rospy.sleep(1.0)
rospy.loginfo('Place successfully')
def main():
#Find an object to pick up
psi = get_planning_scene_interface()
psi.reset()
rospy.loginfo('Doing detection')
detector = TablewareDetection()
det = detector.detect_objects(add_objects_as_mesh=False)
if not det.pickup_goals:
rospy.loginfo('Nothing to pick up!')
return
psi.reset()
#DARRT action client
client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction)
rospy.loginfo('Waiting for DARRT action')
client.wait_for_server()
rospy.loginfo('Found DARRT action')
#DARRTGoal
goal = DARRTGoal()
#pickup goal (from the tabletop detection)
goal.pickup_goal = det.pickup_goals[0]
goal.pickup_goal.arm_name = 'right_arm'
#world interface to tell us whether we're using the
#map or odom_combined frame
wi = WorldInterface()
#place goal (move far away)
place_pose_stamped = PoseStamped()
place_pose_stamped.header.frame_id = wi.world_frame
place_pose_stamped.pose.position.x = -2
place_pose_stamped.pose.position.y = 0
place_pose_stamped.pose.position.z = 0.85
place_pose_stamped.pose.orientation.w = 1.0
goal.place_goal = PlaceGoal(
goal.pickup_goal.arm_name, [place_pose_stamped],
collision_support_surface_name=det.table_name,
collision_object_name=goal.pickup_goal.collision_object_name)
goal.primitives = [goal.PICK, goal.PLACE, goal.BASE_TRANSIT]
goal.planning_time = 30
goal.tries = 3
goal.debug_level = 1
goal.object_sampling_fraction = 0.5
#Send the goal to the action
rospy.loginfo('Sending goal')
client.send_goal_and_wait(goal)
rospy.loginfo('Returned')
#Check the result
result = client.get_result()
if result.error_code != result.SUCCESS:
rospy.logerr('Planning failed. Error code: ' + str(result.error_code))
return
rospy.loginfo('Planning succeeded!')
#at this point the planning is done.
#now we execute and visualize the plan
#visualize trajectory
pub = rospy.Publisher('darrt_trajectory', MarkerArray)
marray = get_trajectory_markers(result.primitive_trajectories)
for i in range(10):
pub.publish(marray)
rospy.sleep(0.05)
#Executor is a Python class for executing DARRT plans
executor = Executor()
#execute trajectory
rospy.loginfo('Press enter to execute')
raw_input()
executor.execute_trajectories(result)
rospy.loginfo('Successfully executed!')
return (tcmpr, res['cluster_information'])
if __name__ == '__main__':
rospy.init_node('grab_and_run')
reset = ResetRobot()
reset.execute()
segment = SegmentScene()
res = segment.execute()
if not res: exit(1)
closest_index = res[1][0][0]
grasp_client = SimpleActionClient('grasp_object', GraspObjectAction)
grasp_client.wait_for_server()
grasp_goal = GraspObjectGoal()
grasp_goal.category_id = -1
grasp_goal.graspable_object = res[0].graspable_objects[closest_index]
grasp_goal.collision_object_name = res[0].collision_object_names[
closest_index]
grasp_goal.collision_support_surface_name = res[
0].collision_support_surface_name
result = grasp_client.send_goal_and_wait(grasp_goal)
if result != GoalStatus.SUCCEEDED:
rospy.logerr('failed to grasp')
reset.execute()
def __init__(self):
self.object_detector = ObjectDetector()
# connect to collision map processing service
rospy.loginfo('waiting for tabletop_collision_map_processing service')
rospy.wait_for_service(
'/tabletop_collision_map_processing/tabletop_collision_map_processing'
)
self.collision_map_processing_srv = rospy.ServiceProxy(
'/tabletop_collision_map_processing/tabletop_collision_map_processing',
TabletopCollisionMapProcessing)
rospy.loginfo('connected to tabletop_collision_map_processing service')
# connect to gripper action server
rospy.loginfo('waiting for wubble_gripper_grasp_action')
self.posture_controller = SimpleActionClient(
'/wubble_gripper_grasp_action', GraspHandPostureExecutionAction)
self.posture_controller.wait_for_server()
rospy.loginfo('connected to wubble_gripper_grasp_action')
rospy.loginfo('waiting for wubble_grasp_status service')
rospy.wait_for_service('/wubble_grasp_status')
self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status',
GraspStatus)
rospy.loginfo('connected to wubble_grasp_status service')
rospy.loginfo('waiting for classify service')
rospy.wait_for_service('/classify')
self.classification_srv = rospy.ServiceProxy('/classify', classify)
rospy.loginfo('connected to classify service')
# connect to gripper action server
rospy.loginfo('waiting for wubble_gripper_action')
self.gripper_controller = SimpleActionClient('/wubble_gripper_action',
WubbleGripperAction)
self.gripper_controller.wait_for_server()
rospy.loginfo('connected to wubble_gripper_action')
# connect to wubble actions
rospy.loginfo('waiting for drop_object')
self.drop_object_client = SimpleActionClient('/drop_object',
DropObjectAction)
self.drop_object_client.wait_for_server()
rospy.loginfo('connected to drop_object')
rospy.loginfo('waiting for grasp_object')
self.grasp_object_client = SimpleActionClient('/grasp_object',
GraspObjectAction)
self.grasp_object_client.wait_for_server()
rospy.loginfo('connected to grasp_object')
rospy.loginfo('waiting for lift_object')
self.lift_object_client = SimpleActionClient('/lift_object',
LiftObjectAction)
self.lift_object_client.wait_for_server()
rospy.loginfo('connected to lift_object')
rospy.loginfo('waiting for place_object')
self.place_object_client = SimpleActionClient('/place_object',
PlaceObjectAction)
self.place_object_client.wait_for_server()
rospy.loginfo('connected to plcae_object')
rospy.loginfo('waiting for push_object')
self.push_object_client = SimpleActionClient('/push_object',
PushObjectAction)
self.push_object_client.wait_for_server()
rospy.loginfo('connected to push_object')
rospy.loginfo('waiting for shake_roll_object')
self.shake_roll_object_client = SimpleActionClient(
'/shake_roll_object', ShakeRollObjectAction)
self.shake_roll_object_client.wait_for_server()
rospy.loginfo('connected to drop_object')
rospy.loginfo('waiting for shake_pitch_object')
self.shake_pitch_object_client = SimpleActionClient(
'/shake_pitch_object', ShakePitchObjectAction)
self.shake_pitch_object_client.wait_for_server()
rospy.loginfo('connected to pitch_object')
rospy.loginfo('waiting for ready_arm')
self.ready_arm_client = SimpleActionClient('/ready_arm',
ReadyArmAction)
self.ready_arm_client.wait_for_server()
rospy.loginfo('connected to ready_arm')
# advertise InfoMax service
rospy.Service('get_category_distribution', InfoMax,
self.process_infomax_request)
rospy.loginfo(
'all services contacted, object_categorization is ready to go')
self.ACTION_INFO = {
InfomaxAction.GRASP: {
'client': self.grasp_object_client,
'goal': GraspObjectGoal(),
'prereqs': [InfomaxAction.GRASP]
},
InfomaxAction.LIFT: {
'client': self.lift_object_client,
'goal': LiftObjectGoal(),
'prereqs': [InfomaxAction.GRASP, InfomaxAction.LIFT]
},
InfomaxAction.SHAKE_ROLL: {
'client':
self.shake_roll_object_client,
'goal':
ShakeRollObjectGoal(),
'prereqs': [
InfomaxAction.GRASP, InfomaxAction.LIFT,
InfomaxAction.SHAKE_ROLL
]
},
InfomaxAction.DROP: {
'client':
self.drop_object_client,
'goal':
DropObjectGoal(),
'prereqs':
[InfomaxAction.GRASP, InfomaxAction.LIFT, InfomaxAction.DROP]
},
InfomaxAction.PLACE: {
'client':
self.place_object_client,
'goal':
PlaceObjectGoal(),
'prereqs':
[InfomaxAction.GRASP, InfomaxAction.LIFT, InfomaxAction.PLACE]
},
InfomaxAction.PUSH: {
'client':
self.push_object_client,
'goal':
PushObjectGoal(),
'prereqs': [
InfomaxAction.GRASP, InfomaxAction.LIFT,
InfomaxAction.PLACE, InfomaxAction.PUSH
]
},
InfomaxAction.SHAKE_PITCH: {
'client':
self.shake_pitch_object_client,
'goal':
ShakePitchObjectGoal(),
'prereqs': [
InfomaxAction.GRASP, InfomaxAction.LIFT,
InfomaxAction.SHAKE_PITCH
]
},
}
