def talker_ctrl():
    global rate_value
    rospy.init_node('usv_simple_ctrl', anonymous=True)
    rate = rospy.Rate(rate_value) # 10h
    # publishes to thruster and rudder topics
    pub_motor = rospy.Publisher('thruster_command', JointState, queue_size=10)
    pub_rudder = rospy.Publisher('joint_setpoint', JointState, queue_size=10)
    pub_result = rospy.Publisher('move_usv/result', Float64, queue_size=10)
    
    # subscribe to state and targer point topics
    rospy.Subscriber("state", Odometry, get_pose)  # get usv position (add 'gps' position latter)
    rospy.Subscriber("move_usv/goal", Odometry, get_target)  # get target position

    while not rospy.is_shutdown():
        try:  
            pub_motor.publish(thruster_ctrl_msg())
            pub_rudder.publish(rudder_ctrl_msg())
            pub_result.publish(verify_result())
            rate.sleep()
        except rospy.ROSInterruptException:
	    rospy.logerr("ROS Interrupt Exception! Just ignore the exception!")
        except rospy.ROSTimeMovedBackwardsException:
	    rospy.logerr("ROS Time Backwards! Just ignore the exception!") 

def talker_ctrl():
    global rate_value
    global result
    # publishes to thruster and rudder topics
    pub_motor = rospy.Publisher('thruster_command', JointState, queue_size=10)
    pub_result = rospy.Publisher('move_usv/result', Float64, queue_size=10)
    rospy.init_node('usv_simple_ctrl', anonymous=True)
    rate = rospy.Rate(rate_value) # 10h
    
    # subscribe to state and targer point topics
    rospy.Subscriber("state", Odometry, get_pose)  # get usv position (add 'gps' position latter)
    rospy.Subscriber("move_usv/goal", Odometry, get_target)  # get target position

    while not rospy.is_shutdown():
        try:    
            pub_motor.publish(thruster_ctrl_msg())
            pub_result.publish(verify_result())
            rate.sleep()
        except rospy.ROSInterruptException:
	    rospy.logerr("ROS Interrupt Exception! Just ignore the exception!")
        except rospy.ROSTimeMovedBackwardsException:
	    rospy.logerr("ROS Time Backwards! Just ignore the exception!") 

def talker_ctrl():
    global rate_value
    rospy.init_node('usv_simple_ctrl', anonymous=True)
    rate = rospy.Rate(rate_value) # 10h
    # publishes to thruster and rudder topics
    pub_motor = rospy.Publisher('thruster_command', JointState, queue_size=10)
    pub_rudder = rospy.Publisher('joint_setpoint', JointState, queue_size=10)
    pub_result = rospy.Publisher('move_usv/result', Float64, queue_size=10)
    
    # subscribe to state and targer point topics
    rospy.Subscriber("state", Odometry, get_pose)  # get usv position (add 'gps' position latter)
    rospy.Subscriber("move_usv/goal", Odometry, get_target)  # get target position

    while not rospy.is_shutdown():
        try:  
            pub_motor.publish(thruster_ctrl_msg())
            pub_rudder.publish(rudder_ctrl_msg())
            pub_result.publish(verify_result())
            rate.sleep()
        except rospy.ROSInterruptException:
	    rospy.logerr("ROS Interrupt Exception! Just ignore the exception!")
        except rospy.ROSTimeMovedBackwardsException:
	    rospy.logerr("ROS Time Backwards! Just ignore the exception!") 

def callback_image(data):
    # et = time.time()
    try:
        cv_image = cv_bridge.imgmsg_to_cv2(data, "bgr8")
    except CvBridgeError as e:
        rospy.logerr('[tf-pose-estimation] Converting Image Error. ' + str(e))
        return

    acquired = tf_lock.acquire(False)
    if not acquired:
        return

    try:
        global scales
        humans = pose_estimator.inference(cv_image, scales)
    finally:
        tf_lock.release()

    msg = humans_to_msg(humans)
    msg.image_w = data.width
    msg.image_h = data.height
    msg.header = data.header

    pub_pose.publish(msg)
    # rospy.loginfo(time.time() - et) 

