def __init__(self): self.allowed_planning_time = 10.0 self.fixed_frame = 'base_link' self.gripper_frame = 'wrist_roll_link' self.group_name = 'arm' self.planning_scene_diff = moveit_msgs.msg.PlanningScene() self.planning_scene_diff.is_diff = True self.planning_scene_diff.robot_state.is_diff = True self.look_around = False self.max_acceleration_scaling_factor = 0 self.max_velocity_scaling_factor = 0 self.num_planning_attempts = 1 self.plan_only = False self.planner_id = 'RRTConnectkConfigDefault' self.replan = False self.replan_attempts = 5 self.replan_delay = 1 self.start_state = moveit_msgs.msg.RobotState() self.start_state.is_diff = True self.tolerance = 0.01 self._orientation_contraints = [] self._pose_goal = None self._joint_names = None self._joint_positions = None self._tf_listener = TransformListener()
def __init__(self, name, server, menu_handler, frame_world, frame_opt_parent, frame_opt_child, frame_sensor, marker_scale): print('Creating a new sensor named ' + name) self.name = name self.server = server self.menu_handler = menu_handler self.listener = TransformListener() self.br = tf.TransformBroadcaster() self.marker_scale = marker_scale # transforms self.world_link = frame_world self.opt_parent_link = frame_opt_parent self.opt_child_link = frame_opt_child self.sensor_link = frame_sensor self.updateAll() # update all the transformations # print('Collected pre, opt and pos transforms.') # # print('preT:\n' + str(self.preT)) # print('optT:\n' + str(self.optT)) # print('posT:\n' + str(self.posT)) self.optTInitial = copy.deepcopy(self.optT) self.createInteractiveMarker() # create interactive marker print('Created interactive marker.') # Start publishing now self.timer_callback = rospy.Timer( rospy.Duration(.1), self.publishTFCallback) # to periodically broadcast
def __init__(self, group, fixed_frame, gripper_frame, cart_srv, listener=None, plan_only=False): self._group = group self._fixed_frame = fixed_frame self._gripper_frame = gripper_frame # a.k.a end-effector frame self._action = actionlib.SimpleActionClient('move_group', MoveGroupAction) self._traj_action = actionlib.SimpleActionClient( 'execute_trajectory', ExecuteTrajectoryAction) self._cart_service = rospy.ServiceProxy(cart_srv, GetCartesianPath) try: self._cart_service.wait_for_service(timeout=3) except: rospy.logerr("Timeout waiting for Cartesian Planning Service!!") self._action.wait_for_server() if listener == None: self._listener = TransformListener() else: self._listener = listener self.plan_only = plan_only self.planner_id = None self.planning_time = 15.0
def __init__(self): # enables self.enable_tilt = True self.enable_look = True self.has_goal = False # pose and lock self.x = 1.0 self.y = 0.0 self.mutex = Lock() self.listener = TransformListener() self.client = actionlib.SimpleActionClient( 'head_controller/point_head', PointHeadAction) self.client.wait_for_server() self.service = rospy.Service('~configure', TiltControl, self.serviceHandler) self.plan_sub = rospy.Subscriber( 'move_base/TrajectoryPlannerROS/local_plan', Path, self.planCallback) self.stat_sub = rospy.Subscriber('move_base/status', GoalStatusArray, self.statCallback)
def __init__(self): self.pose = PoseStamped() self._tf_listener = TransformListener() #self.pose_pub = rospy.Publisher('tool_pose', PoseStamped, queue_size=10) #self.joint_sub = rospy.Subscriber('joint_states', JointState, self.tool_pose_callback) self.joint_srv = rospy.Service('tool_pose', GetToolPose, self.tool_pose_callback)
def __init__(self, name): # wait for moveit while not "/move_group/result" in dict( rospy.get_published_topics()).keys(): rospy.sleep(2) self.group = MoveGroupCommander("arm") self.group.set_start_state_to_current_state() self.group.set_planner_id("RRTConnectkConfigDefault") self.group.set_pose_reference_frame("/base_footprint") self.group.set_max_velocity_scaling_factor(1) self.group.set_num_planning_attempts(50) self.group.set_planning_time(10) self.group.set_goal_position_tolerance(0.01) self.group.set_goal_orientation_tolerance(0.02) self.tf_listener = TransformListener() self._action_name = name self._as = actionlib.SimpleActionServer( self._action_name, elevator.msg.SimpleTargetAction, execute_cb=self.execute_cb, auto_start=False) self._as.start()
def __init__(self, args): rospy.init_node('roadmap_server') self.optimization_distance = rospy.get_param('~optimization_distance', 10); self.tf = TransformListener() stereo_cam = camera.Camera((389.0, 389.0, 89.23 * 1e-3, 323.42, 323.42, 274.95)) self.skel = Skeleton(stereo_cam) # self.skel.node_vdist = 1 if False: self.skel.load(args[1]) self.skel.optimize() self.startframe = 100000 else: self.startframe = 0 self.frame_timestamps = {} self.vo = None self.pub = rospy.Publisher("roadmap", vslam.msg.Roadmap) time.sleep(1) #self.send_map(rospy.time(0)) self.wheel_odom_edges = set() rospy.Subscriber('/wide_stereo/raw_stereo', stereo_msgs.msg.RawStereo, self.handle_raw_stereo, queue_size=2, buff_size=7000000)
def get_cube_names(self, pose): point = PoseStamped() point.header.frame_id = 'base_link' point.header.stamp = rospy.Time() point.pose = pose tf_listener = TransformListener() rospy.sleep(1.0) return tf_listener.transformPose('/map', point)
def __init__(self): self.manip = mic.MoveGroupCommander("manipulator") self.client = act.SimpleActionClient('joint_trajectory_action', trajAction) rp.loginfo("Waiting for server joint_trajectory_action.") self.client.wait_for_server() rp.loginfo("Successfuly connected to server.") self.tfListener = TransformListener()
def __init__(self): self._joint_client = actionlib.SimpleActionClient( JOINT_ACTION_SERVER, control_msgs.msg.FollowJointTrajectoryAction) self._joint_client.wait_for_server(rospy.Duration(10)) self._move_group_client = actionlib.SimpleActionClient( MOVE_GROUP_ACTION_SERVER, MoveGroupAction) self._move_group_client.wait_for_server(rospy.Duration(10)) self._compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK) self._tf_listener = TransformListener()
def filter_by_distance(features, max_distance): # type: (List[Feature], float) -> List[Feature] cam_pixel_to_point = rospy.ServiceProxy('cam_pixel_to_point', CamPixelToPoint) tf_listener = TransformListener() filtered = [] for feature in features: cam_pos = Vector3() cam_pos.x, cam_pos.y = feature.centroid[0], feature.centroid[1] point = cam_pixel_to_point(cam_pos).point # type: PointStamped tf_listener.waitForTransform('camera_link', point.header.frame_id, rospy.Time(rospy.get_time()), rospy.Duration(1)) point = tf_listener.transformPoint('camera_link', point) if point.point.x <= max_distance: filtered.append(feature) return filtered
def __init__(self): self.moving = False self.listener = TransformListener() self.broadcaster = TransformBroadcaster() self.client = actionlib.SimpleActionClient("head_controller/point_head", PointHeadAction) self.client.wait_for_server() self.face_sub = rospy.Subscriber("face_detector/people_tracker_measurements_array", PositionMeasurementArray, self.faceCallback) self.lastFaceCallback = time.time() self.lastHeadTarget = None
def __init__(self, group, frame, listener=None, plan_only=False): self._group = group self._fixed_frame = frame self._action = actionlib.SimpleActionClient('move_group', MoveGroupAction) self._action.wait_for_server() if listener == None: self._listener = TransformListener() else: self._listener = listener self.plan_only = plan_only self.planner_id = None self.planning_time = 15.0
def feature_depths(features): cam_pixel_to_point = rospy.ServiceProxy('cam_pixel_to_point', CamPixelToPoint) tf_listener = TransformListener() depths = [] for feature in features: cam_pos = Vector3() cam_pos.x, cam_pos.y = feature.centroid[0], feature.centroid[1] point = cam_pixel_to_point(cam_pos).point # type: PointStamped tf_listener.waitForTransform('camera_link', point.header.frame_id, rospy.Time(rospy.get_time()), rospy.Duration(1)) point = tf_listener.transformPoint('camera_link', point) depths.append(np.linalg.norm([point.point.x, point.point.y, point.point.z])) return depths
def __init__(self): self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("left_arm", "left_hand", verbose = True) self.move_group = MoveGroupInterface("left_arm", "base_link") find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server(rospy.Duration(15.0)) self.cubes = [] self.tf_listener = TransformListener() # self.gripper_client = actionlib.SimpleActionClient("/robot/end_effector/left_gripper/gripper_action", GripperCommandAction) # self.gripper_client.wait_for_server() # rospy.loginfo("...connected.") rospy.sleep(2.0)
def execute(self, userdata): rospy.logdebug('CamToDropzone: Executing state CamToDropzone') listener = TransformListener() if utils.manipulation is None: utils.manipulation = Manipulation() utils.manipulation.set_turbo_mode() utils.manipulation.set_movement_time(9) # TODO: Exception schmeissen wenn abort_after vergangen ist abort_after = 15 then = int(time.time()) now = int(time.time()) while not listener.frameExists("drop_point"): if not (now - then < abort_after): rospy.logdebug('drop_point frame not found!') return 'fail' rospy.loginfo("wait for drop_point frame") rospy.sleep(2.) now = int(time.time()) then = int(time.time()) now = int(time.time()) while not listener.frameExists("mdl_middle"): if not (now - then < abort_after): rospy.logdebug('CamToDropzone: mdl_middle frame not found!') return 'fail' rospy.sleep(2.) now = int(time.time()) if len(userdata.scan_conveyor_pose) == 0: rospy.logdebug('CamToDropzone: No scanPose, calculate now...') p = utils.manipulation.scan_conveyor_pose() userdata.scan_conveyor_pose.append(p) else: rospy.logdebug('CamToDropzone: scanPose found!') p = userdata.scan_conveyor_pose[0] rospy.logdebug('CamToDropzone: Move arm to scan_conveyor_pose') for i in range(0, 3): if utils.manipulation.move_to(p): return 'scanPoseReached' else: if i == 2: rospy.logdebug('CamToDropzone: Cant move arm to scan_conveyor_pose') return 'fail'
def select_center(features): # type: (List[Feature]) -> Feature cam_pixel_to_point = rospy.ServiceProxy('cam_pixel_to_point', CamPixelToPoint) tf_listener = TransformListener() angles = [] center_ray = np.array([1.0, 0.0, 0.0]) for feature in features: cam_pos = Vector3() cam_pos.x, cam_pos.y = feature.centroid[0], feature.centroid[1] point = cam_pixel_to_point(cam_pos).point # type: PointStamped tf_listener.waitForTransform('camera_link', point.header.frame_id, rospy.Time(rospy.get_time()), rospy.Duration(1)) point = tf_listener.transformPoint('camera_link', point) ray = np.array([point.point.x, point.point.y, point.point.z]) ray /= np.linalg.norm(ray) angle = np.arccos(np.dot(center_ray, ray)) angles.append(abs(angle)) return features[np.argmin(angles)]
def __init__(self): self.arm = rospy.get_param("arm", "r") self.tl = TransformListener() self.perception_srv = rospy.Service("/clickable_world/detect_objects", PerceiveButtons, self.do_perception) self.action_srv = rospy.Service("/clickable_world/grasp_object", ButtonAction, self.do_action) self.obj_seg_srv = rospy.ServiceProxy("/object_button_detect", ObjectButtonDetect) self.grasp_client = actionlib.SimpleActionClient(self.arm + '_overhead_grasp', OverheadGraspAction) self.grasp_client.wait_for_server() self.grasp_setup_client = actionlib.SimpleActionClient(self.arm + '_overhead_grasp_setup', OverheadGraspSetupAction) self.grasp_setup_client.wait_for_server()
def transform_pose(_pose2D, src_frame='CameraTop_frame', dst_frame='/base_footprint', timeout=3): tl = TransformListener() pose_stamped = PoseStamped() pose_stamped.header.stamp = rospy.Time() pose_stamped.header.frame_id = src_frame pose_stamped.pose = Pose(Point(_pose2D.x, _pose2D.y, 0.0), Quaternion()) try: tl.waitForTransform(target_frame=dst_frame, source_frame=src_frame, time=rospy.Time(), timeout=rospy.Duration(timeout)) pose_transf = tl.transformPose(dst_frame, pose_stamped) except Exception as e: rospy.logwarn("Transformation failed!!! %s" % str(e)) return _pose2D return Pose2D(pose_transf.pose.position.x, pose_transf.pose.position.y, 0.0)
def __init__(self): self.last_vel_time = rospy.Time(0) # pose and lock self.x = 1.0 self.y = 0.0 self.mutex = Lock() self.listener = TransformListener() self.client = actionlib.SimpleActionClient( "head_controller/point_head", PointHeadAction) self.client.wait_for_server() self.plan_sub = rospy.Subscriber("/safe_teleop_base/local_plan", Path, self.planCallback) self.vel_sub = rospy.Subscriber("/teleop/cmd_vel/unsafe", Twist, self.velCallback)
def __init__(self): self.has_goal = False # pose and lock self.x = 1.0 self.y = 0.0 self.mutex = Lock() self.listener = TransformListener() self.client = actionlib.SimpleActionClient( "head_controller/point_head", PointHeadAction) self.client.wait_for_server() self.plan_sub = rospy.Subscriber( "move_base/TrajectoryPlannerROS/local_plan", Path, self.planCallback) self.stat_sub = rospy.Subscriber("move_base/status", GoalStatusArray, self.statCallback)
def __init__(self): self.arm = rospy.get_param("arm", "r") self.tl = TransformListener() self.perception_srv = rospy.Service( "/clickable_world/place_table_perceive", PerceiveButtons, self.do_perception) self.action_srv = rospy.Service("/clickable_world/place_object", ButtonAction, self.do_action) self.pc_capture_srv = rospy.ServiceProxy( "/table_detection/surf_seg_capture_pc", std_srvs.srv.Empty) self.table_seg_srv = rospy.ServiceProxy( "/table_detection/segment_surfaces", SegmentSurfaces) self.grasp_client = actionlib.SimpleActionClient( self.arm + '_overhead_grasp', OverheadGraspAction) self.grasp_client.wait_for_server() self.grasp_setup_client = actionlib.SimpleActionClient( self.arm + '_overhead_grasp_setup', OverheadGraspSetupAction) self.grasp_setup_client.wait_for_server()
def __init__(self, *args): TransformListener.__init__(self) python_actions.Action.__init__(self, args[0], args[1], args[2], args[3]) self.name = args[0] try: self.head_controller = rospy.get_param(self.name + "/head_controller") except KeyError: self.head_controller = "head_controller" rospy.set_param(self.name + "/head_controller", self.head_controller) self.head_controller_publisher = rospy.Publisher( self.head_controller + "/command", mechanism_msgs.msg.JointStates) self.people_sub = rospy.Subscriber( "/face_detector/people_tracker_measurements", PositionMeasurement, self.people_position_measurement) self.face_det = rospy.ServiceProxy('/start_detection', StartDetection)
def transform_pose(pose, src_frame, dst_frame, timeout=10): """ Transforms the given pose from src_frame to dst_frame. :param src_frame :param dst_frame :param timeout the amount of time allowed (in secs) for a transformation (default 3) """ if str(type(pose)) != str(type(Pose())): rospy.logwarn(colors.BACKGROUND_RED + "The 'pose' should be a Pose object, not '%s'.%s" % (str(type(pose)).split('\'')[1], colors.NATIVE_COLOR)) from tf.listener import TransformListener assert timeout >= 1 pose_stamped = PoseStamped() pose_stamped.header.stamp = rospy.Time() pose_stamped.header.frame_id = src_frame pose_stamped.pose = pose global _tl if not _tl: _tl = TransformListener() rospy.sleep(0.5) timeout -= 0.5 rospy.loginfo("Transforming position from %s to %s coordinates..." % ( src_frame, dst_frame)) # If something fails we'll return the original pose (for testing # with mocks when tf isn't available) result = pose try: _tl.waitForTransform( target_frame=dst_frame, source_frame=src_frame, time=rospy.Time(), timeout=rospy.Duration(timeout)) pose_transf = _tl.transformPose(dst_frame, pose_stamped) result = pose_transf.pose except Exception as e: rospy.logwarn(colors.BACKGROUND_RED + "Warning! Pose transformation failed! %s%s" % (str(e), colors.NATIVE_COLOR)) return result
class MoveFloorButton: def __init__(self): self.tl = TransformListener() self.perception_srv = rospy.Service("/clickable_world/detect_empty_floor", PerceiveButtons, self.do_perception) self.percept_pub = rospy.Publisher("/clickable_world/floor_button_vis", Marker) self.goal_pub = rospy.Publisher("/clickable_world/move_floor_goal", PoseStamped) self.action_srv = rospy.Service("/clickable_world/move_empty_floor", ButtonAction, self.do_action) self.floor_seg_srv = rospy.ServiceProxy("/move_floor_detect", SegmentFloor) self.floor_move_client = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("[move_floor_button] MoveFloorButton loaded.") def do_perception(self, req): # segment surfaces rospy.loginfo("[move_floor_button] Segmenting floor") self.surfaces = self.floor_seg_srv().surfaces for i in range(len(self.surfaces)): self.surfaces[i].color.r = 0 self.surfaces[i].color.g = 0 self.surfaces[i].color.b = 256 self.surfaces[i].color.a = 256 if len(self.surfaces) != 0: self.percept_pub.publish(self.surfaces[0]) resp = PerceiveButtonsResponse() resp.buttons = self.surfaces return resp def do_action(self, req): rospy.loginfo("[move_floor_button] MoveFloorButton clicked!") # put 3d pt in base frame req.pixel3d.header.stamp = rospy.Time(0) move_pt = self.tl.transformPoint("/base_link", req.pixel3d) floor_move_goal = MoveBaseGoal() floor_move_goal.target_pose.header.frame_id = move_pt.header.frame_id floor_move_goal.target_pose.pose.position = move_pt.point quat = tf_trans.quaternion_from_euler(0, 0, np.arctan2(move_pt.point.y, move_pt.point.x)) floor_move_goal.target_pose.pose.orientation = Quaternion(quat[0], quat[1], quat[2], quat[3]) floor_move_goal.target_pose.header.frame_id = "/base_link" floor_move_goal.target_pose.header.stamp = rospy.Time.now() try: self.goal_pub.publish(floor_move_goal.target_pose) self.floor_move_client.send_goal(floor_move_goal) self.floor_move_client.wait_for_result() cur_pose = self.floor_move_client.get_result() except rospy.ROSInterruptException: print "[move_floor_button] Move floor failed" return ButtonActionResponse()
def __init__(self): # Internal parameters self.publish_rate = rospy.get_param("~publish_rate", 10.0) self.listener = TransformListener() self.desired_pose_frame = rospy.get_param("~desired_pose_frame", "base_link") self.position_match_threshold = rospy.get_param( "~position_match_threshold", 1.0) # Variables to keep track of state self.closest_person = None self.leg_detections = [] self.closest_person_lock = Lock() self.leg_detections_lock = Lock() # Internal helpers self.person_face_distance_func = lambda A, B: np.sqrt( (A.pose.position.x - B.pos.x)**2 + (A.pose.position.y - B.pos.y)**2 + (1.2 - B.pos.z)**2 # A person's face is about 4ft from the floor ) self.leg_detection_is_closest_face = lambda detected_person: ( self.closest_person is not None and self.closest_person. detection_context.pose_source == DetectionContext.POSE_FROM_FACE and self.closest_person.id == detected_person.id) # The subscribers and publishers self.face_sub = rospy.Subscriber( "face_detector/people_tracker_measurements_array", PositionMeasurementArray, self.face_callback) self.leg_sub = rospy.Subscriber("people_tracked", PersonArray, self.leg_callback) self.closest_person_pub = rospy.Publisher('~closest_person', Person, queue_size=10) # Debug self.debug_enabled = rospy.get_param("~debug", False) if self.debug_enabled: self.debug_pub1 = rospy.Publisher("~debug/1", Marker, queue_size=1) self.debug_pub2 = rospy.Publisher("~debug/2", Marker, queue_size=1)
def execute(self, userdata): rospy.logdebug('PlaceTask6: Executing state PlaceTask6') listener = TransformListener() if utils.manipulation is None: utils.manipulation = Manipulation() # TODO: Exception schmeissen wenn abort_after vergangen ist abort_after = 15 then = int(time.time()) now = int(time.time()) while not listener.frameExists("target_zone"): if not now - then < abort_after: rospy.logdebug("PlaceTask6: target_zone frame not found") return 'fail' rospy.loginfo("wait for target_zone frame") rospy.sleep(2.) now = int(time.time()) self.__place_pose = self.get_place_point(userdata) rospy.logdebug('PlaceTask6: Move Arm to Place Pose') rospy.logdebug(self.__place_pose) for i in range(0, 7): if utils.manipulation.move_to(self.__place_pose): rospy.logdebug('PlaceTask6: OpenGripper') utils.manipulation.open_gripper() return 'success' else: rospy.logdebug('PlaceTask6: Calculate new place position...') if i <= 2: self.__place_pose = self.get_place_point(userdata) if i > 2: self.__place_pose = self.get_place_point(userdata, 3) if i == 6: rospy.logdebug('PlaceTask6: Cant find place position!') utils.manipulation.open_gripper() return 'fail'
class ToolPose(object): def __init__(self): self.pose = PoseStamped() self._tf_listener = TransformListener() #self.pose_pub = rospy.Publisher('tool_pose', PoseStamped, queue_size=10) #self.joint_sub = rospy.Subscriber('joint_states', JointState, self.tool_pose_callback) self.joint_srv = rospy.Service('tool_pose', GetToolPose, self.tool_pose_callback) def tool_pose_callback(self, req): ''' get gripper link pose with respect to base_link''' res = GetToolPoseResponse() time = 0 trans = None rot = None self.pose.header.frame_id = 'base_link' while not rospy.is_shutdown(): try: time = self._tf_listener.getLatestCommonTime( 'base_link', 'wrist_roll_link') trans, rot = self._tf_listener.lookupTransform( 'base_link', 'wrist_roll_link', time) break except (tf.Exception, tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e: continue self.pose.pose.position.x = round(trans[0], 4) self.pose.pose.position.y = round(trans[1], 4) self.pose.pose.position.z = round(trans[2], 4) self.pose.pose.orientation.x = round(rot[0], 4) self.pose.pose.orientation.y = round(rot[1], 4) self.pose.pose.orientation.z = round(rot[2], 4) self.pose.pose.orientation.w = round(rot[3], 4) #self.pose_pub.publish(self.pose) res.tool_pose = self.pose return res
def main(): rospy.init_node('find_waypoints') joy_subscriber = SubscriberValue('joy', Joy) tf_listener = TransformListener() names = [ 'initial_pose', 'off_ramp_start', 'off_ramp_end', 'ar_search_1', 'ar_search_2', 'S1', 'S2', 'S3', 'S4', 'S5', 'S6', 'S7', 'S8', 'on_ramp' ] coords = [] for name in names: rospy.sleep(rospy.Duration(2)) print(name) while not joy_subscriber.value.buttons[0]: rospy.sleep(rospy.Duration(0, 1000)) pose = tf_listener.lookupTransform('/map', '/base_link', rospy.Time()) coords.append(pose) print('Saved') out_file = open( path.join(path.dirname(__file__), '..', 'param', 'find_waypoints_generated.yaml'), 'w') out_file.write('named_poses:\n') for name, ((x, y, z), (rx, ry, rz, rw)) in zip(names, coords): out_file.write(' {name}:\n'.format(name=name)) out_file.write(' position:\n') out_file.write(' - {x}\n'.format(x=x)) out_file.write(' - {y}\n'.format(y=y)) out_file.write(' - {z}\n'.format(z=z)) out_file.write(' orientation:\n') out_file.write(' - {x}\n'.format(x=rx)) out_file.write(' - {y}\n'.format(y=ry)) out_file.write(' - {z}\n'.format(z=rz)) out_file.write(' - {w}\n'.format(w=rw))
def __init__(self): self.tl = TransformListener() self.perception_srv = rospy.Service("/clickable_world/detect_empty_floor", PerceiveButtons, self.do_perception) self.percept_pub = rospy.Publisher("/clickable_world/floor_button_vis", Marker) self.goal_pub = rospy.Publisher("/clickable_world/move_floor_goal", PoseStamped) self.action_srv = rospy.Service("/clickable_world/move_empty_floor", ButtonAction, self.do_action) self.floor_seg_srv = rospy.ServiceProxy("/move_floor_detect", SegmentFloor) self.floor_move_client = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("[move_floor_button] MoveFloorButton loaded.")
def __init__(self): self.last_vel_time = rospy.Time(0) # pose and lock self.x = 1.0 self.y = 0.0 self.mutex = Lock() self.listener = TransformListener() self.client = actionlib.SimpleActionClient("head_controller/point_head", PointHeadAction) self.client.wait_for_server() self.plan_sub = rospy.Subscriber("/safe_teleop_base/local_plan", Path, self.planCallback) self.vel_sub = rospy.Subscriber("/teleop/cmd_vel/unsafe", Twist, self.velCallback)
def __init__(self): self.marker_pub = rospy.Publisher(HUMAN_TRIGGER_MARKER_PUB, Marker, queue_size=10) self.coord_signal = rospy.Publisher(COORD_SIGNAL_PUB, CoordinationSignal, queue_size=1) self.tf_listener = TransformListener() self.move_base = SimpleActionClient(MOVE_BASE_ACTION_SRV, MoveBaseAction) rospy.loginfo( "{} waiting for move_base action server.".format(NODE_NAME)) self.move_base.wait_for_server() rospy.loginfo("{} found move_base action server.".format(NODE_NAME)) self.navigating_fact_id = None rospy.loginfo(NODE_NAME + " waiting for underworlds start fact service server.") self.start_fact = rospy.ServiceProxy(START_FACT_SRV_NAME, StartFact) self.start_fact.wait_for_service() rospy.loginfo(NODE_NAME + " found underworlds start fact service server.") rospy.loginfo(NODE_NAME + " waiting for underworlds stop fact service server.") self.end_fact = rospy.ServiceProxy(STOP_FACT_SRV_NAME, EndFact) self.end_fact.wait_for_service() rospy.loginfo(NODE_NAME + " found underworlds stop fact service server.") self.coordination_human_timer = None self.trigger_zone_bb = BoundingBox(3.0, 13.0, 5.0, 16.0) self.state = self.State.IDLE rospy.Timer(rospy.Duration(1.0), lambda _: self.draw_trigger_zone(False), oneshot=True) rospy.Timer(rospy.Duration(0.1), self.loop)
def jointsInfo(printoutRate=0.5, anonym=False): """ Creates new node to listen to /joint_states topic\n Usage:\n - printoutRate - print rate to terminal in Hz - default: 0.5 Hz - anonym [True/False] - do you want to run it as anonymous node - default: False """ rp.init_node("abb_jointListener", anonymous=anonym) tfListener = TransformListener() listener = rp.Subscriber("/joint_states", JointState, __listenCb, tfListener) rate = rp.Rate(printoutRate) rate.sleep() rp.spin()
def __init__(self): self.arm = rospy.get_param("arm", "r") self.tl = TransformListener() self.perception_srv = rospy.Service( "/clickable_world/place_table_perceive", PerceiveButtons, self.do_perception ) self.action_srv = rospy.Service("/clickable_world/place_object", ButtonAction, self.do_action) self.pc_capture_srv = rospy.ServiceProxy("/table_detection/surf_seg_capture_pc", std_srvs.srv.Empty) self.table_seg_srv = rospy.ServiceProxy("/table_detection/segment_surfaces", SegmentSurfaces) self.grasp_client = actionlib.SimpleActionClient(self.arm + "_overhead_grasp", OverheadGraspAction) self.grasp_client.wait_for_server() self.grasp_setup_client = actionlib.SimpleActionClient( self.arm + "_overhead_grasp_setup", OverheadGraspSetupAction ) self.grasp_setup_client.wait_for_server()
def __init__(self): # enables self.enable_tilt = True self.enable_look = True self.has_goal = False # pose and lock self.x = 1.0 self.y = 0.0 self.mutex = Lock() self.listener = TransformListener() self.client = actionlib.SimpleActionClient('head_controller/point_head', PointHeadAction) self.client.wait_for_server() self.service = rospy.Service('~configure', TiltControl, self.serviceHandler) self.plan_sub = rospy.Subscriber('move_base/TrajectoryPlannerROS/local_plan', Path, self.planCallback) self.stat_sub = rospy.Subscriber('move_base/status', GoalStatusArray, self.statCallback)
class Transformer: def __init__(self): self.__listener = TransformListener() def __del__(self): pass def transform_to(self, pose_target, target_frame="/odom_combined"): ''' Transforms the pose_target into the target_frame. :param pose_target: object to transform as PoseStamped/PointStamped/Vector3Stamped/CollisionObject/PointCloud2 :param target_frame: goal frame id :return: transformed object ''' if pose_target.header.frame_id == target_frame: return pose_target odom_pose = None i = 0 while odom_pose is None and i < 10: try: #now = rospy.Time.now() #self.__listener.waitForTransform(target_frame, pose_target.header.frame_id, now, rospy.Duration(4)) if type(pose_target) is CollisionObject: i = 0 new_co = deepcopy(pose_target) for cop in pose_target.primitive_poses: p = PoseStamped() p.header = pose_target.header p.pose.orientation = cop.orientation p.pose.position = cop.position p = self.transform_to(p, target_frame) if p is None: return None new_co.primitive_poses[i].position = p.pose.position new_co.primitive_poses[i].orientation = p.pose.orientation i += 1 new_co.header.frame_id = target_frame return new_co if type(pose_target) is PoseStamped: odom_pose = self.__listener.transformPose(target_frame, pose_target) break if type(pose_target) is Vector3Stamped: odom_pose = self.__listener.transformVector3(target_frame, pose_target) break if type(pose_target) is PointStamped: odom_pose = self.__listener.transformPoint(target_frame, pose_target) break if type(pose_target) is PointCloud2: odom_pose = self.__listener.transformPointCloud(target_frame, pose_target) break except Exception, e: print "tf error:::", e rospy.sleep(0.5) i += 1 rospy.logdebug("tf fail nr. " + str(i)) if odom_pose is None: rospy.logerr("FUUUUUUUUUUUUUU!!!! f*****g tf shit!!!!") return odom_pose
class NavHeadController: def __init__(self): # enables self.enable_tilt = True self.enable_look = True self.has_goal = False # pose and lock self.x = 1.0 self.y = 0.0 self.mutex = Lock() self.listener = TransformListener() self.client = actionlib.SimpleActionClient('head_controller/point_head', PointHeadAction) self.client.wait_for_server() self.service = rospy.Service('~configure', TiltControl, self.serviceHandler) self.plan_sub = rospy.Subscriber('move_base/TrajectoryPlannerROS/local_plan', Path, self.planCallback) self.stat_sub = rospy.Subscriber('move_base/status', GoalStatusArray, self.statCallback) def serviceHandler(self, req): self.enable_tilt = req.enable_tilt self.enable_look = req.enable_look and self.enable_tilt # tilt has to be enabled to look return TiltControlResponse() def statCallback(self, msg): goal = False for status in msg.status_list: if status.status == GoalStatus.ACTIVE: goal = True break self.has_goal = goal def planCallback(self, msg): # Only need to do this if we are looking if not self.enable_look: return # get the goal pose_stamped = msg.poses[-1] pose = pose_stamped.pose # look ahead one meter R = quaternion_matrix([pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]) point = [1, 0, 0, 1] M = R*point p = PointStamped() p.header.frame_id = pose_stamped.header.frame_id p.header.stamp = rospy.Time(0) p.point.x += pose_stamped.pose.position.x + M[0,0] p.point.y += pose_stamped.pose.position.y + M[1,0] p.point.z += pose_stamped.pose.position.z + M[2,0] # transform to base_link p = self.listener.transformPoint('base_link', p) # update with self.mutex: if p.point.x < 0.65: self.x = 0.65 else: self.x = p.point.x if p.point.y > 0.5: self.y = 0.5 elif p.point.y < -0.5: self.y = -0.5 else: self.y = p.point.y def loop(self): while not rospy.is_shutdown(): if self.has_goal and self.enable_tilt: goal = PointHeadGoal() goal.target.header.stamp = rospy.Time.now() goal.target.header.frame_id = 'base_link' with self.mutex: goal.target.point.x = self.x goal.target.point.y = self.y self.x = 1 self.y = 0 goal.target.point.z = 0.75 goal.min_duration = rospy.Duration(1.0) self.client.send_goal(goal) self.client.wait_for_result() with self.mutex: goal.target.point.x = self.x goal.target.point.y = self.y self.x = 1 self.y = 0 goal.target.point.z = 0.0 self.client.send_goal(goal) self.client.wait_for_result() else: rospy.sleep(1.0)
def __init__(self): self.__listener = TransformListener()
class SEMAPEnvironmentServices(): server = None objects = {} tf_broadcaster = None tf_listener = None marker_pub = None range_query2d_marker = None range_query3d_marker = None range_query_marker = None area_query_marker = None volume_query_marker = None def __init__(self): self.setup_node() self.server = InteractiveMarkerServer("semap_environment") self.tf_broadcaster = TransformBroadcaster() self.tf_listener = TransformListener() def setup_node(self): rospy.init_node('semap_environment_services') rospy.loginfo( "SEMAP Environment Services are initializing...\n" ) rospy.Timer(rospy.Duration(0.02), self.publishTF) #rospy.Timer(rospy.Duration(1.0), self.activate_robot_surroundings) rospy.Subscriber("create_object", PoseStamped, self.createObjectCb) #rospy.Subscriber("polygon", PolygonStamped, self.findObjectWithinPolygonCb) #rospy.Subscriber("polygon", PolygonStamped, self.copyObjectWithinPolygonCb) rospy.Subscriber("polygon", PolygonStamped, self.findObjectWithinVolumeCb) rospy.Subscriber("range2d", PointStamped, self.findObjectWithinRange2DCb) rospy.Subscriber("range_query", PointStamped, self.findObjectWithinRangeCb) self.range_query_marker = rospy.Publisher("distance_query", MarkerArray, queue_size=10) self.area_query_marker = rospy.Publisher("area_query", PolygonStamped, queue_size=10) self.volume_query_marker = rospy.Publisher("volume_query", MarkerArray, queue_size=10) self.range_query2d_marker = rospy.Publisher("range_query2d_marker", Marker, queue_size=10) self.marker_pub = rospy.Publisher("collection_marker", MarkerArray, queue_size=10) user = rospy.get_param('~user') password = rospy.get_param('~password') host = rospy.get_param('~host') database = rospy.get_param('~database') initializeConnection(user, password, host, database, False) ## Active Objects srv_refresh_objects = rospy.Service('refresh_objects', ActivateObjects, self.refresh_objects) srv_refresh_all_objects = rospy.Service('refresh_all_objects', ActivateAllObjects, self.refresh_all_objects) srv_activate_objects = rospy.Service('activate_objects', ActivateObjects, self.activate_objects) srv_activate_all_objects = rospy.Service('activate_all_objects', ActivateAllObjects, self.activate_all_objects) srv_deactivate_objects = rospy.Service('deactivate_objects', DeactivateObjects, self.deactivate_objects) srv_deactivate_all_object = rospy.Service('deactivate_all_objects', DeactivateAllObjects, self.deactivate_all_objects) srv_show_objects = rospy.Service('show_objects', ActivateObjects, self.show_objects) srv_show_all_objects = rospy.Service('show_all_objects', ActivateAllObjects, self.show_all_objects) #srv_unshow_objects = rospy.Service('unshow_objects', ActivateObjects, self.activate_all_objects) #srv_unshow_all_objects = rospy.Service('unshow_all_objects', ActivateAllObjects, self.activate_all_objects) srv_show_distance_between_objects = rospy.Service('show_distance_between_objects', GetDistanceBetweenObjects, self.showDistanceBetweenObjects) srv_show_objects_within_range = rospy.Service('show_objects_within_range', GetObjectsWithinRange, self.showObjectWithinRange) srv_show_objects_within_area = rospy.Service('show_objects_within_area', GetObjectsWithinArea, self.showObjectWithinArea) srv_show_objects_within_volume = rospy.Service('show_objects_within_volume', GetObjectsWithinVolume, self.showObjectWithinVolume) srv_activate_objects_in_objects = rospy.Service('activate_objects_in_objects', GetObjectsInObjects, self.activateObjectsInObjects) srv_show_objects_in_objects = rospy.Service('show_objects_in_objects', GetObjectsInObjects, self.showObjectsInObjects) srv_activate_objects_on_objects = rospy.Service('activate_objects_on_objects', GetObjectsOnObjects, self.activateObjectsOnObjects) srv_show_objects_on_objects = rospy.Service('show_objects_on_objects', GetObjectsOnObjects, self.showObjectsOnObjects) rospy.loginfo( "SEMAP DB Services are running...\n" ) def spin(self): rospy.spin() ### Services def activate_objects(self, req): then = rospy.Time.now() if len( req.ids) > 0: rospy.loginfo("SpatialEnv SRVs: activate_objects") #self.deactivate_objects(req) get_res = call_get_object_instances(req.ids) rospy.loginfo("Call Get Object Instances took %f seconds" % (rospy.Time.now() - then).to_sec()) for obj in get_res.objects: #rospy.loginfo("Activate object: %s" % obj.name) # get inst visu from db # if exits: convertNonDBtype # else create one, store and pass on if obj.name in self.objects.keys(): #if self.objects[obj.name].status == "active": #print 'object', obj.name, 'is already active and gets reactivated' if self.objects[obj.name].status == "inactive": del self.objects[obj.name] #print 'object', obj.name, 'is inactive and gets removed from inactive pool' #else: # print 'object was unknown so far, now its active' then2 = rospy.Time.now() active_object = EnvironmentObject(obj.id, obj.name, obj, InteractiveObjectMarker(obj, self.server, None ), status = "active") #rospy.loginfo("Marker took %f seconds" % (rospy.Time.now() - then2).to_sec()) self.objects[obj.name] = active_object #rospy.loginfo("Took %f seconds" % (rospy.Time.now() - now).to_sec()) self.publishTF(None) rospy.loginfo("Took %f seconds" % (rospy.Time.now() - then).to_sec()) rospy.loginfo("SpatialEnv SRVs: activate_objects - done") return ActivateObjectsResponse() def show_objects(self, req): if len( req.ids) > 0: rospy.loginfo("SpatialEnv SRVs: show_objects") get_res = call_get_object_instances(req.ids) for obj in get_res.objects: if obj.name in self.objects.keys(): if self.objects[obj.name].status == "active": print 'object', obj.name, 'is already active' break elif self.objects[obj.name].status == "inactive": print 'object', obj.name, 'is inactive' ghost = EnvironmentObject(obj.id, obj.name, obj, GhostObjectMarker(obj, self.server, None ), status = "inactive") self.objects[obj.name] = ghost else: print 'object was unknown so far, now its inactive' ghost = EnvironmentObject(obj.id, obj.name, obj, GhostObjectMarker(obj, self.server, None ), status = "inactive") self.objects[obj.name] = ghost rospy.loginfo("SpatialEnv SRVs: show_objects - done") return ActivateObjectsResponse() def refresh_objects(self, req): rospy.loginfo("SpatialEnv SRVs: reactivate_objects") print req.ids active_req = ActivateObjectsRequest() inactive_req = ActivateObjectsRequest() for key in self.objects.keys(): if self.objects[key].id in req.ids: if self.objects[key].status == "active": active_req.ids.append( self.objects[key].id ) elif self.objects[key].status == "inactive": inactive_req.ids.append( self.objects[key].id ) self.activate_objects(active_req) self.show_objects(inactive_req) res = ActivateObjectsResponse() rospy.loginfo("SpatialEnv SRVs: reactivate_objects - done") return res def refresh_all_objects(self, req): rospy.loginfo("SpatialEnv SRVs: reactivate_all_objects") active_req = ActivateObjectsRequest() inactive_req = ActivateObjectsRequest() for key in self.objects.keys(): if self.objects[key].status == "active": active_req.ids.append( self.objects[key].id ) elif self.objects[key].status == "inactive": inactive_req.ids.append( self.objects[key].id ) self.activate_objects(active_req) self.show_objects(inactive_req) res = ActivateAllObjectsResponse() rospy.loginfo("SpatialEnv SRVs: reactivate_all_objects - done") return res def deactivate_objects(self, req): rospy.loginfo("SpatialEnv SRVs: deactivate_objects") res = DeactivateObjectsResponse() inactive_req = ActivateObjectsRequest() for key in self.objects.keys(): if self.objects[key].id in req.ids: del self.objects[key] self.show_objects(req) self.publishTF(None) return res def deactivate_all_objects(self, req): rospy.loginfo("SpatialEnv SRVs: deactivate_all_objects") self.objects = {} self.publishTF(None) self.show_all_objects(req) return DeactivateAllObjectsResponse() def activate_all_objects(self, req): rospy.loginfo("SpatialEnv SRVs: activate_all_objects") ids = db().query(ObjectInstance.id).all() req = ActivateObjectsRequest() req.ids = [id for id, in ids] self.activate_objects(req) res = ActivateAllObjectsResponse() rospy.loginfo("SpatialEnv SRVs: activate_all_objects - done") return res def show_all_objects(self, req): rospy.loginfo("SpatialEnv SRVs: show_all_objects") ids = db().query(ObjectInstance.id).all() req = ActivateObjectsRequest() req.ids = [id for id, in ids] self.show_objects(req) res = ActivateAllObjectsResponse() rospy.loginfo("SpatialEnv SRVs: show_all_objects - done") return res def publishTF(self, msg): if len(self.objects) > 0: for key in self.objects.keys(): if self.objects[key].status == "active": object = self.objects[key].object translation = object.pose.pose.position rotation = object.pose.pose.orientation time = rospy.Time.now() #print 'publish tf' self.tf_broadcaster.sendTransform( (translation.x, translation.y, translation.z), \ (rotation.x, rotation.y, rotation.z, rotation.w), \ time, object.name, object.pose.header.frame_id) def activate_robot_surroundings(self, msg): #if activate_robot_surroundings: try: (trans,rot) = self.tf_listener.lookupTransform('/world', '/base_link', rospy.Time(0)) print 'current robot position', trans except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): print 'could not determine current robot position' #else: # robot inactive def createObjectCb(self, pose): print 'Create Object from RViz pose' app = QApplication(sys.argv) widget = ChooseObjectDescriptionWidget(0) widget.show() app.exec_() desc_name, desc_id = widget.getChoice() del app, widget if(desc_id == -1): new_desc = ROSObjectDescription() new_desc.type = desc_name res = call_add_object_descriptions( [ new_desc ] ) print res desc_id = res.ids[0] obj = ROSObjectInstance() obj.description.id = desc_id obj.pose = pose res = call_add_object_instances( [obj] ) call_activate_objects( res.ids ) def activateObjectsInObjects(self, req): res = call_get_objects_in_objects(req.reference, req.target, req.fully_within) ids = [] for pair in res.pairs: if pair.reference_id not in ids: ids.append(pair.reference_id) if pair.target_id not in ids: ids.append(pair.target_id) call_activate_objects( ids ) return res def showObjectsInObjects(self, req): res = call_get_objects_in_objects(req.reference, req.target, req.fully_within) ids = [] for pair in res.pairs: if pair.reference_id not in ids: ids.append(pair.reference_id) if pair.target_id not in ids: ids.append(pair.target_id) call_show_objects( ids ) return res def activateObjectsOnObjects(self, req): print 'GOT SO FAR' res = call_get_objects_on_objects(req.reference_object_types, req.target_object_types, req.threshold) print 'NOW WHAT' ids = [] for pair in res.pairs: #if pair.reference_id not in ids: # ids.append(pair.reference_id) if pair.target_id not in ids: ids.append(pair.target_id) call_activate_objects( ids ) return res def showObjectsOnObjects(self, req): print 'GOT SO FAR' res = call_get_objects_on_objects(req.reference_object_types, req.target_object_types, req.threshold) print 'NOW WHAT' ids = [] for pair in res.pairs: #if pair.reference_id not in ids: # ids.append(pair.reference_id) if pair.target_id not in ids: ids.append(pair.target_id) call_show_objects( ids ) return res def findObjectWithinRange2DCb(self, point_stamped): distance = 2.0 print 'find Objects Within Range2D' #res = call_get_objects_within_range2d([], "Position2D", point_stamped.point, distance, fully_within = False) #res = call_get_objects_within_range2d([], "FootprintBox", point_stamped.point, distance, fully_within = False) #res = call_get_objects_within_range2d(["ConferenceTable", "ConferenceChair"], "Position2D", point_stamped.point, 3.0) #call_activate_objects( res.ids ) marker = Marker() marker.header.frame_id = "world" marker.header.stamp = rospy.get_rostime() marker.ns = "2d_range_query_marker" marker.id = 0 marker.type = 3 #cylinder marker.scale.x = distance*2 marker.scale.y = distance*2 marker.scale.z = 0.01 marker.pose.position = point_stamped.point marker.pose.position.z = 0.0 marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.color.a = 0.5 self.range_query2d_marker.publish(marker) def findObjectWithinRangeCb(self, point_stamped): distance = 1.5 res = call_get_objects_within_range(point_stamped.point, [], "FootprintHull", distance, fully_within = False) ids = [] array = MarkerArray() marker = Marker() marker.header.frame_id = "world" marker.header.stamp = rospy.get_rostime() marker.ns = "range" marker.id = 0 marker.type = 3 #sphere marker.scale.x = distance*2 marker.scale.y = distance*2 marker.scale.z = 0.005 marker.pose.position = point_stamped.point marker.pose.position.z = 0.0 marker.color.r = 0.25 marker.color.g = 0.25 marker.color.b = 0.75 marker.color.a = 0.5 array.markers.append(marker) lines = Marker() lines.header.frame_id = "world" lines.header.stamp = rospy.get_rostime() lines.ns = "lines" lines.id = 0 lines.type = 5 #line list lines.scale.x = 0.05 lines.scale.y = 0.0 lines.scale.z = 0.0 lines.color.r = 0.75 lines.color.g = 0.750 lines.color.b = 0.750 lines.color.a = 1.0 for pair in res.pairs: lines.points += pair.min_dist_line lines.points += pair.max_dist_line if pair.reference_id not in ids: ids.append(pair.reference_id) if pair.target_id not in ids: ids.append(pair.target_id) lines.colors.append(lines.color) text = Marker() text.header.frame_id = "world" text.header.stamp = rospy.get_rostime() text.ns = "distances" text.id = 0 text.type = 9 #line list text.pose.position.x = (pair.min_dist_line[1].x + pair.min_dist_line[0].x)/2 text.pose.position.y = (pair.min_dist_line[1].y + pair.min_dist_line[0].y)/2 text.pose.position.z = (pair.min_dist_line[1].z + pair.min_dist_line[0].z)/2 + 0.05 text.scale.z = 0.2 text.color.r = 1.0 text.color.g = 1.0 text.color.b = 1.0 text.color.a = 1.0 text.text = str(round(pair.min_dist,3)) array.markers.append(text) array.markers.append(lines) id = 0 for m in array.markers: m.id = id id += 1 self.range_query_marker.publish(array) call_activate_objects( ids ) def showObjectWithinRange(self, req): res = call_get_objects_within_range(req.reference_point, req.target_object_types, req.target_object_geometry_type, req.distance, req.fully_within) ids = [] array = MarkerArray() marker = Marker() marker.header.frame_id = "world" marker.header.stamp = rospy.get_rostime() marker.ns = "range" marker.id = 0 marker.type = 3 #sphere marker.scale.x = req.distance*2 marker.scale.y = req.distance*2 marker.scale.z = 0.005 marker.pose.position = req.reference_point marker.pose.position.z = 0.0 marker.color.r = 0.25 marker.color.g = 0.25 marker.color.b = 0.75 marker.color.a = 0.5 array.markers.append(marker) lines = Marker() lines.header.frame_id = "world" lines.header.stamp = rospy.get_rostime() lines.ns = "lines" lines.id = 0 lines.type = 5 #line list lines.scale.x = 0.05 lines.scale.y = 0.0 lines.scale.z = 0.0 lines.color.r = 0.75 lines.color.g = 0.750 lines.color.b = 0.750 lines.color.a = 1.0 for pair in res.pairs: lines.points += pair.min_dist_line lines.points += pair.max_dist_line if pair.reference_id not in ids: ids.append(pair.reference_id) if pair.target_id not in ids: ids.append(pair.target_id) lines.colors.append(lines.color) text = Marker() text.header.frame_id = "world" text.header.stamp = rospy.get_rostime() text.ns = "distances" text.id = 0 text.type = 9 #line list text.pose.position.x = (pair.min_dist_line[1].x + pair.min_dist_line[0].x)/2 text.pose.position.y = (pair.min_dist_line[1].y + pair.min_dist_line[0].y)/2 text.pose.position.z = (pair.min_dist_line[1].z + pair.min_dist_line[0].z)/2 + 0.05 text.scale.z = 0.2 text.color.r = 1.0 text.color.g = 1.0 text.color.b = 1.0 text.color.a = 1.0 text.text = str(round(pair.min_dist,3)) array.markers.append(text) array.markers.append(lines) id = 0 for m in array.markers: m.id = id id += 1 self.range_query_marker.publish(array) call_activate_objects( ids ) return res def findObjectWithinVolumeCb(self, polygon_stamped): extrude = call_extrude_polygon(polygon_stamped.polygon, 0.0, 0.0, 0.9) mesh = PolygonMeshStamped() mesh.header.frame_id = "world" mesh.mesh = extrude.mesh print extrude.mesh res = call_get_objects_within_volume(extrude.mesh, ['Mug','Teapot','Monitor','Keyboard'], 'BoundingBox', True) ids=[] for pair in res.pairs: if pair.reference_id not in ids: ids.append(pair.reference_id) if pair.target_id not in ids: ids.append(pair.target_id) call_activate_objects( ids ) array = MarkerArray() pose = ROSPoseStamped() pose.header.frame_id = 'world' for polygon in extrude.mesh.polygons: poly = Polygon() for point in polygon.vertex_indices: poly.points.append(extrude.mesh.vertices[point]) geo_marker = create_polygon_marker("VolumeQuery", pose, [0,1.0,0,1.0], [0.01, 0.01, 0.01], poly) array.markers.append(geo_marker) id = 0 for m in array.markers: m.header.frame_id = 'world' m.id = id id += 1 self.volume_query_marker.publish(array) def showObjectWithinVolume(self, req): res = call_get_objects_within_volume(req.reference_mesh, req.target_object_types, req.target_object_geometry_type, req.fully_within) ids=[] for pair in res.pairs: if pair.reference_id not in ids: ids.append(pair.reference_id) if pair.target_id not in ids: ids.append(pair.target_id) call_activate_objects( ids ) array = MarkerArray() pose = ROSPoseStamped() pose.header.frame_id = 'world' for polygon in req.reference_mesh.polygons: poly = Polygon() for point in polygon.vertex_indices: poly.points.append(req.reference_mesh.vertices[point]) geo_marker = create_polygon_marker("VolumeQuery", pose, [0,1.0,0,1.0], [0.01, 0.01, 0.01], poly) array.markers.append(geo_marker) id = 0 for m in array.markers: m.header.frame_id = 'world' m.id = id id += 1 self.volume_query_marker.publish(array) return res def findObjectWithinPolygonCb(self, polygon_stamped): res = call_get_objects_within_area(polygon_stamped.polygon, ["Chair"], "Position2D", fully_within = False) ids=[] for pair in res.pairs: if pair.reference_id not in ids: ids.append(pair.reference_id) if pair.target_id not in ids: ids.append(pair.target_id) call_activate_objects( ids ) self.area_query_marker.publish(polygon_stamped) return res def copyObjectWithinPolygonCb(self, polygon_stamped): res = call_get_objects_within_area(polygon_stamped.polygon, [], "Position2D", fully_within = False) ids=[] for pair in res.pairs: if pair.reference_id not in ids: ids.append(pair.reference_id) if pair.target_id not in ids: ids.append(pair.target_id) print 'COPY' copy_res = call_copy_object_instances(ids) if len( copy_res.ids ) > 1: print 'copy_res', copy_res obj_res = call_get_object_instances( [copy_res.ids[0]] ) print 'bind to ', obj_res.objects[0].name frame = obj_res.objects[0].name for id in copy_res.ids: if id != obj_res.objects[0].id: print 'bind', id, 'to', obj_res.objects[0].id call_change_frame(id, frame, False) call_activate_objects( copy_res.ids ) call_refresh_objects( copy_res.ids ) return res def showObjectWithinArea(self, req): res = call_get_objects_within_area(req.reference_polygon, req.target_object_types, req.target_object_geometry_type, req.fully_within) ids=[] for pair in res.pairs: if pair.reference_id not in ids: ids.append(pair.reference_id) if pair.target_id not in ids: ids.append(pair.target_id) call_activate_objects( ids ) polygon_stamped = PolygonStamped() polygon_stamped.header.frame_id = "world" polygon_stamped.polygon = req.reference_polygon self.area_query_marker.publish(polygon_stamped) return res def showDistanceBetweenObjects(self, req): res = call_get_distance_between_objects(req.reference_object_types, req.reference_object_geometry_type, req.target_object_types, req.target_object_geometry_type, req.min_range, req.max_range, req.sort_descending, req.max_distance, return_points = True) array = MarkerArray() lines = Marker() lines.header.frame_id = "world" lines.header.stamp = rospy.get_rostime() lines.ns = "lines" lines.id = 0 lines.type = 5 #line list lines.scale.x = 0.05 lines.scale.y = 0.0 lines.scale.z = 0.0 lines.color.r = 0.75 lines.color.g = 0.750 lines.color.b = 0.750 lines.color.a = 1.0 ids =[] for pair in res.pairs: lines.points += pair.min_dist_line lines.points += pair.max_dist_line if pair.reference_id not in ids: ids.append(pair.reference_id) if pair.target_id not in ids: ids.append(pair.target_id) lines.colors.append(lines.color) text = Marker() text.header.frame_id = "world" text.header.stamp = rospy.get_rostime() text.ns = "distances" text.id = 0 text.type = 9 #line list text.pose.position.x = (pair.min_dist_line[1].x + pair.min_dist_line[0].x)/2 text.pose.position.y = (pair.min_dist_line[1].y + pair.min_dist_line[0].y)/2 text.pose.position.z = (pair.min_dist_line[1].z + pair.min_dist_line[0].z)/2 + 0.05 text.scale.z = 0.2 text.color.r = 1.0 text.color.g = 1.0 text.color.b = 1.0 text.color.a = 1.0 text.text = str(round(pair.min_dist,3)) array.markers.append(text) array.markers.append(lines) id = 0 for m in array.markers: m.id = id id += 1 self.range_query_marker.publish(array) call_activate_objects( ids ) return res def list_active_objects(self, msg): for key in self.objects.keys(): active = active_objects[key] print 'ID :', active.id print 'TYPE :', active.object.description.type print 'NAME :', active.object.name print 'ALIAS :', active.object.alias print 'POSE :', active.object.pose
class PlaceObject: def __init__(self): self.arm = rospy.get_param("arm", "r") self.tl = TransformListener() self.perception_srv = rospy.Service( "/clickable_world/place_table_perceive", PerceiveButtons, self.do_perception ) self.action_srv = rospy.Service("/clickable_world/place_object", ButtonAction, self.do_action) self.pc_capture_srv = rospy.ServiceProxy("/table_detection/surf_seg_capture_pc", std_srvs.srv.Empty) self.table_seg_srv = rospy.ServiceProxy("/table_detection/segment_surfaces", SegmentSurfaces) self.grasp_client = actionlib.SimpleActionClient(self.arm + "_overhead_grasp", OverheadGraspAction) self.grasp_client.wait_for_server() self.grasp_setup_client = actionlib.SimpleActionClient( self.arm + "_overhead_grasp_setup", OverheadGraspSetupAction ) self.grasp_setup_client.wait_for_server() def do_perception(self, req): rospy.loginfo("[place_object] Perceiving table...") # capture a few clouds rate = rospy.Rate(5) for i in range(5): self.pc_capture_srv() rate.sleep() # segment surfaces self.surfaces = self.table_seg_srv().surfaces for i in range(len(self.surfaces)): self.surfaces[i].color.r = 256 self.surfaces[i].color.g = 0 self.surfaces[i].color.b = 256 self.surfaces[i].color.a = 256 resp = PerceiveButtonsResponse() resp.buttons = self.surfaces return resp def do_action(self, req): rospy.loginfo("[place_object] Table clicked, placing object...") # put 3d pt in grasping frame req.pixel3d.header.stamp = rospy.Time(0) place_pt = self.tl.transformPoint("/torso_lift_link", req.pixel3d) # move to place setup position setup_goal = OverheadGraspSetupGoal() setup_goal.disable_head = False setup_goal.open_gripper = False self.grasp_setup_client.send_goal(setup_goal) self.grasp_setup_client.wait_for_result() ############################################################ # Creating place goal grasp_goal = OverheadGraspGoal() grasp_goal.is_grasp = False grasp_goal.disable_head = False grasp_goal.disable_coll = False grasp_goal.grasp_type = OverheadGraspGoal.MANUAL_GRASP grasp_goal.x = place_pt.point.x grasp_goal.y = place_pt.point.y grasp_goal.behavior_name = "overhead_grasp" grasp_goal.sig_level = 0.999 ############################################################ # place object self.grasp_client.send_goal(grasp_goal) self.grasp_client.wait_for_result() result = self.grasp_client.get_result() rospy.loginfo("[place_object] Place result: %s" % result) obj_in_hand = result.grasp_result == "Object placed" # go back to grasp setup setup_goal.open_gripper = True self.grasp_setup_client.send_goal(setup_goal) self.grasp_setup_client.wait_for_result() return ButtonActionResponse()
class MoveGroupInterface(object): ## @brief Constructor for this utility ## @param group Name of the MoveIt! group to command ## @param frame Name of the fixed frame in which planning happens ## @param listener A TF listener instance (optional, will create a new one if None) ## @param plan_only Should we only plan, but not execute? def __init__(self, group, frame, listener=None, plan_only=False): self._group = group self._fixed_frame = frame self._action = actionlib.SimpleActionClient('move_group', MoveGroupAction) self._action.wait_for_server() if listener == None: self._listener = TransformListener() else: self._listener = listener self.plan_only = plan_only self.planner_id = None self.planning_time = 15.0 def get_move_action(self): return self._action ## @brief Move the arm to set of joint position goals def moveToJointPosition(self, joints, positions, tolerance=0.01, wait=True, **kwargs): # Check arguments supported_args = ("max_velocity_scaling_factor", "planner_id", "planning_scene_diff", "planning_time", "plan_only", "start_state") for arg in kwargs.keys(): if not arg in supported_args: rospy.loginfo("moveToJointPosition: unsupported argument: %s", arg) # Create goal g = MoveGroupGoal() # 1. fill in workspace_parameters # 2. fill in start_state try: g.request.start_state = kwargs["start_state"] except KeyError: g.request.start_state.is_diff = True # 3. fill in goal_constraints c1 = Constraints() for i in range(len(joints)): c1.joint_constraints.append(JointConstraint()) c1.joint_constraints[i].joint_name = joints[i] c1.joint_constraints[i].position = positions[i] c1.joint_constraints[i].tolerance_above = tolerance c1.joint_constraints[i].tolerance_below = tolerance c1.joint_constraints[i].weight = 1.0 g.request.goal_constraints.append(c1) # 4. fill in path constraints # 5. fill in trajectory constraints # 6. fill in planner id try: g.request.planner_id = kwargs["planner_id"] except KeyError: if self.planner_id: g.request.planner_id = self.planner_id # 7. fill in group name g.request.group_name = self._group # 8. fill in number of planning attempts try: g.request.num_planning_attempts = kwargs["num_attempts"] except KeyError: g.request.num_planning_attempts = 1 # 9. fill in allowed planning time try: g.request.allowed_planning_time = kwargs["planning_time"] except KeyError: g.request.allowed_planning_time = self.planning_time # Fill in velocity scaling factor try: g.request.max_velocity_scaling_factor = kwargs["max_velocity_scaling_factor"] except KeyError: pass # do not fill in at all # 10. fill in planning options diff try: g.planning_options.planning_scene_diff = kwargs["planning_scene_diff"] except KeyError: g.planning_options.planning_scene_diff.is_diff = True g.planning_options.planning_scene_diff.robot_state.is_diff = True # 11. fill in planning options plan only try: g.planning_options.plan_only = kwargs["plan_only"] except KeyError: g.planning_options.plan_only = self.plan_only # 12. fill in other planning options g.planning_options.look_around = False g.planning_options.replan = False # 13. send goal self._action.send_goal(g) if wait: self._action.wait_for_result() return self._action.get_result() else: return None ## @brief Move the arm, based on a goal pose_stamped for the end effector. def moveToPose(self, pose_stamped, gripper_frame, tolerance=0.01, wait=True, **kwargs): # Check arguments supported_args = ("max_velocity_scaling_factor", "planner_id", "planning_time", "plan_only", "start_state") for arg in kwargs.keys(): if not arg in supported_args: rospy.loginfo("moveToJointPosition: unsupported argument: %s", arg) # Create goal g = MoveGroupGoal() pose_transformed = self._listener.transformPose(self._fixed_frame, pose_stamped) # 1. fill in request workspace_parameters # 2. fill in request start_state try: g.request.start_state = kwargs["start_state"] except KeyError: g.request.start_state.is_diff = True # 3. fill in request goal_constraints c1 = Constraints() c1.position_constraints.append(PositionConstraint()) c1.position_constraints[0].header.frame_id = self._fixed_frame c1.position_constraints[0].link_name = gripper_frame b = BoundingVolume() s = SolidPrimitive() s.dimensions = [tolerance * tolerance] s.type = s.SPHERE b.primitives.append(s) b.primitive_poses.append(pose_transformed.pose) c1.position_constraints[0].constraint_region = b c1.position_constraints[0].weight = 1.0 c1.orientation_constraints.append(OrientationConstraint()) c1.orientation_constraints[0].header.frame_id = self._fixed_frame c1.orientation_constraints[0].orientation = pose_transformed.pose.orientation c1.orientation_constraints[0].link_name = gripper_frame c1.orientation_constraints[0].absolute_x_axis_tolerance = tolerance c1.orientation_constraints[0].absolute_y_axis_tolerance = tolerance c1.orientation_constraints[0].absolute_z_axis_tolerance = tolerance c1.orientation_constraints[0].weight = 1.0 g.request.goal_constraints.append(c1) # 4. fill in request path constraints # 5. fill in request trajectory constraints # 6. fill in request planner id try: g.request.planner_id = kwargs["planner_id"] except KeyError: if self.planner_id: g.request.planner_id = self.planner_id # 7. fill in request group name g.request.group_name = self._group # 8. fill in request number of planning attempts try: g.request.num_planning_attempts = kwargs["num_attempts"] except KeyError: g.request.num_planning_attempts = 1 # 9. fill in request allowed planning time try: g.request.allowed_planning_time = kwargs["planning_time"] except KeyError: g.request.allowed_planning_time = self.planning_time # Fill in velocity scaling factor try: g.request.max_velocity_scaling_factor = kwargs["max_velocity_scaling_factor"] except KeyError: pass # do not fill in at all # 10. fill in planning options diff g.planning_options.planning_scene_diff.is_diff = True g.planning_options.planning_scene_diff.robot_state.is_diff = True # 11. fill in planning options plan only try: g.planning_options.plan_only = kwargs["plan_only"] except KeyError: g.planning_options.plan_only = self.plan_only # 12. fill in other planning options g.planning_options.look_around = False g.planning_options.replan = False # 13. send goal self._action.send_goal(g) if wait: self._action.wait_for_result() return self._action.get_result() else: return None ## @brief Sets the planner_id used for all future planning requests. ## @param planner_id The string for the planner id, set to None to clear def setPlannerId(self, planner_id): self.planner_id = str(planner_id) ## @brief Set default planning time to be used for future planning request. def setPlanningTime(self, time): self.planning_time = time
class GraspObjButton: def __init__(self): self.arm = rospy.get_param("arm", "r") self.tl = TransformListener() self.perception_srv = rospy.Service("/clickable_world/detect_objects", PerceiveButtons, self.do_perception) self.action_srv = rospy.Service("/clickable_world/grasp_object", ButtonAction, self.do_action) self.obj_seg_srv = rospy.ServiceProxy("/object_button_detect", ObjectButtonDetect) self.grasp_client = actionlib.SimpleActionClient(self.arm + '_overhead_grasp', OverheadGraspAction) self.grasp_client.wait_for_server() self.grasp_setup_client = actionlib.SimpleActionClient(self.arm + '_overhead_grasp_setup', OverheadGraspSetupAction) self.grasp_setup_client.wait_for_server() def do_perception(self, req): rospy.loginfo("[grasp_obj_button] Perceiving object buttons...") # perceive object on table self.objects = self.obj_seg_srv().objects for i in range(len(self.objects)): self.objects[i].color.r = 256 self.objects[i].color.g = 0 self.objects[i].color.b = 0 self.objects[i].color.a = 256 # return object buttons resp = PerceiveButtonsResponse() resp.buttons = self.objects return resp def do_action(self, req): rospy.loginfo("[grasp_obj_button] Table clicked, grasping object...") # put 3d pt in grasping frame req.pixel3d.header.stamp = rospy.Time(0) grasp_pt = self.tl.transformPoint("/torso_lift_link", req.pixel3d) # move to grasp setup position setup_goal = OverheadGraspSetupGoal() setup_goal.disable_head = False setup_goal.open_gripper = True self.grasp_setup_client.send_goal(setup_goal) self.grasp_setup_client.wait_for_result() ############################################################ # Creating grasp goal grasp_goal = OverheadGraspGoal() grasp_goal.is_grasp = True grasp_goal.disable_head = False grasp_goal.disable_coll = False grasp_goal.grasp_type=OverheadGraspGoal.VISION_GRASP grasp_goal.x = grasp_pt.point.x grasp_goal.y = grasp_pt.point.y grasp_goal.behavior_name = "overhead_grasp" grasp_goal.sig_level = 0.999 ############################################################ # grasp object self.grasp_client.send_goal(grasp_goal) self.grasp_client.wait_for_result() result = self.grasp_client.get_result() rospy.loginfo("[grasp_obj_button] Grasp result: %s" % result) obj_in_hand = result.grasp_result == "Object grasped" # go back to grasp setup setup_goal.open_gripper = False self.grasp_setup_client.send_goal(setup_goal) self.grasp_setup_client.wait_for_result() return ButtonActionResponse()
class NavHeadController: def __init__(self): self.last_vel_time = rospy.Time(0) # pose and lock self.x = 1.0 self.y = 0.0 self.mutex = Lock() self.listener = TransformListener() self.client = actionlib.SimpleActionClient("head_controller/point_head", PointHeadAction) self.client.wait_for_server() self.plan_sub = rospy.Subscriber("/safe_teleop_base/local_plan", Path, self.planCallback) self.vel_sub = rospy.Subscriber("/teleop/cmd_vel/unsafe", Twist, self.velCallback) def velCallback(self, msg): vel = msg.linear.x * msg.linear.x vel += msg.linear.y * msg.linear.y vel += msg.angular.z * msg.angular.z if vel > 0.0: self.last_vel_time = rospy.Time.now() def planCallback(self, msg): # get the goal pose_stamped = msg.poses[-1] pose = pose_stamped.pose # look ahead one meter R = quaternion_matrix([pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]) point = [1, 0, 0, 1] M = R*point p = PointStamped() p.header.frame_id = pose_stamped.header.frame_id p.header.stamp = rospy.Time(0) p.point.x += pose_stamped.pose.position.x + M[0,0] p.point.y += pose_stamped.pose.position.y + M[1,0] p.point.z += pose_stamped.pose.position.z + M[2,0] # transform to base_link p = self.listener.transformPoint("base_link", p) # update with self.mutex: if p.point.x < 0.65: self.x = 0.65 else: self.x = p.point.x if p.point.y > 0.5: self.y = 0.5 elif p.point.y < -0.5: self.y = -0.5 else: self.y = p.point.y def loop(self): while not rospy.is_shutdown(): if abs((rospy.Time.now() - self.last_vel_time).to_sec()) < 1.0: goal = PointHeadGoal() goal.target.header.stamp = rospy.Time.now() goal.target.header.frame_id = "base_link" with self.mutex: goal.target.point.x = self.x goal.target.point.y = self.y self.x = 1 self.y = 0 goal.target.point.z = 0.0 goal.min_duration = rospy.Duration(1.0) self.client.send_goal(goal) self.client.wait_for_result() with self.mutex: goal.target.point.x = self.x goal.target.point.y = self.y self.x = 1 self.y = 0 goal.target.point.z = 0.75 self.client.send_goal(goal) self.client.wait_for_result() else: rospy.sleep(1.0)
def __init__(self): self.setup_node() self.server = InteractiveMarkerServer("semap_environment") self.tf_broadcaster = TransformBroadcaster() self.tf_listener = TransformListener()