<commit_before>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: levdis.hxx,v $
 *
 *  $Revision: 1.3 $
 *
 *  last change: $Author: rt $ $Date: 2005-09-07 17:22:53 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 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
 *
 ************************************************************************/

#ifndef INCLUDED_I18NPOOL_LEVDIS_HXX
#define INCLUDED_I18NPOOL_LEVDIS_HXX

#ifndef _RTL_USTRING_HXX_
#include <rtl/ustring.hxx>
#endif

/*
 maximal X Ersetzungen  (User: gefundenes darf X Zeichen anders sein)
 maximal Y Einfuegungen (User: gefundenes darf Y Zeichen kuerzer sein)
 maximal Z Loeschungen  (User: gefundenes darf Z Zeichen laenger sein)
 mathematische WLD oder SplitCount  (User: strikt oder relaxed)

 Joker ('?' und '*') fallen natuerlich nicht unter diese Bedingungen.
 Bei einem '?' wird eine Ersetzung nicht gezahlt, bei einem '*' darf
 der gefundene String an dieser Stelle beliebig viele Zeichen laenger sein.

 Strikt: entweder maximal X anders oder Y kuerzer oder Z laenger
 Relaxed: maximal X anders und/oder Y kuerzer und/oder Z laenger

 Wertebereich fuer X,Y,Z ist 0..33 um mit Limit sicher unter
 16-bit-signed-int-max zu bleiben, 31*32*33 gibt das Maximum
 KGV(31,32,33) == 32736
 */

#define LEVDISDEFAULT_XOTHER    2
#define LEVDISDEFAULT_YSHORTER  1
#define LEVDISDEFAULT_ZLONGER   3
#define LEVDISDEFAULT_RELAXED   TRUE

#define LEVDISDEFAULTLIMIT  6       // default nLimit, passt zu x=2, y=1, z=3,
                                    // p=3, q=6, r=2
#define LEVDISDEFAULT_P0    3       // default nRepP0, Gewichtung Ersetzen
#define LEVDISDEFAULT_Q0    6       // default nInsQ0, Gewichtung Einfuegen
#define LEVDISDEFAULT_R0    2       // default nDelR0, Gewichtung Loeschen
/*
 Berechnung von angegebenen Userwerten max Ersetzen, Kuerzer, Laenger mittels
 CalcLPQR(). Unschoen: wenn in WLD z.B. nLimit durch nDelR0 erreicht ist
 (String ist nZ laenger als Pattern), kann kein Zeichen mehr ersetzt werden.
 Dies kann durch Erhoehung von nX oder/und nZ vermieden werden, natuerlich
 mit den entsprechenden Seiteneffekten.. oder aber mit SplitCount (s.u.), was
 der Default bei CalcLPQR() ist.

 Achtung: Kuerzer = WLD.Insert, Laenger = WLD.Delete

 Gezaehlt wird von String nach Pattern (eine Loeschung bedeutet, dass aus
 String etwas geloescht werden muss, um auf Pattern zu kommen)

 Loeschungen zaehlen in diesem Beispiel am wenigsten, da meistens weniger
 bekannt ist, als gesucht wird. Ersetzungen erhalten mittlere Gewichtung
 wegen z.B. falscher Schreibweisen. Einfuegungen werden teuer.

 Oder z.B.: P0 = 1, Q0 = 4, R0 = 4, Limit = 3
 Erlaubt sind maximal 4 Ersetzungen, keine Einfuegung, keine Loeschung
 Entspricht den Userangaben X = 3, Y = 0, Z = 0

 bSplitCount: wenn TRUE, werden Rep/Ins/Del anders gezaehlt.  Der
 Rueckgabewert von WLD ist dann nicht mehr unbedingt die Levenshtein-Distanz,
 sondern kann negativ (-WLD) sein, wenn die WLD groesser als nLimit ist, aber
 die Einzelwerte jeweils innerhalb der Grenzen liegen.
 Bei den Default-Werten hiesse das z.B.: auch wenn der gefundene String 2
 Zeichen laenger ist (nLongerZ), koennen noch 3 Ersetzungen (nOtherX)
 erfolgen.  Zusatz-Gimmick:  Buchstabendreher zaehlen als eine Ersetzung.
 Mathematisch voellig unkorrekt, aber gut fuer den User ;-)

 Zur Erlaeuterung: bei der echten WLD schoepfen alle Aktionen aus einem
 gemeinsamen 100%-Pool, wenn einer alles hat, ist fuer die anderen nichts
 mehr da. Mit bSplitCount hat Replace sein eigenes Wildwasser..
 */

class WLevDisPatternMem     // "sichere" Speicheranforderung im cTor
{
    sal_Unicode     *cp;
    bool            *bp;
public:
    WLevDisPatternMem( sal_Int32 s )    { cp = new sal_Unicode[ s ];
                                          bp = new bool[ s ];
                                        }
    ~WLevDisPatternMem()                { if (cp) delete [] cp;
                                          if (bp) delete [] bp;
                                        }
    sal_Unicode* GetcPtr() const        { return cp; }
    bool* GetbPtr() const               { return bp; }
};

class WLevDisDistanceMem
{
    int*    p;
public:
    WLevDisDistanceMem( size_t s )  { p = 0; NewMem(s); }
    ~WLevDisDistanceMem()           { if (p) delete [] p; }
    int* GetPtr() const             { return p; }
    int* NewMem( size_t s )         {   if (p) delete [] p;
                                        return (p = new int[ s<3 ? 3 : s ]);
                                    }
};

class WLevDistance
{
    sal_Int32       nPatternLen;    // Laenge des Pattern
    WLevDisPatternMem   aPatMem;    // Verwaltung des Pattern Arrays
    sal_Unicode*    cpPattern;      // Pointer auf Pattern Array
    bool*           bpPatIsWild;    // Pointer auf bool Array, ob Pattern Wildcard ist
    sal_Int32       nArrayLen;      // Laenge des Distanz Arrays
    WLevDisDistanceMem  aDisMem;    // Verwaltung des Distanz Arrays
    int*            npDistance;     // Pointer auf Distanz Array
    int             nLimit;         // WLD Limit Ersetzungen/Einfuegungen/Loeschungen
    int             nRepP0;         // Ersetzen Gewichtung
    int             nInsQ0;         // Einfuegen Gewichtung
    int             nDelR0;         // Loeschen Gewichtung
    int             nStars;         // Anzahl '*' Joker im Pattern
    bool            bSplitCount;    // wenn TRUE, werden Rep/Ins/Del getrennt gezaehlt

    void InitData( const sal_Unicode* cPattern );       // fuer die CToren
    inline int Min3( int x, int y, int z );     // inline wegen Schleife
    int Mid3( int x, int y, int z );
    int Max3( int x, int y, int z );
    int GGT( int a, int b );    // Groesster Gemeinsamer Teiler
    int KGV( int a, int b );    // Kleinstes Gemeinsames Vielfaches

public:
    // CToren fuer direktes Setzen der Gewichtung mit Set...()
    // im CTor werden die Defaultwerte fuer Limit/Rep/Ins/Del gesetzt
    WLevDistance( const sal_Unicode* cPattern );
    WLevDistance( const ::rtl::OUString& rPattern );

    // CToren mit Userangaben, danach mit GetLimit() Limit holen
    // interner Aufruf von CalcLPQR()
    // die mathematisch unkorrekte Berechnung wird als Default genommen!
    WLevDistance( const sal_Unicode* cPattern, int nOtherX, int nShorterY,
                    int nLongerZ, bool bRelaxed = true );

    WLevDistance( const WLevDistance& rWLD );
    ~WLevDistance();

    // Berechnung der Levenshtein-Distanz von String zu Pattern
    int WLD( const sal_Unicode* cString, sal_Int32 nStringLen );       // prefered
    int WLD( const sal_Unicode* cString );
    int WLD( const ::rtl::OUString& rString );

    // Berechnung der Gewichtung aus Userangaben, return nLimit
    int CalcLPQR( int nOtherX, int nShorterY, int nLongerZ,
                    bool bRelaxed = true );

    inline int GetLimit() const     { return( nLimit ); }   // Limit holen
    inline int GetReplaceP0() const { return( nRepP0 ); }   // Gewichtungen holen
    inline int GetInsertQ0() const  { return( nInsQ0 ); }
    inline int GetDeleteR0() const  { return( nDelR0 ); }
    inline bool GetSplit() const    { return( bSplitCount ); }
    inline int SetLimit( int nNewLimit );       // Limit setzen,
    inline int SetReplaceP0( int nNewP0 );      // Gewichtungen setzen,
    inline int SetInsertQ0( int nNewQ0 );       // returnen bisherigen Wert
    inline int SetDeleteR0( int nNewR0 );
    inline bool SetSplit( bool bNewSplit );
        // SetSplit( TRUE ) macht nur mit Werten nach CalcLPQR() Sinn!

    inline bool IsNormal( sal_Int32 nPos ) const { return( !bpPatIsWild[nPos] ); }

#ifdef erTEST
    void    ShowTest();
#ifdef erTESTMAT
    void    ShowMatrix( const char* cString );
#endif
#endif

};

inline int WLevDistance::SetLimit( int nNewLimit )
{
    int nTmp = nLimit;
    nLimit = nNewLimit;
    return( nTmp );
}

inline int WLevDistance::SetReplaceP0( int nNewP0 )
{
    int nTmp = nRepP0;
    nRepP0 = nNewP0;
    return( nTmp );
}

inline int WLevDistance::SetInsertQ0( int nNewQ0 )
{
    int nTmp = nInsQ0;
    nInsQ0 = nNewQ0;
    return( nTmp );
}

inline int WLevDistance::SetDeleteR0( int nNewR0 )
{
    int nTmp = nDelR0;
    nDelR0 = nNewR0;
    return( nTmp );
}

inline bool WLevDistance::SetSplit( bool bNewSplit )
{
    bool bTmp = bSplitCount;
    bSplitCount = bNewSplit;
    return( bTmp );
}

#endif      // _LEVDIS_HXX

<commit_msg>INTEGRATION: CWS changefileheader (1.3.216); FILE MERGED 2008/04/01 15:20:08 thb 1.3.216.2: #i85898# Stripping all external header guards 2008/03/31 16:01:29 rt 1.3.216.1: #i87441# Change license header to LPGL v3.<commit_after>/*************************************************************************
 *
 * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
 *
 * Copyright 2008 by Sun Microsystems, Inc.
 *
 * OpenOffice.org - a multi-platform office productivity suite
 *
 * $RCSfile: levdis.hxx,v $
 * $Revision: 1.4 $
 *
 * This file is part of OpenOffice.org.
 *
 * OpenOffice.org is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License version 3
 * only, as published by the Free Software Foundation.
 *
 * OpenOffice.org 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 version 3 for more details
 * (a copy is included in the LICENSE file that accompanied this code).
 *
 * You should have received a copy of the GNU Lesser General Public License
 * version 3 along with OpenOffice.org.  If not, see
 * <http://www.openoffice.org/license.html>
 * for a copy of the LGPLv3 License.
 *
 ************************************************************************/

#ifndef INCLUDED_I18NPOOL_LEVDIS_HXX
#define INCLUDED_I18NPOOL_LEVDIS_HXX

#include <rtl/ustring.hxx>

/*
 maximal X Ersetzungen  (User: gefundenes darf X Zeichen anders sein)
 maximal Y Einfuegungen (User: gefundenes darf Y Zeichen kuerzer sein)
 maximal Z Loeschungen  (User: gefundenes darf Z Zeichen laenger sein)
 mathematische WLD oder SplitCount  (User: strikt oder relaxed)

 Joker ('?' und '*') fallen natuerlich nicht unter diese Bedingungen.
 Bei einem '?' wird eine Ersetzung nicht gezahlt, bei einem '*' darf
 der gefundene String an dieser Stelle beliebig viele Zeichen laenger sein.

 Strikt: entweder maximal X anders oder Y kuerzer oder Z laenger
 Relaxed: maximal X anders und/oder Y kuerzer und/oder Z laenger

 Wertebereich fuer X,Y,Z ist 0..33 um mit Limit sicher unter
 16-bit-signed-int-max zu bleiben, 31*32*33 gibt das Maximum
 KGV(31,32,33) == 32736
 */

#define LEVDISDEFAULT_XOTHER    2
#define LEVDISDEFAULT_YSHORTER  1
#define LEVDISDEFAULT_ZLONGER   3
#define LEVDISDEFAULT_RELAXED   TRUE

#define LEVDISDEFAULTLIMIT  6       // default nLimit, passt zu x=2, y=1, z=3,
                                    // p=3, q=6, r=2