def get_endeffector_pos(self):
        fkreq = SolvePositionFKRequest()
        joints = JointState()
        joints.name = self._limb_right.joint_names()
        joints.position = [self._limb_right.joint_angle(j)
                           for j in joints.name]

        # Add desired pose for forward kinematics
        fkreq.configuration.append(joints)
        fkreq.tip_names.append('right_hand')
        try:
            rospy.wait_for_service(self.name_of_service, 5)
            resp = self.fksvc(fkreq)
        except (rospy.ServiceException, rospy.ROSException), e:
            rospy.logerr("Service call failed: %s" % (e,))
            return False 

def convert_pose(pose, from_frame, to_frame):
    """
    Convert a pose or transform between frames using tf.
        pose            -> A geometry_msgs.msg/Pose that defines the robots position and orientation in a reference_frame
        from_frame      -> A string that defines the original reference_frame of the robot
        to_frame        -> A string that defines the desired reference_frame of the robot to convert to
    """
    global tfBuffer, listener

    # Create Listener objet to recieve and buffer transformations
    trans = None

    try:
        trans = tfBuffer.lookup_transform(to_frame, from_frame, rospy.Time(0), rospy.Duration(1.0))
    except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException), e:
        print(e)
        rospy.logerr('FAILED TO GET TRANSFORM FROM %s to %s' % (to_frame, from_frame))
        return None 

def clamp_float_warn(low, val, upp, name):
    """
    Clamps: low <= val <= upp
    Prints: warning if clamped, error if input is not a float
    @param low: lower bound for the input  (float)
    @param val: input (float ?)
    @param upp: upper bound for the input  (float)
    @param name: input name (str)
    @return: clamp(low,val,upp) if input is a float, None otherwise.
    """
    if not isinstance(val, float):
        rospy.logerr('Invalid input type: ' + name + ' must be a float!')
        return None
    if val < low:
        rospy.logwarn(''.join(['Clamping ' + name, ' (' + str(val) + ')',
                               ' to the lower bound: ',  str(low)]))
        return low
    if val > upp:
        rospy.logwarn(''.join(['Clamping ' + name, ' (' + str(val) + ')',
                               ' to the upper bound: ',  str(upp)]))
        return upp
    return val 

def set_interaction_frame(self, interaction_frame = default_interaction_frame):
        """
        @param interaction_frame:
          - None:  populate with vector of default values
          - Pose: copy all the elements
        """
        if not isinstance(interaction_frame, Pose):
            rospy.logerr('interaction_frame must be of type geometry_msgs.Pose')
            return

        # check for unit quaternion
        quat_sum_square = (interaction_frame.orientation.w * interaction_frame.orientation.w
                          + interaction_frame.orientation.x * interaction_frame.orientation.x
                          + interaction_frame.orientation.y * interaction_frame.orientation.y
                          + interaction_frame.orientation.z * interaction_frame.orientation.z)
        if quat_sum_square  > 1.0 + 1e-7 or quat_sum_square < 1.0 - 1e-7:
            rospy.logerr('Invalid input to quaternion! The quaternion must be a unit quaternion!')
            return

        self._data.interaction_frame = interaction_frame 

def set_interaction_control_mode(self, interaction_mode = default_interaction_control_mode):
        """
        @param interaction_mode:
          - None:  set impedance mode by default
          - mode:  set each direction by the input mode
            (1: impedance, 2: force, 3: impedance with force limit, 4: force with motion limit)
          - [mode]:  copy all elements. Checks length.
        """
        # first check for valid modes
        for i in range(len(interaction_mode)):
            if (interaction_mode[i] < self.impedance_mode
                or interaction_mode[i] > self.force_limit_mode):
                rospy.logerr('Interaction mode option %d is invalid', interaction_mode[i])
                return

        if len(interaction_mode) == 1:
            self._data.interaction_control_mode = [interaction_mode[0]]*self.n_dim_cart
        elif len(interaction_mode) == self.n_dim_cart:
            self._data.interaction_control_mode = deepcopy(interaction_mode)
        else:
            rospy.logerr('The number of interaction_control_mode elements must be 1 or %d',
                         self.n_dim_cart) 

