class TrajectoryExecution:
def __init__(self, side_prefix):
traj_controller_name = '/' + side_prefix + '_arm_controller/joint_trajectory_action'
self.traj_action_client = SimpleActionClient(traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server arm: ' + side_prefix)
self.traj_action_client.wait_for_server()
rospy.loginfo('Trajectory executor is ready for arm: ' + side_prefix)
motion_request_name = '/' + side_prefix + '_arm_motion_request/joint_trajectory_action'
self.action_server = SimpleActionServer(motion_request_name, JointTrajectoryAction, execute_cb=self.move_to_joints)
self.action_server.start()
self.has_goal = False
def move_to_joints(self, traj_goal):
rospy.loginfo('Receieved a trajectory execution request.')
traj_goal.trajectory.header.stamp = (rospy.Time.now() + rospy.Duration(0.1))
self.traj_action_client.send_goal(traj_goal)
rospy.sleep(1)
is_terminated = False
while(not is_terminated):
action_state = self.traj_action_client.get_state()
if (action_state == GoalStatus.SUCCEEDED):
self.action_server.set_succeeded()
is_terminated = True
elif (action_state == GoalStatus.ABORTED):
self.action_server.set_aborted()
is_terminated = True
elif (action_state == GoalStatus.PREEMPTED):
self.action_server.set_preempted()
is_terminated = True
rospy.loginfo('Trajectory completed.')
class ReadyArmActionServer:
def __init__(self):
self.move_arm_client = SimpleActionClient('/move_left_arm', MoveArmAction)
self.ready_arm_server = SimpleActionServer(ACTION_NAME,
ReadyArmAction,
execute_cb=self.ready_arm,
auto_start=False)
def initialize(self):
rospy.loginfo('%s: waiting for move_left_arm action server', ACTION_NAME)
self.move_arm_client.wait_for_server()
rospy.loginfo('%s: connected to move_left_arm action server', ACTION_NAME)
self.ready_arm_server.start()
def ready_arm(self, goal):
if self.ready_arm_server.is_preempt_requested():
rospy.loginfo('%s: preempted' % ACTION_NAME)
self.move_arm_client.cancel_goal()
self.ready_arm_server.set_preempted()
if move_arm_joint_goal(self.move_arm_client,
ARM_JOINTS,
READY_POSITION,
collision_operations=goal.collision_operations):
self.ready_arm_server.set_succeeded()
else:
rospy.logerr('%s: failed to ready arm, aborting', ACTION_NAME)
self.ready_arm_server.set_aborted()
class HeadClient():
def __init__(self):
name_space = '/head_traj_controller/point_head_action'
self.head_client = SimpleActionClient(name_space, PointHeadAction)
self.head_client.wait_for_server()
self.hor_pos = 0.0
self.vert_pos = 0.0
def move_head_vert (self, vert_val):
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = 'torso_lift_link'
head_goal.max_velocity = .3
self.vert_pos = vert_val/50.0 - 1
head_goal.target.point = Point(1.5, self.hor_pos, self.vert_pos)
self.head_client.send_goal(head_goal)
if (self.head_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logwarn('Head action unsuccessful.')
def move_head_hor (self, hor_val):
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = 'torso_lift_link'
head_goal.max_velocity = .3
self.hor_pos = -(hor_val/20.0 - 2.5)
head_goal.target.point = Point(1.5, self.hor_pos, self.vert_pos)
self.head_client.send_goal(head_goal)
if (self.head_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logwarn('Head action unsuccessful.')
class GripperClient():
def __init__(self):
#rospy.init_node('simplegui_gripper_node', anonymous=True)
name_space_r = '/r_gripper_controller/gripper_action'
self.r_gripper_client = SimpleActionClient(name_space_r, GripperCommandAction)
name_space_l = '/l_gripper_controller/gripper_action'
self.l_gripper_client = SimpleActionClient(name_space_l, GripperCommandAction)
self.r_gripper_client.wait_for_server()
self.l_gripper_client.wait_for_server()
def command(self, left, close):
gripper_client = self.l_gripper_client
if not left:
gripper_client = self.r_gripper_client
gripper_goal = GripperCommandGoal()
gripper_goal.command.max_effort = 30.0
if close:
gripper_goal.command.position = 0.0
else:
gripper_goal.command.position = 1.0
gripper_client.send_goal(gripper_goal)
class ActionTasks:
def __init__(self, script_path):
rospy.init_node('action_testr')
rospy.on_shutdown(self.cleanup)
self.fridge = (Pose(Point(0.295, -2.304, 0.0), Quaternion(0.0, 0.0, -0.715, 0.699)))
self.client = SimpleActionClient("move_base", MoveBaseAction)
self.client.wait_for_server()
self.move_to(self.fridge)
def move_to(self, location):
goal = MoveBaseGoal()
goal.target_pose.pose = location
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
self.client.send_goal(goal)
#self.client.wait_for_result()
self.client.wait_for_result(rospy.Duration.from_sec(40.0))
if self.client.get_state() == GoalStatus.SUCCEEDED:
result = self.client.get_result()
print "Result: SUCCEEDED "
elif self.client.get_state() == GoalStatus.PREEMPTED:
print "Action pre-empted"
else:
print "Action failed"
def cleanup(self):
rospy.loginfo("Shutting down talk node...")
def move_arm_to_grasping_joint_pose(joint_names, joint_positions, allowed_contacts=[], link_padding=[], collision_operations=OrderedCollisionOperations()):
move_arm_client = SimpleActionClient('move_left_arm', MoveArmAction)
move_arm_client.wait_for_server()
rospy.loginfo('connected to move_left_arm action server')
goal = MoveArmGoal()
goal.planner_service_name = 'ompl_planning/plan_kinematic_path'
goal.motion_plan_request.planner_id = ''
goal.motion_plan_request.group_name = 'left_arm'
goal.motion_plan_request.num_planning_attempts = 1
goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.0)
goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint(j, p, 0.1, 0.1, 0.0) for (j,p) in zip(joint_names,joint_positions)]
goal.motion_plan_request.allowed_contacts = allowed_contacts
goal.motion_plan_request.link_padding = link_padding
goal.motion_plan_request.ordered_collision_operations = collision_operations
move_arm_client.send_goal(goal)
finished_within_time = move_arm_client.wait_for_result(rospy.Duration(200.0))
if not finished_within_time:
move_arm_client.cancel_goal()
rospy.loginfo('timed out trying to achieve a joint goal')
else:
state = move_arm_client.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo('action finished: %s' % str(state))
return True
else:
rospy.loginfo('action failed: %s' % str(state))
return False
class DoReset(smach.State):
def __init__(self):
smach.State.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['reset_plan'])
self.eef_pub = rospy.Publisher('/CModelRobotOutput', eef_cmd, queue_size=1, latch=True)
self.move_arm_client = SimpleActionClient('/wm_arm_driver_node/execute_plan', executePlanAction)
def execute(self, ud):
rospy.logdebug("Entered 'RO_RESET' state.")
hand_cmd = eef_cmd()
hand_cmd.rACT = 1 # activate gripper
hand_cmd.rGTO = 1 # request to go to position
hand_cmd.rSP = 200 # set activation speed (0[slowest]-255[fastest])
hand_cmd.rFR = 0 # set force limit (0[min] - 255[max])
hand_cmd.rPR = 0 # request to open
self.eef_pub.publish(hand_cmd)
self.move_arm_client.wait_for_server()
goal = executePlanGoal()
goal.trajectory = ud.reset_plan
self.move_arm_client.send_goal(goal)
self.move_arm_client.wait_for_result()
status = self.move_arm_client.get_state()
if status == GoalStatus.SUCCEEDED:
return 'succeeded'
elif status == GoalStatus.PREEMPTED:
return 'preempted'
else:
return 'aborted'
def lookAtLocation(userdata):
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = "base_link"
# Iterate until we find the object we are searching for
index_of_object = 0
while index_of_object < 10:
if userdata.object_found.object_list[index_of_object].name == userdata.object_to_search_for:
break;
index_of_object += 1
head_goal.target.point.x = userdata.object_found.object_list[index_of_object].pose.position.x
head_goal.target.point.y = userdata.object_found.object_list[index_of_object].pose.position.y
head_goal.target.point.z = userdata.object_found.object_list[index_of_object].pose.position.z
head_goal.pointing_frame = "stereo_link"
head_goal.pointing_axis.x = 1.0
head_goal.pointing_axis.y = 0.0
head_goal.pointing_axis.z = 0.0
head_goal.max_velocity = 1.5
head_goal.min_duration.secs = 1.5
client = SimpleActionClient("/head_traj_controller/point_head_action", PointHeadAction)
client.wait_for_server(rospy.Duration(0.5))
client.send_goal(head_goal)
return succeeded
class DropService():
def __init__(self):
rospy.init_node(NAME + 'server' , anonymous=True)
self.client = SimpleActionClient("smart_arm_action", SmartArmAction)
self.gripper_client = SimpleActionClient("smart_arm_gripper_action", SmartArmGripperAction)
self.client.wait_for_server()
self.gripper_client.wait_for_server()
self.service = rospy.Service('drop_service', ArmDropService, self.handle_drop)
rospy.loginfo("%s: Ready for Dropping", NAME)
#drop it like it's hot
def handle_drop(self, req):
rospy.loginfo("told to drop")
success = False
#this seems to be as far as it can open (both straight ahead), same vals anh uses
if self.move_gripper(0.2, -0.2):
success = True
return success
#stolen gripper code from Anh Tran's "block_stacking_actions.py" in wubble_blocks
def move_gripper(self, left_finger, right_finger):
goal = SmartArmGripperGoal()
goal.target_joints = [left_finger, right_finger]
self.gripper_client.send_goal(goal)
self.gripper_client.wait_for_goal_to_finish()
result = self.gripper_client.get_result()
if result.success == False:
rospy.loginfo("Drop failed")
else:
rospy.loginfo("Object Released! Hopefully it isn't sticky")
class Move(smach.State):
def __init__(self):
smach.State.__init__(self, outcomes=['wp_reached', 'wp_not_reached', 'test_finished'],
input_keys=['move_waypoints', 'move_target_wp', 'move_wp_str'],
output_keys=['move_target_wp'])
self.move_base_client = SimpleActionClient('move_base', MoveBaseAction)
self.tts_pub = rospy.Publisher('sara_tts', String, queue_size=1, latch=True)
def execute(self, ud):
self.move_base_client.wait_for_server()
goal = MoveBaseGoal()
goal.target_pose = ud.move_waypoints[ud.move_target_wp]
goal.target_pose.header.stamp = rospy.Time.now()
self.move_base_client.send_goal(goal)
self.move_base_client.wait_for_result()
status = self.move_base_client.get_state()
tts_msg = String()
if status == GoalStatus.SUCCEEDED:
tts_msg.data = "I have reached " + ud.move_wp_str[ud.move_target_wp] + "."
self.tts_pub.publish(tts_msg)
if ud.move_target_wp == 0:
return 'test_finished'
else:
ud.move_target_wp -= 1
return 'succeeded'
else:
return 'wp_not_reached'
def init_jt_client(arm = 'r'):
name = "".join( [ arm, "_arm_controller/joint_trajectory_action" ] )
client = SimpleActionClient(name, JointTrajectoryAction)
rospy.loginfo( 'waiting for action client : %s ...'%name )
client.wait_for_server()
rospy.loginfo( '%s is up and running!'%name )
return client
class NavServer(object):
def __init__(self, name):
rospy.loginfo("Starting '%s'." % name)
self._ps = PNPSimplePluginServer(
name=name,
ActionSpec=StringAction,
execute_cb=self.execute_cb,
auto_start=False
)
self._ps.register_preempt_callback(self.preempt_cb)
self.client = SimpleActionClient('topological_navigation', GotoNodeAction)
self.client.wait_for_server()
self._ps.start()
rospy.loginfo("Done")
def execute_cb(self, goal):
self.client.send_goal_and_wait(GotoNodeGoal(target=goal.value))
result = self.client.get_state()
if result == GoalStatus.PREEMPTED:
return
if result == GoalStatus.SUCCEEDED:
self._ps.set_succeeded()
else:
self._ps.set_aborted()
def preempt_cb(self):
self.client.cancel_all_goals()
self._ps.set_preempted()
class Gripper:
LEFT = "l"
RIGHT = "r"
def __init__(self, direction):
assert direction == Gripper.LEFT or direction == Gripper.RIGHT
self.direction = direction
name_space = "/{0}_gripper_controller/gripper_action".format(self.direction)
self.gripper_client = SimpleActionClient(name_space, GripperCommandAction)
self.gripper_client.wait_for_server()
def open_gripper(self, wait=False):
self.change_state(0.08, wait)
def close_gripper(self, wait=False):
self.change_state(0.0, wait)
def change_state(self, value, wait):
gripper_goal = GripperCommandGoal()
gripper_goal.command.position = value
gripper_goal.command.max_effort = 30.0
self.gripper_client.send_goal(gripper_goal)
if wait:
self.gripper_client.wait_for_result()
if not self.gripper_client.get_result().reached_goal:
time.sleep(1)
def main():
rospy.init_node('trigger_open_door')
# initial door
prior_door = Door()
prior_door.frame_p1.x = 1.0
prior_door.frame_p1.y = -0.5
prior_door.frame_p2.x = 1.0
prior_door.frame_p2.y = 0.5
prior_door.door_p1.x = 1.0
prior_door.door_p1.y = -0.5
prior_door.door_p2.x = 1.0
prior_door.door_p2.y = 0.5
prior_door.travel_dir.x = 1.0
prior_door.travel_dir.y = 0.0
prior_door.travel_dir.z = 0.0
prior_door.rot_dir = Door.ROT_DIR_COUNTERCLOCKWISE
prior_door.hinge = Door.HINGE_P2
prior_door.header.frame_id = "base_footprint"
door = DoorGoal()
door.door = prior_door
ac = SimpleActionClient('move_through_door', DoorAction)
print 'Waiting for open door action server'
ac.wait_for_server()
print 'Sending goal to open door action server'
ac.send_goal_and_wait(door, rospy.Duration(500.0), rospy.Duration(2.0))
class Hello:
"""
a class to greet people need to recognise people
"""
def __init__(self):
self.voice = rospy.get_param("~voice", "voice_en1_mbrola")
# Create the sound client object
self.soundhandle = SoundClient()
# Wait a moment to let the client connect to the
# sound_play server
rospy.sleep(1)
# Make sure any lingering sound_play processes are stopped.
self.soundhandle.stopAll()
#set action server
self.client = SimpleActionClient('face_recognition', face_recognition.msg.FaceRecognitionAction)
# listening for goals.
self.client.wait_for_server()
rospy.sleep(2)
#result will be published on this topic
rospy.Subscriber("/face_recognition/result", FaceRecognitionActionFeedback, self.face_found)
rospy.Subscriber("/face_recognition/feedback", FaceRecognitionActionFeedback, self.Unknown)
self.look_for_face()
def look_for_face(self):
'''
send command look for face once
'''
goal = face_recognition.msg.FaceRecognitionGoal(order_id=0, order_argument="none")
self.client.send_goal(goal)
#self.client.wait_for_result(rospy.Duration.from_sec(6.0))
#if self.client.get_state() == GoalStatus.FAILED:
#self.unknown()
def face_found(self,msg):
'''
clean up feedback to extract the name
check if greeted before answer accordingly
'''
person = str(msg.result.names[0])
rospy.logwarn(person)
self.soundhandle.say("hello "+ person ,self.voice)
#self.task1 = "hello" + person
#rospy.sleep(2)
#return the processed value
self.value = str(self.task1)
def Unknown(self,msg):
self.soundhandle.say("hello what is your name " ,self.voice)
def __str__(self):
return
class Arm:
def __init__(self, arm):
if arm == 'r':
self.arm_client = SAC('r_arm_controller/joint_trajectory_action', JointTrajectoryAction)
self.arm = arm
elif arm == 'l':
self.arm_client = SAC('l_arm_controller/joint_trajectory_action', JointTrajectoryAction)
self.arm = arm
self.arm_client.wait_for_server()
rospy.loginfo('Waiting for server...')
def move(self, angles, time, blocking):
angles = [angles]
times = [time]
timeout=times[-1] + 1.0
goal = JointTrajectoryGoal()
goal.trajectory.joint_names =self.get_joint_names(self.arm)
for (ang, t) in zip(angles, times):
point = JointTrajectoryPoint()
point.positions = ang
point.time_from_start = rospy.Duration(t)
goal.trajectory.points.append(point)
goal.trajectory.header.stamp = rospy.Time.now()
self.arm_client.send_goal(goal)
rospy.sleep(.1)
rospy.loginfo("command sent to client")
status = 0
if blocking: #XXX why isn't this perfect?
end_time = rospy.Time.now() + rospy.Duration(timeout+ .1)
while (
(not rospy.is_shutdown()) and\
(rospy.Time.now() < end_time) and\
(status < gs.SUCCEEDED) and\
(type(self.arm_client.action_client.last_status_msg) != type(None))):
status = self.arm_client.action_client.last_status_msg.status_list[-1].status #XXX get to 80
rospy.Rate(10).sleep()
if status >gs.SUCCEEDED:
rospy.loginfo("goal status achieved. exiting")
else:
rospy.loginfo("ending due to timeout")
result = self.arm_client.get_result()
return result
def get_joint_names(self, char):
return [char+'_shoulder_pan_joint',
char+'_shoulder_lift_joint',
char+'_upper_arm_roll_joint',
char+'_elbow_flex_joint',
char+'_forearm_roll_joint',
char+'_wrist_flex_joint',
char+'_wrist_roll_joint' ]
def close_gripper(which_arm):
""" close the gripper """
gripper_client = SimpleActionClient(which_arm[0] + '_gripper_controller/gripper_action', Pr2GripperCommandAction)
gripper_client.wait_for_server()
##### Define goals for gripper: close all the way
goal_close = Pr2GripperCommandGoal()
goal_close.command.position = 0.0
goal_close.command.max_effort = 20.0
gripper_client.send_goal_and_wait(goal_close) # open gripper for knob
def open_gripper(which_arm):
""" Open the gripper """
gripper_client = SimpleActionClient(which_arm[0] + '_gripper_controller/gripper_action',
Pr2GripperCommandAction)
gripper_client.wait_for_server()
##### Define goals for gripper: open all the way
goal_open = Pr2GripperCommandGoal()
goal_open.command.position = 0.05
goal_open.command.max_effort = 20.0
gripper_client.send_goal_and_wait(goal_open) # open gripper for switch-flipping
class RecordPoseStampedFromPlayMotion():
"""Manage the learning from a play motion gesture"""
def __init__(self):
rospy.loginfo("Initializing RecordFromPlayMotion")
rospy.loginfo("Connecting to AS: '" + PLAY_MOTION_AS + "'")
self.play_motion_as = SimpleActionClient(PLAY_MOTION_AS, PlayMotionAction)
self.play_motion_as.wait_for_server()
rospy.loginfo("Connected.")
self.current_rosbag_name = "uninitialized_rosbag_name"
self.lfee = LearnFromEndEffector(['/tf_to_ps'], ['right_arm'])
def play_and_record(self, motion_name, groups=['right_arm'], bag_name="no_bag_name_set"):
"""Play the specified motion and start recording poses.
Try to get the joints to record from the metadata of the play_motion gesture
or, optionally, specify the joints to track"""
# Check if motion exists in param server
PLAYMOTIONPATH = '/play_motion/motions/'
if not rospy.has_param(PLAYMOTIONPATH + motion_name):
rospy.logerr("Motion named: " + motion_name + " does not exist in param server at " + PLAYMOTIONPATH + motion_name)
return
else:
rospy.loginfo("Found motion " + motion_name + " in param server at " + PLAYMOTIONPATH + motion_name)
# Get it's info
motion_info = rospy.get_param(PLAYMOTIONPATH + motion_name)
# check if joints was specified, if not, get the joints to actually save
if len(groups) > 0:
joints_to_record = groups
else:
joints_to_record = motion_info['groups']
rospy.loginfo("Got groups: " + str(joints_to_record))
# play motion
rospy.loginfo("Playing motion!")
pm_goal = PlayMotionGoal(motion_name, False, 0)
self.play_motion_as.send_goal(pm_goal)
self.lfee.start_learn(motion_name, bag_name)
done_with_motion = False
while not done_with_motion:
state = self.play_motion_as.get_state()
#rospy.loginfo("State is: " + str(state) + " which is: " + goal_status_dict[state])
if state == GoalStatus.ABORTED or state == GoalStatus.SUCCEEDED:
done_with_motion = True
elif state != GoalStatus.PENDING and state != GoalStatus.ACTIVE:
rospy.logerr("We got state " + str(state) + " unexpectedly, motion failed. Aborting.")
return None
rospy.sleep(0.1)
# data is written to rosbag in here
motion_data = self.lfee.stop_learn()
return motion_data
def look_at_r_gripper(self):
name_space = '/head_traj_controller/point_head_action'
head_client = SimpleActionClient(name_space, PointHeadAction)
head_client.wait_for_server()
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = "r_gripper_tool_frame"
head_goal.min_duration = rospy.Duration(0.3)
head_goal.target.point = Point(0, 0, 0)
head_goal.max_velocity = 1.0
head_client.send_goal(head_goal)
head_client.wait_for_result(rospy.Duration(5.0))
if (head_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logwarn('Head action unsuccessful.')
class HeadNode():
def __init__(self):
name_space = '/head_traj_controller/point_head_action'
self.head_client = SimpleActionClient(name_space, PointHeadAction)
self.head_client.wait_for_server()
def moveHead(self, theta, phi):
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = 'base_link'
head_goal.min_duration = rospy.Duration(1.0)
head_goal.target.point = Point(math.cos(theta), math.sin(theta), phi)
self.head_client.send_goal(head_goal)
self.head_client.wait_for_result(rospy.Duration(10.0))
if (self.head_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logwarn('Head action unsuccessful.')
class HeadObjectTracking():
def __init__(self):
name_space = '/head_traj_controller/point_head_action'
self.head_client = SimpleActionClient(name_space, PointHeadAction)
self.head_client.wait_for_server()
self.curr_tracking_point = Point(1, 0, 0.5)
self.point_head(self.curr_tracking_point.x, self.curr_tracking_point.y, self.curr_tracking_point.z)
rospy.Subscriber('catch_me_destination_publisher', AlvarMarker, self.new_tracking_data)
def new_tracking_data(self, marker):
"""
Adds a new tracking data point for the head.
Points the head to a point taken as a moving average over some number of
previous tracking data points.
"""
pos = marker.pose.pose.position
OLD_DATA_WEIGHT = .3
# calculate the moving average of the x, y, z positions
tracking_point = self.curr_tracking_point
avg_x = (self.curr_tracking_point.x * OLD_DATA_WEIGHT) + (pos.x * (1 - OLD_DATA_WEIGHT))
avg_y = (self.curr_tracking_point.y * OLD_DATA_WEIGHT) + (pos.y * (1 - OLD_DATA_WEIGHT))
avg_z = (self.curr_tracking_point.z * OLD_DATA_WEIGHT) + (pos.z * (1 - OLD_DATA_WEIGHT))
# make a new averaged point to track and point the head there
self.curr_tracking_point = Point(avg_x, avg_y, avg_z)
self.point_head(avg_x, avg_y, avg_z)
def point_head(self, x, y, z):
"""
Point the head to the specified point
"""
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = '/torso_lift_link'
head_goal.max_velocity = .3
# The transform seems to aim high. Move it down a little...
head_goal.target.point = Point(x, y, z - .4)
rospy.logdebug('Moving head to\n' + str(head_goal.target.point))
self.head_client.send_goal(head_goal)
def __init__(self):
rospy.init_node('pr2_pose_morph')
self.joint_map = {}
self.joint_map[LEFTY] = ['l_%s'%a for a in ARM_JOINTS]
self.joint_map[RIGHTY] = ['r_%s'%a for a in ARM_JOINTS]
self.joint_map[HEAD] = ['head_pan_joint', 'head_tilt_joint']
self.joint_servers = {}
for action_name in self.joint_map:
server = SimpleActionClient(action_name, JointTrajectoryAction)
self.joint_servers[action_name] = server
self.nav_server = SimpleActionClient("/base_controller/move_sequence", MoveSequenceAction)
for server in [self.nav_server] + self.joint_servers.values():
rospy.loginfo("Waiting for %s..."%server.action_client.ns)
server.wait_for_server()
rospy.loginfo("Ready")
class Torso:
def __init__(self):
name_space = '/torso_controller/position_joint_action'
self.torso_client = SimpleActionClient(name_space, SingleJointPositionAction)
self.torso_client.wait_for_server()
def move(self, position):
assert(position >= 0.0 and position <= 0.2)
self.position = position
up = SingleJointPositionGoal()
up.position = self.position
up.min_duration = rospy.Duration(2.0)
up.max_velocity = 1.0
self.torso_client.send_goal(up)
self.torso_client.wait_for_result()
def move_head():
name_space = '/head_traj_controller/point_head_action'
head_client = SimpleActionClient(name_space, PointHeadAction)
head_client.wait_for_server()
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = self.get_frame()
head_goal.min_duration = rospy.Duration(0.3)
head_goal.target.point = self.get_target()
head_goal.max_velocity = 10.0
head_client.send_goal(head_goal)
head_client.wait_for_result(rospy.Duration(2.0))
if (head_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logwarn('Head action unsuccessful.')
self.gui.show_text_in_rviz("Head!")
class Move_Head:
"""
a class to fins and pick up a object
"""
def __init__(self, text):
#self.task = text#whole message from greeting
self.head_client = SimpleActionClient('head_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
self.head_client.wait_for_server()
ar = text.split(':')
cmd = ar[0]
direction = ar[5]
if direction =='left':
self.move_head(1.1, 0.0)
if direction =='right':
self.move_head(-1.1, 0.0)
if direction =='front':
self.move_head(0.0, 0.0)
def move_head(self, pan , tilt):
# Which joints define the head?
head_joints = ['head_pan_joint', 'head_tilt_mod_joint']
# Create a single-point head trajectory with the head_goal as the end-point
head_trajectory = JointTrajectory()
head_trajectory.joint_names = head_joints
head_trajectory.points.append(JointTrajectoryPoint())
head_trajectory.points[0].positions = pan , tilt
head_trajectory.points[0].velocities = [0.0 for i in head_joints]
head_trajectory.points[0].accelerations = [0.0 for i in head_joints]
head_trajectory.points[0].time_from_start = rospy.Duration(3.0)
# Send the trajectory to the head action server
rospy.loginfo('Moving the head to goal position...')
head_goal = FollowJointTrajectoryGoal()
head_goal.trajectory = head_trajectory
head_goal.goal_time_tolerance = rospy.Duration(0.0)
# Send the goal
self.head_client.send_goal(head_goal)
# Wait for up to 5 seconds for the motion to complete
self.head_client.wait_for_result(rospy.Duration(2.0))
def fixHeadPosition(userdata):
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = "base_link"
head_goal.target.point.x = 1.0
head_goal.target.point.y = 0.0
head_goal.target.point.z = 1.65
head_goal.pointing_frame = "stereo_link"
head_goal.pointing_axis.x = 1.0
head_goal.pointing_axis.y = 0.0
head_goal.pointing_axis.z = 0.0
head_goal.max_velocity = 1.5
head_goal.min_duration.secs = 1.5
client = SimpleActionClient("/head_traj_controller/point_head_action", PointHeadAction)
client.wait_for_server(rospy.Duration(5.0))
client.send_goal(head_goal)
return succeeded
def tilt_head(self, down=True):
name_space = '/head_traj_controller/point_head_action'
head_client = SimpleActionClient(name_space, PointHeadAction)
head_client.wait_for_server()
head_goal = PointHeadGoal()
head_goal.target.header.frame_id = self.get_frame()
head_goal.min_duration = rospy.Duration(0.3)
if down:
head_goal.target.point = Point(1, 0, Head.speed * -0.1)
else:
head_goal.target.point = Point(1, 0, Head.speed * 0.1)
head_goal.max_velocity = 10.0
head_client.send_goal(head_goal)
head_client.wait_for_result(rospy.Duration(2.0))
if (head_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logwarn('Head action unsuccessful.')
def gripper_action(self, gripper_type, gripper_position):
name_space = '/' + gripper_type + '_gripper_controller/gripper_action'
gripper_client = SimpleActionClient(name_space, GripperCommandAction)
gripper_client.wait_for_server()
gripper_goal = GripperCommandGoal()
gripper_goal.command.position = gripper_position
gripper_goal.command.max_effort = 30.0
gripper_client.send_goal(gripper_goal)
# update the state of the current gripper being modified
if (gripper_type == 'l'):
if (gripper_position == 0.0):
self.left_gripper_state = 'closed'
else:
self.left_gripper_state = 'open'
else:
if (gripper_position == 0.0):
self.right_gripper_state = 'closed'
else:
self.right_gripper_state = 'open'
def main():
rospy.init_node("simple_navigation_goals")
move_base_client = SimpleActionClient('move_base', MoveBaseAction)
rospy.loginfo('Connecting to server')
move_base_client.wait_for_server()
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'komodo_1/base_link'
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = -1.0
goal.target_pose.pose.orientation.w = 1.0
rospy.loginfo('Sending goal')
move_base_client.send_goal(goal)
move_base_client.wait_for_result()
if move_base_client.get_state() == GoalStatus.SUCCEEDED:
rospy.loginfo('Hooray, the base moved 1 meter forward')
else:
rospy.loginfo('The base failed to move forward 1 meter for some reason')
class AUVController:
"""Class for controlling AUV in missions.
The class is actually a unified wrap over actions and services calls.
"""
# TODO: Errors handling
# TODO: Devices support
def __init__(self):
"""The constructor.
"""
self.rotation_client_ = SimpleActionClient('stingray_action_rotate',
stingray_movement_msgs.msg.RotateAction)
self.dive_client_ = SimpleActionClient('stingray_action_dive',
stingray_movement_msgs.msg.DiveAction)
self.linear_client_ = SimpleActionClient('stingray_action_linear_movement',
stingray_movement_msgs.msg.LinearMoveAction)
self.rotation_client_.wait_for_server()
self.dive_client_.wait_for_server()
self.linear_client_.wait_for_server()
def rotate(self, yaw_deg: float):
"""Rotates the vehicle. Blocking call.
:param yaw_deg: Angle to rotate in degrees, positive is for counterclockwise, negative - for clockwise.
"""
goal = self._create_rotate_goal(yaw_deg=yaw_deg)
self._exec_action(self.rotation_client_, goal)
def march(self, duration_ms: int, velocity: float, stopping_event: EventBase = None):
"""Moves the vehicle by march. Blocking call. Action execution will be canceled if stopping_event is triggered.
:param duration_ms: Duration of movement in milliseconds.
:param velocity: Velocity of movement, from 0.0 to 1.0, positive to move forward, negative - to backward movement.
:param stopping_event: Event that stops movement.
"""
goal = self._create_march_goal(duration_ms, velocity)
return self._exec_action(self.linear_client_, goal, stopping_event)
def lag(self, duration_ms: int, velocity: float, stopping_event: EventBase = None):
"""Moves the vehicle by lag. Blocking call. Action execution will be canceled if stopping_event is triggered.
:param duration_ms: Duration of the movement in milliseconds.
:param velocity: Velocity of movement, from 0.0 to 1.0, positive to move right, negative - to move left (from vehicle point of view).
:param stopping_event: Event that stops movement.
"""
goal = self._create_lag_goal(duration_ms, velocity)
return self._exec_action(self.linear_client_, goal, stopping_event)
def dive(self, depth_cm: int):
"""Dives the vehicle. Blocking call.
:param depth_cm: Depth of diving in centimeters, must be positive.
"""
goal = stingray_movement_msgs.msg.DiveGoal(depth=depth_cm)
self.dive_client_.send_goal(goal)
self.dive_client_.wait_for_result()
def stop(self):
"""Stops the vehicle. Blocking call.
"""
goal = self._create_stop_goal()
self.linear_client_.send_goal(goal)
self.linear_client_.wait_for_result()
@staticmethod
def _create_rotate_goal(yaw_deg: float):
"""Constructs action goal for rotation action server. This is a private method.
:param yaw_deg: Angle to rotate in degrees, positive is for counterclockwise, negative - for clockwise.
:return: Action goal.
"""
return stingray_movement_msgs.msg.RotateGoal(yaw=yaw_deg)
@staticmethod
def _create_march_goal(duration_ms: int, velocity: float):
"""Constructs action goal for linear movement server to move by march. This is a private method.
:param duration_ms: Duration of movement in milliseconds.
:param velocity: Velocity of movement, from 0.0 to 1.0, positive to move forward, negative - to backward movement.
:return: Action goal.
"""
direction = stingray_movement_msgs.msg.LinearMoveGoal.DIRECTION_MARCH_FORWARD if velocity >= 0 \
else stingray_movement_msgs.msg.LinearMoveGoal.DIRECTION_MARCH_BACKWARDS
goal = stingray_movement_msgs.msg.LinearMoveGoal(direction=direction, duration=duration_ms,
velocity=abs(velocity))
return goal
@staticmethod
def _create_lag_goal(duration_ms: int, velocity: float):
"""Constructs action goal for linear movement server to move by lag. This is a private method.
:param duration_ms: Duration of the movement in milliseconds.
:param velocity: Velocity of movement, from 0.0 to 1.0, positive to move right, negative - to move left (from vehicle point of view).
:return: Action goal.
"""
direction = stingray_movement_msgs.msg.LinearMoveGoal.DIRECTION_LAG_RIGHT if velocity >= 0 \
else stingray_movement_msgs.msg.LinearMoveGoal.DIRECTION_LAG_LEFT
goal = stingray_movement_msgs.msg.LinearMoveGoal(direction=direction, duration=duration_ms,
velocity=abs(velocity))
return goal
@staticmethod
def _create_stop_goal():
"""Constructs action goal to stop any movement. This is a private method.
:return: Action goal.
"""
return stingray_movement_msgs.msg.LinearMoveGoal(
direction=stingray_movement_msgs.msg.LinearMoveGoal.DIRECTION_STOP, duration=0, velocity=0)
@staticmethod
def _exec_action(action_client: SimpleActionClient, goal, stopping_event: EventBase = None):
"""Executes goal with specified action client and optional event that stops action execution when triggered.
This is a private method.
:param action_client: Action client to use.
:param goal: Action goal to execute.
:param stopping_event: Optional event that stops goal execution when triggered.
:return: None if stopping_event is None. True if goal execution is stopped because of triggered stopping_event,
False otherwise.
"""
if stopping_event is None:
action_client.send_goal(goal)
action_client.wait_for_result()
return
stopping_event.start_listening()
action_client.send_goal(goal,
active_cb=None,
feedback_cb=None,
done_cb=None)
check_rate = rospy.Rate(100)
while True:
goal_state = action_client.get_state()
if goal_state == SimpleGoalState.DONE:
stopping_event.stop_listening()
return False
if stopping_event.is_triggered():
action_client.cancel_goal()
return True
check_rate.sleep()
class Explorer():
EXPLORING = 0
DONE = 1
def __init__(self, home_x=2.0, home_y=2.0):
# flag to check for termination conditions
self.is_running = True
self.condition = Driver.NORMAL
# initialized the "viewed "
self.map = OccupancyGrid()
# subscribe to the view_map
self.map_sub = rospy.Subscriber("view_map", OccupancyGrid, self.update_map)
# get file path
# rospack = rospkg.RosPack()
# dir_path = rospack.get_path("exercise1")
# self.home_x = home_x
# self.home_y = home_y
# to_visit = list(reversed(orig_positions))
self.to_visit = copy.deepcopy(self.orig_positions)
self.wait_time = 60.0 # used as seconds in autopilot function
self.is_interrupted = False
# create the action client and send goal
self.client = SimpleActionClient("move_base", MoveBaseAction)
self.client.wait_for_server(rospy.Duration.from_sec(self.wait_time))
## marking list of places to vist
self.location_marker_pub = rospy.Publisher("destination_marker", Marker, queue_size=10)
# self.create_all_markers()
# important to register shutdown node with threads after thread have been created
rospy.on_shutdown(self.shutdown)
def
def explore(self):
## plant flags
## don't stop until you visit everything
def update_state(self):
char = self.kb_thread.pop_c()
if char == 'h':
self.condition = Driver.HOME
elif char == 'r':
self.condition = Driver.RANDOMIZE
elif char == '\x1b':
self.condition = Driver.SHUTDOWN
# print "shutdown condition"
else:
# self.condition = Driver.NORMAL
pass
return self.condition
def cancel_goal(self):
self.client.cancel_all_goals()
def send_move_goal(self, loc):
# create goal
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.pose.position.x = loc[0]
goal.target_pose.pose.position.y = loc[1]
goal.target_pose.pose.position.z = loc[2]
goal.target_pose.pose.orientation.x = loc[3]
goal.target_pose.pose.orientation.y = loc[4]
goal.target_pose.pose.orientation.z = loc[5]
goal.target_pose.pose.orientation.w = loc[6]
goal.target_pose.header.stamp = rospy.Time.now()
self.client.send_goal(goal)
return self.client
# def do_something_interesting(self):
# ''' Output a nice call phrase for the grader.
# Assumes espeak is installed. BAD ASSUMPTION
# '''
# sayings = ["I love you Will", "Great job Will", "Your a great teaching assistant",
# "Am I home yet?", "When will this stop?", "Are you my mother?",
# "Assimov books are great.", "Robot army on its way.",
# "Your my friend.", "Kory deserves 100 percent."]
# espeak_call_str = "espeak -s 120 \'{0}\'".format(choice(sayings))
# os.system(espeak_call_str)
# def create_all_markers(self):
# self.markers = []
# for i in xrange(len(self.orig_positions)):
# loc = self.orig_positions[i]
# marker_id = i
# marker = self.create_location_marker(loc, Marker.SPHERE, Marker.ADD, marker_id, ColorRGBA(0,0,1,1))
# self.markers.append(marker)
# # print "in create all markers", self.markers
def mark_as_to_visit(self, loc, marker_id):
marker = self.create_location_marker(loc, Marker.SPHERE, Marker.ADD, marker_id, ColorRGBA(0,1,0,1))
self.location_marker_pub.publish(marker)
def mark_as_visited(self, loc, marker_id):
marker = self.create_location_marker(loc, color, Marker.SPHERE, Marker.ADD, marker_id, ColorRGBA(0,1,1,1))
self.location_marker_pub.publish(marker)
def create_location_marker(self, loc, marker_type=Marker.SPHERE, marker_action=Marker.ADD, marker_id=0, marker_color=ColorRGBA(1,0,0,1)):
h = Header()
h.frame_id = "map"
h.stamp = rospy.Time.now()
mark = Marker()
mark.header = h
mark.ns = "location_marker"
mark.id = marker_id
mark.type = marker_type
mark.action = marker_action
mark.scale = Vector3(0.25, 0.25, 0.25)
mark.color = marker_color
pose = Pose(Point(loc[0], loc[1], loc[2]), Quaternion(loc[3],loc[4],loc[5],loc[6]))
mark.pose = pose
return mark
### threaded way to do it
def shutdown(self):
self.client.cancel_all_goals()
if __name__ == '__main__':
rospy.init_node('explorer')
msg = '''
Exploring map.
'''
print msg
explorer = Explorer()
explorer.explore()
rospy.spin()
class AcceptanceTestJointLimits(unittest.TestCase):
""" Test that each joint can be moved exactly to its limits and not further.
"""
def setUp(self):
""" Initialize a client for sending trajectories to the controller.
"""
self.client = SimpleActionClient(_CONTROLLER_ACTION_NAME,
FollowJointTrajectoryAction)
if not self.client.wait_for_server(
timeout=rospy.Duration(_ACTION_SERVER_TIMEOUT_SEC)):
self.fail('Timed out waiting for action server ' +
_CONTROLLER_ACTION_NAME)
self.joint_names = sorted(_JOINT_LIMITS_DEGREE.keys())
self.home_positions = [0] * len(self.joint_names)
# read joint limits from urdf
self.joint_velocity_limits = [0] * len(self.joint_names)
robot = URDF.from_parameter_server("robot_description")
for joint in robot.joints:
if joint.name in self.joint_names:
index = self.joint_names.index(joint.name)
self.joint_velocity_limits[index] = joint.limit.velocity
self.assertTrue(all(self.joint_velocity_limits))
def _ask_for_permission(self, test_name):
s = raw_input('Perform ' + test_name + ' [(y)es, (n)o]?: ')
if (s == "n"):
print('\n\nSkip ' + test_name + '\n___TEST-END___\n')
return 0
print('\n\nStart ' + test_name + '\n')
return 1
def _current_joint_state_positions(self):
""" Return the current joint state positions in the order given by self.joint_names.
"""
msg = JointState()
positions = []
try:
msg = rospy.wait_for_message(_JOINT_STATES_TOPIC_NAME,
JointState,
timeout=_WAIT_FOR_MESSAGE_TIMEOUT_SEC)
except rospy.ROSException:
self.fail('Could not retrieve message from topic ' +
_JOINT_STATES_TOPIC_NAME)
for name in self.joint_names:
try:
index = msg.name.index(name)
positions.append(msg.position[index])
except ValueError:
self.fail('Could not retrieve joint state position for ' +
name)
return positions
def _check_joint_limits(self):
""" Check if current joint positions are within the limits.
"""
positions = self._current_joint_state_positions()
for i in range(len(self.joint_names)):
position = positions[i]
name = self.joint_names[i]
limit = radians(
_JOINT_LIMITS_DEGREE[name]) + _JOINT_POSITIONS_TOLERANCE
self.assertGreater(
position, -limit, 'Joint ' + name +
' violates lower limit. Position: ' + str(position))
self.assertLess(
position, limit, 'Joint ' + name +
' violates upper limit. Position: ' + str(position))
def _drive_home(self):
""" Move the robot to the home position.
"""
positions = self._current_joint_state_positions()
diffs = []
for i in range(len(positions)):
diffs.append(abs(positions[i] - self.home_positions[i]))
if any(diff > _JOINT_POSITIONS_TOLERANCE for diff in diffs):
self._execute_trajectory(self.home_positions)
def _execute_trajectory(self, positions):
""" Execute a single point trajectory given through joint positions (in the order given by self.joint_names). Return true on success and
false otherwise.
"""
self.assertEqual(len(positions), len(self.joint_names))
# determine duration from distance, max velocity and velocity scaling
current_positions = self._current_joint_state_positions()
durations = []
for i in range(len(current_positions)):
distance = abs(positions[i] - current_positions[i])
durations.append(distance / self.joint_velocity_limits[i])
duration = max(durations) / _VELOCITY_SCALE
# construct goal
traj_point = JointTrajectoryPoint()
traj_point.positions = positions
traj_point.time_from_start = rospy.Duration(duration)
traj = JointTrajectory()
traj.joint_names = self.joint_names
traj.points.append(traj_point)
goal = FollowJointTrajectoryGoal()
goal.trajectory = traj
# send to trajectory controller
self.client.send_goal(goal)
self.client.wait_for_result()
# evaluate result
result = self.client.get_result()
success = (result.error_code == FollowJointTrajectoryResult.SUCCESSFUL)
if not success:
rospy.loginfo('Failure executing: ' + str(goal))
return success
def _joint_limit_reaching_test(self, joint_name):
""" Test if the robot can be commanded to move exactly to the limits
Test Sequence:
1. Command a movement to the home position.
2. Command a movement to the lower limit.
3. Command a movement to the upper limit.
4. Command a movement to the home position.
Expected Results:
1. Trajectory is executed successfully.
2. Trajectory is executed successfully.
3. Trajectory is executed successfully.
4. Trajectory is executed successfully.
"""
self._drive_home()
index = self.joint_names.index(joint_name)
limit = _JOINT_LIMITS_DEGREE[joint_name]
lower_positions = [0] * len(self.joint_names)
lower_positions[index] = -radians(limit)
self.assertTrue(self._execute_trajectory(lower_positions))
upper_positions = [0] * len(self.joint_names)
upper_positions[index] = radians(limit)
self.assertTrue(self._execute_trajectory(upper_positions))
self._drive_home()
def _joint_limit_overstepping_test(self, joint_name):
""" Test if the robot does not overstep the limits
Test Sequence:
1. Command a movement to the home position.
2. Command a movement overstepping the lower limit.
3. Command a movement overstepping the upper limit.
4. Command a movement to the home position.
Expected Results:
1. Trajectory is executed successfully.
2. Trajectory execution is aborted and the robot does not overstep the limits.
3. Trajectory execution is aborted and the robot does not overstep the limits.
4. Trajectory is executed successfully.
"""
self._drive_home()
index = self.joint_names.index(joint_name)
limit = _JOINT_LIMITS_DEGREE[joint_name]
lower_positions = [0] * len(self.joint_names)
lower_positions[index] = -(radians(limit) + _JOINT_LIMIT_OVERSTEP)
self.assertFalse(self._execute_trajectory(lower_positions))
self._check_joint_limits()
upper_positions = [0] * len(self.joint_names)
upper_positions[index] = radians(limit) + _JOINT_LIMIT_OVERSTEP
self.assertFalse(self._execute_trajectory(upper_positions))
self._check_joint_limits()
self._drive_home()
def test_joint_limits_reaching(self):
""" Perform all reaching tests. Before each test ask the user if he wants to skip it.
"""
for name in self.joint_names:
if not name == _SELF_COLLISION_JOINT_NAME:
if self._ask_for_permission('joint_limit_reaching_test for ' +
name):
self._joint_limit_reaching_test(name)
def test_joint_limits_overstepping(self):
""" Perform all overstepping tests. Before each test ask the user if he wants to skip it.
"""
for name in self.joint_names:
if not name == _SELF_COLLISION_JOINT_NAME:
if self._ask_for_permission(
'joint_limit_overstepping_test for ' + name):
self._joint_limit_overstepping_test(name)
class MoveArm(object):
# Variablen die den Schnitt unmittelbar beeinflussen
offset_tip = 0.102 # Offset in cm (Dicke des Schneidbretts + Abstand zw. Fingerspitze und Klingenunterseite)
blade_len = 0.05 # Laenge der Klinge
ft_limit = 50 # Kraft Grenzwert
ft_threshold = 25 # Kraft Schwellwert
step_down = 0.01 # Standard Schnitttiefe
def __init__(self, enabled=True):
self.enabled = enabled
self.client = SimpleActionClient('/qp_controller/command',
ControllerListAction)
rospy.loginfo('connecting to giskard')
self.client.wait_for_server()
rospy.loginfo('connected to giskard')
self.tip = 'gripper_tool_frame'
self.root = 'base_link'
self.joint_names = [
'shoulder_pan_joint',
'shoulder_lift_joint',
'elbow_joint',
'wrist_1_joint',
'wrist_2_joint',
'wrist_3_joint',
]
self.tfBuffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tfBuffer)
# Subcriber fuer das Topic"/kms40/wrench_zeroed". Wenn Nachrichten empfangen werden, wird die Funktion
# ft_callback aufgerufen.
self.ft_sub = rospy.Subscriber("/kms40/wrench_zeroed", WrenchStamped,
self.ft_callback)
self.ft_list = [] #Liste fuer die gemessenen Kraftwerte.
# Service, um den Offset des Kraftmomentensensors zu aktualisieren.
# Der Service wartet auf ein Objekt vom Typ Trigger.
self.reset_ft = rospy.ServiceProxy("/ft_cleaner/update_offset",
Trigger)
rospy.sleep(1)
self.reset_ft.call(TriggerRequest()) #Trigger Objekt
# Publisher fuer die Position des Endeffektors
self.endeffector_pub = rospy.Publisher('endeffector_position',
PoseStamped)
def send_cart_goal(self,
goal_pose,
translation_weight=1,
rotation_weight=1):
if self.enabled:
goal = ControllerListGoal()
goal.type = ControllerListGoal.STANDARD_CONTROLLER
# translation
controller = Controller()
controller.type = Controller.TRANSLATION_3D
controller.tip_link = self.tip
controller.root_link = self.root
controller.goal_pose = goal_pose
controller.p_gain = 3
controller.enable_error_threshold = True
controller.threshold_value = 0.05
controller.weight = translation_weight
goal.controllers.append(controller)
# rotation
controller = Controller()
controller.type = Controller.ROTATION_3D
controller.tip_link = self.tip
controller.root_link = self.root
controller.goal_pose = goal_pose
controller.p_gain = 3
controller.enable_error_threshold = True
controller.threshold_value = 0.2
controller.weight = rotation_weight
goal.controllers.append(controller)
self.client.send_goal(goal)
result = self.client.wait_for_result(rospy.Duration(10))
print('finished in 10s?: {}'.format(result))
def relative_goal(self,
position,
orientation,
translation_weight=1,
rotation_weight=1):
p = PoseStamped()
p.header.frame_id = self.tip
p.pose.position = Point(*position)
p.pose.orientation = Quaternion(*orientation)
self.send_cart_goal(p, translation_weight, rotation_weight)
def send_joint_goal(self, joint_state):
if self.enabled:
goal = ControllerListGoal()
goal.type = ControllerListGoal.STANDARD_CONTROLLER
# translation
controller = Controller()
controller.type = Controller.JOINT
controller.tip_link = self.tip
controller.root_link = self.root
controller.goal_state = joint_state
controller.p_gain = 3
controller.enable_error_threshold = False
controller.threshold_value = 0.01
controller.weight = 1.0
goal.controllers.append(controller)
self.client.send_goal(goal)
result = self.client.wait_for_result(rospy.Duration(10))
print('finished in 10s?: {}'.format(result))
def ft_callback(self, data):
"""
Callback fuer den Kraft-/Momentensensor
:param data: Sensordaten
:type: WrenchStamped
"""
# Abbruch der Bewegung, wenn gemessene Kraft das Limit ueberschreitet, um Sicherheitsabschaltung des Arms zuvorzukommen.
if abs(data.wrench.force.z) > self.ft_limit:
print("Stop")
self.client.cancel_all_goals()
# Lokalisation des Endeffektors
trans = self.tfBuffer.lookup_transform('arm_mounting_plate', self.tip,
rospy.Time())
p = PoseStamped()
p.header = trans.header
p.pose.position.x = trans.transform.translation.x
p.pose.position.y = trans.transform.translation.y
p.pose.position.z = trans.transform.translation.z
p.pose.orientation.x = trans.transform.rotation.x
p.pose.orientation.y = trans.transform.rotation.y
p.pose.orientation.z = trans.transform.rotation.z
p.pose.orientation.w = trans.transform.rotation.w
self.endeffector_pub.publish(p)
# Der absolute Kraftwert wird ausgelesen und zu einer Liste hinzugefuegt, die die Werte aneinander reiht, um
# spaeter daraus eine Schneidestrategie abzuleiten.
ft = abs(data.wrench.force.z)
self.ft_list.append(ft)
def move_tip_in_amp(self, x, y, z):
"""
Bewegung des Gripper Tool Frame in Bezug auf den Frame 'arm_mounting_plate' (amp).
:type x,y,z: Distanz in Meter
:param x: Bewegung entlang der x-Achse des Frame 'arm_mounting_plate'
:param y: Bewegung entlang der y-Achse des Frame 'arm_mounting_plate'
:param z: Bewegung entlang der z-Achse des Frame 'arm_mounting_plate'
"""
# Ermittelung der Position des Frames Gripper Tool in Bezug auf 'arm_mounting_plate'-Frame
trans = self.tfBuffer.lookup_transform('arm_mounting_plate', self.tip,
rospy.Time())
p = PoseStamped()
p.header.frame_id = 'arm_mounting_plate'
# Die soeben ermittelten Werte werden uebernommen und mit den Werten der gewuenschten Bewegung addiert.
p.pose.position = trans.transform.translation
p.pose.position.x += x
p.pose.position.y += y
p.pose.position.z += z
p.pose.orientation = trans.transform.rotation
cut.send_cart_goal(p)
def distance2table(self):
"""
Berechnung des Abstandes von Klingenunterkante zu Oberflaeche der Schneidunterlage.
:rtype: Distanz in Meter
:return: Abstand zum Tisch
"""
# Abfrage der Position des Frames 'gripper_tool_frame' in Bezug auf 'arm_mounting_plate'.
# Das Frame 'arm_mounting_plate' entspricht dabei der Tischoberkante.
trans = self.tfBuffer.lookup_transform('arm_mounting_plate', self.tip,
rospy.Time())
# Kalkulation des Abstandes von Klingen-Unterseite zum Schneidebrett mit Offset.
distance2table = trans.transform.translation.z - self.offset_tip
return distance2table
def go_to_home(self):
"""
Definition der Standard Position.
:return:
"""
print("Approach Home Pose")
goal_joint_state = JointState()
goal_joint_state.name = self.joint_names
goal_joint_state.position = [
-2.417572323475973, -1.530511204396383, -1.6327641646014612,
-1.5507991949664515, 1.5708668231964111, 1.509663701057434
]
self.send_joint_goal(goal_joint_state)
print("Home Pose Approached")
def align(self):
"""
Ausrichtung des Messers. Das Messer wird nach vorne gekippt, da die Klinge gebogen ist und sonst nicht buendig
mit dem Schneidebrett abschliessen wuerde. Der tiefste Punkt der Klinge befindet sich so zentral ueber dem Objekt.
Ebenfalls wird das Messer um die z-Achse gedreht, da die provisorische Halterung das Messer nicht perfekt
ausgerichtet aufnimmt. Diese Werte sind bei Verwendung von anderem Messer und Halterung anzupassen.
"""
q = quaternion_from_euler(0, -0.15, 0.02, 'ryxz')
cut.relative_goal([0, 0, 0], q)
cut.move_tip_in_amp(-0.01, 0, 0)
# Weitere Bewegungen des Endeffektors, die nicht beruecksichtigt wurden.
# Einfache Schnittbewegung entlang der y-Achse (in Bezug auf gripper_tool_frame) bei gleicher Orientierung des Grippers
# def straight_cut(self):
# d2t = test.distance2table()
# test.relative_goal([0,-d2t,0],[0,0,0,1])
#
# Hackende Bewegung
# def straight_chop(self):
# max_step = 6
# for i in range(max_step):
# test.relative_goal([0,-0.02,0],[0,0,0,1])
# test.relative_goal([0,0.01,0], [0, 0, 0, 1])
#
# Saegende Bewegung
# def saw(self):
# max_step = 6
# for i in range(max_step):
# test.relative_goal([0, -0.005, 0.05], [0, 0, 0, 1])
# test.relative_goal([0, -0.005, -0.05], [0, 0, 0, 1])
#
# Einfache rollende Schnittbewegung
# def roll_simple(self):
# q = quaternion_from_euler(0, 0.3, 0, 'ryxz')
# test.relative_goal([0,0,0],q,translation_weight=100) # Erhohung der Gewichtung der Translation, damit die Spitze genauer in Position bleibt
# test.move_tip_in_amp(0, 0, -0.08)
# q_1 = quaternion_from_euler(0, -0.3, 0, 'ryxz')
# test.relative_goal([0, 0, 0],q_1,translation_weight=100)
#
# Erweiterte rollende Schnittbewegung
# def roll_advanced(self):
# q = quaternion_from_euler(0, 0.3, 0, 'ryxz')
# test.relative_goal([0, 0, 0], q, translation_weight=100)
# test.move_tip_in_amp(0, 0, -0.08)
# test.move_tip_in_amp(-0.05, 0, 0)
# q_1 = quaternion_from_euler(0, -0.3, 0, 'ryxz')
# test.relative_goal([0, 0, 0], q_1, translation_weight=100)
#
#
# def cross_cut(self):
# max_step = 5
# for i in range(max_step):
# q = quaternion_from_euler(0, 0.1, 0, 'ryxz')
# test.relative_goal([0, 0, 0], q, translation_weight=100)
# test.relative_goal([0, -0.01, 0.05], [0, 0, 0, 1])
# q_1 = quaternion_from_euler(0, -0.1, 0, 'ryxz')
# test.relative_goal([0, 0, 0], q_1,translation_weight=100)
# test.relative_goal([0, 0, -0.05], [0, 0, 0, 1])
def master_cut(self):
"""
Funktion fuer die Planung und Ausfuehrung des Schnitte
:return:
"""
# Abfrage des aktuellen Abstands von Klinge zu Schneidebrett.
d2t = cut.distance2table()
# Solange dieser Abstand positiv ist, also sich das Messer oberhalb des Schneidbretts befindet, wird geschnitten.
while d2t > 0:
# Aufruf der Funktion, die die Bewegung unter Beruecksichtig verschiedener Paramenter berechnet und zurueckgibt.
down, side, final = cut.calc_move()
# Bewegung, wenn der gemessene F/T Wert den Schwellwert nicht ueberschritten hat. In diesem Fall erfolgt die
# Bewegung rein entlang der z-Achse.
if side == 0:
cut.move_tip_in_amp(0, 0, -down)
# Wenn F/T-Wert den Grenzwert ueberschreitet, kommt eine Bewegung in x Richtung dazu.
# Dabei wird zunaechst die Klinge ohne Bewegung zurueck gefahren, um von der vollen Klingenlaenge
# zu profitieren. Anschliessend erfolgt eine diagonale Schnittbewegung ueber die gesamte Klingenlaenge.
# Abschliessend eine weitere diagonale Bewegung, um wieder in die Ausgangsposition (x-Achse) zu gelangen.
else:
cut.move_tip_in_amp(-side, 0, -(1 / 4) * down)
cut.move_tip_in_amp(2 * side, 0, -(2 / 4) * down)
cut.move_tip_in_amp(-side, 0, -(1 / 4) * down)
# Wenn die letze Bewegung ausgefuehrte wurde (also Final == True von calc_move() zurueckgegeben wird),
# wird die Funktion beendet. Der Schnittvorgang wird mit Bewegungen entlag der x-Achse abgeschlossen,
# um sicherzustellen, dass das Objekt in Gaenze durchtrennt wird. Abschliessend faehrt das Messer seitlich
# entlang der y-Achse um das abgetrennte Stueck zu separieren.
if final == True:
print("Final")
cut.move_tip_in_amp(-self.blade_len, 0, 0)
cut.move_tip_in_amp(self.blade_len * 1.5, 0, 0)
cut.move_tip_in_amp(-self.blade_len / 2, 0, 0.005)
cut.move_tip_in_amp(0, 0.05, 0)
print("Cut Finished")
return
# Funktion um die Schnittbewegung zu berechnen
def calc_move(self):
"""
Return of three values necessary for cut-move execution.
:return: 1.value:cutting depth; 2.value: lateral move; 3.value: final cut
:type: (float,float,bool)
"""
# Init
final = False
# Abfrage des maximalen F/T-Werts aus der letzten Bewegung
cur_ft = cut.max_ft()
# cur_ft = self.ft_threshold
# Abfrage des aktuellen Abstands von Klingenunterseite zu Schneidebrett
d2t = cut.distance2table()
print("Distance to Table %s" % d2t)
print("Current FT %s" % cur_ft)
# Wenn der Kraftwert den Schwellwert unterschreitet, wird nur entlang der z-Achse geschnitten
if cur_ft < self.ft_threshold:
down = self.step_down
side = 0
# Wenn die Schritttiefe kleiner als der Abstand zur Oberflaeche ist,
# wird die Schritttiefe der naechsten Bewegung auf die verbleibende Distanz gesetzt,
# um Kollisionen mit dem Tisch zu vermeiden.
if d2t <= down:
down = d2t
final = True
else:
# Berechnung der Bewegung, wenn der Kraftschwellwert ueberschritten wird.
# Je hoeher der gemessene Kraftwert, desto geringer die Schnitttiefe.
# Die maximale berechnete Schnitttiefe entspricht der Standardschnitttiefe,
# wenn die Kraft dem Schwellwert entspricht.
down = (self.ft_threshold / cur_ft) * (self.step_down)
# Setzen einer Mindestschnitttiefe
if down < 0.001:
down = 0.001
# Wenn die berechnete Schrittweite kleiner als der Abstand zur Oberflaeche,
# wird die Schrittweite der naechsten Bewegung entsprechend angepasst,
# um Kollisionen mit dem Tisch zu vermeiden.
if d2t <= down:
down = d2t
final = True # Letzte Bewegung
# Die seitliche Bewegung entspricht der Haelfte der Klingenlaenge.
side = self.blade_len / 2
print("Side %s" % side)
print("Down %s" % down)
return (down, side, final)
def max_ft(self):
"""
Funktion um den maximalen F/T waehrend der vergangenen Bewegung auszulesen und zurueckzugeben.
:return: Maximale Kraft waehrend der letzten Bewegung
"""
# Abfrage des max. F/T aus der Liste der F/T Werte
ft_max = max(self.ft_list)
# Nach dem der Wert zwischengespeichert worden ist, wird die Liste fuer den naechsten Durchlauf geleert.
self.ft_list = []
print("Max. FT: %s" % ft_max)
return (ft_max)
#!/usr/bin/env python
import sys
import rospy
from actionlib import SimpleActionClient
from pal_interaction_msgs.msg import TtsAction, TtsGoal
from std_msgs.msg import String
def talk_to_me(words):
#creates the ts goal and sends it off to be spoken
ttg = TtsGoal()
ttg.rawtext.text = words.data
ttg.rawtext.lang_id = "en_GB"
client.send_goal_and_wait(ttg)
#start the node
rospy.init_node('tiago_tts')
# Connect to the text-to-speech action server
client = SimpleActionClient('/tts', TtsAction)
client.wait_for_server()
#Subscribe to get what we want to say
rospy.Subscriber('/hearts/tts', String, talk_to_me)
rospy.spin()
#if __name__ == '__main__':
class PickAruco(object):
def __init__(self):
rospy.loginfo("Initalizing...")
self.bridge = CvBridge()
self.tfBuffer = tf2_ros.Buffer()
self.tf_l = tf2_ros.TransformListener(self.tfBuffer)
rospy.loginfo("Waiting for /pickup_pose AS...")
self.pick_as = SimpleActionClient('/pickup_pose', PickUpPoseAction)
time.sleep(1.0)
if not self.pick_as.wait_for_server(rospy.Duration(20)):
rospy.logerr("Could not connect to /pickup_pose AS")
exit()
rospy.loginfo("Waiting for /place_pose AS...")
self.place_as = SimpleActionClient('/place_pose', PickUpPoseAction)
self.place_as.wait_for_server()
rospy.loginfo("Setting publishers to torso and head controller...")
self.torso_cmd = rospy.Publisher('/torso_controller/command',
JointTrajectory,
queue_size=1)
self.head_cmd = rospy.Publisher('/head_controller/command',
JointTrajectory,
queue_size=1)
self.detected_pose_pub = rospy.Publisher('/detected_aruco_pose',
PoseStamped,
queue_size=1,
latch=True)
rospy.loginfo("Waiting for '/play_motion' AS...")
self.play_m_as = SimpleActionClient('/play_motion', PlayMotionAction)
if not self.play_m_as.wait_for_server(rospy.Duration(20)):
rospy.logerr("Could not connect to /play_motion AS")
exit()
rospy.loginfo("Connected!")
rospy.sleep(1.0)
rospy.loginfo("Done initializing PickAruco.")
def strip_leading_slash(self, s):
return s[1:] if s.startswith("/") else s
def pick_aruco(self, string_operation):
self.prepare_robot()
rospy.sleep(2.0)
rospy.loginfo("spherical_grasp_gui: Waiting for an aruco detection")
aruco_pose = rospy.wait_for_message('/Perception/Pick_Pose',
PoseStamped)
aruco_pose.header.frame_id = self.strip_leading_slash(
aruco_pose.header.frame_id)
rospy.loginfo("Got: " + str(aruco_pose))
rospy.loginfo("spherical_grasp_gui: Transforming from frame: " +
aruco_pose.header.frame_id + " to 'base_footprint'")
ps = PoseStamped()
ps.pose.position = aruco_pose.pose.position
ps.header.stamp = self.tfBuffer.get_latest_common_time(
"base_footprint", aruco_pose.header.frame_id)
ps.header.frame_id = aruco_pose.header.frame_id
transform_ok = False
while not transform_ok and not rospy.is_shutdown():
try:
transform = self.tfBuffer.lookup_transform(
"base_footprint", ps.header.frame_id, rospy.Time(0))
aruco_ps = do_transform_pose(ps, transform)
transform_ok = True
except tf2_ros.ExtrapolationException as e:
rospy.logwarn(
"Exception on transforming point... trying again \n(" +
str(e) + ")")
rospy.sleep(0.01)
ps.header.stamp = self.tfBuffer.get_latest_common_time(
"base_footprint", aruco_pose.header.frame_id)
pick_g = PickUpPoseGoal()
if string_operation == "pick":
rospy.loginfo("Setting cube pose based on bottle detection")
pick_g.object_pose.pose.position = aruco_ps.pose.position
# pick_g.object_pose.pose.position.z -= 0.1*(1.0/2.0)
rospy.loginfo("aruco pose in base_footprint:" + str(pick_g))
pick_g.object_pose.header.frame_id = 'base_footprint'
pick_g.object_pose.pose.orientation.w = 1.0
self.detected_pose_pub.publish(pick_g.object_pose)
rospy.loginfo("Gonna pick:" + str(pick_g))
self.pick_as.send_goal_and_wait(pick_g)
rospy.loginfo("Done!")
result = self.pick_as.get_result()
if str(moveit_error_dict[result.error_code]) != "SUCCESS":
rospy.logerr("Failed to pick, not trying further")
return
# Move torso to its maximum height
self.lift_torso()
# Raise arm
#rospy.loginfo("Moving arm to a safe pose")
#pmg = PlayMotionGoal()
# pmg.motion_name = 'pick_final_pose'
#pmg.skip_planning = False
#self.play_m_as.send_goal_and_wait(pmg)
rospy.loginfo("Raise object done.")
# Place the object back to its position
#rospy.loginfo("Gonna place near where it was")
#pick_g.object_pose.pose.position.z += 0.05
#self.place_as.send_goal_and_wait(pick_g)
#rospy.loginfo("Done!")
def place_aruco(self, string_operation):
rospy.sleep(2.0)
rospy.loginfo("spherical_grasp_gui: Waiting for an put position")
aruco_pose = rospy.wait_for_message('/Perception/Place_Pose',
PoseStamped)
aruco_pose.header.frame_id = self.strip_leading_slash(
aruco_pose.header.frame_id)
aruco_pose.pose.position.x = 0.0
aruco_pose.pose.position.y = 0.0
aruco_pose.pose.position.z = 0.0
rospy.loginfo("Got: " + str(aruco_pose))
rospy.loginfo("spherical_grasp_gui: Transforming from frame: " +
aruco_pose.header.frame_id + " to 'base_footprint'")
ps = PoseStamped()
ps.pose.position = aruco_pose.pose.position
#ps.pose.position.x = 0.9
#ps.pose.position.y = 0.0
#ps.pose.position.z = 0.8
ps.header.stamp = self.tfBuffer.get_latest_common_time(
"base_footprint", aruco_pose.header.frame_id)
ps.header.frame_id = aruco_pose.header.frame_id
transform_ok = False
while not transform_ok and not rospy.is_shutdown():
try:
transform = self.tfBuffer.lookup_transform(
"base_footprint", ps.header.frame_id, rospy.Time(0))
aruco_ps = do_transform_pose(ps, transform)
transform_ok = True
except tf2_ros.ExtrapolationException as e:
rospy.logwarn(
"Exception on transforming point... trying again \n(" +
str(e) + ")")
rospy.sleep(0.01)
ps.header.stamp = self.tfBuffer.get_latest_common_time(
"base_footprint", aruco_pose.header.frame_id)
pick_g = PickUpPoseGoal()
if string_operation == "place":
rospy.loginfo("Setting cube pose based on bottle detection")
pick_g.object_pose.pose.position.x = aruco_ps.pose.position.x + 0.95
pick_g.object_pose.pose.position.y = aruco_ps.pose.position.y
pick_g.object_pose.pose.position.z = aruco_ps.pose.position.z + 1.0
# pick_g.object_pose.pose.position.z -= 0.1*(1.0/2.0)
rospy.loginfo("place pose in base_footprint:" + str(pick_g))
pick_g.object_pose.header.frame_id = 'base_footprint'
pick_g.object_pose.pose.orientation.w = 1.0
self.detected_pose_pub.publish(pick_g.object_pose)
rospy.loginfo("Gonna place:" + str(pick_g))
self.place_as.send_goal_and_wait(pick_g)
rospy.loginfo("Done!")
# Place the object back to its position
# rospy.loginfo("Gonna place near where it was")
# pick_g.object_pose.pose.position.z += 0.05
# self.place_as.send_goal_and_wait(pick_g)
# rospy.loginfo("Done!")
def lift_torso(self):
rospy.loginfo("Moving torso up")
jt = JointTrajectory()
jt.joint_names = ['torso_lift_joint']
jtp = JointTrajectoryPoint()
jtp.positions = [0.34]
jtp.time_from_start = rospy.Duration(2.5)
jt.points.append(jtp)
self.torso_cmd.publish(jt)
def lower_head(self):
rospy.loginfo("Moving head down")
jt = JointTrajectory()
jt.joint_names = ['head_1_joint', 'head_2_joint']
jtp = JointTrajectoryPoint()
jtp.positions = [0.0, -0.45] #0.0, -0.75
jtp.time_from_start = rospy.Duration(2.0)
jt.points.append(jtp)
self.head_cmd.publish(jt)
rospy.loginfo("Done.")
def prepare_robot(self):
rospy.loginfo("Unfold arm safely")
pmg = PlayMotionGoal()
pmg.motion_name = 'pregrasp'
pmg.skip_planning = False
self.play_m_as.send_goal_and_wait(pmg)
rospy.loginfo("Done.")
self.lower_head()
rospy.loginfo("Robot prepared.")
class PickAndPlaceServer(object):
def __init__(self):
rospy.loginfo("Initalizing PickAndPlaceServer...")
self.sg = SphericalGrasps()
rospy.loginfo("Connecting to pickup AS")
self.pickup_ac = SimpleActionClient('/pickup', PickupAction)
self.pickup_ac.wait_for_server()
rospy.loginfo("Succesfully connected.")
rospy.loginfo("Connecting to place AS")
self.place_ac = SimpleActionClient('/place', PlaceAction)
self.place_ac.wait_for_server()
rospy.loginfo("Succesfully connected.")
self.scene = PlanningSceneInterface()
rospy.loginfo("Connecting to /get_planning_scene service")
self.scene_srv = rospy.ServiceProxy('/get_planning_scene',
GetPlanningScene)
self.scene_srv.wait_for_service()
rospy.loginfo("Connected.")
rospy.loginfo("Connecting to clear octomap service...")
self.clear_octomap_srv = rospy.ServiceProxy('/clear_octomap', Empty)
self.clear_octomap_srv.wait_for_service()
rospy.loginfo("Connected!")
# Get the object size
self.object_height = rospy.get_param('~object_height')
self.object_width = rospy.get_param('~object_width')
self.object_depth = rospy.get_param('~object_depth')
# Get the links of the end effector exclude from collisions
self.links_to_allow_contact = rospy.get_param(
'~links_to_allow_contact', None)
if self.links_to_allow_contact is None:
rospy.logwarn(
"Didn't find any links to allow contacts... at param ~links_to_allow_contact"
)
else:
rospy.loginfo("Found links to allow contacts: " +
str(self.links_to_allow_contact))
self.pick_as = SimpleActionServer('/pickup_pose',
PickUpPoseAction,
execute_cb=self.pick_cb,
auto_start=False)
self.pick_as.start()
self.place_as = SimpleActionServer('/place_pose',
PickUpPoseAction,
execute_cb=self.place_cb,
auto_start=False)
self.place_as.start()
def pick_cb(self, goal):
"""
:type goal: PickUpPoseGoal
"""
error_code = self.grasp_object(goal.object_pose)
p_res = PickUpPoseResult()
p_res.error_code = error_code
if error_code != 1:
self.pick_as.set_aborted(p_res)
else:
self.pick_as.set_succeeded(p_res)
def place_cb(self, goal):
"""
:type goal: PickUpPoseGoal
"""
error_code = self.place_object(goal.object_pose)
p_res = PickUpPoseResult()
p_res.error_code = error_code
if error_code != 1:
self.place_as.set_aborted(p_res)
else:
self.place_as.set_succeeded(p_res)
def wait_for_planning_scene_object(self, object_name='part'):
rospy.loginfo("Waiting for object '" + object_name +
"'' to appear in planning scene...")
gps_req = GetPlanningSceneRequest()
gps_req.components.components = gps_req.components.WORLD_OBJECT_NAMES
part_in_scene = False
while not rospy.is_shutdown() and not part_in_scene:
# This call takes a while when rgbd sensor is set
gps_resp = self.scene_srv.call(gps_req)
# check if 'part' is in the answer
for collision_obj in gps_resp.scene.world.collision_objects:
if collision_obj.id == object_name:
part_in_scene = True
break
else:
rospy.sleep(1.0)
rospy.loginfo("'" + object_name + "'' is in scene!")
def grasp_object(self, object_pose):
rospy.loginfo("Removing any previous 'part' object")
self.scene.remove_attached_object("arm_tool_link")
self.scene.remove_world_object("part")
self.scene.remove_world_object("table")
rospy.loginfo("Clearing octomap")
self.clear_octomap_srv.call(EmptyRequest())
rospy.sleep(2.0) # Removing is fast
rospy.loginfo("Adding new 'part' object")
rospy.loginfo("Object pose: %s", object_pose.pose)
#Add object description in scene
self.scene.add_box(
"part", object_pose,
(self.object_depth, self.object_width, self.object_height))
rospy.loginfo("Second%s", object_pose.pose)
self.add_table(copy.deepcopy(object_pose))
# compute grasps
possible_grasps = self.sg.create_grasps_from_object_pose(object_pose)
self.pickup_ac
goal = createPickupGoal("arm_torso", "part", object_pose,
possible_grasps, self.links_to_allow_contact)
rospy.loginfo("Sending goal")
self.pickup_ac.send_goal(goal)
rospy.loginfo("Waiting for result")
self.pickup_ac.wait_for_result()
result = self.pickup_ac.get_result()
rospy.logdebug("Using torso result: " + str(result))
rospy.loginfo("Pick result: " +
str(moveit_error_dict[result.error_code.val]))
return result.error_code.val
def place_object(self, object_pose):
rospy.loginfo("Clearing octomap")
self.clear_octomap_srv.call(EmptyRequest())
self.add_table(copy.deepcopy(object_pose))
possible_placings = self.sg.create_placings_from_object_pose(
object_pose)
# Try only with arm
rospy.loginfo("Trying to place using only arm")
goal = createPlaceGoal(object_pose, possible_placings, "arm", "part",
self.links_to_allow_contact)
rospy.loginfo("Sending goal")
self.place_ac.send_goal(goal)
rospy.loginfo("Waiting for result")
self.place_ac.wait_for_result()
result = self.place_ac.get_result()
rospy.loginfo(str(moveit_error_dict[result.error_code.val]))
if str(moveit_error_dict[result.error_code.val]) != "SUCCESS":
rospy.loginfo("Trying to place with arm and torso")
# Try with arm and torso
goal = createPlaceGoal(object_pose, possible_placings, "arm_torso",
"part", self.links_to_allow_contact)
rospy.loginfo("Sending goal")
self.place_ac.send_goal(goal)
rospy.loginfo("Waiting for result")
self.place_ac.wait_for_result()
result = self.place_ac.get_result()
rospy.logerr(str(moveit_error_dict[result.error_code.val]))
# print result
rospy.loginfo("Result: " +
str(moveit_error_dict[result.error_code.val]))
rospy.loginfo("Removing previous 'part' object")
self.scene.remove_world_object("part")
return result.error_code.val
def add_table(self, object_pose):
table_pose = object_pose
table_pose.pose.orientation = Quaternion()
#define a virtual table below the object
table_height = object_pose.pose.position.z - self.object_width / 2 + 0.075
table_width = 1.8
table_depth = 1.0
table_pose.pose.position.x += 0.35 #0.3
table_pose.pose.position.z += -(
2 * self.object_width) / 2 - table_height / 2
#table_height -= 0.008 #remove few milimeters to prevent contact between the object and the table
self.scene.add_box("table", table_pose,
(table_depth, table_width, table_height))
# # We need to wait for the object part to appear
#self.wait_for_planning_scene_object()
self.wait_for_planning_scene_object("table")
class StateMachine(object):
def callback_pick_pos(self, msg):
self.next_goal.target_pose = msg
def callback_place_pos(self, msg):
self.place_goal.target_pose = msg
def pos_check_aruco_there(self, msg):
self.time_data = msg.header.stamp.secs
self.z_coordinate_cube = msg.pose.position.z
def check_it_get_table(self, msg):
self.x_coord_robot = msg.pose.pose.position.x
self.y_coord_robot = msg.pose.pose.position.y
def __init__(self):
self.node_name = "Student SM"
# Access rosparams
self.cmd_vel_top = rospy.get_param(rospy.get_name() + '/cmd_vel_topic')
self.mv_head_srv_nm = rospy.get_param(rospy.get_name() +
'/move_head_srv')
self.pick_srv_nm = rospy.get_param(rospy.get_name() + '/pick_srv')
# Added Hector
self.place_srv_nm = rospy.get_param(rospy.get_name() + '/place_srv')
self.pick_top = rospy.get_param(rospy.get_name() + '/pick_pose_topic')
self.place_top = rospy.get_param(rospy.get_name() +
'/place_pose_topic')
self.aruco_pose_top = rospy.get_param(rospy.get_name() +
'/aruco_pose_topic')
self.cube_pose = rospy.get_param(rospy.get_name() + '/cube_pose')
self.localize_myself = rospy.get_param(rospy.get_name() +
'/global_loc_srv')
self.clear_cost_map = rospy.get_param(rospy.get_name() +
'/clear_costmaps_srv')
rospy.loginfo("%s: ...A...", self.node_name)
# Subscribe to topics
self.next_goal = MoveBaseGoal()
self.next_goal.target_pose.header.frame_id = "map"
self.pick_pose_sub = rospy.Subscriber(self.pick_top, PoseStamped,
self.callback_pick_pos)
self.place_goal = MoveBaseGoal()
self.place_goal.target_pose.header.frame_id = "map"
self.place_pose_sub = rospy.Subscriber(self.place_top, PoseStamped,
self.callback_place_pos)
self.check_coord = rospy.Subscriber(self.aruco_pose_top, PoseStamped,
self.pos_check_aruco_there)
# ADDED AT LAST movement
self.check_coord = rospy.Subscriber("/amcl_pose",
PoseWithCovarianceStamped,
self.check_it_get_table)
# Wait for service providers
rospy.wait_for_service(self.mv_head_srv_nm, timeout=30)
# ADDED HECTORstd_srvs
rospy.wait_for_service(self.pick_srv_nm, timeout=30)
# Instantiate publishers
self.cmd_vel_pub = rospy.Publisher(self.cmd_vel_top,
Twist,
queue_size=10)
#self.cmd_vel_pub = rospy.Publisher("move_base/goal", Twist, queue_size=10)
# Set up action clients
rospy.loginfo("%s: Waiting for play_motion action server...",
self.node_name)
self.play_motion_ac = SimpleActionClient("/play_motion",
PlayMotionAction)
if not self.play_motion_ac.wait_for_server(rospy.Duration(1000)):
rospy.logerr("%s: Could not connect to /play_motion action server",
self.node_name)
exit()
rospy.loginfo("%s: Connected to play_motion action server",
self.node_name)
self.move_to_dir = SimpleActionClient("/move_base", MoveBaseAction)
if not self.move_to_dir.wait_for_server(rospy.Duration(1000)):
rospy.logerr("%s: Could not connect to /move_base action server",
self.node_name)
exit()
rospy.loginfo("%s: Connected to move_base action server",
self.node_name)
# Init state machine
self.state = 0
#rospy.sleep(1)
self.check_states()
def check_states(self):
while not rospy.is_shutdown() and self.state != 6:
# State 0: Tuck arm
if self.state == 0:
rospy.loginfo("%s: Tucking the arm...", self.node_name)
goal = PlayMotionGoal()
goal.motion_name = 'home'
goal.skip_planning = True
self.play_motion_ac.send_goal(goal)
fail_tucking = self.play_motion_ac.wait_for_result(
rospy.Duration(10.0))
if fail_tucking:
self.play_motion_ac.cancel_goal()
rospy.logerr(
"%s: play_motion failed to tuck arm, reset simulation",
self.node_name)
self.state = 6
else:
rospy.loginfo("%s: Arm tucked.", self.node_name)
self.state = 9
#rospy.sleep(1)
# State 9: Localize myself
if self.state == 9:
rospy.loginfo("%s: STATE 0", self.node_name)
localize_myself_var = rospy.ServiceProxy(
self.localize_myself, Empty)
localize_req = localize_myself_var()
rospy.loginfo("%s: Localization initialized", self.node_name)
self.state = 10
#rospy.sleep(1)
# State 10: Turn around in order to localize in the right direction
if self.state == 10:
rate = rospy.Rate(10)
movement = Twist()
movement.angular.z = 1
cnt = 0
while cnt < 20:
self.cmd_vel_pub.publish(movement)
rate.sleep()
cnt = cnt + 1
self.state = 11
rospy.loginfo(
"%s: After turning Im supposed to know where am I",
self.node_name)
#rospy.sleep(1)
# State 11: Clear costmaps
if self.state == 11:
clear_cost_m = rospy.ServiceProxy(self.clear_cost_map, Empty)
clear_cost_m()
self.state = 12
rospy.loginfo("%s: Costmaps cleared", self.node_name)
#rospy.sleep(1)
# State 12: Send navigation goal
if self.state == 12:
self.move_to_dir.wait_for_server()
self.move_to_dir.send_goal(self.next_goal)
reached_point = self.move_to_dir.wait_for_result()
#rospy.sleep(1)
if reached_point:
self.move_to_dir.cancel_goal()
rospy.loginfo("%s: I reach the table to pick the cube",
self.node_name)
self.state = 13
else:
rospy.logerr("%s: I couldnt reach the table",
self.node_name)
self.state = 6
# State 13: Move head down
if self.state == 13:
try:
rospy.loginfo("%s: Lowering robot head", self.node_name)
move_head_srv = rospy.ServiceProxy(self.mv_head_srv_nm,
MoveHead)
move_head_req = move_head_srv("down")
if move_head_req.success == True:
self.state = 14
rospy.loginfo("%s: Move head down succeded!",
self.node_name)
else:
rospy.logerr("%s: Move head down failed!",
self.node_name)
self.state = 6
#rospy.sleep(3)
except rospy.ServiceException, e:
print "Service call to move_head server failed: %s" % e
# State 14: Pick cube
if self.state == 14:
rospy.loginfo("%s: Picking the cube...", self.node_name)
pick_the_cube = rospy.ServiceProxy(self.pick_srv_nm, SetBool)
pick_cube_req = pick_the_cube(True)
if pick_cube_req.success == True:
self.state = 15
rospy.loginfo("%s: Pick cube succeded!", self.node_name)
else:
rospy.loginfo("%s: Pick cube failed!", self.node_name)
self.state = 6
#rospy.sleep(1)
# State 15: Clear costmaps
if self.state == 15:
clear_cost_m = rospy.ServiceProxy(self.clear_cost_map, Empty)
clear_cost_m()
self.state = 16
rospy.loginfo("%s: Costmaps cleared", self.node_name)
#rospy.sleep(1)
# State 16: Heads up
if self.state == 16:
try:
rospy.loginfo("%s: Upping robot head", self.node_name)
move_head_srv = rospy.ServiceProxy(self.mv_head_srv_nm,
MoveHead)
move_head_req = move_head_srv("up")
if move_head_req.success == True:
self.state = 17
rospy.loginfo("%s: Move head doupwn succeded!",
self.node_name)
else:
rospy.logerr("%s: Move head up failed!",
self.node_name)
self.state = 6
#rospy.sleep(3)
except rospy.ServiceException, e:
print "Service call to move_head server failed: %s" % e
# State 15: Move the robot to chair
if self.state == 17:
self.move_to_dir.wait_for_server()
self.move_to_dir.send_goal(self.place_goal)
reached_point = self.move_to_dir.wait_for_result()
rospy.sleep(1)
if reached_point:
self.move_to_dir.cancel_goal()
rospy.logerr("%s: I reach the table to place the cube",
self.node_name)
self.state = 21
else:
rospy.loginfo(
"%s: I couldnt reach the table to place the cube",
self.node_name)
self.state = 6
#rospy.sleep(1)
# ADDED AT THE END
if self.state == 21:
if (((self.x_coord_robot - 2.6009)**2) +
((self.y_coord_robot + 1.7615)**2) > 1):
rospy.logerr("%s: DESTINATION NOT REACHED", self.node_name)
self.state = 6
else:
rospy.loginfo("%s: I REACH THE TABLE", self.node_name)
self.state = 18
# State 18: Lower robot head service
if self.state == 18:
try:
rospy.loginfo("%s: Lowering robot head", self.node_name)
move_head_srv = rospy.ServiceProxy(self.mv_head_srv_nm,
MoveHead)
move_head_req = move_head_srv("down")
if move_head_req.success == True:
self.state = 19
rospy.loginfo("%s: Move head down succeded!",
self.node_name)
else:
rospy.logerr("%s: Move head down failed!",
self.node_name)
self.state = 6
#rospy.sleep(1)
except rospy.ServiceException, e:
print "Service call to move_head server failed: %s" % e
class TestMoveActionCancelDrop(unittest.TestCase):
def setUp(self):
# create a action client of move group
self._move_client = SimpleActionClient("move_group", MoveGroupAction)
self._move_client.wait_for_server()
moveit_commander.roscpp_initialize(sys.argv)
group_name = moveit_commander.RobotCommander().get_group_names()[0]
group = moveit_commander.MoveGroupCommander(group_name)
# prepare a joint goal
self._goal = MoveGroupGoal()
self._goal.request.group_name = group_name
self._goal.request.max_velocity_scaling_factor = 0.1
self._goal.request.max_acceleration_scaling_factor = 0.1
self._goal.request.start_state.is_diff = True
goal_constraint = Constraints()
joint_values = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]
joint_names = group.get_active_joints()
for i in range(len(joint_names)):
joint_constraint = JointConstraint()
joint_constraint.joint_name = joint_names[i]
joint_constraint.position = joint_values[i]
joint_constraint.weight = 1.0
goal_constraint.joint_constraints.append(joint_constraint)
self._goal.request.goal_constraints.append(goal_constraint)
self._goal.planning_options.planning_scene_diff.robot_state.is_diff = True
def test_cancel_drop_plan_execution(self):
# send the goal
self._move_client.send_goal(self._goal)
# cancel the goal
self._move_client.cancel_goal()
# wait for result
self._move_client.wait_for_result()
# polling the result, since result can come after the state is Done
result = None
while result is None:
result = self._move_client.get_result()
rospy.sleep(0.1)
# check the error code in result
# error code is 0 if the server ends with RECALLED status
self.assertTrue(
result.error_code.val == MoveItErrorCodes.PREEMPTED
or result.error_code.val == 0
)
def test_cancel_drop_plan_only(self):
# set the plan only flag
self._goal.planning_options.plan_only = True
# send the goal
self._move_client.send_goal(self._goal)
# cancel the goal
self._move_client.cancel_goal()
# wait for result
self._move_client.wait_for_result()
# polling the result, since result can come after the state is Done
result = None
while result is None:
result = self._move_client.get_result()
rospy.sleep(0.1)
# check the error code in result
# error code is 0 if the server ends with RECALLED status
self.assertTrue(
result.error_code.val == MoveItErrorCodes.PREEMPTED
or result.error_code.val == 0
)
def test_cancel_resend(self):
# send the goal
self._move_client.send_goal(self._goal)
# cancel the goal
self._move_client.cancel_goal()
# send the goal again
self._move_client.send_goal(self._goal)
# wait for result
self._move_client.wait_for_result()
# polling the result, since result can come after the state is Done
result = None
while result is None:
result = self._move_client.get_result()
rospy.sleep(0.1)
# check the error code in result
self.assertEqual(result.error_code.val, MoveItErrorCodes.SUCCESS)
class Effector(object):
""" Controls the end effector planner. """
def __init__(self, verbose=False):
self.verbose = verbose
self.client = Client(topics.move_end_effector_controller,
MoveEndEffectorAction)
goal = MoveEndEffectorGoal(command=MoveEndEffectorGoal.TEST)
func = partial(retry_action,
client=self.client,
goal=goal,
timeout=20,
msg='Test end effector ')
setattr(self, 'test', func)
goal = MoveEndEffectorGoal(command=MoveEndEffectorGoal.PARK)
func = partial(retry_action,
client=self.client,
goal=goal,
timeout=20,
msg='Park end effector ')
setattr(self, 'park', func)
def cancel_all_goals(self):
log.debug('Waiting for the EndEffector action server...')
self.client.wait_for_server()
log.info('Canceling all goals on EndEffector.')
self.client.cancel_all_goals()
sleep(3)
def point_to(self, target, center='/front_picam_frame'):
""" Points end effector to a target.
:param target: The victim frame ID.
:param center: The center of the frame the we will use.
"""
goal = MoveEndEffectorGoal()
goal.command = MoveEndEffectorGoal.TRACK
goal.point_of_interest = target
goal.center_point = center
log.debug('Waiting for the EndEffector action server.')
self.client.wait_for_server()
log.info('Sending TRACK goal.')
self.client.send_goal(goal)
def slowly_point_to(self, target, center='/front_picam_frame'):
"""
Points end effector to a target, slow enough so the image
is staying still.
:param target: The victim frame ID.
:param center: The center of the frame the we will use.
"""
goal = MoveEndEffectorGoal()
goal.command = MoveEndEffectorGoal.LAX_TRACK
goal.point_of_interest = target
goal.center_point = center
log.debug('Waiting for the EndEffector action server.')
self.client.wait_for_server()
log.info('Sending LAX TRACK goal.')
self.client.send_goal(goal)
sleep(5)
def scan(self):
""" The end effector starts scanning. """
goal = MoveEndEffectorGoal()
goal.command = MoveEndEffectorGoal.SCAN
log.debug('Waiting for the EndEffector action server.')
self.client.wait_for_server()
log.info('Sending SCAN goal.')
self.client.send_goal(goal)
class SmartGrasper(object):
"""
This is the helper library to easily access the different functionalities of the simulated robot
from python.
"""
__last_joint_state = None
__current_model_name = "cricket_ball"
__path_to_models = "/root/.gazebo/models/"
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("/gazebo/delete_model")
self.__delete_model = rospy.ServiceProxy("/gazebo/delete_model", DeleteModel)
rospy.wait_for_service("/gazebo/spawn_sdf_model")
self.__spawn_model = rospy.ServiceProxy("/gazebo/spawn_sdf_model", SpawnModel)
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()
def reset_world(self):
"""
Resets the object poses in the world and the robot joint angles.
"""
self.__switch_ctrl.call(start_controllers=[],
stop_controllers=["hand_controller", "arm_controller", "joint_state_controller"],
strictness=SwitchControllerRequest.BEST_EFFORT)
self.__pause_physics.call()
joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint', 'H1_F1J1', 'H1_F1J2', 'H1_F1J3',
'H1_F2J1', 'H1_F2J2', 'H1_F2J3', 'H1_F3J1', 'H1_F3J2', 'H1_F3J3']
joint_positions = [1.2, 0.3, -1.5, -0.5, -1.5, 0.0, 0.0, -0.3, 0.0, 0.0, -0.3, 0.0, 0.0, -0.3, 0.0]
self.__set_model.call(model_name="smart_grasping_sandbox",
urdf_param_name="robot_description",
joint_names=joint_names,
joint_positions=joint_positions)
timer = Timer(0.0, self.__start_ctrl)
timer.start()
time.sleep(0.1)
self.__unpause_physics.call()
self.__reset_world.call()
def get_object_pose(self):
"""
Gets the pose of the ball in the world frame.
@return The pose of the ball.
"""
return self.__get_pose_srv.call(self.__current_model_name, "world").pose
def get_tip_pose(self):
"""
Gets the current pose of the robot's tooltip in the world frame.
@return the tip pose
"""
return self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose
def move_tip_absolute(self, target):
"""
Moves the tooltip to the absolute target in the world frame
@param target is a geometry_msgs.msg.Pose
@return True on success
"""
self.arm_commander.set_start_state_to_current_state()
self.arm_commander.set_pose_targets([target])
plan = self.arm_commander.plan()
if not self.arm_commander.execute(plan):
return False
return True
def move_tip(self, x=0., y=0., z=0., roll=0., pitch=0., yaw=0.):
"""
Moves the tooltip in the world frame by the given x,y,z / roll,pitch,yaw.
@return True on success
"""
transform = PyKDL.Frame(PyKDL.Rotation.RPY(pitch, roll, yaw),
PyKDL.Vector(-x, -y, -z))
tip_pose = self.get_tip_pose()
tip_pose_kdl = posemath.fromMsg(tip_pose)
final_pose = toMsg(tip_pose_kdl * transform)
self.arm_commander.set_start_state_to_current_state()
self.arm_commander.set_pose_targets([final_pose])
plan = self.arm_commander.plan()
if not self.arm_commander.execute(plan):
return False
return True
def send_command(self, command, duration=0.2):
"""
Send a dictionnary of joint targets to the arm and hand directly.
@param command: a dictionnary of joint names associated with a target:
{"H1_F1J1": -1.0, "shoulder_pan_joint": 1.0}
@param duration: the amount of time it will take to get there in seconds. Needs to be bigger than 0.0
"""
hand_goal = None
arm_goal = None
for joint, target in command.items():
if "H1" in joint:
if not hand_goal:
hand_goal = FollowJointTrajectoryGoal()
point = JointTrajectoryPoint()
point.time_from_start = rospy.Duration.from_sec(duration)
hand_goal.trajectory.points.append(point)
hand_goal.trajectory.joint_names.append(joint)
hand_goal.trajectory.points[0].positions.append(target)
else:
if not arm_goal:
arm_goal = FollowJointTrajectoryGoal()
point = JointTrajectoryPoint()
point.time_from_start = rospy.Duration.from_sec(duration)
arm_goal.trajectory.points.append(point)
arm_goal.trajectory.joint_names.append(joint)
arm_goal.trajectory.points[0].positions.append(target)
if arm_goal:
self.__arm_traj_client.send_goal_and_wait(arm_goal)
if hand_goal:
self.__hand_traj_client.send_goal_and_wait(hand_goal)
def get_current_joint_state(self):
"""
Gets the current state of the robot.
@return joint positions, velocity and efforts as three dictionnaries
"""
joints_position = {n: p for n, p in
zip(self.__last_joint_state.name,
self.__last_joint_state.position)}
joints_velocity = {n: v for n, v in
zip(self.__last_joint_state.name,
self.__last_joint_state.velocity)}
joints_effort = {n: v for n, v in
zip(self.__last_joint_state.name,
self.__last_joint_state.effort)}
return joints_position, joints_velocity, joints_effort
def open_hand(self):
"""
Opens the hand.
@return True on success
"""
self.hand_commander.set_named_target("open")
plan = self.hand_commander.plan()
if not self.hand_commander.execute(plan, wait=True):
return False
return True
def close_hand(self):
"""
Closes the hand.
@return True on success
"""
self.hand_commander.set_named_target("close")
plan = self.hand_commander.plan()
if not self.hand_commander.execute(plan, wait=True):
return False
return True
def check_fingers_collisions(self, enable=True):
"""
Disables or enables the collisions check between the fingers and the objects / table
@param enable: set to True to enable / False to disable
@return True on success
"""
objects = ["cricket_ball__link", "drill__link", "cafe_table__link"]
while self.__pub_planning_scene.get_num_connections() < 1:
rospy.loginfo("waiting for someone to subscribe to the /planning_scene")
rospy.sleep(0.1)
request = PlanningSceneComponents(components=PlanningSceneComponents.ALLOWED_COLLISION_MATRIX)
response = self.__get_planning_scene(request)
acm = response.scene.allowed_collision_matrix
for object_name in objects:
if object_name not in acm.entry_names:
# add object to allowed collision matrix
acm.entry_names += [object_name]
for row in range(len(acm.entry_values)):
acm.entry_values[row].enabled += [False]
new_row = deepcopy(acm.entry_values[0])
acm.entry_values += {new_row}
for index_entry_values, entry_values in enumerate(acm.entry_values):
if "H1_F" in acm.entry_names[index_entry_values]:
for index_value, _ in enumerate(entry_values.enabled):
if acm.entry_names[index_value] in objects:
if enable:
acm.entry_values[index_entry_values].enabled[index_value] = False
else:
acm.entry_values[index_entry_values].enabled[index_value] = True
elif acm.entry_names[index_entry_values] in objects:
for index_value, _ in enumerate(entry_values.enabled):
if "H1_F" in acm.entry_names[index_value]:
if enable:
acm.entry_values[index_entry_values].enabled[index_value] = False
else:
acm.entry_values[index_entry_values].enabled[index_value] = True
planning_scene_diff = PlanningScene(is_diff=True, allowed_collision_matrix=acm)
self.__pub_planning_scene.publish(planning_scene_diff)
rospy.sleep(1.0)
return True
def pick(self):
"""
Does its best to pick the ball.
"""
rospy.loginfo("Moving to Pregrasp")
self.open_hand()
time.sleep(0.1)
ball_pose = self.get_object_pose()
ball_pose.position.z += 0.5
#setting an absolute orientation (from the top)
quaternion = quaternion_from_euler(-pi/2., 0.0, 0.0)
ball_pose.orientation.x = quaternion[0]
ball_pose.orientation.y = quaternion[1]
ball_pose.orientation.z = quaternion[2]
ball_pose.orientation.w = quaternion[3]
self.move_tip_absolute(ball_pose)
time.sleep(0.1)
rospy.loginfo("Grasping")
self.move_tip(y=-0.164)
time.sleep(0.1)
self.check_fingers_collisions(False)
time.sleep(0.1)
self.close_hand()
time.sleep(0.1)
rospy.loginfo("Lifting")
for _ in range(5):
self.move_tip(y=0.01)
time.sleep(0.1)
self.check_fingers_collisions(True)
def swap_object(self, new_model_name):
"""
Replaces the current object with a new one.Replaces
@new_model_name the name of the folder in which the object is (e.g. beer)
"""
try:
self.__delete_model(self.__current_model_name)
except:
rospy.logwarn("Failed to delete: " + self.__current_model_name)
try:
sdf = None
initial_pose = Pose()
initial_pose.position.x = 0.15
initial_pose.position.z = 0.82
with open(self.__path_to_models + new_model_name + "/model.sdf", "r") as model:
sdf = model.read()
res = self.__spawn_model(new_model_name, sdf, "", initial_pose, "world")
rospy.logerr( "RES: " + str(res) )
self.__current_model_name = new_model_name
except:
rospy.logwarn("Failed to delete: " + self.__current_model_name)
def __compute_arm_target_for_ball(self):
ball_pose = self.get_object_pose()
# come at it from the top
arm_target = ball_pose
arm_target.position.z += 0.5
quaternion = quaternion_from_euler(-pi/2., 0.0, 0.0)
arm_target.orientation.x = quaternion[0]
arm_target.orientation.y = quaternion[1]
arm_target.orientation.z = quaternion[2]
arm_target.orientation.w = quaternion[3]
return arm_target
def __pre_grasp(self, arm_target):
self.hand_commander.set_named_target("open")
plan = self.hand_commander.plan()
self.hand_commander.execute(plan, wait=True)
for _ in range(10):
self.arm_commander.set_start_state_to_current_state()
self.arm_commander.set_pose_targets([arm_target])
plan = self.arm_commander.plan()
if self.arm_commander.execute(plan):
return True
def __grasp(self, arm_target):
waypoints = []
waypoints.append(self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose)
arm_above_ball = deepcopy(arm_target)
arm_above_ball.position.z -= 0.12
waypoints.append(arm_above_ball)
self.arm_commander.set_start_state_to_current_state()
(plan, fraction) = self.arm_commander.compute_cartesian_path(waypoints, 0.01, 0.0)
print fraction
if not self.arm_commander.execute(plan):
return False
self.hand_commander.set_named_target("close")
plan = self.hand_commander.plan()
if not self.hand_commander.execute(plan, wait=True):
return False
self.hand_commander.attach_object("cricket_ball__link")
def __lift(self, arm_target):
waypoints = []
waypoints.append(self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose)
arm_above_ball = deepcopy(arm_target)
arm_above_ball.position.z += 0.1
waypoints.append(arm_above_ball)
self.arm_commander.set_start_state_to_current_state()
(plan, fraction) = self.arm_commander.compute_cartesian_path(waypoints, 0.01, 0.0)
print fraction
if not self.arm_commander.execute(plan):
return False
def __start_ctrl(self):
rospy.loginfo("STARTING CONTROLLERS")
self.__switch_ctrl.call(start_controllers=["hand_controller", "arm_controller", "joint_state_controller"],
stop_controllers=[], strictness=1)
def __joint_state_cb(self, msg):
self.__last_joint_state = msg
class GetCoffee(object):
def __init__(self):
self.ask_timeout = 10 #number of seconds to wait for an answer re coffee
self.get_coffee_timeout = 60.0 * 2 #number of seconds to wait for a coffee at the machine
self.deliver_timeout = 30 #number of seconds to wait for the person to retrieve the coffee
self.coffee_wp = 'WayPoint8' #location of the coffee machine
self.people = ['John'] #people to serve
self.initial_location = {
'John': {
'WayPoint1': 0.8,
'WayPoint2': 0.05
}
} #distribution of people's location in the environment
self.wants_coffee = {
'John': 0.7
} #probability of people wanting coffee
# define the mdp action client
self.mdp_client = SimpleActionClient(
'mdp_plan_exec/execute_policy_extended',
ExecutePolicyExtendedAction)
self.mdp_client.wait_for_server()
# build planning problem
model = self.build_mdp_spec()
# send goal to mdp action server
self.mdp_client.send_goal(ExecutePolicyExtendedGoal(spec=model))
self.mdp_client.wait_for_result()
def build_mdp_spec(self):
mdp_vars = [] #the state features other than robot location
actions = [] #the actions other than navigation
for person in self.people: #looping for each person
asked_var_name = 'asked_' + person
asked_var = MdpStateVar(
name=asked_var_name, init_val=0, min_range=0, max_range=1
) #boolean state feature representing whether the person was asked whether they want coffee. This will only become true after the person replies to the question at a certain location.
mdp_vars += [asked_var]
for (
possible_init_location, prob
) in self.initial_location[person].items(
): #looping for each possible location of this person and corresponding probability
wants_var_name = person + 'wants_at_' + possible_init_location
wants_at_var = MdpStateVar(
name=wants_var_name,
init_val=-1,
min_range=-1,
max_range=1
) #state feature representing whether person wants coffee at this location. -1 represents the robot has not asked, 0 represents the person responded no coffee, 1 represents the person responding yes to coffee. In this case we need to remember the location to bring the coffee back, so we add a feature for each possible (person, location) pair.
mdp_vars += [wants_at_var]
#action encoding of asking person whether they want coffee, at possible_init_location
ask_action = MdpAction(
name="ask_" + person + '_at_' +
possible_init_location, #the name of the action to be used in the MDP model
action_server=
'ask_question_server', #the actionlib server implementing the behaviour. This will be called by the policy executor
waypoints=[
possible_init_location
], #the waypoint(s) where the behavior can be executed
pre_conds=[
StringIntPair(wants_var_name, -1),
StringIntPair(asked_var_name, 0)
], #preconditions for this action: it must be unknown whether the person is at this location and the person must not have been asked whether they want coffee
outcomes=[]) #will be filled later
#adding arguments to be passed to the 'ask_question_server' action server during execution. These are in line with the definition of the action msg for the action server located at ros_ws/src/isr_monarch_robot/mbot_task_planning/mbot_action_msgs/action/AskQuestion.action.
mu.add_string_argument(ask_action, "Hi " + person +
". Would you like some coffee?"
) #What the robot should say.
mu.add_string_argument(ask_action,
"yes") #Answer to trigger success
mu.add_float_argument(
ask_action, self.ask_timeout) #waiting for reply timeout
mu.add_string_argument(
ask_action, "OK, I will bring it as soon as I can."
) #What to say if the person was here and said yes to coffee
mu.add_string_argument(
ask_action,
"Good call, caffeine is bad for you and I have more important things to do."
) #what to say if the person was here but said no to coffee
mu.add_string_argument(ask_action, "Looks like no one is here."
) #what to say if person is not here
#the outcomes for the asking action
person_here_prob = self.initial_location[person][
possible_init_location] #probability the person is in location possible_init_location
#person was not in possible_init_location
no_person_outcome = MdpActionOutcome(
probability=1 -
person_here_prob, #probability of this outcome, to be used in the MDP model
waypoint=
possible_init_location, #location of the robot after executing the action. In this case the robot does not move, so it ends in the same place. This will be the case for all actions in this example.
post_conds=[
StringIntPair(wants_var_name, 0)
], #state feature update for this outcome. the state feature representing whether the robot the person wants coffee at this location becomes is there becomes 0.
duration_probs=[
1.0
], #duration probabilities. This does not apply to tis example and will always be one.
durations=[
self.ask_timeout / 2
], #expected time for action execution, to be used in the MDP model. We set expected times to half the timeout. One could also learn these from experience.
status=[
GoalStatus.SUCCEEDED
], #After executing the action in the robot, we check if the actionlib status matched a value in this array. If so, we perform an extra check using the result attribute below.
result=[
StringTriple('timeout', MdpActionOutcome.BOOL_TYPE,
'True')
]
) #After executing the action in the robot, if the check above succeeded, we check if the actionlib result matched the conditions given here. In this case, we check whether the attribute 'timeout' of the result object is equal to the boolean True.
# person is in possible_init_location and wants coffee
wants_coffee_outcome = MdpActionOutcome(
probability=person_here_prob * self.wants_coffee[person],
waypoint=possible_init_location,
post_conds=[
StringIntPair(asked_var_name, 1),
StringIntPair(wants_var_name, 1)
], #in this case, 2 state features are updated. The person has been asked about coffee, and the person wants coffee.
duration_probs=[1.0],
durations=[self.ask_timeout / 2],
status=[GoalStatus.SUCCEEDED],
result=[
StringTriple('timeout', MdpActionOutcome.BOOL_TYPE,
'False'),
StringTriple('response', MdpActionOutcome.BOOL_TYPE,
'True')
]
) # for this outcome to occur during execution, the timeout attribute of the action result must be False, and the response outcome must be True, meaning the person responded 'yes'
doesnt_want_coffee_outcome = MdpActionOutcome(
probability=person_here_prob *
(1 - self.wants_coffee[person]),
waypoint=possible_init_location,
post_conds=[
StringIntPair(asked_var_name, 1),
StringIntPair(wants_var_name, 0)
],
duration_probs=[1.0],
durations=[self.ask_timeout / 2],
status=[GoalStatus.SUCCEEDED],
result=[
StringTriple('timeout', MdpActionOutcome.BOOL_TYPE,
'False'),
StringTriple('response', MdpActionOutcome.BOOL_TYPE,
'False')
])
ask_action.outcomes = [
no_person_outcome, wants_coffee_outcome,
doesnt_want_coffee_outcome
]
actions += [ask_action]
ltl_task = "(F (asked_John = 1))" #hard coded task
#build the mdp spec
mdp_spec = MdpDomainSpec(vars=mdp_vars,
actions=actions,
ltl_task=ltl_task)
return mdp_spec
class MyFrame(wx.Frame):
def __init__(self, parent, id):
the_size = (700, 720) # height from 550 change to 700
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_IO_ns = 'elfin_ros_control/elfin/io_port1/' # 20201126: add IO ns
self.call_read_do_req = ElfinIODReadRequest()
self.call_read_di_req = ElfinIODReadRequest()
self.call_read_do_req.data = True
self.call_read_di_req.data = True
self.call_read_do = rospy.ServiceProxy(self.elfin_IO_ns + 'read_do',
ElfinIODRead)
self.call_read_di = rospy.ServiceProxy(self.elfin_IO_ns + 'read_di',
ElfinIODRead)
# 20201126: add service for write_do
self.call_write_DO = rospy.ServiceProxy(self.elfin_IO_ns + 'write_do',
ElfinIODWrite)
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.DO_btn_lock = threading.Lock() # 20201208: add the threading lock
self.DI_show_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
# 20201208: add the button array
self.DO_btn_display = [0] * 4 # DO states
self.DI_display = [0] * 4 # DI states
self.LED_display = [0] * 4 # LED states
self.End_btn_display = [0] * 4 # end button states
self.btn_height = 370 # 20201126: from 390 change to 370
self.btn_path = os.path.dirname(
os.path.realpath(__file__)) # 20201209: get the elfin_gui.py path
btn_lengths = []
self.DO_DI_btn_length = [
0, 92, 157, 133
] # 20201209: the length come from servo on, servo off, home, stop button
self.btn_interstice = 22 # 20201209: come from btn_interstice
self.display_init()
self.key = []
self.DO_btn = [0, 0, 0, 0, 0, 0, 0,
0] # DO state, first four bits is DO, the other is LED
self.DI_show = [
0, 0, 0, 0, 0, 0, 0, 0
] # DI state, first four bits is DI, the other is the end button
self.power_on_btn = wx.Button(self.panel,
label=' Servo On ',
name='Servo On',
pos=(20, self.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]
self.btn_interstice = (550 - btn_total_length) / 4
btn_pos_tmp = btn_lengths[
0] + self.btn_interstice + 20 # 20201126: 20:init length + btn0 length + btn_inter:gap
self.power_off_btn.SetPosition((btn_pos_tmp, self.btn_height))
btn_pos_tmp += btn_lengths[1] + self.btn_interstice
self.reset_btn.SetPosition((btn_pos_tmp, self.btn_height))
btn_pos_tmp += btn_lengths[2] + self.btn_interstice
self.home_btn.SetPosition((btn_pos_tmp, self.btn_height))
btn_pos_tmp += btn_lengths[3] + self.btn_interstice
self.stop_btn.SetPosition((btn_pos_tmp, self.btn_height))
self.servo_state_label = wx.StaticText(self.panel,
label='Servo state:',
pos=(590, self.btn_height - 10))
self.servo_state_show = wx.TextCtrl(self.panel,
style=(wx.TE_CENTER
| wx.TE_READONLY),
value='',
pos=(600, self.btn_height + 10))
self.servo_state = bool()
self.servo_state_lock = threading.Lock()
self.fault_state_label = wx.StaticText(self.panel,
label='Fault state:',
pos=(590, self.btn_height + 60))
self.fault_state_show = wx.TextCtrl(self.panel,
style=(wx.TE_CENTER
| wx.TE_READONLY),
value='',
pos=(600, self.btn_height + 80))
self.fault_state = bool()
self.fault_state_lock = threading.Lock()
# 20201209: add the description of end button
self.end_button_state_label = wx.StaticText(self.panel,
label='END Button\n state',
pos=(555, self.btn_height +
160))
self.reply_show_label = wx.StaticText(
self.panel, label='Result:',
pos=(20, self.btn_height +
260)) # 20201126: btn_height from 120 change to 260.
self.reply_show = wx.TextCtrl(
self.panel,
style=(wx.TE_CENTER | wx.TE_READONLY),
value='',
size=(670, 30),
pos=(20, self.btn_height +
280)) # 20201126: btn_height from 140 change to 280.
link_textctrl_length = (btn_pos_tmp - 40) / 2
self.ref_links_show_label = wx.StaticText(
self.panel, label='Ref. link:',
pos=(20, self.btn_height +
210)) # 20201126: btn_height from 60 change to 210.
self.ref_link_show = wx.TextCtrl(
self.panel,
style=(wx.TE_READONLY),
value=self.ref_link_name,
size=(link_textctrl_length, 30),
pos=(20, self.btn_height +
230)) # 20201126: btn_height from 80 change to 230.
self.end_link_show_label = wx.StaticText(
self.panel,
label='End link:',
pos=(link_textctrl_length + 30, self.btn_height +
210)) # 20201126: btn_height from 80 change to 200.
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,
self.btn_height + 230))
self.set_links_btn = wx.Button(self.panel,
label='Set links',
name='Set links')
self.set_links_btn.SetPosition(
(btn_pos_tmp, self.btn_height +
230)) # 20201126: btn_height from 75 change to 220.
# the variables about velocity scaling
velocity_scaling_init = rospy.get_param(self.elfin_basic_api_ns +
'velocity_scaling',
default=0.1)
default_velocity_scaling = str(round(velocity_scaling_init, 2))
self.velocity_setting_label = wx.StaticText(
self.panel,
label='Velocity Scaling',
pos=(20, self.btn_height -
55)) # 20201126: btn_height from 70 change to 55
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, self.btn_height -
35)) # 20201126: btn_height from 70 change to 35
self.velocity_setting_txt_lower = wx.StaticText(
self.panel, label='1%',
pos=(20, self.btn_height -
35)) # 20201126: btn_height from 45 change to 35
self.velocity_setting_txt_upper = wx.StaticText(
self.panel, label='100%',
pos=(550, self.btn_height -
35)) # 20201126: btn_height from 45 change to 35
self.velocity_setting_show = wx.TextCtrl(
self.panel,
style=(wx.TE_CENTER | wx.TE_READONLY),
value=default_velocity_scaling,
pos=(600, self.btn_height -
45)) # 20201126: btn_height from 55 change to 45
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_basic_api_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_basic_api_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 display_init(self):
js_pos = [20, 20]
js_btn_length = [70, 70, 61, 80]
js_distances = [10, 20, 10, 26]
dis_h = 50
for i in xrange(len(self.js_display)):
self.jp_button[i] = wx.Button(self.panel,
label='J' + str(i + 1) + ' +',
pos=(js_pos[0],
js_pos[1] + (5 - i) * dis_h),
size=(70, 40))
dis_tmp = js_btn_length[0] + js_distances[0]
self.jp_button[i].Bind(
wx.EVT_LEFT_DOWN,
lambda evt, mark=i + 1: self.teleop_joints(evt, mark))
self.jp_button[i].Bind(
wx.EVT_LEFT_UP,
lambda evt, mark=i + 1: self.release_button(evt, mark))
self.jm_button[i] = wx.Button(self.panel,
label='J' + str(i + 1) + ' -',
pos=(js_pos[0] + dis_tmp,
js_pos[1] + (5 - i) * dis_h),
size=(70, 40))
dis_tmp += js_btn_length[1] + js_distances[1]
self.jm_button[i].Bind(wx.EVT_LEFT_DOWN,
lambda evt, mark=-1 *
(i + 1): self.teleop_joints(evt, mark))
self.jm_button[i].Bind(wx.EVT_LEFT_UP,
lambda evt, mark=-1 *
(i + 1): self.release_button(evt, mark))
pos_js_label = (js_pos[0] + dis_tmp, js_pos[1] + (5 - i) * dis_h)
self.js_label[i] = wx.StaticText(self.panel,
label='J' + str(i + 1) + '/deg:',
pos=pos_js_label)
self.js_label[i].SetPosition(
(pos_js_label[0], pos_js_label[1] +
abs(40 - self.js_label[i].GetSize()[1]) / 2))
dis_tmp += js_btn_length[2] + js_distances[2]
pos_js_display = (js_pos[0] + dis_tmp, js_pos[1] + (5 - i) * dis_h)
self.js_display[i] = wx.TextCtrl(self.panel,
style=(wx.TE_CENTER
| wx.TE_READONLY),
value=' 0000.00 ',
pos=pos_js_display)
self.js_display[i].SetPosition(
(pos_js_display[0], pos_js_display[1] +
abs(40 - self.js_display[i].GetSize()[1]) / 2))
dis_tmp += js_btn_length[3] + js_distances[3]
ps_pos = [js_pos[0] + dis_tmp, 20]
ps_btn_length = [70, 70, 53, 80]
ps_distances = [10, 20, 10, 20]
pcs_btn_label = ['X', 'Y', 'Z', 'Rx', 'Ry', 'Rz']
pcs_label = ['X', 'Y', 'Z', 'R', 'P', 'Y']
unit_label = ['/mm:', '/mm:', '/mm:', '/deg:', '/deg:', '/deg:']
for i in xrange(len(self.ps_display)):
self.pp_button[i] = wx.Button(self.panel,
label=pcs_btn_label[i] + ' +',
pos=(ps_pos[0],
ps_pos[1] + (5 - i) * dis_h),
size=(70, 40))
dis_tmp = ps_btn_length[0] + ps_distances[0]
self.pp_button[i].Bind(
wx.EVT_LEFT_DOWN,
lambda evt, mark=i + 1: self.teleop_pcs(evt, mark))
self.pp_button[i].Bind(
wx.EVT_LEFT_UP,
lambda evt, mark=i + 1: self.release_button(evt, mark))
self.pm_button[i] = wx.Button(self.panel,
label=pcs_btn_label[i] + ' -',
pos=(ps_pos[0] + dis_tmp,
ps_pos[1] + (5 - i) * dis_h),
size=(70, 40))
dis_tmp += ps_btn_length[1] + ps_distances[1]
self.pm_button[i].Bind(wx.EVT_LEFT_DOWN,
lambda evt, mark=-1 *
(i + 1): self.teleop_pcs(evt, mark))
self.pm_button[i].Bind(wx.EVT_LEFT_UP,
lambda evt, mark=-1 *
(i + 1): self.release_button(evt, mark))
pos_ps_label = (ps_pos[0] + dis_tmp, ps_pos[1] + (5 - i) * dis_h)
self.ps_label[i] = wx.StaticText(self.panel,
label=pcs_label[i] +
unit_label[i],
pos=pos_ps_label)
self.ps_label[i].SetPosition(
(pos_ps_label[0], pos_ps_label[1] +
abs(40 - self.ps_label[i].GetSize()[1]) / 2))
dis_tmp += ps_btn_length[2] + ps_distances[2]
pos_ps_display = (ps_pos[0] + dis_tmp, ps_pos[1] + (5 - i) * dis_h)
self.ps_display[i] = wx.TextCtrl(self.panel,
style=(wx.TE_CENTER
| wx.TE_READONLY),
value='',
pos=pos_ps_display)
self.ps_display[i].SetPosition(
(pos_ps_display[0], pos_ps_display[1] +
abs(40 - self.ps_display[i].GetSize()[1]) / 2))
dis_tmp += ps_btn_length[3] + ps_distances[3]
# 20201209: add the DO,LED,DI,end button.
for i in xrange(len(self.DO_btn_display)):
self.DO_btn_display[i] = wx.Button(
self.panel,
label='DO' + str(i),
pos=(20 + (self.DO_DI_btn_length[i] + self.btn_interstice) * i,
self.btn_height + 40))
self.DO_btn_display[i].Bind(
wx.EVT_BUTTON,
lambda evt, marker=i, cl=self.call_write_DO: self.
call_write_DO_command(evt, marker, cl))
self.DI_display[i] = wx.TextCtrl(
self.panel,
style=(wx.TE_CENTER | wx.TE_READONLY),
value='DI' + str(i),
size=(self.DO_btn_display[i].GetSize()),
pos=(20 + (self.DO_DI_btn_length[i] + self.btn_interstice) * i,
self.btn_height + 80))
self.LED_display[i] = wx.Button(
self.panel,
label='LED' + str(i),
pos=(20 + (self.DO_DI_btn_length[i] + self.btn_interstice) * i,
self.btn_height + 120))
self.LED_display[i].Bind(
wx.EVT_BUTTON,
lambda evt, marker=4 + i, cl=self.call_write_DO: self.
call_write_DO_command(evt, marker, cl))
png = wx.Image(
self.btn_path + '/btn_icon/End_btn' + str(i) + '_low.png',
wx.BITMAP_TYPE_PNG).ConvertToBitmap()
self.End_btn_display[i] = wx.StaticBitmap(
self.panel,
-1,
png,
pos=(40 + (self.DO_DI_btn_length[i] + self.btn_interstice) * i,
self.btn_height + 160))
def velocity_setting_cb(self, event):
current_velocity_scaling = self.velocity_setting.GetValue() * 0.01
self.teleop_api_dynamic_reconfig_client.update_configuration(
{'velocity_scaling': current_velocity_scaling})
wx.CallAfter(self.update_velocity_scaling_show,
current_velocity_scaling)
def basic_api_reconfigure_cb(self, config):
if self.velocity_setting_show.GetValue() != config.velocity_scaling:
self.velocity_setting.SetValue(int(config.velocity_scaling * 100))
wx.CallAfter(self.update_velocity_scaling_show,
config.velocity_scaling)
def action_stop(self):
self.action_client.wait_for_server(timeout=rospy.Duration(secs=0.5))
self.action_goal.trajectory.header.stamp.secs = 0
self.action_goal.trajectory.header.stamp.nsecs = 0
self.action_goal.trajectory.points = []
self.action_client.send_goal(self.action_goal)
def teleop_joints(self, event, mark):
self.call_teleop_joint_req.data = mark
resp = self.call_teleop_joint.call(self.call_teleop_joint_req)
wx.CallAfter(self.update_reply_show, resp)
event.Skip()
def teleop_pcs(self, event, mark):
self.call_teleop_cart_req.data = mark
resp = self.call_teleop_cart.call(self.call_teleop_cart_req)
wx.CallAfter(self.update_reply_show, resp)
event.Skip()
def release_button(self, event, mark):
self.call_teleop_stop_req.data = True
resp = self.call_teleop_stop.call(self.call_teleop_stop_req)
wx.CallAfter(self.update_reply_show, resp)
event.Skip()
def call_set_bool_common(self, event, client, request):
btn = event.GetEventObject()
check_list = ['Servo On', 'Servo Off', 'Clear Fault']
# Check servo state
if btn.GetName() == 'Servo On':
servo_enabled = bool()
if self.servo_state_lock.acquire():
servo_enabled = self.servo_state
self.servo_state_lock.release()
if servo_enabled:
resp = SetBoolResponse()
resp.success = False
resp.message = 'Robot is already enabled'
wx.CallAfter(self.update_reply_show, resp)
event.Skip()
return
# Check fault state
if btn.GetName() == 'Clear Fault':
fault_flag = bool()
if self.fault_state_lock.acquire():
fault_flag = self.fault_state
self.fault_state_lock.release()
if not fault_flag:
resp = SetBoolResponse()
resp.success = False
resp.message = 'There is no fault now'
wx.CallAfter(self.update_reply_show, resp)
event.Skip()
return
# Check if the button is in check list
if btn.GetName() in check_list:
self.show_message_dialog(btn.GetName(), client, request)
else:
try:
resp = client.call(request)
wx.CallAfter(self.update_reply_show, resp)
except rospy.ServiceException, e:
resp = SetBoolResponse()
resp.success = False
resp.message = 'no such service in simulation'
wx.CallAfter(self.update_reply_show, resp)
event.Skip()
class MoveBaseClient:
global_tf = None
def __init__(self, record_rate=5, timeout=90):
if MoveBaseClient.global_tf is None:
MoveBaseClient.global_tf = tf.TransformListener()
self.tf = MoveBaseClient.global_tf
self.ac = SimpleActionClient('move_base', MoveBaseAction)
self.record_rate = record_rate
self.base_frame = '/map'
self.target_frame = '/base_footprint'
self.timeout = rospy.Duration(timeout)
self.recording = False
self.other_data = []
self.subscriptions = []
self.subscribers = []
printed = False
limit = rospy.Time.now() + rospy.Duration(30)
while not self.ac.wait_for_server(rospy.Duration(
5.0)) and not rospy.is_shutdown() and rospy.Time.now() < limit:
printed = True
rospy.loginfo('Waiting for move_base server')
if rospy.Time.now() >= limit:
self.ac = None
return
if rospy.is_shutdown():
self.ac = None
return
if printed:
rospy.loginfo('Got move_base server')
def ready(self):
return self.ac is not None
def addSubscription(self, topic, msg_type):
self.subscriptions.append((topic, msg_type))
def reset(self):
for name, proxy in self.resets.iteritems():
proxy()
rospy.loginfo("Reset called on %s" % name)
def cb(self, msg, topic):
if not self.recording:
return
if topic == '/move_base_node/DWAPlannerROS/local_plan':
msg.header.frame_id = '/map'
for i, pose in enumerate(msg.poses):
msg.poses[i] = self.tf.transformPose('/map', pose)
elif topic == '/move_base_node/local_costmap/costmap':
p = PoseStamped()
p.header = msg.header
p.pose = msg.info.origin
np = self.tf.transformPose('/map', p)
msg.header = np.header
msg.info.origin = np.pose
self.other_data.append((rospy.Time.now(), topic, msg))
def goto(self, loc, debug=False):
if self.ac is None:
return []
self.goal = loc
q = quaternion_from_euler(0, 0, loc[2])
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = self.base_frame
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = loc[0]
goal.target_pose.pose.position.y = loc[1]
goal.target_pose.pose.orientation.w = q[3]
goal.target_pose.pose.orientation.x = q[0]
goal.target_pose.pose.orientation.y = q[1]
goal.target_pose.pose.orientation.z = q[2]
rate = rospy.Rate(self.record_rate)
self.data = []
self.other_data = []
self.recording = True
self.subscribers = []
for topic, msg_type in self.subscriptions:
sub = rospy.Subscriber(topic, msg_type, self.cb, topic)
self.subscribers.append(sub)
rospy.sleep(0.5)
self.ac.send_goal(goal)
if debug:
timer = rospy.Timer(rospy.Duration(5), self.print_distance)
start_time = rospy.Time.now()
while not finished(self.ac.get_state()
) and rospy.Time.now() < start_time + self.timeout:
t, pose = get_time_and_pose(self.tf, self.base_frame,
self.target_frame)
if pose is not None:
self.data.append((t, "/robot_pose", pose))
else:
print 'TF Error'
rate.sleep()
if debug:
timer.shutdown()
self.recording = False
rospy.sleep(1)
for sub in self.subscribers:
sub.unregister()
# save footprint
footprint_param = rospy.get_param('/move_base_node/footprint', [])
footprint = Polygon()
if type(footprint_param) == type([]):
for x, y in footprint_param:
footprint.points.append(Point32(x, y, 0.0))
else: #string
for subs in footprint_param[2:-2].split('],['):
x, y = subs.split(',')
footprint.points.append(Point32(float(x), float(y), 0.0))
self.other_data.append((t, '/footprint', footprint))
# save the rest of the configuration
params = rospy.get_param('/')
params_str = std_msgs.msg.String()
params_str.data = yaml.dump(params, default_flow_style=False)
self.other_data.append((t, '/parameters', params_str))
return self.data + self.other_data
def print_distance(self, event=None):
if len(self.data) == 0:
return
pose = self.data[-1][2]
dx = abs(self.goal[0] - pose.x)
dy = abs(self.goal[1] - pose.y)
dt = abs(self.goal[2] - pose.theta)
rospy.loginfo("dx: %.2f dy: %.2f dt: %d" %
(dx, dy, int(dt * 180 / 3.141)))
def load_subscriptions(self):
#TODO: Load classes dynamically
topics = rospy.get_param('/nav_experiments/topics', [])
for topic in topics:
if 'plan' in topic:
self.addSubscription(topic, Path)
elif 'command' in topic or 'cmd_vel' in topic:
self.addSubscription(topic, Twist)
elif 'costmap_updates' in topic:
self.addSubscription(topic, OccupancyGridUpdate)
elif 'costmap/costmap' in topic:
self.addSubscription(topic, OccupancyGrid)
elif 'cloud' in topic:
self.addSubscription(topic, PointCloud2)
else:
rospy.logerr("unknown type for %s" % topic)
self.addSubscription('/collisions', std_msgs.msg.String)
self.addSubscription('/simulation_state', ModelStates)
self.addSubscription('/move_base_node/global_costmap/update_time',
std_msgs.msg.Float32)
self.addSubscription('/move_base_node/local_costmap/update_time',
std_msgs.msg.Float32)
self.addSubscription('/people', people_msgs.msg.People)
class Pick_Place:
def __init__(self):
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('~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)
self._scene.remove_world_object(self._table_object_name)
self._scene.remove_world_object(self._grasp_object_name)
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)
self._pose_place = Pose()
self._pose_place.position.x = self._pose_coke_can.position.x
self._pose_place.position.y = self._pose_coke_can.position.y - 0.06
self._pose_place.position.z = self._pose_coke_can.position.z
self._pose_place.orientation = Quaternion(
*quaternion_from_euler(0.0, 0.0, 0.0))
self._arm = self._robot.get_group(self._arm_group)
self._gripper = self._robot.get_group(self._gripper_group)
self._grasps_ac = SimpleActionClient(
'/autoarm_grasp_pose_server/generate_grasp_poses',
GenerateGraspPosesAction)
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 __del__(self):
# Clean the scene:
self._scene.remove_world_object(self._grasp_object_name)
self._scene.remove_world_object(self._table_object_name)
def _add_table(self, name):
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.45
p.pose.position.y = 0.0
p.pose.position.z = 0.22
q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0))
p.pose.orientation = Quaternion(*q)
# Table size from ~/.gazebo/models/table/model.sdf, using the values
# for the surface link.
self._scene.add_box(name, p, (0.5, 0.4, 0.02))
return p.pose
def _add_grasp_block_(self, name):
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.25
p.pose.position.y = 0.05
p.pose.position.z = 0.32
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
# Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,
# using the measure tape tool from meshlab.
# The box is the bounding box of the coke cylinder.
# The values are taken from the cylinder base diameter and height.
self._scene.add_box(name, p, (0.03, 0.03, 0.09))
return p.pose
def _generate_grasps(self, pose, width):
"""
Generate grasps by using the grasp generator generate action; based on
server_test.py example on moveit_simple_grasps pkg.
"""
# Create goal:
goal = GenerateGraspPosesGoal()
goal.pose = pose
goal.width = width
options = GraspPoseOptions()
# simple_graps.cpp doesn't implement GRASP_AXIS_Z!
#options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z
options.grasp_direction = GraspPoseOptions.GRASP_DIRECTION_UP
options.grasp_rotation = GraspPoseOptions.GRASP_ROTATION_FULL
# @todo disabled because it works better with the default options
#goal.options.append(options)
# Send goal and wait for result:
state = self._grasps_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Grasp goal failed!: %s' %
self._grasps_ac.get_goal_status_text())
return None
grasps = self._grasps_ac.get_result().grasps
# Publish grasps (for debugging/visualization purposes):
self._publish_grasps(grasps)
return grasps
def _generate_places(self, target):
"""
Generate places (place locations), based on
https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/
baxter_pick_place/src/block_pick_place.cpp
"""
# Generate places:
places = []
now = rospy.Time.now()
for angle in numpy.arange(0.0, numpy.deg2rad(360.0),
numpy.deg2rad(1.0)):
# Create place location:
place = PlaceLocation()
place.place_pose.header.stamp = now
place.place_pose.header.frame_id = self._robot.get_planning_frame()
# Set target position:
place.place_pose.pose = copy.deepcopy(target)
# Generate orientation (wrt Z axis):
q = quaternion_from_euler(0.0, 0.0, angle)
place.place_pose.pose.orientation = Quaternion(*q)
# Generate pre place approach:
place.pre_place_approach.desired_distance = self._approach_retreat_desired_dist
place.pre_place_approach.min_distance = self._approach_retreat_min_dist
place.pre_place_approach.direction.header.stamp = now
place.pre_place_approach.direction.header.frame_id = self._robot.get_planning_frame(
)
place.pre_place_approach.direction.vector.x = 0
place.pre_place_approach.direction.vector.y = 0
place.pre_place_approach.direction.vector.z = 0.2
# Generate post place approach:
place.post_place_retreat.direction.header.stamp = now
place.post_place_retreat.direction.header.frame_id = self._robot.get_planning_frame(
)
place.post_place_retreat.desired_distance = self._approach_retreat_desired_dist
place.post_place_retreat.min_distance = self._approach_retreat_min_dist
place.post_place_retreat.direction.vector.x = 0
place.post_place_retreat.direction.vector.y = 0
place.post_place_retreat.direction.vector.z = 0.2
# Add place:
places.append(place)
# Publish places (for debugging/visualization purposes):
self._publish_places(places)
return places
def _create_pickup_goal(self, group, target, grasps):
"""
Create a MoveIt! PickupGoal
"""
# Create goal:
goal = PickupGoal()
goal.group_name = group
goal.target_name = target
goal.possible_grasps.extend(grasps)
goal.allowed_touch_objects.append(target)
goal.support_surface_name = self._table_object_name
# Configure goal planning options:
goal.allowed_planning_time = 7.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 20
return goal
def _create_place_goal(self, group, target, places):
"""
Create a MoveIt! PlaceGoal
"""
# Create goal:
goal = PlaceGoal()
goal.group_name = group
goal.attached_object_name = target
goal.place_locations.extend(places)
# Configure goal planning options:
goal.allowed_planning_time = 7.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 20
return goal
def _pickup(self, group, target, width):
"""
Pick up a target using the planning group
"""
# Obtain possible grasps from the grasp generator server:
grasps = self._generate_grasps(self._pose_coke_can, width)
# Create and send Pickup goal:
goal = self._create_pickup_goal(group, target, grasps)
state = self._pickup_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Pick up goal failed!: %s' %
self._pickup_ac.get_goal_status_text())
return None
result = self._pickup_ac.get_result()
# Check for error:
err = result.error_code.val
if err != MoveItErrorCodes.SUCCESS:
rospy.logwarn('Group %s cannot pick up target %s!: %s' %
(group, target, str(moveit_error_dict[err])))
return False
return True
def _place(self, group, target, place):
"""
Place a target using the planning group
"""
# Obtain possible places:
places = self._generate_places(place)
# Create and send Place goal:
goal = self._create_place_goal(group, target, places)
state = self._place_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Place goal failed!: %s' %
self._place_ac.get_goal_status_text())
return None
result = self._place_ac.get_result()
# Check for error:
err = result.error_code.val
if err != MoveItErrorCodes.SUCCESS:
rospy.logwarn('Group %s cannot place target %s!: %s' %
(group, target, str(moveit_error_dict[err])))
return False
return True
def _publish_grasps(self, grasps):
"""
Publish grasps as poses, using a PoseArray message
"""
if self._grasps_pub.get_num_connections() > 0:
msg = PoseArray()
msg.header.frame_id = self._robot.get_planning_frame()
msg.header.stamp = rospy.Time.now()
for grasp in grasps:
p = grasp.grasp_pose.pose
msg.poses.append(Pose(p.position, p.orientation))
self._grasps_pub.publish(msg)
def _publish_places(self, places):
"""
Publish places as poses, using a PoseArray message
"""
if self._places_pub.get_num_connections() > 0:
msg = PoseArray()
msg.header.frame_id = self._robot.get_planning_frame()
msg.header.stamp = rospy.Time.now()
for place in places:
msg.poses.append(place.place_pose.pose)
self._places_pub.publish(msg)
class StateMachine(object):
def __init__(self):
self.node_name = "Student SM"
# Access rosparams
self.cmd_vel_top = rospy.get_param(rospy.get_name() + '/cmd_vel_topic')
self.mv_head_srv_nm = rospy.get_param(rospy.get_name() +
'/move_head_srv')
#self.amcl_estimate = rospy.get_param(rospy.get_name() + '/amcl_estimate')
self.aruco_pose_topic = rospy.get_param(rospy.get_name() +
'/aruco_pose_topic')
#self.clear_costmaps_srv = rospy.get_param(rospy.get_name() + '/clear_costmaps_srv')
self.cube_pos = rospy.get_param(rospy.get_name() + '/cube_pose')
#self.global_loc_srv = rospy.get_param(rospy.get_name() + '/global_loc_srv')
#self.move_base_feedback = rospy.get_param(rospy.get_name() + '/move_base_feedback')
#self.move_base_frame = rospy.get_param(rospy.get_name() + '/move_base_frame')
#self.nav_goal_topic = rospy.get_param(rospy.get_name() + '/nav_goal_topic')
#self.pick_pose_topic = rospy.get_param(rospy.get_name() + '/pick_pose_topic')
self.pick_srv = rospy.get_param(rospy.get_name() + '/pick_srv')
#self.place_pose_topic = rospy.get_param(rospy.get_name() + '/place_pose_topic')
self.place_srv = rospy.get_param(rospy.get_name() + '/place_srv')
#self.robot_base_frame = rospy.get_param(rospy.get_name() + '/robot_base_frame')
# Subscribe to topics
# ---
# Wait for service providers
rospy.wait_for_service(self.mv_head_srv_nm, timeout=30)
rospy.wait_for_service(self.pick_srv, timeout=30)
rospy.wait_for_service(self.place_srv, timeout=30)
# Instantiate publishers
self.cmd_vel_pub = rospy.Publisher(self.cmd_vel_top,
Twist,
queue_size=10)
self.aruco_pose_pub = rospy.Publisher(self.aruco_pose_topic,
PoseStamped,
queue_size=10)
# Set up action clients
rospy.loginfo("%s: Waiting for play_motion action server...",
self.node_name)
self.play_motion_ac = SimpleActionClient("/play_motion",
PlayMotionAction)
if not self.play_motion_ac.wait_for_server(rospy.Duration(1000)):
rospy.logerr("%s: Could not connect to /play_motion action server",
self.node_name)
exit()
rospy.loginfo("%s: Connected to play_motion action server",
self.node_name)
# Init state machine
self.state = 0
rospy.sleep(3)
self.check_states()
def check_states(self):
while not rospy.is_shutdown() and self.state != 4:
# State 0: Get in safe position
if self.state == 0:
rospy.loginfo("%s: Tucking the arm...", self.node_name)
goal = PlayMotionGoal()
goal.motion_name = 'home'
goal.skip_planning = True
self.play_motion_ac.send_goal(goal)
success_tucking = self.play_motion_ac.wait_for_result(
rospy.Duration(100.0))
if success_tucking:
rospy.loginfo("%s: Arm tucked.", self.node_name)
self.state = 1
else:
self.play_motion_ac.cancel_goal()
rospy.logerr(
"%s: play_motion failed to tuck arm, reset simulation",
self.node_name)
self.state = 5
rospy.sleep(1)
# State 1: Pick up the cube
if self.state == 1:
rospy.loginfo("%s: Picking up the cube...", self.node_name)
try:
self.cube_pos = self.cube_pos.split(',')
pick_pose_msg = PoseStamped()
pick_pose_msg.header.frame_id = "base_footprint"
pick_pose_msg.pose.position.x = float(
self.cube_pos[0]) + 0.03
pick_pose_msg.pose.position.y = float(self.cube_pos[1])
pick_pose_msg.pose.position.z = float(
self.cube_pos[2]) - 0.06
pick_pose_msg.pose.orientation.x = float(self.cube_pos[3])
pick_pose_msg.pose.orientation.y = float(self.cube_pos[4])
pick_pose_msg.pose.orientation.z = float(self.cube_pos[5])
pick_pose_msg.pose.orientation.w = float(self.cube_pos[6])
self.aruco_pose_pub.publish(pick_pose_msg)
pick_srv = rospy.ServiceProxy(self.pick_srv, SetBool)
pick_req = pick_srv(True)
if pick_req.success == True:
rospy.loginfo("%s: Cube picked.", self.node_name)
self.state = 2
else:
rospy.loginfo("%s: Failed to pick cube.",
self.node_name)
self.state = 5
rospy.sleep(3) #it could also be 1
except rospy.ServiceException, e:
print "Service call to pick_srv server failed: %s" % e
# State 2: Move the robot "manually" to the table
if self.state == 2:
move_msg = Twist()
move_msg.angular.z = -1
rate = rospy.Rate(10)
converged = False
cnt = 0
rospy.loginfo("%s: Moving towards table", self.node_name)
while not rospy.is_shutdown() and cnt < 30:
self.cmd_vel_pub.publish(move_msg)
rate.sleep()
cnt = cnt + 1
move_msg.linear.x = 1
move_msg.angular.z = 0
cnt = 0
while not rospy.is_shutdown() and cnt < 9:
self.cmd_vel_pub.publish(move_msg)
rate.sleep()
cnt = cnt + 1
self.state = 3
rospy.sleep(1)
# State 3: Place the cube
if self.state == 3:
rospy.loginfo("%s: Placing the cube...", self.node_name)
try:
place_pose_msg = PoseStamped()
place_pose_msg.header.frame_id = "base_footprint"
place_pose_msg.pose.position.x = float(
self.cube_pos[0]) + 0.03
place_pose_msg.pose.position.y = float(self.cube_pos[1])
place_pose_msg.pose.position.z = float(
self.cube_pos[2]) - 0.06
place_pose_msg.pose.orientation.x = float(self.cube_pos[3])
place_pose_msg.pose.orientation.y = float(self.cube_pos[4])
place_pose_msg.pose.orientation.z = float(self.cube_pos[5])
place_pose_msg.pose.orientation.w = float(self.cube_pos[6])
self.aruco_pose_pub.publish(place_pose_msg)
place_srv = rospy.ServiceProxy(self.place_srv, SetBool)
place_req = place_srv(True)
if place_req.success == True:
rospy.loginfo("%s: Cube placed.", self.node_name)
self.state = 4
else:
rospy.loginfo("%s: Failed to place cube.",
self.node_name)
self.state = 5
rospy.sleep(3) #it could also be 1
except rospy.ServiceException, e:
print "Service call to place_srv server failed: %s" % e
# # State 3: Lower robot head service
# if self.state == 3:
# try:
# rospy.loginfo("%s: Lowering robot head", self.node_name)
# move_head_srv = rospy.ServiceProxy(self.mv_head_srv_nm, MoveHead)
# move_head_req = move_head_srv("down")
# if move_head_req.success == True:
# self.state = 4
# rospy.loginfo("%s: Move head down succeded!", self.node_name)
# else:
# rospy.loginfo("%s: Move head down failed!", self.node_name)
# self.state = 5
# rospy.sleep(3)
# except rospy.ServiceException, e:
# print "Service call to move_head server failed: %s"%e
# Error handling
if self.state == 5:
rospy.logerr(
"%s: State machine failed. Check your code and try again!",
self.node_name)
return
class Driver():
HOME = 0
RANDOMIZE = 1
NORMAL = 2
SHUTDOWN = 3
def __init__(self, home_x=2.0, home_y=2.0):
# flag to check for termination conditions
self.is_running = True
self.condition = Driver.NORMAL
# get positions from file
rospack = rospkg.RosPack()
dir_path = rospack.get_path("exercise1")
file_path = dir_path + "/extra/positions.csv"
# positions = [(2.0, 2.0), (3.0, 3.0), (5.0, 3.0)]
self.orig_positions = np.loadtxt(open(file_path, "rb"), delimiter=",")
print "Locations to visit - pose(x,y,z) quaternion(x,y,z,w) "
print self.orig_positions
# self.home_x = home_x
# self.home_y = home_y
self.start_loc = (2.0, 2.0, 0.0, 0.0, 0.0, 0.988, 0.156)
# to_visit = list(reversed(orig_positions))
self.to_visit = copy.deepcopy(self.orig_positions)
self.wait_time = 60.0 # used as seconds in autopilot function
self.is_interrupted = False
# create the action client and send goal
self.client = SimpleActionClient("move_base", MoveBaseAction)
self.client.wait_for_server(rospy.Duration.from_sec(self.wait_time))
## marking list of places to vist
self.location_marker_pub = rospy.Publisher("destination_marker",
Marker,
queue_size=10)
self.create_all_markers()
# publishing destination thread
self.marker_pub_thread = MarkerPublisherThread(
"marker_pub_thread", 1.0, self.location_marker_pub, self.markers)
# keyboard input thread
self.kb_thread = KeyboardWorkerThread("kb_thread", .05)
# important to register shutdown node with threads after thread have been created
rospy.on_shutdown(self.shutdown)
# start threads before handing off state control to autopilot
self.marker_pub_thread.start()
self.kb_thread.start()
# rospy.on_shutdown(self.shutdown_node)
def autopilot(self):
''' visit each location in visit list '''
while (True):
# print "Locations to visit"
# print self.to_visit
for loc in self.to_visit:
# print
# print
# print "Location to visit"
# print loc
self.send_move_goal(loc)
# self.client.wait_for_result(rospy.Duration.from_sec(self.wait_time)) # blocking call
# could check to make sure its not a failure nor a success
terminal_states = [
GoalStatus.REJECTED, GoalStatus.ABORTED,
GoalStatus.SUCCEEDED, GoalStatus.RECALLED
]
# print "Terminal States: ", terminal_states
movement_state = self.client.get_state()
# print "Condition!!", self.condition
# print terminal_states
while movement_state not in terminal_states:
movement_state = self.client.get_state()
if self.update_state() != Driver.NORMAL:
break
time.sleep(.1)
if movement_state == GoalStatus.SUCCEEDED:
self.do_something_interesting()
if self.condition != Driver.NORMAL:
# print "Condition not normal!!"
break
if self.condition == Driver.HOME:
## Go home and start visiting list again
espeak_call_str = "espeak -s 110 \'Yes master. Returning home.\'"
os.system(espeak_call_str)
self.send_move_goal(self.start_loc)
print "Visiting Home"
## wait till you get home to start the next round
self.client.wait_for_result(
rospy.Duration.from_sec(self.wait_time))
self.do_something_interesting()
elif self.condition == Driver.RANDOMIZE:
## jumble the to-visit list then start visiting list again
np.random.shuffle(self.to_visit)
# print "Shuffled List"
elif self.condition == Driver.SHUTDOWN:
print "Shutting down"
espeak_call_str = "espeak -s 110 \'Goodbye.\'"
os.system(espeak_call_str)
break
self.condition = Driver.NORMAL # everything goes to normal once weird stuff happens
# print "Visiting locations again"
time.sleep(2)
# print "exiting autopilot"
# rospy.signal_shutdown("Finished Autopilot")
def update_state(self):
char = self.kb_thread.pop_c()
if char == 'h':
self.condition = Driver.HOME
elif char == 'r':
self.condition = Driver.RANDOMIZE
elif char == '\x1b':
self.condition = Driver.SHUTDOWN
# print "shutdown condition"
else:
# self.condition = Driver.NORMAL
pass
return self.condition
def cancel_goal(self):
self.client.cancel_all_goals()
def send_move_goal(self, loc):
# create goal
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.pose.position.x = loc[0]
goal.target_pose.pose.position.y = loc[1]
goal.target_pose.pose.position.z = loc[2]
goal.target_pose.pose.orientation.x = loc[3]
goal.target_pose.pose.orientation.y = loc[4]
goal.target_pose.pose.orientation.z = loc[5]
goal.target_pose.pose.orientation.w = loc[6]
goal.target_pose.header.stamp = rospy.Time.now()
self.client.send_goal(goal)
return self.client
def do_something_interesting(self):
''' Output a nice call phrase for the grader.
Assumes espeak is installed. BAD ASSUMPTION
'''
sayings = [
"I love you Will", "Great job Will",
"Your a great teaching assistant", "Am I home yet?",
"When will this stop?", "Are you my mother?",
"Assimov books are great.", "Robot army on its way.",
"Your my friend.", "Kory deserves 100 percent."
]
espeak_call_str = "espeak -s 120 \'{0}\'".format(choice(sayings))
os.system(espeak_call_str)
def create_all_markers(self):
self.markers = []
for i in xrange(len(self.orig_positions)):
loc = self.orig_positions[i]
marker_id = i
marker = self.create_location_marker(loc, Marker.SPHERE,
Marker.ADD, marker_id,
ColorRGBA(0, 0, 1, 1))
self.markers.append(marker)
# print "in create all markers", self.markers
def mark_as_to_visit(self, loc, marker_id):
marker = self.create_location_marker(loc, Marker.SPHERE, Marker.ADD,
marker_id, ColorRGBA(0, 1, 0, 1))
self.location_marker_pub.publish(marker)
def mark_as_visited(self, loc, marker_id):
marker = self.create_location_marker(loc, color, Marker.SPHERE,
Marker.ADD, marker_id,
ColorRGBA(0, 1, 1, 1))
self.location_marker_pub.publish(marker)
def create_location_marker(self,
loc,
marker_type=Marker.SPHERE,
marker_action=Marker.ADD,
marker_id=0,
marker_color=ColorRGBA(1, 0, 0, 1)):
h = Header()
h.frame_id = "map"
h.stamp = rospy.Time.now()
mark = Marker()
mark.header = h
mark.ns = "location_marker"
mark.id = marker_id
mark.type = marker_type
mark.action = marker_action
mark.scale = Vector3(0.25, 0.25, 0.25)
mark.color = marker_color
pose = Pose(Point(loc[0], loc[1], loc[2]),
Quaternion(loc[3], loc[4], loc[5], loc[6]))
mark.pose = pose
return mark
### threaded way to do it
def shutdown(self):
self.client.cancel_all_goals()
# self.marker_pub_thread.quit()
self.marker_pub_thread.terminate()
old_settings = self.kb_thread.old_settings
self.kb_thread.quit()
self.kb_thread.terminate()
# print "terminated termios and restored settings"
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_settings)
roslib.load_manifest('w2_arm_navigation_tests')
import rospy
from actionlib import SimpleActionClient
from actionlib_msgs.msg import GoalStatus
from arm_navigation_msgs.msg import MoveArmAction
from arm_navigation_msgs.msg import MoveArmGoal
from arm_navigation_msgs.msg import JointConstraint
from arm_navigation_msgs.msg import LinkPadding
if __name__ == '__main__':
rospy.init_node('move_arm_simple_joint_goal_node', anonymous=True)
move_arm_client = SimpleActionClient('move_left_arm', MoveArmAction)
rospy.loginfo('init_node complete')
move_arm_client.wait_for_server()
rospy.loginfo('Connected to move_left_arm action server')
goal = MoveArmGoal()
goal.planner_service_name = 'ompl_planning/plan_kinematic_path'
goal.motion_plan_request.planner_id = ''
goal.motion_plan_request.group_name = 'left_arm'
goal.motion_plan_request.num_planning_attempts = 1
goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.0)
links = ('L7_wrist_roll_link', 'L6_wrist_pitch_link',
'L5_forearm_roll_link', 'L4_elbow_flex_link',
'L3_upperarm_roll_link', 'L2_shoulder_pan_link',
'L1_shoulder_pitch_link', 'L8_left_finger_link ',
'L9_right_finger_link')
class Pick_Place:
def __init__(self):
# 检索参数:
self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table')
self._table2_object_name = rospy.get_param('~table_object_name', 'Grasp_Table2')
self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object')
self._grasp_object2_name = rospy.get_param('~grasp_object_name', 'Grasp_Object2')
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)
# 创建（调试）发布者:
self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True)
self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True)
# 创建规划现场，机器人指挥官:
self._scene = PlanningSceneInterface()
self._robot = RobotCommander()
rospy.sleep(1.0)
# 清理现场:
self._scene.remove_world_object(self._table_object_name)
self._scene.remove_world_object(self._table2_object_name)
self._scene.remove_world_object(self._grasp_object_name)
self._scene.remove_world_object(self._grasp_object2_name)
# 将表和可乐罐对象添加到计划场景:
self._pose_table = self._add_table(self._table_object_name)
self._pose_table = self._add_table2(self._table2_object_name)
self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name)
self._pose_coke_can2 = self._add_grasp_block2_(self._grasp_object2_name)
rospy.sleep(1.0)
# 定义目标位置姿势:
self._pose_place = Pose()
self._pose_place.position.x = self._pose_coke_can.position.x-0.5
self._pose_place.position.y = self._pose_coke_can.position.y-0.85
self._pose_place.position.z = self._pose_coke_can.position.z-0.3
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 # base_link is 0.9 above
self._pose_place.position.z = self._pose_place.position.z + 0.05 # target pose a little above
# 检索组 (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('pickup')
self._arm.go(wait=True)
print("Pickup pose")
# 创建抓取生成器“生成”动作客户端:
self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction)
if not self._grasps_ac.wait_for_server(rospy.Duration(1.0)):
rospy.logerr('Grasp generator action client not available!')
rospy.signal_shutdown('Grasp generator action client not available!')
return
# 创建移动组“zhua取”操作客户端:
self._pickup_ac = SimpleActionClient('/pickup', PickupAction)
if not self._pickup_ac.wait_for_server(rospy.Duration(1.0)):
rospy.logerr('Pick up action client not available!')
rospy.signal_shutdown('Pick up action client not available!')
return
# 创建移动组“放置”动作客户端:
self._place_ac = SimpleActionClient('/place', PlaceAction)
if not self._place_ac.wait_for_server(rospy.Duration(1.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')
print("pose_place: ", self._pose_place)
# 放置可乐可能会在支撑表面（桌子）上的其他地方产生异物:
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')
print "-------------test1--------------"
rospy.sleep(1.0)
def __del__(self):
# Clean the scene:
self._scene.remove_world_object(self._grasp_object_name)
self._scene.remove_world_object(self._table_object_name)
self._scene.remove_world_object(self._table2_object_name)
print "-------------test2------------------"
def _add_table(self, name):
"""
创建表并将其添加到场景
"""
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.85
p.pose.position.y = 0.0
p.pose.position.z = 0.70
q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0))
p.pose.orientation = Quaternion(*q)
# Table size from ~/.gazebo/models/table/model.sdf, using the values
# for the surface link.
self._scene.add_box(name, p, (1, 1, 0.05))
print "-------------test3------------------"
return p.pose
def _add_table2(self, name):
"""
创建表并将其添加到场景
"""
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.0
p.pose.position.y = 0.85
p.pose.position.z = 0.40
q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0))
p.pose.orientation = Quaternion(*q)
# Table size from ~/.gazebo/models/table/model.sdf, using the values
# for the surface link.
self._scene.add_box(name, p, (0.5, 0.5, 0.05))
print "-------------test4------------------"
return p.pose
def _add_grasp_block_(self, name):
"""
创建场景并将其添加到场景
"""
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.5
p.pose.position.y = 0.0
p.pose.position.z = 0.74
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
# Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,
# using the measure tape tool from meshlab.
# The box is the bounding box of the coke cylinder.
# The values are taken from the cylinder base diameter and height.
self._scene.add_box(name, p, (0.045, 0.045, 0.045))
print "-------------test5------------------"
return p.pose
def _add_grasp_block2_(self, name):
"""
创建场景并将其添加到场景
"""
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.5
p.pose.position.y = 0.3
p.pose.position.z = 0.74
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
# Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,
# using the measure tape tool from meshlab.
# The box is the bounding box of the coke cylinder.
# The values are taken from the cylinder base diameter and height.
self._scene.add_box(name, p, (0.045, 0.045, 0.045))
print "-------------test6------------------"
return p.pose
def _generate_grasps(self, pose, width):
"""
Generate grasps by using the grasp generator generate action; based on
server_test.py example on moveit_simple_grasps pkg.
使用抓握生成器生成动作来生成抓握； 基于moveit_simple_grasps pkg上的server_test.py示例
Generate the grasp Pose Array data for visualization,
and then send the grasp goal to the grasp server.
生成抓取姿势阵列数据以进行可视化，然后将抓取目标发送到抓取服务器。
"""
# Create goal:
goal = GenerateGraspsGoal()
goal.pose = pose
goal.width = width
options = GraspGeneratorOptions()
# simple_graps.cpp doesn't implement GRASP_AXIS_Z!
#options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z
options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP
options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL
# @todo disabled because it works better with the default options
#goal.options.append(options)
# 发送目标并等待结果:
# 将目标发送到ActionServer，等待目标完成，并且必须抢占目标.
state = self._grasps_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text())
return None
grasps = self._grasps_ac.get_result().grasps
# 发布掌握（用于调试/可视化目的）:
self._publish_grasps(grasps)
print "-------------test7------------------"
return grasps
def _generate_places(self, target):
"""
Generate places (place locations), based on
https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/
baxter_pick_place/src/block_pick_place.cpp
为该位置的姿势创建姿势数组数据
"""
# Generate places:
places = []
now = rospy.Time.now()
for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(1.0)):
# Create place location:
place = PlaceLocation()
place.place_pose.header.stamp = now
place.place_pose.header.frame_id = self._robot.get_planning_frame()
# Set target position:
place.place_pose.pose = copy.deepcopy(target)
# Generate orientation (wrt Z axis):
q = quaternion_from_euler(0.0, 0.0, angle )
place.place_pose.pose.orientation = Quaternion(*q)
# 生成预安置方法:
place.pre_place_approach.desired_distance = self._approach_retreat_desired_dist
place.pre_place_approach.min_distance = self._approach_retreat_min_dist
place.pre_place_approach.direction.header.stamp = now
place.pre_place_approach.direction.header.frame_id = self._robot.get_planning_frame()
place.pre_place_approach.direction.vector.x = 0
place.pre_place_approach.direction.vector.y = 0
place.pre_place_approach.direction.vector.z = 0.1
# Generate post place approach:
place.post_place_retreat.direction.header.stamp = now
place.post_place_retreat.direction.header.frame_id = self._robot.get_planning_frame()
place.post_place_retreat.desired_distance = self._approach_retreat_desired_dist
place.post_place_retreat.min_distance = self._approach_retreat_min_dist
place.post_place_retreat.direction.vector.x = 0
place.post_place_retreat.direction.vector.y = 0
place.post_place_retreat.direction.vector.z = 0.1
# Add place:
places.append(place)
# Publish places (for debugging/visualization purposes):
self._publish_places(places)
print "-------------test8------------------"
return places
def _create_pickup_goal(self, group, target, grasps):
"""
创建一个MoveIt！ 接送目标
创建一个捡起抓取物体的目标
"""
# Create goal:
goal = PickupGoal()
goal.group_name = group
goal.target_name = target
goal.possible_grasps.extend(grasps)
goal.allowed_touch_objects.append(target)
goal.support_surface_name = self._table_object_name
# 配置目标计划选项:
goal.allowed_planning_time = 7.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 20
print "-------------test9------------------"
return goal
def _create_place_goal(self, group, target, places):
"""
Create a MoveIt! PlaceGoal
Create a place goal for MoveIt!
"""
# Create goal:
goal = PlaceGoal()
goal.group_name = group
goal.attached_object_name = target
goal.place_locations.extend(places)
# Configure goal planning options:
goal.allowed_planning_time = 7.0
goal.planning_options.planning_scene_diff.is_diff = True
goal.planning_options.planning_scene_diff.robot_state.is_diff = True
goal.planning_options.plan_only = False
goal.planning_options.replan = True
goal.planning_options.replan_attempts = 20
print "-------------test10------------------"
return goal
def _pickup(self, group, target, width):
"""
使用计划小组选择目标
"""
# 从掌握生成器服务器获取可能的掌握:
grasps = self._generate_grasps(self._pose_coke_can, width)
# 创建并发送提货目标:
goal = self._create_pickup_goal(group, target, grasps)
state = self._pickup_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text())
return None
result = self._pickup_ac.get_result()
# Check for error:
err = result.error_code.val
if err != MoveItErrorCodes.SUCCESS:
rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err])))
return False
print "-------------test11------------------"
return True
def _place(self, group, target, place):
"""
使用计划组放置目标
"""
# Obtain possible places:
places = self._generate_places(place)
# Create and send Place goal:
goal = self._create_place_goal(group, target, places)
state = self._place_ac.send_goal_and_wait(goal)
if state != GoalStatus.SUCCEEDED:
rospy.logerr('Place goal failed!: %s' % self._place_ac.get_goal_status_text())
return None
result = self._place_ac.get_result()
# Check for error:
err = result.error_code.val
if err != MoveItErrorCodes.SUCCESS:
rospy.logwarn('Group %s cannot place target %s!: %s' % (group, target, str(moveit_error_dict[err])))
return False
print "-------------test12------------------"
return True
def _publish_grasps(self, grasps):
"""
使用PoseArray消息将抓取发布为姿势
"""
if self._grasps_pub.get_num_connections() > 0:
msg = PoseArray()
msg.header.frame_id = self._robot.get_planning_frame()
msg.header.stamp = rospy.Time.now()
for grasp in grasps:
p = grasp.grasp_pose.pose
msg.poses.append(Pose(p.position, p.orientation))
self._grasps_pub.publish(msg)
print "-------------test13------------------"
def _publish_places(self, places):
"""
使用PoseArray消息将位置发布为姿势
"""
if self._places_pub.get_num_connections() > 0:
msg = PoseArray()
msg.header.frame_id = self._robot.get_planning_frame()
msg.header.stamp = rospy.Time.now()
for place in places:
msg.poses.append(place.place_pose.pose)
self._places_pub.publish(msg)
print "-------------test14------------------"
class PickAruco(object):
def __init__(self):
rospy.loginfo("Initalizing...")
self.bridge = CvBridge()
self.tfBuffer = tf2_ros.Buffer()
self.tf_l = tf2_ros.TransformListener(self.tfBuffer)
rospy.loginfo("Waiting for /pickup_pose AS...")
self.pick_as = SimpleActionClient('/pickup_pose', PickUpPoseAction)
time.sleep(1.0)
if not self.pick_as.wait_for_server(rospy.Duration(20)):
rospy.logerr("Could not connect to /pickup_pose AS")
exit()
rospy.loginfo("Waiting for /place_pose AS...")
self.place_as = SimpleActionClient('/place_pose', PickUpPoseAction)
self.place_as.wait_for_server()
rospy.loginfo("Setting publishers to torso and head controller...")
self.torso_cmd = rospy.Publisher(
'/torso_controller/command', JointTrajectory, queue_size=1)
self.head_cmd = rospy.Publisher(
'/head_controller/command', JointTrajectory, queue_size=1)
self.detected_pose_pub = rospy.Publisher('/detected_aruco_pose',
PoseStamped,
queue_size=1,
latch=True)
# /arm_controller/command (trajectory_msgs/JointTrajectory)
self.trajectory_cmd = rospy.Publisher(
'/arm_controller/command', JointTrajectory, queue_size=1)
self.gripper_cmd = rospy.Publisher(
'/gripper_controller/command', JointTrajectory, queue_size=1)
self.hey5_cmd = rospy.Publisher(
'/hand_controller/command', JointTrajectory, queue_size=1)
rospy.loginfo("Waiting for '/play_motion' AS...")
self.play_m_as = SimpleActionClient('/play_motion', PlayMotionAction)
if not self.play_m_as.wait_for_server(rospy.Duration(20)):
rospy.logerr("Could not connect to /play_motion AS")
exit()
rospy.loginfo("Connected!")
rospy.sleep(1.0)
rospy.loginfo("Done initializing PickAruco.")
def strip_leading_slash(self, s):
return s[1:] if s.startswith("/") else s
def pick_aruco(self, string_operation):
# self.prepare_robot()
self.open_hey5()
# self.close_hey5()
# rospy.sleep(3.0)
# rospy.loginfo("spherical_grasp_gui: Waiting for an aruco detection")
# # self.turn_wrist() # experimental function
# # # get aruco pose here: from marker
# # aruco_pose = rospy.wait_for_message('/aruco_single/pose', PoseStamped)
# # aruco_pose.header.frame_id = self.strip_leading_slash(aruco_pose.header.frame_id)
# # manually input aruco pose
# aruco_pose = PoseStamped()
# aruco_pose.header.frame_id = 'base_footprint'
# # # original aruco pose
# # aruco_pose.pose.position.x = 0.528450879403
# # aruco_pose.pose.position.y = -0.0486955713869
# # aruco_pose.pose.position.z = 0.922035729832
# aruco_pose.pose.position.x = 0.544079
# aruco_pose.pose.position.y = 0.050645 #+ 0.05
# aruco_pose.pose.position.z = 0.706404 + 0.08
# aruco_pose.pose.orientation.x = 0.5
# aruco_pose.pose.orientation.y = 0.5
# aruco_pose.pose.orientation.z = 0.5
# aruco_pose.pose.orientation.w = 0.5
# rospy.loginfo("Got: " + str(aruco_pose))
# rospy.loginfo("spherical_grasp_gui: Transforming from frame: " +
# aruco_pose.header.frame_id + " to 'base_footprint'")
# ps = PoseStamped()
# ps.pose.position = aruco_pose.pose.position
# ps.header.stamp = self.tfBuffer.get_latest_common_time("base_footprint", aruco_pose.header.frame_id)
# ps.header.frame_id = aruco_pose.header.frame_id
# transform_ok = False
# while not transform_ok and not rospy.is_shutdown():
# try:
# transform = self.tfBuffer.lookup_transform("base_footprint",
# ps.header.frame_id,
# rospy.Time(0))
# aruco_ps = do_transform_pose(ps, transform)
# transform_ok = True
# except tf2_ros.ExtrapolationException as e:
# rospy.logwarn(
# "Exception on transforming point... trying again \n(" +
# str(e) + ")")
# rospy.sleep(0.01)
# ps.header.stamp = self.tfBuffer.get_latest_common_time("base_footprint", aruco_pose.header.frame_id)
# pick_g = PickUpPoseGoal()
# if string_operation == "pick":
# rospy.loginfo("Setting cube pose based on ArUco detection")
# pick_g.object_pose.pose.position = aruco_ps.pose.position
# pick_g.object_pose.pose.position.z -= 0.1*(1.0/2.0)
# rospy.loginfo("aruco pose in base_footprint:" + str(pick_g))
# pick_g.object_pose.header.frame_id = 'base_footprint'
# pick_g.object_pose.pose.orientation.w = 1.0
# self.detected_pose_pub.publish(pick_g.object_pose)
# rospy.loginfo("Gonna pick:" + str(pick_g))
# self.pick_as.send_goal_and_wait(pick_g)
# rospy.loginfo("Done!")
# # some_pose=arm.get_current_pose(end_effector_link).pose
# # rospy.loginfo("current hand pose "+str(some_pose))
# result = self.pick_as.get_result()
# if str(moveit_error_dict[result.error_code]) != "SUCCESS":
# rospy.logerr("Failed to pick, not trying further")
# return
# # Move torso to its maximum height
# self.lift_torso()
# rospy.sleep(3.0)
# # self.turn_wrist()
# aruco_pose.pose.position.x = 0.733477 - 0.2
# aruco_pose.pose.position.y = 0.057126 + 0.07
# aruco_pose.pose.position.z += 0.2 # 0.2 is the allowance for pouring
# drop_pose = deepcopy(aruco_pose)
# # drop_pose.pose.position.x += 0.2
# # drop_pose.pose.position.y += 0.3
# # drop_pose.pose.position.z += 0.2
# # optimize_goal_pose(drop_pose)
# """
# The following are good good_candidates for pose selection (in eular angles):
# [[0, 0, 0], [0, 0, 270], [0, 180, 0], [0, 180, 180] - no, [0, 180, 270], [0, 270, 0], [0, 270, 180], [0, 270, 270]]
# """
# x, y, z, w = eular_to_q(0,270,180)
# # x, y, z, w = eular_to_q(0,270,270)
# drop_pose.pose.orientation.x = -0.5
# drop_pose.pose.orientation.y = 0
# drop_pose.pose.orientation.z = 0
# drop_pose.pose.orientation.w = 1.2
# success = False
# while success==False:
# result = cartesian_move_to(drop_pose, True)
# rospy.loginfo("success of trajectory: "+str(result))
# # define a goal tolerance for replanning and manipulation
# x_arm, y_arm, z_arm = arm_pose()
# x_aim = drop_pose.pose.position.x
# y_aim = drop_pose.pose.position.y
# z_aim = drop_pose.pose.position.z
# goal_deviation = eular_dist(x_arm, y_arm, z_arm, x_aim, y_aim, z_aim)
# rospy.loginfo("Deviation from target pose: "+str(goal_deviation))
# drop_pose.pose.position.x -= 0.01
# drop_pose.pose.position.y -= 0.01
# if result > 0.9 and goal_deviation < 0.1:
# success = True
# x, y, z = q_to_eular(-0.5, 0, 0, 1.2)
# rospy.loginfo("End effector eular angles: " + str([x,y,z]))
# rospy.sleep(3)
# # self.open_gripper()
# # pour liquid
# self.turn_wrist()
# # arm=MoveGroupCommander('arm')
# # arm.allow_replanning(True)
# # end_effector_link=arm.get_end_effector_link()
# # arm.set_goal_position_tolerance(0.03)
# # arm.set_goal_orientation_tolerance(0.025)
# # arm.allow_replanning(True)
# # reference_frame='base_footprint'
# # arm.set_pose_reference_frame(reference_frame)
# # arm.set_planning_time(5)
# # rospy.sleep(2)
# # start_pose=arm.get_current_pose(end_effector_link).pose
# # rospy.loginfo("End effector start pose: " + str(start_pose))
# # x = start_pose.orientation.x
# # y = start_pose.orientation.y
# # z = start_pose.orientation.z
# # w = start_pose.orientation.w
# # x, y, z = q_to_eular(x, y, z, w)
# # rospy.loginfo("End effector eular angles: " + str([x,y,z]))
# # # Raise arm
# # rospy.loginfo("Moving arm to a safe pose")
# # pmg = PlayMotionGoal()
# # pmg.motion_name = 'pick_final_pose'
# # pmg.skip_planning = False
# # self.play_m_as.send_goal_and_wait(pmg)
# # rospy.loginfo("Raise object done.")
# # # Place the object back to its position
# # rospy.loginfo("Gonna place near where it was")
# # pick_g.object_pose.pose.position.z += 0.05
# # pick_g.object_pose.pose.position.y += 0.2
# # self.place_as.send_goal_and_wait(pick_g)
# # rospy.loginfo("Done!")
def lift_torso(self):
rospy.loginfo("Moving torso up")
jt = JointTrajectory()
jt.joint_names = ['torso_lift_joint']
jtp = JointTrajectoryPoint()
jtp.positions = [0.34]
jtp.time_from_start = rospy.Duration(2.5)
jt.points.append(jtp)
self.torso_cmd.publish(jt)
def turn_wrist(self):
wrist_state = -2.0
rospy.loginfo("Turning Arm")
joint_state = rospy.wait_for_message('/joint_states', JointState)
j1, j2, j3, j4, j5, j6, j7 = joint_state.position[:7]
rospy.loginfo("Previous wrist state: "+str(j7))
max_wrist_state = j7 + 2.0
while j7 < max_wrist_state:
jt = JointTrajectory()
jt.joint_names = ['arm_1_joint',
'arm_2_joint', 'arm_3_joint', 'arm_4_joint',
'arm_5_joint', 'arm_6_joint', 'arm_7_joint']
jtp = JointTrajectoryPoint()
jtp.positions = [j1, j2, j3, j4, j5, j6, j7+0.5]
jtp.time_from_start = rospy.Duration(2)
jt.points.append(jtp)
self.trajectory_cmd.publish(jt)
rospy.sleep(0.25)
# check the wrist joint state
joint_state = rospy.wait_for_message('/joint_states', JointState)
j1, j2, j3, j4, j5, j6, j7 = joint_state.position[:7]
# # jtp = JointTrajectoryPoint()
# # jtp.positions = [1.5,0.0,0.0,0.0,0.0,0.0,0.0]
# # jtp.time_from_start = rospy.Duration(6)
# # jt.points.append(jtp)
# # jtp = JointTrajectoryPoint()
# # jtp.positions = [0.0,0.0,0.0,0.0,0.0,0.0,0.0]
# # jtp.time_from_start = rospy.Duration(9)
# # jt.points.append(jtp)
# self.trajectory_cmd.publish(jt)
# rospy.loginfo("Done 1")
# rospy.sleep(0.1)
# jt = JointTrajectory()
# jt.joint_names = ['arm_1_joint',
# 'arm_2_joint', 'arm_3_joint', 'arm_4_joint',
# 'arm_5_joint', 'arm_6_joint', 'arm_7_joint']
# jtp = JointTrajectoryPoint()
# jtp.positions = [3,0.0,0.0,0.0,0.0,0.0,0.0]
# jtp.time_from_start = rospy.Duration(6)
# jt.points.append(jtp)
# self.trajectory_cmd.publish(jt)
# rospy.loginfo("Done 2")
# rospy.sleep(0.1)
# jt = JointTrajectory()
# jt.joint_names = ['arm_1_joint',
# 'arm_2_joint', 'arm_3_joint', 'arm_4_joint',
# 'arm_5_joint', 'arm_6_joint', 'arm_7_joint']
# jtp = JointTrajectoryPoint()
# jtp.positions = [0.0,0.0,0.0,0.0,0.0,0.0,0.0]
# jtp.time_from_start = rospy.Duration(9)
# jt.points.append(jtp)
# self.trajectory_cmd.publish(jt)
# rospy.loginfo("Done 3")
# # rospy.sleep(2.0)
rospy.loginfo("Done.")
def lower_head(self):
rospy.loginfo("Moving head down")
jt = JointTrajectory()
jt.joint_names = ['head_1_joint', 'head_2_joint']
jtp = JointTrajectoryPoint()
jtp.positions = [0.0, -0.75]
jtp.time_from_start = rospy.Duration(2.0)
jt.points.append(jtp)
self.head_cmd.publish(jt)
rospy.loginfo("Done.")
def prepare_robot(self):
rospy.loginfo("Unfold arm safely")
pmg = PlayMotionGoal()
pmg.motion_name = 'arm_raise'
pmg.skip_planning = False
self.play_m_as.send_goal_and_wait(pmg)
rospy.loginfo("Done.")
self.lower_head()
rospy.loginfo("Robot prepared.")
def open_gripper(self):
rospy.loginfo("Opening Gripper")
jt = JointTrajectory()
jt.joint_names = ['gripper_left_finger_joint', 'gripper_right_finger_joint']
jtp = JointTrajectoryPoint()
jtp.positions = [0.044, 0.044]
jtp.time_from_start = rospy.Duration(0.5)
jt.points.append(jtp)
self.gripper_cmd.publish(jt)
rospy.loginfo("Done.")
def open_hey5(self):
rospy.loginfo("Opening Gripper")
jt = JointTrajectory()
jt.joint_names = ['hand_thumb_joint', 'hand_index_joint', 'hand_mrl_joint']
jtp = JointTrajectoryPoint()
jtp.positions = [-1.0, -1.0, -1.0]
jtp.time_from_start = rospy.Duration(0.1)
jt.points.append(jtp)
jtp = JointTrajectoryPoint()
jtp.positions = [0.0, 0.0, 0.0]
jtp.time_from_start = rospy.Duration(2.5)
jt.points.append(jtp)
self.hey5_cmd.publish(jt)
rospy.loginfo("Done.")
def close_hey5(self):
rospy.loginfo("Closing Gripper")
jt = JointTrajectory()
jt.joint_names = ['hand_thumb_joint', 'hand_index_joint', 'hand_mrl_joint']
jtp = JointTrajectoryPoint()
jtp.positions = [2.37, 0.0, 0.0]
jtp.time_from_start = rospy.Duration(0.1)
jt.points.append(jtp)
jtp = JointTrajectoryPoint()
jtp.positions = [6.2, 6.8, 9.2]
jtp.time_from_start = rospy.Duration(2.5)
jt.points.append(jtp)
self.hey5_cmd.publish(jt)
rospy.loginfo("Done.")
#!/usr/bin/env python
import roslib; roslib.load_manifest('wubble2_robot')
import rospy
from actionlib import SimpleActionClient
from object_manipulation_msgs.msg import GraspHandPostureExecutionAction
from object_manipulation_msgs.msg import GraspHandPostureExecutionGoal
if __name__ == '__main__':
rospy.init_node('test_gripper_posture_controller', anonymous=True)
posture_controller = SimpleActionClient('wubble_gripper_grasp_action', GraspHandPostureExecutionAction)
posture_controller.wait_for_server()
rospy.loginfo('got gripper_posture_controller')
pg = GraspHandPostureExecutionGoal()
pg.goal = GraspHandPostureExecutionGoal.RELEASE
posture_controller.send_goal(pg)
posture_controller.wait_for_result()
rospy.loginfo('finished releasing')
pg = GraspHandPostureExecutionGoal()
pg.goal = GraspHandPostureExecutionGoal.GRASP
posture_controller.send_goal(pg)
posture_controller.wait_for_result()
rospy.loginfo('finished grasping')
class Explore(object):
def __init__(self, height):
self.motion_height = height # Height of the motion to the ground
# Create trajectory server
self.exploration_server = SimpleActionServer('assessment_server',
ExecuteAssesstAction,
self.exploreCallback,
False)
self.server_feedback = ExecuteAssesstFeedback()
self.server_result = ExecuteAssesstResult()
# Get client from trajectory server
self.trajectory_client = SimpleActionClient(
"approach_server", ExecuteDroneApproachAction)
self.trajectory_client.wait_for_server()
self.next_point = ExecuteDroneApproachGoal(
) # Message to define next position to look for victims
#Planning scene client
self.frontiers_client = rospy.ServiceProxy('frontiers_server/find',
Frontiers)
self.frontiers_client.wait_for_service()
self.frontiers_req = FrontiersRequest() #Frontiers request message
# Variables
self.sonar_me = Condition()
self.odometry_me = Condition()
self.current_height = None
self.odometry = None
# Subscribe to sonar_height
rospy.Subscriber("sonar_height",
Range,
self.sonar_callback,
queue_size=10)
# Subscribe to ground_truth to monitor the current pose of the robot
rospy.Subscriber("ground_truth/state", Odometry, self.poseCallback)
self.scene_req = GetPlanningSceneRequest()
# Start trajectory server
self.exploration_server.start()
def sonar_callback(self, msg):
'''
Function to update drone height
'''
self.sonar_me.acquire()
self.current_height = msg.range
self.sonar_me.release()
def poseCallback(self, odometry):
'''
Monitor the current position of the robot
'''
self.odometry_me.acquire()
self.odometry = odometry.pose.pose
self.odometry_me.notify()
self.odometry_me.release()
def trajectory_feed(self, msg):
'''
Verifies preemption requisitions
'''
print("\n\n\nASSESSMENT FEEDBACK")
if self.exploration_server.is_preempt_requested():
self.trajectory_client.cancel_goal()
def exploreCallback(self, pose):
'''
Execute a loop looking for frontiers and moving to points unvisited into the defined area
'''
# Wait till the robot pose is received
self.odometry_me.acquire()
while self.odometry == None:
self.odometry_me.wait()
self.next_point.goal = self.odometry
self.odometry_me.release()
self.frontiers_req.x_min = 0.0
self.frontiers_req.x_max = 50.0
self.frontiers_req.y_min = 0.0
self.frontiers_req.y_max = 50.0
trials = 0 # v_search trials
while not rospy.is_shutdown():
self.odometry_me.acquire()
self.server_result.last_pose = self.odometry
self.server_feedback.current_pose = self.odometry
self.odometry_me.release()
if self.exploration_server.is_preempt_requested():
self.exploration_server.set_preempted(self.server_result)
return
self.exploration_server.publish_feedback(self.server_feedback)
self.sonar_me.acquire()
# print("Current height from ground:\n\n{}".format(self.current_height)) # Current distance from ground
h_error = self.motion_height - self.current_height
self.sonar_me.release()
self.odometry_me.acquire()
self.next_point.goal.position.z = self.odometry.position.z + h_error # Desired z position
self.odometry_me.release()
self.trajectory_client.send_goal(self.next_point,
feedback_cb=self.trajectory_feed)
self.trajectory_client.wait_for_result() # Wait for the result
result = self.trajectory_client.get_state(
) # Get the state of the action
# print(result)
if result == GoalStatus.SUCCEEDED:
p = Pose()
self.odometry_me.acquire()
p = self.odometry
self.frontiers_req.explored.append(p)
self.odometry_me.release()
# Verify if all the area have been explored and find next frontier point if needed
# if 'all area explored':
# self.odometry_me.acquire()
# self.server_result.last_pose = self.odometry
# self.odometry_me.release()
# self.exploration_server.set_succeeded(self.server_result)
status = self.findFrontiers()
if not status:
self.exploration_server.set_succeeded(self.server_result)
return
self.odometry_me.acquire()
self.server_result.last_pose = self.odometry
self.odometry_me.release()
# self.next_point.goal.position.y =
# self.next_point.goal.position.x =
# theta =
# Convert desired angle
# q = quaternion_from_euler(0,0,theta,'ryxz')
# self.next_point.goal.orientation.x = q[0]
# self.next_point.goal.orientation.y = q[1]
# self.next_point.goal.orientation.z = q[2]
# self.next_point.goal.orientation.w = q[3]
elif result == GoalStatus.ABORTED:
trials += 1
if trials == 2:
self.exploration_server.set_aborted(self.server_result)
return
def findFrontiers(self):
'''
Return points not visited into the specified frontier
'''
rospy.loginfo("Looking for frontiers!")
frontiers = self.frontiers_client.call(self.frontiers_req)
if frontiers.frontiers:
self.next_point.goal.position.x = frontiers.frontiers[0].x
self.next_point.goal.position.y = frontiers.frontiers[0].y
self.next_point.goal.position.x = self.motion_height
return True
else:
return False
class StateClient(object):
def __init__(self, do_register=True, silent=True):
self._name = rospy.get_name()
self._silent = silent
self._acknowledge_publisher = Publisher('/robot/state/server',
RobotModeMsg,
latch=True,
queue_size=5)
self._state_subscriber = Subscriber('/robot/state/clients',
RobotModeMsg,
self.server_state_information,
queue_size=10)
self._state_info = ServiceProxy('/robot/state/info', GetStateInfo)
self._state_changer = ActionClient('/robot/state/change',
RobotModeAction)
if do_register:
self.client_register()
def client_register(self):
rospy.wait_for_service('/robot/state/register')
self._register_service_client = rospy.ServiceProxy(
'/robot/state/register', RegisterNodeSrv)
req = RegisterNodeSrvRequest()
req.nodeName = self._name
req.type = RegisterNodeSrvRequest.TYPE_STARTED
try:
self._register_service_client(req)
except rospy.ServiceException:
logerr("[%s] Failed to register node. Retrying...", self._name)
def client_initialize(self):
rospy.wait_for_service('/robot/state/register')
self._register_service_client = rospy.ServiceProxy(
'/robot/state/register', RegisterNodeSrv)
req = RegisterNodeSrvRequest()
req.nodeName = self._name
req.type = RegisterNodeSrvRequest.TYPE_INITIALIZED
try:
self._register_service_client(req)
except rospy.ServiceException:
logerr("[%s] Failed to register node. Retrying...", self._name)
def start_transition(self, state):
if not self._silent:
loginfo("[%s] Starting Transition to state %i", self._name, state)
self.transition_complete(state)
def transition_complete(self, state):
if not self._silent:
loginfo("[%s] Node Transition to state %i Completed", self._name,
state)
msg = RobotModeMsg()
msg.nodeName = self._name
msg.mode = state
msg.type = RobotModeMsg.TYPE_ACK
self._acknowledge_publisher.publish(msg)
def get_current_state(self):
""" Returns the current robot state from the state manager. """
req = GetStateInfoRequest()
req.option = GetStateInfoRequest.CURRENT_STATE
try:
res = self._state_info(req)
if res.state == -1:
# There is a bug with the server.
logerr('GetStateInfo: %s is not a valid request.', req.option)
return res.state
except rospy.ServiceException:
logerr('Service GetStateInfo failed to respond.')
return None
def get_previous_state(self):
""" Returns the previous robot state from the state manager. """
req = GetStateInfoRequest()
req.option = GetStateInfoRequest.PREVIOUS_STATE
try:
res = self._state_info(req)
if res.state == -1:
# There is a bug with the server.
logerr('GetStateInfo: %s is not a valid request.', req.option)
return res.state
except rospy.ServiceException:
logerr('Service GetStateInfo failed to respond.')
return None
def transition_to_state(self, state):
if not self._silent:
loginfo("[%s] Requesting transition to state %i", self._name,
state)
msg = RobotModeMsg()
msg.nodeName = self._name
msg.mode = state
msg.type = RobotModeMsg.TYPE_REQUEST
self._acknowledge_publisher.publish(msg)
def change_state_and_wait(self, state, timeout=None):
""" Changes the state and waits for all the clients to change.
:param :state A state to transition to.
:param :timeout A timeout in seconds to complete the state transition.
"""
timeout = rospy.Duration(timeout) if timeout else rospy.Duration()
msg = RobotModeMsg()
msg.mode = state
msg.nodeName = self._name
msg.type = RobotModeMsg.TYPE_REQUEST
goal = RobotModeGoal(modeMsg=msg)
self._state_changer.wait_for_server()
self._state_changer.send_goal(goal)
return self._state_changer.wait_for_result(timeout=timeout)
def server_state_information(self, msg):
if not self._silent:
loginfo("[%s] Received new information from state server",
self._name)
if msg.type == RobotModeMsg.TYPE_TRANSITION:
self.start_transition(msg.mode)
elif msg.type == RobotModeMsg.TYPE_START:
if not self._silent:
loginfo("[%s] System Transitioned, starting work", self._name)
else:
logerr(
"[%s] StateClient received a new state command, that is not understandable",
self._name)
class QualitativeMove(PNPSimplePluginServer):
def __init__(self, name):
rospy.loginfo("Starting %s ..." % name)
self.listener = tf.TransformListener()
self.target_frame = rospy.get_param("~target_frame", "base_link")
with open(rospy.get_param("~config_file"), 'r') as f:
self.distance = yaml.load(f)["distances"]
self.move_client = SimpleActionClient("move_base", MoveBaseAction)
self._ps = PNPSimplePluginServer(
name=name,
ActionSpec=QualitativeMovePepperAction,
execute_cb=self.execute_cb,
auto_start=False)
rospy.loginfo("Creating tracker client")
self.start_client = SimpleActionClient("/start_tracking_person",
TrackPersonAction)
self.start_client.wait_for_server()
rospy.loginfo("Tracker client connected")
self._ps.start()
rospy.loginfo("... done")
def execute_cb(self, goal):
self.start_client.send_goal(TrackPersonGoal(id=goal.id, no_turn=True))
try:
msg = rospy.wait_for_message("/naoqi_driver_node/people_detected",
PersonDetectedArray,
timeout=5.)
except rospy.ROSException as e:
rospy.logwarn(e)
self._ps.set_aborted()
else:
int_id = int(goal.id.split("_")[1])
for p in msg.person_array:
if p.id == int_id:
try:
t = self.listener.getLatestCommonTime(
self.target_frame, msg.header.frame_id)
p_pose = PoseStamped(header=msg.header,
pose=p.person.position)
p_pose.header.stamp = t
bl_pose = self.listener.transformPose(
self.target_frame, p_pose)
except (tf.Exception, tf.LookupException,
tf.ConnectivityException) as ex:
rospy.logwarn(ex)
else:
target_dist = p.person.distance - self.distance[
goal.to]
d = target_dist / p.person.distance
theta = math.atan2(bl_pose.pose.position.y,
bl_pose.pose.position.x)
target_pose = bl_pose
target_pose.pose.position.x *= d
target_pose.pose.position.y *= d
target_pose.pose.orientation.x = 0.
target_pose.pose.orientation.y = 0.
target_pose.pose.orientation.z = math.sin(theta / 2.)
target_pose.pose.orientation.w = math.cos(theta / 2.)
print target_pose
self.move_client.send_goal_and_wait(
MoveBaseGoal(target_pose=target_pose))
finally:
break
res = QualitativeMovePepperResult()
res.result.append(
ActionResult(cond="robot_at_home", truth_value=False))
res.result.append(
ActionResult(cond="robot_pose_unknown", truth_value=True))
self._ps.set_succeeded(res)
class MyFrame(wx.Frame):
def __init__(self,parent,id):
the_size=(700, 520)
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.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 ')
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+60))
self.reply_show=wx.TextCtrl(self.panel, style=(wx.TE_CENTER |wx.TE_READONLY),
value='', size=(550, 30), pos=(20, btn_height+80))
# 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.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.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 display_init(self):
js_pos=[20, 20]
js_btn_length=[70, 70, 61, 80]
js_distances=[10, 20, 10, 26]
dis_h=50
for i in xrange(len(self.js_display)):
self.jp_button[i]=wx.Button(self.panel,
label='J'+str(i+1)+' +',
pos=(js_pos[0],
js_pos[1]+(5-i)*dis_h),
size=(70,40))
dis_tmp=js_btn_length[0]+js_distances[0]
self.jp_button[i].Bind(wx.EVT_LEFT_DOWN,
lambda evt, mark=i+1 : self.teleop_joints(evt, mark) )
self.jp_button[i].Bind(wx.EVT_LEFT_UP,
lambda evt, mark=i+1 : self.release_button(evt, mark) )
self.jm_button[i]=wx.Button(self.panel,
label='J'+str(i+1)+' -',
pos=(js_pos[0]+dis_tmp,
js_pos[1]+(5-i)*dis_h),
size=(70,40))
dis_tmp+=js_btn_length[1]+js_distances[1]
self.jm_button[i].Bind(wx.EVT_LEFT_DOWN,
lambda evt, mark=-1*(i+1) : self.teleop_joints(evt, mark) )
self.jm_button[i].Bind(wx.EVT_LEFT_UP,
lambda evt, mark=-1*(i+1) : self.release_button(evt, mark) )
pos_js_label=(js_pos[0]+dis_tmp, js_pos[1]+(5-i)*dis_h)
self.js_label[i]=wx.StaticText(self.panel,
label='J'+str(i+1)+'/deg:',
pos=pos_js_label)
self.js_label[i].SetPosition((pos_js_label[0], pos_js_label[1]+abs(40-self.js_label[i].GetSize()[1])/2))
dis_tmp+=js_btn_length[2]+js_distances[2]
pos_js_display=(js_pos[0]+dis_tmp, js_pos[1]+(5-i)*dis_h)
self.js_display[i]=wx.TextCtrl(self.panel,
style=(wx.TE_CENTER |wx.TE_READONLY),
value=' 0000.00 ',
pos=pos_js_display)
self.js_display[i].SetPosition((pos_js_display[0], pos_js_display[1]+abs(40-self.js_display[i].GetSize()[1])/2))
dis_tmp+=js_btn_length[3]+js_distances[3]
ps_pos=[js_pos[0]+dis_tmp, 20]
ps_btn_length=[70, 70, 53, 80]
ps_distances=[10, 20, 10, 20]
pcs_btn_label=['X', 'Y', 'Z', 'Rx', 'Ry', 'Rz']
pcs_label=['X', 'Y', 'Z', 'R', 'P', 'Y']
unit_label=['/mm:', '/mm:', '/mm:', '/deg:', '/deg:', '/deg:']
for i in xrange(len(self.ps_display)):
self.pp_button[i]=wx.Button(self.panel,
label=pcs_btn_label[i]+' +',
pos=(ps_pos[0],
ps_pos[1]+(5-i)*dis_h),
size=(70,40))
dis_tmp=ps_btn_length[0]+ps_distances[0]
self.pp_button[i].Bind(wx.EVT_LEFT_DOWN,
lambda evt, mark=i+1 : self.teleop_pcs(evt, mark) )
self.pp_button[i].Bind(wx.EVT_LEFT_UP,
lambda evt, mark=i+1 : self.release_button(evt, mark) )
self.pm_button[i]=wx.Button(self.panel,
label=pcs_btn_label[i]+' -',
pos=(ps_pos[0]+dis_tmp,
ps_pos[1]+(5-i)*dis_h),
size=(70,40))
dis_tmp+=ps_btn_length[1]+ps_distances[1]
self.pm_button[i].Bind(wx.EVT_LEFT_DOWN,
lambda evt, mark=-1*(i+1) : self.teleop_pcs(evt, mark) )
self.pm_button[i].Bind(wx.EVT_LEFT_UP,
lambda evt, mark=-1*(i+1) : self.release_button(evt, mark) )
pos_ps_label=(ps_pos[0]+dis_tmp, ps_pos[1]+(5-i)*dis_h)
self.ps_label[i]=wx.StaticText(self.panel,
label=pcs_label[i]+unit_label[i],
pos=pos_ps_label)
self.ps_label[i].SetPosition((pos_ps_label[0], pos_ps_label[1]+abs(40-self.ps_label[i].GetSize()[1])/2))
dis_tmp+=ps_btn_length[2]+ps_distances[2]
pos_ps_display=(ps_pos[0]+dis_tmp, ps_pos[1]+(5-i)*dis_h)
self.ps_display[i]=wx.TextCtrl(self.panel,
style=(wx.TE_CENTER |wx.TE_READONLY),
value='',
pos=pos_ps_display)
self.ps_display[i].SetPosition((pos_ps_display[0], pos_ps_display[1]+abs(40-self.ps_display[i].GetSize()[1])/2))
dis_tmp+=ps_btn_length[3]+ps_distances[3]
def velocity_setting_cb(self, event):
current_velocity_scaling=self.velocity_setting.GetValue()*0.01
self.teleop_api_dynamic_reconfig_client.update_configuration({'velocity_scaling': current_velocity_scaling})
wx.CallAfter(self.update_velocity_scaling_show, current_velocity_scaling)
def basic_api_reconfigure_cb(self, config):
if self.velocity_setting_show.GetValue()!=config.velocity_scaling:
self.velocity_setting.SetValue(int(config.velocity_scaling*100))
wx.CallAfter(self.update_velocity_scaling_show, config.velocity_scaling)
def action_stop(self):
self.action_client.wait_for_server()
self.action_goal.trajectory.header.stamp.secs=0
self.action_goal.trajectory.header.stamp.nsecs=0
self.action_goal.trajectory.points=[]
self.action_client.send_goal(self.action_goal)
def teleop_joints(self,event,mark):
self.call_teleop_joint_req.data=mark
resp=self.call_teleop_joint.call(self.call_teleop_joint_req)
wx.CallAfter(self.update_reply_show, resp)
event.Skip()
def teleop_pcs(self,event,mark):
self.call_teleop_cart_req.data=mark
resp=self.call_teleop_cart.call(self.call_teleop_cart_req)
wx.CallAfter(self.update_reply_show, resp)
event.Skip()
def release_button(self, event, mark):
self.call_teleop_stop_req.data=True
resp=self.call_teleop_stop.call(self.call_teleop_stop_req)
wx.CallAfter(self.update_reply_show, resp)
event.Skip()
def call_set_bool_common(self, event, client, request):
btn=event.GetEventObject()
check_list=['Servo On', 'Servo Off']
if btn.GetName() in check_list:
self.show_message_dialog(btn.GetName(), client, request)
else:
try:
resp=client.call(request)
wx.CallAfter(self.update_reply_show, resp)
except rospy.ServiceException, e:
resp=SetBoolResponse()
resp.success=False
resp.message='no such service in simulation'
wx.CallAfter(self.update_reply_show, resp)
event.Skip()
class Arm:
''' Interfacing with one arm for controlling mode and action execution'''
_is_autorelease_on = False
def __init__(self, arm_index, tf_listener):
self.arm_index = arm_index
self.tf_listener = tf_listener
self.arm_mode = ArmMode.HOLD
self.gripper_state = None
self.gripper_joint_name = self._side_prefix() + '_gripper_joint'
self.ee_name = self._side_prefix() + '_wrist_roll_link'
self.joint_names = [
self._side_prefix() + '_shoulder_pan_joint',
self._side_prefix() + '_shoulder_lift_joint',
self._side_prefix() + '_upper_arm_roll_joint',
self._side_prefix() + '_elbow_flex_joint',
self._side_prefix() + '_forearm_roll_joint',
self._side_prefix() + '_wrist_flex_joint',
self._side_prefix() + '_wrist_roll_joint'
]
self.all_joint_names = []
self.all_joint_poses = []
self.last_ee_pose = None
self.movement_buffer_size = 40
self.last_unstable_time = rospy.Time.now()
self.arm_movement = []
self.lock = threading.Lock()
rospy.Subscriber('joint_states', JointState, self.joint_states_cb)
switch_controller = 'pr2_controller_manager/switch_controller'
rospy.wait_for_service(switch_controller)
self.switch_service = rospy.ServiceProxy(switch_controller,
SwitchController)
rospy.loginfo('Got response from the switch controller for ' +
self.side() + ' arm.')
# # Create a trajectory action client
traj_controller_name = (self._side_prefix() +
'_arm_controller/joint_trajectory_action')
self.traj_action_client = SimpleActionClient(traj_controller_name,
JointTrajectoryAction)
self.traj_action_client.wait_for_server()
rospy.loginfo('Got response from trajectory action server for ' +
self.side() + ' arm.')
# Set up Inversse Kinematics
self.ik_srv = None
self.ik_request = None
self.ik_joints = None
self.ik_limits = None
self._setup_ik()
gripper_name = (self._side_prefix() +
'_gripper_controller/gripper_action')
self.gripper_client = SimpleActionClient(gripper_name,
Pr2GripperCommandAction)
self.gripper_client.wait_for_server()
rospy.loginfo('Got response from gripper server for ' + self.side() +
' arm.')
self.check_gripper_state()
def _setup_ik(self):
'''Sets up services for inverse kinematics'''
ik_srv_name = '/compute_ik'
rospy.loginfo('IK info service has responded for ' + self.side() +
' arm.')
rospy.wait_for_service(ik_srv_name)
self.ik_srv = rospy.ServiceProxy(ik_srv_name,
GetPositionIK,
persistent=True)
rospy.loginfo('IK service has responded for ' + self.side() + ' arm.')
robot = moveit_commander.RobotCommander()
# Set up common parts of an IK request
self.ik_request = GetPositionIKRequest()
request = self.ik_request.ik_request
request.timeout = rospy.Duration(4)
group_name = self.side() + '_arm'
request.group_name = group_name
self.ik_joints = self.joint_names
self.ik_limits = [robot.get_joint(x).bounds() for x in self.ik_joints]
request.ik_link_name = self.ee_name
request.pose_stamped.header.frame_id = 'base_link'
request.robot_state.joint_state.name = self.ik_joints
request.robot_state.joint_state.position = [0] * len(self.joint_names)
def side(self):
'''Returns the word right or left depending on arm side'''
if (self.arm_index == Side.RIGHT):
return 'right'
elif (self.arm_index == Side.LEFT):
return 'left'
def _side_prefix(self):
''' Returns the letter r or l depending on arm side'''
side = self.side()
return side[0]
def get_ee_state(self, ref_frame='base_link'):
''' Returns end effector pose for the arm'''
try:
time = self.tf_listener.getLatestCommonTime(
ref_frame, self.ee_name)
(position, orientation) = self.tf_listener.lookupTransform(
ref_frame, self.ee_name, time)
tf_pose = Pose()
tf_pose.position = Point(position[0], position[1], position[2])
tf_pose.orientation = Quaternion(orientation[0], orientation[1],
orientation[2], orientation[3])
return tf_pose
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException) as e:
rospy.logwarn('Something wrong with transform request: ' + str(e))
return None
def joint_states_cb(self, msg):
'''Callback function that saves the joint positions when a
joint_states message is received'''
self.lock.acquire()
self.all_joint_names = msg.name
self.all_joint_poses = msg.position
self.lock.release()
def get_joint_state(self, joint_names=None):
'''Returns position for the requested or all arm joints'''
if joint_names is None:
joint_names = self.joint_names
if self.all_joint_names == []:
rospy.logerr("No robot_state messages received!\n")
return []
positions = []
self.lock.acquire()
for joint_name in joint_names:
if joint_name in self.all_joint_names:
index = self.all_joint_names.index(joint_name)
position = self.all_joint_poses[index]
positions.append(position)
else:
rospy.logerr("Joint %s not found!", joint_name)
self.lock.release()
return positions
def _solve_ik(self, ee_pose, seed=None):
'''Gets the IK solution for end effector pose'''
self.ik_request.ik_request.pose_stamped.pose = ee_pose
if seed is None:
# If no see is specified for IK search, start search at midpoint
seed = []
for i in range(0, len(self.ik_joints)):
seed.append(
(self.ik_limits[i][0] + self.ik_limits[i][1]) / 2.0)
if ee_pose is None:
rospy.logerr('End effector pose was None!')
if seed is None:
rospy.logerr('Seed is still None!')
self.ik_request.ik_request.robot_state.joint_state.position = seed
try:
#rospy.loginfo('Sending IK request.')
response = self.ik_srv(self.ik_request)
if (response.error_code.val == response.error_code.SUCCESS):
# The solution contains all robot joints, we only need the joints of one arm.
response_names = response.solution.joint_state.name
response_positions = response.solution.joint_state.position
return [
response_positions[i] for i, x in enumerate(response_names)
if x in self.joint_names
]
else:
return None
except Exception:
rospy.logerr('Exception while getting the IK solution.')
return None
def set_mode(self, mode):
'''Releases or holds the arm by turning the controller on/off'''
controller_name = self._side_prefix() + '_arm_controller'
if mode == ArmMode.RELEASE:
start_controllers = []
stop_controllers = [controller_name]
rospy.loginfo('Switching ' + str(self.side()) +
' arm to the kinesthetic mode')
elif mode == ArmMode.HOLD:
start_controllers = [controller_name]
stop_controllers = []
rospy.loginfo('Switching ' + str(self.side()) +
' to the Joint-control mode')
else:
rospy.logwarn('Unknown mode ' + str(mode) +
'. Keeping the current mode.')
return
try:
self.switch_service(start_controllers, stop_controllers, 1)
self.arm_mode = mode
except rospy.ServiceException:
rospy.logerr("Service did not process request")
def get_mode(self):
'''Returns the current arm mode (released/holding)'''
return self.arm_mode
def _send_gripper_command(self, pos=0.08, eff=30.0, wait=True):
'''Sets the position of the gripper'''
command = Pr2GripperCommandGoal()
command.command.position = pos
command.command.max_effort = eff
self.gripper_client.send_goal(command)
if wait:
self.gripper_client.wait_for_result(rospy.Duration(5.0))
def is_gripper_moving(self):
''' Whether or not the gripper is in the process of opening/closing'''
return (self.gripper_client.get_state() == GoalStatus.ACTIVE
or self.gripper_client.get_state() == GoalStatus.PENDING)
def is_gripper_at_goal(self):
''' Whether or not the gripper has reached its goal'''
return (self.gripper_client.get_state() == GoalStatus.SUCCEEDED)
def get_gripper_state(self):
'''Returns current gripper state'''
return self.gripper_state
def get_gripper_position(self):
joint_name = self.gripper_joint_name
gripper_pos = self.get_joint_state([joint_name])
if gripper_pos != []:
return gripper_pos[0]
def check_gripper_state(self, joint_name=None):
'''Checks gripper state at the hardware level'''
if (joint_name is None):
joint_name = self.gripper_joint_name
gripper_pos = self.get_joint_state([joint_name])
if gripper_pos != []:
if gripper_pos[0] > 0.078:
self.gripper_state = GripperState.OPEN
else:
self.gripper_state = GripperState.CLOSED
else:
rospy.logwarn('Could not update the gripper state.')
def open_gripper(self, pos=0.08, eff=30.0, wait=True):
'''Opens gripper'''
self._send_gripper_command(pos, eff, wait)
self.gripper_state = GripperState.OPEN
def close_gripper(self, pos=0.0, eff=30.0, wait=True):
'''Closes gripper'''
self._send_gripper_command(pos, eff, wait)
self.gripper_state = GripperState.CLOSED
def set_gripper(self, gripper_state):
'''Sets gripper to the desired state'''
if (gripper_state == GripperState.CLOSED):
self.close_gripper()
elif (gripper_state == GripperState.OPEN):
self.open_gripper()
def move_to_joints(self, joints, time_to_joint):
'''Moves the arm to the desired joints'''
# Setup the goal
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory.header.stamp = (rospy.Time.now() +
rospy.Duration(0.1))
traj_goal.trajectory.joint_names = self.joint_names
velocities = [0] * len(joints)
traj_goal.trajectory.points.append(
JointTrajectoryPoint(
positions=joints,
velocities=velocities,
time_from_start=rospy.Duration(time_to_joint)))
self.traj_action_client.send_goal(traj_goal)
def get_time_to_pose(self, target_pose):
'''Returns the time to get to the arm pose held in target_pose.
Args:
target_pose (Pose|None): A Pose holding the pose to
move to, or None if the arm should not move.
Returns:
float|None: How long (in seconds) to allow for moving
arm to the pose in target_pose, or None if the arm
will not move.
'''
# Get readable strings representing the referred arm.
arm_name_lower = self.side()
arm_name_cap = arm_name_lower.capitalize()
# Check whether arm will move at all.
if target_pose is None:
rospy.loginfo('\t' + arm_name_cap + ' arm will not move.')
return None
else:
time_to_pose = Arm._get_time_bw_poses(self.get_ee_state(),
target_pose)
rospy.loginfo('\tDuration until next frame for ' + arm_name_lower +
'arm : ' + str(time_to_pose))
return time_to_pose
@staticmethod
def _get_time_bw_poses(pose0, pose1, velocity=0.2):
'''Determines how much time should be allowed for moving between
pose0 and pose1 at velocity.
Args:
pose0 (Pose)
pose1 (Pose)
velocity (float, optional): Defaults to 0.2.
Returns:
float: How long (in seconds) to allow for moving between
pose0 and pose1 and velocity.
'''
dist = Arm.get_distance_bw_poses(pose0, pose1)
duration = dist / velocity
return (DURATION_MIN_THRESHOLD
if duration < DURATION_MIN_THRESHOLD else duration)
#TODO
def is_executing(self):
'''Whether or not there is an ongoing action execution on the arm'''
return (self.traj_action_client.get_state() == GoalStatus.ACTIVE
or self.traj_action_client.get_state() == GoalStatus.PENDING)
#TODO
def is_successful(self):
'''Whetehr the execution succeeded'''
return (self.traj_action_client.get_state() == GoalStatus.SUCCEEDED)
def get_ik_for_ee(self, ee_pose, seed):
''' Finds the IK solution for given end effector pose'''
joints = self._solve_ik(ee_pose, seed)
## If our seed did not work, try once again with the default seed
if joints is None:
#rospy.logwarn('Could not find IK solution with preferred seed,' +
# 'will try default seed.')
joints = self._solve_ik(ee_pose)
if joints is None:
pass
#rospy.logwarn('IK out of bounds, will use the seed directly.')
else:
rollover = array((array(joints) - array(seed)) / pi, int)
joints -= ((rollover + (sign(rollover) + 1) / 2) / 2) * 2 * pi
return joints
@staticmethod
def get_distance_bw_poses(pose0, pose1):
'''Returns the dissimilarity between two end-effector poses'''
w_pos = 1.0
w_rot = 0.2
pos0 = array((pose0.position.x, pose0.position.y, pose0.position.z))
pos1 = array((pose1.position.x, pose1.position.y, pose1.position.z))
rot0 = array((pose0.orientation.x, pose0.orientation.y,
pose0.orientation.z, pose0.orientation.w))
rot1 = array((pose1.orientation.x, pose1.orientation.y,
pose1.orientation.z, pose1.orientation.w))
pos_dist = w_pos * norm(pos0 - pos1)
rot_dist = w_rot * (1 - dot(rot0, rot1))
if (pos_dist > rot_dist):
dist = pos_dist
else:
dist = rot_dist
return dist
def reset_movement_history(self):
''' Clears the saved history of arm movements'''
self.last_unstable_time = rospy.Time.now()
self.arm_movement = []
def get_movement(self):
'''Returns cumulative movement in recent history'''
return sum(self.arm_movement)
def _record_arm_movement(self, reading):
'''Records the sensed arm movement'''
self.arm_movement = [reading] + self.arm_movement
if (len(self.arm_movement) > self.movement_buffer_size):
self.arm_movement = self.arm_movement[0:self.movement_buffer_size]
def _is_arm_moved_while_holding(self):
'''Checks if user is trying to move the arm while it is stiff'''
threshold = 0.02
if (self.get_mode() == ArmMode.HOLD
and (len(self.arm_movement) == self.movement_buffer_size)
and (self.get_movement() > threshold)):
return True
return False
def _is_arm_stable_while_released(self):
'''Checks if the arm has been stable while being released'''
movement_threshold = 0.02
time_threshold = rospy.Duration(5.0)
is_arm_stable = (self.get_movement() < movement_threshold)
if (not is_arm_stable or self.get_mode() == ArmMode.HOLD):
self.last_unstable_time = rospy.Time.now()
return False
else:
if (rospy.Time.now() - self.last_unstable_time) > time_threshold:
return True
else:
return False
def update(self, is_executing):
''' Periodical update for one arm'''
ee_pose = self.get_ee_state()
if (ee_pose != None and self.last_ee_pose != None):
self._record_arm_movement(
Arm.get_distance_bw_poses(ee_pose, self.last_ee_pose))
self.last_ee_pose = ee_pose
if (not is_executing and Arm._is_autorelease_on):
if (self._is_arm_moved_while_holding()):
rospy.loginfo('Automatically releasing arm.')
self.set_mode(ArmMode.RELEASE)
if (self._is_arm_stable_while_released()):
rospy.loginfo('Automatically holding arm.')
self.set_mode(ArmMode.HOLD)
class IK:
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'
]
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'
]
def __init__(self, side_prefix):
self.side_prefix = side_prefix
# Set up Inversse Kinematics services
ik_info_srv_name = ('pr2_' + self._side() +
'_arm_kinematics_simple/get_ik_solver_info')
ik_srv_name = 'pr2_' + self._side() + '_arm_kinematics_simple/get_ik'
rospy.loginfo('Waiting for IK info service to respond.')
rospy.wait_for_service(ik_info_srv_name)
ik_info_srv = rospy.ServiceProxy(ik_info_srv_name,
GetKinematicSolverInfo)
solver_info = ik_info_srv()
self.ik_joints = solver_info.kinematic_solver_info.joint_names
self.ik_limits = solver_info.kinematic_solver_info.limits
rospy.loginfo('Waiting for IK service to respond.')
rospy.wait_for_service(ik_srv_name)
self.ik_srv = rospy.ServiceProxy(ik_srv_name,
GetPositionIK,
persistent=True)
# Set up common parts of an IK request
self.ik_request = GetPositionIKRequest()
self.ik_request.timeout = rospy.Duration(4.0)
self.ik_request.ik_request.ik_link_name = solver_info.kinematic_solver_info.link_names[
0]
self.ik_request.ik_request.pose_stamped.header.frame_id = 'base_link'
self.ik_request.ik_request.ik_seed_state.joint_state.name = self.ik_joints
self.ik_request.ik_request.ik_seed_state.joint_state.position = [
0
] * len(self.ik_joints)
motion_request_name = '/' + side_prefix + '_arm_motion_request/joint_trajectory_action'
self.traj_action_client = SimpleActionClient(motion_request_name,
JointTrajectoryAction)
rospy.loginfo(
'Waiting for a response from the trajectory motion request server on arm: '
+ side_prefix)
self.traj_action_client.wait_for_server()
rospy.loginfo('IK ready for arm: ' + side_prefix)
def _side(self):
if (self.side_prefix == 'r'):
return 'right'
else:
return 'left'
def move_to_joints(self, positions, time_to_joint):
velocities = [0] * len(positions)
traj_goal = JointTrajectoryGoal()
traj_goal.trajectory.header.stamp = (rospy.Time.now() +
rospy.Duration(0.1))
traj_goal.trajectory.points.append(
JointTrajectoryPoint(
positions=positions,
velocities=velocities,
time_from_start=rospy.Duration(time_to_joint)))
if (self.side_prefix == 'r'):
traj_goal.trajectory.joint_names = IK.r_joint_names
else:
traj_goal.trajectory.joint_names = IK.l_joint_names
self.traj_action_client.send_goal(traj_goal)
def get_ik_for_ee(self, ee_pose, seed=None):
''' Finds the IK solution for given end effector pose'''
self.ik_request.ik_request.pose_stamped.pose = ee_pose
if seed == None:
seed = []
for i in range(0, len(self.ik_joints)):
seed.append((self.ik_limits[i].min_position +
self.ik_limits[i].max_position) / 2.0)
self.ik_request.ik_request.ik_seed_state.joint_state.position = seed
try:
rospy.loginfo('Requesting IK.')
response = self.ik_srv(self.ik_request)
if (response.error_code.val == response.error_code.SUCCESS):
joints = response.solution.joint_state.position
else:
rospy.logwarn('Could not find IK solution.')
return None
except rospy.ServiceException:
rospy.logerr('Exception while getting the IK solution.')
return None
rollover = array((array(joints) - array(seed)) / pi, int)
joints -= ((rollover + (sign(rollover) + 1) / 2) / 2) * 2 * pi
return joints
class Navigation:
def __init__(self):
# initialise state variables
self._reset()
# services
self.robot_pose = Pose()
s = rospy.Service('navigate_to', NavigateTo, self.setNavigate)
# pub/sub
self.nav_done_pub = rospy.Publisher('nav_done', Bool, queue_size=10)
rospy.Subscriber('robot_pose', PoseWithCovarianceStamped,
self._pose_callback)
# NavigateTo
self.move_base_msg = MoveBaseGoal()
self.move_base_msg.target_pose.header.frame_id = 'map'
self.move_base_client = SimpleActionClient('move_base', MoveBaseAction)
self.move_base_client.wait_for_server()
rospy.loginfo("Ready to navigate")
def _pose_callback(self, msg):
self.robot_pose = msg.pose.pose
def _reset(self):
self.state = State.IDLE
self.goal_pose = None
self.move_base_request_sent = False
### service functions
def setExplore(self):
self.state = State.EXPLORING
def setNavigate(self, msg):
self._reset()
self.state = State.NAVIGATING
self.goal_pose = msg.pose
return NavigateToResponse(True)
def setFollow(self):
self.state = State.FOLLOWING
def setGuide(self):
self.state = State.GUIDING
### state based logic
def explore(self):
return
def navigate(self):
if not self.move_base_request_sent:
# create a request to move base
self.move_base_msg.target_pose.header.stamp = rospy.Time.now()
self.move_base_msg.target_pose.pose = self.goal_pose
self.move_base_client.send_goal(self.move_base_msg)
rospy.loginfo('Request sent, waiting for planner to move')
self.move_base_request_sent = True
# Note: I tried using callbacks in send_goal, and using wait_for_response, neither worked and caused the module to wait forever.
# The only active topic in the move_base package seems to be status which can be continously monitored until it reports that the goal has been reached.
# This is not a good solution, but since the correct mechanisms are broken I don't know what else to do.
# Annoyingly something is printing "GOAL Reached!" so there should be a notification of success... I just don't know where it is...
# Workaround for now is to reimplement the distance check here which is not good.
# self.move_base_client.wait_for_result()
# rospy.loginfo(self.move_base_client.get_result)
else:
# Wait for path to be completed
# Note hacky solution to above comment
xdiff = (self.goal_pose.position.x - self.robot_pose.position.x)**2
ydiff = (self.goal_pose.position.y - self.robot_pose.position.y)**2
dist = math.sqrt(xdiff + ydiff)
if dist < 0.2: # 0.2 value comes from YAML file for the local planner and is the xy_goal_tollerance
# notify system of success/failure
# failure will only be available if the interface with move_base is fixed
self.nav_done_pub.publish(True)
rospy.loginfo("Nav done!")
# reset nav state machine
self._reset()
def follow(self):
return
def guide(self):
return
def step(self):
# check collisions TODO
# execute behaviour
if self.state == State.EXPLORING:
self.explore()
elif self.state == State.NAVIGATING:
self.navigate()
elif self.state == State.FOLLOWING:
self.follow()
elif self.state == State.GUIDING:
self.guide()
class PickAruco(object):
def __init__(self):
rospy.loginfo("Initalizing...")
self.bridge = CvBridge()
self.tfBuffer = tf2_ros.Buffer()
self.tf_l = tf2_ros.TransformListener(self.tfBuffer)
rospy.loginfo("Waiting for /pickup_pose AS...")
self.pick_as = SimpleActionClient('/pickup_pose', PickUpPoseAction)
#self.force = rospy.Subscriber("/wrist_tf", WrenchStamped, self.force_value)
time.sleep(1.0)
if not self.pick_as.wait_for_server(rospy.Duration(20)):
rospy.logerr("Could not connect to /pickup_pose AS")
exit()
rospy.loginfo("Waiting for /place_pose AS...")
self.place_as = SimpleActionClient('/place_pose', PickUpPoseAction)
self.place_as.wait_for_server()
rospy.loginfo("Setting publishers to torso and head controller...")
self.torso_cmd = rospy.Publisher(
'/torso_controller/command', JointTrajectory, queue_size=1)
self.head_cmd = rospy.Publisher(
'/head_controller/command', JointTrajectory, queue_size=1)
self.detected_pose_pub = rospy.Publisher('/detected_aruco_pose',
PoseStamped,
queue_size=1,
latch=True)
rospy.loginfo("Waiting for '/play_motion' AS...")
self.place_finish = rospy.ServiceProxy(
'/god_watcher/place_finish', send_flags)
#self.place_finish = SimpleActionClient('/god_watcher/place_finish', send_flags)
self.play_m_as = SimpleActionClient('/play_motion', PlayMotionAction)
self.speak = rospy.ServiceProxy('/tiago/speech/tts', srvTTS)
self.speak.wait_for_service()
rospy.loginfo("Speak node Connected.")
if not self.play_m_as.wait_for_server(rospy.Duration(20)):
rospy.logerr("Could not connect to /play_motion AS")
exit()
rospy.loginfo("Connected!")
rospy.sleep(1.0)
rospy.loginfo("Done initializing PickAruco.")
def strip_leading_slash(self, s):
return s[1:] if s.startswith("/") else s
def force_value(self, data):
force_x = np.absolute(data.wrench.force.x)
force_y = np.absolute(data.wrench.force.y)
force_z = np.absolute(data.wrench.force.z)
force_abs = force_x + force_y + force_z
rospy.loginfo("I have force_abs")
if force_abs >= 10.0:
rospy.loginfo("I got the bag.")
def tiago_speak(self, string_text):
rospy.loginfo(string_text)
msg_speech = srvTTSRequest()
msg_speech.text = string_text
self.speak.call(msg_speech)
rospy.sleep(0.1)
rospy.loginfo("Done talking.")
def pick_aruco(self, string_operation):
self.prepare_robot()
rospy.sleep(2.0)
rospy.loginfo("spherical_grasp_gui: Waiting for an aruco detection")
#rospy.sleep(5.0)
aruco_pose = rospy.wait_for_message('/object_detection/Pick_Pose', PoseStamped)
aruco_pose.header.frame_id = self.strip_leading_slash(aruco_pose.header.frame_id)
rospy.loginfo("Got: " + str(aruco_pose))
rospy.loginfo("spherical_grasp_gui: Transforming from frame: " +
aruco_pose.header.frame_id + " to 'base_footprint'")
ps = PoseStamped()
ps.pose.position = aruco_pose.pose.position
ps.header.stamp = self.tfBuffer.get_latest_common_time("base_footprint", aruco_pose.header.frame_id)
ps.header.frame_id = aruco_pose.header.frame_id
transform_ok = False
while not transform_ok and not rospy.is_shutdown():
try:
transform = self.tfBuffer.lookup_transform("base_footprint",
ps.header.frame_id,
rospy.Time(0))
aruco_ps = do_transform_pose(ps, transform)
transform_ok = True
except tf2_ros.ExtrapolationException as e:
rospy.logwarn(
"Exception on transforming point... trying again \n(" +
str(e) + ")")
rospy.sleep(0.01)
ps.header.stamp = self.tfBuffer.get_latest_common_time("base_footprint", aruco_pose.header.frame_id)
pick_g = PickUpPoseGoal()
if string_operation == "pick":
rospy.loginfo("Setting cube pose based on bottle detection")
pick_g.object_pose.pose.position.x = aruco_ps.pose.position.x + 0.04
pick_g.object_pose.pose.position.y = aruco_ps.pose.position.y - 0.04
pick_g.object_pose.pose.position.z = aruco_ps.pose.position.z + 0.14
# pick_g.object_pose.pose.orientation = aruco_ps.pose.position
# pick_g.object_pose.pose.position = aruco_ps.pose.position
# pick_g.object_pose.pose.position.z -= 0.1*(1.0/2.0)
rospy.loginfo("aruco pose in base_footprint:" + str(pick_g))
pick_g.object_pose.header.frame_id = 'base_footprint'
pick_g.object_pose.pose.orientation.w = 1.0
self.detected_pose_pub.publish(pick_g.object_pose)
rospy.loginfo("Gonna pick:" + str(pick_g))
self.pick_as.send_goal_and_wait(pick_g)
rospy.loginfo("Done!")
result = self.pick_as.get_result()
if str(moveit_error_dict[result.error_code]) != "SUCCESS":
rospy.logerr("Failed to pick, not trying further")
return
rospy.loginfo("######before")
# Move torso to its maximum height
self.lift_torso()
# Raise arm
#rospy.loginfo("Moving arm to a safe pose")
pmg = PlayMotionGoal()
pmg.motion_name = 'pick_final_pose'
pmg.skip_planning = False
self.play_m_as.send_goal_and_wait(pmg)
data = rospy.wait_for_message('/wrist_ft', WrenchStamped)
force_x = np.absolute(data.wrench.force.x)
force_y = np.absolute(data.wrench.force.y)
force_z = np.absolute(data.wrench.force.z)
force_abs = force_x + force_y + force_z
rospy.loginfo("I have force_abs")
#!/usr/bin/env python
import roslib
roslib.load_manifest('rospy')
roslib.load_manifest('actionlib')
roslib.load_manifest('actionlib_msgs')
roslib.load_manifest('control_msgs')
import rospy
from control_msgs.msg import GripperCommandAction
from control_msgs.msg import GripperCommandGoal
from actionlib import SimpleActionClient
from actionlib_msgs.msg import GoalStatus
if __name__ == "__main__":
rospy.init_node('gripper_test_node', anonymous=True)
name_space = '/r_gripper_controller/gripper_action'
gripper_client = SimpleActionClient(name_space, GripperCommandAction)
gripper_client.wait_for_server()
gripper_goal = GripperCommandGoal()
gripper_goal.command.position = 0.00 #Opens the gripper, use 0.0 to close it.
gripper_goal.command.max_effort = 30.0
gripper_client.send_goal(gripper_goal)
gripper_client.wait_for_result(rospy.Duration(10.0))
if (gripper_client.get_state() != GoalStatus.SUCCEEDED):
rospy.logwarn('Gripper action unsuccessful.')