#define LEVDISDEFAULT_P0    3       // default nRepP0, Gewichtung Ersetzen
#define LEVDISDEFAULT_Q0    6       // default nInsQ0, Gewichtung Einfuegen
#define LEVDISDEFAULT_R0    2       // default nDelR0, Gewichtung Loeschen
/*
 Berechnung von angegebenen Userwerten max Ersetzen, Kuerzer, Laenger mittels
 CalcLPQR(). Unschoen: wenn in WLD z.B. nLimit durch nDelR0 erreicht ist
 (String ist nZ laenger als Pattern), kann kein Zeichen mehr ersetzt werden.
 Dies kann durch Erhoehung von nX oder/und nZ vermieden werden, natuerlich
 mit den entsprechenden Seiteneffekten.. oder aber mit SplitCount (s.u.), was
 der Default bei CalcLPQR() ist.

 Achtung: Kuerzer = WLD.Insert, Laenger = WLD.Delete

 Gezaehlt wird von String nach Pattern (eine Loeschung bedeutet, dass aus
 String etwas geloescht werden muss, um auf Pattern zu kommen)

 Loeschungen zaehlen in diesem Beispiel am wenigsten, da meistens weniger
 bekannt ist, als gesucht wird. Ersetzungen erhalten mittlere Gewichtung
 wegen z.B. falscher Schreibweisen. Einfuegungen werden teuer.

 Oder z.B.: P0 = 1, Q0 = 4, R0 = 4, Limit = 3
 Erlaubt sind maximal 4 Ersetzungen, keine Einfuegung, keine Loeschung
 Entspricht den Userangaben X = 3, Y = 0, Z = 0

 bSplitCount: wenn TRUE, werden Rep/Ins/Del anders gezaehlt.  Der
 Rueckgabewert von WLD ist dann nicht mehr unbedingt die Levenshtein-Distanz,
 sondern kann negativ (-WLD) sein, wenn die WLD groesser als nLimit ist, aber
 die Einzelwerte jeweils innerhalb der Grenzen liegen.
 Bei den Default-Werten hiesse das z.B.: auch wenn der gefundene String 2
 Zeichen laenger ist (nLongerZ), koennen noch 3 Ersetzungen (nOtherX)
 erfolgen.  Zusatz-Gimmick:  Buchstabendreher zaehlen als eine Ersetzung.
 Mathematisch voellig unkorrekt, aber gut fuer den User ;-)

 Zur Erlaeuterung: bei der echten WLD schoepfen alle Aktionen aus einem
 gemeinsamen 100%-Pool, wenn einer alles hat, ist fuer die anderen nichts
 mehr da. Mit bSplitCount hat Replace sein eigenes Wildwasser..
 */

class WLevDisPatternMem     // "sichere" Speicheranforderung im cTor
{
    sal_Unicode     *cp;
    bool            *bp;
public:
    WLevDisPatternMem( sal_Int32 s )    { cp = new sal_Unicode[ s ];
                                          bp = new bool[ s ];
                                        }
    ~WLevDisPatternMem()                { if (cp) delete [] cp;
                                          if (bp) delete [] bp;
                                        }
    sal_Unicode* GetcPtr() const        { return cp; }
    bool* GetbPtr() const               { return bp; }
};

class WLevDisDistanceMem
{
    int*    p;
public:
    WLevDisDistanceMem( size_t s )  { p = 0; NewMem(s); }
    ~WLevDisDistanceMem()           { if (p) delete [] p; }
    int* GetPtr() const             { return p; }
    int* NewMem( size_t s )         {   if (p) delete [] p;
                                        return (p = new int[ s<3 ? 3 : s ]);
                                    }
};

class WLevDistance
{
    sal_Int32       nPatternLen;    // Laenge des Pattern
    WLevDisPatternMem   aPatMem;    // Verwaltung des Pattern Arrays
    sal_Unicode*    cpPattern;      // Pointer auf Pattern Array
    bool*           bpPatIsWild;    // Pointer auf bool Array, ob Pattern Wildcard ist
    sal_Int32       nArrayLen;      // Laenge des Distanz Arrays
    WLevDisDistanceMem  aDisMem;    // Verwaltung des Distanz Arrays
    int*            npDistance;     // Pointer auf Distanz Array
    int             nLimit;         // WLD Limit Ersetzungen/Einfuegungen/Loeschungen
    int             nRepP0;         // Ersetzen Gewichtung
    int             nInsQ0;         // Einfuegen Gewichtung
    int             nDelR0;         // Loeschen Gewichtung
    int             nStars;         // Anzahl '*' Joker im Pattern
    bool            bSplitCount;    // wenn TRUE, werden Rep/Ins/Del getrennt gezaehlt

    void InitData( const sal_Unicode* cPattern );       // fuer die CToren
    inline int Min3( int x, int y, int z );     // inline wegen Schleife
    int Mid3( int x, int y, int z );
    int Max3( int x, int y, int z );
    int GGT( int a, int b );    // Groesster Gemeinsamer Teiler
    int KGV( int a, int b );    // Kleinstes Gemeinsames Vielfaches

public:
    // CToren fuer direktes Setzen der Gewichtung mit Set...()
    // im CTor werden die Defaultwerte fuer Limit/Rep/Ins/Del gesetzt
    WLevDistance( const sal_Unicode* cPattern );
    WLevDistance( const ::rtl::OUString& rPattern );

    // CToren mit Userangaben, danach mit GetLimit() Limit holen
    // interner Aufruf von CalcLPQR()
    // die mathematisch unkorrekte Berechnung wird als Default genommen!
    WLevDistance( const sal_Unicode* cPattern, int nOtherX, int nShorterY,
                    int nLongerZ, bool bRelaxed = true );

    WLevDistance( const WLevDistance& rWLD );
    ~WLevDistance();

    // Berechnung der Levenshtein-Distanz von String zu Pattern
    int WLD( const sal_Unicode* cString, sal_Int32 nStringLen );       // prefered
    int WLD( const sal_Unicode* cString );
    int WLD( const ::rtl::OUString& rString );

    // Berechnung der Gewichtung aus Userangaben, return nLimit
    int CalcLPQR( int nOtherX, int nShorterY, int nLongerZ,
                    bool bRelaxed = true );

    inline int GetLimit() const     { return( nLimit ); }   // Limit holen
    inline int GetReplaceP0() const { return( nRepP0 ); }   // Gewichtungen holen
    inline int GetInsertQ0() const  { return( nInsQ0 ); }
    inline int GetDeleteR0() const  { return( nDelR0 ); }
    inline bool GetSplit() const    { return( bSplitCount ); }
    inline int SetLimit( int nNewLimit );       // Limit setzen,
    inline int SetReplaceP0( int nNewP0 );      // Gewichtungen setzen,
    inline int SetInsertQ0( int nNewQ0 );       // returnen bisherigen Wert
    inline int SetDeleteR0( int nNewR0 );
    inline bool SetSplit( bool bNewSplit );
        // SetSplit( TRUE ) macht nur mit Werten nach CalcLPQR() Sinn!

    inline bool IsNormal( sal_Int32 nPos ) const { return( !bpPatIsWild[nPos] ); }

#ifdef erTEST
    void    ShowTest();
#ifdef erTESTMAT
    void    ShowMatrix( const char* cString );
#endif
#endif

};

inline int WLevDistance::SetLimit( int nNewLimit )
{
    int nTmp = nLimit;
    nLimit = nNewLimit;
    return( nTmp );
}

inline int WLevDistance::SetReplaceP0( int nNewP0 )
{
    int nTmp = nRepP0;
    nRepP0 = nNewP0;
    return( nTmp );
}

inline int WLevDistance::SetInsertQ0( int nNewQ0 )
{
    int nTmp = nInsQ0;
    nInsQ0 = nNewQ0;
    return( nTmp );
}

inline int WLevDistance::SetDeleteR0( int nNewR0 )
{
    int nTmp = nDelR0;
    nDelR0 = nNewR0;
    return( nTmp );
}

inline bool WLevDistance::SetSplit( bool bNewSplit )
{
    bool bTmp = bSplitCount;
    bSplitCount = bNewSplit;
    return( bTmp );
}

#endif      // _LEVDIS_HXX

<|endoftext|>


<commit_before>/**
 * @file      rvo_planner.cpp
 * @brief     RVO Planner class
 * @author    Alejandro Bordallo <alex.bordallo@ed.ac.uk>
 * @date      2015-06-30
 * @copyright (MIT) 2015 Edinferno
 */

#include <planner/rvo_planner.hpp>

RVOPlanner::RVOPlanner(ros::NodeHandle* nh) {
  nh_ = nh;
  this->rosSetup();
  this->createPlanner();
}

RVOPlanner::~RVOPlanner() {;}

void RVOPlanner::init() {
  PLANNER_ROBOT_ = 0;
  planner_settings_.request.sim_num = 0;
  planner_settings_.request.time_step = 0.1f;
  planner_settings_.request.defaults.neighbor_dist = 2.0f;
  planner_settings_.request.defaults.max_neighbors = 20;
  planner_settings_.request.defaults.time_horizon_agent = 5.0f;
  planner_settings_.request.defaults.time_horizon_obst = 5.0f;
  planner_settings_.request.defaults.radius = 0.5f;
  planner_settings_.request.defaults.max_speed = 0.3f;
}

void RVOPlanner::rosSetup() {
  ros::service::waitForService("/rvo_wrapper/add_agent");
  ros::service::waitForService("/rvo_wrapper/check_reached_goal");
  ros::service::waitForService("/rvo_wrapper/calc_pref_velocities");
  ros::service::waitForService("/rvo_wrapper/create_rvosim");
  ros::service::waitForService("/rvo_wrapper/do_step");
  ros::service::waitForService("/rvo_wrapper/get_agent_position");
  ros::service::waitForService("/rvo_wrapper/get_agent_velocity");
  ros::service::waitForService("/rvo_wrapper/get_num_agents");
  ros::service::waitForService("/rvo_wrapper/set_agent_goals");
  ros::service::waitForService("/rvo_wrapper/set_agent_defaults");
  ros::service::waitForService("/rvo_wrapper/set_agent_position");
  ros::service::waitForService("/rvo_wrapper/set_time_step");
  add_planner_agent_client_ =
    nh_->serviceClient<rvo_wrapper_msgs::AddAgent>(
      "/rvo_wrapper/add_agent", true);
  check_reached_goal_client_ =
    nh_->serviceClient<rvo_wrapper_msgs::CheckReachedGoal>(
      "/rvo_wrapper/check_reached_goal", true);
  calc_pref_velocities_client_ =
    nh_->serviceClient<rvo_wrapper_msgs::CalcPrefVelocities>(
      "/rvo_wrapper/calc_pref_velocities", true);
  create_planner_client_ =
    nh_->serviceClient<rvo_wrapper_msgs::CreateRVOSim>(
      "/rvo_wrapper/create_rvosim", true);
  do_planner_step_client_ =
    nh_->serviceClient<rvo_wrapper_msgs::DoStep>(
      "/rvo_wrapper/do_step", true);
  get_agent_pos_client_ =
    nh_->serviceClient<rvo_wrapper_msgs::GetAgentPosition>(
      "/rvo_wrapper/get_agent_position", true);
  get_agent_vel_client_ =
    nh_->serviceClient<rvo_wrapper_msgs::GetAgentVelocity>(
      "/rvo_wrapper/get_agent_velocity", true);
  get_num_agents_ =
    nh_->serviceClient<rvo_wrapper_msgs::GetNumAgents>(
      "/rvo_wrapper/get_num_agents", true);
  set_agent_goals_client_ =
    nh_->serviceClient<rvo_wrapper_msgs::SetAgentGoals>(
      "/rvo_wrapper/set_agent_goals", true);
  set_agent_defaults_ =
    nh_->serviceClient<rvo_wrapper_msgs::SetAgentDefaults>(
      "/rvo_wrapper/set_agent_defaults", true);
  set_agent_position_ =
    nh_->serviceClient<rvo_wrapper_msgs::SetAgentPosition>(
      "/rvo_wrapper/set_agent_position", true);
  set_time_step_ =
    nh_->serviceClient<rvo_wrapper_msgs::SetTimeStep>(
      "/rvo_wrapper/set_time_step", true);
}

size_t RVOPlanner::addPlannerAgent(common_msgs::Vector2 agent_pos) {
  rvo_wrapper_msgs::AddAgent msg;
  msg.request.position = agent_pos;
  add_planner_agent_client_.call(msg);
  return msg.response.agent_id;
}

void RVOPlanner::calcPrefVelocities() {
  rvo_wrapper_msgs::CalcPrefVelocities msg;
  calc_pref_velocities_client_.call(msg);
}

bool RVOPlanner::checkReachedGoal() {
  rvo_wrapper_msgs::CheckReachedGoal msg;
  check_reached_goal_client_.call(msg);
  return msg.response.reached;
}

void RVOPlanner::createPlanner() {
  create_planner_client_.call(planner_settings_);
  if (planner_settings_.response.res) {
    ROS_INFO("Planner created");
  } else { ROS_ERROR("Planner not created!"); };
}

void RVOPlanner::doSimStep() {
  rvo_wrapper_msgs::DoStep msg;
  do_planner_step_client_.call(msg);
}

common_msgs::Vector2 RVOPlanner::getAgentPos(size_t agent_no) {
  rvo_wrapper_msgs::GetAgentPosition msg;
  msg.request.agent_id = agent_no;
  get_agent_pos_client_.call(msg);
  return msg.response.position;
}

void RVOPlanner::setPlannerGoal(common_msgs::Vector2 goal) {
  rvo_wrapper_msgs::GetNumAgents num_agents_msg;
  get_num_agents_.call(num_agents_msg);
  rvo_wrapper_msgs::SetAgentGoals agent_goal_msg;
  rvo_wrapper_msgs::SimGoals empty;
  agent_goal_msg.request.sim.push_back(empty);
  for (uint32_t i = 0; i < num_agents_msg.response.num_agents; ++i) {
    agent_goal_msg.request.sim[0].agent.push_back(goal);
  }
  set_agent_goals_client_.call(agent_goal_msg);
}