def send_trajectory(self, wait_for_result=True, timeout=None):
        """
        Checks the trajectory message is complete.
        The message then will be packaged up with the MOTION_START
        command and sent to the motion controller.
        @param wait_for_result:
          - If true, the function will not return until the trajectory is finished
          - If false, return True immediately after sending
        @param timeout: maximum time to wait for trajectory result.
          - If timeout is reached, return None.
        @return: True if the goal finished
        @rtype: bool
        """
        if not self._traj.waypoints:
            rospy.logerr("Trajectory is empty! Cannot send.")
            return None
        self._check_options()
        self._client.send_trajectory(self.to_msg())
        return self._client.wait_for_result(timeout) if wait_for_result else True 

def talker_ctrl():
    global rate_value
    global currentHeading
    global windDir 
    global isTacking
    global heeling
    global spHeading

    rospy.init_node('usv_simple_ctrl', anonymous=True)
    rate = rospy.Rate(rate_value) # 10h
    # publishes to thruster and rudder topics
    #pub_sail = rospy.Publisher('angleLimits', Float64, queue_size=10)
    pub_rudder = rospy.Publisher('joint_setpoint', JointState, queue_size=10)
    pub_result = rospy.Publisher('move_usv/result', Float64, queue_size=10)
    pub_heading = rospy.Publisher('currentHeading', Float64, queue_size=10)
    pub_windDir = rospy.Publisher('windDirection', Float64, queue_size=10)
    pub_heeling = rospy.Publisher('heeling', Float64, queue_size=10)
    pub_spHeading = rospy.Publisher('spHeading', Float64, queue_size=10)
    
    # subscribe to state and targer point topics
    rospy.Subscriber("state", Odometry, get_pose)  # get usv position (add 'gps' position latter)
    rospy.Subscriber("move_usv/goal", Odometry, get_target)  # get target position

    while not rospy.is_shutdown():
        try:
            pub_rudder.publish(rudder_ctrl_msg())
            #pub_sail.publish(sail_ctrl())
            pub_result.publish(verify_result())
            pub_heading.publish(currentHeading)
            pub_windDir.publish(windDir)
            pub_heeling.publish(heeling)
            pub_spHeading.publish(spHeading)
            rate.sleep()
        except rospy.ROSInterruptException:
	    rospy.logerr("ROS Interrupt Exception! Just ignore the exception!")
        except rospy.ROSTimeMovedBackwardsException:
	    rospy.logerr("ROS Time Backwards! Just ignore the exception!") 

def talker_ctrl():
    global rate_value
    global currentHeading
    global windDir 
    rospy.init_node('usv_simple_ctrl', anonymous=True)
    rate = rospy.Rate(rate_value) # 10h
    # publishes to thruster and rudder topics
    #pub_sail = rospy.Publisher('angleLimits', Float64, queue_size=10)
    pub_rudder = rospy.Publisher('joint_setpoint', JointState, queue_size=10)
    pub_result = rospy.Publisher('move_usv/result', Float64, queue_size=10)
    pub_heading= rospy.Publisher('currentHeading', Float64, queue_size=10)
    pub_windDir= rospy.Publisher('windDirection', Float64, queue_size=10)

    
    # subscribe to state and targer point topics
    rospy.Subscriber("state", Odometry, get_pose)  # get usv position (add 'gps' position latter)
    rospy.Subscriber("move_usv/goal", Odometry, get_target)  # get target position

    while not rospy.is_shutdown():
        try:
            pub_rudder.publish(rudder_ctrl_msg())
            #pub_sail.publish(sail_ctrl())
            pub_result.publish(verify_result())
            pub_heading.publish(currentHeading)
            pub_windDir.publish(windDir)
            rate.sleep()
        except rospy.ROSInterruptException:
	    rospy.logerr("ROS Interrupt Exception! Just ignore the exception!")
        except rospy.ROSTimeMovedBackwardsException:
	    rospy.logerr("ROS Time Backwards! Just ignore the exception!") 

