<commit_before>/*************************************************************************
 *
 *  $RCSfile: ZipPackageStream.hxx,v $
 *
 *  $Revision: 1.11 $
 *
 *  last change: $Author: mtg $ $Date: 2001-11-15 20:27:48 $
 *
 *  The Contents of this file are made available subject to the terms of
 *  either of the following licenses
 *
 *         - GNU Lesser General Public License Version 2.1
 *         - Sun Industry Standards Source License Version 1.1
 *
 *  Sun Microsystems Inc., October, 2000
 *
 *  GNU Lesser General Public License Version 2.1
 *  =============================================
 *  Copyright 2000 by Sun Microsystems, Inc.
 *  901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License version 2.1, as published by the Free Software Foundation.
 *
 *  This library is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *  Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with this library; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *  MA  02111-1307  USA
 *
 *
 *  Sun Industry Standards Source License Version 1.1
 *  =================================================
 *  The contents of this file are subject to the Sun Industry Standards
 *  Source License Version 1.1 (the "License"); You may not use this file
 *  except in compliance with the License. You may obtain a copy of the
 *  License at http://www.openoffice.org/license.html.
 *
 *  Software provided under this License is provided on an "AS IS" basis,
 *  WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING,
 *  WITHOUT LIMITATION, WARRANTIES THAT THE SOFTWARE IS FREE OF DEFECTS,
 *  MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE, OR NON-INFRINGING.
 *  See the License for the specific provisions governing your rights and
 *  obligations concerning the Software.
 *
 *  The Initial Developer of the Original Code is: Sun Microsystems, Inc.
 *
 *  Copyright: 2000 by Sun Microsystems, Inc.
 *
 *  All Rights Reserved.
 *
 *  Contributor(s): Martin Gallwey (gallwey@sun.com)
 *
 *
 ************************************************************************/
#ifndef _ZIP_PACKAGE_STREAM_HXX
#define _ZIP_PACKAGE_STREAM_HXX

#ifndef _COM_SUN_STAR_IO_XACTIVEDATASINK_HPP_
#include <com/sun/star/io/XActiveDataSink.hpp>
#endif
#ifndef _ZIP_PACKAGE_ENTRY_HXX
#include <ZipPackageEntry.hxx>
#endif
#ifndef _VOS_REF_H_
#include <vos/ref.hxx>
#endif
#ifndef _ENCRYPTION_DATA_HXX_
#include <EncryptionData.hxx>
#endif
#ifndef _CPPUHELPER_IMPLBASE1_HXX
#include <cppuhelper/implbase1.hxx>
#endif

class ZipPackage;
struct ZipEntry;
class ZipPackageStream : public cppu::ImplInheritanceHelper1
<
    ZipPackageEntry,
    ::com::sun::star::io::XActiveDataSink
>
{
    static com::sun::star::uno::Sequence < sal_Int8 > aImplementationId;
protected:
    com::sun::star::uno::Reference < com::sun::star::io::XInputStream > xStream;
    ZipPackage          &rZipPackage;
    sal_Bool            bToBeCompressed, bToBeEncrypted, bPackageMember, bHaveOwnKey, bIsEncrypted;
    vos::ORef < EncryptionData > xEncryptionData;
public:
    sal_Bool HasOwnKey ()        { return bHaveOwnKey;}
    sal_Bool IsToBeCompressed () { return bToBeCompressed;}
    sal_Bool IsToBeEncrypted ()  { return bToBeEncrypted;}
    sal_Bool IsEncrypted ()      { return bIsEncrypted;}
    sal_Bool IsPackageMember ()  { return bPackageMember;}
    vos::ORef < EncryptionData > & getEncryptionData ()
    { return xEncryptionData;}
    const com::sun::star::uno::Sequence < sal_Int8 >& getKey ()
    { return xEncryptionData->aKey;}
    const com::sun::star::uno::Sequence < sal_uInt8 >& getInitialisationVector ()
    { return xEncryptionData->aInitVector;}
    const com::sun::star::uno::Sequence < sal_uInt8 >& getDigest ()
    { return xEncryptionData->aDigest;}
    const com::sun::star::uno::Sequence < sal_uInt8 >& getSalt ()
    { return xEncryptionData->aSalt;}
    const sal_Int32 getIterationCount ()
    { return xEncryptionData->nIterationCount;}
    const sal_Int32 getSize ()
    { return aEntry.nSize;}

    void SetToBeCompressed (sal_Bool bNewValue) { bToBeCompressed = bNewValue;}
    void SetIsEncrypted (sal_Bool bNewValue) { bIsEncrypted = bNewValue;}
    void SetToBeEncrypted (sal_Bool bNewValue)
    {
        bToBeEncrypted  = bNewValue;
        if ( bToBeEncrypted && xEncryptionData.isEmpty())
            xEncryptionData = new EncryptionData;
        else if ( !bToBeEncrypted && !xEncryptionData.isEmpty() )
            xEncryptionData.unbind();
    }
    void SetPackageMember (sal_Bool bNewValue)  { bPackageMember  = bNewValue;}
    void setKey (const com::sun::star::uno::Sequence < sal_Int8 >& rNewKey )
    { xEncryptionData->aKey = rNewKey;}
    void setInitialisationVector (const com::sun::star::uno::Sequence < sal_uInt8 >& rNewVector )
    { xEncryptionData->aInitVector = rNewVector;}
    void setSalt (const com::sun::star::uno::Sequence < sal_uInt8 >& rNewSalt )
    { xEncryptionData->aSalt = rNewSalt;}
    void setDigest (const com::sun::star::uno::Sequence < sal_uInt8 >& rNewDigest )
    { xEncryptionData->aDigest = rNewDigest;}
    void setIterationCount (const sal_Int32 nNewCount)
    { xEncryptionData->nIterationCount = nNewCount;}
    void setSize (const sal_Int32 nNewSize);

    ZipPackageStream (ZipPackage & rNewPackage);
    virtual ~ZipPackageStream( void );

    void setZipEntry( const ZipEntry &rInEntry);
    ::com::sun::star::uno::Reference< ::com::sun::star::io::XInputStream > SAL_CALL getRawStream( )
        throw(::com::sun::star::uno::RuntimeException);

    static ::com::sun::star::uno::Sequence < sal_Int8 >& static_getImplementationId()
    {
        return aImplementationId;
    }

    // XActiveDataSink
    virtual void SAL_CALL setInputStream( const ::com::sun::star::uno::Reference< ::com::sun::star::io::XInputStream >& aStream )
        throw(::com::sun::star::uno::RuntimeException);
    virtual ::com::sun::star::uno::Reference< ::com::sun::star::io::XInputStream > SAL_CALL getInputStream(  )
        throw(::com::sun::star::uno::RuntimeException);

    // XUnoTunnel
    virtual sal_Int64 SAL_CALL getSomething( const ::com::sun::star::uno::Sequence< sal_Int8 >& aIdentifier )
        throw(::com::sun::star::uno::RuntimeException);

    // XPropertySet
    virtual void SAL_CALL setPropertyValue( const ::rtl::OUString& aPropertyName, const ::com::sun::star::uno::Any& aValue )
        throw(::com::sun::star::beans::UnknownPropertyException, ::com::sun::star::beans::PropertyVetoException, ::com::sun::star::lang::IllegalArgumentException, ::com::sun::star::lang::WrappedTargetException, ::com::sun::star::uno::RuntimeException);
    virtual ::com::sun::star::uno::Any SAL_CALL getPropertyValue( const ::rtl::OUString& PropertyName )
        throw(::com::sun::star::beans::UnknownPropertyException, ::com::sun::star::lang::WrappedTargetException, ::com::sun::star::uno::RuntimeException);

    // XServiceInfo
    virtual ::rtl::OUString SAL_CALL getImplementationName(  )
        throw (::com::sun::star::uno::RuntimeException);
    virtual sal_Bool SAL_CALL supportsService( const ::rtl::OUString& ServiceName )
        throw (::com::sun::star::uno::RuntimeException);
    virtual ::com::sun::star::uno::Sequence< ::rtl::OUString > SAL_CALL getSupportedServiceNames(  )
        throw (::com::sun::star::uno::RuntimeException);
};
#endif
<commit_msg>#101685#,#i6886# merge OOO_STABLE_1_PORTS (1.11-1.11.10.1) -> HEAD<commit_after>/*************************************************************************
 *
 *  $RCSfile: ZipPackageStream.hxx,v $
 *
 *  $Revision: 1.12 $
 *
 *  last change: $Author: hr $ $Date: 2002-08-20 13:04:52 $
 *
 *  The Contents of this file are made available subject to the terms of
 *  either of the following licenses
 *
 *         - GNU Lesser General Public License Version 2.1
 *         - Sun Industry Standards Source License Version 1.1
 *
 *  Sun Microsystems Inc., October, 2000
 *
 *  GNU Lesser General Public License Version 2.1
 *  =============================================
 *  Copyright 2000 by Sun Microsystems, Inc.
 *  901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License version 2.1, as published by the Free Software Foundation.
 *
 *  This library is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *  Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with this library; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *  MA  02111-1307  USA
 *
 *
 *  Sun Industry Standards Source License Version 1.1
 *  =================================================
 *  The contents of this file are subject to the Sun Industry Standards
 *  Source License Version 1.1 (the "License"); You may not use this file
 *  except in compliance with the License. You may obtain a copy of the
 *  License at http://www.openoffice.org/license.html.
 *
 *  Software provided under this License is provided on an "AS IS" basis,
 *  WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING,
 *  WITHOUT LIMITATION, WARRANTIES THAT THE SOFTWARE IS FREE OF DEFECTS,
 *  MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE, OR NON-INFRINGING.
 *  See the License for the specific provisions governing your rights and
 *  obligations concerning the Software.
 *
 *  The Initial Developer of the Original Code is: Sun Microsystems, Inc.
 *
 *  Copyright: 2000 by Sun Microsystems, Inc.
 *
 *  All Rights Reserved.
 *
 *  Contributor(s): Martin Gallwey (gallwey@sun.com)
 *
 *
 ************************************************************************/
#ifndef _ZIP_PACKAGE_STREAM_HXX
#define _ZIP_PACKAGE_STREAM_HXX

#ifndef _COM_SUN_STAR_IO_XACTIVEDATASINK_HPP_
#include <com/sun/star/io/XActiveDataSink.hpp>
#endif
#ifndef _ZIP_PACKAGE_ENTRY_HXX
#include <ZipPackageEntry.hxx>
#endif
#ifndef _VOS_REF_H_
#include <vos/ref.hxx>
#endif
#ifndef _ENCRYPTION_DATA_HXX_
#include <EncryptionData.hxx>
#endif
#ifndef _CPPUHELPER_IMPLBASE1_HXX
#include <cppuhelper/implbase1.hxx>
#endif

class ZipPackage;
struct ZipEntry;
#ifdef MACOSX
class ZipPackageStream : public ZipPackageEntry,
                                             public ::cppu::OWeakObject,
                                                 public ::com::sun::star::io::XActiveDataSink