void RVOPlanner::setPlannerSettings(float time_step,
                                    rvo_wrapper_msgs::AgentDefaults defaults) {
  // Save for new planner generation
  planner_settings_.request.time_step = time_step;
  planner_settings_.request.defaults.neighbor_dist = defaults.neighbor_dist;
  planner_settings_.request.defaults.max_neighbors = defaults.max_neighbors;
  planner_settings_.request.defaults.time_horizon_agent =
    defaults.time_horizon_agent;
  planner_settings_.request.defaults.time_horizon_obst =
    defaults.time_horizon_obst;
  planner_settings_.request.defaults.radius = defaults.radius;
  planner_settings_.request.defaults.max_speed = defaults.max_speed;
  // Set new planner settings
  rvo_wrapper_msgs::SetTimeStep planner_time_step;
  // planner_time_step.request.sim_ids = planner_settings_.request.sim_num;
  planner_time_step.request.time_step = planner_settings_.request.time_step;
  set_time_step_.call(planner_time_step);
  rvo_wrapper_msgs::SetAgentDefaults agent_defaults;
  // agent_defaults.request.sim_ids = planner_settings_.request.sim_num;
  agent_defaults.request.defaults = planner_settings_.request.defaults;
  set_agent_defaults_.call(agent_defaults);
}

void RVOPlanner::planStep() {
  this->calcPrefVelocities();
  this->doSimStep();
}

common_msgs::Vector2 RVOPlanner::getPlannerVel() {
  rvo_wrapper_msgs::GetAgentVelocity msg;
  msg.request.agent_id = PLANNER_ROBOT_;
  get_agent_vel_client_.call(msg);
  if (!msg.response.res) {
    msg.response.velocity.x = 0.0f;
    msg.response.velocity.y = 0.0f;
  }
  return msg.response.velocity;
}

void RVOPlanner::setCurrPose(common_msgs::Vector2 curr_pose) {
  rvo_wrapper_msgs::SetAgentPosition msg;
  msg.request.agent_id = PLANNER_ROBOT_;
  msg.request.position.x = curr_pose.x;
  msg.request.position.y = curr_pose.y;
  set_agent_position_.call(msg);
}
<commit_msg>Fix initialisation of rvo_planner<commit_after>/**
 * @file      rvo_planner.cpp
 * @brief     RVO Planner class
 * @author    Alejandro Bordallo <alex.bordallo@ed.ac.uk>
 * @date      2015-06-30
 * @copyright (MIT) 2015 Edinferno
 */

#include <planner/rvo_planner.hpp>

RVOPlanner::RVOPlanner(ros::NodeHandle* nh) {
  nh_ = nh;
  this->init();
  this->rosSetup();
  this->createPlanner();
}

RVOPlanner::~RVOPlanner() {;}

void RVOPlanner::init() {
  PLANNER_ROBOT_ = 0;
  planner_settings_.request.sim_num = 0;
  planner_settings_.request.time_step = 0.1f;
  planner_settings_.request.defaults.neighbor_dist = 2.0f;
  planner_settings_.request.defaults.max_neighbors = 20;
  planner_settings_.request.defaults.time_horizon_agent = 5.0f;
  planner_settings_.request.defaults.time_horizon_obst = 5.0f;
  planner_settings_.request.defaults.radius = 0.5f;
  planner_settings_.request.defaults.max_speed = 0.3f;
}

void RVOPlanner::rosSetup() {
  ros::service::waitForService("/rvo_wrapper/add_agent");
  ros::service::waitForService("/rvo_wrapper/check_reached_goal");
  ros::service::waitForService("/rvo_wrapper/calc_pref_velocities");
  ros::service::waitForService("/rvo_wrapper/create_rvosim");
  ros::service::waitForService("/rvo_wrapper/do_step");
  ros::service::waitForService("/rvo_wrapper/get_agent_position");
  ros::service::waitForService("/rvo_wrapper/get_agent_velocity");
  ros::service::waitForService("/rvo_wrapper/get_num_agents");
  ros::service::waitForService("/rvo_wrapper/set_agent_goals");
  ros::service::waitForService("/rvo_wrapper/set_agent_defaults");
  ros::service::waitForService("/rvo_wrapper/set_agent_position");
  ros::service::waitForService("/rvo_wrapper/set_time_step");
  add_planner_agent_client_ =
    nh_->serviceClient<rvo_wrapper_msgs::AddAgent>(
      "/rvo_wrapper/add_agent", true);
  check_reached_goal_client_ =
    nh_->serviceClient<rvo_wrapper_msgs::CheckReachedGoal>(
      "/rvo_wrapper/check_reached_goal", true);
  calc_pref_velocities_client_ =
    nh_->serviceClient<rvo_wrapper_msgs::CalcPrefVelocities>(
      "/rvo_wrapper/calc_pref_velocities", true);
  create_planner_client_ =
    nh_->serviceClient<rvo_wrapper_msgs::CreateRVOSim>(
      "/rvo_wrapper/create_rvosim", true);
  do_planner_step_client_ =
    nh_->serviceClient<rvo_wrapper_msgs::DoStep>(
      "/rvo_wrapper/do_step", true);
  get_agent_pos_client_ =
    nh_->serviceClient<rvo_wrapper_msgs::GetAgentPosition>(
      "/rvo_wrapper/get_agent_position", true);
  get_agent_vel_client_ =
    nh_->serviceClient<rvo_wrapper_msgs::GetAgentVelocity>(
      "/rvo_wrapper/get_agent_velocity", true);
  get_num_agents_ =
    nh_->serviceClient<rvo_wrapper_msgs::GetNumAgents>(
      "/rvo_wrapper/get_num_agents", true);
  set_agent_goals_client_ =
    nh_->serviceClient<rvo_wrapper_msgs::SetAgentGoals>(
      "/rvo_wrapper/set_agent_goals", true);
  set_agent_defaults_ =
    nh_->serviceClient<rvo_wrapper_msgs::SetAgentDefaults>(
      "/rvo_wrapper/set_agent_defaults", true);
  set_agent_position_ =
    nh_->serviceClient<rvo_wrapper_msgs::SetAgentPosition>(
      "/rvo_wrapper/set_agent_position", true);
  set_time_step_ =
    nh_->serviceClient<rvo_wrapper_msgs::SetTimeStep>(
      "/rvo_wrapper/set_time_step", true);
}

size_t RVOPlanner::addPlannerAgent(common_msgs::Vector2 agent_pos) {
  rvo_wrapper_msgs::AddAgent msg;
  msg.request.position = agent_pos;
  add_planner_agent_client_.call(msg);
  return msg.response.agent_id;
}

void RVOPlanner::calcPrefVelocities() {
  rvo_wrapper_msgs::CalcPrefVelocities msg;
  calc_pref_velocities_client_.call(msg);
}

bool RVOPlanner::checkReachedGoal() {
  rvo_wrapper_msgs::CheckReachedGoal msg;
  check_reached_goal_client_.call(msg);
  return msg.response.reached;
}

void RVOPlanner::createPlanner() {
  create_planner_client_.call(planner_settings_);
  if (planner_settings_.response.res) {
    ROS_INFO("RVO Planner created");
  } else { ROS_ERROR("Planner not created!"); };
}

void RVOPlanner::doSimStep() {
  rvo_wrapper_msgs::DoStep msg;
  do_planner_step_client_.call(msg);
}

common_msgs::Vector2 RVOPlanner::getAgentPos(size_t agent_no) {
  rvo_wrapper_msgs::GetAgentPosition msg;
  msg.request.agent_id = agent_no;
  get_agent_pos_client_.call(msg);
  return msg.response.position;
}

void RVOPlanner::setPlannerGoal(common_msgs::Vector2 goal) {
  rvo_wrapper_msgs::GetNumAgents num_agents_msg;
  get_num_agents_.call(num_agents_msg);
  rvo_wrapper_msgs::SetAgentGoals agent_goal_msg;
  rvo_wrapper_msgs::SimGoals empty;
  agent_goal_msg.request.sim.push_back(empty);
  agent_goal_msg.request.sim[0].agent.push_back(goal);
  // for (uint32_t i = 0; i < num_agents_msg.response.num_agents; ++i) {
  //   agent_goal_msg.request.sim[0].agent.push_back(goal);
  // }
  set_agent_goals_client_.call(agent_goal_msg);
}

void RVOPlanner::setPlannerSettings(float time_step,
                                    rvo_wrapper_msgs::AgentDefaults defaults) {
  // Save for new planner generation
  planner_settings_.request.time_step = time_step;
  planner_settings_.request.defaults.neighbor_dist = defaults.neighbor_dist;
  planner_settings_.request.defaults.max_neighbors = defaults.max_neighbors;
  planner_settings_.request.defaults.time_horizon_agent =
    defaults.time_horizon_agent;
  planner_settings_.request.defaults.time_horizon_obst =
    defaults.time_horizon_obst;
  planner_settings_.request.defaults.radius = defaults.radius;
  planner_settings_.request.defaults.max_speed = defaults.max_speed;
  // Set new planner settings
  rvo_wrapper_msgs::SetTimeStep planner_time_step;
  // planner_time_step.request.sim_ids = planner_settings_.request.sim_num;
  planner_time_step.request.time_step = planner_settings_.request.time_step;
  set_time_step_.call(planner_time_step);
  rvo_wrapper_msgs::SetAgentDefaults agent_defaults;
  // agent_defaults.request.sim_ids = planner_settings_.request.sim_num;
  agent_defaults.request.defaults = planner_settings_.request.defaults;
  set_agent_defaults_.call(agent_defaults);
}

void RVOPlanner::planStep() {
  ROS_INFO("rvo_planner plan_step");
  // this->createPlanner();
  // ROS_INFO("rvo_planner create_planner");
  // this->setupPlanner();
  // ROS_INFO("rvo_planner setup_planner");
  this->calcPrefVelocities();
  this->doSimStep();
}

common_msgs::Vector2 RVOPlanner::getPlannerVel() {
  rvo_wrapper_msgs::GetAgentVelocity msg;
  msg.request.agent_id = PLANNER_ROBOT_;
  get_agent_vel_client_.call(msg);
  if (!msg.response.res) {
    msg.response.velocity.x = 0.0f;
    msg.response.velocity.y = 0.0f;
  }
  return msg.response.velocity;
}

void RVOPlanner::setCurrPose(common_msgs::Vector2 curr_pose) {
  rvo_wrapper_msgs::SetAgentPosition msg;
  msg.request.agent_id = PLANNER_ROBOT_;
  msg.request.position.x = curr_pose.x;
  msg.request.position.y = curr_pose.y;
  set_agent_position_.call(msg);
}
<|endoftext|>


<commit_before>/*
    Copyright (c) 2005 by Volker Krause <vkrause@kde.org>

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.
    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software Foundation,
    Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, US
*/

#include "nntpjobs.h"

#include "kngroup.h"
#include "kngroupmanager.h"
#include "knserverinfo.h"
#include <kdebug.h>
#include <klocale.h>
#include <QDir>

KNode::GroupListJob::GroupListJob( KNJobConsumer * c, KNServerInfo * a, KNJobItem * i, bool incremental ) :
  KNJobData( KNJobData::JTFetchGroups, c, a, i ),
  mIncremental( incremental )
{
}

void KNode::GroupListJob::execute()
{
  mGroupList.clear();

  KNGroupListData *target = static_cast<KNGroupListData *>( data() );

  KUrl destination = baseUrl();
  QStringList query;
  if ( target->getDescriptions )
    query << "desc=true";
  if ( mIncremental )
    query << QString( "since=%1%2%3+000000" )
        .arg( target->fetchSince.year() % 100, 2, 10, QChar( '0' ) )
        .arg( target->fetchSince.month(), 2, 10, QChar( '0' ) )
        .arg( target->fetchSince.day(), 2, 10, QChar( '0' ) );
  destination.setQuery( query.join( "&" ) );
  KIO::Job* job = KIO::listDir( destination, KIO::HideProgressInfo, true );
  connect( job, SIGNAL(entries(KIO::Job*, const KIO::UDSEntryList&)),
           SLOT(slotEntries(KIO::Job*, const KIO::UDSEntryList&)) );
  connect( job, SIGNAL( result(KJob*) ), SLOT( slotResult(KJob*) ) );
  setupKIOJob( job );
}

void KNode::GroupListJob::slotEntries( KIO::Job * job, const KIO::UDSEntryList & list )
{
  Q_UNUSED( job );
   KNGroupListData *target = static_cast<KNGroupListData *>( data() );

  QString name, desc;
  bool subscribed;
  KNGroup::Status access;
  for( KIO::UDSEntryList::ConstIterator it = list.begin(); it != list.end(); ++it ) {
    access = KNGroup::unknown;
    name = (*it).stringValue( KIO::UDSEntry::UDS_NAME );
    desc = (*it).stringValue( KIO::UDSEntry::UDS_EXTRA );

    int value = (*it).numberValue( KIO::UDSEntry::UDS_ACCESS, -1 );
    if ( value != -1 ) {
      if( value & S_IWOTH )
        access = KNGroup::postingAllowed;
      else if ( value & S_IWGRP )
        access = KNGroup::moderated;
      else
        access = KNGroup::readOnly;
    }

    if ( name.isEmpty() )
      continue;
    if ( target->subscribed.contains( name ) ) {
      target->subscribed.removeAll( name );    // group names are unique, we wont find it again anyway...
      subscribed = true;
    } else {
      subscribed = false;
    }
    kDebug() << "Found group " << name;
    if ( mIncremental )
      mGroupList.append(KNGroupInfo( name, desc, true, subscribed, access ) );
    else
      target->groups->append(KNGroupInfo( name, desc, false, subscribed, access ) );
  }
}