def _get_polly_client(self, aws_access_key_id=None, aws_secret_access_key=None, aws_session_token=None,
                          region_name=None, with_service_model_patch=False):
        """Note we get a new botocore session each time this function is called.
        This is to avoid potential problems caused by inner state of the session.
        """
        botocore_session = get_session()

        if with_service_model_patch:
            # Older versions of botocore don't have polly. We can possibly fix it by appending
            # extra path with polly service model files to the search path.
            current_dir = os.path.dirname(os.path.abspath(__file__))
            service_model_path = os.path.join(current_dir, 'data', 'models')
            botocore_session.set_config_variable('data_path', service_model_path)
            rospy.loginfo('patching service model data path: {}'.format(service_model_path))

        botocore_session.get_component('credential_provider').insert_after('boto-config', AwsIotCredentialProvider())

        botocore_session.user_agent_extra = self._generate_user_agent_suffix()

        session = Session(aws_access_key_id=aws_access_key_id, aws_secret_access_key=aws_secret_access_key,
                          aws_session_token=aws_session_token, region_name=region_name,
                          botocore_session=botocore_session)

        try:
            return session.client("polly")
        except UnknownServiceError:
            # the first time we reach here, we try to fix the problem
            if not with_service_model_patch:
                return self._get_polly_client(aws_access_key_id, aws_secret_access_key, aws_session_token, region_name,
                                              with_service_model_patch=True)
            else:
                # we have tried our best, time to panic
                rospy.logerr('Amazon Polly is not available. Please install the latest boto3.')
                raise 

def _node_request_handler(self, request):
        """The callback function for processing service request.

        It never raises. If anything unexpected happens, it will return a PollyResponse with details of the exception.

        :param request: an instance of PollyRequest
        :return: a PollyResponse
        """
        rospy.loginfo('Amazon Polly Request: {}'.format(request))

        try:
            response = self._dispatch(request)
            rospy.loginfo('will return {}'.format(response))
            return PollyResponse(result=response)
        except Exception as e:
            current_dir = os.path.dirname(os.path.abspath(__file__))
            exc_type = sys.exc_info()[0]

            # not using `issubclass(exc_type, ConnectionError)` for the condition below because some versions
            # of urllib3 raises exception when doing `from requests.exceptions import ConnectionError`
            error_ogg_filename = 'connerror.ogg' if 'ConnectionError' in exc_type.__name__ else 'error.ogg'

            error_details = {
                'Audio File': os.path.join(current_dir, 'data', error_ogg_filename),
                'Audio Type': 'ogg',
                'Exception': {
                    'Type': str(exc_type),
                    'Module': exc_type.__module__,
                    'Name': exc_type.__name__,
                    'Value': str(e),
                },
                'Traceback': traceback.format_exc()
            }

            error_str = json.dumps(error_details)
            rospy.logerr(error_str)
            return PollyResponse(result=error_str) 

def do_speak(goal):
    """The action handler.

    Note that although it responds to client after the audio play is finished, a client can choose
    not to wait by not calling ``SimpleActionClient.waite_for_result()``.
    """
    rospy.loginfo('speech goal: {}'.format(goal))

    res = do_synthesize(goal)
    rospy.loginfo('synthesizer returns: {}'.format(res))

    try:
        r = json.loads(res.result)
    except Exception as e:
        s = 'Expecting JSON from synthesizer but got {}'.format(res.result)
        rospy.logerr('{}. Exception: {}'.format(s, e))
        finish_with_result(s)
        return

    result = ''

    if 'Audio File' in r:
        audio_file = r['Audio File']
        rospy.loginfo('Will play {}'.format(audio_file))
        play(audio_file)
        result = audio_file

    if 'Exception' in r:
        result = '[ERROR] {}'.format(r)
        rospy.logerr(result)

    finish_with_result(result) 

def callback_image(self, data):
        try:
            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
        except CvBridgeError as e:
            rospy.logerr('Converting Image Error. ' + str(e))
            return

        self.frames.append((data.header.stamp, cv_image)) 