#else
class ZipPackageStream : public cppu::ImplInheritanceHelper1
<
    ZipPackageEntry,
    ::com::sun::star::io::XActiveDataSink
>
#endif
{
    static com::sun::star::uno::Sequence < sal_Int8 > aImplementationId;
protected:
    com::sun::star::uno::Reference < com::sun::star::io::XInputStream > xStream;
    ZipPackage          &rZipPackage;
    sal_Bool            bToBeCompressed, bToBeEncrypted, bPackageMember, bHaveOwnKey, bIsEncrypted;
    vos::ORef < EncryptionData > xEncryptionData;
public:
    sal_Bool HasOwnKey ()        { return bHaveOwnKey;}
    sal_Bool IsToBeCompressed () { return bToBeCompressed;}
    sal_Bool IsToBeEncrypted ()  { return bToBeEncrypted;}
    sal_Bool IsEncrypted ()      { return bIsEncrypted;}
    sal_Bool IsPackageMember ()  { return bPackageMember;}
    vos::ORef < EncryptionData > & getEncryptionData ()
    { return xEncryptionData;}
    const com::sun::star::uno::Sequence < sal_Int8 >& getKey ()
    { return xEncryptionData->aKey;}
    const com::sun::star::uno::Sequence < sal_uInt8 >& getInitialisationVector ()
    { return xEncryptionData->aInitVector;}
    const com::sun::star::uno::Sequence < sal_uInt8 >& getDigest ()
    { return xEncryptionData->aDigest;}
    const com::sun::star::uno::Sequence < sal_uInt8 >& getSalt ()
    { return xEncryptionData->aSalt;}
    const sal_Int32 getIterationCount ()
    { return xEncryptionData->nIterationCount;}
    const sal_Int32 getSize ()
    { return aEntry.nSize;}

    void SetToBeCompressed (sal_Bool bNewValue) { bToBeCompressed = bNewValue;}
    void SetIsEncrypted (sal_Bool bNewValue) { bIsEncrypted = bNewValue;}
    void SetToBeEncrypted (sal_Bool bNewValue)
    {
        bToBeEncrypted  = bNewValue;
        if ( bToBeEncrypted && xEncryptionData.isEmpty())
            xEncryptionData = new EncryptionData;
        else if ( !bToBeEncrypted && !xEncryptionData.isEmpty() )
            xEncryptionData.unbind();
    }
    void SetPackageMember (sal_Bool bNewValue)  { bPackageMember  = bNewValue;}
    void setKey (const com::sun::star::uno::Sequence < sal_Int8 >& rNewKey )
    { xEncryptionData->aKey = rNewKey;}
    void setInitialisationVector (const com::sun::star::uno::Sequence < sal_uInt8 >& rNewVector )
    { xEncryptionData->aInitVector = rNewVector;}
    void setSalt (const com::sun::star::uno::Sequence < sal_uInt8 >& rNewSalt )
    { xEncryptionData->aSalt = rNewSalt;}
    void setDigest (const com::sun::star::uno::Sequence < sal_uInt8 >& rNewDigest )
    { xEncryptionData->aDigest = rNewDigest;}
    void setIterationCount (const sal_Int32 nNewCount)
    { xEncryptionData->nIterationCount = nNewCount;}
    void setSize (const sal_Int32 nNewSize);

    ZipPackageStream (ZipPackage & rNewPackage);
    virtual ~ZipPackageStream( void );

    void setZipEntry( const ZipEntry &rInEntry);
    ::com::sun::star::uno::Reference< ::com::sun::star::io::XInputStream > SAL_CALL getRawStream( )
        throw(::com::sun::star::uno::RuntimeException);

    static ::com::sun::star::uno::Sequence < sal_Int8 >& static_getImplementationId()
    {
        return aImplementationId;
    }

#ifdef MACOSX
    // XInterface
    virtual ::com::sun::star::uno::Any SAL_CALL queryInterface( const ::com::sun::star::uno::Type& rType )
        throw(::com::sun::star::uno::RuntimeException);
    virtual void SAL_CALL acquire(  )
        throw();
    virtual void SAL_CALL release(  )
        throw();
#endif

    // XActiveDataSink
    virtual void SAL_CALL setInputStream( const ::com::sun::star::uno::Reference< ::com::sun::star::io::XInputStream >& aStream )
        throw(::com::sun::star::uno::RuntimeException);
    virtual ::com::sun::star::uno::Reference< ::com::sun::star::io::XInputStream > SAL_CALL getInputStream(  )
        throw(::com::sun::star::uno::RuntimeException);

    // XUnoTunnel
    virtual sal_Int64 SAL_CALL getSomething( const ::com::sun::star::uno::Sequence< sal_Int8 >& aIdentifier )
        throw(::com::sun::star::uno::RuntimeException);

    // XPropertySet
    virtual void SAL_CALL setPropertyValue( const ::rtl::OUString& aPropertyName, const ::com::sun::star::uno::Any& aValue )
        throw(::com::sun::star::beans::UnknownPropertyException, ::com::sun::star::beans::PropertyVetoException, ::com::sun::star::lang::IllegalArgumentException, ::com::sun::star::lang::WrappedTargetException, ::com::sun::star::uno::RuntimeException);
    virtual ::com::sun::star::uno::Any SAL_CALL getPropertyValue( const ::rtl::OUString& PropertyName )
        throw(::com::sun::star::beans::UnknownPropertyException, ::com::sun::star::lang::WrappedTargetException, ::com::sun::star::uno::RuntimeException);

    // XServiceInfo
    virtual ::rtl::OUString SAL_CALL getImplementationName(  )
        throw (::com::sun::star::uno::RuntimeException);
    virtual sal_Bool SAL_CALL supportsService( const ::rtl::OUString& ServiceName )
        throw (::com::sun::star::uno::RuntimeException);
    virtual ::com::sun::star::uno::Sequence< ::rtl::OUString > SAL_CALL getSupportedServiceNames(  )
        throw (::com::sun::star::uno::RuntimeException);
};
#endif
<|endoftext|>


<commit_before>/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                           License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2010-2012, Institute Of Software Chinese Academy Of Science, all rights reserved.
// Copyright (C) 2010-2012, Advanced Micro Devices, Inc., all rights reserved.
// Copyright (C) 2010-2012, Multicoreware, Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// @Authors
//    Jia Haipeng, jiahaipeng95@gmail.com
//    Xiaopeng Fu, xiaopeng@multicorewareinc.com
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other oclMaterials provided with the distribution.
//
//   * The name of the copyright holders may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/

#include "precomp.hpp"
#include <vector>

using namespace cv;
using namespace cv::ocl;
using namespace std;