void KNode::GroupListJob::slotResult( KJob * job )
{
  if ( job->error() )
    setError( job->error(), job->errorString() );
  else {
    KNGroupListData *target = static_cast<KNGroupListData *>( data() );

    // TODO: use thread weaver here?
    if ( mIncremental ) {
      setStatus( i18n("Loading group list from disk...") );
      if ( !target->readIn( this ) ) {
        setError( KIO::ERR_COULD_NOT_READ, i18n("Unable to read the group list file") );
        emitFinished();
        return;
      }
      target->merge( &mGroupList );
    }
    setStatus( i18n("Writing group list to disk...") );

    if ( !target->writeOut() )
      setError( KIO::ERR_COULD_NOT_WRITE, i18n("Unable to write the group list file") );
  }

  emitFinished();
}

KNode::GroupLoadJob::GroupLoadJob( KNJobConsumer * c, KNServerInfo * a, KNJobItem * i ) :
  KNJobData( KNJobData::JTLoadGroups, c, a, i )
{
}

void KNode::GroupLoadJob::execute( )
{
  KNGroupListData *target = static_cast<KNGroupListData *>( data() );

  setStatus( i18n("Loading group list from disk...") );
  // TODO: use the thread weaver here
  if ( !target->readIn( this ) )
    setError( KIO::ERR_COULD_NOT_READ, i18n("Unable to read the group list file") );

  emitFinished();
}

KNode::ArticleListJob::ArticleListJob( KNJobConsumer * c, KNServerInfo * a, KNJobItem * i, bool silent ) :
    KNJobData( JTfetchNewHeaders, c, a, i ),
    mSilent( silent )
{
}

void KNode::ArticleListJob::execute()
{
  mArticleList.clear();

  KNGroup* target = static_cast<KNGroup*>( data() );

  KUrl destination = baseUrl();
  destination.setPath( target->groupname() );
  QStringList query;
  query << "first=" + QString::number( target->lastNr() + 1 );
  if ( target->lastNr() <= 0 ) // first fetch
    query << "max=" + QString::number( target->maxFetch() );
  destination.setQuery( query.join( "&" ) );
  KIO::Job* job = KIO::listDir( destination, KIO::HideProgressInfo, true );
  connect( job, SIGNAL(entries(KIO::Job*, const KIO::UDSEntryList&)),
           SLOT(slotEntries(KIO::Job*, const KIO::UDSEntryList&)) );
  connect( job, SIGNAL( result(KJob*) ), SLOT( slotResult(KJob*) ) );
  setupKIOJob( job );
}

void KNode::ArticleListJob::slotEntries( KIO::Job * job, const KIO::UDSEntryList & list )
{
  Q_UNUSED( job );
  mArticleList += list;
}

void KNode::ArticleListJob::slotResult( KJob * _job )
{
  Q_ASSERT( mJob == _job );
  KIO::Job *job = static_cast<KIO::Job*>( _job );
  if ( job->error() )
    setError( job->error(), job->errorString() );
  else {
    createProgressItem();

    KNGroup* target = static_cast<KNGroup*>( data() );
    target->setLastFetchCount( 0 );

    setStatus( i18n("Sorting...") );

    if ( job->metaData().contains( "FirstSerialNumber" ) ) {
      int firstSerNum = job->metaData()["FirstSerialNumber"].toInt();
      target->setFirstNr( firstSerNum );
    }

    target->insortNewHeaders( mArticleList, this );

    if ( job->metaData().contains( "LastSerialNumber" ) ) {
      int lastSerNum = job->metaData()["LastSerialNumber"].toInt();
      target->setLastNr( lastSerNum );
    }
  }

  setComplete();

  emitFinished();
}

KNode::ArticleFetchJob::ArticleFetchJob( KNJobConsumer * c, KNServerInfo * a, KNJobItem * i, bool parse ) :
    KNJobData( JTfetchArticle, c, a, i ),
    mParseArticle( parse )
{
}

void KNode::ArticleFetchJob::execute()
{
  KNRemoteArticle *target = static_cast<KNRemoteArticle*>( data() );
  QString path = static_cast<KNGroup*>( target->collection() )->groupname();

  KUrl url = baseUrl();
  path += QDir::separator();

  // By default, fetch articles by their server-side Id.
  // (some server does not understand the "ARTICLE <msg-id>" command correctly (bug #193550))
  if ( target->articleNumber() != -1 ) {
    path += QString::number( target->articleNumber() );
  } else {
    // User asked to fetch a message by its msg-id
    path += target->messageID()->as7BitString( false );
  }
  url.setPath( path );

  KIO::Job* job = KIO::storedGet( url, KIO::NoReload, KIO::HideProgressInfo );
  connect( job, SIGNAL( result(KJob*) ), SLOT( slotResult(KJob*) ) );
  setupKIOJob( job );
}

void KNode::ArticleFetchJob::slotResult( KJob * job )
{
  if ( job->error() )
    setError( job->error(), job->errorString() );
  else {
    KNRemoteArticle *target = static_cast<KNRemoteArticle*>( data() );
    KIO::StoredTransferJob *j = static_cast<KIO::StoredTransferJob*>( job );
    QByteArray buffer = j->data();
    buffer.replace( "\r\n", "\n" ); // TODO: do this in the io-slave?
    target->setContent( buffer );
    if ( mParseArticle )
      target->parse();
  }

  emitFinished();
}

KNode::ArticlePostJob::ArticlePostJob( KNJobConsumer * c, KNServerInfo * a, KNJobItem * i ) :
    KNJobData( JTpostArticle, c, a, i )
{
}

void KNode::ArticlePostJob::execute( )
{
  KNLocalArticle *target = static_cast<KNLocalArticle*>( data() );

  KUrl url = baseUrl();

  KIO::Job* job = KIO::storedPut( target->encodedContent( true ), url, -1, KIO::Overwrite | KIO::HideProgressInfo );
  connect( job, SIGNAL( result(KJob*) ), SLOT( slotResult(KJob*) ) );
  setupKIOJob( job );
}

void KNode::ArticlePostJob::slotResult( KJob * job )
{
  if ( job->error() )
    setError( job->error(), job->errorString() );

  emitFinished();
}

#include "nntpjobs.moc"
<commit_msg>Correctly set the path of the url of ArticleFetchJob<commit_after>/*
    Copyright (c) 2005 by Volker Krause <vkrause@kde.org>

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.
    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software Foundation,
    Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, US
*/

#include "nntpjobs.h"

#include "kngroup.h"
#include "kngroupmanager.h"
#include "knserverinfo.h"
#include <kdebug.h>
#include <klocale.h>
#include <QDir>

KNode::GroupListJob::GroupListJob( KNJobConsumer * c, KNServerInfo * a, KNJobItem * i, bool incremental ) :
  KNJobData( KNJobData::JTFetchGroups, c, a, i ),
  mIncremental( incremental )
{
}

void KNode::GroupListJob::execute()
{
  mGroupList.clear();

  KNGroupListData *target = static_cast<KNGroupListData *>( data() );

  KUrl destination = baseUrl();
  QStringList query;
  if ( target->getDescriptions )
    query << "desc=true";
  if ( mIncremental )
    query << QString( "since=%1%2%3+000000" )
        .arg( target->fetchSince.year() % 100, 2, 10, QChar( '0' ) )
        .arg( target->fetchSince.month(), 2, 10, QChar( '0' ) )
        .arg( target->fetchSince.day(), 2, 10, QChar( '0' ) );
  destination.setQuery( query.join( "&" ) );
  KIO::Job* job = KIO::listDir( destination, KIO::HideProgressInfo, true );
  connect( job, SIGNAL(entries(KIO::Job*, const KIO::UDSEntryList&)),
           SLOT(slotEntries(KIO::Job*, const KIO::UDSEntryList&)) );
  connect( job, SIGNAL( result(KJob*) ), SLOT( slotResult(KJob*) ) );
  setupKIOJob( job );
}

void KNode::GroupListJob::slotEntries( KIO::Job * job, const KIO::UDSEntryList & list )
{
  Q_UNUSED( job );
   KNGroupListData *target = static_cast<KNGroupListData *>( data() );

  QString name, desc;
  bool subscribed;
  KNGroup::Status access;
  for( KIO::UDSEntryList::ConstIterator it = list.begin(); it != list.end(); ++it ) {
    access = KNGroup::unknown;
    name = (*it).stringValue( KIO::UDSEntry::UDS_NAME );
    desc = (*it).stringValue( KIO::UDSEntry::UDS_EXTRA );

    int value = (*it).numberValue( KIO::UDSEntry::UDS_ACCESS, -1 );
    if ( value != -1 ) {
      if( value & S_IWOTH )
        access = KNGroup::postingAllowed;
      else if ( value & S_IWGRP )
        access = KNGroup::moderated;
      else
        access = KNGroup::readOnly;
    }

    if ( name.isEmpty() )
      continue;
    if ( target->subscribed.contains( name ) ) {
      target->subscribed.removeAll( name );    // group names are unique, we wont find it again anyway...
      subscribed = true;
    } else {
      subscribed = false;
    }
    kDebug() << "Found group " << name;
    if ( mIncremental )
      mGroupList.append(KNGroupInfo( name, desc, true, subscribed, access ) );
    else
      target->groups->append(KNGroupInfo( name, desc, false, subscribed, access ) );
  }
}

void KNode::GroupListJob::slotResult( KJob * job )
{
  if ( job->error() )
    setError( job->error(), job->errorString() );
  else {
    KNGroupListData *target = static_cast<KNGroupListData *>( data() );

    // TODO: use thread weaver here?
    if ( mIncremental ) {
      setStatus( i18n("Loading group list from disk...") );
      if ( !target->readIn( this ) ) {
        setError( KIO::ERR_COULD_NOT_READ, i18n("Unable to read the group list file") );
        emitFinished();
        return;
      }
      target->merge( &mGroupList );
    }
    setStatus( i18n("Writing group list to disk...") );

    if ( !target->writeOut() )
      setError( KIO::ERR_COULD_NOT_WRITE, i18n("Unable to write the group list file") );
  }

  emitFinished();
}

KNode::GroupLoadJob::GroupLoadJob( KNJobConsumer * c, KNServerInfo * a, KNJobItem * i ) :
  KNJobData( KNJobData::JTLoadGroups, c, a, i )
{
}

void KNode::GroupLoadJob::execute( )
{
  KNGroupListData *target = static_cast<KNGroupListData *>( data() );

  setStatus( i18n("Loading group list from disk...") );
  // TODO: use the thread weaver here
  if ( !target->readIn( this ) )
    setError( KIO::ERR_COULD_NOT_READ, i18n("Unable to read the group list file") );

  emitFinished();
}

KNode::ArticleListJob::ArticleListJob( KNJobConsumer * c, KNServerInfo * a, KNJobItem * i, bool silent ) :
    KNJobData( JTfetchNewHeaders, c, a, i ),
    mSilent( silent )
{
}

void KNode::ArticleListJob::execute()
{
  mArticleList.clear();

  KNGroup* target = static_cast<KNGroup*>( data() );

  KUrl destination = baseUrl();
  destination.setPath( target->groupname() );
  QStringList query;
  query << "first=" + QString::number( target->lastNr() + 1 );
  if ( target->lastNr() <= 0 ) // first fetch
    query << "max=" + QString::number( target->maxFetch() );
  destination.setQuery( query.join( "&" ) );
  KIO::Job* job = KIO::listDir( destination, KIO::HideProgressInfo, true );
  connect( job, SIGNAL(entries(KIO::Job*, const KIO::UDSEntryList&)),
           SLOT(slotEntries(KIO::Job*, const KIO::UDSEntryList&)) );
  connect( job, SIGNAL( result(KJob*) ), SLOT( slotResult(KJob*) ) );
  setupKIOJob( job );
}

void KNode::ArticleListJob::slotEntries( KIO::Job * job, const KIO::UDSEntryList & list )
{
  Q_UNUSED( job );
  mArticleList += list;
}

void KNode::ArticleListJob::slotResult( KJob * _job )
{
  Q_ASSERT( mJob == _job );
  KIO::Job *job = static_cast<KIO::Job*>( _job );
  if ( job->error() )
    setError( job->error(), job->errorString() );
  else {
    createProgressItem();

    KNGroup* target = static_cast<KNGroup*>( data() );
    target->setLastFetchCount( 0 );

    setStatus( i18n("Sorting...") );

    if ( job->metaData().contains( "FirstSerialNumber" ) ) {
      int firstSerNum = job->metaData()["FirstSerialNumber"].toInt();
      target->setFirstNr( firstSerNum );
    }

    target->insortNewHeaders( mArticleList, this );

    if ( job->metaData().contains( "LastSerialNumber" ) ) {
      int lastSerNum = job->metaData()["LastSerialNumber"].toInt();
      target->setLastNr( lastSerNum );
    }
  }

  setComplete();

  emitFinished();
}

KNode::ArticleFetchJob::ArticleFetchJob( KNJobConsumer * c, KNServerInfo * a, KNJobItem * i, bool parse ) :
    KNJobData( JTfetchArticle, c, a, i ),
    mParseArticle( parse )
{
}