def move_gripper(limb, action):
    # initialize interfaces
    print("Getting robot state...")
    rs = intera_interface.RobotEnable(CHECK_VERSION)
    init_state = rs.state()
    gripper = None
    original_deadzone = None
    
    def clean_shutdown():
        if gripper and original_deadzone:
            gripper.set_dead_zone(original_deadzone)
        print("Finished.")
    
    try:
        gripper = intera_interface.Gripper(limb + '_gripper')
    except (ValueError, OSError) as e:
        rospy.logerr("Could not detect an electric gripper attached to the robot.")
        clean_shutdown()
        return
    rospy.on_shutdown(clean_shutdown)

    original_deadzone = gripper.get_dead_zone()
    # WARNING: setting the deadzone below this can cause oscillations in
    # the gripper position. However, setting the deadzone to this
    # value is required to achieve the incremental commands in this example
    gripper.set_dead_zone(0.001)
    rospy.loginfo("Gripper deadzone set to {}".format(gripper.get_dead_zone()))
    rospy.loginfo("Enabling robot...")
    rs.enable()
    print("Controlling grippers.")
    if (action == "open"):
        gripper.open()
        rospy.sleep(1.0)
        print("Opened grippers")
    elif (action == "close"):
        gripper.close()
        rospy.sleep(1.0)
        print("Closed grippers")
    
    # force shutdown call if caught by key handler
    rospy.signal_shutdown("Example finished.") 

def _guarded_move_to_joint_position(self, joint_angles, timeout=5.0):
        """
        :param joint_angles:
        :param timeout:
        :return:
        """
        if rospy.is_shutdown():
            return
        if joint_angles:
            self._limb.move_to_joint_positions(joint_angles, timeout=timeout)
        else:
            rospy.logerr("No Joint Angles provided for move_to_joint_positions. Staying put.") 

def create_linear_motion_task(self, target_pose, time=4.0, steps=500):
        """ An *incredibly simple* linearly-interpolated Cartesian move """

        r = rospy.Rate(1 / (time / steps))  # Defaults to 100Hz command rate
        current_pose = self.sawyer_robot._limb.endpoint_pose()
        ik_delta = Pose()
        ik_delta.position.x = (current_pose['position'].x - target_pose.position.x) / steps
        ik_delta.position.y = (current_pose['position'].y - target_pose.position.y) / steps
        ik_delta.position.z = (current_pose['position'].z - target_pose.position.z) / steps
        ik_delta.orientation.x = (current_pose['orientation'].x - target_pose.orientation.x) / steps
        ik_delta.orientation.y = (current_pose['orientation'].y - target_pose.orientation.y) / steps
        ik_delta.orientation.z = (current_pose['orientation'].z - target_pose.orientation.z) / steps
        ik_delta.orientation.w = (current_pose['orientation'].w - target_pose.orientation.w) / steps
        for d in range(int(steps), -1, -1):
            if rospy.is_shutdown():
                return
            ik_step = Pose()
            ik_step.position.x = d * ik_delta.position.x + target_pose.position.x
            ik_step.position.y = d * ik_delta.position.y + target_pose.position.y
            ik_step.position.z = d * ik_delta.position.z + target_pose.position.z
            ik_step.orientation.x = d * ik_delta.orientation.x + target_pose.orientation.x
            ik_step.orientation.y = d * ik_delta.orientation.y + target_pose.orientation.y
            ik_step.orientation.z = d * ik_delta.orientation.z + target_pose.orientation.z
            ik_step.orientation.w = d * ik_delta.orientation.w + target_pose.orientation.w
            joint_angles = self.sawyer_robot._limb.ik_request(ik_step, self.sawyer_robot._tip_name)
            if joint_angles:
                self.sawyer_robot._limb.set_joint_positions(joint_angles)
            else:
                rospy.logerr("No Joint Angles provided for move_to_joint_positions. Staying put.")

            r.sleep()
        r.sleep() 

def print_tasks(self):
        """
        :return:
        """
        try:
            self.mutex.acquire()
            tasksstr = "\n".join([str(t.name) for t in self.tasks])
            rospy.logwarn("---- \ntasks stack: \n" + tasksstr + "\n----")
        except Exception as ex:
            rospy.logerr("error printing task stack: " + str(ex))
        finally:
            self.mutex.release() 