namespace cv
{
namespace ocl
{

///////////////////////////OpenCL kernel strings///////////////////////////
extern const char *stereobm;

}
}
namespace cv
{
namespace ocl
{
namespace stereoBM
{
/////////////////////////////////////////////////////////////////////////
//////////////////////////prefilter_xsbel////////////////////////////////
////////////////////////////////////////////////////////////////////////
void prefilter_xsobel(const oclMat &input, oclMat &output, int prefilterCap)
{
    Context *clCxt = input.clCxt;

    string kernelName = "prefilter_xsobel";
    cl_kernel kernel = openCLGetKernelFromSource(clCxt, &stereobm, kernelName);

    size_t blockSize = 1;
    size_t globalThreads[3] = { input.cols, input.rows, 1 };
    size_t localThreads[3]  = { blockSize, blockSize, 1 };

    openCLVerifyKernel(clCxt, kernel,  localThreads);
    openCLSafeCall(clSetKernelArg(kernel, 0, sizeof(cl_mem), (void *)&input.data));
    openCLSafeCall(clSetKernelArg(kernel, 1, sizeof(cl_mem), (void *)&output.data));
    openCLSafeCall(clSetKernelArg(kernel, 2, sizeof(cl_int), (void *)&input.rows));
    openCLSafeCall(clSetKernelArg(kernel, 3, sizeof(cl_int), (void *)&input.cols));
    openCLSafeCall(clSetKernelArg(kernel, 4, sizeof(cl_int), (void *)&prefilterCap));

    openCLSafeCall(clEnqueueNDRangeKernel(clCxt->impl->clCmdQueue, kernel, 3, NULL,
                                          globalThreads, localThreads, 0, NULL, NULL));

    clFinish(clCxt->impl->clCmdQueue);
    openCLSafeCall(clReleaseKernel(kernel));

}
//////////////////////////////////////////////////////////////////////////
//////////////////////////////common////////////////////////////////////
////////////////////////////////////////////////////////////////////////
#define N_DISPARITIES 8
#define ROWSperTHREAD 21
#define BLOCK_W 128
static inline int divUp(int total, int grain)
{
    return (total + grain - 1) / grain;
}
////////////////////////////////////////////////////////////////////////////
///////////////////////////////stereoBM_GPU////////////////////////////////
////////////////////////////////////////////////////////////////////////////
void stereo_bm(const oclMat &left, const oclMat &right,  oclMat &disp,
               int maxdisp, int winSize,  oclMat &minSSD_buf)
{
    int winsz2 = winSize >> 1;

    //if(winsz2 == 0 || winsz2 >= calles_num)
    //cv::ocl:error("Unsupported window size", __FILE__, __LINE__, __FUNCTION__);

    Context *clCxt = left.clCxt;

    string kernelName = "stereoKernel";
    cl_kernel kernel = openCLGetKernelFromSource(clCxt, &stereobm, kernelName);

    disp.setTo(Scalar_<unsigned char>::all(0));
    minSSD_buf.setTo(Scalar_<unsigned int>::all(0xFFFFFFFF));

    size_t minssd_step = minSSD_buf.step / minSSD_buf.elemSize();
    size_t local_mem_size = (BLOCK_W + N_DISPARITIES * (BLOCK_W + 2 * winsz2)) *
                            sizeof(cl_uint);
    //size_t blockSize = 1;
    size_t localThreads[]  = { BLOCK_W, 1,1};
    size_t globalThreads[] = { divUp(left.cols - maxdisp - 2 * winsz2, BLOCK_W) *BLOCK_W,
                               divUp(left.rows - 2 * winsz2, ROWSperTHREAD),
                               1
                             };

    openCLVerifyKernel(clCxt, kernel, localThreads);
    openCLSafeCall(clSetKernelArg(kernel, 0, sizeof(cl_mem), (void *)&left.data));
    openCLSafeCall(clSetKernelArg(kernel, 1, sizeof(cl_mem), (void *)&right.data));
    openCLSafeCall(clSetKernelArg(kernel, 2, sizeof(cl_mem), (void *)&minSSD_buf.data));
    openCLSafeCall(clSetKernelArg(kernel, 3, sizeof(cl_int), (void *)&minssd_step));
    openCLSafeCall(clSetKernelArg(kernel, 4, sizeof(cl_mem), (void *)&disp.data));
    openCLSafeCall(clSetKernelArg(kernel, 5, sizeof(cl_int), (void *)&disp.step));
    openCLSafeCall(clSetKernelArg(kernel, 6, sizeof(cl_int), (void *)&left.cols));
    openCLSafeCall(clSetKernelArg(kernel, 7, sizeof(cl_int), (void *)&left.rows));
    openCLSafeCall(clSetKernelArg(kernel, 8, sizeof(cl_int), (void *)&left.step));
    openCLSafeCall(clSetKernelArg(kernel, 9, sizeof(cl_int), (void *)&maxdisp));
    openCLSafeCall(clSetKernelArg(kernel, 10, sizeof(cl_int), (void *)&winsz2));
    openCLSafeCall(clSetKernelArg(kernel, 11, local_mem_size, (void *)NULL));

    openCLSafeCall(clEnqueueNDRangeKernel(clCxt->impl->clCmdQueue, kernel, 2, NULL,
                                          globalThreads, localThreads, 0, NULL, NULL));

    clFinish(clCxt->impl->clCmdQueue);
    openCLSafeCall(clReleaseKernel(kernel));
}
////////////////////////////////////////////////////////////////////////////
///////////////////////////////postfilter_textureness///////////////////////
////////////////////////////////////////////////////////////////////////////
void postfilter_textureness(oclMat &left, int winSize,
                            float avergeTexThreshold, oclMat &disparity)
{
    Context *clCxt = left.clCxt;

    string kernelName = "textureness_kernel";
    cl_kernel kernel = openCLGetKernelFromSource(clCxt, &stereobm, kernelName);

    size_t blockSize = 1;
    size_t localThreads[]  = { BLOCK_W, blockSize ,1};
    size_t globalThreads[] = { divUp(left.cols, BLOCK_W) *BLOCK_W,
                               divUp(left.rows, 2 * ROWSperTHREAD),
                               1
                             };

    size_t local_mem_size = (localThreads[0] + localThreads[0] + (winSize / 2) * 2) * sizeof(float);

    openCLVerifyKernel(clCxt, kernel,  localThreads);
    openCLSafeCall(clSetKernelArg(kernel, 0, sizeof(cl_mem), (void *)&disparity.data));
    openCLSafeCall(clSetKernelArg(kernel, 1, sizeof(cl_int), (void *)&disparity.rows));
    openCLSafeCall(clSetKernelArg(kernel, 2, sizeof(cl_int), (void *)&disparity.cols));
    openCLSafeCall(clSetKernelArg(kernel, 3, sizeof(cl_int), (void *)&disparity.step));
    openCLSafeCall(clSetKernelArg(kernel, 4, sizeof(cl_mem), (void *)&left.data));
    openCLSafeCall(clSetKernelArg(kernel, 5, sizeof(cl_int), (void *)&left.rows));
    openCLSafeCall(clSetKernelArg(kernel, 6, sizeof(cl_int), (void *)&left.cols));
    openCLSafeCall(clSetKernelArg(kernel, 7, sizeof(cl_int), (void *)&winSize));
    openCLSafeCall(clSetKernelArg(kernel, 8, sizeof(cl_float), (void *)&avergeTexThreshold));
    openCLSafeCall(clSetKernelArg(kernel, 9, local_mem_size, NULL));
    openCLSafeCall(clEnqueueNDRangeKernel(clCxt->impl->clCmdQueue, kernel, 2, NULL,
                                          globalThreads, localThreads, 0, NULL, NULL));

    clFinish(clCxt->impl->clCmdQueue);
    openCLSafeCall(clReleaseKernel(kernel));
}
//////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////operator/////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
void operator_(oclMat &minSSD, oclMat &leBuf, oclMat &riBuf, int preset, int ndisp,
               int winSize, float avergeTexThreshold, const oclMat &left,
               const oclMat &right, oclMat &disparity)

{
    CV_DbgAssert(left.rows == right.rows && left.cols == right.cols);
    CV_DbgAssert(left.type() == CV_8UC1);
    CV_DbgAssert(right.type() == CV_8UC1);

    disparity.create(left.size(), CV_8UC1);
    minSSD.create(left.size(), CV_32SC1);

    oclMat le_for_bm =  left;
    oclMat ri_for_bm = right;

    if (preset == cv::ocl::StereoBM_OCL::PREFILTER_XSOBEL)
    {
        leBuf.create( left.size(),  left.type());
        riBuf.create(right.size(), right.type());

        prefilter_xsobel( left, leBuf, 31);
        prefilter_xsobel(right, riBuf, 31);

        le_for_bm = leBuf;
        ri_for_bm = riBuf;
    }

    stereo_bm(le_for_bm, ri_for_bm, disparity, ndisp, winSize, minSSD);

    if (avergeTexThreshold)
    {
        postfilter_textureness(le_for_bm, winSize, avergeTexThreshold, disparity);
    }
}
}
}
}
const float defaultAvgTexThreshold = 3;

cv::ocl::StereoBM_OCL::StereoBM_OCL()
    : preset(BASIC_PRESET), ndisp(DEFAULT_NDISP), winSize(DEFAULT_WINSZ),
      avergeTexThreshold(defaultAvgTexThreshold)  {}

cv::ocl::StereoBM_OCL::StereoBM_OCL(int preset_, int ndisparities_, int winSize_)
    : preset(preset_), ndisp(ndisparities_), winSize(winSize_),
      avergeTexThreshold(defaultAvgTexThreshold)
{
    const int max_supported_ndisp = 1 << (sizeof(unsigned char) * 8);
    CV_Assert(0 < ndisp && ndisp <= max_supported_ndisp);
    CV_Assert(ndisp % 8 == 0);
    CV_Assert(winSize % 2 == 1);
}

bool cv::ocl::StereoBM_OCL::checkIfGpuCallReasonable()
{
    return true;
}

void cv::ocl::StereoBM_OCL::operator() ( const oclMat &left, const oclMat &right,
        oclMat &disparity)
{
    cv::ocl::stereoBM::operator_(minSSD, leBuf, riBuf, preset, ndisp, winSize, avergeTexThreshold, left, right, disparity);
}

<commit_msg>fix warnings on Linux<commit_after>/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                           License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2010-2012, Institute Of Software Chinese Academy Of Science, all rights reserved.
// Copyright (C) 2010-2012, Advanced Micro Devices, Inc., all rights reserved.
// Copyright (C) 2010-2012, Multicoreware, Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// @Authors
//    Jia Haipeng, jiahaipeng95@gmail.com
//    Xiaopeng Fu, xiaopeng@multicorewareinc.com
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other oclMaterials provided with the distribution.
//
//   * The name of the copyright holders may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/

#include "precomp.hpp"
#include <vector>

using namespace cv;
using namespace cv::ocl;
using namespace std;