void KNode::ArticleFetchJob::execute()
{
  KNRemoteArticle *target = static_cast<KNRemoteArticle*>( data() );

  KUrl url = baseUrl();

  url.addPath( static_cast<KNGroup*>( target->collection() )->groupname() );

  // By default, fetch articles by their server-side Id.
  // (some server does not understand the "ARTICLE <msg-id>" command correctly (bug #193550))
  if ( target->articleNumber() != -1 ) {
    url.addPath( QString::number( target->articleNumber() ) );
  } else {
    // User asked to fetch a message by its msg-id
    url.addPath( target->messageID()->as7BitString( false ) );
  }

  KIO::Job* job = KIO::storedGet( url, KIO::NoReload, KIO::HideProgressInfo );
  connect( job, SIGNAL( result(KJob*) ), SLOT( slotResult(KJob*) ) );
  setupKIOJob( job );
}

void KNode::ArticleFetchJob::slotResult( KJob * job )
{
  if ( job->error() )
    setError( job->error(), job->errorString() );
  else {
    KNRemoteArticle *target = static_cast<KNRemoteArticle*>( data() );
    KIO::StoredTransferJob *j = static_cast<KIO::StoredTransferJob*>( job );
    QByteArray buffer = j->data();
    buffer.replace( "\r\n", "\n" ); // TODO: do this in the io-slave?
    target->setContent( buffer );
    if ( mParseArticle )
      target->parse();
  }

  emitFinished();
}

KNode::ArticlePostJob::ArticlePostJob( KNJobConsumer * c, KNServerInfo * a, KNJobItem * i ) :
    KNJobData( JTpostArticle, c, a, i )
{
}

void KNode::ArticlePostJob::execute( )
{
  KNLocalArticle *target = static_cast<KNLocalArticle*>( data() );

  KUrl url = baseUrl();

  KIO::Job* job = KIO::storedPut( target->encodedContent( true ), url, -1, KIO::Overwrite | KIO::HideProgressInfo );
  connect( job, SIGNAL( result(KJob*) ), SLOT( slotResult(KJob*) ) );
  setupKIOJob( job );
}

void KNode::ArticlePostJob::slotResult( KJob * job )
{
  if ( job->error() )
    setError( job->error(), job->errorString() );

  emitFinished();
}

#include "nntpjobs.moc"
<|endoftext|>


<commit_before>/****************************************************************/
/*               DO NOT MODIFY THIS HEADER                      */
/* MOOSE - Multiphysics Object Oriented Simulation Environment  */
/*                                                              */
/*           (c) 2010 Battelle Energy Alliance, LLC             */
/*                   ALL RIGHTS RESERVED                        */
/*                                                              */
/*          Prepared by Battelle Energy Alliance, LLC           */
/*            Under Contract No. DE-AC07-05ID14517              */
/*            With the U. S. Department of Energy               */
/*                                                              */
/*            See COPYRIGHT for full restrictions               */
/****************************************************************/

#include "DGKernel.h"
#include "Moose.h"
#include "AsmBlock.h"
#include "MooseVariable.h"
#include "Problem.h"
#include "SubProblem.h"
#include "SystemBase.h"
#include "MaterialData.h"
#include "ParallelUniqueId.h"

// libMesh includes
#include "dof_map.h"
#include "dense_vector.h"
#include "numeric_vector.h"
#include "dense_subvector.h"
#include "libmesh_common.h"

const unsigned int DGKernel::InternalBndId = 123456789;

template<>
InputParameters validParams<DGKernel>()
{
  InputParameters params = validParams<MooseObject>();
  params.addRequiredParam<std::string>("variable", "The name of the variable that this boundary condition applies to");
  params.addPrivateParam<unsigned int>("_boundary_id", DGKernel::InternalBndId);

  params.addPrivateParam<std::string>("built_by_action", "add_dg_kernel");
  return params;
}

DGKernel::DGKernel(const std::string & name, InputParameters parameters) :
    MooseObject(name, parameters),
    Coupleable(parameters, false),
    TwoMaterialPropertyInterface(parameters),

    _problem(*parameters.get<Problem *>("_problem")),
    _subproblem(*parameters.get<SubProblemInterface *>("_subproblem")),
    _sys(*parameters.get<SystemBase *>("_sys")),
    _tid(parameters.get<THREAD_ID>("_tid")),
    _asmb(_subproblem.asmBlock(_tid)),
    _asm_data(_subproblem.assembly(_tid)),
    _var(_sys.getVariable(_tid, parameters.get<std::string>("variable"))),
    _mesh(_subproblem.mesh()),
    _dim(_mesh.dimension()),

    _current_elem(_asm_data.elem()),
    _neighbor_elem(_asm_data.neighbor()),

    _current_side(_asm_data.side()),
    _current_side_elem(_asm_data.sideElem()),

    _q_point(_subproblem.pointsFace(_tid)),
    _qrule(_subproblem.qRuleFace(_tid)),
    _JxW(_subproblem.JxWFace(_tid)),

    _boundary_id(parameters.get<unsigned int>("_boundary_id")),

    _u(_var.sln()),
    _u_old(_var.slnOld()),
    _u_older(_var.slnOlder()),

    _grad_u(_var.gradSln()),
    _grad_u_old(_var.gradSlnOld()),
    _grad_u_older(_var.gradSlnOlder()),

    _second_u(_var.secondSln()),
    _second_u_old(_var.secondSlnOld()),
    _second_u_older(_var.secondSlnOlder()),

    _phi(_asmb.phiFace()),
    _grad_phi(_asmb.gradPhiFace()),
    _second_phi(_asmb.secondPhiFace()),

    _test(_var.phiFace()),
    _grad_test(_var.gradPhiFace()),
    _second_test(_var.secondPhiFace()),

    _normals(_var.normals()),

    _phi_neighbor(_asmb.phiFaceNeighbor()),
    _grad_phi_neighbor(_asmb.gradPhiFaceNeighbor()),
    _second_phi_neighbor(_asmb.secondPhiFaceNeighbor()),

    _test_neighbor(_var.phiFaceNeighbor()),
    _grad_test_neighbor(_var.gradPhiFaceNeighbor()),
    _second_test_neighbor(_var.secondPhiFaceNeighbor()),

    _u_neighbor(_var.slnNeighbor()),
    _u_old_neighbor(_var.slnOldNeighbor()),
    _u_older_neighbor(_var.slnOlderNeighbor()),

    _grad_u_neighbor(_var.gradSlnNeighbor()),
    _grad_u_old_neighbor(_var.gradSlnOldNeighbor()),
    _grad_u_older_neighbor(_var.gradSlnOlderNeighbor()),

    _second_u_neighbor(_var.secondSlnNeighbor())
{
}

DGKernel::~DGKernel()
{
}

void
DGKernel::computeResidual()
{
  Moose::perf_log.push("computeResidual()","DGKernel");

  DenseVector<Number> & re = _asmb.residualBlock(_var.number());
  DenseVector<Number> & neighbor_re = _asmb.residualBlockNeighbor(_var.number());

  for (_qp=0; _qp<_qrule->n_points(); _qp++)
  {
    for (_i=0; _i<_test.size(); _i++)
      re(_i) += _JxW[_qp]*computeQpResidual(Moose::Element);

    for (_i=0; _i<_test_neighbor.size(); _i++)
      neighbor_re(_i) += _JxW[_qp]*computeQpResidual(Moose::Neighbor);
  }

  Moose::perf_log.pop("computeResidual()","DGKernel");
}

void
DGKernel::computeJacobian()
{
  Moose::perf_log.push("computeJacobian()","DGKernel");

  DenseMatrix<Number> & Kee = _asmb.jacobianBlock(_var.number(), _var.number());
  DenseMatrix<Number> & Ken = _asmb.jacobianBlockNeighbor(Moose::ElementElement, _var.number(), _var.number());

  DenseMatrix<Number> & Kne = _asmb.jacobianBlockNeighbor(Moose::NeighborElement, _var.number(), _var.number());
  DenseMatrix<Number> & Knn = _asmb.jacobianBlockNeighbor(Moose::NeighborNeighbor, _var.number(), _var.number());

  for (_qp=0; _qp<_qrule->n_points(); _qp++)
  {
    for (_i=0; _i<_test.size(); _i++)
      for (_j=0; _j<_phi.size(); _j++)
        Kee(_i,_j) += _JxW[_qp]*computeQpJacobian(Moose::ElementElement);

    for (_i=0; _i<_test.size(); _i++)
      for (_j=0; _j<_phi_neighbor.size(); _j++)
        Ken(_i,_j) += _JxW[_qp]*computeQpJacobian(Moose::ElementNeighbor);

    for (_i=0; _i<_test_neighbor.size(); _i++)
      for (_j=0; _j<_phi.size(); _j++)
        Kne(_i,_j) += _JxW[_qp]*computeQpJacobian(Moose::NeighborElement);

    for (_i=0; _i<_test_neighbor.size(); _i++)
      for (_j=0; _j<_phi_neighbor.size(); _j++)
        Knn(_i,_j) += _JxW[_qp]*computeQpJacobian(Moose::NeighborNeighbor);
  }

  Moose::perf_log.pop("computeJacobian()","DGKernel");
}

VariableValue &
DGKernel::coupledNeighborValue(const std::string & var_name, unsigned int comp)
{
  return getVar(var_name, comp)->slnNeighbor();
}

VariableValue &
DGKernel::coupledNeighborValueOld(const std::string & var_name, unsigned int comp)
{
  return getVar(var_name, comp)->slnOldNeighbor();
}

VariableValue &
DGKernel::coupledNeighborValueOlder(const std::string & var_name, unsigned int comp)
{
  return getVar(var_name, comp)->slnOlderNeighbor();
}

VariableGradient &
DGKernel::coupledNeighborGradient(const std::string & var_name, unsigned int comp)
{
  return getVar(var_name, comp)->gradSlnNeighbor();
}

VariableGradient &
DGKernel::coupledNeighborGradientOld(const std::string & var_name, unsigned int comp)
{
  return getVar(var_name, comp)->gradSlnOldNeighbor();
}

VariableSecond &
DGKernel::coupledNeighborSecond(const std::string & var_name, unsigned int comp)
{
  return getVar(var_name, comp)->secondSlnNeighbor();
}
<commit_msg>Fixing #630<commit_after>/****************************************************************/
/*               DO NOT MODIFY THIS HEADER                      */
/* MOOSE - Multiphysics Object Oriented Simulation Environment  */
/*                                                              */
/*           (c) 2010 Battelle Energy Alliance, LLC             */
/*                   ALL RIGHTS RESERVED                        */
/*                                                              */
/*          Prepared by Battelle Energy Alliance, LLC           */
/*            Under Contract No. DE-AC07-05ID14517              */
/*            With the U. S. Department of Energy               */
/*                                                              */
/*            See COPYRIGHT for full restrictions               */
/****************************************************************/

#include "DGKernel.h"
#include "Moose.h"
#include "AsmBlock.h"
#include "MooseVariable.h"
#include "Problem.h"
#include "SubProblem.h"
#include "SystemBase.h"
#include "MaterialData.h"
#include "ParallelUniqueId.h"

// libMesh includes
#include "dof_map.h"
#include "dense_vector.h"
#include "numeric_vector.h"
#include "dense_subvector.h"
#include "libmesh_common.h"

const unsigned int DGKernel::InternalBndId = 123456789;

template<>
InputParameters validParams<DGKernel>()
{
  InputParameters params = validParams<MooseObject>();
  params.addRequiredParam<std::string>("variable", "The name of the variable that this boundary condition applies to");
  params.addPrivateParam<unsigned int>("_boundary_id", DGKernel::InternalBndId);

  params.addPrivateParam<std::string>("built_by_action", "add_dg_kernel");
  return params;
}

DGKernel::DGKernel(const std::string & name, InputParameters parameters) :
    MooseObject(name, parameters),
    Coupleable(parameters, false),
    TwoMaterialPropertyInterface(parameters),

    _problem(*parameters.get<Problem *>("_problem")),
    _subproblem(*parameters.get<SubProblemInterface *>("_subproblem")),
    _sys(*parameters.get<SystemBase *>("_sys")),
    _tid(parameters.get<THREAD_ID>("_tid")),
    _asmb(_subproblem.asmBlock(_tid)),
    _asm_data(_subproblem.assembly(_tid)),
    _var(_sys.getVariable(_tid, parameters.get<std::string>("variable"))),
    _mesh(_subproblem.mesh()),
    _dim(_mesh.dimension()),

    _current_elem(_asm_data.elem()),
    _neighbor_elem(_asm_data.neighbor()),

    _current_side(_asm_data.side()),
    _current_side_elem(_asm_data.sideElem()),

    _q_point(_subproblem.pointsFace(_tid)),
    _qrule(_subproblem.qRuleFace(_tid)),
    _JxW(_subproblem.JxWFace(_tid)),

    _boundary_id(parameters.get<unsigned int>("_boundary_id")),

    _u(_var.sln()),
    _u_old(_var.slnOld()),
    _u_older(_var.slnOlder()),

    _grad_u(_var.gradSln()),
    _grad_u_old(_var.gradSlnOld()),
    _grad_u_older(_var.gradSlnOlder()),

    _second_u(_var.secondSln()),
    _second_u_old(_var.secondSlnOld()),
    _second_u_older(_var.secondSlnOlder()),

    _phi(_asmb.phiFace()),
    _grad_phi(_asmb.gradPhiFace()),
    _second_phi(_asmb.secondPhiFace()),

    _test(_var.phiFace()),
    _grad_test(_var.gradPhiFace()),
    _second_test(_var.secondPhiFace()),

    _normals(_var.normals()),

    _phi_neighbor(_asmb.phiFaceNeighbor()),
    _grad_phi_neighbor(_asmb.gradPhiFaceNeighbor()),
    _second_phi_neighbor(_asmb.secondPhiFaceNeighbor()),

    _test_neighbor(_var.phiFaceNeighbor()),
    _grad_test_neighbor(_var.gradPhiFaceNeighbor()),
    _second_test_neighbor(_var.secondPhiFaceNeighbor()),

    _u_neighbor(_var.slnNeighbor()),
    _u_old_neighbor(_var.slnOldNeighbor()),
    _u_older_neighbor(_var.slnOlderNeighbor()),

    _grad_u_neighbor(_var.gradSlnNeighbor()),
    _grad_u_old_neighbor(_var.gradSlnOldNeighbor()),
    _grad_u_older_neighbor(_var.gradSlnOlderNeighbor()),

    _second_u_neighbor(_var.secondSlnNeighbor())
{
}

