Python std_srvs.srv.Empty() Examples

def __init__(self):
        # ROS initialization:
        rospy.init_node('pose_manager')

        self.poseLibrary = dict()
        self.readInPoses()
        self.poseServer = actionlib.SimpleActionServer("body_pose", BodyPoseAction,
                                                       execute_cb=self.executeBodyPose,
                                                       auto_start=False)
        self.trajectoryClient = actionlib.SimpleActionClient("joint_trajectory", JointTrajectoryAction)
        if self.trajectoryClient.wait_for_server(rospy.Duration(3.0)):
            try:
                rospy.wait_for_service("stop_walk_srv", timeout=2.0)
                self.stopWalkSrv = rospy.ServiceProxy("stop_walk_srv", Empty)
            except:
                rospy.logwarn("stop_walk_srv not available, pose_manager will not stop the walker before executing a trajectory. "
                          +"This is normal if there is no nao_walker running.")
            self.stopWalkSrv = None
            self.poseServer.start()

            rospy.loginfo("pose_manager running, offering poses: %s", self.poseLibrary.keys());

        else:
            rospy.logfatal("Could not connect to required \"joint_trajectory\" action server, is the nao_controller node running?");
            rospy.signal_shutdown("Required component missing"); 

def on_button_reset_pressed(self):
    self._num_pattern_detections = 0
    self._calibration_output = ""
    self._update_required = True

    self.button_start.setEnabled( False )

    try:
      rospy.wait_for_service('dvs_calibration/reset', 1)
      try:
        reset_service = rospy.ServiceProxy('dvs_calibration/reset', Empty)
        resp = reset_service()
      except rospy.ServiceException, e:
        print "Service call failed: %s"%e
    except:
      print "service not available..."
      pass 

def service_server():
    rospy.init_node('service_server')
    s = rospy.Service('call_me', Empty, handle_service)
    print "Ready to serve."
    rospy.spin() 

def stop(self):
        """Stop the sensor.
        """
        # Check that everything is running
        if not self._running:
            logging.warning('PhoXi not running. Aborting stop')
            return False

        # Stop the subscribers
        self._color_im_sub.unregister()
        self._depth_im_sub.unregister()
        self._normal_map_sub.unregister()

        # Disconnect from the camera
        rospy.ServiceProxy('phoxi_camera/disconnect_camera', Empty)()

        self._running = False

        return True 

def call_service():
    rospy.loginfo('waiting service')
    rospy.wait_for_service('call_me')

    try:
        service = rospy.ServiceProxy('call_me', Empty)
        response = service()
    except rospy.ServiceException, e:
        print "Service call failed: %s" % e 

def __init__(self, crazyflies_yaml="../launch/crazyflies.yaml"):
        """Initialize the server. Waits for all ROS services before returning.

        Args:
            timeHelper (TimeHelper): TimeHelper instance.
            crazyflies_yaml (str): If ends in ".yaml", interpret as a path and load
                from file. Otherwise, interpret as YAML string and parse
                directly from string.
        """
        rospy.init_node("CrazyflieAPI", anonymous=False)
        rospy.wait_for_service("/emergency")
        self.emergencyService = rospy.ServiceProxy("/emergency", Empty)
        rospy.wait_for_service("/takeoff")
        self.takeoffService = rospy.ServiceProxy("/takeoff", Takeoff)
        rospy.wait_for_service("/land")
        self.landService = rospy.ServiceProxy("/land", Land)
        # rospy.wait_for_service("/stop")
        # self.stopService = rospy.ServiceProxy("/stop", Stop)
        rospy.wait_for_service("/go_to")
        self.goToService = rospy.ServiceProxy("/go_to", GoTo)
        rospy.wait_for_service("/start_trajectory");
        self.startTrajectoryService = rospy.ServiceProxy("/start_trajectory", StartTrajectory)
        rospy.wait_for_service("/update_params")
        self.updateParamsService = rospy.ServiceProxy("/update_params", UpdateParams)

        if crazyflies_yaml.endswith(".yaml"):
            with open(crazyflies_yaml, 'r') as ymlfile:
                cfg = yaml.load(ymlfile)
        else:
            cfg = yaml.load(crazyflies_yaml)

        self.tf = TransformListener()

        self.crazyflies = []
        self.crazyfliesById = dict()
        for crazyflie in cfg["crazyflies"]:
            id = int(crazyflie["id"])
            initialPosition = crazyflie["initialPosition"]
            cf = Crazyflie(id, initialPosition, self.tf)
            self.crazyflies.append(cf)
            self.crazyfliesById[id] = cf 