namespace cv
{
namespace ocl
{

///////////////////////////OpenCL kernel strings///////////////////////////
extern const char *stereobm;

}
}
namespace cv
{
namespace ocl
{
namespace stereoBM
{
/////////////////////////////////////////////////////////////////////////
//////////////////////////prefilter_xsbel////////////////////////////////
////////////////////////////////////////////////////////////////////////
static void prefilter_xsobel(const oclMat &input, oclMat &output, int prefilterCap)
{
    Context *clCxt = input.clCxt;

    string kernelName = "prefilter_xsobel";
    cl_kernel kernel = openCLGetKernelFromSource(clCxt, &stereobm, kernelName);

    size_t blockSize = 1;
    size_t globalThreads[3] = { input.cols, input.rows, 1 };
    size_t localThreads[3]  = { blockSize, blockSize, 1 };

    openCLVerifyKernel(clCxt, kernel,  localThreads);
    openCLSafeCall(clSetKernelArg(kernel, 0, sizeof(cl_mem), (void *)&input.data));
    openCLSafeCall(clSetKernelArg(kernel, 1, sizeof(cl_mem), (void *)&output.data));
    openCLSafeCall(clSetKernelArg(kernel, 2, sizeof(cl_int), (void *)&input.rows));
    openCLSafeCall(clSetKernelArg(kernel, 3, sizeof(cl_int), (void *)&input.cols));
    openCLSafeCall(clSetKernelArg(kernel, 4, sizeof(cl_int), (void *)&prefilterCap));

    openCLSafeCall(clEnqueueNDRangeKernel(clCxt->impl->clCmdQueue, kernel, 3, NULL,
                                          globalThreads, localThreads, 0, NULL, NULL));

    clFinish(clCxt->impl->clCmdQueue);
    openCLSafeCall(clReleaseKernel(kernel));

}
//////////////////////////////////////////////////////////////////////////
//////////////////////////////common////////////////////////////////////
////////////////////////////////////////////////////////////////////////
#define N_DISPARITIES 8
#define ROWSperTHREAD 21
#define BLOCK_W 128
static inline int divUp(int total, int grain)
{
    return (total + grain - 1) / grain;
}
////////////////////////////////////////////////////////////////////////////
///////////////////////////////stereoBM_GPU////////////////////////////////
////////////////////////////////////////////////////////////////////////////
static void stereo_bm(const oclMat &left, const oclMat &right,  oclMat &disp,
               int maxdisp, int winSize,  oclMat &minSSD_buf)
{
    int winsz2 = winSize >> 1;

    //if(winsz2 == 0 || winsz2 >= calles_num)
    //cv::ocl:error("Unsupported window size", __FILE__, __LINE__, __FUNCTION__);

    Context *clCxt = left.clCxt;

    string kernelName = "stereoKernel";
    cl_kernel kernel = openCLGetKernelFromSource(clCxt, &stereobm, kernelName);

    disp.setTo(Scalar_<unsigned char>::all(0));
    minSSD_buf.setTo(Scalar_<unsigned int>::all(0xFFFFFFFF));

    size_t minssd_step = minSSD_buf.step / minSSD_buf.elemSize();
    size_t local_mem_size = (BLOCK_W + N_DISPARITIES * (BLOCK_W + 2 * winsz2)) *
                            sizeof(cl_uint);
    //size_t blockSize = 1;
    size_t localThreads[]  = { BLOCK_W, 1,1};
    size_t globalThreads[] = { divUp(left.cols - maxdisp - 2 * winsz2, BLOCK_W) *BLOCK_W,
                               divUp(left.rows - 2 * winsz2, ROWSperTHREAD),
                               1
                             };

    openCLVerifyKernel(clCxt, kernel, localThreads);
    openCLSafeCall(clSetKernelArg(kernel, 0, sizeof(cl_mem), (void *)&left.data));
    openCLSafeCall(clSetKernelArg(kernel, 1, sizeof(cl_mem), (void *)&right.data));
    openCLSafeCall(clSetKernelArg(kernel, 2, sizeof(cl_mem), (void *)&minSSD_buf.data));
    openCLSafeCall(clSetKernelArg(kernel, 3, sizeof(cl_int), (void *)&minssd_step));
    openCLSafeCall(clSetKernelArg(kernel, 4, sizeof(cl_mem), (void *)&disp.data));
    openCLSafeCall(clSetKernelArg(kernel, 5, sizeof(cl_int), (void *)&disp.step));
    openCLSafeCall(clSetKernelArg(kernel, 6, sizeof(cl_int), (void *)&left.cols));
    openCLSafeCall(clSetKernelArg(kernel, 7, sizeof(cl_int), (void *)&left.rows));
    openCLSafeCall(clSetKernelArg(kernel, 8, sizeof(cl_int), (void *)&left.step));
    openCLSafeCall(clSetKernelArg(kernel, 9, sizeof(cl_int), (void *)&maxdisp));
    openCLSafeCall(clSetKernelArg(kernel, 10, sizeof(cl_int), (void *)&winsz2));
    openCLSafeCall(clSetKernelArg(kernel, 11, local_mem_size, (void *)NULL));

    openCLSafeCall(clEnqueueNDRangeKernel(clCxt->impl->clCmdQueue, kernel, 2, NULL,
                                          globalThreads, localThreads, 0, NULL, NULL));

    clFinish(clCxt->impl->clCmdQueue);
    openCLSafeCall(clReleaseKernel(kernel));
}
////////////////////////////////////////////////////////////////////////////
///////////////////////////////postfilter_textureness///////////////////////
////////////////////////////////////////////////////////////////////////////
static void postfilter_textureness(oclMat &left, int winSize,
                            float avergeTexThreshold, oclMat &disparity)
{
    Context *clCxt = left.clCxt;

    string kernelName = "textureness_kernel";
    cl_kernel kernel = openCLGetKernelFromSource(clCxt, &stereobm, kernelName);

    size_t blockSize = 1;
    size_t localThreads[]  = { BLOCK_W, blockSize ,1};
    size_t globalThreads[] = { divUp(left.cols, BLOCK_W) *BLOCK_W,
                               divUp(left.rows, 2 * ROWSperTHREAD),
                               1
                             };

    size_t local_mem_size = (localThreads[0] + localThreads[0] + (winSize / 2) * 2) * sizeof(float);

    openCLVerifyKernel(clCxt, kernel,  localThreads);
    openCLSafeCall(clSetKernelArg(kernel, 0, sizeof(cl_mem), (void *)&disparity.data));
    openCLSafeCall(clSetKernelArg(kernel, 1, sizeof(cl_int), (void *)&disparity.rows));
    openCLSafeCall(clSetKernelArg(kernel, 2, sizeof(cl_int), (void *)&disparity.cols));
    openCLSafeCall(clSetKernelArg(kernel, 3, sizeof(cl_int), (void *)&disparity.step));
    openCLSafeCall(clSetKernelArg(kernel, 4, sizeof(cl_mem), (void *)&left.data));
    openCLSafeCall(clSetKernelArg(kernel, 5, sizeof(cl_int), (void *)&left.rows));
    openCLSafeCall(clSetKernelArg(kernel, 6, sizeof(cl_int), (void *)&left.cols));
    openCLSafeCall(clSetKernelArg(kernel, 7, sizeof(cl_int), (void *)&winSize));
    openCLSafeCall(clSetKernelArg(kernel, 8, sizeof(cl_float), (void *)&avergeTexThreshold));
    openCLSafeCall(clSetKernelArg(kernel, 9, local_mem_size, NULL));
    openCLSafeCall(clEnqueueNDRangeKernel(clCxt->impl->clCmdQueue, kernel, 2, NULL,
                                          globalThreads, localThreads, 0, NULL, NULL));

    clFinish(clCxt->impl->clCmdQueue);
    openCLSafeCall(clReleaseKernel(kernel));
}
//////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////operator/////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
static void operator_(oclMat &minSSD, oclMat &leBuf, oclMat &riBuf, int preset, int ndisp,
               int winSize, float avergeTexThreshold, const oclMat &left,
               const oclMat &right, oclMat &disparity)

{
    CV_DbgAssert(left.rows == right.rows && left.cols == right.cols);
    CV_DbgAssert(left.type() == CV_8UC1);
    CV_DbgAssert(right.type() == CV_8UC1);

    disparity.create(left.size(), CV_8UC1);
    minSSD.create(left.size(), CV_32SC1);

    oclMat le_for_bm =  left;
    oclMat ri_for_bm = right;

    if (preset == cv::ocl::StereoBM_OCL::PREFILTER_XSOBEL)
    {
        leBuf.create( left.size(),  left.type());
        riBuf.create(right.size(), right.type());

        prefilter_xsobel( left, leBuf, 31);
        prefilter_xsobel(right, riBuf, 31);

        le_for_bm = leBuf;
        ri_for_bm = riBuf;
    }

    stereo_bm(le_for_bm, ri_for_bm, disparity, ndisp, winSize, minSSD);

    if (avergeTexThreshold)
    {
        postfilter_textureness(le_for_bm, winSize, avergeTexThreshold, disparity);
    }
}
}
}
}
const float defaultAvgTexThreshold = 3;

cv::ocl::StereoBM_OCL::StereoBM_OCL()
    : preset(BASIC_PRESET), ndisp(DEFAULT_NDISP), winSize(DEFAULT_WINSZ),
      avergeTexThreshold(defaultAvgTexThreshold)  {}

cv::ocl::StereoBM_OCL::StereoBM_OCL(int preset_, int ndisparities_, int winSize_)
    : preset(preset_), ndisp(ndisparities_), winSize(winSize_),
      avergeTexThreshold(defaultAvgTexThreshold)
{
    const int max_supported_ndisp = 1 << (sizeof(unsigned char) * 8);
    CV_Assert(0 < ndisp && ndisp <= max_supported_ndisp);
    CV_Assert(ndisp % 8 == 0);
    CV_Assert(winSize % 2 == 1);
}

bool cv::ocl::StereoBM_OCL::checkIfGpuCallReasonable()
{
    return true;
}

void cv::ocl::StereoBM_OCL::operator() ( const oclMat &left, const oclMat &right,
        oclMat &disparity)
{
    cv::ocl::stereoBM::operator_(minSSD, leBuf, riBuf, preset, ndisp, winSize, avergeTexThreshold, left, right, disparity);
}

<|endoftext|>


<commit_before>/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include "modules/planning/planning.h"

#include <algorithm>

#include "google/protobuf/repeated_field.h"
#include "modules/common/adapters/adapter_manager.h"
#include "modules/common/time/time.h"
#include "modules/common/vehicle_state/vehicle_state.h"
#include "modules/common/vehicle_state/vehicle_state.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/planner/em/em_planner.h"
#include "modules/planning/planner/rtk/rtk_replay_planner.h"
#include "modules/planning/trajectory_stitcher/trajectory_stitcher.h"