DGKernel::~DGKernel()
{
}

void
DGKernel::computeResidual()
{
  Moose::perf_log.push("computeResidual()","DGKernel");

  DenseVector<Number> & re = _asmb.residualBlock(_var.number());
  DenseVector<Number> & neighbor_re = _asmb.residualBlockNeighbor(_var.number());

  for (_qp=0; _qp<_qrule->n_points(); _qp++)
  {
    for (_i=0; _i<_test.size(); _i++)
      re(_i) += _JxW[_qp]*computeQpResidual(Moose::Element);

    for (_i=0; _i<_test_neighbor.size(); _i++)
      neighbor_re(_i) += _JxW[_qp]*computeQpResidual(Moose::Neighbor);
  }

  Moose::perf_log.pop("computeResidual()","DGKernel");
}

void
DGKernel::computeJacobian()
{
  Moose::perf_log.push("computeJacobian()","DGKernel");

  DenseMatrix<Number> & Kee = _asmb.jacobianBlock(_var.number(), _var.number());
  DenseMatrix<Number> & Ken = _asmb.jacobianBlockNeighbor(Moose::ElementNeighbor, _var.number(), _var.number());

  DenseMatrix<Number> & Kne = _asmb.jacobianBlockNeighbor(Moose::NeighborElement, _var.number(), _var.number());
  DenseMatrix<Number> & Knn = _asmb.jacobianBlockNeighbor(Moose::NeighborNeighbor, _var.number(), _var.number());

  for (_qp=0; _qp<_qrule->n_points(); _qp++)
  {
    for (_i=0; _i<_test.size(); _i++)
      for (_j=0; _j<_phi.size(); _j++)
        Kee(_i,_j) += _JxW[_qp]*computeQpJacobian(Moose::ElementElement);

    for (_i=0; _i<_test.size(); _i++)
      for (_j=0; _j<_phi_neighbor.size(); _j++)
        Ken(_i,_j) += _JxW[_qp]*computeQpJacobian(Moose::ElementNeighbor);

    for (_i=0; _i<_test_neighbor.size(); _i++)
      for (_j=0; _j<_phi.size(); _j++)
        Kne(_i,_j) += _JxW[_qp]*computeQpJacobian(Moose::NeighborElement);

    for (_i=0; _i<_test_neighbor.size(); _i++)
      for (_j=0; _j<_phi_neighbor.size(); _j++)
        Knn(_i,_j) += _JxW[_qp]*computeQpJacobian(Moose::NeighborNeighbor);
  }

  Moose::perf_log.pop("computeJacobian()","DGKernel");
}

VariableValue &
DGKernel::coupledNeighborValue(const std::string & var_name, unsigned int comp)
{
  return getVar(var_name, comp)->slnNeighbor();
}

VariableValue &
DGKernel::coupledNeighborValueOld(const std::string & var_name, unsigned int comp)
{
  return getVar(var_name, comp)->slnOldNeighbor();
}

VariableValue &
DGKernel::coupledNeighborValueOlder(const std::string & var_name, unsigned int comp)
{
  return getVar(var_name, comp)->slnOlderNeighbor();
}

VariableGradient &
DGKernel::coupledNeighborGradient(const std::string & var_name, unsigned int comp)
{
  return getVar(var_name, comp)->gradSlnNeighbor();
}

VariableGradient &
DGKernel::coupledNeighborGradientOld(const std::string & var_name, unsigned int comp)
{
  return getVar(var_name, comp)->gradSlnOldNeighbor();
}

VariableSecond &
DGKernel::coupledNeighborSecond(const std::string & var_name, unsigned int comp)
{
  return getVar(var_name, comp)->secondSlnNeighbor();
}
<|endoftext|>


<commit_before>/****************************************************************************
 *
 *   Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name ECL nor the names of its contributors may 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
 * COPYRIGHT OWNER 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.
 *
 ****************************************************************************/

/**
 * @file terrain_estimator.cpp
 * Function for fusing rangefinder measurements to estimate terrain vertical position/
 *
 * @author Paul Riseborough <p_riseborough@live.com.au>
 *
 */

#include "ekf.h"
#include <ecl.h>
#include <mathlib/mathlib.h>

bool Ekf::initHagl()
{
	bool initialized = false;

	if (!_control_status.flags.in_air) {
		// if on ground, do not trust the range sensor, but assume a ground clearance
		_terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance;
		// use the ground clearance value as our uncertainty
		_terrain_var = sq(_params.rng_gnd_clearance);
		_time_last_fake_hagl_fuse = _time_last_imu;
		initialized = true;

	} else if (shouldUseRangeFinderForHagl()
		   && _range_sensor.isDataHealthy()) {
		// if we have a fresh measurement, use it to initialise the terrain estimator
		_terrain_vpos = _state.pos(2) + _range_sensor.getDistBottom();
		// initialise state variance to variance of measurement
		_terrain_var = sq(_params.range_noise);
		// success
		initialized = true;

	} else if (shouldUseOpticalFlowForHagl()
		   && _flow_for_terrain_data_ready) {
		// initialise terrain vertical position to origin as this is the best guess we have
		_terrain_vpos = fmaxf(0.0f,  _state.pos(2));
		_terrain_var = 100.0f;
		initialized = true;

	} else {
		// no information - cannot initialise
	}

	if (initialized) {
		// has initialized with valid data
		_time_last_hagl_fuse = _time_last_imu;
	}

	return initialized;
}

bool Ekf::shouldUseRangeFinderForHagl() const
{
	return (_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseRangeFinder);
}

bool Ekf::shouldUseOpticalFlowForHagl() const
{
	return (_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseOpticalFlow);
}

void Ekf::runTerrainEstimator()
{
	// If we are on ground, store the local position and time to use as a reference
	if (!_control_status.flags.in_air) {
		_last_on_ground_posD = _state.pos(2);
	}

	// Perform initialisation check and
	// on ground, continuously reset the terrain estimator
	if (!_terrain_initialised || !_control_status.flags.in_air) {
		_terrain_initialised = initHagl();

	} else {

		// predict the state variance growth where the state is the vertical position of the terrain underneath the vehicle

		// process noise due to errors in vehicle height estimate
		_terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_p_noise);

		// process noise due to terrain gradient
		_terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_gradient)
				* (sq(_state.vel(0)) + sq(_state.vel(1)));

		// limit the variance to prevent it becoming badly conditioned
		_terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f);

		// Fuse range finder data if available
		if (shouldUseRangeFinderForHagl()
		    && _range_sensor.isDataHealthy()) {
			fuseHagl();
		}

		if (shouldUseOpticalFlowForHagl()
		    && _flow_for_terrain_data_ready) {
			fuseFlowForTerrain();
			_flow_for_terrain_data_ready = false;
		}

		// constrain _terrain_vpos to be a minimum of _params.rng_gnd_clearance larger than _state.pos(2)
		if (_terrain_vpos - _state.pos(2) < _params.rng_gnd_clearance) {
			_terrain_vpos = _params.rng_gnd_clearance + _state.pos(2);
		}
	}

	updateTerrainValidity();
}

void Ekf::fuseHagl()
{
	// get a height above ground measurement from the range finder assuming a flat earth
	const float meas_hagl = _range_sensor.getDistBottom();

	// predict the hagl from the vehicle position and terrain height
	const float pred_hagl = _terrain_vpos - _state.pos(2);

	// calculate the innovation
	_hagl_innov = pred_hagl - meas_hagl;

	// calculate the observation variance adding the variance of the vehicles own height uncertainty
	const float obs_variance = fmaxf(P(9,9) * _params.vehicle_variance_scaler, 0.0f)
			     + sq(_params.range_noise)
			     + sq(_params.range_noise_scaler * _range_sensor.getRange());

	// calculate the innovation variance - limiting it to prevent a badly conditioned fusion
	_hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance);

	// perform an innovation consistency check and only fuse data if it passes
	const float gate_size = fmaxf(_params.range_innov_gate, 1.0f);
	_hagl_test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var);

	if (_hagl_test_ratio <= 1.0f) {
		// calculate the Kalman gain
		float gain = _terrain_var / _hagl_innov_var;
		// correct the state
		_terrain_vpos -= gain * _hagl_innov;
		// correct the variance
		_terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f);
		// record last successful fusion event
		_time_last_hagl_fuse = _time_last_imu;
		_innov_check_fail_status.flags.reject_hagl = false;

	} else {
		// If we have been rejecting range data for too long, reset to measurement
		if (isTimedOut(_time_last_hagl_fuse, (uint64_t)10E6)) {
			_terrain_vpos = _state.pos(2) + meas_hagl;
			_terrain_var = obs_variance;

		} else {
			_innov_check_fail_status.flags.reject_hagl = true;
		}
	}
}

void Ekf::fuseFlowForTerrain()
{
	// calculate optical LOS rates using optical flow rates that have had the body angular rate contribution removed
	// correct for gyro bias errors in the data used to do the motion compensation
	// Note the sign convention used: A positive LOS rate is a RH rotation of the scene about that axis.
	const Vector2f opt_flow_rate = Vector2f(_flow_compensated_XY_rad) / _flow_sample_delayed.dt + Vector2f(_flow_gyro_bias);

	// get latest estimated orientation
	const float q0 = _state.quat_nominal(0);
	const float q1 = _state.quat_nominal(1);
	const float q2 = _state.quat_nominal(2);
	const float q3 = _state.quat_nominal(3);

	// calculate the optical flow observation variance
	const float R_LOS = calcOptFlowMeasVar();

	// get rotation matrix from earth to body
	const Dcmf earth_to_body = quatToInverseRotMat(_state.quat_nominal);

	// calculate the sensor position relative to the IMU
	const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;

	// calculate the velocity of the sensor relative to the imu in body frame
	// Note: _flow_sample_delayed.gyro_xyz is the negative of the body angular velocity, thus use minus sign
	const Vector3f vel_rel_imu_body = Vector3f(-_flow_sample_delayed.gyro_xyz / _flow_sample_delayed.dt) % pos_offset_body;

	// calculate the velocity of the sensor in the earth frame
	const Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body;

	// rotate into body frame
	const Vector3f vel_body = earth_to_body * vel_rel_earth;

	const float t0 = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;

	// constrain terrain to minimum allowed value and predict height above ground
	_terrain_vpos = fmaxf(_terrain_vpos, _params.rng_gnd_clearance + _state.pos(2));
	const float pred_hagl = _terrain_vpos - _state.pos(2);

	// Calculate observation matrix for flow around the vehicle x axis
	const float Hx = vel_body(1) * t0 / (pred_hagl * pred_hagl);

	// Constrain terrain variance to be non-negative
	_terrain_var = fmaxf(_terrain_var, 0.0f);

	// Cacluate innovation variance
	_flow_innov_var[0] = Hx * Hx * _terrain_var + R_LOS;

	// calculate the kalman gain for the flow x measurement
	const float Kx = _terrain_var * Hx / _flow_innov_var[0];

	// calculate prediced optical flow about x axis
	const float pred_flow_x = vel_body(1) * earth_to_body(2, 2) / pred_hagl;

	// calculate flow innovation (x axis)
	_flow_innov[0] = pred_flow_x - opt_flow_rate(0);

	// calculate correction term for terrain variance
	const float KxHxP =  Kx * Hx * _terrain_var;

	// innovation consistency check
	const float gate_size = fmaxf(_params.flow_innov_gate, 1.0f);
	float flow_test_ratio = sq(_flow_innov[0]) / (sq(gate_size) * _flow_innov_var[0]);

	// do not perform measurement update if badly conditioned
	if (flow_test_ratio <= 1.0f) {
		_terrain_vpos += Kx * _flow_innov[0];
		// guard against negative variance
		_terrain_var = fmaxf(_terrain_var - KxHxP, 0.0f);
		_time_last_of_fuse = _time_last_imu;
	}

	// Calculate observation matrix for flow around the vehicle y axis
	const float Hy = -vel_body(0) * t0 / (pred_hagl * pred_hagl);

	// Calculuate innovation variance
	_flow_innov_var[1] = Hy * Hy * _terrain_var + R_LOS;

	// calculate the kalman gain for the flow y measurement
	const float Ky = _terrain_var * Hy / _flow_innov_var[1];

	// calculate prediced optical flow about y axis
	const float pred_flow_y = -vel_body(0) * earth_to_body(2, 2) / pred_hagl;

	// calculate flow innovation (y axis)
	_flow_innov[1] = pred_flow_y - opt_flow_rate(1);

	// calculate correction term for terrain variance
	const float KyHyP =  Ky * Hy * _terrain_var;

	// innovation consistency check
	flow_test_ratio = sq(_flow_innov[1]) / (sq(gate_size) * _flow_innov_var[1]);

	if (flow_test_ratio <= 1.0f) {
		_terrain_vpos += Ky * _flow_innov[1];
		// guard against negative variance
		_terrain_var = fmaxf(_terrain_var - KyHyP, 0.0f);
		_time_last_of_fuse = _time_last_imu;
	}
}