def cmdStop(self):
        """Interrupts any high-level command to stop and cut motor power.

        Intended for non-emergency scenarios, e.g. landing with the possibility
        of taking off again later. Future low- or high-level commands will
        restart the motors. Equivalent of :meth:`stop()` when in high-level mode.
        """
        self.cmdStopPublisher.publish(std_msgs.msg.Empty()) 

def __init__(self, action_size):
        self.goal_x = 0
        self.goal_y = 0
        self.heading = 0
        self.action_size = action_size
        self.initGoal = True
        self.get_goalbox = False
        self.position = Pose()
        self.pub_cmd_vel = rospy.Publisher('cmd_vel', Twist, queue_size=5)
        self.sub_odom = rospy.Subscriber('odom', Odometry, self.getOdometry)
        self.reset_proxy = rospy.ServiceProxy('gazebo/reset_simulation', Empty)
        self.unpause_proxy = rospy.ServiceProxy('gazebo/unpause_physics', Empty)
        self.pause_proxy = rospy.ServiceProxy('gazebo/pause_physics', Empty)
        self.respawn_goal = Respawn() 

def __init__(self, action_size):
        self.goal_x = 0
        self.goal_y = 0
        self.heading = 0
        self.action_size = action_size
        self.initGoal = True
        self.get_goalbox = False
        self.position = Pose()
        self.pub_cmd_vel = rospy.Publisher('cmd_vel', Twist, queue_size=5)
        self.sub_odom = rospy.Subscriber('odom', Odometry, self.getOdometry)
        self.reset_proxy = rospy.ServiceProxy('gazebo/reset_simulation', Empty)
        self.unpause_proxy = rospy.ServiceProxy('gazebo/unpause_physics', Empty)
        self.pause_proxy = rospy.ServiceProxy('gazebo/pause_physics', Empty)
        self.respawn_goal = Respawn() 

def __init__(self, action_size):
        self.goal_x = 0
        self.goal_y = 0
        self.heading = 0
        self.action_size = action_size
        self.initGoal = True
        self.get_goalbox = False
        self.position = Pose()
        self.pub_cmd_vel = rospy.Publisher('cmd_vel', Twist, queue_size=5)
        self.sub_odom = rospy.Subscriber('odom', Odometry, self.getOdometry)
        self.reset_proxy = rospy.ServiceProxy('gazebo/reset_simulation', Empty)
        self.unpause_proxy = rospy.ServiceProxy('gazebo/unpause_physics', Empty)
        self.pause_proxy = rospy.ServiceProxy('gazebo/pause_physics', Empty)
        self.respawn_goal = Respawn() 

def __init__(self, action_size):
        self.goal_x = 0
        self.goal_y = 0
        self.heading = 0
        self.action_size = action_size
        self.initGoal = True
        self.get_goalbox = False
        self.position = Pose()
        self.pub_cmd_vel = rospy.Publisher('cmd_vel', Twist, queue_size=5)
        self.sub_odom = rospy.Subscriber('odom', Odometry, self.getOdometry)
        self.reset_proxy = rospy.ServiceProxy('gazebo/reset_simulation', Empty)
        self.unpause_proxy = rospy.ServiceProxy('gazebo/unpause_physics', Empty)
        self.pause_proxy = rospy.ServiceProxy('gazebo/pause_physics', Empty)
        self.respawn_goal = Respawn() 

def on_button_save_calibration_pressed(self):
    try:
      rospy.wait_for_service('dvs_calibration/save', 1)
      try:
        save_calibration_service = rospy.ServiceProxy('dvs_calibration/save', Empty)
        resp = save_calibration_service()
      except rospy.ServiceException, e:
        print "Service call failed: %s"%e
    except:
      print "service not available..."
      pass 