def spawn_sdf(name, description_xml, pose, reference_frame):
    rospy.wait_for_service('/gazebo/spawn_sdf_model')
    try:
        spawn_sdf = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel)
        resp_sdf = spawn_sdf(name, description_xml, "/", pose, reference_frame)
    except rospy.ServiceException as e:
        rospy.logerr("Spawn SDF service call failed: {0}".format(e)) 

def test_right_hand_ros():
    """
    Test the cube orientation sensing using ROS
    """
    rospy.init_node('cv_detection_right_hand_camera')

    camera_name = "right_hand_camera"

    camera_helper = CameraHelper(camera_name, "base", 0)

    bridge = CvBridge()

    try:
        while not rospy.is_shutdown():
            # Take picture
            img_data = camera_helper.take_single_picture()

            # Convert to OpenCV format
            cv_image = bridge.imgmsg_to_cv2(img_data, "bgr8")

            # Save for debugging
            #cv2.imwrite("/tmp/debug.png", cv_image)

            # Get cube rotation
            angles = get_cubes_z_rotation(cv_image)
            print(angles)

            # Wait for a key press
            cv2.waitKey(1)

            rospy.sleep(0.1)
    except CvBridgeError, err:
        rospy.logerr(err)

    # Exit 

def _plan_to_position(self, position):
        pose = [position[0],
                position[1],
                position[2],
                np.pi,
                0.0,
                0.0]

        replan_count = 0
        self.group.set_pose_target(pose, end_effector_link='iiwa_link_ee')
        plan = self.group.plan()

        move_distance = self._calc_plan_statistics(plan)
        print("plan length is", len(plan.joint_trajectory.points) )


        while len(plan.joint_trajectory.points) > self.MAX_PATH_LENGTH:
            print("Replan after plan length:", len(plan.joint_trajectory.points))
            print("replanned", replan_count, "times")
            pose[5] = 2 * np.pi * random.random()
            self.group.set_pose_target(pose, end_effector_link='iiwa_link_ee')
            plan = self.group.plan()
            replan_count += 1

            # if replan_count > 20 and len(plan.joint_trajectory.points) < 20:
            #     rospy.logerr("Exiting with lower standards.  This make break")
            #     break

            move_distance = self._calc_plan_statistics(plan)

            if replan_count > 20:
                rospy.logerr("Planning failed.  Attempting to reset position")
                self.move_kuka_to_neutral()
                replan_count = 0



        self._calc_plan_statistics(plan, print_stats=True)

        return plan 

def main():
    """RSDK URDF Fragment Example:
    This example shows a proof of concept for
    adding your URDF fragment to the robot's
    onboard URDF (which is currently in use).
    """
    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                     description=main.__doc__)
    required = parser.add_argument_group('required arguments')
    required.add_argument(
        '-f', '--file', metavar='PATH', required=True,
        help='Path to URDF file to send'
    )
    required.add_argument(
        '-l', '--link', required=False, default="right_hand", #parent
        help='URDF Link already to attach fragment to (usually <left/right>_hand)'
    )
    required.add_argument(
        '-j', '--joint', required=False, default="right_gripper_base",
        help='Root joint for fragment (usually <left/right>_gripper_base)'
    )
    parser.add_argument("-d", "--duration", type=lambda t:abs(float(t)),
            default=5.0, help="[in seconds] Duration to publish fragment")
    args = parser.parse_args(rospy.myargv()[1:])

    rospy.init_node('rsdk_configure_urdf', anonymous=True)

    if not os.access(args.file, os.R_OK):
        rospy.logerr("Cannot read file at '%s'" % (args.file,))
        return 1
    send_urdf(args.link, args.joint, args.file, args.duration)
    return 0 