bool Ekf::isTerrainEstimateValid() const
{
	return _hagl_valid;
}

void Ekf::updateTerrainValidity()
{
	// we have been fusing range finder measurements in the last 5 seconds
	const bool recent_range_fusion = isRecent(_time_last_hagl_fuse, (uint64_t)5e6);

	// we have been fusing optical flow measurements for terrain estimation within the last 5 seconds
	// this can only be the case if the main filter does not fuse optical flow
	const bool recent_flow_for_terrain_fusion = isRecent(_time_last_of_fuse, (uint64_t)5e6)
						    && !_control_status.flags.opt_flow;

	_hagl_valid = (_terrain_initialised && (recent_range_fusion || recent_flow_for_terrain_fusion));

	_hagl_sensor_status.flags.range_finder = shouldUseRangeFinderForHagl()
						 && recent_range_fusion
						 && (_time_last_fake_hagl_fuse != _time_last_hagl_fuse);
	_hagl_sensor_status.flags.flow = shouldUseOpticalFlowForHagl()
					 && recent_flow_for_terrain_fusion;
}

// get the estimated vertical position of the terrain relative to the NED origin
float Ekf::getTerrainVertPos() const
{
	return _terrain_vpos;
}
<commit_msg>Get rid of non functional piece of code<commit_after>/****************************************************************************
 *
 *   Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name ECL nor the names of its contributors may 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
 * COPYRIGHT OWNER 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.
 *
 ****************************************************************************/

/**
 * @file terrain_estimator.cpp
 * Function for fusing rangefinder measurements to estimate terrain vertical position/
 *
 * @author Paul Riseborough <p_riseborough@live.com.au>
 *
 */

#include "ekf.h"
#include <ecl.h>
#include <mathlib/mathlib.h>

bool Ekf::initHagl()
{
	bool initialized = false;

	if (!_control_status.flags.in_air) {
		// if on ground, do not trust the range sensor, but assume a ground clearance
		_terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance;
		// use the ground clearance value as our uncertainty
		_terrain_var = sq(_params.rng_gnd_clearance);
		_time_last_fake_hagl_fuse = _time_last_imu;
		initialized = true;

	} else if (shouldUseRangeFinderForHagl()
		   && _range_sensor.isDataHealthy()) {
		// if we have a fresh measurement, use it to initialise the terrain estimator
		_terrain_vpos = _state.pos(2) + _range_sensor.getDistBottom();
		// initialise state variance to variance of measurement
		_terrain_var = sq(_params.range_noise);
		// success
		initialized = true;

	} else if (shouldUseOpticalFlowForHagl()
		   && _flow_for_terrain_data_ready) {
		// initialise terrain vertical position to origin as this is the best guess we have
		_terrain_vpos = fmaxf(0.0f,  _state.pos(2));
		_terrain_var = 100.0f;
		initialized = true;

	} else {
		// no information - cannot initialise
	}

	if (initialized) {
		// has initialized with valid data
		_time_last_hagl_fuse = _time_last_imu;
	}

	return initialized;
}

bool Ekf::shouldUseRangeFinderForHagl() const
{
	return (_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseRangeFinder);
}

bool Ekf::shouldUseOpticalFlowForHagl() const
{
	return (_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseOpticalFlow);
}

void Ekf::runTerrainEstimator()
{
	// If we are on ground, store the local position and time to use as a reference
	if (!_control_status.flags.in_air) {
		_last_on_ground_posD = _state.pos(2);
	}

	// Perform initialisation check and
	// on ground, continuously reset the terrain estimator
	if (!_terrain_initialised || !_control_status.flags.in_air) {
		_terrain_initialised = initHagl();

	} else {

		// predict the state variance growth where the state is the vertical position of the terrain underneath the vehicle

		// process noise due to errors in vehicle height estimate
		_terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_p_noise);

		// process noise due to terrain gradient
		_terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_gradient)
				* (sq(_state.vel(0)) + sq(_state.vel(1)));

		// limit the variance to prevent it becoming badly conditioned
		_terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f);

		// Fuse range finder data if available
		if (shouldUseRangeFinderForHagl()
		    && _range_sensor.isDataHealthy()) {
			fuseHagl();
		}

		if (shouldUseOpticalFlowForHagl()
		    && _flow_for_terrain_data_ready) {
			fuseFlowForTerrain();
			_flow_for_terrain_data_ready = false;
		}

		// constrain _terrain_vpos to be a minimum of _params.rng_gnd_clearance larger than _state.pos(2)
		if (_terrain_vpos - _state.pos(2) < _params.rng_gnd_clearance) {
			_terrain_vpos = _params.rng_gnd_clearance + _state.pos(2);
		}
	}

	updateTerrainValidity();
}

void Ekf::fuseHagl()
{
	// get a height above ground measurement from the range finder assuming a flat earth
	const float meas_hagl = _range_sensor.getDistBottom();

	// predict the hagl from the vehicle position and terrain height
	const float pred_hagl = _terrain_vpos - _state.pos(2);

	// calculate the innovation
	_hagl_innov = pred_hagl - meas_hagl;

	// calculate the observation variance adding the variance of the vehicles own height uncertainty
	const float obs_variance = fmaxf(P(9,9) * _params.vehicle_variance_scaler, 0.0f)
			     + sq(_params.range_noise)
			     + sq(_params.range_noise_scaler * _range_sensor.getRange());

	// calculate the innovation variance - limiting it to prevent a badly conditioned fusion
	_hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance);

	// perform an innovation consistency check and only fuse data if it passes
	const float gate_size = fmaxf(_params.range_innov_gate, 1.0f);
	_hagl_test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var);

	if (_hagl_test_ratio <= 1.0f) {
		// calculate the Kalman gain
		float gain = _terrain_var / _hagl_innov_var;
		// correct the state
		_terrain_vpos -= gain * _hagl_innov;
		// correct the variance
		_terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f);
		// record last successful fusion event
		_time_last_hagl_fuse = _time_last_imu;
		_innov_check_fail_status.flags.reject_hagl = false;

	} else {
		// If we have been rejecting range data for too long, reset to measurement
		if (isTimedOut(_time_last_hagl_fuse, (uint64_t)10E6)) {
			_terrain_vpos = _state.pos(2) + meas_hagl;
			_terrain_var = obs_variance;

		} else {
			_innov_check_fail_status.flags.reject_hagl = true;
		}
	}
}

void Ekf::fuseFlowForTerrain()
{
	// calculate optical LOS rates using optical flow rates that have had the body angular rate contribution removed
	// correct for gyro bias errors in the data used to do the motion compensation
	// Note the sign convention used: A positive LOS rate is a RH rotation of the scene about that axis.
	const Vector2f opt_flow_rate = _flow_compensated_XY_rad / _flow_sample_delayed.dt + Vector2f(_flow_gyro_bias);

	// get latest estimated orientation
	const float q0 = _state.quat_nominal(0);
	const float q1 = _state.quat_nominal(1);
	const float q2 = _state.quat_nominal(2);
	const float q3 = _state.quat_nominal(3);

	// calculate the optical flow observation variance
	const float R_LOS = calcOptFlowMeasVar();

	// get rotation matrix from earth to body
	const Dcmf earth_to_body = quatToInverseRotMat(_state.quat_nominal);

	// calculate the sensor position relative to the IMU
	const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;

	// calculate the velocity of the sensor relative to the imu in body frame
	// Note: _flow_sample_delayed.gyro_xyz is the negative of the body angular velocity, thus use minus sign
	const Vector3f vel_rel_imu_body = Vector3f(-_flow_sample_delayed.gyro_xyz / _flow_sample_delayed.dt) % pos_offset_body;

	// calculate the velocity of the sensor in the earth frame
	const Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body;

	// rotate into body frame
	const Vector3f vel_body = earth_to_body * vel_rel_earth;

	const float t0 = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;

	// constrain terrain to minimum allowed value and predict height above ground
	_terrain_vpos = fmaxf(_terrain_vpos, _params.rng_gnd_clearance + _state.pos(2));
	const float pred_hagl = _terrain_vpos - _state.pos(2);

	// Calculate observation matrix for flow around the vehicle x axis
	const float Hx = vel_body(1) * t0 / (pred_hagl * pred_hagl);

	// Constrain terrain variance to be non-negative
	_terrain_var = fmaxf(_terrain_var, 0.0f);

	// Cacluate innovation variance
	_flow_innov_var[0] = Hx * Hx * _terrain_var + R_LOS;

	// calculate the kalman gain for the flow x measurement
	const float Kx = _terrain_var * Hx / _flow_innov_var[0];

	// calculate prediced optical flow about x axis
	const float pred_flow_x = vel_body(1) * earth_to_body(2, 2) / pred_hagl;

	// calculate flow innovation (x axis)
	_flow_innov[0] = pred_flow_x - opt_flow_rate(0);

	// calculate correction term for terrain variance
	const float KxHxP =  Kx * Hx * _terrain_var;

	// innovation consistency check
	const float gate_size = fmaxf(_params.flow_innov_gate, 1.0f);
	float flow_test_ratio = sq(_flow_innov[0]) / (sq(gate_size) * _flow_innov_var[0]);

	// do not perform measurement update if badly conditioned
	if (flow_test_ratio <= 1.0f) {
		_terrain_vpos += Kx * _flow_innov[0];
		// guard against negative variance
		_terrain_var = fmaxf(_terrain_var - KxHxP, 0.0f);
		_time_last_of_fuse = _time_last_imu;
	}

	// Calculate observation matrix for flow around the vehicle y axis
	const float Hy = -vel_body(0) * t0 / (pred_hagl * pred_hagl);

	// Calculuate innovation variance
	_flow_innov_var[1] = Hy * Hy * _terrain_var + R_LOS;

	// calculate the kalman gain for the flow y measurement
	const float Ky = _terrain_var * Hy / _flow_innov_var[1];

	// calculate prediced optical flow about y axis
	const float pred_flow_y = -vel_body(0) * earth_to_body(2, 2) / pred_hagl;

	// calculate flow innovation (y axis)
	_flow_innov[1] = pred_flow_y - opt_flow_rate(1);

	// calculate correction term for terrain variance
	const float KyHyP =  Ky * Hy * _terrain_var;

	// innovation consistency check
	flow_test_ratio = sq(_flow_innov[1]) / (sq(gate_size) * _flow_innov_var[1]);

	if (flow_test_ratio <= 1.0f) {
		_terrain_vpos += Ky * _flow_innov[1];
		// guard against negative variance
		_terrain_var = fmaxf(_terrain_var - KyHyP, 0.0f);
		_time_last_of_fuse = _time_last_imu;
	}
}

bool Ekf::isTerrainEstimateValid() const
{
	return _hagl_valid;
}

void Ekf::updateTerrainValidity()
{
	// we have been fusing range finder measurements in the last 5 seconds
	const bool recent_range_fusion = isRecent(_time_last_hagl_fuse, (uint64_t)5e6);

	// we have been fusing optical flow measurements for terrain estimation within the last 5 seconds
	// this can only be the case if the main filter does not fuse optical flow
	const bool recent_flow_for_terrain_fusion = isRecent(_time_last_of_fuse, (uint64_t)5e6)
						    && !_control_status.flags.opt_flow;

	_hagl_valid = (_terrain_initialised && (recent_range_fusion || recent_flow_for_terrain_fusion));

	_hagl_sensor_status.flags.range_finder = shouldUseRangeFinderForHagl()
						 && recent_range_fusion
						 && (_time_last_fake_hagl_fuse != _time_last_hagl_fuse);
	_hagl_sensor_status.flags.flow = shouldUseOpticalFlowForHagl()
					 && recent_flow_for_terrain_fusion;
}

// get the estimated vertical position of the terrain relative to the NED origin
float Ekf::getTerrainVertPos() const
{
	return _terrain_vpos;
}
<|endoftext|>