def on_button_start_calibration_pressed(self):
    self.button_start.setEnabled( False )

    try:
      rospy.wait_for_service('dvs_calibration/start', 1)
      try:
        start_calibration_service = rospy.ServiceProxy('dvs_calibration/start', Empty)
        resp = start_calibration_service()
      except rospy.ServiceException, e:
        print "Service call failed: %s"%e
    except:
      print "service not available..."
      pass 

def tare(self):
        """Zero out (tare) the sensor.
        """
        if not self._running:
            raise ValueError('Weight sensor is not running!')
        rospy.ServiceProxy('weight_sensor/tare', Empty)() 

def frames(self):
        """Retrieve a new frame from the PhoXi and convert it to a ColorImage,
        a DepthImage, and an IrImage.

        Returns
        -------
        :obj:`tuple` of :obj:`ColorImage`, :obj:`DepthImage`, :obj:`IrImage`, :obj:`numpy.ndarray`
            The ColorImage, DepthImage, and IrImage of the current frame.
        """
        # Run a software trigger
        times = []

        rospy.ServiceProxy('phoxi_camera/start_acquisition', Empty)()
        rospy.ServiceProxy('phoxi_camera/trigger_image', TriggerImage)()

        self._cur_color_im = None
        self._cur_depth_im = None
        self._cur_normal_map = None

        rospy.ServiceProxy('phoxi_camera/get_frame', GetFrame)(-1)

        max_time = 5.0
        time_waiting = 0.0
        while self._cur_color_im is None or self._cur_depth_im is None or self._cur_normal_map is None:
            time.sleep(0.05)
            time_waiting += 0.05
            if time_waiting > max_time:
                raise SensorUnresponsiveException('PhoXi sensor seems to be non-responsive')
        return self._cur_color_im, self._cur_depth_im, None 

def __init__(self, rate=20.0, id_mask='F1804'):
        """Initialize the weight publisher.

        Parameters
        ----------
        id_mask : str
            A template for the first n digits of the device IDs for valid load cells.
        """
        self._rate = rospy.Rate(rate)
        self._pub = rospy.Publisher('~weights', Float32MultiArray, queue_size=10)

        rospy.loginfo('Connecting serial')

        self._serials = self._connect(id_mask)
        if len(self._serials) == 0:
            raise ValueError('Error -- No loadstar weight sensors connected to machine!')

        # Tare the sensor
        rospy.loginfo('Tareing')
        self._tare()

        # Flush the sensor's communications
        self._flush()

        # Set up Tare service
        self._tare_service = rospy.Service('~tare', Empty, self._handle_tare)

        # Main loop -- read and publish
        while not rospy.is_shutdown():
            weights = self._read_weights()
            self._pub.publish(Float32MultiArray(data=weights))
            self._rate.sleep() 

def __init__(self):
        self.index = 0
        self.gazeboModels = None
        self.img_np = None
        self.pose = Odometry()
        self.action = Twist()

        rospy.Subscriber(overheadImageTopic, Image, self.imageCallback)
        rospy.Subscriber(huskyCmdVelTopic, Twist, self.cmdVelCallback)

        self.listener = tf.TransformListener()
        self.writer = NpzDataset('husky_data')

        self.trans = None
        self.rot = None
        self.roll, self.pitch, self.yaw = 0., 0., 0.

        self.trans_threshold = 0.25

        rospy.wait_for_service("/gazebo/pause_physics")
        self.pause = rospy.ServiceProxy("/gazebo/pause_physics", EmptySrv)
        self.unpause = rospy.ServiceProxy("/gazebo/unpause_physics", EmptySrv)
        self.set_model_state = rospy.ServiceProxy("/gazebo/set_model_state",
                SetModelState)
        self.set_link_state = rospy.ServiceProxy("/gazebo/set_link_state",
                SetLinkState)
        self.get_link_state = rospy.ServiceProxy("/gazebo/get_link_state",
                GetLinkState)
        self.pose = None 

def GetOpenGripperService():
    return GetService("/costar/gripper/open", EmptySrv) 

def GetCloseGripperService():
    return GetService("/costar/gripper/close", EmptySrv) 

def GetDetectObjectsService(srv='/costar_perception/segmenter'):
    '''
    Get a service that will update object positions

    Parameters:
    ----------
    srv: service, defaults to the correct name for costar
    '''
    return GetService(srv, EmptySrv) 

