Agent.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2012, Daniel Claes, Maastricht University
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Maastricht University nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 
00031 #include "collvoid_local_planner/Agent.h"
00032 #include "collvoid_local_planner/orca.h"
00033 #include <boost/foreach.hpp>
00034 
00035 namespace collvoid{
00036 
00037   void Agent::computeOrcaVelocity(Vector2 pref_velocity, bool convex){
00038 
00039     orca_lines_.clear();
00040     orca_lines_.insert(orca_lines_.end(), additional_orca_lines_.begin(), additional_orca_lines_.end());
00041 
00042     
00043     const size_t num_obst_lines = orca_lines_.size();
00044    
00045     if (controlled_) {
00046       /* Create agent ORCA lines. */
00047       BOOST_FOREACH (AgentPtr agent, agent_neighbors_) {
00048         double timestep = agent->timestep_;
00049         if (timestep < EPSILON)
00050           timestep = sim_period_;
00051         Line line;
00052         if (!convex) {
00053           line = createOrcaLine(this, agent.get(), trunc_time_, timestep, left_pref_, cur_allowed_error_);
00054         }
00055         else {
00056           VO new_agent_vo = createVO(position_, footprint_, velocity_, agent->position_, agent->footprint_, agent->velocity_, VOS);
00057           line = createOrcaLine(new_agent_vo.combined_radius, new_agent_vo.relative_position, velocity_, agent->velocity_, trunc_time_, timestep, left_pref_, cur_allowed_error_, agent->controlled_);
00058         }
00059         orca_lines_.push_back(line);
00060       }
00061     }
00062     size_t line_fail = linearProgram2(orca_lines_, max_speed_x_, pref_velocity, false, new_velocity_);
00063 
00064     if (line_fail < orca_lines_.size()) {
00065       linearProgram3(orca_lines_, num_obst_lines, line_fail, max_speed_x_, new_velocity_);
00066     }
00067 
00068   }
00069 
00070   void Agent::computeClearpathVelocity(Vector2 pref_velocity){
00071     if (controlled_) {
00072       computeAgentVOs();
00073     }
00074     new_velocity_ = calculateClearpathVelocity(samples_, vo_agents_, additional_orca_lines_, pref_velocity, max_speed_x_, use_truncation_);
00075   }
00076 
00077   void Agent::computeSampledVelocity(Vector2 pref_velocity){
00078     if (controlled_) {
00079       computeAgentVOs();
00080     }
00081     new_velocity_ = calculateNewVelocitySampled(samples_, vo_agents_, pref_velocity, max_speed_x_, use_truncation_);
00082   }
00083 
00084   
00085   void Agent::computeAgentVOs(){
00086     BOOST_FOREACH (AgentPtr agent, agent_neighbors_) {
00087       VO new_agent_vo;
00088       //use footprint or radius to create VO
00089       if (convex_){
00090         if (agent->controlled_){
00091           new_agent_vo = createVO(position_, footprint_, velocity_, agent->position_, agent->footprint_, agent->velocity_, type_vo_);
00092         }
00093         else {
00094           new_agent_vo = createVO(position_, footprint_, velocity_, agent->position_, agent->footprint_, agent->velocity_, VOS);
00095         }
00096       }
00097       else {
00098         if (agent->controlled_) {
00099           new_agent_vo = createVO(position_, radius_, velocity_, agent->position_, agent->radius_, agent->velocity_, type_vo_);
00100         }
00101         else {
00102           new_agent_vo = createVO(position_, radius_, velocity_, agent->position_, agent->radius_, agent->velocity_, VOS);
00103         }
00104         
00105       }
00106       //truncate
00107       if (use_truncation_) {
00108         new_agent_vo = createTruncVO(new_agent_vo, trunc_time_);
00109       }
00110       vo_agents_.push_back(new_agent_vo);
00111 
00112     }
00113   }
00114 
00115   void Agent::setLeftPref(double left_pref) {
00116     this->left_pref_ = left_pref;
00117   }
00118 
00119   void Agent::setRadius(double radius){
00120     this->radius_ = radius;
00121   }
00122 
00123   void Agent::setTruncTime(double trunc_time){
00124     this->trunc_time_ = trunc_time;
00125   }
00126 
00127 
00128   void Agent::setSimPeriod(double sim_period){
00129     sim_period_ = sim_period;
00130   }
00131 
00132   
00133   //used in orca for orcalines creation...
00134   collvoid::Vector2 Agent::getPosition(){
00135     return this->position_;
00136   }
00137   
00138   collvoid::Vector2 Agent::getVelocity(){
00139     return this->velocity_;
00140   }
00141   
00142   double Agent::getRadius(){
00143     return radius_;
00144   }
00145 
00146 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


collvoid_local_planner
Author(s): Daniel Claes
autogenerated on Sun Aug 25 2013 10:10:23