<commit_before>/*=========================================================================

  Program:   Visualization Toolkit
  Module:    TestLinePlot.cxx

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/

#include "vtkRenderer.h"
#include "vtkRenderWindow.h"
#include "vtkSmartPointer.h"
#include "vtkChartXY.h"
#include "vtkPlotPoints.h"
#include "vtkTable.h"
#include "vtkFloatArray.h"
#include "vtkContextView.h"
#include "vtkContextScene.h"
#include "vtkRenderWindowInteractor.h"

//----------------------------------------------------------------------------
int TestScatterPlot(int , char * [])
{
  // Set up a 2D scene, add an XY chart to it
  vtkSmartPointer<vtkContextView> view =
      vtkSmartPointer<vtkContextView>::New();
  view->GetRenderer()->SetBackground(1.0, 1.0, 1.0);
  view->GetRenderWindow()->SetSize(400, 300);
  vtkSmartPointer<vtkChartXY> chart = vtkSmartPointer<vtkChartXY>::New();
  view->GetScene()->AddItem(chart);
  chart->SetShowLegend(true);

  // Create a table with some points in it...
  vtkSmartPointer<vtkTable> table = vtkSmartPointer<vtkTable>::New();
  vtkSmartPointer<vtkFloatArray> arrX = vtkSmartPointer<vtkFloatArray>::New();
  arrX->SetName("X Axis");
  table->AddColumn(arrX);
  vtkSmartPointer<vtkFloatArray> arrC = vtkSmartPointer<vtkFloatArray>::New();
  arrC->SetName("Cosine");
  table->AddColumn(arrC);
  vtkSmartPointer<vtkFloatArray> arrS = vtkSmartPointer<vtkFloatArray>::New();
  arrS->SetName("Sine");
  table->AddColumn(arrS);
  vtkSmartPointer<vtkFloatArray> arrT = vtkSmartPointer<vtkFloatArray>::New();
  arrT->SetName("Tan");
  table->AddColumn(arrT);
  // Test charting with a few more points...
  int numPoints = 40;
  float inc = 7.5 / (numPoints-1);
  table->SetNumberOfRows(numPoints);
  for (int i = 0; i < numPoints; ++i)
    {
    table->SetValue(i, 0, i * inc);
    table->SetValue(i, 1, cos(i * inc) + 0.0);
    table->SetValue(i, 2, sin(i * inc) + 0.0);
    table->SetValue(i, 3, tan(i * inc) + 0.5);
    }

  // Add multiple line plots, setting the colors etc
  vtkPlot *points = chart->AddPlot(vtkChart::POINTS);
  points->SetInput(table, 0, 1);
  points->SetColor(0, 0, 0, 255);
  points->SetWidth(1.0);
  vtkPlotPoints::SafeDownCast(points)->SetMarkerStyle(vtkPlotPoints::CROSS);
  points = chart->AddPlot(vtkChart::POINTS);
  points->SetInput(table, 0, 2);
  points->SetColor(0, 0, 0, 255);
  points->SetWidth(1.0);
  vtkPlotPoints::SafeDownCast(points)->SetMarkerStyle(vtkPlotPoints::PLUS);
  points = chart->AddPlot(vtkChart::POINTS);
  points->SetInput(table, 0, 3);
  points->SetColor(0, 0, 255, 255);
  points->SetWidth(4.0);

  //Finally render the scene and compare the image to a reference image
  view->GetRenderWindow()->SetMultiSamples(0);
  view->GetInteractor()->Initialize();
  view->GetInteractor()->Start();

  return EXIT_SUCCESS;
}
<commit_msg>ENH: Use vtkNew instead of vtkSmartPointer.<commit_after>/*=========================================================================

  Program:   Visualization Toolkit
  Module:    TestLinePlot.cxx

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/

#include "vtkRenderer.h"
#include "vtkRenderWindow.h"
#include "vtkNew.h"
#include "vtkChartXY.h"
#include "vtkPlotPoints.h"
#include "vtkTable.h"
#include "vtkFloatArray.h"
#include "vtkStringArray.h"
#include "vtkContextView.h"
#include "vtkContextScene.h"
#include "vtkRenderWindowInteractor.h"

//----------------------------------------------------------------------------
int TestScatterPlot(int , char * [])
{
  // Set up a 2D scene, add an XY chart to it
  vtkNew<vtkContextView> view;
  view->GetRenderer()->SetBackground(1.0, 1.0, 1.0);
  view->GetRenderWindow()->SetSize(400, 300);
  vtkNew<vtkChartXY> chart;
  view->GetScene()->AddItem(chart.GetPointer());
  chart->SetShowLegend(true);

  // Create a table with some points in it...
  vtkNew<vtkTable> table;
  vtkNew<vtkFloatArray> arrX;
  arrX->SetName("X Axis");
  table->AddColumn(arrX.GetPointer());
  vtkNew<vtkFloatArray> arrC;
  arrC->SetName("Cosine");
  table->AddColumn(arrC.GetPointer());
  vtkNew<vtkFloatArray> arrS;
  arrS->SetName("Sine");
  table->AddColumn(arrS.GetPointer());
  vtkNew<vtkFloatArray> arrT;
  arrT->SetName("Tan");
  table->AddColumn(arrT.GetPointer());

  // Test charting with a few more points...
  int numPoints = 40;
  float inc = 7.5 / (numPoints-1);
  table->SetNumberOfRows(numPoints);
  for (int i = 0; i < numPoints; ++i)
    {
    table->SetValue(i, 0, i * inc);
    table->SetValue(i, 1, cos(i * inc) + 0.0);
    table->SetValue(i, 2, sin(i * inc) + 0.0);
    table->SetValue(i, 3, tan(i * inc) + 0.5);
    }

  // Add multiple line plots, setting the colors etc
  vtkPlot *points = chart->AddPlot(vtkChart::POINTS);
  points->SetInput(table.GetPointer(), 0, 1);
  points->SetColor(0, 0, 0, 255);
  points->SetWidth(1.0);
  vtkPlotPoints::SafeDownCast(points)->SetMarkerStyle(vtkPlotPoints::CROSS);
  points = chart->AddPlot(vtkChart::POINTS);
  points->SetInput(table.GetPointer(), 0, 2);
  points->SetColor(0, 0, 0, 255);
  points->SetWidth(1.0);
  vtkPlotPoints::SafeDownCast(points)->SetMarkerStyle(vtkPlotPoints::PLUS);
  points = chart->AddPlot(vtkChart::POINTS);
  points->SetInput(table.GetPointer(), 0, 3);
  points->SetColor(0, 0, 255, 255);
  points->SetWidth(4.0);

  //Finally render the scene and compare the image to a reference image
  view->GetRenderWindow()->SetMultiSamples(0);
  view->GetInteractor()->Initialize();
  view->GetInteractor()->Start();

  return EXIT_SUCCESS;
}
<|endoftext|>


<commit_before>#include "ScoreDefinition.h"
#include "../common.h"
#include "../mahjong-algorithm/points_calculator.h"

USING_NS_CC;

static std::vector<std::string> g_vec;

Scene *ScoreDefinitionScene::createScene(size_t idx) {
    if (g_vec.empty()) {
        ValueVector valueVec = FileUtils::getInstance()->getValueVectorFromFile("score_definition.xml");
        g_vec.reserve(valueVec.size());
        std::transform(valueVec.begin(), valueVec.end(), std::back_inserter(g_vec), std::bind(&Value::asString, std::placeholders::_1));
    }

    auto scene = Scene::create();
    auto layer = new (std::nothrow) ScoreDefinitionScene();
    layer->initWithIndex(idx);

    scene->addChild(layer);
    return scene;
}

bool ScoreDefinitionScene::initWithIndex(size_t idx) {
    if (!Layer::init()) {
        return false;
    }

    auto listener = EventListenerKeyboard::create();
    listener->onKeyReleased = CC_CALLBACK_2(Layer::onKeyReleased, this);
    _eventDispatcher->addEventListenerWithSceneGraphPriority(listener, this);

    Size visibleSize = Director::getInstance()->getVisibleSize();
    Vec2 origin = Director::getInstance()->getVisibleOrigin();

    Label *tileLabel = Label::createWithSystemFont(mahjong::points_name[idx], "Arial", 20);
    this->addChild(tileLabel);
    tileLabel->setPosition(Vec2(origin.x + visibleSize.width * 0.5f,
        origin.y + visibleSize.height - tileLabel->getContentSize().height * 0.5f));

    ui::Button *backBtn = ui::Button::create();
    this->addChild(backBtn);
    backBtn->setScale9Enabled(true);
    backBtn->setContentSize(Size(45, 20));
    backBtn->setTitleFontSize(16);
    backBtn->setTitleText("返回");
    backBtn->setPosition(Vec2(origin.x + 15, origin.y + visibleSize.height - 10));
    backBtn->addClickEventListener([](Ref *) {
        Director::getInstance()->popScene();
    });

    std::string &text = g_vec[idx];
    float scale = 1.0f;
    float maxWidth = (visibleSize.width - 10) / 18;
    if (maxWidth < 25) {
        scale = maxWidth / 27;
    }

    char tilesStr[128];
    mahjong::TILE tiles[14];
    long tilesCnt;
    char imgStr[1024];

    std::string::size_type pos = text.find('[');
    while (pos != std::string::npos) {
        const char *str = text.c_str();
        int readLen;
        if (sscanf(str + pos + 1, "%[^]]%n", tilesStr, &readLen) != EOF && str[pos + readLen + 1] == ']'
            && mahjong::parse_tiles(tilesStr, tiles, &tilesCnt) != nullptr) {
            size_t totalWriteLen = 0;
            for (long i = 0; i < tilesCnt; ++i) {
                int writeLen = snprintf(imgStr + totalWriteLen , sizeof(imgStr) - totalWriteLen, "", tilesImageName[tiles[i]], (int)(27 * scale), (int)(39 * scale));
                totalWriteLen += writeLen;
            }
            text.replace(pos, readLen + 2, imgStr);
            pos = text.find('[', pos + totalWriteLen);
        }
        else {
            pos = text.find('[', pos + 1);
        }
    }

    ui::RichText *richText = ui::RichText::createWithXML(text);
    richText->setContentSize(Size(visibleSize.width - 10, 0));
    richText->ignoreContentAdaptWithSize(false);
    richText->setVerticalSpace(2);
    richText->formatText();
    this->addChild(richText);
    richText->setPosition(Vec2(origin.x + visibleSize.width * 0.5f, origin.y + visibleSize.height - 40));
    return true;
}

void ScoreDefinitionScene::onKeyReleased(EventKeyboard::KeyCode keyCode, Event *unusedEvent) {
    if (keyCode == EventKeyboard::KeyCode::KEY_BACK) {
        Director::getInstance()->popScene();
    }
}
<commit_msg>封装成一个函数<commit_after>#include "ScoreDefinition.h"
#include "../common.h"
#include "../mahjong-algorithm/points_calculator.h"

USING_NS_CC;

static std::vector<std::string> g_vec;

static void replaceTilesToImage(std::string &text, float scale) {
    char tilesStr[128];
    mahjong::TILE tiles[14];
    long tilesCnt;
    char imgStr[1024];

    std::string::size_type pos = text.find('[');
    while (pos != std::string::npos) {
        const char *str = text.c_str();
        int readLen;
        if (sscanf(str + pos + 1, "%[^]]%n", tilesStr, &readLen) != EOF
            && str[pos + readLen + 1] == ']'
            && mahjong::parse_tiles(tilesStr, tiles, &tilesCnt) != nullptr) {
            size_t totalWriteLen = 0;
            for (long i = 0; i < tilesCnt; ++i) {
                int writeLen = snprintf(imgStr + totalWriteLen, sizeof(imgStr) - totalWriteLen,
                    "", tilesImageName[tiles[i]],
                    (int)(27 * scale), (int)(39 * scale));
                totalWriteLen += writeLen;
            }
            text.replace(pos, readLen + 2, imgStr);
            pos = text.find('[', pos + totalWriteLen);
        }
        else {
            pos = text.find('[', pos + 1);
        }
    }
}

Scene *ScoreDefinitionScene::createScene(size_t idx) {
    if (g_vec.empty()) {
        ValueVector valueVec = FileUtils::getInstance()->getValueVectorFromFile("score_definition.xml");
        g_vec.reserve(valueVec.size());
        std::transform(valueVec.begin(), valueVec.end(), std::back_inserter(g_vec), std::bind(&Value::asString, std::placeholders::_1));
    }

    auto scene = Scene::create();
    auto layer = new (std::nothrow) ScoreDefinitionScene();
    layer->initWithIndex(idx);

    scene->addChild(layer);
    return scene;
}

bool ScoreDefinitionScene::initWithIndex(size_t idx) {
    if (!Layer::init()) {
        return false;
    }

    auto listener = EventListenerKeyboard::create();
    listener->onKeyReleased = CC_CALLBACK_2(Layer::onKeyReleased, this);
    _eventDispatcher->addEventListenerWithSceneGraphPriority(listener, this);

    Size visibleSize = Director::getInstance()->getVisibleSize();
    Vec2 origin = Director::getInstance()->getVisibleOrigin();

    Label *tileLabel = Label::createWithSystemFont(mahjong::points_name[idx], "Arial", 20);
    this->addChild(tileLabel);
    tileLabel->setPosition(Vec2(origin.x + visibleSize.width * 0.5f,
        origin.y + visibleSize.height - tileLabel->getContentSize().height * 0.5f));

    ui::Button *backBtn = ui::Button::create();
    this->addChild(backBtn);
    backBtn->setScale9Enabled(true);
    backBtn->setContentSize(Size(45, 20));
    backBtn->setTitleFontSize(16);
    backBtn->setTitleText("返回");
    backBtn->setPosition(Vec2(origin.x + 15, origin.y + visibleSize.height - 10));
    backBtn->addClickEventListener([](Ref *) {
        Director::getInstance()->popScene();
    });

    std::string &text = g_vec[idx];
    float scale = 1.0f;
    float maxWidth = (visibleSize.width - 10) / 18;
    if (maxWidth < 25) {
        scale = maxWidth / 27;
    }
    replaceTilesToImage(text, scale);

    ui::RichText *richText = ui::RichText::createWithXML(text);
    richText->setContentSize(Size(visibleSize.width - 10, 0));
    richText->ignoreContentAdaptWithSize(false);
    richText->setVerticalSpace(2);
    richText->formatText();
    this->addChild(richText);
    richText->setPosition(Vec2(origin.x + visibleSize.width * 0.5f, origin.y + visibleSize.height - 40));
    return true;
}

void ScoreDefinitionScene::onKeyReleased(EventKeyboard::KeyCode keyCode, Event *unusedEvent) {
    if (keyCode == EventKeyboard::KeyCode::KEY_BACK) {
        Director::getInstance()->popScene();
    }
}
<|endoftext|>