def __init__(self):
# Retrieve params:
self._grasp_object_name = rospy.get_param('~grasp_object_name', 'lego_block')
self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.016)
self._arm_group = rospy.get_param('~arm', 'arm_move_group')
self._gripper_group = rospy.get_param('~gripper', 'gripper')
self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.01)
self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.4)
# Create (debugging) publishers:
self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True)
# Create planning scene where we will add the objects etc.
self._scene = PlanningSceneInterface()
# Create robot commander: interface to comand the manipulator programmatically (get the planning_frame for exemple
self._robot = RobotCommander()
rospy.sleep(1.0)
# Clean the scene (remove the old objects:
self._scene.remove_world_object(self._grasp_object_name)
# Add table and Coke can objects to the planning scene:
# TODO get the position of the detected object
self._pose_object_grasp = self._add_object_grasp(self._grasp_object_name)
rospy.sleep(1.0)
# Retrieve groups (arm and gripper):
self._arm = self._robot.get_group(self._arm_group)
self._gripper = self._robot.get_group(self._gripper_group)
# Create grasp generator 'generate' action client:
self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction)
if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Grasp generator action client not available!')
rospy.signal_shutdown('Grasp generator action client not available!')
return
# Create move group 'pickup' action client:
self._pickup_ac = SimpleActionClient('/pickup', PickupAction)
if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Pick up action client not available!')
rospy.signal_shutdown('Pick up action client not available!')
return
# Pick 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')
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()
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
def __init__(self):
self.action_index = 0
self.posture_controller = SimpleActionClient('/wubble_gripper_grasp_action', GraspHandPostureExecutionAction)
self.gripper_controller = SimpleActionClient('/wubble_gripper_action', WubbleGripperAction)
self.ready_arm_client = SimpleActionClient('/ready_arm', ReadyArmAction)
self.collision_map_processing_srv = rospy.ServiceProxy('/tabletop_collision_map_processing/tabletop_collision_map_processing', TabletopCollisionMapProcessing)
self.get_grasp_status_srv = rospy.ServiceProxy('/wubble_grasp_status', GraspStatus)
class ExplorationController:
def __init__(self):
self._plan_service = rospy.ServiceProxy('get_exploration_path', GetRobotTrajectory)
self._move_base = SimpleActionClient('move_base', MoveBaseAction)
def run(self):
r = rospy.Rate(1 / 7.0)
while not rospy.is_shutdown():
self.run_once()
r.sleep()
def run_once(self):
path = self._plan_service().trajectory
poses = path.poses
if not path.poses:
rospy.loginfo('No frontiers left.')
return
rospy.loginfo('Moving to frontier...')
self.move_to_pose(poses[-1])
def move_to_pose(self, pose_stamped, timeout=20.0):
goal = MoveBaseGoal()
goal.target_pose = pose_stamped
self._move_base.send_goal(goal)
self._move_base.wait_for_result(rospy.Duration(timeout))
return self._move_base.get_state() == GoalStatus.SUCCEEDED
def __init__(self):
rospy.loginfo("Initializing ChipBehaviour")
moveit_commander.roscpp_initialize(sys.argv)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
self.group = moveit_commander.MoveGroupCommander("right_arm_torso")
self.pose_pub = rospy.Publisher('/arm_pointing_pose',
geometry_msgs.msg.PoseStamped,
queue_size=1)
self.tts_ac = SimpleActionClient('/tts', TtsAction)
self.tts_ac.wait_for_server()
self.point_head_ac = SimpleActionClient('/head_controller/point_head_action',
PointHeadAction)
self.point_head_ac.wait_for_server(rospy.Duration(10.0))
self.faces_sub = rospy.Subscriber('/pal_face/faces',
FaceDetections,
self.faces_cb,
queue_size=1)
def __init__(self, name):
# stuff for grasp planning
self.tf_listener = tf.TransformListener()
self.tf_broadcaster = tf.TransformBroadcaster()
self.cbbf = ClusterBoundingBoxFinder(self.tf_listener, self.tf_broadcaster, "base_link")
self.last_objects = RecognizedObjectArray()
#rospy.Subscriber("object_array", RecognizedObjectArray, self.objects_callback)
self.sub = rospy.Subscriber("/recognized_object_array", RecognizedObjectArray, self.objects_callback)
self.grasp_publisher = rospy.Publisher("generated_grasps", PoseArray)
rospy.loginfo("Connecting to pickup AS")
self.pickup_ac = SimpleActionClient('/pickup', PickupAction)
#pickup_ac.wait_for_server() # needed?
rospy.loginfo("Connecting to grasp generator AS")
self.grasps_ac = SimpleActionClient('/grasp_generator_server/generate', GenerateBlockGraspsAction)
#grasps_ac.wait_for_server() # needed?
#planning scene for motion planning
self.scene = PlanningSceneInterface()
# blocking action server
self.grasp_obj_as = ActionServer(name, GraspObjectAction, self.goal_callback, self.cancel_callback, False)
self.feedback = GraspObjectFeedback()
self.result = GraspObjectResult()
self.current_goal = None
self.grasp_obj_as.start()
def __init__(self):
self.r_joint_names = ['r_shoulder_pan_joint',
'r_shoulder_lift_joint',
'r_upper_arm_roll_joint',
'r_elbow_flex_joint',
'r_forearm_roll_joint',
'r_wrist_flex_joint',
'r_wrist_roll_joint']
self.l_joint_names = ['l_shoulder_pan_joint',
'l_shoulder_lift_joint',
'l_upper_arm_roll_joint',
'l_elbow_flex_joint',
'l_forearm_roll_joint',
'l_wrist_flex_joint',
'l_wrist_roll_joint']
self.all_joint_names = []
self.all_joint_poses = []
self.lock = threading.Lock()
rospy.Subscriber('joint_states', JointState, self.joint_states_cb)
r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server for RIGHT arm...')
self.r_traj_action_client.wait_for_server()
l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server for LEFT arm...')
self.l_traj_action_client.wait_for_server()
class StationHold(HandlerBase):
alarm_name = 'station-hold'
def __init__(self):
self.move_client = SimpleActionClient('/move_to', MoveAction)
self.change_wrench = rospy.ServiceProxy('/wrench/select', MuxSelect)
self.broadcaster = AlarmBroadcaster(self.alarm_name)
def _client_cb(self, terminal_state, result):
if terminal_state != 3:
rospy.logwarn('Station hold goal failed (Status={}, Result={})'.format(
TerminalState.to_string(terminal_state), result))
return
try:
self.change_wrench('autonomous')
rospy.loginfo('Now station holding')
self.broadcaster.clear_alarm()
except rospy.ServiceException as e:
rospy.logwarn('Station hold changing wrench failed: {}'.format(e))
def raised(self, alarm):
rospy.loginfo("Attempting to station hold")
station_hold_goal = MoveGoal()
station_hold_goal.move_type = 'hold'
self.move_client.send_goal(station_hold_goal, done_cb=self._client_cb)
def cleared(self, alarm):
# When cleared, do nothing and just wait for new goal / custom wrench
pass
def __init__(self):
rospy.loginfo('__init__ start')
# create a node
rospy.init_node(NODE)
# Action server to receive goals
self.path_server = SimpleActionServer('/path_executor/execute_path', ExecutePathAction,
self.handle_path, auto_start=False)
self.path_server.start()
# publishers & clients
self.visualization_publisher = rospy.Publisher('/path_executor/current_path', Path)
# get parameters from launch file
self.use_obstacle_avoidance = rospy.get_param('~use_obstacle_avoidance', True)
# action server based on use_obstacle_avoidance
if self.use_obstacle_avoidance == False:
self.goal_client = SimpleActionClient('/motion_controller/move_to', MoveToAction)
else:
self.goal_client = SimpleActionClient('/bug2/move_to', MoveToAction)
self.goal_client.wait_for_server()
# other fields
self.goal_index = 0
self.executePathGoal = None
self.executePathResult = ExecutePathResult()
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 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 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)
def __init__(self, file):
self.fileOut = file
rospy.init_node("object_swatter", anonymous=True)
self.all_objects = []
self.all_object_names = []
self.current_obj_idx = -1
self.swat_time = rospy.Time.now()
self.model_data = None
self.blockToSwat = ""
self.min_front_dist = 10.0
self.cmd_vel_pub = rospy.Publisher("cmd_vel", Twist)
self.allPoints = []
self.inProgress = False
# rospy.Subscriber('overhead_objects', PoseArray, self.update_object_positions)
rospy.Subscriber("gazebo/model_states", ModelStates, self.read_model)
# self.sh_pan_speed_srv = rospy.ServiceProxy('shoulder_pan_controller/set_speed', SetSpeed)
self.base_client = SimpleActionClient("erratic_base_action", ErraticBaseAction)
self.arm_client = SimpleActionClient("smart_arm_action", SmartArmAction)
self.gripper_client = SimpleActionClient("smart_arm_gripper_action", SmartArmGripperAction)
rospy.Subscriber("tilt_laser/scan", LaserScan, self.filter_scan)
# rospy.wait_for_service('shoulder_pan_controller/set_speed')
self.base_client.wait_for_server()
self.arm_client.wait_for_server()
self.gripper_client.wait_for_server()
rospy.loginfo("Connected to all action servers")
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
def __init__(self):
# Initialize new node
rospy.init_node(NAME)#, anonymous=False)
#initialize the clients to interface with
self.arm_client = SimpleActionClient("smart_arm_action", SmartArmAction)
self.gripper_client = SimpleActionClient("smart_arm_gripper_action", SmartArmGripperAction)
self.move_client = SimpleActionClient("erratic_base_action", ErraticBaseAction)
self.move_client.wait_for_server()
self.arm_client.wait_for_server()
self.gripper_client.wait_for_server()
# Initialize tf listener (remove?)
self.tf = tf.TransformListener()
# Initialize erratic base action server
self.result = SmartArmGraspResult()
self.feedback = SmartArmGraspFeedback()
self.server = SimpleActionServer(NAME, SmartArmGraspAction, self.execute_callback)
#define the offsets
# These offsets were determines expirmentally using the simulator
# They were tested using points stamped with /map
self.xOffset = 0.025
self.yOffset = 0.0
self.zOffset = 0.12 #.05 # this does work!
rospy.loginfo("%s: Pick up Action Server is ready to accept goals", NAME)
rospy.loginfo("%s: Offsets are [%f, %f, %f]", NAME, self.xOffset, self.yOffset, self.zOffset )
def __init__(self):
self.all_objects = []
self.current_obj_idx = -1
self.swat_time = rospy.Time.now()
self.min_dist = 10
self.front_dist = 10
self.avg_front_dist = 10
self.min_front_dist = 10
rospy.Subscriber('overhead_objects', PoseArray, self.update_object_positions)
rospy.Subscriber('tilt_laser/scan', LaserScan, self.filter_scan)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
self.sh_pan_speed_srv = rospy.ServiceProxy('shoulder_pan_controller/set_speed', SetSpeed)
self.classify_obj_srv = rospy.ServiceProxy('classify_object', ClassifyObject)
self.base_client = SimpleActionClient('erratic_base_action', ErraticBaseAction)
self.arm_client = SimpleActionClient('smart_arm_action', SmartArmAction)
self.gripper_client = SimpleActionClient('smart_arm_gripper_action', SmartArmGripperAction)
rospy.wait_for_service('shoulder_pan_controller/set_speed')
rospy.wait_for_service('classify_object')
self.base_client.wait_for_server()
self.arm_client.wait_for_server()
self.gripper_client.wait_for_server()
rospy.loginfo('Connected to all action servers')
def __init__(self, joint_mover):
rospy.loginfo("Waiting for the move_arm actions")
self.move_right_arm_client = SimpleActionClient("move_right_arm", MoveArmAction)
self.move_right_arm_client.wait_for_server()
self.move_left_arm_client = SimpleActionClient("move_left_arm", MoveArmAction)
self.move_left_arm_client.wait_for_server()
rospy.loginfo("waiting for action make_static_collision_map action server")
self.make_static_collision_map_client = actionlib.SimpleActionClient(
'make_static_collision_map',
MakeStaticCollisionMapAction)
self.make_static_collision_map_client.wait_for_server()
rospy.loginfo("waiting for service set_planning_scene_diff")
self.planning_scene_client = rospy.ServiceProxy(
"/environment_server/set_planning_scene_diff",
SetPlanningSceneDiff)
self.planning_scene_client.wait_for_service()
self.tf_listener = tf.TransformListener()
self.right_ik = pr2_control_utilities.IKUtilities("right",
tf_listener=self.tf_listener)
self.left_ik = pr2_control_utilities.IKUtilities("left",
tf_listener=self.tf_listener)
self.joint_mover = joint_mover
rospy.loginfo("%s is ready", self.__class__.__name__)
def __init__(self):
self._goal = ""
self._aISeq = 0
rospy.logdebug("Initializing KeyframeExecutor")
rospy.logdebug("Waiting for set_effector_position")
rospy.wait_for_service('set_effector_position')
self._setEffectorPositionProxy = rospy.ServiceProxy(
"set_effector_position", SetEffectorPosition)
rospy.logdebug("Waiting for set_effector_positions")
rospy.wait_for_service('set_effector_positions')
self._setEffectorPositionsProxy = rospy.ServiceProxy(
"set_effector_positions", SetEffectorPositions)
self._postureClient = SimpleActionClient('body_pose_naoqi',
BodyPoseWithSpeedAction)
rospy.logdebug("Waiting for body_pose_naoqi")
self._postureClient.wait_for_server()
self._client = SimpleActionClient("joint_trajectory",
JointTrajectoryAction)
rospy.logdebug("Waiting for joint_trajectory")
self._client.wait_for_server()
self._functionLookUp = {
"angleInterpolation" : self._angleInterpolation,
"wait" : self._wait,
"setEffectorPosition" : self._setEffectorPosition,
"setEffectorPositions" : self._setEffectorPositions,
"posture" : self._posture
}
def __init__(self):
super(Pr2LookAtFace, self).__init__()
self._client = SimpleActionClient('face_detector_action',FaceDetectorAction)
self._head_client = SimpleActionClient('/head_traj_controller/point_head_action', PointHeadAction)
self._timer = None
self._executing = False
self._pending_face_goal = False
self._pending_head_goal = False
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)
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
def __init__(self, ik, joint_mover):
self.move_right_arm_client = SimpleActionClient("move_right_arm", MoveArmAction)
self.move_right_arm_client.wait_for_server()
self.move_left_arm_client = SimpleActionClient("move_left_arm", MoveArmAction)
self.move_left_arm_client.wait_for_server()
self.ik = ik
self.joint_mover = joint_mover
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 __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...')
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 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 __init__(self, robot_name, side, tf_listener):
self.robot_name = robot_name
self.side = side
if (self.side is "left") or (self.side is "right"):
pass
else:
raise Exception("Side should be either: left or right")
self.tf_listener = tf_listener
self._occupied_by = None
self._operational = True # In simulation, there will be no hardware cb
# Get stuff from the parameter server
self.offset = self.load_param('skills/arm/offset/' + self.side)
self.marker_to_grippoint_offset = self.load_param('skills/arm/offset/marker_to_grippoint')
self.joint_names = self.load_param('skills/arm/joint_names')
self.joint_names = [name + "_" + self.side for name in self.joint_names]
self.torso_joint_names = rospy.get_param('/'+self.robot_name+'/skills/torso/joint_names')
self.default_configurations = self.load_param('skills/arm/default_configurations')
self.default_trajectories = self.load_param('skills/arm/default_trajectories')
# listen to the hardware status to determine if the arm is available
rospy.Subscriber("/amigo/hardware_status", DiagnosticArray, self.cb_hardware_status)
# Init gripper actionlib
self._ac_gripper = SimpleActionClient(
"/" + robot_name + "/" + self.side + "_arm/gripper/action", GripperCommandAction)
# Init graps precompute actionlib
self._ac_grasp_precompute = SimpleActionClient(
"/" + robot_name + "/" + self.side + "_arm/grasp_precompute", GraspPrecomputeAction)
# Init joint trajectory action server
self._ac_joint_traj = SimpleActionClient(
"/" + robot_name + "/body/joint_trajectory_action", FollowJointTrajectoryAction)
# ToDo: don't hardcode? // Comment from @reinzor, this always fails on
# startup of action server, why do we need this, who uses this info?
server_timeout = 0.25
self._ac_gripper_present = self._ac_gripper.wait_for_server(timeout=rospy.Duration(server_timeout))
if not self._ac_gripper_present:
rospy.loginfo("Cannot find gripper {0} server".format(self.side))
self._ac_grasp_precompute_present = self._ac_grasp_precompute.wait_for_server(timeout=rospy.Duration(server_timeout))
if not self._ac_grasp_precompute_present:
rospy.loginfo("Cannot find grasp precompute {0} server".format(self.side))
self._ac_joint_traj_present = self._ac_joint_traj.wait_for_server(timeout=rospy.Duration(server_timeout))
if not self._ac_joint_traj_present:
rospy.loginfo("Cannot find joint trajectory action server {0}".format(self.side))
# Init marker publisher
self._marker_publisher = rospy.Publisher(
"/" + robot_name + "/" + self.side + "_arm/grasp_target",
visualization_msgs.msg.Marker, queue_size=10)
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
def __init__(self):
self.r1 = [-1.2923559122018107, -0.24199198117104131, -1.6400091364915879, -1.5193418228083817, 182.36402145110227, -0.18075144121090148, -5.948327320167482]
self.r2 = [-0.6795931033163289, -0.22651111024292614, -1.748569353944001, -0.7718906399352281, 182.36402145110227, -0.18075144121090148, -5.948327320167482]
self.r3 = [-0.2760036900225221, -0.12322070913238689, -1.5566246267792472, -0.7055856541215724, 182.30397617484758, -1.1580488044879909, -6.249409047256675]
self.l1 = [1.5992829923087575, -0.10337038946934723, 1.5147248511783875, -1.554810647097346, 6.257580605941875, -0.13119498120772766, -107.10011839122919]
self.l2 = [0.8243548398730115, -0.10751554070146568, 1.53941949443935, -0.7765233026995009, 6.257580605941875, -0.13119498120772766, -107.10011839122919]
self.l3 = [0.31464495636226303, -0.06335699084094017, 1.2294536150663598, -0.7714563278010775, 6.730191306327954, -1.1396012223560352, -107.19066045395644]
self.v = [0] * len(self.r1)
self.r_joint_names = ['r_shoulder_pan_joint',
'r_shoulder_lift_joint',
'r_upper_arm_roll_joint',
'r_elbow_flex_joint',
'r_forearm_roll_joint',
'r_wrist_flex_joint',
'r_wrist_roll_joint']
self.l_joint_names = ['l_shoulder_pan_joint',
'l_shoulder_lift_joint',
'l_upper_arm_roll_joint',
'l_elbow_flex_joint',
'l_forearm_roll_joint',
'l_wrist_flex_joint',
'l_wrist_roll_joint']
self._action_name = 'kill'
self._tf = tf.TransformListener()
#initialize base controller
topic_name = '/base_controller/command'
self._base_publisher = rospy.Publisher(topic_name, Twist)
#Initialize the sound controller
self._sound_client = SoundClient()
# Create a trajectory action client
r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
self.r_traj_action_client = SimpleActionClient(r_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server for RIGHT arm...')
self.r_traj_action_client.wait_for_server()
l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
self.l_traj_action_client = SimpleActionClient(l_traj_controller_name, JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory action server for LEFT arm...')
self.l_traj_action_client.wait_for_server()
self.pose = None
self._marker_sub = rospy.Subscriber('catch_me_destination_publisher', AlvarMarker, self.marker_callback)
#initialize this client
self._as = SimpleActionServer(self._action_name, KillAction, execute_cb=self.run, auto_start=False)
self._as.start()
rospy.loginfo('%s: started' % self._action_name)
def __init__(self, name='', topics='', time=0.0):
self.client = SimpleActionClient(OnlineBagger.BAG_TOPIC,
BagOnlineAction)
self.goal = BagOnlineGoal(bag_name=name, topics=topics, bag_time=time)
self.result = None
self.total_it = 0
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()
#! /usr/bin/env python
import rospy
from actionlib import SimpleActionClient
from rmsc_messages.msg import AggressiveGainBuffAction, AggressiveGainBuffActionGoal
def help():
print '''
1 - SendGoal
2 - CancelGoal
3 - Exit
'''
if __name__ == "__main__":
rospy.init_node("aggressive_gain_buff_client")
ac_ = SimpleActionClient("aggressive_gain_buff_action", AggressiveGainBuffAction)
ac_.wait_for_server()
print 'Aggressive Gain Buff Sever Connected!'
help()
while not rospy.is_shutdown():
cmd = raw_input('cmd:')
if cmd == '1':
route_index = input('Input Route Index:')
g = AggressiveGainBuffActionGoal()
g.goal.route_index = route_index
ac_.send_goal(g.goal)
elif cmd == '2':
ac_.cancel_all_goals()
elif cmd == '3':
break
else:
print 'Invalid Command!'
import tf
from actionlib import SimpleActionClient
from rmsc_messages.msg import LookAndMoveAction, LookAndMoveActionGoal
def help():
print '''
1 - SendGoal
2 - CancelGoal
3 - Exit
'''
if __name__ == "__main__":
rospy.init_node("look_n_move_client")
ac_ = SimpleActionClient("look_n_move_node_action", LookAndMoveAction)
ac_.wait_for_server()
print 'sever connected!'
help()
while not rospy.is_shutdown():
cmd = raw_input('cmd:')
if cmd == '1':
x = input('offset_x:')
y = input('offset_y:')
angle = input('offset_angle:')
g = LookAndMoveActionGoal()
q = tf.transformations.quaternion_from_euler(0., 0., angle)
g.goal.relative_pose.pose.position.x = x
g.goal.relative_pose.pose.position.y = y
g.goal.relative_pose.pose.orientation.z = q[2]
g.goal.relative_pose.pose.orientation.w = q[3]
def __init__(self, tf_listener):
'''
Args:
tf_listener (TransformListener)
'''
self._social_gaze = rospy.get_param("/social_gaze")
self._play_sound = rospy.get_param("/play_sound")
self._tf_listener = tf_listener
# arm services
self.move_arm_to_joints_plan_srv = rospy.ServiceProxy(
'move_arm_to_joints_plan', MoveArm)
rospy.wait_for_service('move_arm_to_joints_plan')
self.move_arm_to_joints_srv = rospy.ServiceProxy(
'move_arm_to_joints', MoveArmTraj)
rospy.wait_for_service('move_arm_to_joints')
self.move_arm_to_pose_srv = rospy.ServiceProxy('move_arm_to_pose',
MoveArm)
rospy.wait_for_service('move_arm_to_pose')
self.start_move_arm_to_pose_srv = rospy.ServiceProxy(
'start_move_arm_to_pose', MoveArm)
rospy.wait_for_service('start_move_arm_to_pose')
self.is_reachable_srv = rospy.ServiceProxy('is_reachable', MoveArm)
rospy.wait_for_service('is_reachable')
self.is_arm_moving_srv = rospy.ServiceProxy('is_arm_moving',
GetArmMovement)
rospy.wait_for_service('is_arm_moving')
self.relax_arm_srv = rospy.ServiceProxy('relax_arm', Empty)
rospy.wait_for_service('relax_arm')
self.reset_arm_movement_history_srv = rospy.ServiceProxy(
'reset_arm_movement_history', Empty)
rospy.wait_for_service('reset_arm_movement_history')
self.get_gripper_state_srv = rospy.ServiceProxy(
'get_gripper_state', GetGripperState)
rospy.wait_for_service('get_gripper_state')
self.get_joint_states_srv = rospy.ServiceProxy('get_joint_states',
GetJointStates)
rospy.wait_for_service('get_joint_states')
self.get_ee_pose_srv = rospy.ServiceProxy('get_ee_pose', GetEEPose)
rospy.wait_for_service('get_ee_pose')
self.set_gripper_state_srv = rospy.ServiceProxy(
'set_gripper_state', SetGripperState)
rospy.wait_for_service('set_gripper_state')
rospy.loginfo("Got all arm services")
# head services
self.gaze_client = SimpleActionClient('gaze_action', GazeAction)
self.gaze_client.wait_for_server(rospy.Duration(5))
self.current_gaze_goal_srv = rospy.ServiceProxy(
'get_current_gaze_goal', GetGazeGoal)
rospy.wait_for_service('get_current_gaze_goal')
rospy.loginfo("Got all head services")
# world services
self._get_nearest_object_srv = rospy.ServiceProxy(
'get_nearest_object', GetNearestObject)
rospy.wait_for_service('get_nearest_object')
rospy.loginfo("Got all world services")
# sound stuff
self._sound_client = SoundClient()
laser_client.wait_for_result()
result = laser_client.get_result()
if result.success == False:
print "Action failed"
else:
print "Result: " + str(result.tilt_position)
if __name__ == '__main__':
try:
x = Swatter()
print "Setup Swatter"
rospy.init_node(NAME, anonymous=True)
head_client = SimpleActionClient("wubble_head_action", WubbleHeadAction)
arm_client = SimpleActionClient("smart_arm_action", SmartArmAction)
#arm_cient_angled = SimpleActionClient("", )
gripper_client = SimpleActionClient("smart_arm_gripper_action", SmartArmGripperAction)
laser_client = SimpleActionClient('hokuyo_laser_tilt_action', HokuyoLaserTiltAction)
base_client = SimpleActionClient("erratic_base_action", ErraticBaseAction)
base_client.wait_for_server()
head_client.wait_for_server()
arm_client.wait_for_server()
gripper_client.wait_for_server()
laser_client.wait_for_server()
print "Starting block swatting."
def __init__(self, *a, **k):
super(PathPlanningMixin, self).__init__(*a, **k)
self._path_planner_client = SimpleActionClient('/path_planner', PoseStamped)
def main(argv) -> None:
"""
Main entry point
This design of putting all pub/sub in main function is intentional
since we want to avoid pub/sub being instantiated more than once.
:param argv:
"""
rospy.init_node('waypoint_node')
# Register publishers first
pub_reached = rospy.Publisher("~reached", String,
queue_size=1) # FIXME decide queue_size
# Register subscribers
ds = __DroneStates()
# For middleware
waypoint_topic_name = "~waypoint"
_ = rospy.Subscriber(waypoint_topic_name, PoseStamped, ds.store_waypoint)
# Register actionlib clients
takeoff_topic = rospy.resolve_name("action/takeoff")
takeoff_client = SimpleActionClient(takeoff_topic, TakeoffAction)
landing_topic = rospy.resolve_name("action/landing")
landing_client = SimpleActionClient(landing_topic, LandingAction)
pose_topic = rospy.resolve_name("action/pose")
pose_client = SimpleActionClient(pose_topic, PoseAction)
def action_pose_done_cb(goal_state, goal_result):
rospy.logdebug("Reached\n %s" % str(ds.curr_waypoint.pose.position))
ds.reset_curr_waypoint()
def shutdown() -> None: # TODO Better place for this code
"""Stop the drone when this ROS node shuts down"""
# TODO Safe landing
pass
rospy.on_shutdown(shutdown)
# TODO Wait for hector quadrotor controllers to spawn
rospy.sleep(1)
rate = rospy.Rate(100) # 100Hz TODO Pass sleep rate as a parameter?
is_driving = False
while not rospy.is_shutdown():
rate.sleep()
# Simple controller code for drones # TODO Need better controller
if not is_driving: # IDLE
if ds._waypoints.empty(): # FIXME accessing protected member
pass # Keep idling
else:
ds.set_curr_waypoint()
pose_client.wait_for_server()
pose_goal = PoseGoal(target_pose=ds.target_pose())
rospy.logdebug("Sending pose goal\n %s" % str(pose_goal))
pose_client.send_goal(PoseGoal(target_pose=ds.target_pose()),
done_cb=action_pose_done_cb)
is_driving = True
else: # DRIVING
if ds.reached == ReachedEnum.NO:
pass # Keep driving
else:
if ds.reached == ReachedEnum.YES_AND_REPORT:
pub_reached.publish(ds.report_reached())
is_driving = False
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)
table_pose = copy.deepcopy(object_pose)
#table_pose1 = copy.deepcopy(object_pose)
#table_pose2 = copy.deepcopy(object_pose)
#table_pose3 = copy.deepcopy(object_pose)
#define a virtual table below the object
table_height = 0.24
table_width = 0.20
table_depth = 0.06
table_pose.pose.position.z += -0.30 - table_height / 2
table_pose.pose.position.y += 0.04
#table_height = 0.38
#table_width = 0.30
#table_depth = 0.10
#table_pose.pose.position.z += -0.03 -table_height / 2 - 0.10
#table_pose.pose.position.y += 0.05
#table_height -= 0.02 #remove few milimeters to prevent contact between the object and the table
#table_height1 = 0.80
#table_width1 = 0.40
#table_depth1 = 0.35
#table_pose1.pose.position.z += -(self.object_height) / 2 + table_height1 / 2
#table_pose1.pose.position.y += 0.5
#table_height1 -= 0.008 # remove few milimeters to prevent contact between the object and the table
#table_height2 = 0.80
#table_width2 = 0.40
#table_depth2 = 0.35
#table_pose2.pose.position.z += -(self.object_height) / 2 + table_height2 / 2
#table_pose2.pose.position.y -= 0.5
#table_height2 -= 0.008 # remove few milimeters to prevent contact between the object and the table
#table_height3 = 0.40
#table_width3 = 1.0
#table_depth3 = 0.35
#table_pose3.pose.position.z += -(self.object_height) / 2 + table_height3 / 2 + 0.45
#table_height3 -= 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))
#self.scene.add_box("table1", table_pose1, (table_depth1, table_width1, table_height1)) #add by chen
#self.scene.add_box("table2", table_pose2, (table_depth2, table_width2, table_height2)) #add by chen
#self.scene.add_box("table3", table_pose3, (table_depth3, table_width3, table_height3)) # add by chen
# # We need to wait for the object part to appear
self.wait_for_planning_scene_object()
#########self.wait_for_planning_scene_object("table")
#self.wait_for_planning_scene_object("table1")
#self.wait_for_planning_scene_object("table2")
#self.wait_for_planning_scene_object("table3")
# 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")
rospy.sleep(2.0) # sleep a little while
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("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")
#self.scene.remove_world_object("table1")
#self.scene.remove_world_object("table2")
#self.scene.remove_world_object("table3")
rospy.loginfo("Clearing octomap")
self.clear_octomap_srv.call(EmptyRequest())
rospy.sleep(2.0) # Removing is fast
#rospy.loginfo("Adding new 'part2' object")
rospy.loginfo("Object pose: %s", object_pose.pose)
# Add object description in scene
#self.scene.add_box("part2", object_pose, (self.object_depth, self.object_width, self.object_height))
rospy.loginfo("Second%s", object_pose.pose)
table_pose = copy.deepcopy(object_pose)
# define a virtual table below the object
table_height = object_pose.pose.position.z - self.object_height / 2
table_width = 1.2
table_depth = 1.2
table_pose.pose.position.z += -(
self.object_height) / 2 - table_height / 2
table_height -= 0.02 # 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))
#self.wait_for_planning_scene_object("part2")
####self.wait_for_planning_scene_object("table")
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
class Manager(object):
ALL = "ALL"
def __init__(self, name):
rospy.loginfo("Starting node: %s" % name)
config_file = rospy.get_param("~config_file", '')
if config_file == '':
rospy.logfatal("No config file specified")
raise rospy.ROSException("No config file specified")
return
self.config = rosparam.load_file(config_file)[0][0]
if self.ALL in self.config.keys():
self.all = self.config[self.ALL]["events"]
else:
self.all = []
print self.config
rospy.loginfo("Creating mary client")
self.mary_client = SimpleActionClient("speak", maryttsAction)
self.mary_client.wait_for_server()
rospy.loginfo(" ... done")
rospy.Subscriber("/task_executor/events",
TaskEvent,
callback=self.callback)
def callback(self, msg):
if msg.task.action[1:] in self.config.keys():
events = itertools.chain(
self.all, self.config[msg.task.action[1:]]["events"])
else:
events = self.all
for event in events:
execute = True
if msg.event == eval("TaskEvent." + event.keys()[0]):
if "compare" in event.values()[0].keys():
comps = event.values()[0]["compare"]
for comp in comps:
if not eval(
str(comp["static_value"]) +
comp["comparison"] + "msg." +
comp["task_field"]):
execute = False
break
if "compare_to_topic" in event.values()[0].keys():
comps = event.values()[0]["compare_to_topic"]
for comp in comps:
res = rospy.wait_for_message(
comp["topic"],
roslib.message.get_message_class(
rostopic.get_topic_type(comp["topic"],
True)[0]))
if not eval("res." + comp["field"] +
comp["comparison"] + "msg." +
comp["task_field"]):
execute = False
break
if execute:
self.mary_client.send_goal_and_wait(
maryttsGoal(text=event.values()[0]["text"]))
rospy.loginfo("Saying: " + event.values()[0]["text"])
StateMachine.add('close_goal',
SimpleActionState(topic, MoveBaseAction,
goal_cb=close_goal_cb),
transitions={'succeeded': 'move_base',
'aborted': 'close_goal',
'preempted': 'close_goal'})
return sm
if __name__ == '__main__':
rospy.init_node('move_base_recovery')
topic = rospy.get_param('~topic')
base_frame = rospy.get_param('~base_frame')
move_base = SimpleActionClient(topic, MoveBaseAction)
move_base.wait_for_server()
sm_recovery = create_machine(topic, base_frame)
recovery_server = ActionServerWrapper(
topic + '_recovery', MoveBaseAction, sm_recovery,
['succeeded'], ['aborted'], ['preempted'],
goal_key='goal_message')
recovery_server.run_server()
rospy.spin()
class Manipulator:
def __init__(self):
self.arm = moveit_commander.MoveGroupCommander('arm')
self.hand = moveit_commander.MoveGroupCommander('gripper')
# self.group.set_planning_time(10)
self.arm.set_pose_reference_frame('manip_base_link')
# self.hand.set_planning_time(10)
self.hand.set_pose_reference_frame('manip_base_link')
self.__hand_traj_client = SimpleActionClient(
"/gripper_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(60.0)) is False:
rospy.logfatal(
"Failed to connect to /gripper_controller/follow_joint_trajectory in 4sec."
)
raise Exception(
"Failed to connect to /gripper_controller/follow_joint_trajectory in 4sec."
)
if self.__arm_traj_client.wait_for_server(
timeout=rospy.Duration(60.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.joint_trajectory = rospy.Publisher('/joint_trajectory',
JointTrajectory,
queue_size=10)
self.update_start_state = rospy.Publisher(
'/rviz/moveit/update_start_state', Empty, queue_size=10)
rospy.wait_for_service('/get_planning_scene', 60.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)
rospy.Service('manipulator', setGoal, self.handler)
self.tf = tf.TransformListener()
self.tf.waitForTransform('manip_base_link', 'base', rospy.Time(),
rospy.Duration(1.0))
self.is_moving = False
rospy.loginfo('[manip] Going Home in 5 seconds...')
self.home()
def display_planned_path_callback(self, data):
self.plan = data.trajectory[0].joint_trajectory
def execute(self):
self.joint_trajectory.publish(self.plan)
self.is_moving = True
def feedback_callback(self, data):
self.is_moving = False
if self.update_start_state.get_num_connections() > 0:
self.update_start_state.publish()
def execute_plan(self):
self.arm.plan()
rospy.sleep(2)
if self.plan != None:
self.execute()
while self.is_moving:
pass
rospy.sleep(2)
self.plan = None
return True
else:
return False
def execute_cartesian_plan(self, waypoints):
plan, fraction = self.arm.compute_cartesian_path(waypoints,
0.001,
0,
avoid_collisions=True)
rospy.loginfo('[manip] Cartesian path fraction: %.2f.' % fraction)
rospy.sleep(2)
if self.plan != None and fraction > 0.9:
self.arm.execute(plan, wait=True)
rospy.sleep(5)
self.plan = None
return True
else:
self.plan = None
return False
def advance(self, waypoints, avoid_col=False):
plan, fraction = self.arm.compute_cartesian_path(
waypoints, 0.001, 0, avoid_collisions=avoid_col)
rospy.loginfo('[manip] Cartesian path fraction: %.2f.' % fraction)
rospy.sleep(2)
if self.plan != None and fraction > 0.9:
self.execute()
while self.is_moving:
pass
rospy.sleep(5)
self.plan = None
return True
else:
self.plan = None
return False
def reset_manipulator(self):
self.arm.set_named_target('home')
plan = self.arm.plan()
if not self.arm.execute(plan, wait=True):
return False
self.open_gripper()
return True
def home(self):
self.arm.set_named_target('home')
plan = self.arm.plan()
if not self.arm.execute(plan, wait=True):
return False
return True
def front(self):
self.arm.set_named_target('front')
plan = self.arm.plan()
if not self.arm.execute(plan, wait=True):
return False
return True
def attack(self):
self.arm.set_named_target('attack')
plan = self.arm.plan()
if not self.arm.execute(plan, wait=True):
return False
return True
def open_gripper(self):
self.hand.set_named_target('open_gripper')
plan = self.hand.plan()
if not self.hand.execute(plan, wait=True):
return False
return True
def close_gripper(self):
self.hand.set_named_target('close_gripper')
plan = self.hand.plan()
success = self.hand.execute(plan, wait=True)
return success
def handler(self, request):
type = request.type.lower()
goal = request.goal
pose = Pose()
quaternion = tf.transformations.quaternion_from_euler(
goal.rx, goal.ry, goal.rz)
pose.position.x = goal.x
pose.position.y = goal.y
pose.position.z = goal.z
pose.orientation.x = quaternion[0]
pose.orientation.y = quaternion[1]
pose.orientation.z = quaternion[2]
pose.orientation.w = quaternion[3]
if type == 'reset':
success = self.reset_manipulator()
elif type == 'home':
success = self.home()
elif type == 'open':
success = self.open_gripper()
elif type == 'old_close':
success = self.close_gripper()
elif type == 'front':
success = self.front()
elif type == 'attack':
success = self.attack()
elif type == '':
target_pose = copy.copy(pose)
self.arm.set_pose_target(target_pose)
plan = self.arm.plan()
success = self.arm.execute(plan, wait=True)
if success:
return 'SUCCEEDED'
else:
return 'FAILED'
elif type == 'pick':
target_pose = copy.deepcopy(pose)
self.arm.set_pose_target(target_pose)
plan = self.arm.plan()
success = self.arm.execute(plan, wait=True)
rospy.sleep(10)
self.close_gripper()
rospy.sleep(5)
self.home()
if success:
return 'SUCCEEDED'
else:
return 'FAILED'
elif type == 'place':
target_pose = copy.deepcopy(pose)
self.arm.set_pose_target(target_pose)
plan = self.arm.plan()
success = self.arm.execute(plan, wait=True)
rospy.sleep(5)
self.open_gripper()
rospy.sleep(10)
self.home()
elif type == 'point':
self.close_gripper()
joint_goal = self.arm.get_current_joint_values()
joint_goal[0] = 0.35
joint_goal[1] = 0.0
joint_goal[2] = 0.0
joint_goal[3] = -2.35
joint_goal[4] = 0.0
joint_goal[5] = -0.35
self.arm.set_joint_value_target(joint_goal)
plan = self.arm.plan()
success = self.arm.execute(plan, wait=True)
if success:
return 'SUCCEEDED'
else:
return 'FAILED'
def test_grasp_action(self):
"""Test sending a grasp."""
goal = GraspGoal()
goal.grasp = self.mk_grasp({
'LFJ3': 1.4, 'RFJ3': 1.4, 'MFJ3': 1.4, 'FFJ3': 1.4,
'LFJ0': 3.0, 'RFJ0': 3.0, 'MFJ0': 3.0, 'FFJ0': 3.0,
})
# Get a action client
client = SimpleActionClient('grasp', GraspAction)
client.wait_for_server()
# Send pre-grasp
goal.pre_grasp = True
client.send_goal(goal)
client.wait_for_result(rospy.Duration.from_sec(20.0))
self.assertEqual(client.get_state(), GoalStatus.SUCCEEDED,
"Action did not return in SUCCEEDED state.")
rospy.sleep(2)
# Send grasp
goal.pre_grasp = False
client.send_goal(goal)
client.wait_for_result(rospy.Duration.from_sec(20.0))
self.assertEqual(client.get_state(), GoalStatus.SUCCEEDED,
"Action did not return in SUCCEEDED state.")
class Arm(object):
CART_SERVER = '/arm_controller/move_arm_cart'
def __init__(self, default_pitch=(math.pi / 2.0)):
rospy.loginfo('Initializing arm control.')
rospy.loginfo('Waiting for [%s] server...' % (self.CART_SERVER))
self.move_arm_cart_server = SimpleActionClient(self.CART_SERVER,
MoveArmAction)
self.move_arm_cart_server.wait_for_server()
self.script_server = simple_script_server()
self.pitch = default_pitch
def move_to(self, where):
if isinstance(where, str):
self._move_to_pose(where)
elif len(where) == 5:
self._move_to_joints(where)
elif len(where) == 4 or len(where) == 3:
self._move_to_cartesian(where)
def _move_to_joints(self, joints):
self.script_server.move('arm', [joints])
def _move_to_cartesian(self, coordinates):
"""
Move the arm to the pose given by (x, y, z, pitch) tuple. The pitch is
optional and can be omitted.
"""
g = MoveArmGoal()
pc = PositionConstraint()
pc.header.frame_id = "/base_link"
pc.header.stamp = rospy.Time.now()
pc.position.x = coordinates[0]
pc.position.y = coordinates[1]
pc.position.z = coordinates[2]
g.motion_plan_request.goal_constraints.position_constraints.append(pc)
oc = OrientationConstraint()
r = 0.0
p = coordinates[3] if len(coordinates) == 4 else self.pitch
y = 0.0
(qx, qy, qz, qw) = tf.transformations.quaternion_from_euler(r, p, y)
oc.header.frame_id = "/base_link"
oc.header.stamp = rospy.Time.now()
oc.orientation.x = qx
oc.orientation.y = qy
oc.orientation.z = qz
oc.orientation.w = qw
g.motion_plan_request.goal_constraints.orientation_constraints.append(
oc)
self.move_arm_cart_server.send_goal(g)
rospy.loginfo('Sent move arm goal, waiting for result...')
self.move_arm_cart_server.wait_for_result()
rv = self.move_arm_cart_server.get_result().error_code.val
print rv
if not rv == 1:
raise Exception('Failed to move the arm to the given pose.')
def _move_to_pose(self, pose):
self.script_server.move('arm', pose)
def open_gripper(self):
self.script_server.move('gripper', 'open')
def close_gripper(self):
self.script_server.move('gripper', 'close')
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 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')
# Subscribe to topics
# Wait for service providers
rospy.wait_for_service(self.mv_head_srv_nm, timeout=30)
# Instantiate publishers
self.cmd_vel_pub = rospy.Publisher(self.cmd_vel_top, 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)
# 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: Move the robot "manually" to door
if self.state == 0:
move_msg = Twist()
move_msg.linear.x = 1
rate = rospy.Rate(10)
converged = False
cnt = 0
rospy.loginfo("%s: Moving towards door", self.node_name)
while not rospy.is_shutdown() and cnt < 25:
self.cmd_vel_pub.publish(move_msg)
rate.sleep()
cnt = cnt + 1
self.state = 1
rospy.sleep(1)
# State 1: Tuck arm
if self.state == 1:
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 tuck: ", self.play_motion_ac.get_result())
self.state = 2
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 2: Move the robot "manually" to chair
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 < 5:
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 < 15:
self.cmd_vel_pub.publish(move_msg)
rate.sleep()
cnt = cnt + 1
self.state = 3
rospy.sleep(1)
# 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
rospy.loginfo("%s: State machine finished!", self.node_name)
return
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 Robot:
'''Robot class that provides an interface to arm
and head (maybe base later)'''
def __init__(self, tf_listener):
'''
Args:
tf_listener (TransformListener)
'''
self._social_gaze = rospy.get_param("/social_gaze")
self._play_sound = rospy.get_param("/play_sound")
self._tf_listener = tf_listener
# arm services
self.move_arm_to_joints_plan_srv = rospy.ServiceProxy(
'move_arm_to_joints_plan', MoveArm)
rospy.wait_for_service('move_arm_to_joints_plan')
self.move_arm_to_joints_srv = rospy.ServiceProxy(
'move_arm_to_joints', MoveArmTraj)
rospy.wait_for_service('move_arm_to_joints')
self.move_arm_to_pose_srv = rospy.ServiceProxy('move_arm_to_pose',
MoveArm)
rospy.wait_for_service('move_arm_to_pose')
self.start_move_arm_to_pose_srv = rospy.ServiceProxy(
'start_move_arm_to_pose', MoveArm)
rospy.wait_for_service('start_move_arm_to_pose')
self.is_reachable_srv = rospy.ServiceProxy('is_reachable', MoveArm)
rospy.wait_for_service('is_reachable')
self.is_arm_moving_srv = rospy.ServiceProxy('is_arm_moving',
GetArmMovement)
rospy.wait_for_service('is_arm_moving')
self.relax_arm_srv = rospy.ServiceProxy('relax_arm', Empty)
rospy.wait_for_service('relax_arm')
self.reset_arm_movement_history_srv = rospy.ServiceProxy(
'reset_arm_movement_history', Empty)
rospy.wait_for_service('reset_arm_movement_history')
self.get_gripper_state_srv = rospy.ServiceProxy(
'get_gripper_state', GetGripperState)
rospy.wait_for_service('get_gripper_state')
self.get_joint_states_srv = rospy.ServiceProxy('get_joint_states',
GetJointStates)
rospy.wait_for_service('get_joint_states')
self.get_ee_pose_srv = rospy.ServiceProxy('get_ee_pose', GetEEPose)
rospy.wait_for_service('get_ee_pose')
self.set_gripper_state_srv = rospy.ServiceProxy(
'set_gripper_state', SetGripperState)
rospy.wait_for_service('set_gripper_state')
rospy.loginfo("Got all arm services")
# head services
self.gaze_client = SimpleActionClient('gaze_action', GazeAction)
self.gaze_client.wait_for_server(rospy.Duration(5))
self.current_gaze_goal_srv = rospy.ServiceProxy(
'get_current_gaze_goal', GetGazeGoal)
rospy.wait_for_service('get_current_gaze_goal')
rospy.loginfo("Got all head services")
# world services
self._get_nearest_object_srv = rospy.ServiceProxy(
'get_nearest_object', GetNearestObject)
rospy.wait_for_service('get_nearest_object')
rospy.loginfo("Got all world services")
# sound stuff
self._sound_client = SoundClient()
# arm stuff
def move_arm_to_pose(self, arm_state):
'''Move robot's arm to pose
Args:
arm_state (ArmState) : contains pose info
Return:
bool : success
'''
try:
result = self.move_arm_to_pose_srv(arm_state).success
except Exception, e:
result = False
return result
class NearestWLANFinderClient(object):
"""A test for osm_bridge_ros.py 's nearest_wlan_finder action"""
def __init__(self):
rospy.loginfo("inside __init__ of NearestWLANFinderClient")
SERVER = "/nearest_wlan"
self.client = SimpleActionClient(SERVER, NearestWLANAction)
self.client.wait_for_server()
rospy.loginfo("Connected to server")
req = NearestWLANGoal(x=0.0,
y=5.0,
is_x_y_provided=True,
floor='BRSU_L0')
self.client.send_goal(req, done_cb=self.done_cb)
self.client.wait_for_result()
req = NearestWLANGoal(x=0.0, y=5.0, is_x_y_provided=True, floor='127')
self.client.send_goal(req, done_cb=self.done_cb)
self.client.wait_for_result()
req = NearestWLANGoal(is_x_y_provided=False, area='BRSU_A_L0_A1')
self.client.send_goal(req, done_cb=self.done_cb)
self.client.wait_for_result()
req = NearestWLANGoal(is_x_y_provided=False, area='125')
self.client.send_goal(req, done_cb=self.done_cb)
self.client.wait_for_result()
req = NearestWLANGoal(is_x_y_provided=False,
local_area='BRSU_A_L0_A1_LA1')
self.client.send_goal(req, done_cb=self.done_cb)
self.client.wait_for_result()
req = NearestWLANGoal(is_x_y_provided=False, local_area='120')
self.client.send_goal(req, done_cb=self.done_cb)
self.client.wait_for_result()
rospy.signal_shutdown("Test complete")
def done_cb(self, status, result):
# rospy.loginfo(status)
# rospy.loginfo(result)
try:
assert (result.point.id == 2658)
rospy.loginfo("Test passed")
except Exception as e:
rospy.logerr("Test failed")
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 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, -1.0]
jtp.time_from_start = rospy.Duration(2.0)
jt.points.append(jtp)
self.head_cmd.publish(jt)
rospy.loginfo("Done.")
def lower_torso(self):
rospy.loginfo("Moving torso down")
jt = JointTrajectory()
jt.joint_names = ['torso_lift_joint']
jtp = JointTrajectoryPoint()
jtp.positions = [0.00]
jtp.time_from_start = rospy.Duration(2.5)
jt.points.append(jtp)
self.torso_cmd.publish(jt)
def pick_aruco(self, aruco_pose):
# rospy.loginfo("spherical_grasp_gui: Waiting for an aruco detection")
# aruco_pose = PoseStamped()
# aruco_pose.header.frame_id = "base_footprint"
# aruco_pose.pose.position.x = 0.492726171711
# aruco_pose.pose.position.y = -0.140456914268
# aruco_pose.pose.position.z = 0.133627714861
# aruco_pose.pose.orientation.x = 0.544020678534
# aruco_pose.pose.orientation.y = 0.507215268602
# aruco_pose.pose.orientation.z = 0.500950386556
# aruco_pose.pose.orientation.w = 0.442518793765
# aruco_pose = rospy.wait_for_message('/aruco_single/pose', PoseStamped)
aruco_pose.header.frame_id = self.strip_leading_slash(
aruco_pose.header.frame_id)
rospy.loginfo("Got: " + str(aruco_pose))
self.prepare_robot()
rospy.sleep(2.0)
# rospy.loginfo("spherical_grasp_gui: Transforming from frame: {} to 'base_footprint'".format(aruco_pose.header.frame_id))
# 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()
pick_g = PickUpPoseGoal()
aruco_ps = aruco_pose
# 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!")
result = self.pick_as.get_result()
if str(moveit_error_dict[result.error_code]) != "SUCCESS":
rospy.logerr(
"Failed to pick, not trying further - error code was {}".
format(str(moveit_error_dict[result.error_code])))
# return
# move to final position
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.")
# Move torso to its maximum height
# self.lift_torso()
#self.lower_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 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 unfolding arm.")
self.lower_torso()
self.lower_head()
rospy.loginfo("Robot prepared.")
client.send_goal(goal)
client.wait_for_result()
if client.get_state() == GoalStatus.SUCCEEDED:
result = client.get_result()
print "Result: " + str(result.base_position)
elif client.get_state() == GoalStatus.PREEMPTED:
print "Action pre-empted"
else:
print "Action failed"
if __name__ == '__main__':
try:
rospy.init_node(NAME, anonymous=True)
client = SimpleActionClient("erratic_base_action", ErraticBaseAction)
client.wait_for_server()
print "Go exactly 1 meter forward & 1 meter to the right..."
position = Point(x=1.0, y=-1.0)
orientation = Quaternion(w=1.0)
move_to('/base_footprint', position,
orientation) # No vicinity specified
rospy.sleep(2.0)
print "Go to rocky sphere..."
position = Point(x=-2.6, y=1.7)
orientation = Quaternion(w=1.0)
move_to('/map', position, orientation, 0.9)
rospy.sleep(2.0)
class CokeCanPickAndPlace:
def __init__(self):
# Retrieve params:
self._table_object_name = rospy.get_param('~table_object_name',
'table')
self._grasp_object_name = rospy.get_param('~grasp_object_name',
'coke_can')
self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.06)
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.6)
self._approach_retreat_min_dist = rospy.get_param(
'~approach_retreat_min_dist', 0.4)
# Create (debugging) publishers:
self._grasps_pub = rospy.Publisher('grasps',
PoseArray,
queue_size=1,
latch=True)
self._places_pub = rospy.Publisher('places',
PoseArray,
queue_size=1,
latch=True)
# Create planning scene and robot commander:
self._scene = PlanningSceneInterface()
self._robot = RobotCommander()
rospy.sleep(1.0)
# Clean the scene:
self._scene.remove_world_object(self._table_object_name)
self._scene.remove_world_object(self._grasp_object_name)
# Add table and Coke can objects to the planning scene:
self._pose_table = self._add_table(self._table_object_name)
self._pose_coke_can = self._add_coke_can(self._grasp_object_name)
rospy.sleep(1.0)
# Define target place pose:
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.5
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))
rospy.loginfo('x = %f, y = %f, z = %f', self._pose_place.position.x,
self._pose_place.position.y, self._pose_place.position.z)
# Retrieve groups (arm and gripper):
self._arm = self._robot.get_group(self._arm_group)
#self._gripper = self._robot.get_group(self._gripper_group)
# Create grasp generator 'generate' action client:
self._grasps_ac = SimpleActionClient(
'/moveit_simple_grasps_server/generate', GenerateGraspsAction)
if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Grasp generator action client not available!')
rospy.signal_shutdown(
'Grasp generator action client not available!')
return
# Create move group 'pickup' action client:
self._pickup_ac = SimpleActionClient('/pickup', PickupAction)
if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Pick up action client not available!')
rospy.signal_shutdown('Pick up action client not available!')
return
# Create move group 'place' action client:
self._place_ac = SimpleActionClient('/place', PlaceAction)
if not self._place_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Place action client not available!')
rospy.signal_shutdown('Place action client not available!')
return
# Pick Coke can object:
while not self._pickup(self._arm_group, self._grasp_object_name,
self._grasp_object_width):
rospy.logwarn('Pick up failed! Retrying ...')
rospy.sleep(1.0)
rospy.loginfo('Pick up successfully')
# Place Coke can object on another place on the support surface (table):
while not self._place(self._arm_group, self._grasp_object_name,
self._pose_place):
rospy.logwarn('Place failed! Retrying ...')
rospy.sleep(1.0)
rospy.loginfo('Place successfully')
def __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 = 1.0
p.pose.position.y = 0.0
p.pose.position.z = 1.0
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.5, 0.8, 0.03))
return p.pose
def _add_coke_can(self, name):
p = PoseStamped()
p.header.frame_id = self._robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.75 - 0.01
p.pose.position.y = 0.25 - 0.01
p.pose.position.z = 1.00 + (0.3 + 0.03) / 2.0
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.15, 0.15, 0.3))
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 = 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)
# 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 = -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 = 1
# 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 = 5.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 = 10
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 = 5.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 = 10
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)
#! /usr/bin/env python
import sys
import rospy
from actionlib import SimpleActionClient
from actionlib_msgs.msg import GoalStatus
from mir_planning_msgs.msg import GenericExecuteAction, GenericExecuteGoal
from diagnostic_msgs.msg import KeyValue
if __name__ == '__main__':
rospy.init_node('pick_object_client_tester')
client = SimpleActionClient('wbc_pick_object_server', GenericExecuteAction)
client.wait_for_server()
if len(sys.argv) == 3:
goal = GenericExecuteGoal()
obj = str(sys.argv[1]).upper()
location = str(sys.argv[2]).upper()
goal.parameters.append(KeyValue(key='object', value=obj))
goal.parameters.append(KeyValue(key='location', value=location))
rospy.loginfo('Sending following goal to place object server')
rospy.loginfo(goal)
client.send_goal(goal)
timeout = 15.0
finished_within_time = client.wait_for_result(
rospy.Duration.from_sec(int(timeout)))
if not finished_within_time:
def __init__(self):
# Retrieve params:
self._table_object_name = rospy.get_param('~table_object_name',
'table')
self._grasp_object_name = rospy.get_param('~grasp_object_name',
'coke_can')
self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.06)
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.6)
self._approach_retreat_min_dist = rospy.get_param(
'~approach_retreat_min_dist', 0.4)
# Create (debugging) publishers:
self._grasps_pub = rospy.Publisher('grasps',
PoseArray,
queue_size=1,
latch=True)
self._places_pub = rospy.Publisher('places',
PoseArray,
queue_size=1,
latch=True)
# Create planning scene and robot commander:
self._scene = PlanningSceneInterface()
self._robot = RobotCommander()
rospy.sleep(1.0)
# Clean the scene:
self._scene.remove_world_object(self._table_object_name)
self._scene.remove_world_object(self._grasp_object_name)
# Add table and Coke can objects to the planning scene:
self._pose_table = self._add_table(self._table_object_name)
self._pose_coke_can = self._add_coke_can(self._grasp_object_name)
rospy.sleep(1.0)
# Define target place pose:
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.5
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))
rospy.loginfo('x = %f, y = %f, z = %f', self._pose_place.position.x,
self._pose_place.position.y, self._pose_place.position.z)
# Retrieve groups (arm and gripper):
self._arm = self._robot.get_group(self._arm_group)
#self._gripper = self._robot.get_group(self._gripper_group)
# Create grasp generator 'generate' action client:
self._grasps_ac = SimpleActionClient(
'/moveit_simple_grasps_server/generate', GenerateGraspsAction)
if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Grasp generator action client not available!')
rospy.signal_shutdown(
'Grasp generator action client not available!')
return
# Create move group 'pickup' action client:
self._pickup_ac = SimpleActionClient('/pickup', PickupAction)
if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Pick up action client not available!')
rospy.signal_shutdown('Pick up action client not available!')
return
# Create move group 'place' action client:
self._place_ac = SimpleActionClient('/place', PlaceAction)
if not self._place_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Place action client not available!')
rospy.signal_shutdown('Place action client not available!')
return
# Pick Coke can object:
while not self._pickup(self._arm_group, self._grasp_object_name,
self._grasp_object_width):
rospy.logwarn('Pick up failed! Retrying ...')
rospy.sleep(1.0)
rospy.loginfo('Pick up successfully')
# Place Coke can object on another place on the support surface (table):
while not self._place(self._arm_group, self._grasp_object_name,
self._pose_place):
rospy.logwarn('Place failed! Retrying ...')
rospy.sleep(1.0)
rospy.loginfo('Place successfully')
class PathExecutor:
_feedback = ExecutePathFeedback()
_result = ExecutePathResult()
def __init__(self, name):
# self._goal_publisher = Simple
# self.pe_client_for_bug2= SimpleActionClient('/bug2/move_to',MoveToAction)
# self.pe_client_for_bug2.wait_for_server()
print "----------------- in INIT ---------------------------------------"
rospy.loginfo("Creating an action server")
# Creating a client to perform the bug2 function
self._move_to_client = SimpleActionClient('/bug2/move_to',
MoveToAction)
# Creating an action server
self._as = SimpleActionServer('/path_executor/execute_path', ExecutePathAction, \
execute_cb=self.execute_cb, auto_start=False)
# Starting the server
self._as.start()
# Using a flag to aid preemption
self.flag = True
# Using a flag to detect the completion of 3 goals
self.success = True
def execute_cb(self, goal):
# Executed every time a goal is sent by the client
self._move_to_client.wait_for_server()
# printing the goals in the terminal
rospy.loginfo(goal.path.poses)
# Creating an instance of MoveToGoal to compose a goal message
self.move = MoveToGoal()
# Iterating through the goals and sending it to the move_to_client
for i in range(len(goal.path.poses)):
if i == 2:
self.success = True
else:
self.success = False
self._pose = goal.path.poses[i]
self.move.target_pose = self._pose
self._move_to_client.send_goal(self.move,
done_cb=self.move_to_done_cb)
self._move_to_client.wait_for_result()
# self.success = True
rospy.spin()
# setting the server's preempted() function if it's requested
while flag:
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._as.set_preempted()
break
def move_to_done_cb(self, state, result):
if (state == 3):
#
self._result.visited.append(True)
self._feedback.reached = True
self._feedback.pose = self._pose
elif (state == 4):
self._feedback.reached = False
#
rospy.loginfo(self._result)
self._as.publish_feedback(self._feedback)
if self.success == True:
print "All goals are considered"
self._as.set_succeeded(self._result)
def __init__(self):
rospy.wait_for_service('get_exploration_path', timeout=2)
self._plan_service = rospy.ServiceProxy('get_exploration_path',
GetRobotTrajectory)
self._move_base = SimpleActionClient('move_base', MoveBaseAction)
class GiskardWrapper(object):
def __init__(self, giskard_topic=u'giskardpy/command', ns=u'giskard'):
if giskard_topic is not None:
self.client = SimpleActionClient(giskard_topic, MoveAction)
self.update_world = rospy.ServiceProxy(u'{}/update_world'.format(ns), UpdateWorld)
self.marker_pub = rospy.Publisher(u'visualization_marker_array', MarkerArray, queue_size=10)
rospy.wait_for_service(u'{}/update_world'.format(ns))
self.client.wait_for_server()
self.tip_to_root = {}
self.robot_urdf = URDFObject(rospy.get_param(u'robot_description'))
self.collisions = []
self.clear_cmds()
self.object_js_topics = {}
rospy.sleep(.3)
def get_robot_name(self):
return self.robot_urdf.get_name()
def get_root(self):
return self.robot_urdf.get_root()
def set_cart_goal(self, root, tip, pose_stamped, trans_max_speed=None, rot_max_speed=None):
"""
:param tip:
:type tip: str
:param pose_stamped:
:type pose_stamped: PoseStamped
"""
self.set_translation_goal(root, tip, pose_stamped, max_speed=trans_max_speed)
self.set_rotation_goal(root, tip, pose_stamped, max_speed=rot_max_speed)
def set_translation_goal(self, root, tip, pose_stamped, weight=None, gain=None, max_speed=None):
"""
:param tip:
:type tip: str
:param pose_stamped:
:type pose_stamped: PoseStamped
"""
if not gain and not max_speed and not weight:
constraint = CartesianConstraint()
constraint.type = CartesianConstraint.TRANSLATION_3D
constraint.root_link = str(root)
constraint.tip_link = str(tip)
constraint.goal = pose_stamped
self.cmd_seq[-1].cartesian_constraints.append(constraint)
else:
constraint = Constraint()
constraint.type = u'CartesianPosition'
params = {}
params[u'root_link'] = root
params[u'tip_link'] = tip
params[u'goal'] = convert_ros_message_to_dictionary(pose_stamped)
if gain:
params[u'gain'] = gain
if max_speed:
params[u'max_speed'] = max_speed
if weight:
params[u'weight'] = weight
constraint.parameter_value_pair = json.dumps(params)
self.cmd_seq[-1].constraints.append(constraint)
def set_rotation_goal(self, root, tip, pose_stamped, weight=None, gain=None, max_speed=None):
"""
:param tip:
:type tip: str
:param pose_stamped:
:type pose_stamped: PoseStamped
"""
if not gain and not max_speed and not weight:
constraint = CartesianConstraint()
constraint.type = CartesianConstraint.ROTATION_3D
constraint.root_link = str(root)
constraint.tip_link = str(tip)
constraint.goal = pose_stamped
self.cmd_seq[-1].cartesian_constraints.append(constraint)
else:
constraint = Constraint()
constraint.type = u'CartesianOrientationSlerp'
params = {}
params[u'root_link'] = root
params[u'tip_link'] = tip
params[u'goal'] = convert_ros_message_to_dictionary(pose_stamped)
if gain:
params[u'gain'] = gain
if max_speed:
params[u'max_speed'] = max_speed
if weight:
params[u'weight'] = weight
constraint.parameter_value_pair = json.dumps(params)
self.cmd_seq[-1].constraints.append(constraint)
def set_joint_goal(self, joint_state, weight=None, gain=None, max_speed=None):
"""
:param joint_state:
:type joint_state: dict
"""
if not weight and not gain and not max_speed:
constraint = JointConstraint()
constraint.type = JointConstraint.JOINT
if isinstance(joint_state, dict):
for joint_name, joint_position in joint_state.items():
constraint.goal_state.name.append(joint_name)
constraint.goal_state.position.append(joint_position)
elif isinstance(joint_state, JointState):
constraint.goal_state = joint_state
self.cmd_seq[-1].joint_constraints.append(constraint)
elif isinstance(joint_state, dict):
for joint_name, joint_position in joint_state.items():
constraint = Constraint()
constraint.type = u'JointPosition'
params = {}
params[u'joint_name'] = joint_name
params[u'goal'] = joint_position
if weight:
params[u'weight'] = weight
if gain:
params[u'gain'] = gain
if max_speed:
params[u'max_speed'] = max_speed
constraint.parameter_value_pair = json.dumps(params)
self.cmd_seq[-1].constraints.append(constraint)
def align_planes(self, tip, tip_normal, root=None, root_normal=None):
"""
:type tip: str
:type tip_normal: Vector3Stamped
:type root: str
:type root_normal: Vector3Stamped
:return:
"""
root = root if root else self.get_root()
tip_normal = convert_ros_message_to_dictionary(tip_normal)
if not root_normal:
root_normal = Vector3Stamped()
root_normal.header.frame_id = self.get_root()
root_normal.vector.z = 1
root_normal = convert_ros_message_to_dictionary(root_normal)
self.set_json_goal(u'AlignPlanes', tip=tip, tip_normal=tip_normal, root=root, root_normal=root_normal)
def gravity_controlled_joint(self, joint_name, object_name):
self.set_json_goal(u'GravityJoint', joint_name=joint_name, object_name=object_name)
def update_god_map(self, updates):
self.set_json_goal(u'UpdateGodMap', updates=updates)
def update_yaml(self, updates):
self.set_json_goal(u'UpdateYaml', updates=updates)
def pointing(self, tip, goal_point, root=None, pointing_axis=None, weight=None):
kwargs = {u'tip':tip,
u'goal_point':goal_point}
if root is not None:
kwargs[u'root'] = root
if pointing_axis is not None:
kwargs[u'pointing_axis'] = convert_ros_message_to_dictionary(pointing_axis)
if weight is not None:
kwargs[u'weight'] = weight
kwargs[u'goal_point'] = convert_ros_message_to_dictionary(goal_point)
self.set_json_goal(u'Pointing', **kwargs)
def set_json_goal(self, constraint_type, **kwargs):
constraint = Constraint()
constraint.type = constraint_type
for k, v in kwargs.items():
if isinstance(v, Message):
kwargs[k] = convert_ros_message_to_dictionary(v)
constraint.parameter_value_pair = json.dumps(kwargs)
self.cmd_seq[-1].constraints.append(constraint)
def set_collision_entries(self, collisions):
self.cmd_seq[-1].collisions.extend(collisions)
def allow_collision(self, robot_links=(CollisionEntry.ALL,), body_b=CollisionEntry.ALL,
link_bs=(CollisionEntry.ALL,)):
"""
:param robot_links: list of robot link names as str, None or empty list means all
:type robot_links: list
:param body_b: name of the other body, use the robots name to modify self collision behavior, empty string means all bodies
:type body_b: str
:param link_bs: list of link name of body_b, None or empty list means all
:type link_bs: list
"""
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.ALLOW_COLLISION
collision_entry.robot_links = [str(x) for x in robot_links]
collision_entry.body_b = str(body_b)
collision_entry.link_bs = [str(x) for x in link_bs]
self.set_collision_entries([collision_entry])
def avoid_collision(self, min_dist, robot_links=(CollisionEntry.ALL,), body_b=CollisionEntry.ALL,
link_bs=(CollisionEntry.ALL,)):
"""
:param min_dist: the distance giskard is trying to keep between specified links
:type min_dist: float
:param robot_links: list of robot link names as str, None or empty list means all
:type robot_links: list
:param body_b: name of the other body, use the robots name to modify self collision behavior, empty string means all bodies
:type body_b: str
:param link_bs: list of link name of body_b, None or empty list means all
:type link_bs: list
"""
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.AVOID_COLLISION
collision_entry.min_dist = min_dist
collision_entry.robot_links = [str(x) for x in robot_links]
collision_entry.body_b = str(body_b)
collision_entry.link_bs = [str(x) for x in link_bs]
self.set_collision_entries([collision_entry])
def allow_all_collisions(self):
"""
Allows all collisions for next goal.
"""
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.ALLOW_COLLISION
collision_entry.robot_links = [CollisionEntry.ALL]
collision_entry.body_b = CollisionEntry.ALL
collision_entry.link_bs = [CollisionEntry.ALL]
self.set_collision_entries([collision_entry])
def allow_self_collision(self):
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.ALLOW_COLLISION
collision_entry.robot_links = [CollisionEntry.ALL]
collision_entry.body_b = self.get_robot_name()
collision_entry.link_bs = [CollisionEntry.ALL]
self.set_collision_entries([collision_entry])
def set_self_collision_distance(self, min_dist=0.05):
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.AVOID_COLLISION
collision_entry.robot_links = [CollisionEntry.ALL]
collision_entry.body_b = self.get_robot_name()
collision_entry.link_bs = [CollisionEntry.ALL]
collision_entry.min_dist = min_dist
self.set_collision_entries([collision_entry])
def avoid_all_collisions(self, distance=0.05):
"""
Avoids all collisions for next goal.
:param distance: the distance that giskard is trying to keep from all objects
:type distance: float
"""
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.AVOID_COLLISION
collision_entry.robot_links = [CollisionEntry.ALL]
collision_entry.body_b = CollisionEntry.ALL
collision_entry.link_bs = [CollisionEntry.ALL]
collision_entry.min_dist = distance
self.set_collision_entries([collision_entry])
def add_cmd(self, max_trajectory_length=20):
move_cmd = MoveCmd()
# move_cmd.max_trajectory_length = max_trajectory_length
self.cmd_seq.append(move_cmd)
def clear_cmds(self):
"""
Removes all move commands from the current goal, collision entries are left untouched.
"""
self.cmd_seq = []
self.add_cmd()
def plan_and_execute(self, wait=True):
"""
:param wait: this function block if wait=True
:type wait: bool
:return: result from giskard
:rtype: MoveResult
"""
goal = self._get_goal()
if wait:
self.client.send_goal_and_wait(goal)
return self.client.get_result()
else:
self.client.send_goal(goal)
def plan(self, wait=True):
"""
:param wait: this function block if wait=True
:type wait: bool
:return: result from giskard
:rtype: MoveResult
"""
goal = self._get_goal()
goal.type = MoveGoal.PLAN_ONLY
if wait:
self.client.send_goal_and_wait(goal)
return self.client.get_result()
else:
self.client.send_goal(goal)
def get_collision_entries(self):
return self.cmd_seq
def _get_goal(self):
goal = MoveGoal()
goal.cmd_seq = self.cmd_seq
goal.type = MoveGoal.PLAN_AND_EXECUTE
self.clear_cmds()
return goal
def interrupt(self):
self.client.cancel_goal()
def get_result(self, timeout=rospy.Duration()):
"""
Waits for giskardpy result and returns it. Only used when plan_and_execute was called with wait=False
:type timeout: rospy.Duration
:rtype: MoveResult
"""
self.client.wait_for_result(timeout)
return self.client.get_result()
def clear_world(self):
"""
:rtype: UpdateWorldResponse
"""
req = UpdateWorldRequest(UpdateWorldRequest.REMOVE_ALL, WorldBody(), False, PoseStamped())
return self.update_world.call(req)
def remove_object(self, name):
"""
:param name:
:type name: str
:return:
:rtype: UpdateWorldResponse
"""
object = WorldBody()
object.name = str(name)
req = UpdateWorldRequest(UpdateWorldRequest.REMOVE, object, False, PoseStamped())
return self.update_world.call(req)
def add_box(self, name=u'box', size=(1, 1, 1), frame_id=u'map', position=(0, 0, 0), orientation=(0, 0, 0, 1),
pose=None):
"""
:param name:
:param size: (x length, y length, z length)
:param frame_id:
:param position:
:param orientation:
:param pose:
:return:
"""
box = make_world_body_box(name, size[0], size[1], size[2])
if pose is None:
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id)
pose.pose.position = Point(*position)
pose.pose.orientation = Quaternion(*orientation)
req = UpdateWorldRequest(UpdateWorldRequest.ADD, box, False, pose)
return self.update_world.call(req)
def add_sphere(self, name=u'sphere', size=1, frame_id=u'map', position=(0, 0, 0), orientation=(0, 0, 0, 1),
pose=None):
object = WorldBody()
object.type = WorldBody.PRIMITIVE_BODY
object.name = str(name)
if pose is None:
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id)
pose.pose.position = Point(*position)
pose.pose.orientation = Quaternion(*orientation)
object.shape.type = SolidPrimitive.SPHERE
object.shape.dimensions.append(size)
req = UpdateWorldRequest(UpdateWorldRequest.ADD, object, False, pose)
return self.update_world.call(req)
def add_mesh(self, name=u'mesh', mesh=u'', frame_id=u'map', position=(0, 0, 0), orientation=(0, 0, 0, 1),
pose=None):
object = WorldBody()
object.type = WorldBody.MESH_BODY
object.name = str(name)
if pose is None:
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id)
pose.pose.position = Point(*position)
pose.pose.orientation = Quaternion(*orientation)
object.mesh = mesh
req = UpdateWorldRequest(UpdateWorldRequest.ADD, object, False, pose)
return self.update_world.call(req)
def add_cylinder(self, name=u'cylinder', size=(1, 1), frame_id=u'map', position=(0, 0, 0), orientation=(0, 0, 0, 1),
pose=None):
object = WorldBody()
object.type = WorldBody.PRIMITIVE_BODY
object.name = str(name)
if pose is None:
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id)
pose.pose.position = Point(*position)
pose.pose.orientation = Quaternion(*orientation)
object.shape.type = SolidPrimitive.CYLINDER
object.shape.dimensions.append(size[0])
object.shape.dimensions.append(size[1])
req = UpdateWorldRequest(UpdateWorldRequest.ADD, object, False, pose)
return self.update_world.call(req)
def attach_box(self, name=u'box', size=None, frame_id=None, position=None, orientation=None):
"""
:type name: str
:type size: list
:type frame_id: str
:type position: list
:type orientation: list
:rtype: UpdateWorldResponse
"""
box = make_world_body_box(name, size[0], size[1], size[2])
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id) if frame_id is not None else u'map'
pose.pose.position = Point(*(position if position is not None else [0, 0, 0]))
pose.pose.orientation = Quaternion(*(orientation if orientation is not None else [0, 0, 0, 1]))
req = UpdateWorldRequest(UpdateWorldRequest.ADD, box, True, pose)
return self.update_world.call(req)
def attach_cylinder(self, name=u'cylinder', height=1, radius=1, frame_id=None, position=None, orientation=None):
"""
:type name: str
:type size: list
:type frame_id: str
:type position: list
:type orientation: list
:rtype: UpdateWorldResponse
"""
cylinder = make_world_body_cylinder(name, height, radius)
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id) if frame_id is not None else u'map'
pose.pose.position = Point(*(position if position is not None else [0, 0, 0]))
pose.pose.orientation = Quaternion(*(orientation if orientation is not None else [0, 0, 0, 1]))
req = UpdateWorldRequest(UpdateWorldRequest.ADD, cylinder, True, pose)
return self.update_world.call(req)
def attach_object(self, name, link_frame_id):
"""
:type name: str
:type link_frame_id: str
:return: UpdateWorldResponse
"""
req = UpdateWorldRequest()
req.rigidly_attached = True
req.body.name = name
req.pose.header.frame_id = link_frame_id
req.operation = UpdateWorldRequest.ADD
return self.update_world.call(req)
def detach_object(self, object_name):
req = UpdateWorldRequest()
req.body.name = object_name
req.operation = req.DETACH
return self.update_world.call(req)
def add_urdf(self, name, urdf, pose, js_topic=u''):
urdf_body = WorldBody()
urdf_body.name = str(name)
urdf_body.type = WorldBody.URDF_BODY
urdf_body.urdf = str(urdf)
urdf_body.joint_state_topic = str(js_topic)
req = UpdateWorldRequest(UpdateWorldRequest.ADD, urdf_body, False, pose)
if js_topic:
# FIXME publisher has to be removed, when object gets deleted
# FIXME there could be sync error, if objects get added/removed by something else
self.object_js_topics[name] = rospy.Publisher(js_topic, JointState, queue_size=10)
return self.update_world.call(req)
def set_object_joint_state(self, object_name, joint_states):
if isinstance(joint_states, dict):
joint_states = dict_to_joint_states(joint_states)
self.object_js_topics[object_name].publish(joint_states)
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.")
self.pick_g = None
self.move_pub = rospy.Publisher('/mobile_base_controller/cmd_vel',
Twist,
queue_size=1)
self.scene = PlanningSceneInterface()
def strip_leading_slash(self, s):
return s[1:] if s.startswith("/") else s
def pick_aruco(self, string_operation):
#rospy.sleep(2.0)
if string_operation == "pick":
self.prepare_robot()
# Detect object
rospy.loginfo(
"spherical_grasp_gui: Waiting for an aruco detection")
aruco_pose = rospy.wait_for_message('/aruco_single/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.pose.orientation = aruco_pose.pose.orientation
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()
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!")
# 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.")
# Move to safe
home = False
i = 0
#self.move_back(5)
while not home and i < 3:
home = self.move_arm_home()
i += 1
rospy.sleep(0.5)
result = self.pick_as.get_result()
if str(moveit_error_dict[result.error_code]) != "SUCCESS":
rospy.logerr("Failed to pick, not trying further")
return TriggerResponse(False, "Failed to pick")
self.pick_g = pick_g
return TriggerResponse(True, "Succeeded")
elif string_operation == "place":
if not self.pick_g:
rospy.logerr("Failed to place, no object position known")
return TriggerResponse(
False, "Failed to place, no object position known")
pick_g = self.pick_g
# Move torso to its maximum height
self.lift_torso()
# Raise arm
rospy.loginfo("Moving arm to a safe pose")
pmg = PlayMotionGoal()
pmg.motion_name = 'prepare_grasp' #'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.35
self.place_as.send_goal_and_wait(pick_g)
rospy.loginfo("Done!")
# Move to safe
home = False
i = 0
#self.move_back(5)
while not home and i < 3:
home = self.move_arm_home()
i += 1
rospy.sleep(0.5)
result = self.place_as.get_result()
if str(moveit_error_dict[result.error_code]) != "SUCCESS":
rospy.logerr("Failed to place")
return TriggerResponse(False, "Failed to place")
self.pick_g = None
return TriggerResponse(True, "Succeeded")
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.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.")
def move_back(self, n):
for i in range(n):
self.move_pub.publish(Twist(linear=Vector3(-0.15, 0, 0)))
rospy.sleep(0.05)
def move_arm_home(self):
rospy.loginfo("Fold arm safely")
pmg = PlayMotionGoal()
pmg.motion_name = 'home'
pmg.skip_planning = False
res = self.play_m_as.send_goal_and_wait(pmg)
if res != GoalStatus.SUCCEEDED:
return False
self.scene.remove_world_object("table")
rospy.loginfo("Done.")
return True
marker = False
moveit_commander.roscpp_initialize(sys.argv)
scene = moveit_commander.PlanningSceneInterface()
robot = moveit_commander.RobotCommander()
interpreter = moveit_commander.MoveGroupCommandInterpreter()
interpreter.execute("use arm")
group = interpreter.get_active_group()
eef_link=group.get_end_effector_link()
scene.remove_attached_object(eef_link, name="pringles")
scene.remove_world_object("pringles")
scene.remove_world_object("table")
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory)
rate = rospy.Rate(1)
play_m_as = SimpleActionClient('/play_motion', PlayMotionAction)
if not play_m_as.wait_for_server(rospy.Duration(20)):
rospy.logerr("Could not connect to /play_motion AS")
exit()
rospy.loginfo("Connected!")
head_cmd = rospy.Publisher(
'/head_controller/command', JointTrajectory, queue_size=1)
jt = JointTrajectory()
jt.joint_names = ['head_1_joint', 'head_2_joint']
jtp = JointTrajectoryPoint()
jtp.positions = [0, -0.56]
jtp.time_from_start = rospy.Duration(2.0)
jt.points.append(jtp)
head_cmd.publish(jt)
init = datetime.now()
#Detect object and determine its speed
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)