def execute(self, inputs, outputs, gvm):
    service = "/clear"
    rospy.wait_for_service(service)
    clear_turtle_area_service = rospy.ServiceProxy(service, Empty)
    resp1 = clear_turtle_area_service()
    self.logger.info("ROS external module: executed the {} service".format(service))
    return 0 

def __init__(self, maze_id=0, continuous=continuous):
        self.maze_id = maze_id
        self.continuous = continuous
        self.goal_space = config.goal_space[maze_id]
        self.start_space = config.start_space[maze_id]
        # Launch the simulation with the given launch file name
        '''
        uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
        cli_args = ['rl_nav', 'nav_gazebo.launch']  # , 'maze_id:={}'.format(self.maze_id)]
        roslaunch_args = cli_args[2:]
        roslaunch_file = [(roslaunch.rlutil.resolve_launch_arguments(cli_args), roslaunch_args)]
        launch = roslaunch.parent.ROSLaunchParent(uuid, roslaunch_file)
        launch.start()        
        '''
        uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
        roslaunch.configure_logging(uuid)
        path = rospkg.RosPack().get_path('rl_nav')  # 'rospack find' python api, find the path of rl_nav package
        self.launch = roslaunch.parent.ROSLaunchParent(uuid, [path + '/launch/nav_gazebo.launch'])
        self.launch.start()

        rospy.init_node('env_node')
        time.sleep(10)  # Wait for gzserver to launch

        self.vel_pub = rospy.Publisher('/mobile_base/commands/velocity', Twist, queue_size=5)
        self.unpause = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
        self.pause = rospy.ServiceProxy('/gazebo/pause_physics', Empty)
        self.reset_proxy = rospy.ServiceProxy('/gazebo/reset_simulation', Empty)
        self.get_state = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
        self.set_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)

        '''
        self.goal = self.goal_space[np.random.choice(len(self.goal_space))]
        start = self.start_space[np.random.choice(len(self.start_space))]
        self.set_start(start[0], start[1], np.random.uniform(0, 2*math.pi))
        d0, theta0 = self.rectangular2polar(self.goal[0]-start[0], self.goal[1]-start[1])
        self.p = [d0, theta0]  # relative target position        
        '''
        self.goal = []
        self.p = []
        self.reward = 0
        self.success = False

        self.img_height = config.input_dim[0]
        self.img_width = config.input_dim[1]
        self.img_channels = config.input_dim[2]
        self._states = dict(shape=(self.img_height, self.img_width, self.img_channels), type='float')
        self._actions = dict(num_actions=5, type='int')
        if self.continuous:
            self._actions = dict(linear_vel=dict(shape=(), type='float', min_value=0.0, max_value=1.0),
                                 angular_vel=dict(shape=(), type='float', min_value=-1.0, max_value=1.0))
        self.vel_cmd = [0., 0.] 

def run(self):
        '''
        Bring up all necessary components of the simulation. You should only
        need to do this once: after the first call to run(), you can repeatedly
        call reset() to restore the simulation to a good state.
        '''

        # ---------------------------------------------------------------------
        # Start the roscore
        roscore = subprocess.Popen(['roscore'])
        self.procs.append(roscore)
        self.sleep(1.)

        # We create a node handle so we can interact with ROS services.
        rospy.init_node('simulation_manager_node')
        
        # ---------------------------------------------------------------------
        # Start gazebo
        # This is the "launch" option -- it should dec
        gazebo = subprocess.Popen(['roslaunch',
            'costar_simulation',
            self.launch_file,
            'gui:=%s'%str(self.gui)])
        self.procs.append(gazebo)
        self.sleep(1.)

        rospy.wait_for_service('gazebo/unpause_physics')
        rospy.wait_for_service('/publish_planning_scene')
        self.pause_srv = rospy.ServiceProxy('gazebo/pause_physics',EmptySrv)
        self.unpause_srv = rospy.ServiceProxy('gazebo/unpause_physics',EmptySrv)
        self.publish_scene_srv = rospy.ServiceProxy('/publish_planning_scene',
                                                    EmptySrv)

        # ---------------------------------------------------------------------
        # Start rviz
        if self.rviz:
            rviz = subprocess.Popen(['roslaunch', 'costar_simulation', 'rviz.launch'])
            self.procs.append(rviz)
            self.sleep(3.)
        
        # ---------------------------------------------------------------------
        # Reset the simulation. This puts the robot into its initial state, and
        # is also responsible for creating and updating positions of any objects.
        self.pause()
        self.experiment.reset()
        self.sleep(1.)
        self.experiment.reset()
        self.publish_scene_srv()

        # ---------------------------------------------------------------------
        # Start controllers
        roscontrol = subprocess.Popen(['roslaunch', 'costar_simulation', 'controllers.launch'])
        self.procs.append(roscontrol)

        # ---------------------------------------------------------------------
        # Start reset service
        self.reset_srv = rospy.Service("costar_simulation/reset", EmptySrv, self.reset_srv_cb)
        self.resume()
        self.publish_scene_srv() 