def comp_gripper():
    import visual_mpc.envs.robot_envs.grippers.weiss as weiss_pkg
    urdf_frag = '/'.join(
        str.split(weiss_pkg.__file__, '/')[:-1]) + '/wsg50_xml/wsg_50_mod.urdf'
    rospy.init_node('rsdk_configure_urdf', anonymous=True)
    if not os.access(urdf_frag, os.R_OK):
        rospy.logerr("Cannot read file at '%s'" % (urdf_frag))
        sys.exit(1)
    send_urdf('right_hand', 'gripper_base_link', urdf_frag, 1e6) 

def _NavigateToGoal(self, goal):
		moveBaseGoal = self._CreateMoveBaseGoal(goal)
		
		self._Client.send_goal(moveBaseGoal)
		self._Client.wait_for_result()
		if self._Client.get_state() == GoalStatus.SUCCEEDED:
			rospy.loginfo("Goal reached")
		else:
			rospy.logerr("Could not execute goal for some reason") 

def _generate_grasps(self, pose, width):
        """
        Generate grasps by using the grasp generator generate action; based on
        server_test.py example on moveit_simple_grasps pkg.
        """

        # Create goal:
        goal = GenerateGraspsGoal()

        goal.pose  = pose
        goal.width = width

        options = GraspGeneratorOptions()
        # simple_graps.cpp doesn't implement GRASP_AXIS_Z!
        #options.grasp_axis      = GraspGeneratorOptions.GRASP_AXIS_Z
        options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP
        options.grasp_rotation  = GraspGeneratorOptions.GRASP_ROTATION_FULL

        # @todo disabled because it works better with the default options
        #goal.options.append(options)

        # Send goal and wait for result:
        state = self._grasps_ac.send_goal_and_wait(goal)
        if state != GoalStatus.SUCCEEDED:
            rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text())
            return None

        grasps = self._grasps_ac.get_result().grasps

        # Publish grasps (for debugging/visualization purposes):
        self._publish_grasps(grasps)

        return grasps 

def _pickup(self, group, target, width):
        """
        Pick up a target using the planning group
        """

        # Obtain possible grasps from the grasp generator server:
        grasps = self._generate_grasps(self._pose_coke_can, width)

        # Create and send Pickup goal:
        goal = self._create_pickup_goal(group, target, grasps)

        state = self._pickup_ac.send_goal_and_wait(goal)
        if state != GoalStatus.SUCCEEDED:
            rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text())
            return None

        result = self._pickup_ac.get_result()

        # Check for error:
        err = result.error_code.val
        if err != MoveItErrorCodes.SUCCESS:
            rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err])))

            return False

        return True 

def _place(self, group, target, place):
        """
        Place a target using the planning group
        """

        # Obtain possible places:
        places = self._generate_places(place)

        # Create and send Place goal:
        goal = self._create_place_goal(group, target, places)

        state = self._place_ac.send_goal_and_wait(goal)
        if state != GoalStatus.SUCCEEDED:
            rospy.logerr('Place goal failed!: %s' % self._place_ac.get_goal_status_text())
            return None

        result = self._place_ac.get_result()

        # Check for error:
        err = result.error_code.val
        if err != MoveItErrorCodes.SUCCESS:
            rospy.logwarn('Group %s cannot place target %s!: %s' % (group, target, str(moveit_error_dict[err])))

            return False

        return True 

def _generate_grasps(self, pose, width):
        """
        Generate grasps by using the grasp generator generate action; based on
        server_test.py example on moveit_simple_grasps pkg.
        """

        # Create goal:
        goal = GenerateGraspsGoal()

        goal.pose  = pose
        goal.width = width

        options = GraspGeneratorOptions()
        # simple_graps.cpp doesn't implement GRASP_AXIS_Z!
        #options.grasp_axis      = GraspGeneratorOptions.GRASP_AXIS_Z
        options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP
        options.grasp_rotation  = GraspGeneratorOptions.GRASP_ROTATION_FULL

        # @todo disabled because it works better with the default options
        #goal.options.append(options)

        # Send goal and wait for result:
        state = self._grasps_ac.send_goal_and_wait(goal)
        if state != GoalStatus.SUCCEEDED:
            rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text())
            return None

        grasps = self._grasps_ac.get_result().grasps

        # Publish grasps (for debugging/visualization purposes):
        self._publish_grasps(grasps)

        return grasps