namespace apollo {
namespace planning {

using apollo::common::ErrorCode;
using apollo::common::Status;
using apollo::common::TrajectoryPoint;
using apollo::common::VehicleState;
using apollo::common::adapter::AdapterManager;
using apollo::common::time::Clock;

std::string Planning::Name() const { return "planning"; }

void Planning::RegisterPlanners() {
  planner_factory_.Register(
      PlanningConfig::RTK, []() -> Planner* { return new RTKReplayPlanner(); });
  planner_factory_.Register(PlanningConfig::EM,
                            []() -> Planner* { return new EMPlanner(); });
}

const Frame* Planning::GetFrame() const { return frame_.get(); }
const hdmap::PncMap* Planning::GetPncMap() const { return pnc_map_.get(); }

bool Planning::InitFrame(const uint32_t sequence_num) {
  frame_.reset(new Frame(sequence_num));
  if (AdapterManager::GetRoutingResult()->Empty()) {
    AERROR << "Routing is empty";
    return false;
  }
  common::TrajectoryPoint point;
  frame_->SetVehicleInitPose(VehicleState::instance()->pose());
  frame_->SetRoutingResult(
      AdapterManager::GetRoutingResult()->GetLatestObserved());
  if (FLAGS_enable_prediction && !AdapterManager::GetPrediction()->Empty()) {
    const auto& prediction =
        AdapterManager::GetPrediction()->GetLatestObserved();
    frame_->SetPrediction(prediction);
    ADEBUG << "Get prediction";
  }

  if (!frame_->Init()) {
    AERROR << "failed to init frame";
    return false;
  }
  return true;
}

Status Planning::Init() {
  pnc_map_.reset(new hdmap::PncMap(FLAGS_map_filename));
  Frame::SetMap(pnc_map_.get());

  if (!apollo::common::util::GetProtoFromFile(FLAGS_planning_config_file,
                                              &config_)) {
    AERROR << "failed to load planning config file "
           << FLAGS_planning_config_file;
    return Status(
        ErrorCode::PLANNING_ERROR,
        "failed to load planning config file: " + FLAGS_planning_config_file);
  }

  AdapterManager::Init(FLAGS_adapter_config_path);
  if (AdapterManager::GetLocalization() == nullptr) {
    std::string error_msg("Localization is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (AdapterManager::GetChassis() == nullptr) {
    std::string error_msg("Chassis is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (AdapterManager::GetRoutingResult() == nullptr) {
    std::string error_msg("RoutingResult is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  // TODO(all) temporarily use the offline routing data.
  if (!AdapterManager::GetRoutingResult()->HasReceived()) {
    if (!AdapterManager::GetRoutingResult()->FeedFile(
            FLAGS_offline_routing_file)) {
      auto error_msg = common::util::StrCat(
          "Failed to load offline routing file ", FLAGS_offline_routing_file);
      AERROR << error_msg;
      return Status(ErrorCode::PLANNING_ERROR, error_msg);
    } else {
      AWARN << "Using offline routing file " << FLAGS_offline_routing_file;
    }
  }
  if (AdapterManager::GetPrediction() == nullptr) {
    std::string error_msg("Prediction is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }

  RegisterPlanners();
  planner_ = planner_factory_.CreateObject(config_.planner_type());
  if (!planner_) {
    return Status(
        ErrorCode::PLANNING_ERROR,
        "planning is not initialized with config : " + config_.DebugString());
  }

  return planner_->Init(config_);
}

Status Planning::Start() {
  static ros::Rate loop_rate(FLAGS_planning_loop_rate);
  while (ros::ok()) {
    RunOnce();
    ros::spinOnce();
    loop_rate.sleep();
  }
  return Status::OK();
}

void Planning::RecordInput(ADCTrajectory* trajectory_pb) {
  if (!FLAGS_enable_record_debug) {
    ADEBUG << "Skip record input into debug";
    return;
  }
  auto planning_data = trajectory_pb->mutable_debug()->mutable_planning_data();
  auto adc_position = planning_data->mutable_adc_position();
  const auto& localization =
      AdapterManager::GetLocalization()->GetLatestObserved();
  adc_position->CopyFrom(localization);

  const auto& chassis = AdapterManager::GetChassis()->GetLatestObserved();
  auto debug_chassis = planning_data->mutable_chassis();
  debug_chassis->CopyFrom(chassis);

  const auto& routing_result =
      AdapterManager::GetRoutingResult()->GetLatestObserved();

  auto debug_routing = planning_data->mutable_routing();
  debug_routing->CopyFrom(routing_result);
}

void Planning::RunOnce() {
  AdapterManager::Observe();
  if (AdapterManager::GetLocalization()->Empty()) {
    AERROR << "Localization is not available; skip the planning cycle";
    return;
  }

  if (AdapterManager::GetChassis()->Empty()) {
    AERROR << "Chassis is not available; skip the planning cycle";
    return;
  }
  if (AdapterManager::GetRoutingResult()->Empty()) {
    AERROR << "RoutingResult is not available; skip the planning cycle";
    return;
  }

  // FIXME(all): enable prediction check when perception and prediction is
  // ready.
  // if (FLAGS_enable_prediction && AdapterManager::GetPrediction()->Empty()) {
  //   AERROR << "Prediction is not available; skip the planning cycle";
  //   return;
  // }

  AINFO << "Start planning ...";
  const double start_timestamp = apollo::common::time::ToSecond(Clock::Now());

  // localization
  const auto& localization =
      AdapterManager::GetLocalization()->GetLatestObserved();
  ADEBUG << "Get localization:" << localization.DebugString();

  // chassis
  const auto& chassis = AdapterManager::GetChassis()->GetLatestObserved();
  ADEBUG << "Get chassis:" << chassis.DebugString();

  common::VehicleState::instance()->Update(localization, chassis);

  const double planning_cycle_time = 1.0 / FLAGS_planning_loop_rate;

  ADCTrajectory trajectory_pb;
  RecordInput(&trajectory_pb);

  const uint32_t frame_num = AdapterManager::GetPlanning()->GetSeqNum() + 1;
  if (!InitFrame(frame_num)) {
    AERROR << "Init frame failed";
    return;
  }

  bool is_auto_mode = chassis.driving_mode() == chassis.COMPLETE_AUTO_DRIVE;
  bool res_planning = Plan(is_auto_mode, start_timestamp, planning_cycle_time,
      &trajectory_pb);

  const double end_timestamp = apollo::common::time::ToSecond(Clock::Now());
  const double time_diff_ms = (end_timestamp - start_timestamp) * 1000;
  trajectory_pb.mutable_latency_stats()->set_total_time_ms(time_diff_ms);
  ADEBUG << "Planning latency: " << trajectory_pb.latency_stats().DebugString();

  if (res_planning) {
    AdapterManager::FillPlanningHeader("planning", &trajectory_pb);
    trajectory_pb.mutable_header()->set_timestamp_sec(start_timestamp);
    AdapterManager::PublishPlanning(trajectory_pb);
    ADEBUG << "Planning succeeded:" << trajectory_pb.header().DebugString();
  } else {
    AERROR << "Planning failed";
  }
}

void Planning::Stop() {}

bool Planning::Plan(const bool is_on_auto_mode,
                    const double current_time_stamp,
                    const double planning_cycle_time,
                    ADCTrajectory* trajectory_pb) {

  const auto& stitching_trajectory =
      TrajectoryStitcher::compute_stitching_trajectory(
          is_on_auto_mode, current_time_stamp, planning_cycle_time,
          last_publishable_trajectory_);

  frame_->SetPlanningStartPoint(stitching_trajectory.back());

  PublishableTrajectory publishable_trajectory;
  auto status = planner_->Plan(stitching_trajectory.back(),
      frame_.get(), &publishable_trajectory, trajectory_pb->mutable_debug(),
      trajectory_pb->mutable_latency_stats());

  if (status != Status::OK()) {
    AERROR << "planner failed to make a driving plan";
    last_publishable_trajectory_.Clear();
    return false;
  }

  publishable_trajectory.prepend_trajectory_points(
      stitching_trajectory.begin(), stitching_trajectory.end() - 1);

  publishable_trajectory.set_header_time(current_time_stamp);

  publishable_trajectory.populate_trajectory_protobuf(trajectory_pb);

  // update last publishable trajectory;
  last_publishable_trajectory_ = std::move(publishable_trajectory);
  return true;
}

}  // namespace planning
}  // namespace apollo
<commit_msg>planning passes GEAR_DRIVE.<commit_after>/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include "modules/planning/planning.h"

#include <algorithm>

#include "google/protobuf/repeated_field.h"
#include "modules/common/adapters/adapter_manager.h"
#include "modules/common/time/time.h"
#include "modules/common/vehicle_state/vehicle_state.h"
#include "modules/common/vehicle_state/vehicle_state.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/planner/em/em_planner.h"
#include "modules/planning/planner/rtk/rtk_replay_planner.h"
#include "modules/planning/trajectory_stitcher/trajectory_stitcher.h"

namespace apollo {
namespace planning {

using apollo::common::ErrorCode;
using apollo::common::Status;
using apollo::common::TrajectoryPoint;
using apollo::common::VehicleState;
using apollo::common::adapter::AdapterManager;
using apollo::common::time::Clock;

std::string Planning::Name() const { return "planning"; }

void Planning::RegisterPlanners() {
  planner_factory_.Register(
      PlanningConfig::RTK, []() -> Planner* { return new RTKReplayPlanner(); });
  planner_factory_.Register(PlanningConfig::EM,
                            []() -> Planner* { return new EMPlanner(); });
}

const Frame* Planning::GetFrame() const { return frame_.get(); }
const hdmap::PncMap* Planning::GetPncMap() const { return pnc_map_.get(); }

bool Planning::InitFrame(const uint32_t sequence_num) {
  frame_.reset(new Frame(sequence_num));
  if (AdapterManager::GetRoutingResult()->Empty()) {
    AERROR << "Routing is empty";
    return false;
  }
  common::TrajectoryPoint point;
  frame_->SetVehicleInitPose(VehicleState::instance()->pose());
  frame_->SetRoutingResult(
      AdapterManager::GetRoutingResult()->GetLatestObserved());
  if (FLAGS_enable_prediction && !AdapterManager::GetPrediction()->Empty()) {
    const auto& prediction =
        AdapterManager::GetPrediction()->GetLatestObserved();
    frame_->SetPrediction(prediction);
    ADEBUG << "Get prediction";
  }

  if (!frame_->Init()) {
    AERROR << "failed to init frame";
    return false;
  }
  return true;
}

Status Planning::Init() {
  pnc_map_.reset(new hdmap::PncMap(FLAGS_map_filename));
  Frame::SetMap(pnc_map_.get());

  if (!apollo::common::util::GetProtoFromFile(FLAGS_planning_config_file,
                                              &config_)) {
    AERROR << "failed to load planning config file "
           << FLAGS_planning_config_file;
    return Status(
        ErrorCode::PLANNING_ERROR,
        "failed to load planning config file: " + FLAGS_planning_config_file);
  }

  AdapterManager::Init(FLAGS_adapter_config_path);
  if (AdapterManager::GetLocalization() == nullptr) {
    std::string error_msg("Localization is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (AdapterManager::GetChassis() == nullptr) {
    std::string error_msg("Chassis is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (AdapterManager::GetRoutingResult() == nullptr) {
    std::string error_msg("RoutingResult is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  // TODO(all) temporarily use the offline routing data.
  if (!AdapterManager::GetRoutingResult()->HasReceived()) {
    if (!AdapterManager::GetRoutingResult()->FeedFile(
            FLAGS_offline_routing_file)) {
      auto error_msg = common::util::StrCat(
          "Failed to load offline routing file ", FLAGS_offline_routing_file);
      AERROR << error_msg;
      return Status(ErrorCode::PLANNING_ERROR, error_msg);
    } else {
      AWARN << "Using offline routing file " << FLAGS_offline_routing_file;
    }
  }
  if (AdapterManager::GetPrediction() == nullptr) {
    std::string error_msg("Prediction is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }

  RegisterPlanners();
  planner_ = planner_factory_.CreateObject(config_.planner_type());
  if (!planner_) {
    return Status(
        ErrorCode::PLANNING_ERROR,
        "planning is not initialized with config : " + config_.DebugString());
  }

  return planner_->Init(config_);
}

Status Planning::Start() {
  static ros::Rate loop_rate(FLAGS_planning_loop_rate);
  while (ros::ok()) {
    RunOnce();
    ros::spinOnce();
    loop_rate.sleep();
  }
  return Status::OK();
}

void Planning::RecordInput(ADCTrajectory* trajectory_pb) {
  if (!FLAGS_enable_record_debug) {
    ADEBUG << "Skip record input into debug";
    return;
  }
  auto planning_data = trajectory_pb->mutable_debug()->mutable_planning_data();
  auto adc_position = planning_data->mutable_adc_position();
  const auto& localization =
      AdapterManager::GetLocalization()->GetLatestObserved();
  adc_position->CopyFrom(localization);

  const auto& chassis = AdapterManager::GetChassis()->GetLatestObserved();
  auto debug_chassis = planning_data->mutable_chassis();
  debug_chassis->CopyFrom(chassis);

  const auto& routing_result =
      AdapterManager::GetRoutingResult()->GetLatestObserved();

  auto debug_routing = planning_data->mutable_routing();
  debug_routing->CopyFrom(routing_result);
}

void Planning::RunOnce() {
  AdapterManager::Observe();
  if (AdapterManager::GetLocalization()->Empty()) {
    AERROR << "Localization is not available; skip the planning cycle";
    return;
  }

  if (AdapterManager::GetChassis()->Empty()) {
    AERROR << "Chassis is not available; skip the planning cycle";
    return;
  }
  if (AdapterManager::GetRoutingResult()->Empty()) {
    AERROR << "RoutingResult is not available; skip the planning cycle";
    return;
  }

  // FIXME(all): enable prediction check when perception and prediction is
  // ready.
  // if (FLAGS_enable_prediction && AdapterManager::GetPrediction()->Empty()) {
  //   AERROR << "Prediction is not available; skip the planning cycle";
  //   return;
  // }

  AINFO << "Start planning ...";
  const double start_timestamp = apollo::common::time::ToSecond(Clock::Now());

  // localization
  const auto& localization =
      AdapterManager::GetLocalization()->GetLatestObserved();
  ADEBUG << "Get localization:" << localization.DebugString();

  // chassis
  const auto& chassis = AdapterManager::GetChassis()->GetLatestObserved();
  ADEBUG << "Get chassis:" << chassis.DebugString();

  common::VehicleState::instance()->Update(localization, chassis);

  const double planning_cycle_time = 1.0 / FLAGS_planning_loop_rate;

  ADCTrajectory trajectory_pb;
  RecordInput(&trajectory_pb);

  const uint32_t frame_num = AdapterManager::GetPlanning()->GetSeqNum() + 1;
  if (!InitFrame(frame_num)) {
    AERROR << "Init frame failed";
    return;
  }

  bool is_auto_mode = chassis.driving_mode() == chassis.COMPLETE_AUTO_DRIVE;
  bool res_planning = Plan(is_auto_mode, start_timestamp, planning_cycle_time,
      &trajectory_pb);

  const double end_timestamp = apollo::common::time::ToSecond(Clock::Now());
  const double time_diff_ms = (end_timestamp - start_timestamp) * 1000;
  trajectory_pb.mutable_latency_stats()->set_total_time_ms(time_diff_ms);
  ADEBUG << "Planning latency: " << trajectory_pb.latency_stats().DebugString();

  if (res_planning) {
    AdapterManager::FillPlanningHeader("planning", &trajectory_pb);
    trajectory_pb.mutable_header()->set_timestamp_sec(start_timestamp);
    // TODO(all): integrate reverse gear
    trajectory_pb.set_gear(canbus::Chassis::GEAR_DRIVE);
    AdapterManager::PublishPlanning(trajectory_pb);
    ADEBUG << "Planning succeeded:" << trajectory_pb.header().DebugString();
  } else {
    AERROR << "Planning failed";
  }
}

void Planning::Stop() {}

bool Planning::Plan(const bool is_on_auto_mode,
                    const double current_time_stamp,
                    const double planning_cycle_time,
                    ADCTrajectory* trajectory_pb) {

  const auto& stitching_trajectory =
      TrajectoryStitcher::compute_stitching_trajectory(
          is_on_auto_mode, current_time_stamp, planning_cycle_time,
          last_publishable_trajectory_);

  frame_->SetPlanningStartPoint(stitching_trajectory.back());

  PublishableTrajectory publishable_trajectory;
  auto status = planner_->Plan(stitching_trajectory.back(),
      frame_.get(), &publishable_trajectory, trajectory_pb->mutable_debug(),
      trajectory_pb->mutable_latency_stats());

  if (status != Status::OK()) {
    AERROR << "planner failed to make a driving plan";
    last_publishable_trajectory_.Clear();
    return false;
  }

  publishable_trajectory.prepend_trajectory_points(
      stitching_trajectory.begin(), stitching_trajectory.end() - 1);

  publishable_trajectory.set_header_time(current_time_stamp);

  publishable_trajectory.populate_trajectory_protobuf(trajectory_pb);

  // update last publishable trajectory;
  last_publishable_trajectory_ = std::move(publishable_trajectory);
  return true;
}

}  // namespace planning
}  // namespace apollo
<|endoftext|>


<commit_before>/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include "modules/planning/planning.h"

#include <algorithm>

#include "google/protobuf/repeated_field.h"
#include "modules/common/adapters/adapter_manager.h"
#include "modules/common/time/time.h"
#include "modules/common/vehicle_state/vehicle_state.h"
#include "modules/planning/common/data_center.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/planner/em/em_planner.h"
#include "modules/planning/planner/rtk/rtk_replay_planner.h"
#include "modules/planning/trajectory_stitcher/trajectory_stitcher.h"

namespace apollo {
namespace planning {

using apollo::common::TrajectoryPoint;
using apollo::common::adapter::AdapterManager;
using apollo::common::time::Clock;
using apollo::common::Status;
using apollo::common::ErrorCode;

std::string Planning::Name() const { return "planning"; }

void Planning::RegisterPlanners() {
  planner_factory_.Register(
      PlanningConfig::RTK, []() -> Planner* { return new RTKReplayPlanner(); });
  planner_factory_.Register(PlanningConfig::EM,
                            []() -> Planner* { return new EMPlanner(); });
}

Status Planning::Init() {
  if (!apollo::common::util::GetProtoFromFile(FLAGS_planning_config_file,
                                              &config_)) {
    AERROR << "failed to load planning config file "
           << FLAGS_planning_config_file;
    return Status(
        ErrorCode::PLANNING_ERROR,
        "failed to load planning config file: " + FLAGS_planning_config_file);
  }

  AdapterManager::Init(FLAGS_adapter_config_path);
  if (AdapterManager::GetLocalization() == nullptr) {
    std::string error_msg("Localization is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (AdapterManager::GetChassis() == nullptr) {
    std::string error_msg("Chassis is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (AdapterManager::GetRoutingResult() == nullptr) {
    std::string error_msg("RoutingResult is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  // TODO(all) temporarily use the offline routing data.
  if (!AdapterManager::GetRoutingResult()->HasReceived()) {
    if (!AdapterManager::GetRoutingResult()->FeedFile(
            FLAGS_offline_routing_file)) {
      auto error_msg = common::util::StrCat(
          "Failed to load offline routing file ", FLAGS_offline_routing_file);
      AERROR << error_msg;
      return Status(ErrorCode::PLANNING_ERROR, error_msg);
    } else {
      AWARN << "Using offline routing file " << FLAGS_offline_routing_file;
    }
  }
  if (AdapterManager::GetPrediction() == nullptr) {
    std::string error_msg("Prediction is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }

  RegisterPlanners();
  planner_ = planner_factory_.CreateObject(config_.planner_type());
  if (!planner_) {
    return Status(
        ErrorCode::PLANNING_ERROR,
        "planning is not initialized with config : " + config_.DebugString());
  }

  return planner_->Init(config_);
}

Status Planning::Start() {
  static ros::Rate loop_rate(FLAGS_planning_loop_rate);
  while (ros::ok()) {
    RunOnce();
    ros::spinOnce();
    loop_rate.sleep();
  }
  return Status::OK();
}

void Planning::RecordInput(ADCTrajectory* trajectory_pb) {
  if (!FLAGS_enable_record_debug) {
    ADEBUG << "Skip record input into debug";
    return;
  }
  auto planning_data = trajectory_pb->mutable_debug()->mutable_planning_data();
  auto adc_position = planning_data->mutable_adc_position();
  const auto& localization =
      AdapterManager::GetLocalization()->GetLatestObserved();
  adc_position->CopyFrom(localization);

  const auto& chassis = AdapterManager::GetChassis()->GetLatestObserved();
  auto debug_chassis = planning_data->mutable_chassis();
  debug_chassis->CopyFrom(chassis);

  const auto& routing_result =
      AdapterManager::GetRoutingResult()->GetLatestObserved();

  auto debug_routing = planning_data->mutable_routing();
  debug_routing->CopyFrom(routing_result);
}

void Planning::RunOnce() {
  AdapterManager::Observe();
  if (AdapterManager::GetLocalization()->Empty()) {
    AERROR << "Localization is not available; skip the planning cycle";
    return;
  }

  if (AdapterManager::GetChassis()->Empty()) {
    AERROR << "Chassis is not available; skip the planning cycle";
    return;
  }
  if (AdapterManager::GetRoutingResult()->Empty()) {
    AERROR << "RoutingResult is not available; skip the planning cycle";
    return;
  }

  // FIXME(all): enable prediction check when perception and prediction is
  // ready.
  // if (FLAGS_enable_prediction && AdapterManager::GetPrediction()->Empty()) {
  //   AERROR << "Prediction is not available; skip the planning cycle";
  //   return;
  // }

  AINFO << "Start planning ...";
  const double start_timestamp = apollo::common::time::ToSecond(Clock::Now());

  // localization
  const auto& localization =
      AdapterManager::GetLocalization()->GetLatestObserved();
  ADEBUG << "Get localization:" << localization.DebugString();

  // chassis
  const auto& chassis = AdapterManager::GetChassis()->GetLatestObserved();
  ADEBUG << "Get chassis:" << chassis.DebugString();

  common::VehicleState::instance()->Update(localization, chassis);

  double planning_cycle_time = 1.0 / FLAGS_planning_loop_rate;
  // the execution_start_time is the estimated time when the planned trajectory
  // will be executed by the controller.
  double execution_start_time =
      apollo::common::time::ToSecond(apollo::common::time::Clock::Now()) +
      planning_cycle_time;

  ADCTrajectory trajectory_pb;
  RecordInput(&trajectory_pb);

  if (!DataCenter::instance()->init_current_frame(
          AdapterManager::GetPlanning()->GetSeqNum() + 1)) {
    AERROR << "DataCenter init frame failed";
    return;
  }

  bool is_auto_mode = chassis.driving_mode() == chassis.COMPLETE_AUTO_DRIVE;
  bool res_planning = Plan(is_auto_mode, execution_start_time, &trajectory_pb);

  const double end_timestamp = apollo::common::time::ToSecond(Clock::Now());
  const double time_diff_ms = (end_timestamp - start_timestamp) * 1000;
  trajectory_pb.mutable_latency_stats()->set_total_time_ms(time_diff_ms);
  ADEBUG << "Planning latency: " << trajectory_pb.latency_stats().DebugString();

  if (res_planning) {
    AdapterManager::FillPlanningHeader("planning",
                                       trajectory_pb.mutable_header());
    trajectory_pb.mutable_header()->set_timestamp_sec(execution_start_time);
    AdapterManager::PublishPlanning(trajectory_pb);
    ADEBUG << "Planning succeeded:" << trajectory_pb.header().DebugString();
  } else {
    AERROR << "Planning failed";
  }
}

void Planning::Stop() {}

bool Planning::Plan(const bool is_on_auto_mode, const double publish_time,
                    ADCTrajectory* trajectory_pb) {
  // if 1. the auto-driving mode is off or
  //    2. we don't have the trajectory from last planning cycle or
  //    3. the position deviation from actual and target is too high
  // then planning from current vehicle state.
  auto stitching_trajectory = TrajectoryStitcher::compute_stitching_trajectory(
      last_publishable_trajectory_);

  auto planning_start_point = stitching_trajectory.back();

  if (FLAGS_enable_record_debug) {
    trajectory_pb->mutable_debug()
        ->mutable_planning_data()
        ->mutable_init_point()
        ->CopyFrom(stitching_trajectory.back());
    trajectory_pb->mutable_debug()->mutable_planning_data()->set_is_replan(
        true);
  }

  auto status = planner_->Plan(planning_start_point, trajectory_pb);
  if (status != Status::OK()) {
    AERROR << "planner failed to make a driving plan";
    last_publishable_trajectory_.Clear();
    return false;
  }

  InsertFrontTrajectoryPoints(trajectory_pb, stitching_trajectory);

  // update last publishable trajectory;
  last_publishable_trajectory_.Clear();
  for (int i = 0; i < trajectory_pb->trajectory_point_size(); ++i) {
    last_publishable_trajectory_.add_trajectory_point(
        trajectory_pb->trajectory_point(i));
  }
  last_publishable_trajectory_.set_header_time(
      trajectory_pb->header().timestamp_sec());
  return true;
}

void Planning::InsertFrontTrajectoryPoints(
    ADCTrajectory* trajectory_pb,
    const std::vector<apollo::common::TrajectoryPoint>& points) const {
  if (points.empty()) {
    AINFO << "stitch points are empty";
    return;
  }

  int i = 0;
  int j = trajectory_pb->trajectory_point_size() - 1;
  while (i < j) {
    trajectory_pb->mutable_trajectory_point()->SwapElements(i, j);
    ++i;
    --j;
  }

  j = points.size() - 1;
  while (j >= 0) {
    trajectory_pb->add_trajectory_point()->CopyFrom(points[j]);
    --j;
  }

  i = 0;
  j = trajectory_pb->trajectory_point_size() - 1;
  while (i < j) {
    trajectory_pb->mutable_trajectory_point()->SwapElements(i, j);
    ++i;
    --j;
  }
  return;
}

}  // namespace planning
}  // namespace apollo
<commit_msg>Fixed the merging error in trajectory stitching<commit_after>/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include "modules/planning/planning.h"

#include <algorithm>

#include "google/protobuf/repeated_field.h"
#include "modules/common/adapters/adapter_manager.h"
#include "modules/common/time/time.h"
#include "modules/common/vehicle_state/vehicle_state.h"
#include "modules/planning/common/data_center.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/planner/em/em_planner.h"
#include "modules/planning/planner/rtk/rtk_replay_planner.h"
#include "modules/planning/trajectory_stitcher/trajectory_stitcher.h"

namespace apollo {
namespace planning {

using apollo::common::TrajectoryPoint;
using apollo::common::adapter::AdapterManager;
using apollo::common::time::Clock;
using apollo::common::Status;
using apollo::common::ErrorCode;

std::string Planning::Name() const { return "planning"; }

void Planning::RegisterPlanners() {
  planner_factory_.Register(
      PlanningConfig::RTK, []() -> Planner* { return new RTKReplayPlanner(); });
  planner_factory_.Register(PlanningConfig::EM,
                            []() -> Planner* { return new EMPlanner(); });
}

Status Planning::Init() {
  if (!apollo::common::util::GetProtoFromFile(FLAGS_planning_config_file,
                                              &config_)) {
    AERROR << "failed to load planning config file "
           << FLAGS_planning_config_file;
    return Status(
        ErrorCode::PLANNING_ERROR,
        "failed to load planning config file: " + FLAGS_planning_config_file);
  }

  AdapterManager::Init(FLAGS_adapter_config_path);
  if (AdapterManager::GetLocalization() == nullptr) {
    std::string error_msg("Localization is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (AdapterManager::GetChassis() == nullptr) {
    std::string error_msg("Chassis is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (AdapterManager::GetRoutingResult() == nullptr) {
    std::string error_msg("RoutingResult is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  // TODO(all) temporarily use the offline routing data.
  if (!AdapterManager::GetRoutingResult()->HasReceived()) {
    if (!AdapterManager::GetRoutingResult()->FeedFile(
            FLAGS_offline_routing_file)) {
      auto error_msg = common::util::StrCat(
          "Failed to load offline routing file ", FLAGS_offline_routing_file);
      AERROR << error_msg;
      return Status(ErrorCode::PLANNING_ERROR, error_msg);
    } else {
      AWARN << "Using offline routing file " << FLAGS_offline_routing_file;
    }
  }
  if (AdapterManager::GetPrediction() == nullptr) {
    std::string error_msg("Prediction is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }

  RegisterPlanners();
  planner_ = planner_factory_.CreateObject(config_.planner_type());
  if (!planner_) {
    return Status(
        ErrorCode::PLANNING_ERROR,
        "planning is not initialized with config : " + config_.DebugString());
  }

  return planner_->Init(config_);
}

Status Planning::Start() {
  static ros::Rate loop_rate(FLAGS_planning_loop_rate);
  while (ros::ok()) {
    RunOnce();
    ros::spinOnce();
    loop_rate.sleep();
  }
  return Status::OK();
}

void Planning::RecordInput(ADCTrajectory* trajectory_pb) {
  if (!FLAGS_enable_record_debug) {
    ADEBUG << "Skip record input into debug";
    return;
  }
  auto planning_data = trajectory_pb->mutable_debug()->mutable_planning_data();
  auto adc_position = planning_data->mutable_adc_position();
  const auto& localization =
      AdapterManager::GetLocalization()->GetLatestObserved();
  adc_position->CopyFrom(localization);

  const auto& chassis = AdapterManager::GetChassis()->GetLatestObserved();
  auto debug_chassis = planning_data->mutable_chassis();
  debug_chassis->CopyFrom(chassis);

  const auto& routing_result =
      AdapterManager::GetRoutingResult()->GetLatestObserved();

  auto debug_routing = planning_data->mutable_routing();
  debug_routing->CopyFrom(routing_result);
}

void Planning::RunOnce() {
  AdapterManager::Observe();
  if (AdapterManager::GetLocalization()->Empty()) {
    AERROR << "Localization is not available; skip the planning cycle";
    return;
  }

  if (AdapterManager::GetChassis()->Empty()) {
    AERROR << "Chassis is not available; skip the planning cycle";
    return;
  }
  if (AdapterManager::GetRoutingResult()->Empty()) {
    AERROR << "RoutingResult is not available; skip the planning cycle";
    return;
  }

  // FIXME(all): enable prediction check when perception and prediction is
  // ready.
  // if (FLAGS_enable_prediction && AdapterManager::GetPrediction()->Empty()) {
  //   AERROR << "Prediction is not available; skip the planning cycle";
  //   return;
  // }

  AINFO << "Start planning ...";
  const double start_timestamp = apollo::common::time::ToSecond(Clock::Now());

  // localization
  const auto& localization =
      AdapterManager::GetLocalization()->GetLatestObserved();
  ADEBUG << "Get localization:" << localization.DebugString();

  // chassis
  const auto& chassis = AdapterManager::GetChassis()->GetLatestObserved();
  ADEBUG << "Get chassis:" << chassis.DebugString();

  common::VehicleState::instance()->Update(localization, chassis);

  double planning_cycle_time = 1.0 / FLAGS_planning_loop_rate;
  // the execution_start_time is the estimated time when the planned trajectory
  // will be executed by the controller.
  double execution_start_time =
      apollo::common::time::ToSecond(apollo::common::time::Clock::Now()) +
      planning_cycle_time;

  ADCTrajectory trajectory_pb;
  RecordInput(&trajectory_pb);

  if (!DataCenter::instance()->init_current_frame(
          AdapterManager::GetPlanning()->GetSeqNum() + 1)) {
    AERROR << "DataCenter init frame failed";
    return;
  }

  bool is_auto_mode = chassis.driving_mode() == chassis.COMPLETE_AUTO_DRIVE;
  bool res_planning = Plan(is_auto_mode, execution_start_time, &trajectory_pb);

  const double end_timestamp = apollo::common::time::ToSecond(Clock::Now());
  const double time_diff_ms = (end_timestamp - start_timestamp) * 1000;
  trajectory_pb.mutable_latency_stats()->set_total_time_ms(time_diff_ms);
  ADEBUG << "Planning latency: " << trajectory_pb.latency_stats().DebugString();

  if (res_planning) {
    AdapterManager::FillPlanningHeader("planning",
                                       trajectory_pb.mutable_header());
    trajectory_pb.mutable_header()->set_timestamp_sec(execution_start_time);
    AdapterManager::PublishPlanning(trajectory_pb);
    ADEBUG << "Planning succeeded:" << trajectory_pb.header().DebugString();
  } else {
    AERROR << "Planning failed";
  }
}

void Planning::Stop() {}

bool Planning::Plan(const bool is_on_auto_mode, const double publish_time,
                    ADCTrajectory* trajectory_pb) {
  // if 1. the auto-driving mode is off or
  //    2. we don't have the trajectory from last planning cycle or
  //    3. the position deviation from actual and target is too high
  // then planning from current vehicle state.
  auto stitching_trajectory = TrajectoryStitcher::compute_stitching_trajectory(
      last_publishable_trajectory_);

  auto planning_start_point = stitching_trajectory.back();

  if (FLAGS_enable_record_debug) {
    trajectory_pb->mutable_debug()
        ->mutable_planning_data()
        ->mutable_init_point()
        ->CopyFrom(stitching_trajectory.back());
    trajectory_pb->mutable_debug()->mutable_planning_data()->set_is_replan(
        true);
  }

  auto status = planner_->Plan(planning_start_point, trajectory_pb);
  if (status != Status::OK()) {
    AERROR << "planner failed to make a driving plan";
    last_publishable_trajectory_.Clear();
    return false;
  }

  InsertFrontTrajectoryPoints(trajectory_pb,
          std::vector<decltype(planning_start_point)>(
                  stitching_trajectory.begin(),
                  stitching_trajectory.end() - 1));

  // update last publishable trajectory;
  last_publishable_trajectory_.Clear();
  for (int i = 0; i < trajectory_pb->trajectory_point_size(); ++i) {
    last_publishable_trajectory_.add_trajectory_point(
        trajectory_pb->trajectory_point(i));
  }
  last_publishable_trajectory_.set_header_time(
      trajectory_pb->header().timestamp_sec());
  return true;
}

void Planning::InsertFrontTrajectoryPoints(
    ADCTrajectory* trajectory_pb,
    const std::vector<apollo::common::TrajectoryPoint>& points) const {
  if (points.empty()) {
    AINFO << "stitch points are empty";
    return;
  }

  int i = 0;
  int j = trajectory_pb->trajectory_point_size() - 1;
  while (i < j) {
    trajectory_pb->mutable_trajectory_point()->SwapElements(i, j);
    ++i;
    --j;
  }

  j = points.size() - 1;
  while (j >= 0) {
    trajectory_pb->add_trajectory_point()->CopyFrom(points[j]);
    --j;
  }

  i = 0;
  j = trajectory_pb->trajectory_point_size() - 1;
  while (i < j) {
    trajectory_pb->mutable_trajectory_point()->SwapElements(i, j);
    ++i;
    --j;
  }
  return;
}

}  // namespace planning
}  // namespace apollo
<|endoftext|>


<commit_before>
// WorldStorage.cpp

// Implements the cWorldStorage class representing the chunk loading / saving thread

// To add a new storage schema, implement a cWSSchema descendant and add it to cWorldStorage::InitSchemas()

#include "Globals.h"
#include "WorldStorage.h"
#include "WSSCompact.h"
#include "cWorld.h"
#include "cChunkGenerator.h"

/// Example storage schema - forgets all chunks ;)
class cWSSForgetful :
	public cWSSchema
{
public:
	cWSSForgetful(cWorld * a_World) : cWSSchema(a_World) {}

protected:
	// cWSSchema overrides:
	virtual bool LoadChunk(const cChunkPtr & a_Chunk) override {return false; }
	virtual bool SaveChunk(const cChunkPtr & a_Chunk) override {return true; }
	virtual const AString GetName(void) const override {return "forgetful"; }
} ;

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// cWorldStorage:

cWorldStorage::cWorldStorage(void) :
	super("cWorldStorage"),
	m_World(NULL),
	m_SaveSchema(NULL)
{
}

cWorldStorage::~cWorldStorage()
{
	for (cWSSchemaList::iterator itr = m_Schemas.begin(); itr != m_Schemas.end(); ++itr)
	{
		delete *itr;
	}  // for itr - m_Schemas[]
	m_LoadQueue.clear();
	m_SaveQueue.clear();
}

bool cWorldStorage::Start(cWorld * a_World, const AString & a_StorageSchemaName)
{
	m_World = a_World;
	m_StorageSchemaName = a_StorageSchemaName;
	InitSchemas();

	return super::Start();
}

void cWorldStorage::WaitForFinish(void)
{
	LOG("Waiting for the world storage to finish saving");

	// Cancel all loading requests:
	cCSLock Lock(m_CSLoadQueue);
	m_LoadQueue.clear();

	// Wait for the thread to finish:
	mShouldTerminate = true;
	m_Event.Set();
	super::Wait();
}

void cWorldStorage::QueueLoadChunk(cChunkPtr & a_Chunk)
{
	// Queues the chunk for loading; if not loaded, the chunk will be generated
	cCSLock Lock(m_CSLoadQueue);
	m_LoadQueue.remove(a_Chunk);  // Don't add twice
	m_LoadQueue.push_back(a_Chunk);
	m_Event.Set();
}

void cWorldStorage::QueueSaveChunk(cChunkPtr & a_Chunk)
{
	cCSLock Lock(m_CSSaveQueue);
	m_SaveQueue.remove(a_Chunk);  // Don't add twice
	m_SaveQueue.push_back(a_Chunk);
	m_Event.Set();
}

void cWorldStorage::UnqueueLoad(const cChunkPtr & a_Chunk)
{
	cCSLock Lock(m_CSLoadQueue);
	m_LoadQueue.remove(a_Chunk);
}

void cWorldStorage::UnqueueSave(const cChunkPtr & a_Chunk)
{
	cCSLock Lock(m_CSSaveQueue);
	m_SaveQueue.remove(a_Chunk);
}

void cWorldStorage::InitSchemas(void)
{
	// The first schema added is considered the default
	m_Schemas.push_back(new cWSSCompact(m_World));
	m_Schemas.push_back(new cWSSForgetful(m_World));
	// Add new schemas here

	if (m_StorageSchemaName == "Default")
	{
		m_SaveSchema = m_Schemas.front();
		return;
	}
	for (cWSSchemaList::iterator itr = m_Schemas.begin(); itr != m_Schemas.end(); ++itr)
	{
		if ((*itr)->GetName() == m_StorageSchemaName)
		{
			m_SaveSchema = *itr;
			return;
		}
	}  // for itr - m_Schemas[]

	// Unknown schema selected, let the admin know:
	LOGWARNING("Unknown storage schema name \"%s\". Using default. Available schemas:", m_StorageSchemaName.c_str());
	for (cWSSchemaList::iterator itr = m_Schemas.begin(); itr != m_Schemas.end(); ++itr)
	{
		LOGWARNING("\t\"%s\"", (*itr)->GetName().c_str());
	}
	m_SaveSchema = m_Schemas.front();
}

void cWorldStorage::Execute(void)
{
	while (!mShouldTerminate)
	{
		m_Event.Wait();

		// Process both queues until they are empty again:
		bool HasMore;
		do
		{
			HasMore = false;
			if (mShouldTerminate)
			{
				return;
			}

			// Load 1 chunk:
			cChunkPtr ToLoad;
			{
				cCSLock Lock(m_CSLoadQueue);
				if (m_LoadQueue.size() > 0)
				{
					ToLoad = m_LoadQueue.front();
					m_LoadQueue.pop_front();
				}
				HasMore = (m_LoadQueue.size() > 0);
			}
			if ((ToLoad != NULL) && !LoadChunk(ToLoad))
			{
				// The chunk couldn't be loaded, generate it:
				m_World->GetGenerator().GenerateChunk(ToLoad->GetPosX(), ToLoad->GetPosZ());
			}

			// Save 1 chunk:
			cChunkPtr Save;
			{
				cCSLock Lock(m_CSSaveQueue);
				if (m_SaveQueue.size() > 0)
				{
					Save = m_SaveQueue.front();
					m_SaveQueue.pop_front();
				}
				HasMore = HasMore || (m_SaveQueue.size() > 0);
			}
			if ((Save != NULL) && (!m_SaveSchema->SaveChunk(Save)))
			{
				LOGWARNING("Cannot save chunk [%d, %d]", Save->GetPosX(), Save->GetPosZ());
			}
		} while (HasMore);
	}
}

bool cWorldStorage::LoadChunk(const cChunkPtr & a_Chunk)
{
	if (a_Chunk->IsValid())
	{
		// Already loaded (can happen, since the queue is async)
		return true;
	}

	if (m_SaveSchema->LoadChunk(a_Chunk))
	{
		return true;
	}

	for (cWSSchemaList::iterator itr = m_Schemas.begin(); itr != m_Schemas.end(); ++itr)
	{
		if ((*itr)->LoadChunk(a_Chunk))
		{
			return true;
		}
	}

	return false;
}

<commit_msg>Fixed a deadlock in WorldStorage<commit_after>
// WorldStorage.cpp

// Implements the cWorldStorage class representing the chunk loading / saving thread

// To add a new storage schema, implement a cWSSchema descendant and add it to cWorldStorage::InitSchemas()

#include "Globals.h"
#include "WorldStorage.h"
#include "WSSCompact.h"
#include "cWorld.h"
#include "cChunkGenerator.h"

/// Example storage schema - forgets all chunks ;)
class cWSSForgetful :
	public cWSSchema
{
public:
	cWSSForgetful(cWorld * a_World) : cWSSchema(a_World) {}

protected:
	// cWSSchema overrides:
	virtual bool LoadChunk(const cChunkPtr & a_Chunk) override {return false; }
	virtual bool SaveChunk(const cChunkPtr & a_Chunk) override {return true; }
	virtual const AString GetName(void) const override {return "forgetful"; }
} ;

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// cWorldStorage:

cWorldStorage::cWorldStorage(void) :
	super("cWorldStorage"),
	m_World(NULL),
	m_SaveSchema(NULL)
{
}

cWorldStorage::~cWorldStorage()
{
	for (cWSSchemaList::iterator itr = m_Schemas.begin(); itr != m_Schemas.end(); ++itr)
	{
		delete *itr;
	}  // for itr - m_Schemas[]
	m_LoadQueue.clear();
	m_SaveQueue.clear();
}

bool cWorldStorage::Start(cWorld * a_World, const AString & a_StorageSchemaName)
{
	m_World = a_World;
	m_StorageSchemaName = a_StorageSchemaName;
	InitSchemas();

	return super::Start();
}

void cWorldStorage::WaitForFinish(void)
{
	LOG("Waiting for the world storage to finish saving");

	{
		// Cancel all loading requests:
		cCSLock Lock(m_CSLoadQueue);
		m_LoadQueue.clear();
	}

	// Wait for the thread to finish:
	mShouldTerminate = true;
	m_Event.Set();
	super::Wait();
}

void cWorldStorage::QueueLoadChunk(cChunkPtr & a_Chunk)
{
	// Queues the chunk for loading; if not loaded, the chunk will be generated
	cCSLock Lock(m_CSLoadQueue);
	m_LoadQueue.remove(a_Chunk);  // Don't add twice
	m_LoadQueue.push_back(a_Chunk);
	m_Event.Set();
}

void cWorldStorage::QueueSaveChunk(cChunkPtr & a_Chunk)
{
	cCSLock Lock(m_CSSaveQueue);
	m_SaveQueue.remove(a_Chunk);  // Don't add twice
	m_SaveQueue.push_back(a_Chunk);
	m_Event.Set();
}

void cWorldStorage::UnqueueLoad(const cChunkPtr & a_Chunk)
{
	cCSLock Lock(m_CSLoadQueue);
	m_LoadQueue.remove(a_Chunk);
}

void cWorldStorage::UnqueueSave(const cChunkPtr & a_Chunk)
{
	cCSLock Lock(m_CSSaveQueue);
	m_SaveQueue.remove(a_Chunk);
}

void cWorldStorage::InitSchemas(void)
{
	// The first schema added is considered the default
	m_Schemas.push_back(new cWSSCompact(m_World));
	m_Schemas.push_back(new cWSSForgetful(m_World));
	// Add new schemas here

	if (m_StorageSchemaName == "Default")
	{
		m_SaveSchema = m_Schemas.front();
		return;
	}
	for (cWSSchemaList::iterator itr = m_Schemas.begin(); itr != m_Schemas.end(); ++itr)
	{
		if ((*itr)->GetName() == m_StorageSchemaName)
		{
			m_SaveSchema = *itr;
			return;
		}
	}  // for itr - m_Schemas[]

	// Unknown schema selected, let the admin know:
	LOGWARNING("Unknown storage schema name \"%s\". Using default. Available schemas:", m_StorageSchemaName.c_str());
	for (cWSSchemaList::iterator itr = m_Schemas.begin(); itr != m_Schemas.end(); ++itr)
	{
		LOGWARNING("\t\"%s\"", (*itr)->GetName().c_str());
	}
	m_SaveSchema = m_Schemas.front();
}

void cWorldStorage::Execute(void)
{
	while (!mShouldTerminate)
	{
		m_Event.Wait();

		// Process both queues until they are empty again:
		bool HasMore;
		do
		{
			HasMore = false;
			if (mShouldTerminate)
			{
				return;
			}

			// Load 1 chunk:
			cChunkPtr ToLoad;
			{
				cCSLock Lock(m_CSLoadQueue);
				if (m_LoadQueue.size() > 0)
				{
					ToLoad = m_LoadQueue.front();
					m_LoadQueue.pop_front();
				}
				HasMore = (m_LoadQueue.size() > 0);
			}
			if ((ToLoad != NULL) && !LoadChunk(ToLoad))
			{
				// The chunk couldn't be loaded, generate it:
				m_World->GetGenerator().GenerateChunk(ToLoad->GetPosX(), ToLoad->GetPosZ());
			}

			// Save 1 chunk:
			cChunkPtr Save;
			{
				cCSLock Lock(m_CSSaveQueue);
				if (m_SaveQueue.size() > 0)
				{
					Save = m_SaveQueue.front();
					m_SaveQueue.pop_front();
				}
				HasMore = HasMore || (m_SaveQueue.size() > 0);
			}
			if ((Save != NULL) && (!m_SaveSchema->SaveChunk(Save)))
			{
				LOGWARNING("Cannot save chunk [%d, %d]", Save->GetPosX(), Save->GetPosZ());
			}
		} while (HasMore);
	}
}

bool cWorldStorage::LoadChunk(const cChunkPtr & a_Chunk)
{
	if (a_Chunk->IsValid())
	{
		// Already loaded (can happen, since the queue is async)
		return true;
	}

	if (m_SaveSchema->LoadChunk(a_Chunk))
	{
		return true;
	}

	for (cWSSchemaList::iterator itr = m_Schemas.begin(); itr != m_Schemas.end(); ++itr)
	{
		if ((*itr)->LoadChunk(a_Chunk))
		{
			return true;
		}
	}

	return false;
}

<|endoftext|>