def __init__(self):
        NaoqiNode.__init__(self, 'nao_walker')

        self.connectNaoQi()

        # walking pattern params:
        self.stepFrequency = rospy.get_param('~step_frequency', 0.5)

        self.useStartWalkPose = rospy.get_param('~use_walk_pose', False)
        self.needsStartWalkPose = True

        # other params
        self.maxHeadSpeed = rospy.get_param('~max_head_speed', 0.2)
        # initial stiffness (defaults to 0 so it doesn't strain the robot when no teleoperation is running)
        # set to 1.0 if you want to control the robot immediately
        initStiffness = rospy.get_param('~init_stiffness', 0.0)

        self.useFootGaitConfig = rospy.get_param('~use_foot_gait_config', False)
        rospy.loginfo("useFootGaitConfig = %d" % self.useFootGaitConfig)
        if self.useFootGaitConfig:
            self.footGaitConfig = rospy.get_param('~foot_gait_config', self.motionProxy.getFootGaitConfig("Default"))
        else:
            self.footGaitConfig = self.motionProxy.getFootGaitConfig("Default")

        # TODO: parameterize
        if initStiffness > 0.0 and initStiffness <= 1.0:
            self.motionProxy.stiffnessInterpolation('Body', initStiffness, 0.5)

        try:
            enableFootContactProtection = rospy.get_param('~enable_foot_contact_protection')
            self.motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", enableFootContactProtection]])
            if enableFootContactProtection:
                rospy.loginfo("Enabled foot contact protection")
            else:
                rospy.loginfo("Disabled foot contact protection")
        except KeyError:
            # do not change foot contact protection
            pass

        # last: ROS subscriptions (after all vars are initialized)
        rospy.Subscriber("cmd_vel", Twist, self.handleCmdVel, queue_size=1)
        rospy.Subscriber("cmd_pose", Pose2D, self.handleTargetPose, queue_size=1)
        rospy.Subscriber("cmd_step", StepTarget, self.handleStep, queue_size=50)

        # Create ROS publisher for speech
        self.pub = rospy.Publisher("speech", String, latch = True)

        # ROS services (blocking functions)
        self.cmdPoseSrv = rospy.Service("cmd_pose_srv", CmdPoseService, self.handleTargetPoseService)
        self.cmdVelSrv = rospy.Service("cmd_vel_srv", CmdVelService, self.handleCmdVelService)
        self.stepToSrv = rospy.Service("cmd_step_srv", StepTargetService, self.handleStepSrv)
        self.stopWalkSrv = rospy.Service("stop_walk_srv", Empty, self.handleStopWalkSrv)
        self.needsStartWalkPoseSrv = rospy.Service("needs_start_walk_pose_srv", Empty, self.handleNeedsStartWalkPoseSrv)
        self.readFootGaitConfigSrv = rospy.Service("read_foot_gait_config_srv", Empty, self.handleReadFootGaitConfigSrv)
        self.setArmsEnabledSrv = rospy.Service("enable_arms_walking_srv", SetArmsEnabled, self.handleSetArmsEnabledSrv)

        self.say("Walker online")

        rospy.loginfo("nao_walker initialized") 

def __init__(self):
        rospy.init_node("gazebo_keyboard_teleoperation", anonymous=True)

        # subscriber to get joint angles froim gazebo
        rospy.Subscriber("/joint_state", JointState, self._callback_joint_states)

        # listener for transoforms
        self.listener = tf.TransformListener()

        # publishers for joint angles to gazebo
        self.head_pan_pub = rospy.Publisher(
            "/head_pan_cntrl/command", Float64, queue_size=1
        )
        self.head_tilt_pub = rospy.Publisher(
            "/head_tilt_cntrl/command", Float64, queue_size=1
        )
        self.joint_1_pub = rospy.Publisher(
            "/joint_1_cntrl/command", Float64, queue_size=1
        )
        self.joint_2_pub = rospy.Publisher(
            "/joint_2_cntrl/command", Float64, queue_size=1
        )
        self.joint_3_pub = rospy.Publisher(
            "/joint_3_cntrl/command", Float64, queue_size=1
        )
        self.joint_4_pub = rospy.Publisher(
            "/joint_4_cntrl/command", Float64, queue_size=1
        )
        self.joint_5_pub = rospy.Publisher(
            "/joint_5_cntrl/command", Float64, queue_size=1
        )
        self.joint_6_pub = rospy.Publisher(
            "/joint_6_cntrl/command", Float64, queue_size=1
        )
        self.joint_7_pub = rospy.Publisher(
            "/joint_7_cntrl/command", Float64, queue_size=1
        )

        # service to reset the gazebo world
        rospy.wait_for_service("/gazebo/reset_world")
        self.reset_world = rospy.ServiceProxy("/gazebo/reset_world", Empty)

        # service to reset the gazebo robot
        rospy.wait_for_service("/gazebo/reset_simulation")
        self.reset_simulation = rospy.ServiceProxy("/gazebo/reset_simulation", Empty)

        # services to pause and unpause the simulations
        rospy.wait_for_service("/gazebo/pause_physics")
        self.pause_sim = rospy.ServiceProxy("/gazebo/pause_physics", Empty)
        rospy.wait_for_service("/gazebo/unpause_physics")
        self.unpause_sim = rospy.ServiceProxy("/gazebo/unpause_physics", Empty) 

def __init__(self):
        self.hist_bins_q = rospy.get_param('~histogram/bins/quality')
        self.hist_bins_a = rospy.get_param('~histogram/bins/angle')

        self.dist_from_best_scale = rospy.get_param('~cost/dist_from_best_scale')
        self.dist_from_best_gain = rospy.get_param('~cost/dist_from_best_gain')
        self.dist_from_prev_view_scale = rospy.get_param('~cost/dist_from_prev_view_scale')
        self.dist_from_prev_view_gain = rospy.get_param('~cost/dist_from_prev_view_gain')

        self.height = (rospy.get_param('~height/z1'), rospy.get_param('~height/z2'))

        # Create a GridWorld where we will store values.
        self.gw_bounds = np.array([
            [rospy.get_param('~histogram/bounds/x1'), rospy.get_param('~histogram/bounds/y1')],
            [rospy.get_param('~histogram/bounds/x2'), rospy.get_param('~histogram/bounds/y2')]
        ])
        self.gw_res = rospy.get_param('~histogram/resolution')

        self.reset_gridworld(EmptySrv())
        self.hist_mean = 0
        self.fgw = GridWorld(self.gw_bounds, self.gw_res)
        self.fgw.add_grid('failures', 0.0)

        # Useful meshgrid for distance calculations
        xs = np.arange(self.gw.bounds[0, 0], self.gw.bounds[1, 0] - 1e-6, self.gw.res) + self.gw.res / 2
        ys = np.arange(self.gw.bounds[0, 1], self.gw.bounds[1, 1] - 1e-6, self.gw.res) + self.gw.res / 2
        self._xv, self._yv = np.meshgrid(xs, ys)

        # Get the camera parameters
        cam_info_topic = rospy.get_param('~camera/info_topic')
        camera_info_msg = rospy.wait_for_message(cam_info_topic, CameraInfo)
        self.cam_K = np.array(camera_info_msg.K).reshape((3, 3))

        self.img_pub = rospy.Publisher('~visualisation', Image, queue_size=1)
        rospy.Service('~update_grid', NextViewpoint, self.update_service_handler)
        rospy.Service('~reset_grid', EmptySrv, self.reset_gridworld)
        rospy.Service('~add_failure_point', AddFailurePoint, self.add_failure_point_callback)

        self.base_frame = rospy.get_param('~camera/base_frame')
        self.camera_frame = rospy.get_param('~camera/camera_frame')
        self.img_crop_size = rospy.get_param('~camera/crop_size')
        self.img_crop_y_offset = rospy.get_param('~camera/crop_y_offset')
        self.cam_fov = rospy.get_param('~camera/fov')

        self.counter = 0
        self.curr_depth_img = None
        self.curr_img_time = 0
        self.last_image_pose = None
        rospy.Subscriber(rospy.get_param('~camera/depth_topic'), Image, self._depth_img_callback, queue_size=1) 

def __init__(self):
        entropy_node_name = '/grasp_entropy_node'
        rospy.wait_for_service(entropy_node_name + '/update_grid')
        self.entropy_srv = rospy.ServiceProxy(entropy_node_name + '/update_grid', NextViewpoint)
        rospy.wait_for_service(entropy_node_name + '/reset_grid')
        self.entropy_reset_srv = rospy.ServiceProxy(entropy_node_name + '/reset_grid', EmptySrv)
        rospy.wait_for_service(entropy_node_name + '/add_failure_point')
        self.add_failure_point_srv = rospy.ServiceProxy(entropy_node_name + '/add_failure_point', AddFailurePoint)

        self.curr_velocity_publish_rate = 100.0  # Hz
        self.curr_velo_pub = rospy.Publisher('/cartesian_velocity_node_controller/cartesian_velocity', Twist, queue_size=1)
        self.max_velo = 0.1
        self.curr_velo = Twist()
        self.best_grasp = Pose()
        self.viewpoints = 0
        self.grasp_width = 0.10
        self._in_velo_loop = False

        self.update_rate = 10.0  # Hz
        update_topic_name = '~/update'
        self.update_pub = rospy.Publisher(update_topic_name, Empty, queue_size=1)
        rospy.Subscriber(update_topic_name, Empty, self.__update_callback, queue_size=1)

        self.cs = ControlSwitcher({'moveit': 'position_joint_trajectory_controller',
                                   'velocity': 'cartesian_velocity_node_controller'})
        self.cs.switch_controller('moveit')
        self.pc = PandaCommander(group_name='panda_arm_hand')

        self.robot_state = None
        self.ROBOT_ERROR_DETECTED = False
        self.BAD_UPDATE = False
        rospy.Subscriber('/franka_state_controller/franka_states', FrankaState, self.__robot_state_callback, queue_size=1)

        # Centre and above the bin
        self.pregrasp_pose = [(rospy.get_param('/grasp_entropy_node/histogram/bounds/x2') + rospy.get_param('/grasp_entropy_node/histogram/bounds/x1'))/2,
                              (rospy.get_param('/grasp_entropy_node/histogram/bounds/y2') + rospy.get_param('/grasp_entropy_node/histogram/bounds/y1'))/2 + 0.08,
                              rospy.get_param('/grasp_entropy_node/height/z1') + 0.066,
                              2**0.5/2, -2**0.5/2, 0, 0]

        self.last_weight = 0
        self.__weight_increase_check()

        self.experiment = Experiment() 

def __init__(self, id, initialPosition, tf):
        """Constructor.

        Args:
            id (int): Integer ID in range [0, 255]. The last byte of the robot's
                radio address.
            initialPosition (iterable of float): Initial position of the robot:
                [x, y, z]. Typically on the floor of the experiment space with
                z == 0.0.
            tf (tf.TransformListener): ROS TransformListener used to query the
                robot's current state.
        """
        self.id = id
        prefix = "/cf" + str(id)
        self.prefix = prefix
        self.initialPosition = np.array(initialPosition)

        self.tf = tf

        rospy.wait_for_service(prefix + "/set_group_mask")
        self.setGroupMaskService = rospy.ServiceProxy(prefix + "/set_group_mask", SetGroupMask)
        rospy.wait_for_service(prefix + "/takeoff")
        self.takeoffService = rospy.ServiceProxy(prefix + "/takeoff", Takeoff)
        rospy.wait_for_service(prefix + "/land")
        self.landService = rospy.ServiceProxy(prefix + "/land", Land)
        # rospy.wait_for_service(prefix + "/stop")
        # self.stopService = rospy.ServiceProxy(prefix + "/stop", Stop)
        rospy.wait_for_service(prefix + "/go_to")
        self.goToService = rospy.ServiceProxy(prefix + "/go_to", GoTo)
        rospy.wait_for_service(prefix + "/upload_trajectory")
        self.uploadTrajectoryService = rospy.ServiceProxy(prefix + "/upload_trajectory", UploadTrajectory)
        rospy.wait_for_service(prefix + "/start_trajectory")
        self.startTrajectoryService = rospy.ServiceProxy(prefix + "/start_trajectory", StartTrajectory)
        rospy.wait_for_service(prefix + "/update_params")
        self.updateParamsService = rospy.ServiceProxy(prefix + "/update_params", UpdateParams)

        self.cmdFullStatePublisher = rospy.Publisher(prefix + "/cmd_full_state", FullState, queue_size=1)
        self.cmdFullStateMsg = FullState()
        self.cmdFullStateMsg.header.seq = 0
        self.cmdFullStateMsg.header.frame_id = "/world"

        self.cmdStopPublisher = rospy.Publisher(prefix + "/cmd_stop", std_msgs.msg.Empty, queue_size=1)

        self.cmdVelPublisher = rospy.Publisher(prefix + "/cmd_vel", geometry_msgs.msg.Twist, queue_size=1)

        self.cmdPositionPublisher = rospy.Publisher(prefix + "/cmd_position", Position, queue_size=1)
        self.cmdPositionMsg = Position()
        self.cmdPositionMsg.header.seq = 0
        self.cmdPositionMsg.header.frame_id = "/world"

        self.cmdVelocityWorldPublisher = rospy.Publisher(prefix + "/cmd_velocity_world", VelocityWorld, queue_size=1)
        self.cmdVelocityWorldMsg = VelocityWorld()
        self.cmdVelocityWorldMsg.header.seq = 0
        self.cmdVelocityWorldMsg.header.frame_id = "/world" 

def reset(self):
        """
        Reset environment and setup for new episode.

        Returns:
            initial state of reset environment.
        """

        # Resets the state of the environment and returns an initial observation.
        start_index = np.random.choice(len(self.start_space))
        goal_index = np.random.choice(len(self.goal_space))
        # start_index, goal_index = np.random.choice(len(self.start_space), 2, replace=False)
        start = self.start_space[start_index]
        theta =  np.random.uniform(0, 2.0*math.pi)  # 1.0/2*math.pi  # 4.0/3*math.pi  #  
        self.set_start(start[0], start[1], theta)
        self.goal = self.goal_space[goal_index]
        self.set_goal(self.goal[0], self.goal[1])
        d0, alpha0 = self.goal2robot(self.goal[0] - start[0], self.goal[1] - start[1], theta)
        # print(d0, alpha0)
        self.p = [d0, alpha0]  # relative target position
        self.reward = 0

        self.success = False
        self.vel_cmd = [0., 0.]

        rospy.wait_for_service('/gazebo/reset_simulation')
        try:
            # reset_proxy.call()
            self.reset_proxy
        except rospy.ServiceException:
            print("/gazebo/reset_simulation service call failed")

        # Unpause simulation to make observation
        rospy.wait_for_service('/gazebo/unpause_physics')
        try:
            # self.unpause
            rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
        except rospy.ServiceException:
            print("/gazebo/unpause_physics service call failed")

        image_data = None
        cv_image = None
        while image_data is None:
            image_data = rospy.wait_for_message('/camera/rgb/image_raw', Image)
            # h = image_data.height
            # w = image_data.width
            cv_image = CvBridge().imgmsg_to_cv2(image_data, "bgr8")

        rospy.wait_for_service('/gazebo/pause_physics')
        try:
            self.pause
        except rospy.ServiceException:
            print("/gazebo/pause_physics service call failed")

        if self.img_channels == 1:
            cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
        cv_image = cv2.resize(cv_image, (self.img_width, self.img_height))  # width, height

        observation = cv_image  # .reshape(1, 1, cv_image.shape[0], cv_image.shape[1])
        return observation